ray2.h
Go to the documentation of this file.
1 
7 #ifndef RAY2_H
8 #define RAY2_H
9 
10 namespace argos {
11  class CRay2;
12 }
13 
14 #include <argos3/core/utility/math/vector2.h>
15 
16 namespace argos {
17 
18  class CRay2 {
19 
20  public:
21 
22  CRay2() {
23  }
24 
25  CRay2(const CVector2& c_start,
26  const CVector2& c_end) :
27  m_cStart(c_start), m_cEnd(c_end) {
28  }
29 
30  CRay2(const CVector2& c_start,
31  const CVector2& c_direction,
32  Real f_length) {
33  Set(c_start, c_direction, f_length);
34  }
35 
36  inline const CVector2& GetStart() const {
37  return m_cStart;
38  }
39 
40  inline const CVector2& GetEnd() const {
41  return m_cEnd;
42  }
43 
44  inline void SetStart(const CVector2& c_start) {
45  m_cStart = c_start;
46  }
47 
48  inline void SetEnd(const CVector2& c_end) {
49  m_cEnd = c_end;
50  }
51 
52  inline void Set(const CVector2& c_start, const CVector2& c_end) {
53  m_cStart = c_start;
54  m_cEnd = c_end;
55  }
56 
57  inline void Set(const CVector2& c_start, const CVector2& c_direction, Real f_length) {
58  m_cStart = c_start;
59  /* Same as, but faster than
60  m_cEnd = m_cStart + f_length * c_direction; */
61  m_cEnd = m_cStart;
62  m_cEnd += f_length * c_direction;
63  }
64 
65  inline void GetDirection(CVector2& c_buffer) const {
66  /* Same as, but faster than
67  c_buffer = (m_cEnd - m_cStart).Normalize(); */
68  c_buffer = m_cEnd;
69  c_buffer -= m_cStart;
70  c_buffer.Normalize();
71  }
72 
73  inline void GetInverseDirection(CVector2& c_buffer) const {
74  /* Same as, but faster than
75  c_buffer = (m_cEnd - m_cStart).Normalize(); */
76  c_buffer = m_cStart;
77  c_buffer -= m_cEnd;
78  c_buffer.Normalize();
79  }
80 
81  inline Real GetLength() const {
82  return (m_cEnd - m_cStart).Length();
83  }
84 
85  inline void ToVector(CVector2& c_buffer) const {
86  /* Same as, but faster than
87  c_buffer = m_cEnd - m_cStart; */
88  c_buffer = m_cEnd;
89  c_buffer -= m_cStart;
90  }
91 
92  /* Returns the point on the line corresponding to f_t */
93  inline void GetPoint(CVector2& c_point,
94  Real f_t) const {
95  c_point.SetX(m_cStart.GetX() + f_t * (m_cEnd.GetX() - m_cStart.GetX()));
96  c_point.SetY(m_cStart.GetY() + f_t * (m_cEnd.GetY() - m_cStart.GetY()));
97  }
98 
99  /* Returns the distance from the ray2 start to the point on the line corresponding to f_t */
100  inline Real GetDistance(Real f_t) const {
101  return ::sqrt(Square(f_t * (m_cEnd.GetX() - m_cStart.GetX())) +
102  Square(f_t * (m_cEnd.GetY() - m_cStart.GetY())));
103  }
104 
105  /* Returns <tt>true</tt> if the passed ray intersects the current one. */
106  inline bool Intersects(const CRay2& c_ray) const {
107  Real fDiscriminant =
108  (c_ray.m_cEnd.GetY() - c_ray.m_cStart.GetY()) *
109  (m_cEnd.GetX() - m_cStart.GetX()) -
110  (c_ray.m_cEnd.GetX() - c_ray.m_cStart.GetX()) *
111  (m_cEnd.GetY() - m_cStart.GetY());
112  if(Abs(fDiscriminant) < 1e-4) {
113  /* The rays are parallel */
114  return false;
115  }
116  /* If we get here, we know the rays are not parallel */
117  /* Calculate value of T on ray 1 for the intersection point */
118  Real fT1 =
119  (m_cStart.GetX() - c_ray.m_cStart.GetX()) *
120  (c_ray.m_cEnd.GetY() - c_ray.m_cStart.GetY()) -
121  (m_cStart.GetY() - c_ray.m_cStart.GetY()) *
122  (c_ray.m_cEnd.GetX() - c_ray.m_cStart.GetX());
123  /* If T on ray 1 is outside the ray, no intersection */
124  if(fT1 < 0.0 || fT1 > 1.0) return false;
125  /* Calculate value of T on ray 2 for the intersection point */
126  Real fT2;
127  if(Abs(c_ray.m_cEnd.GetY() - c_ray.m_cStart.GetY()) > 1e-4) {
128  /* Ray 2 is not vertical */
129  fT2 =
130  (m_cStart.GetY() - c_ray.m_cStart.GetY()) +
131  (m_cEnd.GetY() - m_cStart.GetY()) *
132  fT1 /
133  (c_ray.m_cEnd.GetY() - c_ray.m_cStart.GetY());
134  }
135  else {
136  /* Ray 2 is vertical */
137  fT2 =
138  (m_cStart.GetX() - c_ray.m_cStart.GetX()) +
139  (m_cEnd.GetX() - m_cStart.GetX()) *
140  fT1 /
141  (c_ray.m_cEnd.GetX() - c_ray.m_cStart.GetX());
142  }
143  /* If T on ray 2 is inside the ray, intersection */
144  return(fT2 >= 0.0 && fT2 <= 1.0);
145  }
146 
153  inline friend std::ostream& operator<<(std::ostream& c_os,
154  const CRay2& c_ray) {
155  c_os << c_ray.GetStart() << " -> " << c_ray.GetEnd();
156  return c_os;
157  }
158 
159  private:
160 
161  CVector2 m_cStart;
162  CVector2 m_cEnd;
163 
164  };
165 
166 }
167 
168 #endif
Real GetDistance(Real f_t) const
Definition: ray2.h:100
T Square(const T &t_v)
Returns the square of the value of the passed argument.
Definition: general.h:128
CRay2()
Definition: ray2.h:22
CRay2(const CVector2 &c_start, const CVector2 &c_direction, Real f_length)
Definition: ray2.h:30
void GetPoint(CVector2 &c_point, Real f_t) const
Definition: ray2.h:93
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector2.h:78
T Abs(const T &t_v)
Returns the absolute value of the passed argument.
Definition: general.h:25
void Set(const CVector2 &c_start, const CVector2 &c_end)
Definition: ray2.h:52
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector2.h:94
void ToVector(CVector2 &c_buffer) const
Definition: ray2.h:85
void GetDirection(CVector2 &c_buffer) const
Definition: ray2.h:65
void SetStart(const CVector2 &c_start)
Definition: ray2.h:44
void SetEnd(const CVector2 &c_end)
Definition: ray2.h:48
void SetX(Real f_x)
Sets the x coordinate of this vector.
Definition: vector2.h:86
void GetInverseDirection(CVector2 &c_buffer) const
Definition: ray2.h:73
A 2D vector class.
Definition: vector2.h:25
CRay2(const CVector2 &c_start, const CVector2 &c_end)
Definition: ray2.h:25
void Set(const CVector2 &c_start, const CVector2 &c_direction, Real f_length)
Definition: ray2.h:57
void SetY(Real f_y)
Sets the y coordinate of this vector.
Definition: vector2.h:102
Real GetLength() const
Definition: ray2.h:81
const CVector2 & GetEnd() const
Definition: ray2.h:40
CVector2 & Normalize()
Normalizes this vector.
Definition: vector2.h:151
const CVector2 & GetStart() const
Definition: ray2.h:36
friend std::ostream & operator<<(std::ostream &c_os, const CRay2 &c_ray)
Serializes the contents of the passed ray onto a stream.
Definition: ray2.h:153
bool Intersects(const CRay2 &c_ray) const
Definition: ray2.h:106
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12