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