← Back to branch summary

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

« back to all changes in this revision

Viewing changes to tests/box2d/Box2D/Collision/b2Collision.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/b2Collision.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/b2Collision.h>
 
20
#include <Box2D/Collision/b2Distance.h>
 
21
 
 
22
void b2WorldManifold::Initialize(const b2Manifold* manifold,
 
23
                                                  const b2Transform& xfA, float32 radiusA,
 
24
                                                  const b2Transform& xfB, float32 radiusB)
 
25
{
 
26
        if (manifold->pointCount == 0)
 
27
        {
 
28
                return;
 
29
        }
 
30
 
 
31
        switch (manifold->type)
 
32
        {
 
33
        case b2Manifold::e_circles:
 
34
                {
 
35
                        normal.Set(1.0f, 0.0f);
 
36
                        b2Vec2 pointA = b2Mul(xfA, manifold->localPoint);
 
37
                        b2Vec2 pointB = b2Mul(xfB, manifold->points[0].localPoint);
 
38
                        if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon)
 
39
                        {
 
40
                                normal = pointB - pointA;
 
41
                                normal.Normalize();
 
42
                        }
 
43
 
 
44
                        b2Vec2 cA = pointA + radiusA * normal;
 
45
                        b2Vec2 cB = pointB - radiusB * normal;
 
46
                        points[0] = 0.5f * (cA + cB);
 
47
                }
 
48
                break;
 
49
 
 
50
        case b2Manifold::e_faceA:
 
51
                {
 
52
                        normal = b2Mul(xfA.q, manifold->localNormal);
 
53
                        b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint);
 
54
                        
 
55
                        for (int32 i = 0; i < manifold->pointCount; ++i)
 
56
                        {
 
57
                                b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);
 
58
                                b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal;
 
59
                                b2Vec2 cB = clipPoint - radiusB * normal;
 
60
                                points[i] = 0.5f * (cA + cB);
 
61
                        }
 
62
                }
 
63
                break;
 
64
 
 
65
        case b2Manifold::e_faceB:
 
66
                {
 
67
                        normal = b2Mul(xfB.q, manifold->localNormal);
 
68
                        b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint);
 
69
 
 
70
                        for (int32 i = 0; i < manifold->pointCount; ++i)
 
71
                        {
 
72
                                b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint);
 
73
                                b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal;
 
74
                                b2Vec2 cA = clipPoint - radiusA * normal;
 
75
                                points[i] = 0.5f * (cA + cB);
 
76
                        }
 
77
 
 
78
                        // Ensure normal points from A to B.
 
79
                        normal = -normal;
 
80
                }
 
81
                break;
 
82
        }
 
83
}
 
84
 
 
85
void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints],
 
86
                                          const b2Manifold* manifold1, const b2Manifold* manifold2)
 
87
{
 
88
        for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
 
89
        {
 
90
                state1[i] = b2_nullState;
 
91
                state2[i] = b2_nullState;
 
92
        }
 
93
 
 
94
        // Detect persists and removes.
 
95
        for (int32 i = 0; i < manifold1->pointCount; ++i)
 
96
        {
 
97
                b2ContactID id = manifold1->points[i].id;
 
98
 
 
99
                state1[i] = b2_removeState;
 
100
 
 
101
                for (int32 j = 0; j < manifold2->pointCount; ++j)
 
102
                {
 
103
                        if (manifold2->points[j].id.key == id.key)
 
104
                        {
 
105
                                state1[i] = b2_persistState;
 
106
                                break;
 
107
                        }
 
108
                }
 
109
        }
 
110
 
 
111
        // Detect persists and adds.
 
112
        for (int32 i = 0; i < manifold2->pointCount; ++i)
 
113
        {
 
114
                b2ContactID id = manifold2->points[i].id;
 
115
 
 
116
                state2[i] = b2_addState;
 
117
 
 
118
                for (int32 j = 0; j < manifold1->pointCount; ++j)
 
119
                {
 
120
                        if (manifold1->points[j].id.key == id.key)
 
121
                        {
 
122
                                state2[i] = b2_persistState;
 
123
                                break;
 
124
                        }
 
125
                }
 
126
        }
 
127
}
 
128
 
 
129
// From Real-time Collision Detection, p179.
 
130
bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const
 
