DualCoding: AgentData.cc Source File

00001 //-*-c++-*-
00002 
00003 #include <vector>
00004 #include <iostream>
00005 #include <math.h>
00006 
00007 #include "BaseData.h"    // superclass
00008 #include "Point.h"       // Point data member
00009 #include "Measures.h"    // coordinate_t; AngPi data member
00010 #include "ShapeTypes.h"  // agentDataType
00011 
00012 #include "SketchSpace.h"
00013 #include "Sketch.h"
00014 #include "ShapeSpace.h"  // required by DATASTUFF_CC
00015 #include "ShapeRoot.h"   // required by DATASTUFF_CC
00016 
00017 #include "AgentData.h"
00018 #include "ShapeAgent.h"
00019 
00020 namespace DualCoding {
00021 
00022 DATASTUFF_CC(AgentData);
00023 
00024 AgentData::AgentData(ShapeSpace& _space, const Point &c) 
00025   : BaseData(_space,agentDataType), center_pt(c), orientation_pt(c), orientation(0)
00026 { mobile = true; }
00027   
00028 AgentData::AgentData(const AgentData& otherData)
00029   : BaseData(otherData), 
00030     center_pt(otherData.center_pt), 
00031     orientation_pt(otherData.orientation_pt),
00032     orientation(otherData.orientation)
00033 { mobile = true; }
00034 
00035 bool AgentData::isMatchFor(const ShapeRoot&) const {
00036   return false;
00037 }
00038 
00039 //! Print information about this shape. (Virtual in BaseData.)
00040 void
00041 AgentData::printParams() const {
00042   cout << "Type = " << getTypeName();
00043   cout << "Shape ID = " << getId() << endl;
00044   cout << "Parent ID = " << getParentId() << endl;
00045 
00046   // Print critical points.
00047   cout << endl;
00048   cout << "center{" << getCentroid().coords(1) << ", " << getCentroid().coords(2) << "}" << endl;
00049     
00050   cout << "orientation = " << orientation << endl;
00051   //cout << "color = " << getColor() << endl;
00052   printf("color = %d %d %d\n",getColor().red,getColor().green,getColor().blue);
00053 
00054   cout << "mobile = " << isMobile() << endl;
00055   cout << "viewable = " << isViewable() << endl;
00056 }
00057 
00058 
00059 //! Transformations. (Virtual in BaseData.)
00060 void AgentData::applyTransform(const NEWMAT::Matrix& Tmat) {
00061   //  cout << "AgentData::applyTransform: " << getId() << endl;
00062   //  cout << Tmat << endl;
00063   center_pt.applyTransform(Tmat);
00064   orientation_pt.applyTransform(Tmat);
00065   orientation = orientation - (AngTwoPi)atan2(Tmat(1,2),Tmat(1,1));
00066   //  updateOrientation();
00067 }
00068 
00069 //! Functions to set properties.
00070 //{
00071 void
00072 AgentData::setOrientation(AngTwoPi _orientation) {
00073   orientation = _orientation;
00074   orientation_pt.setCoords(getCentroid().coords(1) + 0.5*cos(orientation),
00075          getCentroid().coords(2) + 0.5*sin(orientation));
00076   deleteRendering();
00077 }
00078 //}
00079 
00080 void AgentData::updateOrientation() {
00081   Point heading = orientation_pt-center_pt;
00082   orientation = atan2(heading.coordY(),heading.coordX());
00083 }
00084 
00085 void AgentData::projectToGround(const NEWMAT::Matrix& camToBase,
00086         const NEWMAT::ColumnVector& groundplane) {
00087   center_pt.projectToGround(camToBase,groundplane);
00088   orientation_pt.projectToGround(camToBase,groundplane);
00089   updateOrientation();
00090 }
00091 
00092 bool AgentData::updateParams(const ShapeRoot& other, bool force) {
00093   if (isMatchFor(other) || force) {
00094     const AgentData& other_agent = ShapeRootTypeConst(other,AgentData).getData();
00095     int other_conf = other_agent.getConfidence();
00096     if (other_conf <= 0)
00097       return true;
00098     center_pt = (center_pt*confidence + other_agent.getCentroid()*other_conf) / (confidence+other_conf);
00099     orientation = orientation*((orientation_t)confidence/(confidence+other_conf))
00100       + other_agent.getOrientation()*((orientation_t)confidence/(confidence+other_conf));
00101     return true;
00102   }
00103   return false;
00104 }
00105 
00106 //! Render into a sketch space and return reference. (Private.)
00107 Sketch<bool>* AgentData::render() const {
00108   Sketch<bool>* draw_result =
00109     new Sketch<bool>(space->getDualSpace(), "render("+getName()+")");
00110   draw_result = 0;
00111 
00112   // JJW This does not take orientation into account.
00113   // should probably take principal axis into account later on
00114   int cx = int(getCentroid().coords(1));
00115   int cy = int(getCentroid().coords(2));
00116   
00117   // Sure the agent rendering is terribly inefficient, but it works
00118   const float a = 2.0;
00119   const float b = 2.0;
00120   const float x_skip = atan(1/(0.5*a)); // minimum x-diff w/o gaps 
00121   for(float x = (cx-a); x<(cx+a); x+=x_skip) {
00122     float y_y0_sq = (b*b) * (1 - (x-cx)*(x-cx)/(a*a));
00123     if(y_y0_sq > 0) {
00124       int y_bot = cy + (int)(sqrt(y_y0_sq));
00125       int y_top = cy - (int)(sqrt(y_y0_sq));
00126       (*draw_result)((int)x,y_bot) = true;
00127       (*draw_result)((int)x,y_top) = true;
00128     }
00129   }
00130   (*draw_result)(cx-(int)a, cy) = true; // fill in "holes" at ends
00131   (*draw_result)(cx+(int)a, cy) = true;
00132   return draw_result;
00133 }
00134 
00135 } // namespace