00001
00002
00003 #include <vector>
00004 #include <iostream>
00005 #include <math.h>
00006
00007 #include "BaseData.h"
00008 #include "Point.h"
00009 #include "Shared/Measures.h"
00010 #include "ShapeTypes.h"
00011
00012 #include "SketchSpace.h"
00013 #include "Sketch.h"
00014 #include "ShapeSpace.h"
00015 #include "ShapeRoot.h"
00016
00017 #include "AgentData.h"
00018 #include "ShapeAgent.h"
00019
00020 using namespace std;
00021
00022 namespace DualCoding {
00023
00024 DATASTUFF_CC(AgentData);
00025
00026 AgentData::AgentData(ShapeSpace& _space, const Point &c)
00027 : BaseData(_space,agentDataType), center_pt(c), orientation_pt(c), orientation(0)
00028 { mobile = true; }
00029
00030 AgentData::AgentData(const AgentData& otherData)
00031 : BaseData(otherData),
00032 center_pt(otherData.center_pt),
00033 orientation_pt(otherData.orientation_pt),
00034 orientation(otherData.orientation)
00035 { mobile = true; }
00036
00037 bool AgentData::isMatchFor(const ShapeRoot&) const {
00038 return false;
00039 }
00040
00041
00042 void
00043 AgentData::printParams() const {
00044 cout << "Type = " << getTypeName();
00045 cout << "Shape ID = " << getId() << endl;
00046 cout << "Parent ID = " << getParentId() << endl;
00047
00048
00049 cout << endl;
00050 cout << "center{" << getCentroid().coords(1) << ", " << getCentroid().coords(2) << "}" << endl;
00051
00052 cout << "orientation = " << orientation << endl;
00053
00054 printf("color = %d %d %d\n",getColor().red,getColor().green,getColor().blue);
00055
00056 cout << "mobile = " << getMobile() << endl;
00057 cout << "viewable = " << isViewable() << endl;
00058 }
00059
00060
00061
00062 void AgentData::applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref) {
00063
00064
00065 center_pt.applyTransform(Tmat,newref);
00066 orientation_pt.applyTransform(Tmat,newref);
00067 orientation = orientation - (AngTwoPi)atan2(Tmat(1,2),Tmat(1,1));
00068
00069 }
00070
00071
00072
00073 void
00074 AgentData::setOrientation(AngTwoPi _orientation) {
00075 orientation = _orientation;
00076 orientation_pt.setCoords(getCentroid().coords(1) + 0.5*cos(orientation),
00077 getCentroid().coords(2) + 0.5*sin(orientation));
00078 deleteRendering();
00079 }
00080
00081
00082 void AgentData::updateOrientation() {
00083 Point heading = orientation_pt-center_pt;
00084 orientation = atan2(heading.coordY(),heading.coordX());
00085 }
00086
00087 void AgentData::projectToGround(const NEWMAT::Matrix& camToBase,
00088 const NEWMAT::ColumnVector& groundplane) {
00089 center_pt.projectToGround(camToBase,groundplane);
00090 orientation_pt.projectToGround(camToBase,groundplane);
00091 updateOrientation();
00092 }
00093
00094 bool AgentData::updateParams(const ShapeRoot& other, bool force) {
00095 if (isMatchFor(other) || force) {
00096 const AgentData& other_agent = ShapeRootTypeConst(other,AgentData).getData();
00097 int other_conf = other_agent.getConfidence();
00098 if (other_conf <= 0)
00099 return true;
00100 center_pt = (center_pt*confidence + other_agent.getCentroid()*other_conf) / (confidence+other_conf);
00101 orientation = orientation*((orientation_t)confidence/(confidence+other_conf))
00102 + other_agent.getOrientation()*((orientation_t)confidence/(confidence+other_conf));
00103 return true;
00104 }
00105 return false;
00106 }
00107
00108
00109 Sketch<bool>* AgentData::render() const {
00110 Sketch<bool>* draw_result =
00111 new Sketch<bool>(space->getDualSpace(), "render("+getName()+")");
00112 draw_result = 0;
00113
00114
00115
00116 int cx = int(getCentroid().coords(1));
00117 int cy = int(getCentroid().coords(2));
00118
00119
00120 const float a = 2.0;
00121 const float b = 2.0;
00122 const float x_skip = atan(1/(0.5*a));
00123 for(float x = (cx-a); x<(cx+a); x+=x_skip) {
00124 float y_y0_sq = (b*b) * (1 - (x-cx)*(x-cx)/(a*a));
00125 if(y_y0_sq > 0) {
00126 int y_bot = cy + (int)(sqrt(y_y0_sq));
00127 int y_top = cy - (int)(sqrt(y_y0_sq));
00128 (*draw_result)((int)x,y_bot) = true;
00129 (*draw_result)((int)x,y_top) = true;
00130 }
00131 }
00132 (*draw_result)(cx-(int)a, cy) = true;
00133 (*draw_result)(cx+(int)a, cy) = true;
00134 return draw_result;
00135 }
00136
00137 }