131
{
 
132
        float32 tmin = -b2_maxFloat;
 
133
        float32 tmax = b2_maxFloat;
 
134
 
 
135
        b2Vec2 p = input.p1;
 
136
        b2Vec2 d = input.p2 - input.p1;
 
137
        b2Vec2 absD = b2Abs(d);
 
138
 
 
139
        b2Vec2 normal;
 
140
 
 
141
        for (int32 i = 0; i < 2; ++i)
 
142
        {
 
143
                if (absD(i) < b2_epsilon)
 
144
                {
 
145
                        // Parallel.
 
146
                        if (p(i) < lowerBound(i) || upperBound(i) < p(i))
 
147
                        {
 
148
                                return false;
 
149
                        }
 
150
                }
 
151
                else
 
152
                {
 
153
                        float32 inv_d = 1.0f / d(i);
 
154
                        float32 t1 = (lowerBound(i) - p(i)) * inv_d;
 
155
                        float32 t2 = (upperBound(i) - p(i)) * inv_d;
 
156
 
 
157
                        // Sign of the normal vector.
 
158
                        float32 s = -1.0f;
 
159
 
 
160
                        if (t1 > t2)
 
161
                        {
 
162
                                b2Swap(t1, t2);
 
163
                                s = 1.0f;
 
164
                        }
 
165
 
 
166
                        // Push the min up
 
167
                        if (t1 > tmin)
 
168
                        {
 
169
                                normal.SetZero();
 
170
                                normal(i) = s;
 
171
                                tmin = t1;
 
172
                        }
 
173
 
 
174
                        // Pull the max down
 
175
                        tmax = b2Min(tmax, t2);
 
176
 
 
177
                        if (tmin > tmax)
 
178
                        {
 
179
                                return false;
 
180
                        }
 
181
                }
 
182
        }
 
183
 
 
184
        // Does the ray start inside the box?
 
185
        // Does the ray intersect beyond the max fraction?
 
186
        if (tmin < 0.0f || input.maxFraction < tmin)
 
187
        {
 
188
                return false;
 
189
        }
 
190
 
 
191
        // Intersection.
 
192
        output->fraction = tmin;
 
193
        output->normal = normal;
 
194
        return true;
 
195
}
 
196
 
 
197
// Sutherland-Hodgman clipping.
 
198
int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
 
199
                                                const b2Vec2& normal, float32 offset, int32 vertexIndexA)
 
200
{
 
201
        // Start with no output points
 
202
        int32 numOut = 0;
 
203
 
 
204
        // Calculate the distance of end points to the line
 
205
        float32 distance0 = b2Dot(normal, vIn[0].v) - offset;
 
206
        float32 distance1 = b2Dot(normal, vIn[1].v) - offset;
 
207
 
 
208
        // If the points are behind the plane
 
209
        if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
 
210
        if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
 
211
 
 
212
        // If the points are on different sides of the plane
 
213
        if (distance0 * distance1 < 0.0f)
 
214
        {
 
215
                // Find intersection point of edge and plane
 
216
                float32 interp = distance0 / (distance0 - distance1);
 
217
                vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
 
218
 
 
219
                // VertexA is hitting edgeB.
 
220
                vOut[numOut].id.cf.indexA = vertexIndexA;
 
221
                vOut[numOut].id.cf.indexB = vIn[0].id.cf.indexB;
 
222
                vOut[numOut].id.cf.typeA = b2ContactFeature::e_vertex;
 
223
                vOut[numOut].id.cf.typeB = b2ContactFeature::e_face;
 
224
                ++numOut;
 
225
        }
 
226
 
 
227
        return numOut;
 
228
}
 
229
 
 
230
bool b2TestOverlap(     const b2Shape* shapeA, int32 indexA,
 
231
                                        const b2Shape* shapeB, int32 indexB,
 
232
                                        const b2Transform& xfA, const b2Transform& xfB)
 
233
{
 
234
        b2DistanceInput input;
 
235
        input.proxyA.Set(shapeA, indexA);
 
236
        input.proxyB.Set(shapeB, indexB);
 
237
        input.transformA = xfA;
 
238
        input.transformB = xfB;
 
239
        input.useRadii = true;
 
240
 
 
241
        b2SimplexCache cache;
 
242
        cache.count = 0;
 
243
 
 
244
        b2DistanceOutput output;
 
245
 
 
246
        b2Distance(&output, &cache, &input);
 
247
 
 
248
        return output.distance < 10.0f * b2_epsilon;
 
249
}