Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

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 // missing isfinite, Aibo crashes when it does denormalized float ops anyway... :(
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   //cout << "Deleting ShapeSpace " << name << " at " << this << endl;
00056   // do not call clear() here; VRmixin::theAgent's reference count (in AgentData) may be wrong if the static Shape was already deleted
00057 };
00058 
00059 ShapeRoot& ShapeSpace::addShape(BaseData *p) {
00060   p->id = ++id_counter;
00061   p->importAdjust();  // adjust any components that need it (e.g., for polygons)
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;  // return value is used by MapBuilder::importLocalToWorld
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();  // make sure cache is valid;
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 // must used fixed output format because Java can't parse scientific notation
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)) { // lineshape
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)) { // ellipseshape
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)) { // pointshape
00193   ;
00194       }
00195 
00196       else if (allShapes_vec[i]->isType(agentDataType)) { // agentshape
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   //offsetZ needs to be written when dealing with 3D bounding box
00204   liststream << "halfDimX: " << halfDimXYZ[0] << endl;
00205   liststream << "halfDimY: " << halfDimXYZ[1] << endl;
00206   //dimH needs to be written when dealing with 3D bounding box
00207       }
00208 
00209       else if (allShapes_vec[i]->isType(sphereDataType)) { // sphereshape
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)) { // polygonshape
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)) { // blobshape
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)) { // brickshape
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)) { // pyramidshape
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)) { // localizationparticleshape
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)) { // markershape
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)) { // siftshape
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)) { // apriltagshape
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)) { // graphicsshape
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)) { // dominoshape
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)) { // naughtshape
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)) { // crossshape
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)) { // skeletonshape
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   // JJW will this cause a memory leak?
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 } // namespace

DualCoding 5.1CVS
Generated Mon May 9 04:56:27 2016 by Doxygen 1.6.3