00001 #include "point.h" 00002 00003 Point2D::Point2D(): 00004 x(0.0), 00005 y(0.0) 00006 { 00007 00008 } 00009 00010 Point2D::Point2D(double _x, double _y): 00011 x(_x), 00012 y(_y) 00013 { 00014 00015 } 00016 00017 OrientedPoint2D::OrientedPoint2D(): 00018 Point2D(0.0,0.0), 00019 theta(0.0) 00020 { 00021 00022 } 00023 00024 OrientedPoint2D::OrientedPoint2D(double _x, double _y, double _theta): 00025 Point2D(_x,_y), 00026 theta(_theta) 00027 { 00028 00029 } 00030 00031 OrientedPoint2D OrientedPoint2D::ominus() const 00032 { 00033 double ctheta = cos(theta), stheta = sin(theta); 00034 return OrientedPoint2D(-x * ctheta - y * stheta, 00035 x * stheta - y * ctheta, 00036 -theta); 00037 } 00038 00039 OrientedPoint2D OrientedPoint2D::ominus(const OrientedPoint2D& point) const 00040 { 00041 double ctheta = cos(theta), stheta = sin(theta); 00042 return OrientedPoint2D( (point.x - x) * ctheta + (point.y - y) * stheta, 00043 -(point.x - x) * stheta + (point.y - y) * ctheta, 00044 point.theta - theta); 00045 00046 } 00047 00048 Point2D OrientedPoint2D::ominus(const Point2D& point) const 00049 { 00050 double ctheta = cos(theta), stheta = sin(theta); 00051 return Point2D( (point.x - x) * ctheta + (point.y - y) * stheta, 00052 -(point.x - x) * stheta + (point.y - y) * ctheta); 00053 00054 } 00055 00056 OrientedPoint2D OrientedPoint2D::oplus(const OrientedPoint2D& point) const 00057 { 00058 double ctheta = cos(theta), stheta = sin(theta); 00059 return OrientedPoint2D(x + point.x * ctheta - point.y * stheta, 00060 y + point.x * stheta + point.y * ctheta, 00061 theta + point.theta); 00062 } 00063 00064 Point2D OrientedPoint2D::oplus(const Point2D& point) const 00065 { 00066 double ctheta = cos(theta), stheta = sin(theta); 00067 return Point2D(x + point.x * ctheta - point.y * stheta, 00068 y + point.x * stheta + point.y * ctheta); 00069 } 00070 00071 00072 double normAngle(double angle, double base) { 00073 double pi2 = 2*M_PI; 00074 double min2pi = base + pi2; 00075 while(angle>=min2pi) angle -= pi2; 00076 while(angle<base) angle += pi2; 00077 return angle; 00078 } 00079