1 /*
2 * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
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 // 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 const b2Vec2* vertices1 = poly1->m_vertices;
27 const b2Vec2* normals1 = poly1->m_normals;
28
29 int32 count2 = poly2->m_vertexCount;
30 const b2Vec2* vertices2 = poly2->m_vertices;
31
32 b2Assert(0 <= edge1 && edge1 < poly1->m_vertexCount);
33
34 // Convert normal from poly1's frame into poly2's frame.
35 b2Vec2 normal1World = b2Mul(xf1.q, normals1[edge1]);
36 b2Vec2 normal1 = b2MulT(xf2.q, normal1World);
37
38 // Find support vertex on poly2 for -normal.
39 int32 index = 0;
40 float32 minDot = b2_maxFloat;
41
42 for (int32 i = 0; i < count2; ++i)
43 {
44 float32 dot = b2Dot(vertices2[i], normal1);
45 if (dot < minDot)
46 {
47 minDot = dot;
48 index = i;
49 }
50 }
51
52 b2Vec2 v1 = b2Mul(xf1, vertices1[edge1]);
53 b2Vec2 v2 = b2Mul(xf2, vertices2[index]);
54 float32 separation = b2Dot(v2 - v1, normal1World);
55 return separation;
56 }
57
58 // 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)59 static float32 b2FindMaxSeparation(int32* edgeIndex,
60 const b2PolygonShape* poly1, const b2Transform& xf1,
61 const b2PolygonShape* poly2, const b2Transform& xf2)
62 {
63 int32 count1 = poly1->m_vertexCount;
64 const b2Vec2* normals1 = poly1->m_normals;
65
66 // Vector pointing from the centroid of poly1 to the centroid of poly2.
67 b2Vec2 d = b2Mul(xf2, poly2->m_centroid) - b2Mul(xf1, poly1->m_centroid);
68 b2Vec2 dLocal1 = b2MulT(xf1.q, d);
69
70 // Find edge normal on poly1 that has the largest projection onto d.
71 int32 edge = 0;
72 float32 maxDot = -b2_maxFloat;
73 for (int32 i = 0; i < count1; ++i)
74 {
75 float32 dot = b2Dot(normals1[i], dLocal1);
76 if (dot > maxDot)
77 {
78 maxDot = dot;
79 edge = i;
80 }
81 }
82
83 // Get the separation for the edge normal.
84 float32 s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
85
86 // Check the separation for the previous edge normal.
87 int32 prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
88 float32 sPrev = b2EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
89
90 // Check the separation for the next edge normal.
91 int32 nextEdge = edge + 1 < count1 ? edge + 1 : 0;
92 float32 sNext = b2EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
93
94 // Find the best edge and the search direction.
95 int32 bestEdge;
96 float32 bestSeparation;
97 int32 increment;
98 if (sPrev > s && sPrev > sNext)
99 {
100 increment = -1;
101 bestEdge = prevEdge;
102 bestSeparation = sPrev;
103 }
104 else if (sNext > s)
105 {
106 increment = 1;
107 bestEdge = nextEdge;
108 bestSeparation = sNext;
109 }
110 else
111 {
112 *edgeIndex = edge;
113 return s;
114 }
115
116 // Perform a local search for the best edge normal.
117 for ( ; ; )
118 {
119 if (increment == -1)
120 edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
121 else
122 edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
123
124 s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
125
126 if (s > bestSeparation)
127 {
128 bestEdge = edge;
129 bestSeparation = s;
130 }
131 else
132 {
133 break;
134 }
135 }
136
137 *edgeIndex = bestEdge;
138 return bestSeparation;
139 }
140
b2FindIncidentEdge(b2ClipVertex c[2],const b2PolygonShape * poly1,const b2Transform & xf1,int32 edge1,const b2PolygonShape * poly2,const b2Transform & xf2)141 static void b2FindIncidentEdge(b2ClipVertex c[2],
142 const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1,
143 const b2PolygonShape* poly2, const b2Transform& xf2)
144 {
145 const b2Vec2* normals1 = poly1->m_normals;
146
147 int32 count2 = poly2->m_vertexCount;
148 const b2Vec2* vertices2 = poly2->m_vertices;
149 const b2Vec2* normals2 = poly2->m_normals;
150
151 b2Assert(0 <= edge1 && edge1 < poly1->m_vertexCount);
152
153 // Get the normal of the reference edge in poly2's frame.
154 b2Vec2 normal1 = b2MulT(xf2.q, b2Mul(xf1.q, normals1[edge1]));
155
156 // Find the incident edge on poly2.
157 int32 index = 0;
158 float32 minDot = b2_maxFloat;
159 for (int32 i = 0; i < count2; ++i)
160 {
161 float32 dot = b2Dot(normal1, normals2[i]);
162 if (dot < minDot)
163 {
164 minDot = dot;
165 index = i;
166 }
167 }
168
169 // Build the clip vertices for the incident edge.
170 int32 i1 = index;
171 int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0;
172
173 c[0].v = b2Mul(xf2, vertices2[i1]);
174 c[0].id.cf.indexA = (uint8)edge1;
175 c[0].id.cf.indexB = (uint8)i1;
176 c[0].id.cf.typeA = b2ContactFeature::e_face;
177 c[0].id.cf.typeB = b2ContactFeature::e_vertex;
178
179 c[1].v = b2Mul(xf2, vertices2[i2]);
180 c[1].id.cf.indexA = (uint8)edge1;
181 c[1].id.cf.indexB = (uint8)i2;
182 c[1].id.cf.typeA = b2ContactFeature::e_face;
183 c[1].id.cf.typeB = b2ContactFeature::e_vertex;
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 int32 iv1 = edge1;
246 int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
247
248 b2Vec2 v11 = vertices1[iv1];
249 b2Vec2 v12 = vertices1[iv2];
250
251 b2Vec2 localTangent = v12 - v11;
252 localTangent.Normalize();
253
254 b2Vec2 localNormal = b2Cross(localTangent, 1.0f);
255 b2Vec2 planePoint = 0.5f * (v11 + v12);
256
257 b2Vec2 tangent = b2Mul(xf1.q, localTangent);
258 b2Vec2 normal = b2Cross(tangent, 1.0f);
259
260 v11 = b2Mul(xf1, v11);
261 v12 = b2Mul(xf1, v12);
262
263 // Face offset.
264 float32 frontOffset = b2Dot(normal, v11);
265
266 // Side offsets, extended by polytope skin thickness.
267 float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius;
268 float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius;
269
270 // Clip incident edge against extruded edge1 side edges.
271 b2ClipVertex clipPoints1[2];
272 b2ClipVertex clipPoints2[2];
273 int np;
274
275 // Clip to box side 1
276 np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1);
277
278 if (np < 2)
279 return;
280
281 // Clip to negative box side 1
282 np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);
283
284 if (np < 2)
285 {
286 return;
287 }
288
289 // Now clipPoints2 contains the clipped points.
290 manifold->localNormal = localNormal;
291 manifold->localPoint = planePoint;
292
293 int32 pointCount = 0;
294 for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
295 {
296 float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
297
298 if (separation <= totalRadius)
299 {
300 b2ManifoldPoint* cp = manifold->points + pointCount;
301 cp->localPoint = b2MulT(xf2, clipPoints2[i].v);
302 cp->id = clipPoints2[i].id;
303 if (flip)
304 {
305 // Swap features
306 b2ContactFeature cf = cp->id.cf;
307 cp->id.cf.indexA = cf.indexB;
308 cp->id.cf.indexB = cf.indexA;
309 cp->id.cf.typeA = cf.typeB;
310 cp->id.cf.typeB = cf.typeA;
311 }
312 ++pointCount;
313 }
314 }
315
316 manifold->pointCount = pointCount;
317 }
318