← Back to branch summary

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

« back to all changes in this revision

Viewing changes to tests/box2d/Box2D/Collision/b2Distance.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/b2Distance.cpp
 
1
/*
 
2
* Copyright (c) 2007-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/b2Distance.h>
 
20
#include <Box2D/Collision/Shapes/b2CircleShape.h>
 
21
#include <Box2D/Collision/Shapes/b2EdgeShape.h>
 
22
#include <Box2D/Collision/Shapes/b2ChainShape.h>
 
23
#include <Box2D/Collision/Shapes/b2PolygonShape.h>
 
24
 
 
25
// GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
 
26
int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters;
 
27
 
 
28
void b2DistanceProxy::Set(const b2Shape* shape, int32 index)
 
29
{
 
30
        switch (shape->GetType())
 
31
        {
 
32
        case b2Shape::e_circle:
 
33
                {
 
34
                        const b2CircleShape* circle = (b2CircleShape*)shape;
 
35
                        m_vertices = &circle->m_p;
 
36
                        m_count = 1;
 
37
                        m_radius = circle->m_radius;
 
38
                }
 
39
                break;
 
40
 
 
41
        case b2Shape::e_polygon:
 
42
                {
 
43
                        const b2PolygonShape* polygon = (b2PolygonShape*)shape;
 
44
                        m_vertices = polygon->m_vertices;
 
45
                        m_count = polygon->m_vertexCount;
 
46
                        m_radius = polygon->m_radius;
 
47
                }
 
48
                break;
 
49
 
 
50
        case b2Shape::e_chain:
 
51
                {
 
52
                        const b2ChainShape* chain = (b2ChainShape*)shape;
 
53
                        b2Assert(0 <= index && index < chain->m_count);
 
54
 
 
55
                        m_buffer[0] = chain->m_vertices[index];
 
56
                        if (index + 1 < chain->m_count)
 
57
                        {
 
58
                                m_buffer[1] = chain->m_vertices[index + 1];
 
59
                        }
 
60
                        else
 
61
                        {
 
62
                                m_buffer[1] = chain->m_vertices[0];
 
63
                        }
 
64
 
 
65
                        m_vertices = m_buffer;
 
66
                        m_count = 2;
 
67
                        m_radius = chain->m_radius;
 
68
                }
 
69
                break;
 
70
 
 
71
        case b2Shape::e_edge:
 
72
                {
 
73
                        const b2EdgeShape* edge = (b2EdgeShape*)shape;
 
74
                        m_vertices = &edge->m_vertex1;
 
75
                        m_count = 2;
 
76
                        m_radius = edge->m_radius;
 
77
                }
 
78
                break;
 
79
 
 
80
        default:
 
81
                b2Assert(false);
 
82
        }
 
83
}
 
84
 
 
85
 
 
86
struct b2SimplexVertex
 
87
{
 
88
        b2Vec2 wA;              // support point in proxyA
 
89
        b2Vec2 wB;              // support point in proxyB
 
90
        b2Vec2 w;               // wB - wA
 
91
        float32 a;              // barycentric coordinate for closest point
 
92
        int32 indexA;   // wA index
 
93
        int32 indexB;   // wB index
 
94
};
 
95
 
 
96
struct b2Simplex
 
97
{
 
98
        void ReadCache( const b2SimplexCache* cache,
 
99
                                        const b2DistanceProxy* proxyA, const b2Transform& transformA,
 
100
                                        const b2DistanceProxy* proxyB, const b2Transform& transformB)
 
101
        {
 
102
                b2Assert(cache->count <= 3);
 
103
                
 
104
                // Copy data from cache.
 
105
                m_count = cache->count;
 
106
                b2SimplexVertex* vertices = &m_v1;
 
107
                for (int32 i = 0; i < m_count; ++i)
 
108
                {
 
109
                        b2SimplexVertex* v = vertices + i;
 
110
                        v->indexA = cache->indexA[i];
 
111
                        v->indexB = cache->indexB[i];
 
112
                        b2Vec2 wALocal = proxyA->GetVertex(v->indexA);
 
113
                        b2Vec2 wBLocal = proxyB->GetVertex(v->indexB);
 
114
                        v->wA = b2Mul(transformA, wALocal);
 
115
                        v->wB = b2Mul(transformB, wBLocal);
 
116
                        v->w = v->wB - v->wA;
 
117
                        v->a = 0.0f;
 
118
                }
 
119
 
 
120
                // Compute the new simplex metric, if it is substantially different than
 
121
                // old metric then flush the simplex.
 
122
                if (m_count > 1)
 
123
                {
 
124
                        float32 metric1 = cache->metric;
 
125
                        float32 metric2 = GetMetric();
 
126
                        if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon)
 
127
                        {
 
128
                                // Reset the simplex.
 
129
                                m_count = 0;
 
130
                        }
 
131
                }
 
132
 
 
133
                // If the cache is empty or invalid ...
 
134
                if (m_count == 0)
 
135
                {
 
136
                        b2SimplexVertex* v = vertices + 0;
 
137
                        v->indexA = 0;
 
138
                        v->indexB = 0;
 
139
                        b2Vec2 wALocal = proxyA->GetVertex(0);
 
140
                        b2Vec2 wBLocal = proxyB->GetVertex(0);
 
141
                        v->wA = b2Mul(transformA, wALocal);
 
142
                        v->wB = b2Mul(transformB, wBLocal);
 
143
                        v->w = v->wB - v->wA;
 
144
                        m_count = 1;
 
145
                }
 
146
        }
 
147
 
 
148
        void WriteCache(b2SimplexCache* cache) const
 
149
        {
 
150
                cache->metric = GetMetric();
 
151
                cache->count = uint16(m_count);
 
152
                const b2SimplexVertex* vertices = &m_v1;
 
153
                for (int32 i = 0; i < m_count; ++i)
 
154
                {
 
155
                        cache->indexA[i] = uint8(vertices[i].indexA);
 
156
                        cache->indexB[i] = uint8(vertices[i].indexB);
 
157
                }
 
158
        }
 
159
 
 
160
        b2Vec2 GetSearchDirection() const
 
161
        {
 
162
                switch (m_count)
 
163
                {
 
164
                case 1:
 
165
                        return -m_v1.w;
 
166
 
 
167
                case 2:
 
168
                        {
 
169
                                b2Vec2 e12 = m_v2.w - m_v1.w;
 
170
                                float32 sgn = b2Cross(e12, -m_v1.w);
 
171
                                if (sgn > 0.0f)
 
172
                                {
 
173
                                        // Origin is left of e12.
 
174
                                        return b2Cross(1.0f, e12);
 
175
                                }
 
176
                                else
 
177
                                {
 
178
                                        // Origin is right of e12.
 
179
                                        return b2Cross(e12, 1.0f);
 
180
                                }
 
181
                        }
 
182
 
 
183
                default:
 
184
                        b2Assert(false);
 
185
                        return b2Vec2_zero;
 
186
                }
 
187
        }
 
188
 
 
189
        b2Vec2 GetClosestPoint() const
 
190
        {
 
191
                switch (m_count)
 
192
                {
 
193
                case 0:
 
194
                        b2Assert(false);
 
195
                        return b2Vec2_zero;
 
196
 
 
197
                case 1:
 
198
                        return m_v1.w;
 
199
 
 
200
                case 2:
 
201
                        return m_v1.a * m_v1.w + m_v2.a * m_v2.w;
 
202
 
 
203
                case 3:
 
204
                        return b2Vec2_zero;
 
205
 
 
206
                default:
 
207
                        b2Assert(false);
 
208
                        return b2Vec2_zero;
 
209
                }
 
210
        }
 
211
 
 
212
        void GetWitnessPoints(b2Vec2* pA, b2Vec2* pB) const
 
213
        {
 
214
                switch (m_count)
 
215
                {
 
216
                case 0:
 
217
                        b2Assert(false);
 
218
                        break;
 
219
 
 
220
                case 1:
 
221
                        *pA = m_v1.wA;
 
222
                        *pB = m_v1.wB;
 
223
                        break;
 
224
 
 
225
                case 2:
 
226
                        *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA;
 
227
                        *pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB;
 
228
                        break;
 
229
 
 
230
                case 3:
 
231
                        *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA;
 
232
                        *pB = *pA;
 
233
                        break;
 
234
 
 
235
                default:
 
236
                        b2Assert(false);
 
237
                        break;
 
238
                }
 
239
        }
 
240
 
 
241
        float32 GetMetric() const
 
242
        {
 
243
                switch (m_count)
 
244
                {
 
245
                case 0:
 
246
                        b2Assert(false);
 
247
                        return 0.0;
 
248
 
 
249
                case 1:
 
250
                        return 0.0f;
 
251
 
 
252
                case 2:
 
253
                        return b2Distance(m_v1.w, m_v2.w);
 
254
 
 
255
                case 3:
 
256
                        return b2Cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w);
 
257
 
 
258
                default:
 
259
                        b2Assert(false);
 
260
                        return 0.0f;
 
261
                }
 
262
        }
 
263
 
 
264
        void Solve2();
 
265
        void Solve3();
 
266
 
 
267
        b2SimplexVertex m_v1, m_v2, m_v3;
 
268
        int32 m_count;
 
269
};
 
270
 
 
271
 
 
272
// Solve a line segment using barycentric coordinates.
 
273
//
 
274
// p = a1 * w1 + a2 * w2
 
275
// a1 + a2 = 1
 
276
//
 
277
// The vector from the origin to the closest point on the line is
 
278
// perpendicular to the line.
 
279
// e12 = w2 - w1
 
280
// dot(p, e) = 0
 
281
// a1 * dot(w1, e) + a2 * dot(w2, e) = 0
 
282
//
 
283
// 2-by-2 linear system
 
284
// [1      1     ][a1] = [1]
 
285
// [w1.e12 w2.e12][a2] = [0]
 
286
//
 
287
// Define
 
288
// d12_1 =  dot(w2, e12)
 
289
// d12_2 = -dot(w1, e12)
 
290
// d12 = d12_1 + d12_2
 
291
//
 
292
// Solution
 
293
// a1 = d12_1 / d12
 
294
// a2 = d12_2 / d12
 
295
void b2Simplex::Solve2()
 
296
{
 
297
        b2Vec2 w1 = m_v1.w;
 
298
        b2Vec2 w2 = m_v2.w;
 
299
        b2Vec2 e12 = w2 - w1;
 
300
 
 
301
        // w1 region
 
302
        float32 d12_2 = -b2Dot(w1, e12);
 
303
        if (d12_2 <= 0.0f)
 
304
        {
 
305
                // a2 <= 0, so we clamp it to 0
 
306
                m_v1.a = 1.0f;
 
307
                m_count = 1;
 
308
                return;
 
309
        }
 
310
 
 
311
        // w2 region
 
312
        float32 d12_1 = b2Dot(w2, e12);
 
313
        if (d12_1 <= 0.0f)
 
314
        {
 
315
                // a1 <= 0, so we clamp it to 0
 
316
                m_v2.a = 1.0f;
 
317
                m_count = 1;
 
318
                m_v1 = m_v2;
 
319
                return;
 
320
        }
 
321
 
 
322
        // Must be in e12 region.
 
323
        float32 inv_d12 = 1.0f / (d12_1 + d12_2);
 
324
        m_v1.a = d12_1 * inv_d12;
 
325
        m_v2.a = d12_2 * inv_d12;
 
326
        m_count = 2;
 
327
}
 
328
 
 
329
// Possible regions:
 
330
// - points[2]
 
331
// - edge points[0]-points[2]
 
332
// - edge points[1]-points[2]
 
333
// - inside the triangle
 
334
void b2Simplex::Solve3()
 
335
{
 
336
        b2Vec2 w1 = m_v1.w;
 
337
        b2Vec2 w2 = m_v2.w;
 
338
        b2Vec2 w3 = m_v3.w;
 
339
 
 
340
        // Edge12
 
341
        // [1      1     ][a1] = [1]
 
342
        // [w1.e12 w2.e12][a2] = [0]
 
343
        // a3 = 0
 
344
        b2Vec2 e12 = w2 - w1;
 
345
        float32 w1e12 = b2Dot(w1, e12);
 
346
        float32 w2e12 = b2Dot(w2, e12);
 
347
        float32 d12_1 = w2e12;
 
348
        float32 d12_2 = -w1e12;
 
349
 
 
350
        // Edge13
 
351
        // [1      1     ][a1] = [1]
 
352
        // [w1.e13 w3.e13][a3] = [0]
 
353
        // a2 = 0
 
354
        b2Vec2 e13 = w3 - w1;
 
355
        float32 w1e13 = b2Dot(w1, e13);
 
356
        float32 w3e13 = b2Dot(w3, e13);
 
357
        float32 d13_1 = w3e13;
 
358
        float32 d13_2 = -w1e13;
 
359
 
 
360
        // Edge23
 
361
        // [1      1     ][a2] = [1]
 
362
        // [w2.e23 w3.e23][a3] = [0]
 
363
        // a1 = 0
 
364
        b2Vec2 e23 = w3 - w2;
 
365
        float32 w2e23 = b2Dot(w2, e23);
 
366
        float32 w3e23 = b2Dot(w3, e23);
 
367
        float32 d23_1 = w3e23;
 
368
        float32 d23_2 = -w2e23;
 
369
        
 
370
        // Triangle123
 
371
        float32 n123 = b2Cross(e12, e13);
 
372
 
 
373
        float32 d123_1 = n123 * b2Cross(w2, w3);
 
374
        float32 d123_2 = n123 * b2Cross(w3, w1);
 
375
        float32 d123_3 = n123 * b2Cross(w1, w2);
 
376
 
 
377
        // w1 region
 
378
        if (d12_2 <= 0.0f && d13_2 <= 0.0f)
 
379
        {
 
380
                m_v1.a = 1.0f;
 
381
                m_count = 1;
 
382
                return;
 
383
        }
 
384
 
 
385
        // e12
 
386
        if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
 
387
        {
 
388
                float32 inv_d12 = 1.0f / (d12_1 + d12_2);
 
389
                m_v1.a = d12_1 * inv_d12;
 
390
                m_v2.a = d12_2 * inv_d12;
 
391
                m_count = 2;
 
392
                return;
 
393
        }
 
394
 
 
395
        // e13
 
396
        if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
 
397
        {
 
398
                float32 inv_d13 = 1.0f / (d13_1 + d13_2);
 
399
                m_v1.a = d13_1 * inv_d13;
 
400
                m_v3.a = d13_2 * inv_d13;
 
401
                m_count = 2;
 
402
                m_v2 = m_v3;
 
403
                return;
 
404
        }
 
405
 
 
406
        // w2 region
 
407
        if (d12_1 <= 0.0f && d23_2 <= 0.0f)
 
408
        {
 
409
                m_v2.a = 1.0f;
 
410
                m_count = 1;
 
411
                m_v1 = m_v2;
 
412
                return;
 
413
        }
 
414
 
 
415
        // w3 region
 
416
        if (d13_1 <= 0.0f && d23_1 <= 0.0f)
 
417
        {
 
418
                m_v3.a = 1.0f;
 
419
                m_count = 1;
 
420
                m_v1 = m_v3;
 
421
                return;
 
422
        }
 
423
 
 
424
        // e23
 
425
        if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
 
426
        {
 
427
                float32 inv_d23 = 1.0f / (d23_1 + d23_2);
 
428
                m_v2.a = d23_1 * inv_d23;
 
429
                m_v3.a = d23_2 * inv_d23;
 
430
                m_count = 2;
 
431
                m_v1 = m_v3;
 
432
                return;
 
433
        }
 
434
 
 
435
        // Must be in triangle123
 
436
        float32 inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
 
437
        m_v1.a = d123_1 * inv_d123;
 
438
        m_v2.a = d123_2 * inv_d123;
 
439
        m_v3.a = d123_3 * inv_d123;
 
440
        m_count = 3;
 
441
}
 
442
 
 
443
void b2Distance(b2DistanceOutput* output,
 
444
                                b2SimplexCache* cache,
 
445
                                const b2DistanceInput* input)
 
446
{
 
447
        ++b2_gjkCalls;
 
448
 
 
449
        const b2DistanceProxy* proxyA = &input->proxyA;
 
450
        const b2DistanceProxy* proxyB = &input->proxyB;
 
451
 
 
452
        b2Transform transformA = input->transformA;
 
453
        b2Transform transformB = input->transformB;
 
454
 
 
455
        // Initialize the simplex.
 
456
        b2Simplex simplex;
 
457
        simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);
 
458
 
 
459
        // Get simplex vertices as an array.
 
460
        b2SimplexVertex* vertices = &simplex.m_v1;
 
461
        const int32 k_maxIters = 20;
 
462
 
 
463
        // These store the vertices of the last simplex so that we
 
464
        // can check for duplicates and prevent cycling.
 
465
        int32 saveA[3], saveB[3];
 
466
        int32 saveCount = 0;
 
467
 
 
468
        b2Vec2 closestPoint = simplex.GetClosestPoint();
 
469
        float32 distanceSqr1 = closestPoint.LengthSquared();
 
470
        float32 distanceSqr2 = distanceSqr1;
 
471
 
 
472
        // Main iteration loop.
 
473
        int32 iter = 0;
 
474
        while (iter < k_maxIters)
 
475
        {
 
476
                // Copy simplex so we can identify duplicates.
 
477
                saveCount = simplex.m_count;
 
478
                for (int32 i = 0; i < saveCount; ++i)
 
479
                {
 
480
                        saveA[i] = vertices[i].indexA;
 
481
                        saveB[i] = vertices[i].indexB;
 
482
                }
 
483
 
 
484
                switch (simplex.m_count)
 
485
                {
 
486
                case 1:
 
487
                        break;
 
488
 
 
489
                case 2:
 
490
                        simplex.Solve2();
 
491
                        break;
 
492
 
 
493
                case 3:
 
494
                        simplex.Solve3();
 
495
                        break;
 
496
 
 
497
                default:
 
498
                        b2Assert(false);
 
499
                }
 
500
 
 
501
                // If we have 3 points, then the origin is in the corresponding triangle.
 
502
                if (simplex.m_count == 3)
 
503
                {
 
504
                        break;
 
505
                }
 
506
 
 
507
                // Compute closest point.
 
508
                b2Vec2 p = simplex.GetClosestPoint();
 
509
                distanceSqr2 = p.LengthSquared();
 
510
 
 
511
                // Ensure progress
 
512
                if (distanceSqr2 >= distanceSqr1)
 
513
                {
 
514
                        //break;
 
515
                }
 
516
                distanceSqr1 = distanceSqr2;
 
517
 
 
518
                // Get search direction.
 
519
                b2Vec2 d = simplex.GetSearchDirection();
 
520
 
 
521
                // Ensure the search direction is numerically fit.
 
522
                if (d.LengthSquared() < b2_epsilon * b2_epsilon)
 
523
                {
 
524
                        // The origin is probably contained by a line segment
 
525
                        // or triangle. Thus the shapes are overlapped.
 
526
 
 
527
                        // We can't return zero here even though there may be overlap.
 
528
                        // In case the simplex is a point, segment, or triangle it is difficult
 
529
                        // to determine if the origin is contained in the CSO or very close to it.
 
530
                        break;
 
531
                }
 
532
 
 
533
                // Compute a tentative new simplex vertex using support points.
 
534
                b2SimplexVertex* vertex = vertices + simplex.m_count;
 
535
                vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d));
 
536
                vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA));
 
537
                b2Vec2 wBLocal;
 
538
                vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d));
 
539
                vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB));
 
540
                vertex->w = vertex->wB - vertex->wA;
 
541
 
 
542
                // Iteration count is equated to the number of support point calls.
 
543
                ++iter;
 
544
                ++b2_gjkIters;
 
545
 
 
546
                // Check for duplicate support points. This is the main termination criteria.
 
547
                bool duplicate = false;
 
548
                for (int32 i = 0; i < saveCount; ++i)
 
549
                {
 
550
                        if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i])
 
551
                        {
 
552
                                duplicate = true;
 
553
                                break;
 
554
                        }
 
555
                }
 
556
 
 
557
                // If we found a duplicate support point we must exit to avoid cycling.
 
558
                if (duplicate)
 
559
                {
 
560
                        break;
 
561
                }
 
562
 
 
563
                // New vertex is ok and needed.
 
564
                ++simplex.m_count;
 
565
        }
 
566
 
 
567
        b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter);
 
568
 
 
569
        // Prepare output.
 
570
        simplex.GetWitnessPoints(&output->pointA, &output->pointB);
 
571
        output->distance = b2Distance(output->pointA, output->pointB);
 
572
        output->iterations = iter;
 
573
 
 
574
        // Cache the simplex.
 
575
        simplex.WriteCache(cache);
 
576
 
 
577
        // Apply radii if requested.
 
578
        if (input->useRadii)
 
579
        {
 
580
                float32 rA = proxyA->m_radius;
 
581
                float32 rB = proxyB->m_radius;
 
582
 
 
583
                if (output->distance > rA + rB && output->distance > b2_epsilon)
 
584
                {
 
585
                        // Shapes are still no overlapped.
 
586
                        // Move the witness points to the outer surface.
 
587
                        output->distance -= rA + rB;
 
588
                        b2Vec2 normal = output->pointB - output->pointA;
 
589
                        normal.Normalize();
 
590
                        output->pointA += rA * normal;
 
591
                        output->pointB -= rB * normal;
 
592
                }
 
593
                else
 
594
                {
 
595
                        // Shapes are overlapped when radii are considered.
 
596
                        // Move the witness points to the middle.
 
597
                        b2Vec2 p = 0.5f * (output->pointA + output->pointB);
 
598
                        output->pointA = p;
 
599
                        output->pointB = p;
 
600
                        output->distance = 0.0f;
 
601
                }
 
602
        }
 
603
}