box.cpp
Go to the documentation of this file.
1 #include "box.h"
2 #include "ray3.h"
3 
4 namespace argos {
5 
6  /****************************************/
7  /****************************************/
8 
9  bool CBox::Intersects(Real& f_t_on_ray,
10  const CRay3& c_ray) {
11  /* Transform the ray so the origin is the axis-aligned box base */
12  CVector3 cRayStart = c_ray.GetStart();
13  CVector3 cInvRayDir;
14  c_ray.GetDirection(cInvRayDir);
15  cRayStart -= m_cBasePos;
16  cRayStart.Rotate(m_cOrientation.Inverse());
17  cInvRayDir.Rotate(m_cOrientation.Inverse());
18  /* Calculate the inverse direction */
19  cInvRayDir.Set(1.0 / cInvRayDir.GetX(),
20  1.0 / cInvRayDir.GetY(),
21  1.0 / cInvRayDir.GetZ());
22  /* X plane */
23  Real fT1 = (m_cXBounds.GetMin() - cRayStart.GetX()) * cInvRayDir.GetX();
24  Real fT2 = (m_cXBounds.GetMax() - cRayStart.GetX()) * cInvRayDir.GetX();
25  Real fTmin = Min(fT1, fT2);
26  Real fTmax = Max(fT1, fT2);
27  /* Y plane */
28  fT1 = (m_cYBounds.GetMin() - cRayStart.GetY()) * cInvRayDir.GetY();
29  fT2 = (m_cYBounds.GetMax() - cRayStart.GetY()) * cInvRayDir.GetY();
30  fTmin = Max(fTmin, Min(fT1, fT2));
31  fTmax = Min(fTmax, Max(fT1, fT2));
32  if(fTmin > fTmax) return false;
33  /* Z plane */
34  fT1 = (m_cZBounds.GetMin() - cRayStart.GetZ()) * cInvRayDir.GetZ();
35  fT2 = (m_cZBounds.GetMax() - cRayStart.GetZ()) * cInvRayDir.GetZ();
36  fTmin = Max(fTmin, Min(fT1, fT2));
37  fTmax = Min(fTmax, Max(fT1, fT2));
38  if(fTmin > fTmax) return false;
39  /* The t we search for is the smallest non-negative */
40  if(fTmin >= 0) f_t_on_ray = fTmin / c_ray.GetLength();
41  else if(fTmax >= 0) f_t_on_ray = fTmax / c_ray.GetLength();
42  else return false;
43  return true;
44  }
45 
46  /****************************************/
47  /****************************************/
48 
49 }
T Max(const T &t_v1, const T &t_v2)
Returns the bigger of the two passed arguments.
Definition: general.h:95
A 3D vector class.
Definition: vector3.h:29
T GetMax() const
Definition: range.h:48
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:93
CVector3 & Rotate(const CQuaternion &c_quaternion)
Rotates this vector by the given quaternion.
Definition: vector3.cpp:25
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:109
T GetMin() const
Definition: range.h:32
void Set(const Real f_x, const Real f_y, const Real f_z)
Sets the vector contents from Cartesian coordinates.
Definition: vector3.h:143
CQuaternion Inverse() const
Definition: quaternion.h:98
CVector3 & GetStart()
Definition: ray3.h:37
void GetDirection(CVector3 &c_buffer) const
Definition: ray3.h:80
Real GetLength() const
Definition: ray3.h:96
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
Real GetZ() const
Returns the z coordinate of this vector.
Definition: vector3.h:125
bool Intersects(Real &f_t_on_ray, const CRay3 &c_ray)
Definition: box.cpp:9
T Min(const T &t_v1, const T &t_v2)
Returns the smaller of the two passed arguments.
Definition: general.h:77