Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

ParticleShapes.cc

Go to the documentation of this 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 void PfRoot::loadLms(const std::vector<ShapeRoot> &lms, bool isWorld, std::vector<PfRoot*>& landmarks){
00020   int id;
00021   int type;
00022   rgb color;
00023   bool mobile;
00024   deleteLms(landmarks);
00025   landmarks.reserve(lms.size());
00026   for (unsigned int i = 0; i<lms.size(); i++){
00027     type = lms[i]->getType();
00028     color = lms[i]->getColor();
00029     mobile = lms[i]->getMobile();
00030     if (type == lineDataType) {
00031       const Shape<LineData> &line = ShapeRootTypeConst(lms[i],LineData);
00032       id = line->getId();
00033       const EndPoint &pt1 = line->end1Pt();
00034       const EndPoint &pt2 = line->end2Pt();
00035       PfLine *land = new PfLine(id, color, mobile, pt1.coordX(), pt1.coordY(), 
00036         pt2.coordX(), pt2.coordY(), pt1.isValid(), pt2.isValid());
00037       land->link = &lms[i];
00038       land->orientation = atan2(pt2.coordY()-pt1.coordY(), pt2.coordX()-pt1.coordX());
00039       land->length = sqrt((pt2.coordX()-pt1.coordX())*(pt2.coordX()-pt1.coordX()) +
00040         (pt2.coordY()-pt1.coordY())*(pt2.coordY()-pt1.coordY()));
00041       landmarks.push_back(land);
00042       if ( isWorld ) {
00043   PfLine *land2 = new PfLine(id, color, mobile, pt2.coordX(), pt2.coordY(),
00044            pt1.coordX(), pt1.coordY(), pt2.isValid(), pt1.isValid());
00045   land2->link = &lms[i];
00046   land2->orientation = land->orientation;
00047   land2->length = land->length;
00048   landmarks.push_back(land2);
00049       }
00050     }
00051     else if (type == ellipseDataType) {
00052       const Shape<EllipseData> &ellipse = ShapeRootTypeConst(lms[i],EllipseData);
00053       id = ellipse->getId();
00054       const Point &pt = ellipse->getCentroid();
00055       PfEllipse* land = new PfEllipse(id, color, mobile, pt.coordX(), pt.coordY());
00056       land->link = &lms[i];
00057       landmarks.push_back(land);
00058     }
00059     else if (type == pointDataType) {
00060       const Shape<PointData> &point = ShapeRootTypeConst(lms[i],PointData);
00061       id = point->getId();
00062       const Point &pt = point->getCentroid();
00063       PfPoint* land = new PfPoint(id, color, mobile, pt.coordX(), pt.coordY());
00064       land->link = &lms[i];
00065       landmarks.push_back(land);
00066     }
00067     else if (type == blobDataType) {
00068       const Shape<BlobData> &blob = ShapeRootTypeConst(lms[i],BlobData);
00069       id = blob->getId();
00070       Point pt = blob->getCentroid();
00071       PfBlob* land = new PfBlob(id, color, mobile, pt.coordX(), pt.coordY());
00072       land->link = &lms[i];
00073       landmarks.push_back(land);
00074     }
00075   }
00076 }
00077 
00078 void PfRoot::deleteLms(std::vector<PfRoot*>& vec) {
00079   for ( unsigned int i = 0; i < vec.size(); i++ )
00080     delete vec[i];
00081   vec.clear();
00082 }
00083 
00084 void PfRoot::findBounds(const std::vector<PfRoot*> &landmarks, 
00085        coordinate_t &xmin, coordinate_t &ymin,
00086        coordinate_t &xmax, coordinate_t &ymax) {
00087   if ( landmarks.size() == 0 ) {  // should never get here
00088     cout << "Error in PFRoot::findBounds -- empty landmark vector" << endl;
00089     return;
00090   }
00091   xmin = xmax = landmarks[0]->x;
00092   ymin = ymax = landmarks[0]->y;
00093   for ( size_t i = 1; i<landmarks.size(); i++ ) {
00094     if ( (*landmarks[i]).x < xmin )
00095       xmin = landmarks[i]->x;
00096     else if  ( landmarks[i]->x > xmax )
00097       xmax = landmarks[i]->x;
00098     if ( landmarks[i]->y < ymin )
00099       ymin = landmarks[i]->y;
00100     else if  ( landmarks[i]->y > ymax )
00101       ymax = landmarks[i]->y;
00102   }
00103 }
00104 
00105 void PfRoot::printLms(const std::vector<PfRoot*> &landmarks) {
00106   for (unsigned int i = 0; i<landmarks.size(); i++)
00107     cout << *landmarks[i] << endl;
00108 }
00109 
00110 void PfRoot::printRootInfo(std::ostream &os) const {
00111     os << "id=" << id
00112        << ", x=" << x
00113        << ", y=" << y
00114        << ", mobile=" << mobile;
00115   }
00116 
00117 ostream& operator<<(std::ostream &os, const PfRoot &obj) {
00118   obj.print(os);
00119   return os;
00120 }
00121 
00122 void PfLine::print(std::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(std::ostream &os) const {
00132   os << "PfEllipse(";
00133   printRootInfo(os);
00134   os << ")";
00135 }
00136 
00137 void PfPoint::print(std::ostream &os) const {
00138   os << "PfPoint(";
00139   printRootInfo(os);
00140   os << ")";
00141 }
00142 
00143 void PfBlob::print(std::ostream &os) const {
00144   os << "PfBlob(";
00145   printRootInfo(os);
00146   os << ")";
00147 }
00148 
00149 
00150 } // namespace

DualCoding 4.0
Generated Thu Nov 22 00:52:36 2007 by Doxygen 1.5.4