1 /*************************************************************************
2 * *
3 * Tokamak Physics Engine, Copyright (C) 2002-2007 David Lam. *
4 * All rights reserved. Email: david@tokamakphysics.com *
5 * Web: www.tokamakphysics.com *
6 * *
7 * This library is distributed in the hope that it will be useful, *
8 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
9 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
10 * LICENSE.TXT for more details. *
11 * *
12 *************************************************************************/
13
14 #include "tokamak.h"
15 #include "containers.h"
16 #include "scenery.h"
17 #include "collision.h"
18 #include "collision2.h"
19 #include "constraint.h"
20 #include "rigidbody.h"
21
22 #include <assert.h>
23 #include <stdio.h>
24
25 #define INSIDE_BOX_BOUNDARY(_dir) (flag2[_dir] < 0.0f)
26
27 #define BOX_SPHERE_DO_TEST(whichCase, _dir) {configuration = whichCase; dir = _dir;}
28
Box2SphereTest(neCollisionResult & result,TConvex & boxA,neT3 & transA,TConvex & sphereB,neT3 & transB)29 void Box2SphereTest(neCollisionResult & result, TConvex & boxA, neT3 & transA, TConvex & sphereB, neT3 & transB)
30 {
31 f32 penetration;
32
33 result.penetrate = false;
34
35 neV3 sphereCenter;
36
37 sphereCenter = (transA.FastInverse() * transB).pos;
38
39 neV3 flag1, flag2;
40
41 for (s32 i = 0; i < 3; i++)
42 flag1[i] = sphereCenter[i] < 0.0f ? -1.0f: 1.0f;
43
44 neV3 sphereCenterAbs;
45
46 sphereCenterAbs = sphereCenter * flag1;
47
48 flag2 = sphereCenterAbs - boxA.as.box.boxSize;
49
50 s32 configuration, dir;
51
52 if (INSIDE_BOX_BOUNDARY(1))
53 {
54 if (INSIDE_BOX_BOUNDARY(2))
55 if (INSIDE_BOX_BOUNDARY(0))
56 configuration = -1; //center inside the box
57 else
58 BOX_SPHERE_DO_TEST(0, 0)
59 else
60 if (INSIDE_BOX_BOUNDARY(0))
61 BOX_SPHERE_DO_TEST(0, 2)
62 else
63 BOX_SPHERE_DO_TEST(1, 1)
64 }
65 else if (INSIDE_BOX_BOUNDARY(2))
66 {
67 if (INSIDE_BOX_BOUNDARY(0))
68 BOX_SPHERE_DO_TEST(0, 1)
69 else
70 BOX_SPHERE_DO_TEST(1, 2)
71 }
72 else if (INSIDE_BOX_BOUNDARY(0))
73 {
74 BOX_SPHERE_DO_TEST(1, 0)
75 }
76 else
77 {
78 BOX_SPHERE_DO_TEST(2, 0)
79 }
80
81 neV3 contactA;
82
83 if (configuration == -1)
84 {
85 //find the shallowest penetration
86 neV3 depth; depth = boxA.as.box.boxSize - flag2;
87 s32 k;
88
89 if (depth[0] < depth[1])
90 {
91 if (depth[0] < depth[2])
92 { //x
93 k = 0;
94 }
95 else
96 { //z
97 k = 2;
98 }
99 }
100 else if (depth[1] < depth[2])
101 { //y
102 k = 1;
103 }
104 else
105 { //z
106 k = 2;
107 }
108 ASSERT(depth[k] >= 0.0f);
109
110 result.depth = depth[k] + sphereB.Radius();
111
112 result.penetrate = true;
113
114 result.collisionFrame[2] = transA.rot[k] * flag1[k] * -1.0f;
115
116 result.contactB = transB.pos + result.collisionFrame[2] * sphereB.Radius();
117
118 result.contactA = result.contactB - result.collisionFrame[2] * result.depth;
119 }
120 else if (configuration == 0)
121 {
122 penetration = sphereB.Radius() + boxA.BoxSize(dir) - sphereCenterAbs[dir];
123
124 if (penetration > 0.0f)
125 {
126 result.depth = penetration;
127
128 result.penetrate = true;
129
130 result.collisionFrame[2] = transA.rot[dir] * flag1[dir] * -1.0f;
131
132 result.contactB = transB.pos + result.collisionFrame[2] * sphereB.Radius();
133
134 result.contactA = result.contactB - result.collisionFrame[2] * penetration;
135 }
136 }
137 else if (configuration == 1)
138 {
139 s32 dir1, dir2;
140
141 dir1 = neNextDim1[dir];
142
143 dir2 = neNextDim2[dir];
144
145 contactA[dir] = sphereCenter[dir];
146
147 contactA[dir1] = flag1[dir1] * boxA.BoxSize(dir1);
148
149 contactA[dir2] = flag1[dir2] * boxA.BoxSize(dir2);
150
151 neV3 sub = contactA - sphereCenter;
152
153 f32 lenSq = sub[dir1] * sub[dir1] +
154 sub[dir2] * sub[dir2];
155
156 if (lenSq > sphereB.RadiusSq())
157 return;
158
159 f32 len = sqrtf(lenSq);
160
161 sub *= 1.0f / len;
162
163 penetration = sphereB.Radius() - len;
164
165 ASSERT(penetration > 0.0f);
166
167 result.depth = penetration;
168
169 result.penetrate = true;
170
171 result.collisionFrame[2] = transA.rot * sub;
172
173 result.contactA = transA * contactA;
174
175 result.contactB = transB.pos + result.collisionFrame[2] * sphereB.Radius();
176 }
177 else if (configuration == 2)
178 {
179 contactA.SetZero();
180
181 for (s32 i = 0; i < 3; i++)
182 contactA[i] += flag1[i] * boxA.BoxSize(i);
183
184 neV3 sub = contactA - sphereCenter;
185
186 f32 lenSq = sub.Dot(sub);
187
188 if (lenSq > sphereB.RadiusSq())
189 return;
190
191 f32 len = sqrtf(lenSq);
192
193 penetration = sphereB.Radius() - len;
194
195 sub *= 1.0f / len;
196
197 ASSERT(penetration > 0.0f);
198
199 result.depth = penetration;
200
201 result.penetrate = true;
202
203 result.collisionFrame[2] = transA.rot * sub;
204
205 result.contactA = transA * contactA;
206
207 result.contactB = transB.pos + result.collisionFrame[2] * sphereB.Radius();
208 }
209 return;
210 }
211
Sphere2TerrainTest(neCollisionResult & result,TConvex & sphereA,neT3 & transA,TConvex & terrainB)212 void Sphere2TerrainTest(neCollisionResult & result, TConvex & sphereA, neT3 & transA, TConvex & terrainB)
213 {
214 neSimpleArray<s32> & _triIndex = *terrainB.as.terrain.triIndex;
215
216 s32 triangleCount = _triIndex.GetUsedCount();
217
218 neArray<neTriangle_> & triangleArray = *terrainB.as.terrain.triangles;
219
220 ConvexTestResult res[2];
221
222 s32 finalTriIndex = -1;
223 s32 currentRes = 1;
224 s32 testRes = 0;
225
226 res[currentRes].depth = -1.0e6f;
227 res[currentRes].valid = false;
228 res[testRes].depth = 1.0e6f;
229
230 s32 terrainMatID = 0;
231
232 for (s32 i = 0; i < triangleCount; i++)
233 {
234 s32 test = _triIndex[i];
235
236 neTriangle_ * t = &triangleArray[_triIndex[i]];
237
238 TriangleParam triParam;
239
240 triParam.vert[0] = terrainB.vertices[t->indices[0]];
241 triParam.vert[1] = terrainB.vertices[t->indices[1]];
242 triParam.vert[2] = terrainB.vertices[t->indices[2]];
243
244 triParam.edges[0] = triParam.vert[1] - triParam.vert[0];
245 triParam.edges[1] = triParam.vert[2] - triParam.vert[1];
246 triParam.edges[2] = triParam.vert[0] - triParam.vert[2];
247 triParam.normal = triParam.edges[0].Cross(triParam.edges[1]);
248 triParam.normal.Normalize();
249 triParam.d = triParam.normal.Dot(triParam.vert[0]);
250
251 if (t->flag == neTriangle::NE_TRI_TRIANGLE)
252 {
253 if (SphereTriTest(transA.pos, sphereA.Radius(), res[testRes], triParam))
254 {
255 if (res[testRes].depth > res[currentRes].depth)
256 {
257 s32 tmp = testRes;
258
259 testRes = currentRes;
260
261 currentRes = tmp;
262
263 terrainMatID = t->materialID;
264
265 finalTriIndex = _triIndex[i];
266 }
267 }
268 }
269 else if (t->flag == neTriangle::NE_TRI_HEIGHT_MAP)
270 {
271 }
272 else
273 {
274 ASSERT(0);
275 }
276 }
277 if (res[currentRes].valid)
278 {
279 result.penetrate = true;
280
281 result.depth = res[currentRes].depth;
282
283 result.collisionFrame[2] = res[currentRes].contactNormal;
284
285 result.materialIdB = terrainMatID;
286
287 result.contactA = res[currentRes].contactA;
288
289 result.contactB = res[currentRes].contactB;
290 }
291 else
292 {
293 result.penetrate = false;
294 }
295 }
296
MeasureSphereAndTriEdge(const neV3 & center,f32 radius,ConvexTestResult & result,TriangleParam & tri,s32 whichEdge)297 void MeasureSphereAndTriEdge(const neV3 & center, f32 radius, ConvexTestResult & result, TriangleParam & tri, s32 whichEdge)
298 {
299 s32 whichVert0, whichVert1;
300
301 whichVert0 = whichEdge;
302
303 whichVert1 = neNextDim1[whichEdge];
304
305 f32 penetrate;
306
307 neV3 dir = tri.edges[whichEdge];
308
309 f32 edgeLen = dir.Length();
310
311 if (neIsConsiderZero(edgeLen))
312 {
313 dir.SetZero();
314 }
315 else
316 {
317 dir *= (1.0f / edgeLen);
318 }
319 neV3 vert2Point = center - tri.vert[whichVert0];
320
321 f32 dot = dir.Dot(vert2Point);
322
323 neV3 project = tri.vert[whichVert0] + dot * dir;
324
325 if (dot > 0.0f && dot < edgeLen)
326 {
327 neV3 diff = center - project;
328
329 f32 len = diff.Length();
330
331 penetrate = radius - len;
332
333 if (penetrate > 0.0f && penetrate < result.depth && penetrate < radius)
334 {
335 result.valid = true;
336
337 result.depth = penetrate;
338
339 result.contactNormal = diff * (1.0f / len);
340
341 result.contactA = center - result.contactNormal * radius;
342
343 result.contactB = project;
344 }
345 }
346 }
347
MeasureSphereAndTriVert(const neV3 & center,f32 radius,ConvexTestResult & result,TriangleParam & tri,s32 whichVert)348 void MeasureSphereAndTriVert(const neV3 & center, f32 radius, ConvexTestResult & result, TriangleParam & tri, s32 whichVert)
349 {
350 neV3 diff = center - tri.vert[whichVert];
351
352 f32 len = diff.Length();
353
354 f32 penetrate = radius - len;
355
356 if (penetrate > 0.0f)
357 {
358 result.valid = true;
359
360 result.depth = penetrate;
361
362 result.contactNormal = diff * (1.0f / len);
363
364 result.contactA = center - result.contactNormal * radius;
365
366 result.contactB = tri.vert[whichVert];
367 }
368 }
369
SphereTriTest(const neV3 & center,f32 radius,ConvexTestResult & result,TriangleParam & tri)370 neBool SphereTriTest(const neV3 & center, f32 radius, ConvexTestResult & result, TriangleParam & tri)
371 {
372 //check sphere and triangle plane
373 result.depth = 1.e5f;
374 result.valid = false;
375
376 f32 distFromPlane = tri.normal.Dot(center) - tri.d;
377
378 f32 factor = 1.0f;
379
380 if (distFromPlane < 0.0f)
381 factor = -1.0f;
382
383 f32 penetrated = radius - distFromPlane * factor;
384
385 if (penetrated <= 0.0f)
386 return false;
387
388 neV3 contactB = center - tri.normal * distFromPlane;
389
390 s32 pointInside = tri.IsPointInside(contactB);
391
392 if (pointInside == -1) // inside the triangle
393 {
394 result.depth = penetrated;
395
396 result.contactA = center - tri.normal * factor * radius; //on the sphere
397
398 result.contactB = contactB;
399
400 result.valid = true;
401
402 result.contactNormal = tri.normal * factor;
403
404 return true;
405 }
406
407 switch (pointInside)
408 {
409 case 0:
410 MeasureSphereAndTriVert(center, radius, result, tri, 0);
411 break;
412
413 case 1:
414 MeasureSphereAndTriEdge(center, radius, result, tri, 0);
415 break;
416
417 case 2:
418 MeasureSphereAndTriVert(center, radius, result, tri, 1);
419 break;
420
421 case 3:
422 MeasureSphereAndTriEdge(center, radius, result, tri, 1);
423 break;
424
425 case 4:
426 MeasureSphereAndTriVert(center, radius, result, tri, 2);
427 break;
428
429 case 5:
430 MeasureSphereAndTriEdge(center, radius, result, tri, 2);
431 break;
432 }
433
434 return result.valid;
435 }
436
Sphere2SphereTest(neCollisionResult & result,TConvex & sphereA,neT3 & transA,TConvex & sphereB,neT3 & transB)437 void Sphere2SphereTest(neCollisionResult & result, TConvex & sphereA, neT3 & transA, TConvex & sphereB, neT3 & transB)
438 {
439 neV3 sub = transA.pos - transB.pos;
440
441 f32 dot = sub.Dot(sub);
442
443 f32 totalLen = sphereA.Radius() + sphereB.Radius();
444
445 totalLen *= totalLen;
446
447 if (dot >= totalLen)
448 {
449 result.penetrate = false;
450
451 return;
452 }
453
454 if (neIsConsiderZero(dot))
455 {
456 result.penetrate = false;
457
458 return;
459 }
460 f32 len = sub.Length();
461
462 sub *= 1.0f / len;
463
464 result.depth = sphereA.Radius() + sphereB.Radius() - len;
465
466 ASSERT(result.depth > 0.0f);
467
468 result.penetrate = true;
469
470 result.collisionFrame[2] = sub;
471
472 result.contactA = transA.pos - sub * sphereA.Radius();
473
474 result.contactB = transB.pos + sub * sphereB.Radius();
475 }