ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00007 #ifndef RAY3_H 00008 #define RAY3_H 00009 00010 namespace argos { 00011 class CRay3; 00012 class CPlane; 00013 } 00014 00015 #include <argos3/core/utility/math/vector3.h> 00016 00017 namespace argos { 00018 00019 class CRay3 { 00020 00021 public: 00022 00023 CRay3() { 00024 } 00025 00026 CRay3(const CVector3& c_start, 00027 const CVector3& c_end) : 00028 m_cStart(c_start), m_cEnd(c_end) { 00029 } 00030 00031 CRay3(const CVector3& c_start, 00032 const CVector3& c_direction, 00033 Real f_length) { 00034 Set(c_start, c_direction, f_length); 00035 } 00036 00037 inline CVector3& GetStart() { 00038 return m_cStart; 00039 } 00040 00041 inline const CVector3& GetStart() const { 00042 return m_cStart; 00043 } 00044 00045 inline CVector3& GetEnd() { 00046 return m_cEnd; 00047 } 00048 00049 inline const CVector3& GetEnd() const { 00050 return m_cEnd; 00051 } 00052 00053 inline void SetStart(const CVector3& c_start) { 00054 m_cStart = c_start; 00055 } 00056 00057 inline void SetEnd(const CVector3& c_end) { 00058 m_cEnd = c_end; 00059 } 00060 00061 inline void Set(const CVector3& c_start, const CVector3& c_end) { 00062 m_cStart = c_start; 00063 m_cEnd = c_end; 00064 } 00065 00066 inline void Set(const CVector3& c_start, const CVector3& c_direction, Real f_length) { 00067 m_cStart = c_start; 00068 /* Same as, but faster than 00069 m_cEnd = m_cStart + f_length * c_direction; */ 00070 m_cEnd = m_cStart; 00071 m_cEnd += f_length * c_direction; 00072 } 00073 00074 inline void GetDirection(CVector3& c_buffer) const { 00075 /* Same as, but faster than 00076 c_buffer = (m_cEnd - m_cStart).Normalize(); */ 00077 c_buffer = m_cEnd; 00078 c_buffer -= m_cStart; 00079 c_buffer.Normalize(); 00080 } 00081 00082 inline void GetInverseDirection(CVector3& c_buffer) const { 00083 /* Same as, but faster than 00084 c_buffer = (m_cEnd - m_cStart).Normalize(); */ 00085 c_buffer = m_cStart; 00086 c_buffer -= m_cEnd; 00087 c_buffer.Normalize(); 00088 } 00089 00090 inline Real GetLength() const { 00091 return (m_cEnd - m_cStart).Length(); 00092 } 00093 00094 inline CVector3& ToVector(CVector3& c_buffer) const { 00095 /* Same as, but faster than 00096 c_buffer = m_cEnd - m_cStart; */ 00097 c_buffer = m_cEnd; 00098 c_buffer -= m_cStart; 00099 return c_buffer; 00100 } 00101 00102 /* Returns the point on the line corresponding to f_t */ 00103 inline void GetPoint(CVector3& c_point, 00104 Real f_t) const { 00105 c_point.SetX(m_cStart.GetX() + f_t * (m_cEnd.GetX() - m_cStart.GetX())); 00106 c_point.SetY(m_cStart.GetY() + f_t * (m_cEnd.GetY() - m_cStart.GetY())); 00107 c_point.SetZ(m_cStart.GetZ() + f_t * (m_cEnd.GetZ() - m_cStart.GetZ())); 00108 } 00109 00110 /* Returns the distance from the ray3 start to the point on the line corresponding to f_t */ 00111 inline Real GetDistance(Real f_t) const { 00112 return ::sqrt(Square(f_t * (m_cEnd.GetX() - m_cStart.GetX())) + 00113 Square(f_t * (m_cEnd.GetY() - m_cStart.GetY())) + 00114 Square(f_t * (m_cEnd.GetZ() - m_cStart.GetZ()))); 00115 } 00116 00117 /* 00118 * Calculates the ray-plane intersection point. 00119 * @param c_plane The plane whose intersection with this ray must be calculated 00120 * @param c_point The resulting intersection point is returned here 00121 * @return true if intersection occurred, false if ray and plane are parallel or ray lies on plane 00122 */ 00123 bool Intersects(const CPlane& c_plane, 00124 CVector3& c_point) const; 00125 00126 private: 00127 00128 CVector3 m_cStart; 00129 CVector3 m_cEnd; 00130 00131 }; 00132 00133 } 00134 00135 #endif