Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

LocalizationParticleData.cc

Go to the documentation of this file.
00001 //-*-c++-*-
00002 
00003 #include <math.h>
00004 
00005 #include "Sketch.h"
00006 #include "LocalizationParticleData.h"
00007 #include "ShapeLocalizationParticle.h"
00008 
00009 using namespace std;
00010 
00011 namespace DualCoding {
00012 
00013 LocalizationParticleData::LocalizationParticleData(ShapeSpace& _space, 
00014                const Point &_location, 
00015                const AngTwoPi _orientation,
00016                const float _weight) :
00017   BaseData(_space, getStaticType()), location(_location), orientation(_orientation), weight(_weight) {}
00018 
00019 LocalizationParticleData::LocalizationParticleData(ShapeSpace &_space, 
00020                const PFShapeLocalization::particle_type& particle) :
00021   BaseData(_space, getStaticType()), 
00022   location(Point(particle.x,particle.y,0,_space.getRefFrameType())),
00023   orientation(particle.theta),
00024   weight(particle.weight)
00025 {}
00026 
00027 LocalizationParticleData::LocalizationParticleData(const LocalizationParticleData &other) :
00028   BaseData(other), location(other.location), orientation(other.orientation), weight(other.weight) {}
00029 
00030 
00031 DATASTUFF_CC(LocalizationParticleData);
00032 
00033 bool LocalizationParticleData::updateParams(const ShapeRoot&, bool) { return true; }
00034 
00035 void LocalizationParticleData::applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref) {
00036   location.applyTransform(Tmat,newref);
00037   orientation += atan2(Tmat(1,2),Tmat(1,1));
00038 }
00039 
00040 void LocalizationParticleData::printParams() const {
00041   cout << "Type = " << getTypeName() << "  ID=" << getId() << "  ParentID=" << getParentId() << endl;
00042   cout << "  location{" << location.coordX() << ", " << location.coordY() << "}"
00043        << ", orientation = " << orientation << endl;
00044 }
00045 
00046 bool LocalizationParticleData::isMatchFor(const ShapeRoot& other) const {
00047   if (!(isSameTypeAs(other) && isSameColorAs(other)))
00048     return false;
00049   const Shape<LocalizationParticleData>& other_particle = ShapeRootTypeConst(other,LocalizationParticleData);
00050   float dist = location.distanceFrom(other_particle->getCentroid());
00051   return dist < 20; // *** DST hack
00052 }
00053 
00054 LocalizationParticleData& LocalizationParticleData::operator=(const LocalizationParticleData &other) {
00055   if ( this != &other ) {
00056     BaseData::operator=(other);
00057     location = other.location;
00058     orientation = other.orientation;
00059     weight = other.weight;
00060   }
00061   return *this;
00062 }
00063 
00064 //! Render into a sketch space and return a pointer. (Private.)
00065 Sketch<bool>* LocalizationParticleData::render() const {
00066   SketchSpace &SkS = space->getDualSpace();
00067   NEWMAT::ColumnVector ctr(getCentroid().getCoords());
00068   SkS.applyTmat(ctr);
00069   int const cx = int(ctr(1));
00070   int const cy = int(ctr(2));
00071   Sketch<bool>& draw_result = 
00072     *new Sketch<bool>(SkS, "render("+getName()+")");
00073   draw_result = false;
00074   draw_result(cx,cy) = true;  
00075   return &draw_result;
00076 }
00077 
00078 
00079 } // namespace
00080 

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