SeparationScenario.cpp
Go to the documentation of this file.
1 /*
2  * Original work Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
3  * Modified work Copyright (c) 2017 Louis Langholtz https://github.com/louis-langholtz/PlayRho
4  *
5  * This software is provided 'as-is', without any express or implied
6  * warranty. In no event will the authors be held liable for any damages
7  * arising from the use of this software.
8  *
9  * Permission is granted to anyone to use this software for any purpose,
10  * including commercial applications, and to alter it and redistribute it
11  * freely, subject to the following restrictions:
12  *
13  * 1. The origin of this software must not be misrepresented; you must not
14  * claim that you wrote the original software. If you use this software
15  * in a product, an acknowledgment in the product documentation would be
16  * appreciated but is not required.
17  * 2. Altered source versions must be plainly marked as such, and must not be
18  * misrepresented as being the original software.
19  * 3. This notice may not be removed or altered from any source distribution.
20  */
21 
22 #include <PlayRho/Collision/SeparationScenario.hpp>
23 #include <PlayRho/Collision/DistanceProxy.hpp>
24 
25 namespace playrho {
26 namespace d2 {
27 namespace {
28 
29 LengthIndexPair
30 FindMinSeparationForPoints(const SeparationScenario& scenario,
31  const Transformation& xfA, const Transformation& xfB)
32 {
33  const auto dirA = InverseRotate(+scenario.axis, xfA.q);
34  const auto dirB = InverseRotate(-scenario.axis, xfB.q);
35  const auto indexA = GetSupportIndex(scenario.proxyA, dirA);
36  const auto indexB = GetSupportIndex(scenario.proxyB, dirB);
37  const auto pointA = Transform(scenario.proxyA.GetVertex(indexA), xfA);
38  const auto pointB = Transform(scenario.proxyB.GetVertex(indexB), xfB);
39  const auto delta = pointB - pointA;
40  return LengthIndexPair{Dot(delta, scenario.axis), IndexPair{indexA, indexB}};
41 }
42 
43 LengthIndexPair
44 FindMinSeparationForFaceA(const SeparationScenario& scenario,
45  const Transformation& xfA, const Transformation& xfB)
46 {
47  const auto normal = Rotate(scenario.axis, xfA.q);
48  const auto indexA = InvalidVertex;
49  const auto pointA = Transform(scenario.localPoint, xfA);
50  const auto dir = InverseRotate(-normal, xfB.q);
51  const auto indexB = GetSupportIndex(scenario.proxyB, dir);
52  const auto pointB = Transform(scenario.proxyB.GetVertex(indexB), xfB);
53  const auto delta = pointB - pointA;
54  return LengthIndexPair{Dot(delta, normal), IndexPair{indexA, indexB}};
55 }
56 
57 LengthIndexPair
58 FindMinSeparationForFaceB(const SeparationScenario& scenario,
59  const Transformation& xfA, const Transformation& xfB)
60 {
61  const auto normal = Rotate(scenario.axis, xfB.q);
62  const auto dir = InverseRotate(-normal, xfA.q);
63  const auto indexA = GetSupportIndex(scenario.proxyA, dir);
64  const auto pointA = Transform(scenario.proxyA.GetVertex(indexA), xfA);
65  const auto indexB = InvalidVertex;
66  const auto pointB = Transform(scenario.localPoint, xfB);
67  const auto delta = pointA - pointB;
68  return LengthIndexPair{Dot(delta, normal), IndexPair{indexA, indexB}};
69 }
70 
71 Length EvaluateForPoints(const SeparationScenario& scenario,
72  const Transformation& xfA, const Transformation& xfB,
73  IndexPair indexPair)
74 {
75  const auto pointA = Transform(scenario.proxyA.GetVertex(std::get<0>(indexPair)), xfA);
76  const auto pointB = Transform(scenario.proxyB.GetVertex(std::get<1>(indexPair)), xfB);
77  const auto delta = pointB - pointA;
78  return Dot(delta, scenario.axis);
79 }
80 
81 Length EvaluateForFaceA(const SeparationScenario& scenario,
82  const Transformation& xfA, const Transformation& xfB,
83  IndexPair indexPair)
84 {
85  const auto normal = Rotate(scenario.axis, xfA.q);
86  const auto pointA = Transform(scenario.localPoint, xfA);
87  const auto pointB = Transform(scenario.proxyB.GetVertex(std::get<1>(indexPair)), xfB);
88  const auto delta = pointB - pointA;
89  return Dot(delta, normal);
90 }
91 
92 Length EvaluateForFaceB(const SeparationScenario& scenario,
93  const Transformation& xfA, const Transformation& xfB,
94  IndexPair indexPair)
95 {
96  const auto normal = Rotate(scenario.axis, xfB.q);
97  const auto pointB = Transform(scenario.localPoint, xfB);
98  const auto pointA = Transform(scenario.proxyA.GetVertex(std::get<0>(indexPair)), xfA);
99  const auto delta = pointA - pointB;
100  return Dot(delta, normal);
101 }
102 
103 } // namespace anonymous
104 
105 SeparationScenario
106 GetSeparationScenario(IndexPair3 indices,
107  const DistanceProxy& proxyA, const Transformation& xfA,
108  const DistanceProxy& proxyB, const Transformation& xfB)
109 {
110  assert(!empty(indices));
111  assert(proxyA.GetVertexCount() > 0);
112  assert(proxyB.GetVertexCount() > 0);
113 
114  const auto numIndices = GetNumValidIndices(indices);
115  const auto type = (numIndices == 1)?
116  SeparationScenario::e_points: ((std::get<0>(indices[0]) == std::get<0>(indices[1]))?
117  SeparationScenario::e_faceB: SeparationScenario::e_faceA);
118 
119  switch (type)
120  {
121  case SeparationScenario::e_faceB:
122  {
123  const auto ip0 = indices[0];
124  const auto ip1 = indices[1];
125 
126  // Two points on B and one on A.
127  const auto localPointB1 = proxyB.GetVertex(std::get<1>(ip0));
128  const auto localPointB2 = proxyB.GetVertex(std::get<1>(ip1));
129  const auto axis = GetUnitVector(GetFwdPerpendicular(localPointB2 - localPointB1),
130  UnitVec::GetZero());
131  const auto normal = Rotate(axis, xfB.q);
132  const auto localPoint = (localPointB1 + localPointB2) / 2;
133  const auto pointB = Transform(localPoint, xfB);
134  const auto localPointA = proxyA.GetVertex(std::get<0>(ip0));
135  const auto pointA = Transform(localPointA, xfA);
136  const auto deltaPoint = pointA - pointB;
137  const auto axisIt = (Dot(deltaPoint, normal) < 0_m)? -axis: axis;
138  return SeparationScenario{proxyA, proxyB, axisIt, localPoint, type};
139  }
140  case SeparationScenario::e_faceA:
141  {
142  const auto ip0 = indices[0];
143  const auto ip1 = indices[1];
144 
145  // Two points on A and one or two points on B.
146  const auto localPointA1 = proxyA.GetVertex(std::get<0>(ip0));
147  const auto localPointA2 = proxyA.GetVertex(std::get<0>(ip1));
148  const auto axis = GetUnitVector(GetFwdPerpendicular(localPointA2 - localPointA1),
149  UnitVec::GetZero());
150  const auto normal = Rotate(axis, xfA.q);
151  const auto localPoint = (localPointA1 + localPointA2) / 2;
152  const auto pointA = Transform(localPoint, xfA);
153  const auto localPointB = proxyB.GetVertex(std::get<1>(ip0));
154  const auto pointB = Transform(localPointB, xfB);
155  const auto deltaPoint = pointB - pointA;
156  const auto axisIt = (Dot(deltaPoint, normal) < 0_m)? -axis: axis;
157  return SeparationScenario{proxyA, proxyB, axisIt, localPoint, type};
158  }
159  case SeparationScenario::e_points:
160  break;
161  }
162 
163  assert(type == SeparationScenario::e_points);
164  const auto ip0 = indices[0];
165  const auto localPointA = proxyA.GetVertex(std::get<0>(ip0));
166  const auto localPointB = proxyB.GetVertex(std::get<1>(ip0));
167  const auto pointA = Transform(localPointA, xfA);
168  const auto pointB = Transform(localPointB, xfB);
169  const auto axis = GetUnitVector(pointB - pointA, UnitVec::GetZero());
170  return SeparationScenario{proxyA, proxyB, axis, GetInvalid<Length2>(), type};
171 }
172 
173 LengthIndexPair FindMinSeparation(const SeparationScenario& scenario,
174  const Transformation& xfA, const Transformation& xfB)
175 {
176  switch (scenario.type)
177  {
178  case SeparationScenario::e_faceA: return FindMinSeparationForFaceA(scenario, xfA, xfB);
179  case SeparationScenario::e_faceB: return FindMinSeparationForFaceB(scenario, xfA, xfB);
180  case SeparationScenario::e_points: break;
181  }
182  assert(scenario.type == SeparationScenario::e_points);
183  return FindMinSeparationForPoints(scenario, xfA, xfB);
184 }
185 
186 Length Evaluate(const SeparationScenario& scenario,
187  const Transformation& xfA, const Transformation& xfB,
188  IndexPair indexPair)
189 {
190  switch (scenario.type)
191  {
192  case SeparationScenario::e_faceA: return EvaluateForFaceA(scenario, xfA, xfB, indexPair);
193  case SeparationScenario::e_faceB: return EvaluateForFaceB(scenario, xfA, xfB, indexPair);
194  case SeparationScenario::e_points: break;
195  }
196  assert(scenario.type == SeparationScenario::e_points);
197  return EvaluateForPoints(scenario, xfA, xfB, indexPair);
198 }
199 
200 } // namespace d2
201 } // namespace playrho