DualCoding: ParticleShapes.cc Source File

00001 //-*-c++-*-
00002 
00003 #include <iostream>
00004 #include <vector>
00005 #include <fstream>
00006 
00007 #include "ShapeRoot.h"
00008 #include "ShapeLine.h"
00009 #include "ShapeEllipse.h"
00010 #include "ShapeBlob.h"
00011 #include "ShapePoint.h"
00012 
00013 #include "ParticleShapes.h"
00014 
00015 using namespace std;
00016 
00017 namespace DualCoding {
00018 
00019 vector<PfRoot*>* PfRoot::loadLms(const vector<ShapeRoot> &lms, bool isWorld){
00020   int id;
00021   int type;
00022   rgb color;
00023   vector<PfRoot*> *landmarks = new vector<PfRoot*>(2*lms.size(),(PfRoot*)NULL);
00024   landmarks->clear();
00025   for (unsigned int i = 0; i<lms.size(); i++){
00026     type = lms[i]->getType();
00027     color = lms[i]->getColor();
00028     if (type == lineDataType) {
00029       const Shape<LineData> &line = ShapeRootTypeConst(lms[i],LineData);
00030       id = line->getId();
00031       const EndPoint &pt1 = line->end1Pt();
00032       const EndPoint &pt2 = line->end2Pt();
00033       PfLine *land = new PfLine(id, color, pt1.coordX(), pt1.coordY(), 
00034         pt2.coordX(), pt2.coordY(), pt1.isValid(), pt2.isValid());
00035       land->link = &lms[i];
00036       land->orientation = atan2(pt2.coordY()-pt1.coordY(), pt2.coordX()-pt1.coordX());
00037       land->length = sqrt((pt2.coordX()-pt1.coordX())*(pt2.coordX()-pt1.coordX()) +
00038         (pt2.coordY()-pt1.coordY())*(pt2.coordY()-pt1.coordY()));
00039       landmarks->push_back(land);
00040       if ( isWorld ) {
00041   PfLine *land2 = new PfLine(id, color, pt2.coordX(), pt2.coordY(),
00042                pt1.coordX(), pt1.coordY(), pt2.isValid(), pt1.isValid());
00043   land2->link = &lms[i];
00044   land2->orientation = land->orientation;
00045   land2->length = land->length;
00046   landmarks->push_back(land2);
00047       }
00048     }
00049     else if (type == ellipseDataType) {
00050       const Shape<EllipseData> &ellipse = ShapeRootTypeConst(lms[i],EllipseData);
00051       id = ellipse->getId();
00052       const Point &pt = ellipse->getCentroid();
00053       PfEllipse* land = new PfEllipse(id, color, pt.coordX(), pt.coordY());
00054       land->link = &lms[i];
00055       landmarks->push_back(land);
00056     }
00057     else if (type == pointDataType) {
00058       const Shape<PointData> &point = ShapeRootTypeConst(lms[i],PointData);
00059       id = point->getId();
00060       const Point &pt = point->getCentroid();
00061       PfPoint* land = new PfPoint(id, color, pt.coordX(), pt.coordY());
00062       land->link = &lms[i];
00063       landmarks->push_back(land);
00064     }
00065     else if (type == blobDataType) {
00066       const Shape<BlobData> &blob = ShapeRootTypeConst(lms[i],BlobData);
00067       id = blob->getId();
00068       Point pt = blob->getCentroid();
00069       PfBlob* land = new PfBlob(id, color, pt.coordX(), pt.coordY());
00070       land->link = &lms[i];
00071       landmarks->push_back(land);
00072     }
00073   }
00074   return landmarks;
00075 }
00076 
00077 void PfRoot::deleteLms(vector<PfRoot*> *vec) {
00078   if ( vec != NULL ) {
00079     for ( unsigned int i = 0; i < vec->size(); i++ )
00080       delete (*vec)[i];
00081     delete vec;
00082   }
00083 }
00084 
00085 void PfRoot::findBounds(const vector<PfRoot*> &landmarks, 
00086        coordinate_t &xmin, coordinate_t &ymin,
00087        coordinate_t &xmax, coordinate_t &ymax) {
00088   if ( landmarks.size() == 0 ) {  // should never get here
00089     cout << "Error in PFRoot::findBounds -- empty landmark vector" << endl;
00090     return;
00091   }
00092   xmin = xmax = landmarks[0]->x;
00093   ymin = ymax = landmarks[0]->y;
00094   for ( size_t i = 1; i<landmarks.size(); i++ ) {
00095     if ( (*landmarks[i]).x < xmin )
00096       xmin = landmarks[i]->x;
00097     else if  ( landmarks[i]->x > xmax )
00098       xmax = landmarks[i]->x;
00099     if ( landmarks[i]->y < ymin )
00100       ymin = landmarks[i]->y;
00101     else if  ( landmarks[i]->y > ymax )
00102       ymax = landmarks[i]->y;
00103   }
00104 }
00105 
00106 void PfRoot::printLms(const vector<PfRoot*> &landmarks) {
00107   for (unsigned int i = 0; i<landmarks.size(); i++)
00108     cout << *landmarks[i] << endl;
00109 }
00110 
00111 void PfRoot::printRootInfo(ostream &os) const {
00112     os << "id=" << id
00113        << ", x=" << x
00114        << ", y=" << y;
00115   }
00116 
00117 ostream& operator<<(ostream &os, const PfRoot &obj) {
00118   obj.print(os);
00119   return os;
00120 }
00121 
00122 void PfLine::print(ostream &os) const {
00123   os << "PfLine(";
00124   printRootInfo(os);
00125   os << ", x2=" << x2
00126      << ", y2=" << y2
00127      << ", length=" << length
00128      << ")";
00129 }
00130 
00131 void PfEllipse::print(ostream &os) const {
00132   os << "PfEllipse(";
00133   printRootInfo(os);
00134   os << ")";
00135 }
00136 
00137 void PfPoint::print(ostream &os) const {
00138   os << "PfPoint(";
00139   printRootInfo(os);
00140   os << ")";
00141 }
00142 
00143 void PfBlob::print(ostream &os) const {
00144   os << "PfBlob(";
00145   printRootInfo(os);
00146   os << ")";
00147 }
00148 
00149 
00150 } // namespace