TriangleCapsuleContactCalculation-inl.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2013, SimQuest Solutions Inc.
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 
16 #ifndef SURGSIM_MATH_TRIANGLECAPSULECONTACTCALCULATION_INL_H
17 #define SURGSIM_MATH_TRIANGLECAPSULECONTACTCALCULATION_INL_H
18 
19 #include "SurgSim/Math/Valid.h"
21 
22 namespace SurgSim
23 {
24 
25 namespace Math
26 {
27 
29 {
30 
40 template <class T, int MOpt>
41 Eigen::Matrix<T, 3, 1, MOpt> pointWithTangentOnEllipse(const Eigen::Matrix<T, 3, 1, MOpt>& center,
42  const Eigen::Matrix<T, 3, 1, MOpt>& majorAxis,
43  const Eigen::Matrix<T, 3, 1, MOpt>& minorAxis,
44  const double majorRadius, const double minorRadius,
45  const Eigen::Matrix<T, 3, 1, MOpt>& tangent)
46 {
47  Eigen::Transform<T, 3, Eigen::Isometry> transform;
48  transform.translation() = center;
49  transform.linear().col(0) = majorAxis;
50  transform.linear().col(1) = minorAxis;
51  transform.linear().col(2) = majorAxis.cross(minorAxis);
52 
53  // tangent in local coordinates.
54  Eigen::Matrix<T, 3, 1, MOpt> localTangent = transform.inverse().linear() * tangent;
55 
56  // Slope of this tangent
57  T m = localTangent[1] / localTangent[0];
58 
59  // Ellipse equation: x*x/a*a + y*y/b*b = 1
60  // Rewriting ellipse equation in the form, y = f(x): y = sqrt(a*a - x*x) * b / a
61  // Slope of ellipse: y' = -x*b*b/a*a*y
62  // This slope is equal to the slope of the localTangent. So, we can solve for x and y.
63  T x = std::sqrt((m * m * majorRadius * majorRadius * majorRadius * majorRadius) /
64  (minorRadius * minorRadius + m * m * majorRadius * majorRadius));
65  T y = (minorRadius / majorRadius) *
66  std::sqrt(majorRadius * majorRadius - x * x) * ((m > 0.0) ? -1.0 : 1.0);
67 
68  // Transforming this point into world coordinates.
69  return transform * Eigen::Matrix<T, 3, 1, MOpt>(x, y, static_cast<T>(0));
70 }
71 
75 template <class T, int MOpt>
77 {
78  typedef Eigen::Matrix<T, 3, 1, MOpt> Vector3;
79  typedef Eigen::Matrix<T, 2, 1, MOpt> Vector2;
80  typedef Eigen::Transform<T, 3, Eigen::Isometry> RigidTransform3;
81 
82 public:
89  const Vector3& tv0, const Vector3& tv1, const Vector3& tv2,
90  const Vector3& tn,
91  const Vector3& cv0, const Vector3& cv1,
92  double cr)
93  : m_tv0(tv0), m_tv1(tv1), m_tv2(tv2), m_tn(tn),
94  m_cvTop(cv0), m_cvBottom(cv1), m_cr(cr)
95  {
96  m_epsilon = static_cast<T>(Geometry::DistanceEpsilon);
100  m_cLength = m_cAxis.norm();
102  if (m_cAxis.dot(tn) > 0.0)
103  {
104  m_cvTop = cv1;
105  m_cvBottom = cv0;
106  m_cAxis = -m_cAxis;
107  }
108  }
109 
112  {
113  return m_distance < (m_cr - m_epsilon);
114  }
115 
122  void calculateContact(T* penetrationDepth, Vector3* penetrationPointTriangle, Vector3* penetrationPointCapsule,
123  Vector3* contactNormal, Vector3* penetrationPointCapsuleAxis)
124  {
125  if (isIntersecting())
126  {
127  bool result =
132 
133  SURGSIM_ASSERT(result) << "At this point, there has to be an intersection.";
134 
135  *penetrationDepth = m_penetrationDepth;
136  *penetrationPointTriangle = m_penetrationPointTriangle;
137  *penetrationPointCapsule = m_penetrationPointCapsule;
138  *contactNormal = m_contactNormal;
139  *penetrationPointCapsuleAxis = m_penetrationPointCapsuleAxis;
140  }
141  }
142 
143 private:
149  {
150  if (m_distance > m_epsilon)
151  {
153  m_contactNormal.normalize();
156  return true;
157  }
158 
159  return false;
160  }
161 
167  {
168  if (std::abs(m_cAxis.dot(m_tn) + 1.0) < m_epsilon)
169  {
174  return true;
175  }
176 
177  return false;
178  }
179 
186  {
190  {
192 
193  auto projectionCvBottom = (m_cvBottom + m_tn * (m_tv0 - m_cvBottom).dot(m_tn)).eval();
194  if (SurgSim::Math::isPointInsideTriangle(projectionCvBottom, m_tv0, m_tv1, m_tv2, m_tn))
195  {
199  }
200  else
201  {
204  }
206  return true;
207  }
208 
209  return false;
210  }
211 
218  {
219  if (m_distance != 0.0)
220  {
221  return false;
222  }
223 
224  // Extruding the triangle along the direction of -tn creates a volume. Clip off the capsule axis that is
225  // outside of this volume.
226  Vector3 v[3] = {m_tv0, m_tv1, m_tv2};
227  Vector3 planeN[4] = {m_tn, (-m_tn.cross(m_tv1 - m_tv0)).normalized(), (-m_tn.cross(m_tv2 - m_tv1)).normalized(),
228  (-m_tn.cross(m_tv0 - m_tv2)).normalized()};
229  double planeD[4] = {-m_tv0.dot(planeN[0]), -m_tv0.dot(planeN[1]), -m_tv1.dot(planeN[2]), -m_tv2.dot(planeN[3])};
230  auto segmentStart = m_cvTop;
231  auto segmentEnd = m_cvBottom;
232 
233  size_t j = clipSegmentAgainstTriangle(&segmentStart, &segmentEnd, v, planeN, planeD);
234 
235  if (j == 0)
236  {
242  return true;
243  }
244 
245  Vector3 deepestPoint = ((segmentStart - segmentEnd).dot(planeN[0]) < 0.0) ? segmentStart : segmentEnd;
246  Vector3 edgeVertices[2] = {v[(j - 1) % 3], v[j % 3]};
247  Vector3 triangleEdge = (edgeVertices[1] - edgeVertices[0]).normalized();
248 
249  // The capsule axis intersects the plane (planeN[j], planeD[j]). First, the capsule is treated as a cylinder.
250  // Its intersection with this plane is an ellipse. The deepest point on this ellipse along -tn and bounded
251  // by vectors (edgeVertices[0] -> edgeVertices[0] - tn) and (edgeVertices[1] -> edgeVertices[1] - tn) is found.
252  Vector3 center = deepestPoint;
253  Vector3 majorAxis = (triangleEdge * (triangleEdge.dot(m_cAxis)) + m_tn * (m_tn.dot(m_cAxis))).normalized();
254  double majorRadius = farthestIntersectionLineCylinder(center, majorAxis, &deepestPoint);
255  SURGSIM_ASSERT(isValid(majorRadius)) << "The major radius of the ellipse should be a valid number.";
256 
257  if (std::abs(majorAxis.dot(triangleEdge)) > m_epsilon)
258  {
259  // majorApex is not the deepest point because the ellipse is angled. The deepest point is between majorApex
260  // and minorApex on the circumference of the ellipse, and the tangent at that point is parallel to the
261  // triangleEdge.
262  auto minorAxis = planeN[j].cross(majorAxis);
263  double minorRadius = farthestIntersectionLineCylinder(center, minorAxis);
264  SURGSIM_ASSERT(isValid(minorRadius)) << "The minor radius of the ellipse should be a valid number.";
265 
266  deepestPoint = pointWithTangentOnEllipse(center, majorAxis, minorAxis, majorRadius, minorRadius,
267  triangleEdge);
268  Vector3 result;
269  if (std::abs(distancePointSegment(deepestPoint, m_cvTop, m_cvBottom, &result) - m_cr) > m_epsilon)
270  {
271  // The deepest point is not on the capsule, which means that the capsule end (the sphere) is
272  // intersecting the triangle edge plane (planeN[j], planeD[j]). The intersection between them is a
273  // circle. Define a 2D co-ordinate system with the origin at edgeVertices[0], the x-axis as
274  // triangleEdge, and the y-axis as tn. Transforming the circle to this 2D co-ordinate system, creates a
275  // circle of radius, r, with its center at x, y. Now the deepest point on this circle is (x, y - r).
276  Vector3 origin = edgeVertices[0], xAxis = triangleEdge, yAxis = m_tn, zAxis = planeN[j];
277 
278  double sphereCenterToXYPlane = (m_cvBottom - origin).dot(zAxis);
279  SURGSIM_ASSERT(m_cr + m_epsilon >= sphereCenterToXYPlane)
280  << "The sphere center is too far from the triangle edge plane.";
281 
282  double circleRadius = std::sqrt(m_cr * m_cr - sphereCenterToXYPlane * sphereCenterToXYPlane);
283  SURGSIM_ASSERT(isValid(circleRadius))
284  << "The radius of the circle of intersection between the sphere and the plane is invalid.";
285  Vector3 circleCenter = m_cvBottom - zAxis * sphereCenterToXYPlane;
286  double x = (circleCenter - origin).dot(xAxis);
287  double y = (circleCenter - origin).dot(yAxis) - circleRadius;
288  deepestPoint = xAxis * x + yAxis * y + origin;
289  }
290  }
291 
292  // Project deepestPoint on the triangle edge to make sure it is within the edge.
293  auto edgeLength = (edgeVertices[1] - edgeVertices[0]).norm();
294  double deepestPointDotEdge = triangleEdge.dot(deepestPoint - edgeVertices[0]);
295  if (deepestPointDotEdge <= -m_epsilon || deepestPointDotEdge >= edgeLength + m_epsilon)
296  {
297  // In this case, the intersection of the cylinder with the triangle edge plane gives an ellipse
298  // that is close to a triangle corner and the deepest penetration point on the ellipse is
299  // actually outside the triangle.
300  // Solution: find the deepest point on the ellipse which projection is still on the triangle.
301  // For that: Find the corner edgeVertex of the triangle closest to deepestPointDotEdge
302  // Find the deepest penetration point verifying P - tn*t and the cylinder equation.
303 
304  // The triangle point to consider is edgeVertices[0] or edgeVertices[1].
305  Vector3 edgeVertex = (deepestPointDotEdge < 0.0) ? edgeVertices[0] : edgeVertices[1];
306  double d =
308  SURGSIM_ASSERT(isValid(d)) << "There must be a part of the ellipse between the triangle edge at this point";
309  }
310 
312  m_penetrationPointCapsule = deepestPoint;
313  m_penetrationDepth = -m_tn.dot(deepestPoint - m_tv0);
315 
316  return true;
317  }
318 
323  double farthestIntersectionLineCylinder(const Vector3& lineStart, const Vector3& lineDir, Vector3* point = nullptr)
324  {
326  {
327  Vector3 j, k;
329 
330  m_cTransform.translation() = m_cvTop;
331  m_cTransform.linear().col(0) = m_cAxis;
332  m_cTransform.linear().col(1) = j;
333  m_cTransform.linear().col(2) = k;
334  m_cInverseTransform = m_cTransform.inverse();
335  }
336 
337  // Transform the problem in the cylinder space to solve the local cylinder equation y^2 + z^2 = r^2
338  // Point on ellipse should be on the line, P + t.(D)
339  // => Py^2 + t^2.Dy^2 + 2.Py.t.Dy + Pz^2 + t^2.Dz^2 + 2.Pz.t.Dz = r^2
340  // => t^2.(Dy^2 + Dz^2) + t.(2.Py.Dy + 2.Pz.Dz) + (Py^2 + Pz^2 - r^2) = 0
341  // Let a = (Dy^2 + Dz^2), b = (2.Py.Dy + 2.Pz.Dz), c = (Py^2 + Pz^2 - r^2):
342  // => t^2.a + t.b + c = 0, whose solution is:
343  // (-b +/- sqrt(b^2 - 4*a*c))/2*a
344  auto const P = (m_cInverseTransform.getValue() * lineStart).eval();
345  auto const D = (m_cInverseTransform.getValue().linear() * lineDir).eval();
346 
347  T a = D[1] * D[1] + D[2] * D[2];
348  T b = static_cast<T>(2) * (P[1] * D[1] + P[2] * D[2]);
349  T c = (P[1] * P[1] + P[2] * P[2] - m_cr * m_cr);
350 
351  T discriminant = b * b - static_cast<T>(4) * a * c;
352 
353  if (discriminant < 0.0)
354  {
355  // Cannot use a sqrt on a negative number. Push it to zero if it is small.
356  if (discriminant >= -Geometry::ScalarEpsilon)
357  {
358  discriminant = 0.0;
359  }
360  else
361  {
362  return std::numeric_limits<T>::quiet_NaN();
363  }
364  }
365 
366  // We have two solutions. We want the larger value.
367  double d = (-b / (static_cast<T>(2) * a)) + std::abs(std::sqrt(discriminant) / (static_cast<T>(2) * a));
369  if (point != nullptr)
370  {
371  *point = lineStart + lineDir * d;
372  }
373 
374  return d;
375  }
376 
383  double farthestIntersectionLineCapsule(const Vector3& lineStart, const Vector& lineDir,
384  Vector3* point, Vector3* pointOnCapsuleAxis)
385  {
386  // Transform the problem into the capsule space to solve the local capsule equation. The capsule coordinate
387  // system has its origin on one the capsule ends (m_cvTop), the x axis is along the capsule axis (m_cAxis), and
388  // the y and z axes are any orthogonal vectors to the capsule axis. The equation of the capsule can be written
389  // as the following:
390  // x^2 + y^2 + z^2 = r^2 | x < 0 ------ [1]
391  // y^2 + z^2 = r^2 | 0 < x < length ------ [2]
392  // (x - length)^2 + y^2 + z^2 = r^2 | x > length ------ [3]
393  // Point should be on the line, P + t.(D)
394 
395  // First find the intersection with an infinite cylinder centered around the capsule axis.
396  double d = farthestIntersectionLineCylinder(lineStart, lineDir, point);
398  *point = lineStart + lineDir * d;
399 
400  // When x <0 or x > length, the equation of the capsule is that of the sphere (from equations [1] and [3]). So,
401  // the intersection between the line (P + t.D) and the sphere is calculated as below. Here l is the length of
402  // the capsule.
403  // => ((P + t.D).x - l)^2 + (P + t.D).y^2 + (P + t.D).z^2 = r^2
404  // => Px^2 + t^2.Dx^2 + l^2 + 2.Px.t.Dx - 2.t.Dx.l - 2.Px.l +
405  // Py^2 + t^2.Dy^2 + 2.Py.t.Dy + Pz^2 + t^2.Dz^2 + 2.Pz.t.Dz = r^2
406  // => t^2.(Dx^2 + Dy^2 + Dz^2) + t.(2.Px.Dx + 2.Py.Dy + 2.Pz.Dz - 2.Dx.l) +
407  // (Px^2 + Py^2 + Pz^2 + l^2 - 2.Px.l - r^2) = 0
408  // Let a = (Dx^2 + Dy^2 + Dz^2),
409  // b = (2.Px.Dx + 2.Py.Dy + 2.Pz.Dz - 2.Dx.l),
410  // c = (Px^2 + Py^2 + Pz^2 + l^2 - 2.Px.l - r^2):
411  double x = ((*point) - m_cvTop).dot(m_cAxis);
412  if (x <= 0.0 || x >= m_cLength)
413  {
414  x = (x <= 0.0) ? 0.0 : m_cLength;
415 
416  auto const P = (m_cInverseTransform.getValue() * lineStart).eval();
417  auto const D = (m_cInverseTransform.getValue().linear() * lineDir).eval();
418 
419  T a = D[0] * D[0] + D[1] * D[1] + D[2] * D[2];
420  T b = static_cast<T>(2) * (P[0] * D[0] + P[1] * D[1] + P[2] * D[2] - D[0] * x);
421  T c = (P[0] * P[0] + P[1] * P[1] + P[2] * P[2] + x * x - static_cast<T>(2) * P[0] * x - m_cr * m_cr);
422 
423  // => t^2.a + t.b + c = 0, whose solution is:
424  // (-b +/- sqrt(b^2 - 4*a*c))/2*a
425  T discriminant = b * b - static_cast<T>(4) * a * c;
426 
427  if (discriminant < 0.0 && discriminant >= -Geometry::ScalarEpsilon)
428  {
429  discriminant = 0.0;
430  }
431 
432  // We have two solutions. We want the smaller value.
433  d = (-b - std::abs(std::sqrt(discriminant))) / (static_cast<T>(2) * a);
435  *point = lineStart + lineDir * d;
436  }
437 
438  *pointOnCapsuleAxis = m_cTransform * Vector3(x, 0.0, 0.0);
439  return d;
440  }
441 
448  size_t clipSegmentAgainstTriangle(Vector3* segmentStart, Vector3* segmentEnd, Vector3* v, Vector3* planeN,
449  double* planeD)
450  {
451  double ratio, dStart, dEnd;
452  size_t j = 4;
453  for (size_t i = 0; i < 4; ++i)
454  {
455  dStart = segmentStart->dot(planeN[i]) + planeD[i];
456  dEnd = segmentEnd->dot(planeN[i]) + planeD[i];
457 
458  if (dStart < -m_epsilon && dEnd > m_epsilon)
459  {
460  ratio = std::abs(dStart) / (std::abs(dStart) + dEnd);
461  *segmentEnd = *segmentStart + (*segmentEnd - *segmentStart) * ratio;
462  j = i;
463  }
464  else if (dStart > m_epsilon && dEnd < -m_epsilon)
465  {
466  ratio = dStart / (dStart + std::abs(dEnd));
467  *segmentStart = *segmentStart + (*segmentEnd - *segmentStart) * ratio;
468  j = i;
469  }
470  else if (dStart < m_epsilon && dEnd > m_epsilon)
471  {
472  *segmentEnd = *segmentStart;
473  j = i;
474  }
475  else if (dStart > m_epsilon && dEnd < m_epsilon)
476  {
477  *segmentStart = *segmentEnd;
478  j = i;
479  }
480  }
481 
482  SURGSIM_ASSERT(j < 4) << "The clipping should have happened at least with the triangle plane.";
483  return j;
484  }
485 
498  double m_cr;
499  double m_cLength;
502  double m_distance;
517 };
518 }
519 
520 template <class T, int MOpt> inline
522  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
523  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
524  const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
525  const Eigen::Matrix<T, 3, 1, MOpt>& tn,
526  const Eigen::Matrix<T, 3, 1, MOpt>& cv0,
527  const Eigen::Matrix<T, 3, 1, MOpt>& cv1,
528  double cr,
529  T* penetrationDepth,
530  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointTriangle,
531  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsule,
532  Eigen::Matrix<T, 3, 1, MOpt>* contactNormal,
533  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsuleAxis)
534 {
536  calc(tv0, tv1, tv2, tn, cv0, cv1, cr);
537 
538  if (calc.isIntersecting())
539  {
540  calc.calculateContact(penetrationDepth, penetrationPointTriangle, penetrationPointCapsule,
541  contactNormal, penetrationPointCapsuleAxis);
542  return true;
543  }
544 
545  return false;
546 }
547 
548 
549 template <class T, int MOpt> inline
551  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
552  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
553  const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
554  const Eigen::Matrix<T, 3, 1, MOpt>& cv0,
555  const Eigen::Matrix<T, 3, 1, MOpt>& cv1,
556  double cr,
557  T* penetrationDepth,
558  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
559  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
560  Eigen::Matrix<T, 3, 1, MOpt>* contactNormal,
561  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsuleAxis)
562 {
563  Eigen::Matrix<T, 3, 1, MOpt> tn = (tv1 - tv0).cross(tv2 - tv0);
564  Eigen::Matrix<T, 3, 1, MOpt> ca = cv1 - cv0;
565  if (tn.isZero() || ca.isZero())
566  {
567  // Degenerate triangle/capsule passed to calculateContactTriangleTriangle
568  return false;
569  }
570  tn.normalize();
571  return calculateContactTriangleCapsule(tv0, tv1, tv2, tn, cv0, cv1, cr, penetrationDepth,
572  penetrationPoint0, penetrationPoint1, contactNormal, penetrationPointCapsuleAxis);
573 }
574 
575 } // namespace Math
576 
577 } // namespace SurgSim
578 
579 #endif // SURGSIM_MATH_TRIANGLECAPSULECONTACTCALCULATION_INL_H
SURGSIM_ASSERT
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::axisAwayFromTriangle
bool axisAwayFromTriangle()
This function handles the contact data calculation for the case where there is an intersection betwee...
Definition: TriangleCapsuleContactCalculation-inl.h:148
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::m_cLength
double m_cLength
Capsule ends, axis , radius and length.
Definition: TriangleCapsuleContactCalculation-inl.h:499
SurgSim::DataStructures::OptionalValue< RigidTransform3 >
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::m_tv1
Vector3 m_tv1
Definition: TriangleCapsuleContactCalculation-inl.h:489
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::Vector3
Eigen::Matrix< T, 3, 1, MOpt > Vector3
Definition: TriangleCapsuleContactCalculation-inl.h:78
SurgSim::Math::buildOrthonormalBasis
bool buildOrthonormalBasis(Eigen::Matrix< T, 3, 1, VOpt > *i, Eigen::Matrix< T, 3, 1, VOpt > *j, Eigen::Matrix< T, 3, 1, VOpt > *k)
Helper method to construct an orthonormal basis (i, j, k) given the 1st vector direction.
Definition: Vector.h:178
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::m_penetrationDepth
T m_penetrationDepth
Definition: TriangleCapsuleContactCalculation-inl.h:505
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::axisPerpendicularToTriangle
bool axisPerpendicularToTriangle()
This function handles the contact data calculation for the case where there is an intersection betwee...
Definition: TriangleCapsuleContactCalculation-inl.h:166
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::m_penetrationPointTriangle
Vector3 m_penetrationPointTriangle
Definition: TriangleCapsuleContactCalculation-inl.h:506
SurgSim::Math::distancePointSegment
T distancePointSegment(const Eigen::Matrix< T, 3, 1, MOpt > &pt, const Eigen::Matrix< T, 3, 1, MOpt > &sv0, const Eigen::Matrix< T, 3, 1, MOpt > &sv1, Eigen::Matrix< T, 3, 1, MOpt > *result)
Point segment distance, if the projection of the closest point is not within the segments,...
Definition: Geometry.h:297
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::m_cInverseTransform
SurgSim::DataStructures::OptionalValue< RigidTransform3 > m_cInverseTransform
The inverse transform of the capsule.
Definition: TriangleCapsuleContactCalculation-inl.h:514
OptionalValue.h
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::m_cr
double m_cr
Capsule ends, axis , radius and length.
Definition: TriangleCapsuleContactCalculation-inl.h:498
SurgSim::Math::isPointInsideTriangle
bool isPointInsideTriangle(const Eigen::Matrix< T, 3, 1, MOpt > &pt, const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, const Eigen::Matrix< T, 3, 1, MOpt > &tn)
Check if a point is inside a triangle.
Definition: Geometry.h:159
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::farthestIntersectionLineCylinder
double farthestIntersectionLineCylinder(const Vector3 &lineStart, const Vector3 &lineDir, Vector3 *point=nullptr)
Definition: TriangleCapsuleContactCalculation-inl.h:323
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::m_tv0
Vector3 m_tv0
Definition: TriangleCapsuleContactCalculation-inl.h:488
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::m_tn
Vector3 m_tn
Definition: TriangleCapsuleContactCalculation-inl.h:491
SurgSim::Math::distanceSegmentTriangle
T distanceSegmentTriangle(const Eigen::Matrix< T, 3, 1, MOpt > &sv0, const Eigen::Matrix< T, 3, 1, MOpt > &sv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, Eigen::Matrix< T, 3, 1, MOpt > *segmentPoint, Eigen::Matrix< T, 3, 1, MOpt > *trianglePoint)
Calculate the distance of a line segment to a triangle.
Definition: Geometry.h:1280
SurgSim
Definition: CompoundShapeToGraphics.cpp:30
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::m_cTransform
RigidTransform3 m_cTransform
The transform of the capsule.
Definition: TriangleCapsuleContactCalculation-inl.h:512
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::m_penetrationPointCapsule
Vector3 m_penetrationPointCapsule
Definition: TriangleCapsuleContactCalculation-inl.h:507
SurgSim::Math::isValid
bool isValid(float value)
Check if a float value is valid.
Definition: Valid-inl.h:98
SurgSim::DataStructures::OptionalValue::hasValue
bool hasValue() const
Query if this object has been assigned a value.
Definition: OptionalValue.h:56
Valid.h
Declarations of isValid(), isSubnormal() and setSubnormalToZero().
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::calculateContact
void calculateContact(T *penetrationDepth, Vector3 *penetrationPointTriangle, Vector3 *penetrationPointCapsule, Vector3 *contactNormal, Vector3 *penetrationPointCapsuleAxis)
Calculate the contact info.
Definition: TriangleCapsuleContactCalculation-inl.h:122
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::axisTouchingTriangle
bool axisTouchingTriangle()
This function handles the contact data calculation for the case where there is an intersection betwee...
Definition: TriangleCapsuleContactCalculation-inl.h:185
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::farthestIntersectionLineCapsule
double farthestIntersectionLineCapsule(const Vector3 &lineStart, const Vector &lineDir, Vector3 *point, Vector3 *pointOnCapsuleAxis)
Definition: TriangleCapsuleContactCalculation-inl.h:383
SurgSim::Math::isPointOnTriangleEdge
bool isPointOnTriangleEdge(const Eigen::Matrix< T, 3, 1, MOpt > &pt, const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, const Eigen::Matrix< T, 3, 1, MOpt > &tn)
Check if a point is on the edge of a triangle.
Definition: Geometry.h:206
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::RigidTransform3
Eigen::Transform< T, 3, Eigen::Isometry > RigidTransform3
Definition: TriangleCapsuleContactCalculation-inl.h:80
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::m_distance
double m_distance
Distance between triangle and capsule.
Definition: TriangleCapsuleContactCalculation-inl.h:502
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::axisThroughTriangle
bool axisThroughTriangle()
This function handles the contact data calculation for the case where there is an intersection betwee...
Definition: TriangleCapsuleContactCalculation-inl.h:217
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::Vector2
Eigen::Matrix< T, 2, 1, MOpt > Vector2
Definition: TriangleCapsuleContactCalculation-inl.h:79
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::m_penetrationPointCapsuleAxis
Vector3 m_penetrationPointCapsuleAxis
Definition: TriangleCapsuleContactCalculation-inl.h:509
SurgSim::DataStructures::OptionalValue::getValue
const T & getValue() const
Gets the value.
Definition: OptionalValue.h:78
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation
Class used to find the intersection between a triangle and a capsule.
Definition: TriangleCapsuleContactCalculation-inl.h:77
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation
TriangleCapsuleContactCalculation(const Vector3 &tv0, const Vector3 &tv1, const Vector3 &tv2, const Vector3 &tn, const Vector3 &cv0, const Vector3 &cv1, double cr)
Constructor.
Definition: TriangleCapsuleContactCalculation-inl.h:88
SurgSim::Math::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
A dynamic size column vector.
Definition: Vector.h:68
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::clipSegmentAgainstTriangle
size_t clipSegmentAgainstTriangle(Vector3 *segmentStart, Vector3 *segmentEnd, Vector3 *v, Vector3 *planeN, double *planeD)
Definition: TriangleCapsuleContactCalculation-inl.h:448
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::m_epsilon
T m_epsilon
epsilon
Definition: TriangleCapsuleContactCalculation-inl.h:516
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::m_contactNormal
Vector3 m_contactNormal
Definition: TriangleCapsuleContactCalculation-inl.h:508
SurgSim::Math::calculateContactTriangleCapsule
bool calculateContactTriangleCapsule(const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, const Eigen::Matrix< T, 3, 1, MOpt > &tn, const Eigen::Matrix< T, 3, 1, MOpt > &cv0, const Eigen::Matrix< T, 3, 1, MOpt > &cv1, double cr, T *penetrationDepth, Eigen::Matrix< T, 3, 1, MOpt > *penetrationPointTriangle, Eigen::Matrix< T, 3, 1, MOpt > *penetrationPointCapsule, Eigen::Matrix< T, 3, 1, MOpt > *contactNormal, Eigen::Matrix< T, 3, 1, MOpt > *penetrationPointCapsuleAxis)
Calculate the contact between a capsule and a triangle.
Definition: TriangleCapsuleContactCalculation-inl.h:521
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::m_cAxis
Vector3 m_cAxis
Capsule ends, axis , radius and length.
Definition: TriangleCapsuleContactCalculation-inl.h:497
SurgSim::Math::TriangleCapsuleContactCalculation::pointWithTangentOnEllipse
Eigen::Matrix< T, 3, 1, MOpt > pointWithTangentOnEllipse(const Eigen::Matrix< T, 3, 1, MOpt > &center, const Eigen::Matrix< T, 3, 1, MOpt > &majorAxis, const Eigen::Matrix< T, 3, 1, MOpt > &minorAxis, const double majorRadius, const double minorRadius, const Eigen::Matrix< T, 3, 1, MOpt > &tangent)
Find the point on (positive X side of) ellipse parallel to the given tangent.
Definition: TriangleCapsuleContactCalculation-inl.h:41
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::m_cvBottom
Vector3 m_cvBottom
Capsule ends, axis , radius and length.
Definition: TriangleCapsuleContactCalculation-inl.h:496
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::m_cvTop
Vector3 m_cvTop
Capsule ends, axis , radius and length.
Definition: TriangleCapsuleContactCalculation-inl.h:495
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::m_tv2
Vector3 m_tv2
Definition: TriangleCapsuleContactCalculation-inl.h:490
SurgSim::Math::TriangleCapsuleContactCalculation::TriangleCapsuleContactCalculation::isIntersecting
bool isIntersecting()
Definition: TriangleCapsuleContactCalculation-inl.h:111