1 /*
2 * Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
3 *
4 * This software is provided 'as-is', without any express or implied
5 * warranty. In no event will the authors be held liable for any damages
6 * arising from the use of this software.
7 * Permission is granted to anyone to use this software for any purpose,
8 * including commercial applications, and to alter it and redistribute it
9 * freely, subject to the following restrictions:
10 * 1. The origin of this software must not be misrepresented; you must not
11 * claim that you wrote the original software. If you use this software
12 * in a product, an acknowledgment in the product documentation would be
13 * appreciated but is not required.
14 * 2. Altered source versions must be plainly marked as such, and must not be
15 * misrepresented as being the original software.
16 * 3. This notice may not be removed or altered from any source distribution.
17 */
18
19 #include "b2Collision.h"
20 #include "Shapes/b2PolygonShape.h"
21
22 struct ClipVertex
23 {
24 b2Vec2 v;
25 b2ContactID id;
26 };
27
ClipSegmentToLine(ClipVertex vOut[2],ClipVertex vIn[2],const b2Vec2 & normal,float32 offset)28 static int32 ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2],
29 const b2Vec2& normal, float32 offset)
30 {
31 // Start with no output points
32 int32 numOut = 0;
33
34 // Calculate the distance of end points to the line
35 float32 distance0 = b2Dot(normal, vIn[0].v) - offset;
36 float32 distance1 = b2Dot(normal, vIn[1].v) - offset;
37
38 // If the points are behind the plane
39 if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
40 if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
41
42 // If the points are on different sides of the plane
43 if (distance0 * distance1 < 0.0f)
44 {
45 // Find intersection point of edge and plane
46 float32 interp = distance0 / (distance0 - distance1);
47 vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
48 if (distance0 > 0.0f)
49 {
50 vOut[numOut].id = vIn[0].id;
51 }
52 else
53 {
54 vOut[numOut].id = vIn[1].id;
55 }
56 ++numOut;
57 }
58
59 return numOut;
60 }
61
62 // Find the separation between poly1 and poly2 for a give edge normal on poly1.
EdgeSeparation(const b2PolygonShape * poly1,const b2XForm & xf1,int32 edge1,const b2PolygonShape * poly2,const b2XForm & xf2)63 static float32 EdgeSeparation(const b2PolygonShape* poly1, const b2XForm& xf1, int32 edge1,
64 const b2PolygonShape* poly2, const b2XForm& xf2)
65 {
66 int32 count1 = poly1->GetVertexCount();
67 const b2Vec2* vertices1 = poly1->GetVertices();
68 const b2Vec2* normals1 = poly1->GetNormals();
69
70 int32 count2 = poly2->GetVertexCount();
71 const b2Vec2* vertices2 = poly2->GetVertices();
72
73 b2Assert(0 <= edge1 && edge1 < count1);
74
75 // Convert normal from poly1's frame into poly2's frame.
76 b2Vec2 normal1World = b2Mul(xf1.R, normals1[edge1]);
77 b2Vec2 normal1 = b2MulT(xf2.R, normal1World);
78
79 // Find support vertex on poly2 for -normal.
80 int32 index = 0;
81 float32 minDot = B2_FLT_MAX;
82
83 for (int32 i = 0; i < count2; ++i)
84 {
85 float32 dot = b2Dot(vertices2[i], normal1);
86 if (dot < minDot)
87 {
88 minDot = dot;
89 index = i;
90 }
91 }
92
93 b2Vec2 v1 = b2Mul(xf1, vertices1[edge1]);
94 b2Vec2 v2 = b2Mul(xf2, vertices2[index]);
95 float32 separation = b2Dot(v2 - v1, normal1World);
96 return separation;
97 }
98
99 // Find the max separation between poly1 and poly2 using edge normals from poly1.
FindMaxSeparation(int32 * edgeIndex,const b2PolygonShape * poly1,const b2XForm & xf1,const b2PolygonShape * poly2,const b2XForm & xf2)100 static float32 FindMaxSeparation(int32* edgeIndex,
101 const b2PolygonShape* poly1, const b2XForm& xf1,
102 const b2PolygonShape* poly2, const b2XForm& xf2)
103 {
104 int32 count1 = poly1->GetVertexCount();
105 const b2Vec2* normals1 = poly1->GetNormals();
106
107 // Vector pointing from the centroid of poly1 to the centroid of poly2.
108 b2Vec2 d = b2Mul(xf2, poly2->GetCentroid()) - b2Mul(xf1, poly1->GetCentroid());
109 b2Vec2 dLocal1 = b2MulT(xf1.R, d);
110
111 // Find edge normal on poly1 that has the largest projection onto d.
112 int32 edge = 0;
113 float32 maxDot = -B2_FLT_MAX;
114 for (int32 i = 0; i < count1; ++i)
115 {
116 float32 dot = b2Dot(normals1[i], dLocal1);
117 if (dot > maxDot)
118 {
119 maxDot = dot;
120 edge = i;
121 }
122 }
123
124 // Get the separation for the edge normal.
125 float32 s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
126 if (s > 0.0f)
127 {
128 return s;
129 }
130
131 // Check the separation for the previous edge normal.
132 int32 prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
133 float32 sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
134 if (sPrev > 0.0f)
135 {
136 return sPrev;
137 }
138
139 // Check the separation for the next edge normal.
140 int32 nextEdge = edge + 1 < count1 ? edge + 1 : 0;
141 float32 sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
142 if (sNext > 0.0f)
143 {
144 return sNext;
145 }
146
147 // Find the best edge and the search direction.
148 int32 bestEdge;
149 float32 bestSeparation;
150 int32 increment;
151 if (sPrev > s && sPrev > sNext)
152 {
153 increment = -1;
154 bestEdge = prevEdge;
155 bestSeparation = sPrev;
156 }
157 else if (sNext > s)
158 {
159 increment = 1;
160 bestEdge = nextEdge;
161 bestSeparation = sNext;
162 }
163 else
164 {
165 *edgeIndex = edge;
166 return s;
167 }
168
169 // Perform a local search for the best edge normal.
170 for ( ; ; )
171 {
172 if (increment == -1)
173 edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
174 else
175 edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
176
177 s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
178 if (s > 0.0f)
179 {
180 return s;
181 }
182
183 if (s > bestSeparation)
184 {
185 bestEdge = edge;
186 bestSeparation = s;
187 }
188 else
189 {
190 break;
191 }
192 }
193
194 *edgeIndex = bestEdge;
195 return bestSeparation;
196 }
197
FindIncidentEdge(ClipVertex c[2],const b2PolygonShape * poly1,const b2XForm & xf1,int32 edge1,const b2PolygonShape * poly2,const b2XForm & xf2)198 static void FindIncidentEdge(ClipVertex c[2],
199 const b2PolygonShape* poly1, const b2XForm& xf1, int32 edge1,
200 const b2PolygonShape* poly2, const b2XForm& xf2)
201 {
202 int32 count1 = poly1->GetVertexCount();
203 const b2Vec2* normals1 = poly1->GetNormals();
204
205 int32 count2 = poly2->GetVertexCount();
206 const b2Vec2* vertices2 = poly2->GetVertices();
207 const b2Vec2* normals2 = poly2->GetNormals();
208
209 b2Assert(0 <= edge1 && edge1 < count1);
210
211 // Get the normal of the reference edge in poly2's frame.
212 b2Vec2 normal1 = b2MulT(xf2.R, b2Mul(xf1.R, normals1[edge1]));
213
214 // Find the incident edge on poly2.
215 int32 index = 0;
216 float32 minDot = B2_FLT_MAX;
217 for (int32 i = 0; i < count2; ++i)
218 {
219 float32 dot = b2Dot(normal1, normals2[i]);
220 if (dot < minDot)
221 {
222 minDot = dot;
223 index = i;
224 }
225 }
226
227 // Build the clip vertices for the incident edge.
228 int32 i1 = index;
229 int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0;
230
231 c[0].v = b2Mul(xf2, vertices2[i1]);
232 c[0].id.features.referenceEdge = (uint8)edge1;
233 c[0].id.features.incidentEdge = (uint8)i1;
234 c[0].id.features.incidentVertex = 0;
235
236 c[1].v = b2Mul(xf2, vertices2[i2]);
237 c[1].id.features.referenceEdge = (uint8)edge1;
238 c[1].id.features.incidentEdge = (uint8)i2;
239 c[1].id.features.incidentVertex = 1;
240 }
241
242 // Find edge normal of max separation on A - return if separating axis is found
243 // Find edge normal of max separation on B - return if separation axis is found
244 // Choose reference edge as min(minA, minB)
245 // Find incident edge
246 // Clip
247
248 // The normal points from 1 to 2
b2CollidePolygons(b2Manifold * manifold,const b2PolygonShape * polyA,const b2XForm & xfA,const b2PolygonShape * polyB,const b2XForm & xfB)249 void b2CollidePolygons(b2Manifold* manifold,
250 const b2PolygonShape* polyA, const b2XForm& xfA,
251 const b2PolygonShape* polyB, const b2XForm& xfB)
252 {
253 manifold->pointCount = 0;
254
255 int32 edgeA = 0;
256 float32 separationA = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
257 if (separationA > 0.0f)
258 return;
259
260 int32 edgeB = 0;
261 float32 separationB = FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
262 if (separationB > 0.0f)
263 return;
264
265 const b2PolygonShape* poly1; // reference poly
266 const b2PolygonShape* poly2; // incident poly
267 b2XForm xf1, xf2;
268 int32 edge1; // reference edge
269 uint8 flip;
270 const float32 k_relativeTol = 0.98f;
271 const float32 k_absoluteTol = 0.001f;
272
273 // TODO_ERIN use "radius" of poly for absolute tolerance.
274 if (separationB > k_relativeTol * separationA + k_absoluteTol)
275 {
276 poly1 = polyB;
277 poly2 = polyA;
278 xf1 = xfB;
279 xf2 = xfA;
280 edge1 = edgeB;
281 flip = 1;
282 }
283 else
284 {
285 poly1 = polyA;
286 poly2 = polyB;
287 xf1 = xfA;
288 xf2 = xfB;
289 edge1 = edgeA;
290 flip = 0;
291 }
292
293 ClipVertex incidentEdge[2];
294 FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
295
296 int32 count1 = poly1->GetVertexCount();
297 const b2Vec2* vertices1 = poly1->GetVertices();
298
299 b2Vec2 v11 = vertices1[edge1];
300 b2Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];
301
302 b2Vec2 dv = v12 - v11;
303 b2Vec2 sideNormal = b2Mul(xf1.R, v12 - v11);
304 sideNormal.Normalize();
305 b2Vec2 frontNormal = b2Cross(sideNormal, 1.0f);
306
307 v11 = b2Mul(xf1, v11);
308 v12 = b2Mul(xf1, v12);
309
310 float32 frontOffset = b2Dot(frontNormal, v11);
311 float32 sideOffset1 = -b2Dot(sideNormal, v11);
312 float32 sideOffset2 = b2Dot(sideNormal, v12);
313
314 // Clip incident edge against extruded edge1 side edges.
315 ClipVertex clipPoints1[2];
316 ClipVertex clipPoints2[2];
317 int np;
318
319 // Clip to box side 1
320 np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
321
322 if (np < 2)
323 return;
324
325 // Clip to negative box side 1
326 np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2);
327
328 if (np < 2)
329 return;
330
331 // Now clipPoints2 contains the clipped points.
332 manifold->normal = flip ? -frontNormal : frontNormal;
333
334 int32 pointCount = 0;
335 for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
336 {
337 float32 separation = b2Dot(frontNormal, clipPoints2[i].v) - frontOffset;
338
339 if (separation <= 0.0f)
340 {
341 b2ManifoldPoint* cp = manifold->points + pointCount;
342 cp->separation = separation;
343 cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v);
344 cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v);
345 cp->id = clipPoints2[i].id;
346 cp->id.features.flip = flip;
347 ++pointCount;
348 }
349 }
350
351 manifold->pointCount = pointCount;
352 }
353