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 <Box2D/Collision/b2Collision.h>
20#include <Box2D/Collision/Shapes/b2PolygonShape.h>
21
22// Find the max separation between poly1 and poly2 using edge normals from poly1.
23static float32 b2FindMaxSeparation(int32* edgeIndex,
24 const b2PolygonShape* poly1, const b2Transform& xf1,
25 const b2PolygonShape* poly2, const b2Transform& xf2)
26{
27 int32 count1 = poly1->m_count;
28 int32 count2 = poly2->m_count;
29 const b2Vec2* n1s = poly1->m_normals;
30 const b2Vec2* v1s = poly1->m_vertices;
31 const b2Vec2* v2s = poly2->m_vertices;
32 b2Transform xf = b2MulT(xf2, xf1);
33
34 int32 bestIndex = 0;
35 float32 maxSeparation = -b2_maxFloat;
36 for (int32 i = 0; i < count1; ++i)
37 {
38 // Get poly1 normal in frame2.
39 b2Vec2 n = b2Mul(xf.q, n1s[i]);
40 b2Vec2 v1 = b2Mul(xf, v1s[i]);
41
42 // Find deepest point for normal i.
43 float32 si = b2_maxFloat;
44 for (int32 j = 0; j < count2; ++j)
45 {
46 float32 sij = b2Dot(n, v2s[j] - v1);
47 if (sij < si)
48 {
49 si = sij;
50 }
51 }
52
53 if (si > maxSeparation)
54 {
55 maxSeparation = si;
56 bestIndex = i;
57 }
58 }
59
60 *edgeIndex = bestIndex;
61 return maxSeparation;
62}
63
64static void b2FindIncidentEdge(b2ClipVertex c[2],
65 const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1,
66 const b2PolygonShape* poly2, const b2Transform& xf2)
67{
68 const b2Vec2* normals1 = poly1->m_normals;
69
70 int32 count2 = poly2->m_count;
71 const b2Vec2* vertices2 = poly2->m_vertices;
72 const b2Vec2* normals2 = poly2->m_normals;
73
74 b2Assert(0 <= edge1 && edge1 < poly1->m_count);
75
76 // Get the normal of the reference edge in poly2's frame.
77 b2Vec2 normal1 = b2MulT(xf2.q, b2Mul(xf1.q, normals1[edge1]));
78
79 // Find the incident edge on poly2.
80 int32 index = 0;
81 float32 minDot = b2_maxFloat;
82 for (int32 i = 0; i < count2; ++i)
83 {
84 float32 dot = b2Dot(normal1, normals2[i]);
85 if (dot < minDot)
86 {
87 minDot = dot;
88 index = i;
89 }
90 }
91
92 // Build the clip vertices for the incident edge.
93 int32 i1 = index;
94 int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0;
95
96 c[0].v = b2Mul(xf2, vertices2[i1]);
97 c[0].id.cf.indexA = (uint8)edge1;
98 c[0].id.cf.indexB = (uint8)i1;
99 c[0].id.cf.typeA = b2ContactFeature::e_face;
100 c[0].id.cf.typeB = b2ContactFeature::e_vertex;
101
102 c[1].v = b2Mul(xf2, vertices2[i2]);
103 c[1].id.cf.indexA = (uint8)edge1;
104 c[1].id.cf.indexB = (uint8)i2;
105 c[1].id.cf.typeA = b2ContactFeature::e_face;
106 c[1].id.cf.typeB = b2ContactFeature::e_vertex;
107}
108
109// Find edge normal of max separation on A - return if separating axis is found
110// Find edge normal of max separation on B - return if separation axis is found
111// Choose reference edge as min(minA, minB)
112// Find incident edge
113// Clip
114
115// The normal points from 1 to 2
116void b2CollidePolygons(b2Manifold* manifold,
117 const b2PolygonShape* polyA, const b2Transform& xfA,
118 const b2PolygonShape* polyB, const b2Transform& xfB)
119{
120 manifold->pointCount = 0;
121 float32 totalRadius = polyA->m_radius + polyB->m_radius;
122
123 int32 edgeA = 0;
124 float32 separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
125 if (separationA > totalRadius)
126 return;
127
128 int32 edgeB = 0;
129 float32 separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
130 if (separationB > totalRadius)
131 return;
132
133 const b2PolygonShape* poly1; // reference polygon
134 const b2PolygonShape* poly2; // incident polygon
135 b2Transform xf1, xf2;
136 int32 edge1; // reference edge
137 uint8 flip;
138 const float32 k_tol = 0.1f * b2_linearSlop;
139
140 if (separationB > separationA + k_tol)
141 {
142 poly1 = polyB;
143 poly2 = polyA;
144 xf1 = xfB;
145 xf2 = xfA;
146 edge1 = edgeB;
147 manifold->type = b2Manifold::e_faceB;
148 flip = 1;
149 }
150 else
151 {
152 poly1 = polyA;
153 poly2 = polyB;
154 xf1 = xfA;
155 xf2 = xfB;
156 edge1 = edgeA;
157 manifold->type = b2Manifold::e_faceA;
158 flip = 0;
159 }
160
161 b2ClipVertex incidentEdge[2];
162 b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
163
164 int32 count1 = poly1->m_count;
165 const b2Vec2* vertices1 = poly1->m_vertices;
166
167 int32 iv1 = edge1;
168 int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
169
170 b2Vec2 v11 = vertices1[iv1];
171 b2Vec2 v12 = vertices1[iv2];
172
173 b2Vec2 localTangent = v12 - v11;
174 localTangent.Normalize();
175
176 b2Vec2 localNormal = b2Cross(localTangent, 1.0f);
177 b2Vec2 planePoint = 0.5f * (v11 + v12);
178
179 b2Vec2 tangent = b2Mul(xf1.q, localTangent);
180 b2Vec2 normal = b2Cross(tangent, 1.0f);
181
182 v11 = b2Mul(xf1, v11);
183 v12 = b2Mul(xf1, v12);
184
185 // Face offset.
186 float32 frontOffset = b2Dot(normal, v11);
187
188 // Side offsets, extended by polytope skin thickness.
189 float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius;
190 float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius;
191
192 // Clip incident edge against extruded edge1 side edges.
193 b2ClipVertex clipPoints1[2];
194 b2ClipVertex clipPoints2[2];
195 int np;
196
197 // Clip to box side 1
198 np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1);
199
200 if (np < 2)
201 return;
202
203 // Clip to negative box side 1
204 np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);
205
206 if (np < 2)
207 {
208 return;
209 }
210
211 // Now clipPoints2 contains the clipped points.
212 manifold->localNormal = localNormal;
213 manifold->localPoint = planePoint;
214
215 int32 pointCount = 0;
216 for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
217 {
218 float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
219
220 if (separation <= totalRadius)
221 {
222 b2ManifoldPoint* cp = manifold->points + pointCount;
223 cp->localPoint = b2MulT(xf2, clipPoints2[i].v);
224 cp->id = clipPoints2[i].id;
225 if (flip)
226 {
227 // Swap features
228 b2ContactFeature cf = cp->id.cf;
229 cp->id.cf.indexA = cf.indexB;
230 cp->id.cf.indexB = cf.indexA;
231 cp->id.cf.typeA = cf.typeB;
232 cp->id.cf.typeB = cf.typeA;
233 }
234 ++pointCount;
235 }
236 }
237
238 manifold->pointCount = pointCount;
239}
240