?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Added: dSpaceSetSublevel/dSpaceGetSublevel and possibility to collide a space as...
[ode.git] / ode / src / collision_trimesh_sphere.cpp
blob6d4396998d111ea03a747e4b843ad7241b267faf
1 /*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23 // TriMesh code by Erwin de Vries.
24
25 #include <ode/collision.h>
26 #include <ode/matrix.h>
27 #include <ode/rotation.h>
28 #include <ode/odemath.h>
29 #include "collision_util.h"
30
31 #if dTRIMESH_ENABLED
32 #include "collision_trimesh_internal.h"
33
34 #if dTRIMESH_OPCODE
35 #define MERGECONTACTS
36
37 // Ripped from Opcode 1.1.
38 static bool GetContactData(const dVector3& Center, dReal Radius, const dVector3 Origin, const dVector3 Edge0, const dVector3 Edge1, dReal& Dist, dReal& u, dReal& v){
39
40 // now onto the bulk of the collision...
41
42 dVector3 Diff;
43 Diff[0] = Origin[0] - Center[0];
44 Diff[1] = Origin[1] - Center[1];
45 Diff[2] = Origin[2] - Center[2];
46 Diff[3] = Origin[3] - Center[3];
47
48 dReal A00 = dDOT(Edge0, Edge0);
49 dReal A01 = dDOT(Edge0, Edge1);
50 dReal A11 = dDOT(Edge1, Edge1);
51
52 dReal B0 = dDOT(Diff, Edge0);
53 dReal B1 = dDOT(Diff, Edge1);
54
55 dReal C = dDOT(Diff, Diff);
56
57 dReal Det = dFabs(A00 * A11 - A01 * A01);
58 u = A01 * B1 - A11 * B0;
59 v = A01 * B0 - A00 * B1;
60
61 dReal DistSq;
62
63 if (u + v <= Det){
64 if(u < REAL(0.0)){
65 if(v < REAL(0.0)){ // region 4
66 if(B0 < REAL(0.0)){
67 v = REAL(0.0);
68 if (-B0 >= A00){
69 u = REAL(1.0);
70 DistSq = A00 + REAL(2.0) * B0 + C;
71 }
72 else{
73 u = -B0 / A00;
74 DistSq = B0 * u + C;
75 }
76 }
77 else{
78 u = REAL(0.0);
79 if(B1 >= REAL(0.0)){
80 v = REAL(0.0);
81 DistSq = C;
82 }
83 else if(-B1 >= A11){
84 v = REAL(1.0);
85 DistSq = A11 + REAL(2.0) * B1 + C;
86 }
87 else{
88 v = -B1 / A11;
89 DistSq = B1 * v + C;
90 }
91 }
92 }
93 else{ // region 3
94 u = REAL(0.0);
95 if(B1 >= REAL(0.0)){
96 v = REAL(0.0);
97 DistSq = C;
98 }
99 else if(-B1 >= A11){
100 v = REAL(1.0);
101 DistSq = A11 + REAL(2.0) * B1 + C;
102 }
103 else{
104 v = -B1 / A11;
105 DistSq = B1 * v + C;
106 }
107 }
108 }
109 else if(v < REAL(0.0)){ // region 5
110 v = REAL(0.0);
111 if (B0 >= REAL(0.0)){
112 u = REAL(0.0);
113 DistSq = C;
114 }
115 else if (-B0 >= A00){
116 u = REAL(1.0);
117 DistSq = A00 + REAL(2.0) * B0 + C;
118 }
119 else{
120 u = -B0 / A00;
121 DistSq = B0 * u + C;
122 }
123 }
124 else{ // region 0
125 // minimum at interior point
126 if (Det == REAL(0.0)){
127 u = REAL(0.0);
128 v = REAL(0.0);
129 DistSq = FLT_MAX;
130 }
131 else{
132 dReal InvDet = REAL(1.0) / Det;
133 u *= InvDet;
134 v *= InvDet;
135 DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
136 }
137 }
138 }
139 else{
140 dReal Tmp0, Tmp1, Numer, Denom;
141
142 if(u < REAL(0.0)){ // region 2
143 Tmp0 = A01 + B0;
144 Tmp1 = A11 + B1;
145 if (Tmp1 > Tmp0){
146 Numer = Tmp1 - Tmp0;
147 Denom = A00 - REAL(2.0) * A01 + A11;
148 if (Numer >= Denom){
149 u = REAL(1.0);
150 v = REAL(0.0);
151 DistSq = A00 + REAL(2.0) * B0 + C;
152 }
153 else{
154 u = Numer / Denom;
155 v = REAL(1.0) - u;
156 DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
157 }
158 }
159 else{
160 u = REAL(0.0);
161 if(Tmp1 <= REAL(0.0)){
162 v = REAL(1.0);
163 DistSq = A11 + REAL(2.0) * B1 + C;
164 }
165 else if(B1 >= REAL(0.0)){
166 v = REAL(0.0);
167 DistSq = C;
168 }
169 else{
170 v = -B1 / A11;
171 DistSq = B1 * v + C;
172 }
173 }
174 }
175 else if(v < REAL(0.0)){ // region 6
176 Tmp0 = A01 + B1;
177 Tmp1 = A00 + B0;
178 if (Tmp1 > Tmp0){
179 Numer = Tmp1 - Tmp0;
180 Denom = A00 - REAL(2.0) * A01 + A11;
181 if (Numer >= Denom){
182 v = REAL(1.0);
183 u = REAL(0.0);
184 DistSq = A11 + REAL(2.0) * B1 + C;
185 }
186 else{
187 v = Numer / Denom;
188 u = REAL(1.0) - v;
189 DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
190 }
191 }
192 else{
193 v = REAL(0.0);
194 if (Tmp1 <= REAL(0.0)){
195 u = REAL(1.0);
196 DistSq = A00 + REAL(2.0) * B0 + C;
197 }
198 else if(B0 >= REAL(0.0)){
199 u = REAL(0.0);
200 DistSq = C;
201 }
202 else{
203 u = -B0 / A00;
204 DistSq = B0 * u + C;
205 }
206 }
207 }
208 else{ // region 1
209 Numer = A11 + B1 - A01 - B0;
210 if (Numer <= REAL(0.0)){
211 u = REAL(0.0);
212 v = REAL(1.0);
213 DistSq = A11 + REAL(2.0) * B1 + C;
214 }
215 else{
216 Denom = A00 - REAL(2.0) * A01 + A11;
217 if (Numer >= Denom){
218 u = REAL(1.0);
219 v = REAL(0.0);
220 DistSq = A00 + REAL(2.0) * B0 + C;
221 }
222 else{
223 u = Numer / Denom;
224 v = REAL(1.0) - u;
225 DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
226 }
227 }
228 }
229 }
230
231 Dist = dSqrt(dFabs(DistSq));
232
233 if (Dist <= Radius){
234 Dist = Radius - Dist;
235 return true;
236 }
237 else return false;
238 }
239
240 int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
241 dIASSERT (Stride >= (int)sizeof(dContactGeom));
242 dIASSERT (g1->type == dTriMeshClass);
243 dIASSERT (SphereGeom->type == dSphereClass);
244 dIASSERT ((Flags & NUMC_MASK) >= 1);
245
246 dxTriMesh* TriMesh = (dxTriMesh*)g1;
247
248 // Init
249 const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
250 const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
251
252 TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache();
253 SphereCollider& Collider = pccColliderCache->_SphereCollider;
254
255 const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
256 dReal Radius = dGeomSphereGetRadius(SphereGeom);
257
258 // Sphere
259 Sphere Sphere;
260 Sphere.mCenter.x = Position[0];
261 Sphere.mCenter.y = Position[1];
262 Sphere.mCenter.z = Position[2];
263 Sphere.mRadius = Radius;
264
265 Matrix4x4 amatrix;
266
267 // TC results
268 if (TriMesh->doSphereTC) {
269 dxTriMesh::SphereTC* sphereTC = 0;
270 for (int i = 0; i < TriMesh->SphereTCCache.size(); i++){
271 if (TriMesh->SphereTCCache[i].Geom == SphereGeom){
272 sphereTC = &TriMesh->SphereTCCache[i];
273 break;
274 }
275 }
276
277 if (!sphereTC){
278 TriMesh->SphereTCCache.push(dxTriMesh::SphereTC());
279
280 sphereTC = &TriMesh->SphereTCCache[TriMesh->SphereTCCache.size() - 1];
281 sphereTC->Geom = SphereGeom;
282 }
283
284 // Intersect
285 Collider.SetTemporalCoherence(true);
286 Collider.Collide(*sphereTC, Sphere, TriMesh->Data->BVTree, null,
287 &MakeMatrix(TLPosition, TLRotation, amatrix));
288 }
289 else {
290 Collider.SetTemporalCoherence(false);
291 Collider.Collide(pccColliderCache->defaultSphereCache, Sphere, TriMesh->Data->BVTree, null,
292 &MakeMatrix(TLPosition, TLRotation, amatrix));
293 }
294
295 if (! Collider.GetContactStatus()) {
296 // no collision occurred
297 return 0;
298 }
299
300 // get results
301 int TriCount = Collider.GetNbTouchedPrimitives();
302 const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
303
304 if (TriCount != 0){
305 if (TriMesh->ArrayCallback != null){
306 TriMesh->ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount);
307 }
308
309 int OutTriCount = 0;
310 for (int i = 0; i < TriCount; i++){
311 if (OutTriCount == (Flags & NUMC_MASK)){
312 break;
313 }
314
315 const int TriIndex = Triangles[i];
316
317 dVector3 dv[3];
318 if (!Callback(TriMesh, SphereGeom, TriIndex))
319 continue;
320
321 FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
322
323 dVector3& v0 = dv[0];
324 dVector3& v1 = dv[1];
325 dVector3& v2 = dv[2];
326
327 dVector3 vu;
328 vu[0] = v1[0] - v0[0];
329 vu[1] = v1[1] - v0[1];
330 vu[2] = v1[2] - v0[2];
331 vu[3] = REAL(0.0);
332
333 dVector3 vv;
334 vv[0] = v2[0] - v0[0];
335 vv[1] = v2[1] - v0[1];
336 vv[2] = v2[2] - v0[2];
337 vv[3] = REAL(0.0);
338
339 // Get plane coefficients
340 dVector4 Plane;
341 dCROSS(Plane, =, vu, vv);
342
343 // Even though all triangles might be initially valid,
344 // a triangle may degenerate into a segment after applying
345 // space transformation.
346 if (!dSafeNormalize3(Plane)) {
347 continue;
348 }
349
350 /* If the center of the sphere is within the positive halfspace of the
351 * triangle's plane, allow a contact to be generated.
352 * If the center of the sphere made it into the positive halfspace of a
353 * back-facing triangle, then the physics update and/or velocity needs
354 * to be adjusted (penetration has occured anyway).
355 */
356
357 dReal side = dDOT(Plane,Position) - dDOT(Plane, v0);
358
359 if(side < REAL(0.0)) {
360 continue;
361 }
362
363 dReal Depth;
364 dReal u, v;
365 if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){
366 continue; // Sphere doesn't hit triangle
367 }
368
369 if (Depth < REAL(0.0)){
370 Depth = REAL(0.0);
371 }
372
373 dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
374
375 dReal w = REAL(1.0) - u - v;
376 Contact->pos[0] = (v0[0] * w) + (v1[0] * u) + (v2[0] * v);
377 Contact->pos[1] = (v0[1] * w) + (v1[1] * u) + (v2[1] * v);
378 Contact->pos[2] = (v0[2] * w) + (v1[2] * u) + (v2[2] * v);
379 Contact->pos[3] = REAL(0.0);
380
381 // Using normal as plane (reversed)
382 Contact->normal[0] = -Plane[0];
383 Contact->normal[1] = -Plane[1];
384 Contact->normal[2] = -Plane[2];
385 Contact->normal[3] = REAL(0.0);
386
387 // Depth returned from GetContactData is depth along
388 // contact point - sphere center direction
389 // we'll project it to contact normal
390 dVector3 dir;
391 dir[0] = Position[0]-Contact->pos[0];
392 dir[1] = Position[1]-Contact->pos[1];
393 dir[2] = Position[2]-Contact->pos[2];
394 dReal dirProj = dDOT(dir, Plane) / dSqrt(dDOT(dir, dir));
395 Contact->depth = Depth * dirProj;
396 //Contact->depth = Radius - side; // (mg) penetration depth is distance along normal not shortest distance
397 Contact->side1 = TriIndex;
398
399 //Contact->g1 = TriMesh;
400 //Contact->g2 = SphereGeom;
401
402 OutTriCount++;
403 }
404 #ifdef MERGECONTACTS // Merge all contacts into 1
405 if (OutTriCount != 0){
406 dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride);
407
408 if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT)){
409 Contact->normal[0] *= Contact->depth;
410 Contact->normal[1] *= Contact->depth;
411 Contact->normal[2] *= Contact->depth;
412 Contact->normal[3] *= Contact->depth;
413
414 for (int i = 1; i < OutTriCount; i++){
415 dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
416
417 Contact->pos[0] += TempContact->pos[0];
418 Contact->pos[1] += TempContact->pos[1];
419 Contact->pos[2] += TempContact->pos[2];
420 Contact->pos[3] += TempContact->pos[3];
421
422 Contact->normal[0] += TempContact->normal[0] * TempContact->depth;
423 Contact->normal[1] += TempContact->normal[1] * TempContact->depth;
424 Contact->normal[2] += TempContact->normal[2] * TempContact->depth;
425 Contact->normal[3] += TempContact->normal[3] * TempContact->depth;
426 }
427
428 Contact->pos[0] /= OutTriCount;
429 Contact->pos[1] /= OutTriCount;
430 Contact->pos[2] /= OutTriCount;
431 Contact->pos[3] /= OutTriCount;
432
433 // Remember to divide in square space.
434 Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal) / OutTriCount);
435
436 dNormalize3(Contact->normal);
437 }
438
439 Contact->g1 = TriMesh;
440 Contact->g2 = SphereGeom;
441
442 // TODO:
443 // Side1 now contains index of triangle that gave first hit
444 // Probably we should find index of triangle with deepest penetration
445
446 return 1;
447 }
448 else return 0;
449 #elif defined MERGECONTACTNORMALS // Merge all normals, and distribute between all contacts
450 if (OutTriCount != 0){
451 if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT)){
452 dVector3& Normal = SAFECONTACT(Flags, Contacts, 0, Stride)->normal;
453 Normal[0] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
454 Normal[1] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
455 Normal[2] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
456 Normal[3] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
457
458 for (int i = 1; i < OutTriCount; i++){
459 dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
460
461 Normal[0] += Contact->normal[0] * Contact->depth;
462 Normal[1] += Contact->normal[1] * Contact->depth;
463 Normal[2] += Contact->normal[2] * Contact->depth;
464 Normal[3] += Contact->normal[3] * Contact->depth;
465 }
466 dNormalize3(Normal);
467
468 for (int i = 1; i < OutTriCount; i++){
469 dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
470
471 Contact->normal[0] = Normal[0];
472 Contact->normal[1] = Normal[1];
473 Contact->normal[2] = Normal[2];
474 Contact->normal[3] = Normal[3];
475
476 Contact->g1 = TriMesh;
477 Contact->g2 = SphereGeom;
478 }
479 }
480 else{
481 SAFECONTACT(Flags, Contacts, 0, Stride)->g1 = TriMesh;
482 SAFECONTACT(Flags, Contacts, 0, Stride)->g2 = SphereGeom;
483 }
484
485 return OutTriCount;
486 }
487 else return 0;
488 #else //MERGECONTACTNORMALS // Just gather penetration depths and return
489 for (int i = 0; i < OutTriCount; i++){
490 dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
491
492 //Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal));
493
494 /*Contact->normal[0] /= Contact->depth;
495 Contact->normal[1] /= Contact->depth;
496 Contact->normal[2] /= Contact->depth;
497 Contact->normal[3] /= Contact->depth;*/
498
499 Contact->g1 = TriMesh;
500 Contact->g2 = SphereGeom;
501 }
502
503 return OutTriCount;
504 #endif // MERGECONTACTS
505 }
506 else return 0;
507 }
508 #endif // dTRIMESH_OPCODE
509
510 #if dTRIMESH_GIMPACT
511 int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride)
512 {
513 dIASSERT (Stride >= (int)sizeof(dContactGeom));
514 dIASSERT (g1->type == dTriMeshClass);
515 dIASSERT (SphereGeom->type == dSphereClass);
516 dIASSERT ((Flags & NUMC_MASK) >= 1);
517
518 dxTriMesh* TriMesh = (dxTriMesh*)g1;
519 dVector3& Position = *(dVector3*)dGeomGetPosition(SphereGeom);
520 dReal Radius = dGeomSphereGetRadius(SphereGeom);
521 //Create contact list
522 GDYNAMIC_ARRAY trimeshcontacts;
523 GIM_CREATE_CONTACT_LIST(trimeshcontacts);
524
525 g1 -> recomputeAABB();
526 SphereGeom -> recomputeAABB();
527
528 //Collide trimeshes
529 gim_trimesh_sphere_collisionODE(&TriMesh->m_collision_trimesh,Position,Radius,&trimeshcontacts);
530
531 if(trimeshcontacts.m_size == 0)
532 {
533 GIM_DYNARRAY_DESTROY(trimeshcontacts);
534 return 0;
535 }
536
537 GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts);
538
539 unsigned contactcount = trimeshcontacts.m_size;
540 unsigned maxcontacts = (unsigned)(Flags & NUMC_MASK);
541 if (contactcount > maxcontacts)
542 {
543 contactcount = maxcontacts;
544 }
545
546 dContactGeom* pcontact;
547 unsigned i;
548
549 for (i=0;i<contactcount;i++)
550 {
551 pcontact = SAFECONTACT(Flags, Contacts, i, Stride);
552
553 pcontact->pos[0] = ptrimeshcontacts->m_point[0];
554 pcontact->pos[1] = ptrimeshcontacts->m_point[1];
555 pcontact->pos[2] = ptrimeshcontacts->m_point[2];
556 pcontact->pos[3] = REAL(1.0);
557
558 pcontact->normal[0] = ptrimeshcontacts->m_normal[0];
559 pcontact->normal[1] = ptrimeshcontacts->m_normal[1];
560 pcontact->normal[2] = ptrimeshcontacts->m_normal[2];
561 pcontact->normal[3] = 0;
562
563 pcontact->depth = ptrimeshcontacts->m_depth;
564 pcontact->g1 = g1;
565 pcontact->g2 = SphereGeom;
566
567 ptrimeshcontacts++;
568 }
569
570 GIM_DYNARRAY_DESTROY(trimeshcontacts);
571
572 return (int)contactcount;
573 }
574 #endif // dTRIMESH_GIMPACT
575
576 #endif // dTRIMESH_ENABLED
Open Dynamics Engine