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 "stdio.h"
15 /*
16 #ifdef _WIN32
17 #include <windows.h>
18 #endif
19 */
20 #include "tokamak.h"
21 #include "containers.h"
22 #include "scenery.h"
23 #include "collision.h"
24 #include "constraint.h"
25 #include "rigidbody.h"
26 #include "scenery.h"
27 #include "stack.h"
28 #include "simulator.h"
29 #include "message.h"
30
CollisionTestSensor(TConvex * obbA,neSensor_ * sensorsA,neT3 & transA,neCollision & colB,neT3 & transB,neRigidBodyBase * body)31 void CollisionTestSensor(TConvex * obbA, neSensor_ * sensorsA, neT3 & transA, neCollision & colB, neT3 & transB, neRigidBodyBase * body)
32 {
33 neT3 convex2WorldB;
34
35 convex2WorldB = transB * colB.obb.c2p;
36
37 neT3 world2convexB;
38
39 world2convexB = convex2WorldB.FastInverse();
40
41 neT3 AtoB;
42
43 AtoB = world2convexB * transA;
44
45 if (colB.convexCount == 1)
46 {
47 neSensorItem * si = (neSensorItem *)sensorsA;
48
49 while (si)
50 {
51 neSensor_ * s = (neSensor_ *) si;
52
53 si = si->next;
54
55 neSensor_ tmp = *s;
56
57 tmp.depth = 0.0f;
58
59 tmp.pos = AtoB * s->pos;
60
61 tmp.dir = AtoB.rot * s->dir;
62
63 tmp.length = s->length;
64
65 SensorTest(tmp, colB.obb, convex2WorldB);
66
67 if (tmp.depth > 0.0f && tmp.depth > s->depth)
68 {
69 s->depth = tmp.depth;
70 s->body = tmp.body;
71 s->materialID = tmp.materialID;
72 s->normal = tmp.normal;
73 s->contactPoint = convex2WorldB * tmp.contactPoint;
74 s->body = body;
75 /*
76 char ss[256];
77
78 sprintf(ss, "normal = %f, %f, %f \n", s->normal[0], s->normal[1], s->normal[2]);
79 OutputDebugString(ss);
80 */ }
81 }
82 }
83 else
84 {
85 neSensorItem * si = (neSensorItem *)sensorsA;
86
87 while (si)
88 {
89 neSensor_ * s = (neSensor_ *) si;
90
91 si = si->next;
92
93 neSensor_ tmp = *s;
94
95 tmp.depth = 0.0f;
96
97 TConvexItem * ti = (TConvexItem *)colB.convex;
98
99 while (ti)
100 {
101 TConvex * t = (TConvex *)ti;
102
103 ti = ti->next;
104
105 convex2WorldB = transB * t->c2p;
106
107 world2convexB = convex2WorldB.FastInverse();
108
109 AtoB = world2convexB * transA;
110
111 tmp.pos = AtoB * s->pos;
112
113 tmp.dir = AtoB.rot * s->dir;
114
115 tmp.length = s->length;
116
117 //SensorTest(tmp, colB.obb, convex2WorldB);
118 SensorTest(tmp, *t, convex2WorldB);
119
120 if (tmp.depth > 0.0f && tmp.depth > s->depth)
121 {
122 s->depth = tmp.depth;
123 s->body = tmp.body;
124 s->materialID = tmp.materialID;
125 s->normal = tmp.normal;
126 s->contactPoint = convex2WorldB * tmp.contactPoint;
127 s->body = body;
128 }
129 }
130 }
131 }
132 }
133
SameSide(const neV3 & p1,const neV3 & p2,const neV3 & a,const neV3 & edge)134 NEINLINE neBool SameSide(const neV3 & p1, const neV3 & p2, const neV3 & a, const neV3 & edge)
135 {
136 neV3 cp1 = edge.Cross(p1 - a);
137
138 neV3 cp2 = edge.Cross(p2 - a);
139
140 f32 dot = cp1.Dot(cp2);
141
142 return (dot >= 0.0f);
143 }
144
145 /*
146 *
147 neBool found = false;
148
149 f32 dist, ratio, factor, depth;
150
151 neV3 contact;
152
153 s32 i;
154
155 for (i = 0; i < 3; i++)
156 {
157 if (!neIsConsiderZero(sensorA.dir[i]))
158 {
159 if (sensorA.dir[i] > 0.0f)
160 {
161 if (sensorA.pos[i] > -convexB.as.box.boxSize[i])
162 continue;
163
164 factor = 1.0f;
165 }
166 else
167 {
168 if (sensorA.pos[i] < convexB.as.box.boxSize[i])
169 continue;
170
171 factor = -1.0f;
172 }
173 dist = factor * (convexB.as.box.boxSize[i] - sensorA.pos[i]);
174
175 ASSERT(dist > 0.0f);
176
177 if (dist > neAbs(sensorA.dir[i]))
178 return;
179
180 ratio = dist / neAbs(sensorA.dir[i]);
181
182 contact = sensorA.pos + sensorA.dir * ratio;
183
184 s32 other1, other2;
185
186 other1 = (i + 1)%3;
187
188 other2 = (i + 2)%3;
189
190 if (contact[other1] >= convexB.as.box.boxSize[other1] || contact[other1] <= -convexB.as.box.boxSize[other1])
191 continue;
192
193 if (contact[other2] >= convexB.as.box.boxSize[other2] || contact[other2] <= -convexB.as.box.boxSize[other2])
194 continue;
195
196 found = true;
197
198 depth = (1.0f - ratio) * sensorA.length;
199
200 break;
201 }
202 else if (sensorA.pos[i] >= convexB.as.box.boxSize[i] || sensorA.pos[i] <= -convexB.as.box.boxSize[i])
203 {
204 return;
205 }
206 }
207 if (found)
208 {
209 sensorA.depth = depth;
210
211 sensorA.normal = transB.rot[i] * factor * -1.0f;
212
213 sensorA.contactPoint = contact;
214
215 sensorA.materialID = convexB.matIndex;
216 }
217
218 */
SensorTest(neSensor_ & sensorA,TConvex & convexB,neT3 & transB)219 void SensorTest(neSensor_ & sensorA, TConvex & convexB, neT3 & transB)
220 {
221 if (convexB.type == TConvex::BOX)
222 {
223 int nearDim = -1;
224 int farDim = -1;
225
226 // set Tnear = - infinity, Tfar = infinity
227 // For each pair of planes P associated with X, Y, and Z do:
228 // (example using X planes)
229 // if direction Xd = 0 then the ray is parallel to the X planes, so
230 // if origin Xo is not between the slabs ( Xo < Xl or Xo > Xh) then return false
231 // else, if the ray is not parallel to the plane then
232 // begin
233 // compute the intersection distance of the planes
234 // T1 = (Xl - Xo) / Xd
235 // T2 = (Xh - Xo) / Xd
236 // If T1 > T2 swap (T1, T2) /* since T1 intersection with near plane */
237 // If T1 > Tnear set Tnear =T1 /* want largest Tnear */
238 // If T2 < Tfar set Tfar="T2" /* want smallest Tfar */
239 // If Tnear > Tfar box is missed so return false
240 // If Tfar < 0 box is behind ray return false end
241
242
243 float tNear = -1.0e6;
244 float tFar = 1.0e6;
245
246 for (int i = 0; i < 3; i++)
247 {
248 if (neIsConsiderZero(sensorA.dir[i]))
249 {
250 if (sensorA.pos[i] < -convexB.as.box.boxSize[i] ||
251 sensorA.pos[i] > convexB.as.box.boxSize[i])
252 {
253 return;
254 }
255 }
256 float t1 = (-convexB.as.box.boxSize[i] - sensorA.pos[i]) / sensorA.dir[i];
257
258 float t2 = (convexB.as.box.boxSize[i] - sensorA.pos[i]) / sensorA.dir[i];
259
260 float tt;
261
262 if (t1 > t2)
263 {
264 tt = t1;
265 t1 = t2;
266 t2 = tt;
267 }
268
269 if (t1 > tNear)
270 {
271 tNear = t1;
272 nearDim = i;
273 }
274
275 if (t2 < tFar)
276 {
277 tFar = t2;
278 farDim = i;
279 }
280
281 if (tNear > tFar)
282 return;
283
284 if (tFar < 0)
285 return;
286
287 }
288 //assert(nearDim != -1);
289 //assert(farDim != -1);
290
291 if (tNear > 1.0f)
292 return;
293
294 neV3 contact = sensorA.pos + tNear * sensorA.dir;
295
296 neV3 sensorEnd = sensorA.pos + sensorA.dir;
297
298 f32 depth = (sensorEnd - contact).Length();
299
300 sensorA.depth = depth;
301
302 f32 factor = (sensorA.dir[nearDim] >= 0) ? -1.0f : 1.0f;
303 sensorA.normal = transB.rot[nearDim] * factor;
304
305 sensorA.contactPoint = contact;
306
307 sensorA.materialID = convexB.matIndex;
308 }
309 else if (convexB.type == TConvex::TERRAIN)
310 {
311 neSimpleArray<s32> & _triIndex = *convexB.as.terrain.triIndex;
312
313 s32 triangleCount = _triIndex.GetUsedCount();
314
315 neArray<neTriangle_> & triangleArray = *convexB.as.terrain.triangles;
316
317 for (s32 i = 0; i < triangleCount; i++)
318 {
319 s32 test = _triIndex[i];
320
321 neTriangle_ * t = &triangleArray[_triIndex[i]];
322
323 neV3 * vert[3];
324
325 neV3 edges[3];
326
327 neV3 normal;
328
329 f32 d;
330
331 vert[0] = &convexB.vertices[t->indices[0]];
332 vert[1] = &convexB.vertices[t->indices[1]];
333 vert[2] = &convexB.vertices[t->indices[2]];
334
335 edges[0] = *vert[1] - *vert[0];
336 edges[1] = *vert[2] - *vert[1];
337 edges[2] = *vert[0] - *vert[2];
338 normal = edges[0].Cross(edges[1]);
339
340 normal.Normalize();
341
342 d = normal.Dot(*vert[0]);
343
344 f32 nd = normal.Dot(sensorA.dir);
345
346 f32 np = normal.Dot(sensorA.pos);
347
348 f32 t1;
349
350 t1 = (d - np) / nd;
351
352 if (t1 > 1.0f || t1 < 0.0f)
353 continue;
354
355 neV3 contactPoint = sensorA.pos + sensorA.dir * t1;
356
357 if (!SameSide(contactPoint, *vert[2], *vert[0], edges[0]))
358 continue;
359
360 if (!SameSide(contactPoint, *vert[0], *vert[1], edges[1]))
361 continue;
362
363 if (!SameSide(contactPoint, *vert[1], *vert[2], edges[2]))
364 continue;
365
366 sensorA.depth = (1.0f - t1) * sensorA.length;
367
368 if (nd > 0.0)
369 sensorA.normal = normal * -1.0f;
370 else
371 sensorA.normal = normal;
372
373 sensorA.contactPoint = contactPoint;
374
375 sensorA.materialID = t->materialID;
376 }
377 }
378 else
379 {
380 // other primitives to do
381 }
382 }
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403