← Back to branch summary

~ubuntu-branches/ubuntu/vivid/emscripten/vivid

« back to all changes in this revision

Viewing changes to tests/box2d/Box2D/Collision/b2CollidePolygon.cpp

  • Committer: Package Import Robot
  • Author(s): Sylvestre Ledru
  • Date: 2013-05-02 13:11:51 UTC
  • Revision ID: package-import@ubuntu.com-20130502131151-q8dvteqr1ef2x7xz
Tags: upstream-1.4.1~20130504~adb56cb
Import upstream version 1.4.1~20130504~adb56cb

Show diffs side-by-side

added added

removed removed

Lines of Context:
tests/box2d/Box2D/Collision/b2CollidePolygon.cpp
 
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 separation between poly1 and poly2 for a give edge normal on poly1.
 
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.
 
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
 
 
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
 
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
}