ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00007 #ifndef VECTOR2_H 00008 #define VECTOR2_H 00009 00010 namespace argos { 00011 class CRotationMatrix2; 00012 } 00013 00014 #include <argos3/core/utility/math/general.h> 00015 #include <argos3/core/utility/math/angles.h> 00016 #include <argos3/core/utility/string_utilities.h> 00017 #include <iostream> 00018 #include <cmath> 00019 00020 namespace argos { 00021 00025 class CVector2 { 00026 00027 friend class CRotationMatrix2; 00028 friend class CTransformationMatrix2; 00029 00030 public: 00031 00033 static const CVector2 X; 00034 00036 static const CVector2 Y; 00037 00043 CVector2() : 00044 m_fX(0.0), 00045 m_fY(0.0) { 00046 } 00047 00055 CVector2(Real f_x, 00056 Real f_y) : 00057 m_fX(f_x), 00058 m_fY(f_y) { 00059 } 00060 00068 CVector2(Real f_length, 00069 const CRadians& f_angle) : 00070 m_fX(Cos(f_angle) * f_length), 00071 m_fY(Sin(f_angle) * f_length) { 00072 } 00073 00078 inline Real GetX() const { 00079 return m_fX; 00080 } 00081 00086 inline void SetX(Real f_x) { 00087 m_fX = f_x; 00088 } 00089 00094 inline Real GetY() const { 00095 return m_fY; 00096 } 00097 00102 inline void SetY(Real f_y) { 00103 m_fY = f_y; 00104 } 00105 00111 inline void Set(Real f_x, Real f_y) { 00112 m_fX = f_x; 00113 m_fY = f_y; 00114 } 00115 00123 inline void FromPolarCoordinates(Real f_length, 00124 const CRadians& f_angle) { 00125 m_fX = Cos(f_angle) * f_length; 00126 m_fY = Sin(f_angle) * f_length; 00127 } 00128 00133 inline Real SquareLength() const { 00134 return Square(m_fX) + Square(m_fY); 00135 } 00136 00141 inline Real Length() const { 00142 return Sqrt(SquareLength()); 00143 } 00144 00151 inline CVector2& Normalize() { 00152 *this /= Length(); 00153 return *this; 00154 } 00155 00160 inline CRadians Angle() const { 00161 return ATan2(m_fY, m_fX); 00162 } 00163 00169 inline CVector2& Rotate(const CRadians& c_angle) { 00170 Real fSin, fCos; 00171 #ifdef ARGOS_SINCOS 00172 SinCos(c_angle, fSin, fCos); 00173 #else 00174 fSin = Sin(c_angle); 00175 fCos = Cos(c_angle); 00176 #endif 00177 Real fX = m_fX * fCos - m_fY * fSin; 00178 Real fY = m_fX * fSin + m_fY * fCos; 00179 m_fX = fX; 00180 m_fY = fY; 00181 return *this; 00182 } 00183 00189 inline Real DotProduct(const CVector2& c_vector2) const { 00190 return m_fX * c_vector2.m_fX + m_fY * c_vector2.m_fY; 00191 } 00192 00202 inline CVector2& Scale(Real f_scale_x, 00203 Real f_scale_y) { 00204 m_fX *= f_scale_x; 00205 m_fY *= f_scale_y; 00206 return *this; 00207 } 00208 00213 inline CVector2& Perpendicularize() { 00214 Real fNewX = -m_fY; 00215 m_fY = m_fX; 00216 m_fX = fNewX; 00217 return *this; 00218 } 00219 00224 inline CVector2& Absolute() { 00225 m_fX = Abs(m_fX); 00226 m_fY = Abs(m_fY); 00227 return *this; 00228 } 00229 00236 inline bool operator==(const CVector2& c_vector2) { 00237 return (m_fX == c_vector2.m_fX && m_fY == c_vector2.m_fY); 00238 } 00239 00246 inline bool operator!=(const CVector2& c_vector2) { 00247 return (m_fX != c_vector2.m_fX || m_fY != c_vector2.m_fY); 00248 } 00249 00255 inline CVector2& operator+=(const CVector2& c_vector2) { 00256 m_fX += c_vector2.m_fX; 00257 m_fY += c_vector2.m_fY; 00258 return *this; 00259 } 00260 00266 inline CVector2& operator-=(const CVector2& c_vector2) { 00267 m_fX -= c_vector2.m_fX; 00268 m_fY -= c_vector2.m_fY; 00269 return *this; 00270 } 00271 00277 inline CVector2& operator*=(Real f_value) { 00278 m_fX *= f_value; 00279 m_fY *= f_value; 00280 return *this; 00281 } 00282 00288 inline CVector2& operator/=(Real f_value) { 00289 m_fX /= f_value; 00290 m_fY /= f_value; 00291 return *this; 00292 } 00293 00299 inline CVector2 operator+(const CVector2& c_vector2) const { 00300 CVector2 cResult(*this); 00301 cResult += c_vector2; 00302 return cResult; 00303 } 00304 00310 inline CVector2 operator-(const CVector2& c_vector2) const { 00311 CVector2 cResult(*this); 00312 cResult -= c_vector2; 00313 return cResult; 00314 } 00315 00321 inline CVector2 operator*(Real f_value) const { 00322 CVector2 cResult(*this); 00323 cResult *= f_value; 00324 return cResult; 00325 } 00326 00332 inline CVector2 operator/(Real f_value) const { 00333 CVector2 cResult(*this); 00334 cResult /= f_value; 00335 return cResult; 00336 } 00337 00344 inline friend CVector2 operator*(Real f_value, 00345 const CVector2& c_vector2) { 00346 return c_vector2 * f_value; 00347 } 00348 00349 inline CVector2 operator-() const { 00350 return CVector2(-m_fX, -m_fY); 00351 } 00352 00359 inline friend std::ostream& operator<<(std::ostream& c_os, 00360 const CVector2& c_vector2) { 00361 c_os << c_vector2.m_fX << "," 00362 << c_vector2.m_fY; 00363 return c_os; 00364 } 00365 00372 inline friend std::istream& operator>>(std::istream& c_is, 00373 CVector2& c_vector2) { 00374 Real fValues[2]; 00375 ParseValues<Real> (c_is, 2, fValues, ','); 00376 c_vector2.Set(fValues[0], fValues[1]); 00377 return c_is; 00378 } 00379 00380 private: 00381 00383 Real m_fX; 00384 00386 Real m_fY; 00387 00388 }; 00389 00390 /****************************************/ 00391 /****************************************/ 00392 00399 inline Real SquareDistance(const CVector2& c_v1, const CVector2& c_v2) { 00400 return (c_v1 - c_v2).SquareLength(); 00401 } 00402 00409 inline Real Distance(const CVector2& c_v1, const CVector2& c_v2) { 00410 return (c_v1 - c_v2).Length(); 00411 } 00412 00413 /****************************************/ 00414 /****************************************/ 00415 00416 } 00417 00418 #endif