ShapeSpace.cc
Go to the documentation of this file.00001 #include <iostream>
00002 #include <iomanip>
00003 #include <vector>
00004 #include <sstream>
00005 #include <string>
00006 #include <float.h>
00007 #include <cmath>
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 "ShapeLocalizationParticle.h"
00020 #include "ShapeMarker.h"
00021 #include "ShapeCylinder.h"
00022 #include "ShapeSift.h"
00023 #include "ShapeAprilTag.h"
00024 #include "ShapeGraphics.h"
00025 #include "ShapeDomino.h"
00026 #include "ShapeNaught.h"
00027 #include "ShapeCross.h"
00028 #include "ShapeSkeleton.h"
00029
00030 #include "BaseData.h"
00031 #include "SketchSpace.h"
00032 #include "ShapeSpace.h"
00033 #include "VRmixin.h"
00034
00035 using namespace std;
00036
00037 #ifdef PLATFORM_APERIOS
00038
00039 static int isfinite(float s) { return (s == s) && ((s == 0) || (s != 2*s)); }
00040 #endif
00041
00042 namespace DualCoding {
00043
00044 ShapeSpace::ShapeSpace(SketchSpace* dualSkS, int init_id, std::string const _name,
00045 ReferenceFrameType_t _refFrameType) :
00046 name(_name.length() == 0 ? dualSkS->name : _name),
00047 dualSpace(dualSkS), id_counter(init_id), shapeCache(),
00048 refFrameType(_refFrameType)
00049 {
00050 shapeCache = std::vector<ShapeRoot>(30);
00051 shapeCache.clear();
00052 }
00053
00054 ShapeSpace::~ShapeSpace(void) {
00055
00056
00057 };
00058
00059 ShapeRoot& ShapeSpace::addShape(BaseData *p) {
00060 p->id = ++id_counter;
00061 p->importAdjust();
00062 shapeCache.push_back(ShapeRoot(p));
00063 return shapeCache.back();
00064 };
00065
00066 void ShapeSpace::importShapes(std::vector<ShapeRoot>& foreign_shapes) {
00067 for (std::vector<ShapeRoot>::const_iterator it = foreign_shapes.begin();
00068 it != foreign_shapes.end(); ++it)
00069 importShape(*it);
00070 }
00071
00072 BaseData* ShapeSpace::importShape(const ShapeRoot& foreign_shape) {
00073 BaseData *own = foreign_shape->clone();
00074 own->space = this;
00075 own->parentId = foreign_shape.id;
00076 own->lastMatchId = foreign_shape.id;
00077 own->refcount = 0;
00078 addShape(own);
00079 return own;
00080 }
00081
00082 void ShapeSpace::deleteShape(ShapeRoot &b) {
00083 if ( b.isValid() )
00084 for ( std::vector<ShapeRoot>::iterator it = shapeCache.begin();
00085 it != shapeCache.end(); ++it ) {
00086 if ( it->id == b.id ) {
00087 shapeCache.erase(it);
00088 break;
00089 }
00090 }
00091 else
00092 std::cerr << "ERROR: Attempt to delete an invalid " << b << std::endl;
00093 }
00094
00095 void ShapeSpace::deleteShapes(std::vector<ShapeRoot>& shapes_vec) {
00096 for (size_t i=0; i < shapes_vec.size(); i++)
00097 deleteShape(shapes_vec[i]);
00098 }
00099
00100 void ShapeSpace::clear() {
00101 shapeCache.clear();
00102 if ( this == &VRmixin::worldShS && VRmixin::theAgent.isValid() )
00103 shapeCache.push_back(VRmixin::theAgent);
00104 }
00105
00106 void ShapeSpace::applyTransform(const fmat::Transform& Tmat, const ReferenceFrameType_t newref) {
00107 std::vector<ShapeRoot> allShapes_vec = allShapes();
00108 const size_t nshapes = allShapes_vec.size();
00109 for(size_t i = 0; i < nshapes; i++)
00110 allShapes_vec[i]->applyTransform(Tmat,newref);
00111 }
00112
00113 Point ShapeSpace::getCentroid() {
00114 std::vector<ShapeRoot> allShapes_vec = allShapes();
00115 return(getCentroidOfSubset(allShapes_vec));
00116 }
00117
00118 Point ShapeSpace::getCentroidOfSubset(const std::vector<ShapeRoot>& subsetShapes_vec) {
00119 const size_t nshapes = subsetShapes_vec.size();
00120 Point subset_centroid_pt = Point(0,0);
00121 for(size_t cur_shape = 0; cur_shape < nshapes; cur_shape++)
00122 subset_centroid_pt += subsetShapes_vec[cur_shape]->getCentroid();
00123 return(subset_centroid_pt/(float)nshapes);
00124 }
00125
00126 std::vector<ShapeRoot> ShapeSpace::allShapes(ShapeType_t type) {
00127 allShapes();
00128 std::vector<ShapeRoot> result;
00129 for ( std::vector<ShapeRoot>::const_iterator it = shapeCache.begin();
00130 it != shapeCache.end(); it++ )
00131 if ( (*it)->getType() == type )
00132 result.push_back(*it);
00133 return result;
00134 }
00135
00136 std::vector<ShapeRoot> ShapeSpace::allShapes(rgb color) {
00137 std::vector<ShapeRoot> result(shapeCache.size());
00138 result.clear();
00139 for ( std::vector<ShapeRoot>::const_iterator it = shapeCache.begin();
00140 it != shapeCache.end(); ++it )
00141 if ( (*it)->getColor() == color )
00142 result.push_back(*it);
00143 return result;
00144 }
00145
00146 ShapeRoot ShapeSpace::getShapeFromId(int id) {
00147 for ( std::vector<ShapeRoot>::const_iterator it = shapeCache.begin();
00148 it != shapeCache.end(); ++it )
00149 if ( (*it)->getId() == id )
00150 return *it;
00151 return ShapeRoot();
00152 }
00153
00154
00155 #define writept(x) fixed << setprecision(2) << x.coordX() \
00156 << " " << fixed << setprecision(2) << x.coordY() << " " << fixed << setprecision(2) << x.coordZ()
00157
00158 std::string ShapeSpace::getShapeListForGUI(void) {
00159 std::vector<ShapeRoot> &allShapes_vec = allShapes();
00160 std::ostringstream liststream;
00161 for(unsigned int i = 0; i < allShapes_vec.size(); i++) {
00162 if ( allShapes_vec[i]->isViewable() ) {
00163 liststream << "shape" << endl;
00164 liststream << "id: " << allShapes_vec[i]->getId() << endl;
00165 liststream << "parentId: " << allShapes_vec[i]->getParentId() << endl;
00166 liststream << "name: " << allShapes_vec[i]->getName() << endl;
00167 liststream << "shapetype: " << allShapes_vec[i]->getType() << endl;
00168 liststream << "color: " << toString(allShapes_vec[i]->getColor()) << endl;
00169 liststream << "cxyz: " << writept(allShapes_vec[i]->getCentroid()) << endl;
00170 liststream << "obst/lm: " << allShapes_vec[i]->isObstacle() << " " << allShapes_vec[i]->isLandmark() << endl;
00171
00172 if(allShapes_vec[i]->isType(lineDataType)) {
00173 const Shape<LineData>& current = ShapeRootTypeConst(allShapes_vec[i],LineData);
00174 liststream << "e1xyz: " << writept(current->end1Pt()) << endl;
00175 liststream << "e2xyz: " << writept(current->end2Pt()) << endl;
00176 liststream << "r:" << current->getRhoNorm() << endl;
00177 liststream << "theta: " << current->getThetaNorm() << endl;
00178 liststream << "end1: " << (current->end1Pt().isValid()) << " "
00179 << (current->end1Pt().isActive()) << endl;
00180 liststream << "end2: " << (current->end2Pt().isValid()) << " "
00181 << (current->end2Pt().isActive()) << endl;
00182 }
00183
00184 else if (allShapes_vec[i]->isType(ellipseDataType)) {
00185 const Shape<EllipseData>& current = ShapeRootTypeConst(allShapes_vec[i],EllipseData);
00186 liststream << "axes: " << current->getSemimajor()
00187 << " " << current->getSemiminor() << endl;
00188 liststream << "orientation: " << current->getOrientation()
00189 << endl;
00190 }
00191
00192 else if (allShapes_vec[i]->isType(pointDataType)) {
00193 ;
00194 }
00195
00196 else if (allShapes_vec[i]->isType(agentDataType)) {
00197 const Shape<AgentData>& current = ShapeRootTypeConst(allShapes_vec[i],AgentData);
00198 liststream << "orientation: " << current->getOrientation() << endl;
00199 fmat::Column<3> offset = current->getBoundingBoxOffset();
00200 fmat::Column<3> halfDimXYZ = current->getBoundingBoxHalfDims();
00201 liststream << "offsetX: " << offset[0] << endl;
00202 liststream << "offsetY: " << offset[1] << endl;
00203
00204 liststream << "halfDimX: " << halfDimXYZ[0] << endl;
00205 liststream << "halfDimY: " << halfDimXYZ[1] << endl;
00206
00207 }
00208
00209 else if (allShapes_vec[i]->isType(sphereDataType)) {
00210 const Shape<SphereData>& current = ShapeRootTypeConst(allShapes_vec[i],SphereData);
00211 liststream << "radius: " << current->getRadius() << endl;
00212 }
00213
00214 else if (allShapes_vec[i]->isType(polygonDataType)) {
00215 const Shape<PolygonData>& current = ShapeRootTypeConst(allShapes_vec[i],PolygonData);
00216 liststream << "num_vtx: " << current->getVertices().size() << endl;
00217 for (std::vector<Point>::const_iterator vtx_it = current->getVertices().begin();
00218 vtx_it != current->getVertices().end(); vtx_it++)
00219 liststream << "vertex: " << writept((*vtx_it)) << endl;
00220 liststream << "end1_valid: " << (current->end1Ln().end1Pt().isValid()) << endl;
00221 liststream << "end2_valid: " << (current->end2Ln().end2Pt().isValid()) << endl;
00222 }
00223
00224 else if (allShapes_vec[i]->isType(blobDataType)) {
00225 const Shape<BlobData>& current = ShapeRootTypeConst(allShapes_vec[i],BlobData);
00226 const char* colorname = ProjectInterface::getColorName(current->getColor());
00227 liststream << "colorname: " << (colorname != NULL ? string(colorname) : toString(current->getColor())) << endl;
00228 liststream << "area: " << current->getArea() << endl;
00229 liststream << "orient: " << current->orientation << endl;
00230 liststream << "topLeft: " << writept(current->topLeft) << endl;
00231 liststream << "topRight: " << writept(current->topRight) << endl;
00232 liststream << "bottomLeft: " << writept(current->bottomLeft) << endl;
00233 liststream << "bottomRight :" << writept(current->bottomRight) << endl;
00234 liststream << "validTop: " << (current->topValid ? "true" : "false") << endl;
00235 liststream << "validBottom: " << (current->bottomValid ? "true" : "false") << endl;
00236 liststream << "validLeft: " << (current->leftValid ? "true" : "false") << endl;
00237 liststream << "validRight: " << (current->rightValid ? "true" : "false") << endl;
00238
00239 }
00240 else if (allShapes_vec[i]->isType(brickDataType)) {
00241 const Shape<BrickData>& current = ShapeRootTypeConst(allShapes_vec[i],BrickData);
00242 liststream << "GFL: " << writept(current->getGFL()) << endl;
00243 liststream << "GBL: " << writept(current->getGBL()) << endl;
00244 liststream << "GFR: " << writept(current->getGFR()) << endl;
00245 liststream << "GBR: " << writept(current->getGBR()) << endl;
00246 liststream << "TFL: " << writept(current->getTFL()) << endl;
00247 liststream << "TBL: " << writept(current->getTBL()) << endl;
00248 liststream << "TFR: " << writept(current->getTFR()) << endl;
00249 liststream << "TBR: " << writept(current->getTBR()) << endl;
00250 }
00251 else if (allShapes_vec[i]->isType(pyramidDataType)) {
00252 const Shape<PyramidData>& current = ShapeRootTypeConst(allShapes_vec[i],PyramidData);
00253 liststream << "FL: " << writept(current->getFL()) << endl;
00254 liststream << "BL: " << writept(current->getBL()) << endl;
00255 liststream << "FR: " << writept(current->getFR()) << endl;
00256 liststream << "BR: " << writept(current->getBR()) << endl;
00257 liststream << "Top: " << writept(current->getTop()) << endl;
00258 }
00259 else if (allShapes_vec[i]->isType(localizationParticleDataType)) {
00260 const Shape<LocalizationParticleData>& current = ShapeRootTypeConst(allShapes_vec[i],LocalizationParticleData);
00261 const float weight = current->getWeight();
00262 liststream << "orient/weight: " << current->getOrientation()
00263 << " " << (!isfinite(weight) ? (weight>0 ? FLT_MAX : -FLT_MAX) : weight) << endl;
00264 }
00265 else if (allShapes_vec[i]->isType(markerDataType)) {
00266 const Shape<MarkerData>& current = ShapeRootTypeConst(allShapes_vec[i],MarkerData);
00267 liststream << MarkerData::getMarkerTypeName(current->typeOfMarker) << ":" << current->getMarkerDescription() << endl;
00268 }
00269 else if (allShapes_vec[i]->isType(cylinderDataType)) {
00270 const Shape<CylinderData>& current = ShapeRootTypeConst(allShapes_vec[i], CylinderData);
00271 liststream << "height: " << current->getHeight() << endl << "radius: " << current->getRadius() << endl;
00272 const fmat::Quaternion q = current->getOrientation().aboutX(M_PI/2);
00273 liststream << "orientation: " << q.getW() << " " << q.getX()
00274 << " " << q.getY() << " " << q.getZ() << endl;
00275 }
00276 else if (allShapes_vec[i]->isType(siftDataType)) {
00277 const Shape<SiftData>& current = ShapeRootTypeConst(allShapes_vec[i],SiftData);
00278 liststream << "objectName: " << current->getObjectName() << endl;
00279 liststream << "objectID: " << current->getObjectID() << endl;
00280 liststream << "modelName: " << current->getModelName() << endl;
00281 liststream << "topLeft: " << writept(current->getTopLeft()) << endl;
00282 liststream << "topRight: " << writept(current->getTopRight()) << endl;
00283 liststream << "bottomLeft: " << writept(current->getBottomLeft()) << endl;
00284 liststream << "bottomRight :" << writept(current->getBottomRight()) << endl;
00285 }
00286 else if (allShapes_vec[i]->isType(aprilTagDataType)) {
00287 const Shape<AprilTagData>& current = ShapeRootTypeConst(allShapes_vec[i],AprilTagData);
00288 const AprilTags::TagDetection &td = current->getTagDetection();
00289 const fmat::Quaternion q = current->getQuaternion();
00290 liststream << "tagID: " << current->getTagID() << endl;
00291 liststream << "orient: " << q.ypr()[0] << endl;
00292 liststream << "quaternion: " << q.getW() << " " << q.getX() << " " << q.getY() << " " << q.getZ() << endl;
00293 liststream << "hammingDistance: " << td.hammingDistance << endl;
00294 liststream << "p0: " << td.p[0].first << " " << td.p[0].second << endl;
00295 liststream << "p1: " << td.p[1].first << " " << td.p[1].second << endl;
00296 liststream << "p2: " << td.p[2].first << " " << td.p[2].second << endl;
00297 liststream << "p3: " << td.p[3].first << " " << td.p[3].second << endl;
00298 }
00299 else if (allShapes_vec[i]->isType(graphicsDataType)) {
00300 const Shape<GraphicsData>& current = ShapeRootTypeConst(allShapes_vec[i],GraphicsData);
00301 liststream << "numelts: " << current->elements.size() << endl;
00302 for (unsigned int j = 0; j < current->elements.size(); j++) {
00303 const GraphicsData::GraphicsElement *elt = current->elements[j];
00304 switch ( elt->getType() ) {
00305
00306 case GraphicsData::lineGType: {
00307 const GraphicsData::LineElement *line =
00308 dynamic_cast<const GraphicsData::LineElement*>(elt);
00309 liststream << "lineElement: " << elt->getType() << " " << toString(elt->getColor()) << " " << elt->getName() << endl;
00310 liststream << "points: " << line->pt1.first << " " << line->pt1.second << " "
00311 << line->pt2.first << " " << line->pt2.second << endl;
00312 break; }
00313
00314 case GraphicsData::polygonGType: {
00315 const GraphicsData::PolygonElement *polygon =
00316 dynamic_cast<const GraphicsData::PolygonElement*>(elt);
00317 liststream<< "polygonElement: " << elt->getType() << " "<< toString(elt->getColor()) << " " << elt->getName() << endl;
00318 liststream << "numvtx: "<< polygon->vertices.size() << endl;
00319 for (unsigned int k = 0; k < polygon->vertices.size(); k++) {
00320 std::pair<float,float> vert = polygon->vertices[k];
00321 liststream << "vertex: " << vert.first << " " << vert.second << endl;
00322 }
00323 liststream << "closed: " << (polygon->closed ? "true" : "false") << endl;
00324 break; }
00325
00326 case GraphicsData::ellipseGType: {
00327 const GraphicsData::EllipseElement *ellipse =
00328 dynamic_cast<const GraphicsData::EllipseElement*>(elt);
00329 liststream << "ellipseElement: " << elt->getType() << " " << toString(elt->getColor()) << " " << elt->getName() << endl;
00330 liststream << "centroid: " << ellipse->center.first << " " << ellipse->center.second << endl;
00331 liststream << "semimajor: " << ellipse->semimajor << endl;
00332 liststream << "semiminor: " << ellipse->semiminor << endl;
00333 liststream << "orientation: " << ellipse->orientation << endl;
00334 liststream << "filled: : " << (ellipse->filled ? "true" : "false") << endl;
00335 break; }
00336
00337 case GraphicsData::textGType: {
00338 const GraphicsData::TextElement *text =
00339 dynamic_cast<const GraphicsData::TextElement*>(elt);
00340 liststream << "textElement: " << elt->getType() << " " << toString(elt->getColor()) << " " << elt->getName() << endl;
00341 liststream << "startpoint: " << text->startpt.first << " " << text->startpt.second << endl;
00342 liststream << "message: " << text->msg << endl;
00343 break; }
00344
00345 case GraphicsData::locParticleGType: {
00346 const GraphicsData::LocalizationParticleElement *locParticle =
00347 dynamic_cast<const GraphicsData::LocalizationParticleElement*>(elt);
00348 liststream << "localizationParticleElement: " << elt->getType() << " " << toString(elt->getColor()) << " " << elt->getName() << endl;
00349 liststream << "centroid: " << locParticle->getCentroid().coordX() << " " << locParticle->getCentroid().coordY() << endl;
00350 liststream << "orientation: " << locParticle->getOrientation()
00351 << " weight: " << locParticle->getWeight() << endl;
00352 break; }
00353
00354 case GraphicsData::axisAngleGType: {
00355 const GraphicsData::AxisAngleElement *axisAngle = dynamic_cast<const GraphicsData::AxisAngleElement*>(elt);
00356 liststream << "axisAngleElement: " << elt->getType() << " " << toString(elt->getColor()) << endl;
00357 liststream << "Quaternion: " << axisAngle->q.getW() << " " << axisAngle->q.getX() << " " << axisAngle->q.getY() << " " << axisAngle->q.getZ() << endl;
00358 liststream << "Centroid: " << axisAngle->centroid[0] << " " << axisAngle->centroid[1] << " " << axisAngle->centroid[2] << endl;
00359 break; }
00360
00361 case GraphicsData::pointGType: {
00362 const GraphicsData::PointElement *point =
00363 dynamic_cast<const GraphicsData::PointElement*>(elt);
00364 liststream << "pointElement: " << elt->getType() << " " << toString(elt->getColor()) << " " << elt->getName() << endl;
00365 liststream << "centroid: " << point->center.first << " " << point->center.second << endl;
00366 break; }
00367 case GraphicsData::boundingGType: {
00368 const GraphicsData::BoundingBoxElement *bb = dynamic_cast<const GraphicsData::BoundingBoxElement*>(elt);
00369 liststream << "boundingElement: " << elt->getType() << " " << toString(elt->getColor()) << endl;
00370 if (bb->vertices.size() <= 0) {
00371 liststream << "draw vertex: " << "false" << endl;
00372 liststream << "centroid: " << bb->centroid[0] << " " << bb->centroid[1] << " " << bb->centroid[2] << endl;
00373 liststream << "dimensions: " << bb->w << " " << bb->h << " " << bb->l << endl;
00374 liststream << "quaternion: " << bb->q.getW() << " " << bb->q.getX() << " " << bb->q.getY() << " " << bb->q.getZ() << endl;
00375 }
00376 else {
00377 liststream << "draw vertex: " << "true" << endl;
00378 liststream << "numer of components: " << bb->vertices.size() << endl;
00379 for (size_t vi = 0; vi < bb->vertices.size(); vi++)
00380 for (size_t vj = 0; vj < bb->vertices[vi].size(); vj++)
00381 liststream << "vertex point: " << bb->vertices[vi][vj][0]
00382 << " " << bb->vertices[vi][vj][1]
00383 << " " << bb->vertices[vi][vj][2] << endl;
00384 }
00385 break; }
00386
00387 default:
00388 std::cerr << "Bad GraphicsElement type: " << elt->getType() << std::endl;
00389 }
00390 }
00391 }
00392 else if (allShapes_vec[i]->isType(dominoDataType)) {
00393 const Shape<DominoData>& current = ShapeRootTypeConst(allShapes_vec[i],DominoData);
00394 liststream << "GFL: " << writept(current->getGFL()) << endl;
00395 liststream << "GBL: " << writept(current->getGBL()) << endl;
00396 liststream << "GFR: " << writept(current->getGFR()) << endl;
00397 liststream << "GBR: " << writept(current->getGBR()) << endl;
00398 liststream << "TFL: " << writept(current->getTFL()) << endl;
00399 liststream << "TBL: " << writept(current->getTBL()) << endl;
00400 liststream << "TFR: " << writept(current->getTFR()) << endl;
00401 liststream << "TBR: " << writept(current->getTBR()) << endl;
00402 liststream << "values: " << current->getLowValue() << " " << current->getHighValue() << endl;
00403 }
00404 else if (allShapes_vec[i]->isType(naughtDataType)) {
00405 const Shape<NaughtData>& current = ShapeRootTypeConst(allShapes_vec[i], NaughtData);
00406 liststream << "height: " << current->getHeight() << endl;
00407 liststream << "radius: " << current->getRadius() << endl;
00408 }
00409 else if (allShapes_vec[i]->isType(crossDataType)) {
00410 const Shape<CrossData>& current = ShapeRootTypeConst(allShapes_vec[i], CrossData);
00411 liststream << "e1xyz: " << writept(current->getLine1().end1Pt()) << endl;
00412 liststream << "e2xyz: " << writept(current->getLine1().end2Pt()) << endl;
00413 liststream << "e3xyz: " << writept(current->getLine2().end1Pt()) << endl;
00414 liststream << "e4xyz: " << writept(current->getLine2().end2Pt()) << endl;
00415 liststream << "armwidth: " << current->getArmWidth() << endl;
00416 }
00417 else if (allShapes_vec[i]->isType(skeletonDataType)) {
00418 const Shape<SkeletonData>& skelshape = ShapeRootTypeConst(allShapes_vec[i],SkeletonData);
00419 const Skeleton &skel = skelshape->getSkeleton();
00420 int numJoints = 0;
00421 for (unsigned int j = 1; j<=Skeleton::NumSkelJoints; j++)
00422 if ( skel.validJoint(j) ) ++numJoints;
00423 liststream << "numJoints: " << numJoints << endl;
00424 for (unsigned int j = 1; j<=Skeleton::NumSkelJoints; j++)
00425 if ( skel.validJoint(j) )
00426 liststream << "joint: " << j << " "
00427 << skel.joints[j].coordX() << " " << skel.joints[j].coordY() << " " << skel.joints[j].coordZ() << std::endl;
00428 }
00429 }
00430 }
00431 return liststream.str();
00432 }
00433
00434 #undef writept
00435
00436 void ShapeSpace::printParams() {
00437 cout << endl;
00438 cout << "SHAPE SPACE : PARAMETERS BEGIN" << endl;
00439 int cur, num;
00440
00441 std::vector<ShapeRoot> &allShapes_vec = allShapes();
00442 num = (int)(allShapes_vec.size());
00443
00444 for(cur = 0; cur < num; cur++) {
00445
00446 cout << "SHAPE " << allShapes_vec[cur].getId()
00447 << " (" << cur+1 << " of " << num << ")"
00448 << endl;
00449
00450 allShapes_vec[cur]->printParams();
00451
00452 cout << "===========================================" << endl;
00453 cout << endl;
00454
00455 }
00456
00457 cout << endl;
00458 cout << "SHAPE SPACE : PARAMETERS END" << endl;
00459 }
00460
00461 void ShapeSpace::printSummary()
00462 {
00463
00464 std::vector<ShapeRoot> allShapes_vec = allShapes();
00465 int cur;
00466 int num = (int)(allShapes_vec.size());
00467 cout << "SHAPE SPACE : SUMMARY BEGIN" << endl;
00468 std::vector<int> shape_counts;
00469 shape_counts.resize(numDataTypes,0);
00470
00471 cout << endl << "Shape Listing:" << endl;
00472 for(cur = 0; cur < num; cur++) {
00473 cout << "Shape " << allShapes_vec[cur].getId()
00474 << " (" << cur+1 << " of " << num << ")."
00475 << "\tType: " << allShapes_vec[cur]->getTypeName()
00476 << endl;
00477 shape_counts[allShapes_vec[cur]->getType()]++;
00478 }
00479
00480 cout << endl << "Shape Counts:" << endl;
00481 num = numDataTypes;
00482 for(cur = 0; cur < num; cur++) {
00483 cout << "Shape Type " << cur << " " << data_name(cur)
00484 << ":\t" << shape_counts[cur] << endl;
00485 }
00486 cout << endl;
00487
00488 cout << "SHAPE SPACE : SUMMARY END" << endl;
00489 }
00490
00491 void ShapeSpace::deleteShapeType(ShapeType_t t) {
00492 std::vector<ShapeRoot> temp;
00493 temp.reserve(shapeCache.size());
00494 for ( std::vector<ShapeRoot>::const_iterator it = shapeCache.begin();
00495 it != shapeCache.end(); it++ )
00496 if ( (*it)->getType() != t )
00497 temp.push_back(*it);
00498 shapeCache = temp;
00499 }
00500
00501 }