aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src/collision_trimesh_sphere.cpp
diff options
context:
space:
mode:
authordan miller2007-10-19 05:22:23 +0000
committerdan miller2007-10-19 05:22:23 +0000
commit1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43 (patch)
tree51bcae7a1b8381a6bf6fd8025a7de1e30fe0045d /libraries/ode-0.9/ode/src/collision_trimesh_sphere.cpp
parentone more for the gipper (diff)
downloadopensim-SC-1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43.zip
opensim-SC-1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43.tar.gz
opensim-SC-1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43.tar.bz2
opensim-SC-1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43.tar.xz
trying to fix my screwup, please hold on
Diffstat (limited to 'libraries/ode-0.9/ode/src/collision_trimesh_sphere.cpp')
-rw-r--r--libraries/ode-0.9/ode/src/collision_trimesh_sphere.cpp573
1 files changed, 0 insertions, 573 deletions
diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_sphere.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_sphere.cpp
deleted file mode 100644
index 7ee9b4b..0000000
--- a/libraries/ode-0.9/ode/src/collision_trimesh_sphere.cpp
+++ /dev/null
@@ -1,573 +0,0 @@
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
33#define TRIMESH_INTERNAL
34#include "collision_trimesh_internal.h"
35
36#if dTRIMESH_OPCODE
37#define MERGECONTACTS
38
39// Ripped from Opcode 1.1.
40static bool GetContactData(const dVector3& Center, dReal Radius, const dVector3 Origin, const dVector3 Edge0, const dVector3 Edge1, dReal& Dist, dReal& u, dReal& v){
41
42 // now onto the bulk of the collision...
43
44 dVector3 Diff;
45 Diff[0] = Origin[0] - Center[0];
46 Diff[1] = Origin[1] - Center[1];
47 Diff[2] = Origin[2] - Center[2];
48 Diff[3] = Origin[3] - Center[3];
49
50 dReal A00 = dDOT(Edge0, Edge0);
51 dReal A01 = dDOT(Edge0, Edge1);
52 dReal A11 = dDOT(Edge1, Edge1);
53
54 dReal B0 = dDOT(Diff, Edge0);
55 dReal B1 = dDOT(Diff, Edge1);
56
57 dReal C = dDOT(Diff, Diff);
58
59 dReal Det = dFabs(A00 * A11 - A01 * A01);
60 u = A01 * B1 - A11 * B0;
61 v = A01 * B0 - A00 * B1;
62
63 dReal DistSq;
64
65 if (u + v <= Det){
66 if(u < REAL(0.0)){
67 if(v < REAL(0.0)){ // region 4
68 if(B0 < REAL(0.0)){
69 v = REAL(0.0);
70 if (-B0 >= A00){
71 u = REAL(1.0);
72 DistSq = A00 + REAL(2.0) * B0 + C;
73 }
74 else{
75 u = -B0 / A00;
76 DistSq = B0 * u + C;
77 }
78 }
79 else{
80 u = REAL(0.0);
81 if(B1 >= REAL(0.0)){
82 v = REAL(0.0);
83 DistSq = C;
84 }
85 else if(-B1 >= A11){
86 v = REAL(1.0);
87 DistSq = A11 + REAL(2.0) * B1 + C;
88 }
89 else{
90 v = -B1 / A11;
91 DistSq = B1 * v + C;
92 }
93 }
94 }
95 else{ // region 3
96 u = REAL(0.0);
97 if(B1 >= REAL(0.0)){
98 v = REAL(0.0);
99 DistSq = C;
100 }
101 else if(-B1 >= A11){
102 v = REAL(1.0);
103 DistSq = A11 + REAL(2.0) * B1 + C;
104 }
105 else{
106 v = -B1 / A11;
107 DistSq = B1 * v + C;
108 }
109 }
110 }
111 else if(v < REAL(0.0)){ // region 5
112 v = REAL(0.0);
113 if (B0 >= REAL(0.0)){
114 u = REAL(0.0);
115 DistSq = C;
116 }
117 else if (-B0 >= A00){
118 u = REAL(1.0);
119 DistSq = A00 + REAL(2.0) * B0 + C;
120 }
121 else{
122 u = -B0 / A00;
123 DistSq = B0 * u + C;
124 }
125 }
126 else{ // region 0
127 // minimum at interior point
128 if (Det == REAL(0.0)){
129 u = REAL(0.0);
130 v = REAL(0.0);
131 DistSq = FLT_MAX;
132 }
133 else{
134 dReal InvDet = REAL(1.0) / Det;
135 u *= InvDet;
136 v *= InvDet;
137 DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
138 }
139 }
140 }
141 else{
142 dReal Tmp0, Tmp1, Numer, Denom;
143
144 if(u < REAL(0.0)){ // region 2
145 Tmp0 = A01 + B0;
146 Tmp1 = A11 + B1;
147 if (Tmp1 > Tmp0){
148 Numer = Tmp1 - Tmp0;
149 Denom = A00 - REAL(2.0) * A01 + A11;
150 if (Numer >= Denom){
151 u = REAL(1.0);
152 v = REAL(0.0);
153 DistSq = A00 + REAL(2.0) * B0 + C;
154 }
155 else{
156 u = Numer / Denom;
157 v = REAL(1.0) - u;
158 DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
159 }
160 }
161 else{
162 u = REAL(0.0);
163 if(Tmp1 <= REAL(0.0)){
164 v = REAL(1.0);
165 DistSq = A11 + REAL(2.0) * B1 + C;
166 }
167 else if(B1 >= REAL(0.0)){
168 v = REAL(0.0);
169 DistSq = C;
170 }
171 else{
172 v = -B1 / A11;
173 DistSq = B1 * v + C;
174 }
175 }
176 }
177 else if(v < REAL(0.0)){ // region 6
178 Tmp0 = A01 + B1;
179 Tmp1 = A00 + B0;
180 if (Tmp1 > Tmp0){
181 Numer = Tmp1 - Tmp0;
182 Denom = A00 - REAL(2.0) * A01 + A11;
183 if (Numer >= Denom){
184 v = REAL(1.0);
185 u = REAL(0.0);
186 DistSq = A11 + REAL(2.0) * B1 + C;
187 }
188 else{
189 v = Numer / Denom;
190 u = REAL(1.0) - v;
191 DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
192 }
193 }
194 else{
195 v = REAL(0.0);
196 if (Tmp1 <= REAL(0.0)){
197 u = REAL(1.0);
198 DistSq = A00 + REAL(2.0) * B0 + C;
199 }
200 else if(B0 >= REAL(0.0)){
201 u = REAL(0.0);
202 DistSq = C;
203 }
204 else{
205 u = -B0 / A00;
206 DistSq = B0 * u + C;
207 }
208 }
209 }
210 else{ // region 1
211 Numer = A11 + B1 - A01 - B0;
212 if (Numer <= REAL(0.0)){
213 u = REAL(0.0);
214 v = REAL(1.0);
215 DistSq = A11 + REAL(2.0) * B1 + C;
216 }
217 else{
218 Denom = A00 - REAL(2.0) * A01 + A11;
219 if (Numer >= Denom){
220 u = REAL(1.0);
221 v = REAL(0.0);
222 DistSq = A00 + REAL(2.0) * B0 + C;
223 }
224 else{
225 u = Numer / Denom;
226 v = REAL(1.0) - u;
227 DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
228 }
229 }
230 }
231 }
232
233 Dist = dSqrt(dFabs(DistSq));
234
235 if (Dist <= Radius){
236 Dist = Radius - Dist;
237 return true;
238 }
239 else return false;
240}
241
242int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
243 dIASSERT (Stride >= (int)sizeof(dContactGeom));
244 dIASSERT (g1->type == dTriMeshClass);
245 dIASSERT (SphereGeom->type == dSphereClass);
246 dIASSERT ((Flags & NUMC_MASK) >= 1);
247
248 dxTriMesh* TriMesh = (dxTriMesh*)g1;
249
250 // Init
251 const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
252 const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
253
254 SphereCollider& Collider = TriMesh->_SphereCollider;
255
256 const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
257 dReal Radius = dGeomSphereGetRadius(SphereGeom);
258
259 // Sphere
260 Sphere Sphere;
261 Sphere.mCenter.x = Position[0];
262 Sphere.mCenter.y = Position[1];
263 Sphere.mCenter.z = Position[2];
264 Sphere.mRadius = Radius;
265
266 Matrix4x4 amatrix;
267
268 // TC results
269 if (TriMesh->doSphereTC) {
270 dxTriMesh::SphereTC* sphereTC = 0;
271 for (int i = 0; i < TriMesh->SphereTCCache.size(); i++){
272 if (TriMesh->SphereTCCache[i].Geom == SphereGeom){
273 sphereTC = &TriMesh->SphereTCCache[i];
274 break;
275 }
276 }
277
278 if (!sphereTC){
279 TriMesh->SphereTCCache.push(dxTriMesh::SphereTC());
280
281 sphereTC = &TriMesh->SphereTCCache[TriMesh->SphereTCCache.size() - 1];
282 sphereTC->Geom = SphereGeom;
283 }
284
285 // Intersect
286 Collider.SetTemporalCoherence(true);
287 Collider.Collide(*sphereTC, Sphere, TriMesh->Data->BVTree, null,
288 &MakeMatrix(TLPosition, TLRotation, amatrix));
289 }
290 else {
291 Collider.SetTemporalCoherence(false);
292 Collider.Collide(dxTriMesh::defaultSphereCache, Sphere, TriMesh->Data->BVTree, null,
293 &MakeMatrix(TLPosition, TLRotation, amatrix));
294 }
295
296 if (! Collider.GetContactStatus()) {
297 // no collision occurred
298 return 0;
299 }
300
301 // get results
302 int TriCount = Collider.GetNbTouchedPrimitives();
303 const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
304
305 if (TriCount != 0){
306 if (TriMesh->ArrayCallback != null){
307 TriMesh->ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount);
308 }
309
310 int OutTriCount = 0;
311 for (int i = 0; i < TriCount; i++){
312 if (OutTriCount == (Flags & NUMC_MASK)){
313 break;
314 }
315
316 const int TriIndex = Triangles[i];
317
318 dVector3 dv[3];
319 if (!Callback(TriMesh, SphereGeom, TriIndex))
320 continue;
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 dReal Area = dSqrt(dDOT(Plane, Plane)); // We can use this later
344 Plane[0] /= Area;
345 Plane[1] /= Area;
346 Plane[2] /= Area;
347
348 Plane[3] = dDOT(Plane, v0);
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) - Plane[3];
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
511int 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 //Collide trimeshes
526 gim_trimesh_sphere_collision(&TriMesh->m_collision_trimesh,Position,Radius,&trimeshcontacts);
527
528 if(trimeshcontacts.m_size == 0)
529 {
530 GIM_DYNARRAY_DESTROY(trimeshcontacts);
531 return 0;
532 }
533
534 GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts);
535
536 unsigned contactcount = trimeshcontacts.m_size;
537 unsigned maxcontacts = (unsigned)(Flags & NUMC_MASK);
538 if (contactcount > maxcontacts)
539 {
540 contactcount = maxcontacts;
541 }
542
543 dContactGeom* pcontact;
544 unsigned i;
545
546 for (i=0;i<contactcount;i++)
547 {
548 pcontact = SAFECONTACT(Flags, Contacts, i, Stride);
549
550 pcontact->pos[0] = ptrimeshcontacts->m_point[0];
551 pcontact->pos[1] = ptrimeshcontacts->m_point[1];
552 pcontact->pos[2] = ptrimeshcontacts->m_point[2];
553 pcontact->pos[3] = REAL(1.0);
554
555 pcontact->normal[0] = ptrimeshcontacts->m_normal[0];
556 pcontact->normal[1] = ptrimeshcontacts->m_normal[1];
557 pcontact->normal[2] = ptrimeshcontacts->m_normal[2];
558 pcontact->normal[3] = 0;
559
560 pcontact->depth = ptrimeshcontacts->m_depth;
561 pcontact->g1 = g1;
562 pcontact->g2 = SphereGeom;
563
564 ptrimeshcontacts++;
565 }
566
567 GIM_DYNARRAY_DESTROY(trimeshcontacts);
568
569 return (int)contactcount;
570}
571#endif // dTRIMESH_GIMPACT
572
573#endif // dTRIMESH_ENABLED
generated by cgit v1.1 at 2024-12-27 00:40:16 +0000