ARGoS  3
A parallel, multi-engine simulator for swarm robotics
core/utility/math/ray3.h
Go to the documentation of this file.
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