1 /*
2 * Copyright (c) 2006-2009 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 "b2PolygonShape.h"
21
22 // Find the separation between poly1 and poly2 for a give edge normal on poly1.
b2EdgeSeparation(const b2PolygonShape * poly1,const b2Transform & xf1,int32 edge1,const b2PolygonShape * poly2,const b2Transform & xf2)23 static float32 b2EdgeSeparation(const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1,
24 const b2PolygonShape* poly2, const b2Transform& xf2)
25 {
26 int32 count1 = poly1->m_vertexCount;
27 const b2Vec2* vertices1 = poly1->m_vertices;
28 const b2Vec2* normals1 = poly1->m_normals;
29
30 int32 count2 = poly2->m_vertexCount;
31 const b2Vec2* vertices2 = poly2->m_vertices;
32
33 b2Assert(0 <= edge1 && edge1 < count1);
34
35 // Convert normal from poly1's frame into poly2's frame.
36 b2Vec2 normal1World = b2Mul(xf1.R, normals1[edge1]);
37 b2Vec2 normal1 = b2MulT(xf2.R, normal1World);
38
39 // Find support vertex on poly2 for -normal.
40 int32 index = 0;
41 float32 minDot = b2_maxFloat;
42
43 for (int32 i = 0; i < count2; ++i)
44 {
45 float32 dot = b2Dot(vertices2[i], normal1);
46 if (dot < minDot)
47 {
48 minDot = dot;
49 index = i;
50 }
51 }
52
53 b2Vec2 v1 = b2Mul(xf1, vertices1[edge1]);
54 b2Vec2 v2 = b2Mul(xf2, vertices2[index]);
55 float32 separation = b2Dot(v2 - v1, normal1World);
56 return separation;
57 }
58
59 // Find the max separation between poly1 and poly2 using edge normals from poly1.
b2FindMaxSeparation(int32 * edgeIndex,const b2PolygonShape * poly1,const b2Transform & xf1,const b2PolygonShape * poly2,const b2Transform & xf2)60 static float32 b2FindMaxSeparation(int32* edgeIndex,
61 const b2PolygonShape* poly1, const b2Transform& xf1,
62 const b2PolygonShape* poly2, const b2Transform& xf2)
63 {
64 int32 count1 = poly1->m_vertexCount;
65 const b2Vec2* normals1 = poly1->m_normals;
66
67 // Vector pointing from the centroid of poly1 to the centroid of poly2.
68 b2Vec2 d = b2Mul(xf2, poly2->m_centroid) - b2Mul(xf1, poly1->m_centroid);
69 b2Vec2 dLocal1 = b2MulT(xf1.R, d);
70
71 // Find edge normal on poly1 that has the largest projection onto d.
72 int32 edge = 0;
73 float32 maxDot = -b2_maxFloat;
74 for (int32 i = 0; i < count1; ++i)
75 {
76 float32 dot = b2Dot(normals1[i], dLocal1);
77 if (dot > maxDot)
78 {
79 maxDot = dot;
80 edge = i;
81 }
82 }
83
84 // Get the separation for the edge normal.
85 float32 s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
86
87 // Check the separation for the previous edge normal.
88 int32 prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
89 float32 sPrev = b2EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
90
91 // Check the separation for the next edge normal.
92 int32 nextEdge = edge + 1 < count1 ? edge + 1 : 0;
93 float32 sNext = b2EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
94
95 // Find the best edge and the search direction.
96 int32 bestEdge;
97 float32 bestSeparation;
98 int32 increment;
99 if (sPrev > s && sPrev > sNext)
100 {
101 increment = -1;
102 bestEdge = prevEdge;
103 bestSeparation = sPrev;
104 }
105 else if (sNext > s)
106 {
107 increment = 1;
108 bestEdge = nextEdge;
109 bestSeparation = sNext;
110 }
111 else
112 {
113 *edgeIndex = edge;
114 return s;
115 }
116
117 // Perform a local search for the best edge normal.
118 for ( ; ; )
119 {
120 if (increment == -1)
121 edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
122 else
123 edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
124
125 s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
126
127 if (s > bestSeparation)
128 {
129 bestEdge = edge;
130 bestSeparation = s;
131 }
132 else
133 {
134 break;
135 }
136 }
137
138 *edgeIndex = bestEdge;
139 return bestSeparation;
140 }
141
b2FindIncidentEdge(b2ClipVertex c[2],const b2PolygonShape * poly1,const b2Transform & xf1,int32 edge1,const b2PolygonShape * poly2,const b2Transform & xf2)142 static void b2FindIncidentEdge(b2ClipVertex c[2],
143 const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1,
144 const b2PolygonShape* poly2, const b2Transform& xf2)
145 {
146 int32 count1 = poly1->m_vertexCount;
147 const b2Vec2* normals1 = poly1->m_normals;
148
149 int32 count2 = poly2->m_vertexCount;
150 const b2Vec2* vertices2 = poly2->m_vertices;
151 const b2Vec2* normals2 = poly2->m_normals;
152
153 b2Assert(0 <= edge1 && edge1 < count1);
154
155 // Get the normal of the reference edge in poly2's frame.
156 b2Vec2 normal1 = b2MulT(xf2.R, b2Mul(xf1.R, normals1[edge1]));
157
158 // Find the incident edge on poly2.
159 int32 index = 0;
160 float32 minDot = b2_maxFloat;
161 for (int32 i = 0; i < count2; ++i)
162 {
163 float32 dot = b2Dot(normal1, normals2[i]);
164 if (dot < minDot)
165 {
166 minDot = dot;
167 index = i;
168 }
169 }
170
171 // Build the clip vertices for the incident edge.
172 int32 i1 = index;
173 int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0;
174
175 c[0].v = b2Mul(xf2, vertices2[i1]);
176 c[0].id.features.referenceEdge = (uint8)edge1;
177 c[0].id.features.incidentEdge = (uint8)i1;
178 c[0].id.features.incidentVertex = 0;
179
180 c[1].v = b2Mul(xf2, vertices2[i2]);
181 c[1].id.features.referenceEdge = (uint8)edge1;
182 c[1].id.features.incidentEdge = (uint8)i2;
183 c[1].id.features.incidentVertex = 1;
184 }
185
186 // Find edge normal of max separation on A - return if separating axis is found
187 // Find edge normal of max separation on B - return if separation axis is found
188 // Choose reference edge as min(minA, minB)
189 // Find incident edge
190 // Clip
191
192 // The normal points from 1 to 2
b2CollidePolygons(b2Manifold * manifold,const b2PolygonShape * polyA,const b2Transform & xfA,const b2PolygonShape * polyB,const b2Transform & xfB)193 void b2CollidePolygons(b2Manifold* manifold,
194 const b2PolygonShape* polyA, const b2Transform& xfA,
195 const b2PolygonShape* polyB, const b2Transform& xfB)
196 {
197 manifold->pointCount = 0;
198 float32 totalRadius = polyA->m_radius + polyB->m_radius;
199
200 int32 edgeA = 0;
201 float32 separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
202 if (separationA > totalRadius)
203 return;
204
205 int32 edgeB = 0;
206 float32 separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
207 if (separationB > totalRadius)
208 return;
209
210 const b2PolygonShape* poly1; // reference polygon
211 const b2PolygonShape* poly2; // incident polygon
212 b2Transform xf1, xf2;
213 int32 edge1; // reference edge
214 uint8 flip;
215 const float32 k_relativeTol = 0.98f;
216 const float32 k_absoluteTol = 0.001f;
217
218 if (separationB > k_relativeTol * separationA + k_absoluteTol)
219 {
220 poly1 = polyB;
221 poly2 = polyA;
222 xf1 = xfB;
223 xf2 = xfA;
224 edge1 = edgeB;
225 manifold->type = b2Manifold::e_faceB;
226 flip = 1;
227 }
228 else
229 {
230 poly1 = polyA;
231 poly2 = polyB;
232 xf1 = xfA;
233 xf2 = xfB;
234 edge1 = edgeA;
235 manifold->type = b2Manifold::e_faceA;
236 flip = 0;
237 }
238
239 b2ClipVertex incidentEdge[2];
240 b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
241
242 int32 count1 = poly1->m_vertexCount;
243 const b2Vec2* vertices1 = poly1->m_vertices;
244
245 b2Vec2 v11 = vertices1[edge1];
246 b2Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];
247
248 b2Vec2 localTangent = v12 - v11;
249 localTangent.Normalize();
250
251 b2Vec2 localNormal = b2Cross(localTangent, 1.0f);
252 b2Vec2 planePoint = 0.5f * (v11 + v12);
253
254 b2Vec2 tangent = b2Mul(xf1.R, localTangent);
255 b2Vec2 normal = b2Cross(tangent, 1.0f);
256
257 v11 = b2Mul(xf1, v11);
258 v12 = b2Mul(xf1, v12);
259
260 // Face offset.
261 float32 frontOffset = b2Dot(normal, v11);
262
263 // Side offsets, extended by polytope skin thickness.
264 float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius;
265 float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius;
266
267 // Clip incident edge against extruded edge1 side edges.
268 b2ClipVertex clipPoints1[2];
269 b2ClipVertex clipPoints2[2];
270 int np;
271
272 // Clip to box side 1
273 np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1);
274
275 if (np < 2)
276 return;
277
278 // Clip to negative box side 1
279 np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2);
280
281 if (np < 2)
282 {
283 return;
284 }
285
286 // Now clipPoints2 contains the clipped points.
287 manifold->localNormal = localNormal;
288 manifold->localPoint = planePoint;
289
290 int32 pointCount = 0;
291 for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
292 {
293 float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
294
295 if (separation <= totalRadius)
296 {
297 b2ManifoldPoint* cp = manifold->points + pointCount;
298 cp->localPoint = b2MulT(xf2, clipPoints2[i].v);
299 cp->id = clipPoints2[i].id;
300 cp->id.features.flip = flip;
301 ++pointCount;
302 }
303 }
304
305 manifold->pointCount = pointCount;
306 }
307