SegmentSegmentCcdContactCalculation-inl.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2013-2016, 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_SEGMENTSEGMENTCCDCONTACTCALCULATION_INL_H
17 #define SURGSIM_MATH_SEGMENTSEGMENTCCDCONTACTCALCULATION_INL_H
18 
19 #include <Eigen/Core>
20 #include <Eigen/Geometry>
21 
23 #include "SurgSim/Math/Scalar.h"
24 
25 namespace SurgSim
26 {
27 namespace Math
28 {
29 
39 template <class T, int MOpt>
41  T time,
42  const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& A,
43  const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& B,
44  const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& C,
45  const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& D,
46  Eigen::Matrix<T, 2, 1, MOpt>* barycentricCoordinates)
47 {
48  Eigen::Matrix<T, 3, 1, MOpt> At = interpolate(A.first, A.second, time);
49  Eigen::Matrix<T, 3, 1, MOpt> Bt = interpolate(B.first, B.second, time);
50  Eigen::Matrix<T, 3, 1, MOpt> Ct = interpolate(C.first, C.second, time);
51  Eigen::Matrix<T, 3, 1, MOpt> Dt = interpolate(D.first, D.second, time);
52 
53  // P = A + alpha.AB and P = C + beta.CD
54  // A + alpha.AB = C + beta.CD
55  // (AB -CD).(alpha) = (AC) which is a 3x2 linear system A.x = b
56  // (beta )
57  // Let's solve it using (A^t.A)^-1.(A^t.A) = Id2x2
58  // (A^t.A)^-1.A^t.(A.x) = (A^t.A)^-1.A^t.b
59  // x = (A^t.A)^-1.A^t.b
60 
61  Eigen::Matrix<T, 3, 2> matrixA;
62  matrixA.col(0) = Bt - At;
63  matrixA.col(1) = -(Dt - Ct);
64 
65  Eigen::Matrix<T, 3, 1, MOpt> b = Ct - At;
66  Eigen::Matrix<T, 2, 2> inv;
67  bool invertible;
68  (matrixA.transpose() * matrixA).computeInverseWithCheck(inv, invertible);
69  if (!invertible)
70  {
71  return false;
72  }
73 
74  *barycentricCoordinates = inv * matrixA.transpose() * b;
75 
76  for (int i = 0; i < 2; i++)
77  {
78  if (std::abs((*barycentricCoordinates)[i]) < Math::Geometry::ScalarEpsilon)
79  {
80  (*barycentricCoordinates)[i] = 0.0;
81  }
82  if (std::abs(1.0 - (*barycentricCoordinates)[i]) < Math::Geometry::ScalarEpsilon)
83  {
84  (*barycentricCoordinates)[i] = 1.0;
85  }
86  }
87 
88  return
89  (*barycentricCoordinates)[0] >= 0.0 && (*barycentricCoordinates)[0] <= 1.0 &&
90  (*barycentricCoordinates)[1] >= 0.0 && (*barycentricCoordinates)[1] <= 1.0;
91 }
92 
106 template <class T, int MOpt> inline
108  const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& A,
109  const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& B,
110  const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& C,
111  const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& D,
112  T* timeOfImpact, T* s0p1Factor, T* s1p1Factor)
113 {
114  std::array<T, 3> roots;
115  int numberOfRoots = timesOfCoplanarityInRange01(A, B, C, D, &roots);
116 
117  // The roots are all in [0..1] and ordered ascendingly
118  for (int rootId = 0; rootId < numberOfRoots; ++rootId)
119  {
120  Eigen::Matrix<T, 2, 1, MOpt> barycentricCoordinates;
121  if (areSegmentsIntersecting(roots[rootId], A, B, C, D, &barycentricCoordinates))
122  {
123  // The segments AB and CD are coplanar at time t, and they intersect
124  *timeOfImpact = roots[rootId];
125  *s0p1Factor = barycentricCoordinates[0];
126  *s1p1Factor = barycentricCoordinates[1];
127 
128  // None of these assertion should be necessary, but just to double check
129  SURGSIM_ASSERT(*timeOfImpact >= 0.0 && *timeOfImpact <= 1.0);
130  SURGSIM_ASSERT(*s0p1Factor >= 0.0 && *s0p1Factor <= 1.0);
131  SURGSIM_ASSERT(*s1p1Factor >= 0.0 && *s1p1Factor <= 1.0);
132 
133  return true;
134  }
135  }
136 
137  return false;
138 }
139 
140 }; // namespace Math
141 }; // namespace SurgSim
142 
143 #endif // SURGSIM_MATH_SEGMENTSEGMENTCCDCONTACTCALCULATION_INL_H
SURGSIM_ASSERT
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
Scalar.h
SurgSim::Math::interpolate
Eigen::Quaternion< T, QOpt > interpolate(const Eigen::Quaternion< T, QOpt > &q0, const Eigen::Quaternion< T, QOpt > &q1, T t)
Interpolate (slerp) between 2 quaternions.
Definition: Quaternion.h:149
SurgSim
Definition: CompoundShapeToGraphics.cpp:29
SurgSim::Math::calculateCcdContactSegmentSegment
bool calculateCcdContactSegmentSegment(const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &A, const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &B, const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &C, const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &D, T *timeOfImpact, T *s0p1Factor, T *s1p1Factor)
Continuous collision detection between two segments AB and CD.
Definition: SegmentSegmentCcdContactCalculation-inl.h:107
SurgSim::Math::timesOfCoplanarityInRange01
int timesOfCoplanarityInRange01(const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &A, const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &B, const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &C, const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &D, std::array< T, 3 > *timesOfCoplanarity)
Test when 4 points are coplanar in the range [0..1] given their linear motion.
Definition: Geometry.h:1840
SurgSim::Math::barycentricCoordinates
bool barycentricCoordinates(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, 2, 1, MOpt > *coordinates)
Calculate the barycentric coordinates of a point with respect to a line segment.
Definition: Geometry.h:71
CubicSolver.h
SurgSim::Math::areSegmentsIntersecting
bool areSegmentsIntersecting(T time, const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &A, const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &B, const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &C, const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &D, Eigen::Matrix< T, 2, 1, MOpt > *barycentricCoordinates)
Check if 2 segments intersect at a given time of their motion.
Definition: SegmentSegmentCcdContactCalculation-inl.h:40