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