00001
00002
00003 #include <math.h>
00004
00005 #include "Shared/Config.h"
00006 #include "Motion/Kinematics.h"
00007
00008 #include "Shared/Measures.h"
00009 #include "Point.h"
00010 #include "LineData.h"
00011 #include "ShapeTypes.h"
00012 #include "VRmixin.h"
00013
00014 using namespace std;
00015
00016 namespace DualCoding {
00017
00018 Point::Point(void) : coords(4), refFrameType(unspecified) {
00019 coords << 0 << 0 << 0 << 1;
00020 }
00021
00022 Point::Point(coordinate_t const &xp, coordinate_t const &yp, coordinate_t zp,
00023 ReferenceFrameType_t ref) : coords(4), refFrameType(ref) {
00024 coords << xp << yp << zp << 1;
00025 };
00026
00027 Point::Point(const NEWMAT::ColumnVector& c, ReferenceFrameType_t ref) :
00028 coords(4), refFrameType(ref) { coords = c; }
00029
00030
00031
00032 void Point::setCoords(const Point& otherPt) {
00033 coords = otherPt.coords;
00034 }
00035
00036 void Point::setCoords(coordinate_t x, coordinate_t y, coordinate_t z) {
00037 coords << x << y << z << 1;
00038 }
00039
00040
00041 float Point::distanceFrom(const Point& otherPt) const {
00042 return (coords - otherPt.coords).NormFrobenius();
00043 }
00044
00045 float Point::xyDistanceFrom(const Point& other) const {
00046 float dx = coords(1)-other.coords(1);
00047 float dy = coords(2)-other.coords(2);
00048 return sqrt(dx*dx+dy*dy);
00049 }
00050
00051 float Point::xyNorm() const { return sqrt(coords(1)*coords(1) + coords(2)*coords(2)); }
00052
00053 bool Point::isLeftOf(const Point& other, float distance) const {
00054 switch ( refFrameType ) {
00055 case camcentric:
00056 return coordX()+distance < other.coordX();
00057 case egocentric:
00058 return coordY()-distance > other.coordY();
00059 case allocentric:
00060 default:
00061 cout << "Allocentric Point::isLeftOf fudged for now" << endl;
00062 return coordY()-distance > other.coordY();
00063
00064
00065 }
00066 }
00067
00068 bool Point::isRightOf(const Point& other, float distance) const {
00069 return other.isLeftOf(*this,distance); }
00070
00071 bool Point::isAbove(const Point& other,float distance) const {
00072 switch ( refFrameType ) {
00073 case camcentric:
00074 return coordY()+distance < other.coordY();
00075 case egocentric:
00076 return coordX()-distance > other.coordX();
00077 case allocentric:
00078 default:
00079 cout << "Allocentric Point::isLeftOf fudged for now" << endl;
00080 return coordX()-distance > other.coordX();
00081
00082
00083 }
00084 }
00085
00086 bool Point::isBelow(const Point& other, float distance) const {
00087 return other.isAbove(*this,distance); }
00088
00089
00090
00091 void Point::applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref) {
00092 coords = Tmat*coords;
00093 if ( newref != unspecified )
00094 refFrameType = newref;
00095 }
00096
00097 void Point::projectToGround(const NEWMAT::Matrix& camToBase,
00098 const NEWMAT::ColumnVector& groundPlane) {
00099 NEWMAT::Matrix T2 = camToBase.i();
00100 T2.SubMatrix(4,4,1,3)=T2.SubMatrix(1,3,4,4).t()*T2.SubMatrix(1,3,1,3);
00101 T2.SubMatrix(1,3,4,4)=0;
00102
00103
00104 const float normX = 2*(coordX()/VRmixin::camSkS.getWidth()) - 1;
00105 const float normY = 2*(coordY()/VRmixin::camSkS.getHeight()) - 1;
00106 NEWMAT::ColumnVector camera_point(4), camPlane(4);
00107 config->vision.computeRay(normX, normY,
00108 camera_point(1),camera_point(2),camera_point(3));
00109 camera_point(4) = 1;
00110 camPlane = T2*groundPlane;
00111
00112 float denom=0;
00113 for(unsigned int i=1; i<=3; i++)
00114 denom+=camera_point(i)*camPlane(i);
00115 NEWMAT::ColumnVector intersect=camera_point;
00116 if(denom==0)
00117 intersect(4)=0;
00118 else {
00119 float s=camPlane(4)/denom;
00120 for(unsigned int i=1; i<=3; i++)
00121 intersect(i)*=s;
00122 intersect(4)=1;
00123 }
00124 coords = camToBase*intersect;
00125 refFrameType = egocentric;
00126 }
00127
00128 void Point::projectToGround(int xres, int yres,
00129 const NEWMAT::ColumnVector& ground_plane) {
00130 #ifdef TGT_HAS_CAMERA
00131
00132 const float normX = 2*(coordX()/xres) - 1;
00133 const float normY = 2*(coordY()/yres) - 1;
00134 NEWMAT::ColumnVector camera_point(4), ground_point(4);
00135 config->vision.computeRay(normX, normY,
00136 camera_point(1),camera_point(2),camera_point(3));
00137 camera_point(4) = 1;
00138 ground_point =
00139 kine->projectToPlane(CameraFrameOffset, camera_point,
00140 BaseFrameOffset, ground_plane, BaseFrameOffset);
00141
00142
00143
00144 coords(1) = ground_point(1) / ground_point(4);
00145 coords(2) = ground_point(2) / ground_point(4);
00146 coords(3) = ground_point(3) / ground_point(4);
00147 #endif
00148 }
00149
00150
00151
00152 float Point::getHeightAbovePoint(const Point& groundPoint, const NEWMAT::ColumnVector& groundplane) {
00153 #ifndef TGT_HAS_CAMERA
00154 return groundPoint.coordZ();
00155 #else
00156 float camX, camY, camZ;
00157 NEWMAT::Matrix baseToCam = kine->jointToBase(CameraFrameOffset);
00158 NEWMAT::ColumnVector camOffset = baseToCam.Column(4);
00159
00160
00161
00162 camOffset = camOffset - groundplane;
00163 camOffset(4) = 1;
00164
00165 Kinematics::unpack(camOffset, camX, camY, camZ);
00166
00167 float dx = camX - coords(1);
00168 float dy = camY - coords(2);
00169 float dz = camZ - coords(3);
00170
00171 float dHorizCam = sqrt(dx*dx + dy*dy);
00172
00173 Point dP = groundPoint - *this;
00174 float dHorizPoint = sqrt(dP.coordX()*dP.coordX() + dP.coordY()*dP.coordY());
00175
00176 return dz*dHorizPoint/dHorizCam;
00177 #endif
00178 }
00179
00180
00181 Point Point::operator+ (const Point& b) const { return Point(*this) += b; }
00182
00183 Point& Point::operator+= (const Point& b) {
00184 coords += b.coords;
00185 makeRefFrameCompatible(b);
00186 return *this;
00187 }
00188
00189 Point Point::operator- (const Point& b) const { return Point(*this) -= b; }
00190
00191 Point& Point::operator-= (const Point& b) {
00192 coords -= b.coords;
00193 makeRefFrameCompatible(b);
00194 return *this;
00195 }
00196
00197 void Point::makeRefFrameCompatible(const Point &other) {
00198 refFrameType = ( refFrameType == unspecified || refFrameType == other.refFrameType ) ?
00199 other.refFrameType : unspecified;
00200 }
00201
00202 Point Point::operator* (float const b) const { return Point(coords*b,refFrameType); }
00203
00204 Point& Point::operator *= (float const b) {
00205 coords *= b;
00206 return *this;
00207 }
00208
00209
00210 Point Point::operator/ (float const b) const { return Point(coords/b,refFrameType); }
00211
00212 Point& Point::operator /= (float const b) {
00213 coords /= b;
00214 return *this;
00215 }
00216
00217 bool Point::operator ==(const Point& b) const {
00218 return (coordX() == b.coordX()
00219 && coordY() == b.coordY()
00220 && coordZ() == b.coordZ());
00221 }
00222
00223 std::ostream& operator<< (std::ostream& out, const Point &p) {
00224 switch ( p.refFrameType ) {
00225 case unspecified:
00226 out << "u:";
00227 break;
00228 case camcentric:
00229 out << "c:";
00230 break;
00231 case egocentric:
00232 out << "e:";
00233 break;
00234 case allocentric:
00235 out << "a:";
00236 break;
00237 }
00238 out << "[" << p.coordX() << ", " << p.coordY()
00239 << ", " << p.coordZ() << "]";
00240 return out;
00241 }
00242
00243 void Point::printData()
00244 {
00245 cout<<"{"<<coordX()<<","<<coordY()<<","<<coordZ()<<"}";
00246 }
00247
00248 }