00001
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;
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
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 }
00080