00001 #include <iostream>
00002 #include <vector>
00003 #include <sstream>
00004 #include <string>
00005
00006 #include "Shared/newmat/newmat.h"
00007 #include "Shared/newmat/newmatio.h"
00008
00009 #include "ShapeRoot.h"
00010 #include "ShapeAgent.h"
00011 #include "ShapeLine.h"
00012 #include "ShapeEllipse.h"
00013 #include "ShapePoint.h"
00014 #include "ShapeSphere.h"
00015 #include "ShapeBlob.h"
00016 #include "ShapePolygon.h"
00017 #include "ShapeBrick.h"
00018 #include "ShapePyramid.h"
00019 #include "BaseData.h"
00020 #include "SketchSpace.h"
00021 #include "ShapeSpace.h"
00022 #include "VRmixin.h"
00023
00024 namespace DualCoding {
00025
00026 ShapeSpace::ShapeSpace(SketchSpace* dualSkS, int init_id, string const _name,
00027 ReferenceFrameType_t _refFrameType) :
00028 name(_name.length() == 0 ? dualSkS->name : _name),
00029 dualSpace(dualSkS), id_counter(init_id), shapeCache(),
00030 refFrameType(_refFrameType)
00031 {
00032 shapeCache = vector<ShapeRoot>(30);
00033 shapeCache.clear();
00034 }
00035
00036 ShapeSpace::~ShapeSpace(void) {
00037
00038
00039 };
00040
00041 ShapeRoot& ShapeSpace::addShape(BaseData *p) {
00042 p->id = ++id_counter;
00043 shapeCache.push_back(ShapeRoot(p));
00044 return shapeCache[shapeCache.size()-1];
00045 };
00046
00047 void ShapeSpace::importShapes(vector<ShapeRoot>& foreign_shapes) {
00048 for (vector<ShapeRoot>::const_iterator it = foreign_shapes.begin();
00049 it != foreign_shapes.end();
00050 ++it) importShape(*it);
00051 }
00052
00053 BaseData* ShapeSpace::importShape(const ShapeRoot& foreign_shape) {
00054 BaseData *own = foreign_shape->clone();
00055 own->space = this;
00056 own->parentId = 0;
00057 own->lastMatchId = foreign_shape.id;
00058 own->refcount = 0;
00059 addShape(own);
00060 return own;
00061 }
00062
00063 void ShapeSpace::deleteShape(ShapeRoot &b) {
00064 if ( b.isValid() )
00065 for ( vector<ShapeRoot>::iterator it = shapeCache.begin();
00066 it != shapeCache.end(); ++it ) {
00067 if ( it->id == b.id ) {
00068 shapeCache.erase(it);
00069 break;
00070 }
00071 }
00072 else
00073 std::cerr << "ERROR: Attempt to delete an invalid " << b << std::endl;
00074 }
00075
00076 void ShapeSpace::deleteShapes(vector<ShapeRoot>& shapes_vec) {
00077 for (size_t i=0; i < shapes_vec.size(); i++)
00078 deleteShape(shapes_vec[i]);
00079 }
00080
00081 void ShapeSpace::clear() {
00082 shapeCache.clear();
00083 if ( this == &VRmixin::worldShS && VRmixin::theAgent.isValid() )
00084 shapeCache.push_back(VRmixin::theAgent);
00085 }
00086
00087 void ShapeSpace::applyTransform(const NEWMAT::Matrix& Tmat) {
00088 vector<ShapeRoot> allShapes_vec = allShapes();
00089 const size_t nshapes = allShapes_vec.size();
00090 for(size_t i = 0; i < nshapes; i++)
00091 allShapes_vec[i]->applyTransform(Tmat);
00092 }
00093
00094 Point ShapeSpace::getCentroid() {
00095 vector<ShapeRoot> allShapes_vec = allShapes();
00096 return(getCentroidOfSubset(allShapes_vec));
00097 }
00098
00099 Point ShapeSpace::getCentroidOfSubset(const vector<ShapeRoot>& subsetShapes_vec) {
00100 const size_t nshapes = subsetShapes_vec.size();
00101 Point subset_centroid_pt = Point(0,0);
00102 for(size_t cur_shape = 0; cur_shape < nshapes; cur_shape++)
00103 subset_centroid_pt += subsetShapes_vec[cur_shape]->getCentroid();
00104 return(subset_centroid_pt/(float)nshapes);
00105 }
00106
00107 std::vector<ShapeRoot> ShapeSpace::allShapes(ShapeType_t type) {
00108 allShapes();
00109 std::vector<ShapeRoot> result;
00110 for ( std::vector<ShapeRoot>::const_iterator it = shapeCache.begin();
00111 it != shapeCache.end(); it++ )
00112 if ( (*it)->getType() == type )
00113 result.push_back(*it);
00114 return result;
00115 }
00116
00117 std::vector<ShapeRoot> ShapeSpace::allShapes(rgb color) {
00118 std::vector<ShapeRoot> result(shapeCache.size());
00119 result.clear();
00120 for ( std::vector<ShapeRoot>::const_iterator it = shapeCache.begin();
00121 it != shapeCache.end(); ++it )
00122 if ( (*it)->getColor() == color )
00123 result.push_back(*it);
00124 return result;
00125 }
00126
00127 string ShapeSpace::getShapeListForGUI(void) {
00128 vector<ShapeRoot> &allShapes_vec = allShapes();
00129 ostringstream liststream;
00130 #define writept(x) x.coordX() << " " << x.coordY() << " " << x.coordZ()
00131 for(unsigned int i = 0; i < allShapes_vec.size(); i++) {
00132 if ( allShapes_vec[i]->isViewable() ) {
00133 liststream << "shape" << endl;
00134 liststream << "id: " << allShapes_vec[i]->getId() << endl;
00135 liststream << "parentId: " << allShapes_vec[i]->getParentId() << endl;
00136 liststream << "name: " << allShapes_vec[i]->getName() << endl;
00137 liststream << "shapetype: " << allShapes_vec[i]->getType() << endl;
00138 liststream << "color: " << ProjectInterface::toString(allShapes_vec[i]->getColor()) << endl;
00139 liststream << "cxyz: " << writept(allShapes_vec[i]->getCentroid()) << endl;
00140
00141 if(allShapes_vec[i]->isType(lineDataType)) {
00142 const Shape<LineData>& current = ShapeRootTypeConst(allShapes_vec[i],LineData);
00143 liststream << "e1xyz: " << writept(current->end1Pt()) << endl;
00144 liststream << "e2xyz: " << writept(current->end2Pt()) << endl;
00145 liststream << "r:" << current->getRhoNorm() << endl;
00146 liststream << "theta: " << current->getThetaNorm() << endl;
00147 liststream << "end1: " << (current->end1Pt().isValid()) << " "
00148 << (current->end1Pt().isActive()) << endl;
00149 liststream << "end2: " << (current->end2Pt().isValid()) << " "
00150 << (current->end2Pt().isActive()) << endl;
00151 }
00152
00153 else if (allShapes_vec[i]->isType(ellipseDataType)) {
00154 const Shape<EllipseData>& current = ShapeRootTypeConst(allShapes_vec[i],EllipseData);
00155 liststream << "axes: " << current->getSemimajor()
00156 << " " << current->getSemiminor() << endl;
00157 liststream << "orientation: " << current->getOrientation()
00158 << endl;
00159 }
00160
00161 else if (allShapes_vec[i]->isType(pointDataType)) {
00162 ;
00163 }
00164
00165 else if (allShapes_vec[i]->isType(agentDataType)) {
00166 const Shape<AgentData>& current = ShapeRootTypeConst(allShapes_vec[i],AgentData);
00167 liststream << "orientation: " << current->getOrientation() << endl;
00168 }
00169
00170 else if (allShapes_vec[i]->isType(sphereDataType)) {
00171 const Shape<SphereData>& current = ShapeRootTypeConst(allShapes_vec[i],SphereData);
00172 liststream << "radius: " << current->getRadius() << endl;
00173 }
00174
00175 else if (allShapes_vec[i]->isType(polygonDataType)) {
00176 const Shape<PolygonData>& current = ShapeRootTypeConst(allShapes_vec[i],PolygonData);
00177 liststream << "num_vtx: " << current->getVertices().size() << endl;
00178 for (vector<Point>::const_iterator vtx_it = current->getVertices().begin();
00179 vtx_it != current->getVertices().end(); vtx_it++)
00180 liststream << "vertex: " << writept((*vtx_it)) << endl;
00181 liststream << "end1_valid: " << (current->end1Ln().end1Pt().isValid()) << endl;
00182 liststream << "end2_valid: " << (current->end2Ln().end2Pt().isValid()) << endl;
00183 }
00184
00185 else if (allShapes_vec[i]->isType(blobDataType)) {
00186 const Shape<BlobData>& current = ShapeRootTypeConst(allShapes_vec[i],BlobData);
00187 liststream << "area: " << current->getArea() << endl;
00188 liststream << "orient: " << current->orientation << endl;
00189 liststream << "topLeft: " << writept(current->topLeft) << endl;
00190 liststream << "topRight: " << writept(current->topRight) << endl;
00191 liststream << "bottomLeft: " << writept(current->bottomLeft) << endl;
00192 liststream << "bottomRight :" << writept(current->bottomRight) << endl;
00193 }
00194 else if (allShapes_vec[i]->isType(brickDataType)) {
00195 const Shape<BrickData>& current = ShapeRootTypeConst(allShapes_vec[i],BrickData);
00196 liststream << "GFL: " << writept(current->getGFL()) << endl;
00197 liststream << "GBL: " << writept(current->getGBL()) << endl;
00198 liststream << "GFR: " << writept(current->getGFR()) << endl;
00199 liststream << "GBR: " << writept(current->getGBR()) << endl;
00200 liststream << "TFL: " << writept(current->getTFL()) << endl;
00201 liststream << "TBL: " << writept(current->getTBL()) << endl;
00202 liststream << "TFR: " << writept(current->getTFR()) << endl;
00203 liststream << "TBR: " << writept(current->getTBR()) << endl;
00204 }
00205 else if (allShapes_vec[i]->isType(pyramidDataType)) {
00206 const Shape<PyramidData>& current = ShapeRootTypeConst(allShapes_vec[i],PyramidData);
00207 liststream << "FL: " << writept(current->getFL()) << endl;
00208 liststream << "BL: " << writept(current->getBL()) << endl;
00209 liststream << "FR: " << writept(current->getFR()) << endl;
00210 liststream << "BR: " << writept(current->getBR()) << endl;
00211 liststream << "Top: " << writept(current->getTop()) << endl;
00212 }
00213 }
00214 }
00215 #undef writept
00216 return liststream.str();
00217 }
00218
00219 void
00220 ShapeSpace::printParams()
00221 {
00222 cout << endl;
00223 cout << "SHAPE SPACE : PARAMETERS BEGIN" << endl;
00224 int cur, num;
00225
00226 vector<ShapeRoot> &allShapes_vec = allShapes();
00227 num = (int)(allShapes_vec.size());
00228
00229 for(cur = 0; cur < num; cur++) {
00230
00231 cout << "SHAPE " << allShapes_vec[cur].getId()
00232 << " (" << cur+1 << " of " << num << ")"
00233 << endl;
00234
00235 allShapes_vec[cur]->printParams();
00236
00237 cout << "===========================================" << endl;
00238 cout << endl;
00239
00240 }
00241
00242 cout << endl;
00243 cout << "SHAPE SPACE : PARAMETERS END" << endl;
00244 }
00245
00246
00247
00248 void
00249 ShapeSpace::printSummary()
00250 {
00251
00252 vector<ShapeRoot> allShapes_vec = allShapes();
00253 int cur;
00254 int num = (int)(allShapes_vec.size());
00255 cout << "SHAPE SPACE : SUMMARY BEGIN" << endl;
00256 vector<int> shape_counts;
00257 shape_counts.resize(numDataTypes,0);
00258
00259 cout << endl << "Shape Listing:" << endl;
00260 for(cur = 0; cur < num; cur++) {
00261 cout << "Shape " << allShapes_vec[cur].getId()
00262 << " (" << cur+1 << " of " << num << ")."
00263 << "\tType: " << allShapes_vec[cur]->getTypeName()
00264 << endl;
00265 shape_counts[allShapes_vec[cur]->getType()]++;
00266 }
00267
00268 cout << endl << "Shape Counts:" << endl;
00269 num = numDataTypes;
00270 for(cur = 0; cur < num; cur++) {
00271 cout << "Shape Type " << cur << " " << data_name(cur)
00272 << ":\t" << shape_counts[cur] << endl;
00273 }
00274 cout << endl;
00275
00276 cout << "SHAPE SPACE : SUMMARY END" << endl;
00277 }
00278
00279 }