cylinder.cpp
Go to the documentation of this file.
1 #include "cylinder.h"
2 #include "ray3.h"
3 
4 namespace argos {
5 
6  /****************************************/
7  /****************************************/
8 
9  bool CCylinder::Intersects(Real& f_t_on_ray,
10  const CRay3& c_ray) {
11  /* Solution candidates */
12  UInt32 unSolutionCount = 0;
13  Real pfSolutions[4];
14  /* Position of top cap relative to bottom one */
15  CVector3 cRelTopCap = m_fHeight * m_cAxis;
16  /*
17  * Check intersection with cylinder
18  */
19  CVector3 cPRA = c_ray.GetStart() - m_cBasePos;
20  CVector3 cBeta = cPRA - m_cAxis.DotProduct(cPRA) * m_cAxis;
21  CVector3 cRayDir;
22  c_ray.GetDirection(cRayDir);
23  Real fDRA = m_cAxis.DotProduct(cRayDir);
24  CVector3 cAlpha = cRayDir - fDRA * m_cAxis;
25  Real fA = cAlpha.SquareLength();
26  Real fB = 2.0 * cAlpha.DotProduct(cBeta);
27  Real fC = cBeta.SquareLength() - Square(m_fRadius);
28  Real fDelta = Square(fB) - 4.0 * fA * fC;
29  if(fDelta == 0) {
30  /* One candidate solution, is it within the cylinder caps? */
31  pfSolutions[0] = -fB / (2.0 * fA);
32  if(pfSolutions[0] > 0.0) {
33  CVector3 cTest = cPRA + pfSolutions[0] * cRayDir;
34  if(m_cAxis.DotProduct(cTest) > 0) {
35  cTest -= cRelTopCap;
36  if(m_cAxis.DotProduct(cTest) < 0) {
37  ++unSolutionCount;
38  }
39  }
40  }
41  }
42  else if(fDelta > 0) {
43  /* Two candidate solutions, are they within the cylinder caps? */
44  CVector3 cTest;
45  /* Test first solution */
46  pfSolutions[0] = (-fB + Sqrt(fDelta)) / (2.0 * fA);
47  if(pfSolutions[0] > 0.0) {
48  cTest = cPRA + pfSolutions[0] * cRayDir;
49  if(m_cAxis.DotProduct(cTest) > 0) {
50  cTest -= cRelTopCap;
51  if(m_cAxis.DotProduct(cTest) < 0) {
52  ++unSolutionCount;
53  }
54  }
55  }
56  /* Test second solution */
57  pfSolutions[unSolutionCount] = (-fB - Sqrt(fDelta)) / (2.0 * fA);
58  if(pfSolutions[unSolutionCount] > 0.0) {
59  cTest = cPRA + pfSolutions[unSolutionCount] * cRayDir;
60  if(m_cAxis.DotProduct(cTest) > 0) {
61  cTest -= cRelTopCap;
62  if(m_cAxis.DotProduct(cTest) < 0) {
63  ++unSolutionCount;
64  }
65  }
66  }
67  }
68  /*
69  * Check intersection with bottom and top caps
70  */
71  /* If the directions of the cylinder axis and the ray are parallel,
72  * nothing to do */
73  if(fDRA > 10e-6) {
74  Real fPPRA = m_cAxis.DotProduct(cPRA);
75  /* Bottom cap */
76  pfSolutions[unSolutionCount] = -fPPRA / fDRA;
77  if(pfSolutions[unSolutionCount] > 0.0 &&
78  (cPRA + pfSolutions[unSolutionCount] * cRayDir).SquareLength() < Square(m_fRadius)) {
79  ++unSolutionCount;
80  }
81  /* Top cap */
82  pfSolutions[unSolutionCount] = -(fPPRA - m_fHeight) / fDRA;
83  if(pfSolutions[unSolutionCount] > 0.0 &&
84  (cPRA - cRelTopCap + pfSolutions[unSolutionCount] * cRayDir).SquareLength() < Square(m_fRadius)) {
85  ++unSolutionCount;
86  }
87  }
88  /*
89  * All possible solutions have been found, take the closest
90  */
91  if(unSolutionCount == 0) {
92  return false;
93  }
94  f_t_on_ray = pfSolutions[0];
95  for(UInt32 i = 1; i < unSolutionCount; ++i) {
96  if(pfSolutions[i] < f_t_on_ray)
97  f_t_on_ray = pfSolutions[i];
98  }
99  f_t_on_ray /= c_ray.GetLength();
100  return true;
101  }
102 
103  /****************************************/
104  /****************************************/
105 
106 }
T Square(const T &t_v)
Returns the square of the value of the passed argument.
Definition: general.h:128
A 3D vector class.
Definition: vector3.h:29
#define Sqrt
Definition: general.h:64
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
Real SquareLength() const
Returns the square length of this vector.
Definition: vector3.h:197
unsigned int UInt32
32-bit unsigned integer.
Definition: datatypes.h:97
CVector3 & GetStart()
Definition: ray3.h:37
void GetDirection(CVector3 &c_buffer) const
Definition: ray3.h:80
Real GetLength() const
Definition: ray3.h:96
Real DotProduct(const CVector3 &c_vector3) const
Returns the dot product between this vector and the passed one.
Definition: vector3.h:348
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
bool Intersects(Real &f_t_on_ray, const CRay3 &c_ray)
Definition: cylinder.cpp:9