ode/src/collision_trimesh_opcode.cpp

   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 #include "collision_trimesh_internal.h"
  31
  32 #if dTRIMESH_ENABLED
  33 #if dTRIMESH_OPCODE
  34
  35
  36 void TrimeshCollidersCache::InitOPCODECaches()
  37 {
  38         _RayCollider.SetDestination(&Faces);
  39
  40 /* -- not used
  41         _PlanesCollider.SetTemporalCoherence(true);
  42 */
  43
  44         _SphereCollider.SetTemporalCoherence(true);
  45         _SphereCollider.SetPrimitiveTests(false);
  46
  47         _OBBCollider.SetTemporalCoherence(true);
  48
  49         // no first-contact test (i.e. return full contact info)
  50         _AABBTreeCollider.SetFirstContact( false );
  51         // temporal coherence only works with "first contact" tests
  52         _AABBTreeCollider.SetTemporalCoherence(false);
  53         // Perform full BV-BV tests (true) or SAT-lite tests (false)
  54         _AABBTreeCollider.SetFullBoxBoxTest( true );
  55         // Perform full Primitive-BV tests (true) or SAT-lite tests (false)
  56         _AABBTreeCollider.SetFullPrimBoxTest( true );
  57         const char* msg;
  58         if ((msg =_AABBTreeCollider.ValidateSettings()))
  59                 dDebug (d_ERR_UASSERT, msg, " (%s:%d)", __FILE__,__LINE__);
  60
  61 /* -- not used
  62         _LSSCollider.SetTemporalCoherence(false);
  63         _LSSCollider.SetPrimitiveTests(false);
  64         _LSSCollider.SetFirstContact(false);
  65 */
  66 }
  67
  68
  69
  70 // Trimesh data
  71 dxTriMeshData::dxTriMeshData() : UseFlags( NULL )
  72 {
  73 #if !dTRIMESH_ENABLED
  74   dUASSERT(false, "dTRIMESH_ENABLED is not defined. Trimesh geoms will not work");
  75 #endif
  76 }
  77
  78 dxTriMeshData::~dxTriMeshData()
  79 {
  80         if ( UseFlags )
  81                 delete [] UseFlags;
  82 }
  83
  84 void
  85 dxTriMeshData::Build(const void* Vertices, int VertexStide, int VertexCount,
  86                      const void* Indices, int IndexCount, int TriStride,
  87                      const void* in_Normals,
  88                      bool Single)
  89 {
  90 #if dTRIMESH_ENABLED
  91
  92     Mesh.SetNbTriangles(IndexCount / 3);
  93     Mesh.SetNbVertices(VertexCount);
  94     Mesh.SetPointers((IndexedTriangle*)Indices, (Point*)Vertices);
  95     Mesh.SetStrides(TriStride, VertexStide);
  96     Mesh.SetSingle(Single);
  97
  98     // Build tree
  99     BuildSettings Settings;
 100     // recommended in Opcode User Manual
 101     //Settings.mRules = SPLIT_COMPLETE | SPLIT_SPLATTERPOINTS | SPLIT_GEOMCENTER;
 102     // used in ODE, why?
 103     //Settings.mRules = SPLIT_BEST_AXIS;
 104
 105     // best compromise?
 106     Settings.mRules = SPLIT_BEST_AXIS | SPLIT_SPLATTER_POINTS | SPLIT_GEOM_CENTER;
 107
 108
 109     OPCODECREATE TreeBuilder;
 110     TreeBuilder.mIMesh = &Mesh;
 111
 112     TreeBuilder.mSettings = Settings;
 113     TreeBuilder.mNoLeaf = true;
 114     TreeBuilder.mQuantized = false;
 115
 116     TreeBuilder.mKeepOriginal = false;
 117     TreeBuilder.mCanRemap = false;
 118
 119
 120
 121     BVTree.Build(TreeBuilder);
 122
 123     // compute model space AABB
 124     dVector3 AABBMax, AABBMin;
 125     AABBMax[0] = AABBMax[1] = AABBMax[2] = (dReal) -dInfinity;
 126     AABBMin[0] = AABBMin[1] = AABBMin[2] = (dReal) dInfinity;
 127     if( Single ) {
 128         const char* verts = (const char*)Vertices;
 129         for( int i = 0; i < VertexCount; ++i ) {
 130         const float* v = (const float*)verts;
 131         if( v[0] > AABBMax[0] ) AABBMax[0] = v[0];
 132         if( v[1] > AABBMax[1] ) AABBMax[1] = v[1];
 133         if( v[2] > AABBMax[2] ) AABBMax[2] = v[2];
 134         if( v[0] < AABBMin[0] ) AABBMin[0] = v[0];
 135         if( v[1] < AABBMin[1] ) AABBMin[1] = v[1];
 136         if( v[2] < AABBMin[2] ) AABBMin[2] = v[2];
 137         verts += VertexStide;
 138         }
 139     } else {
 140         const char* verts = (const char*)Vertices;
 141         for( int i = 0; i < VertexCount; ++i ) {
 142         const double* v = (const double*)verts;
 143         if( v[0] > AABBMax[0] ) AABBMax[0] = (dReal) v[0];
 144         if( v[1] > AABBMax[1] ) AABBMax[1] = (dReal) v[1];
 145         if( v[2] > AABBMax[2] ) AABBMax[2] = (dReal) v[2];
 146         if( v[0] < AABBMin[0] ) AABBMin[0] = (dReal) v[0];
 147         if( v[1] < AABBMin[1] ) AABBMin[1] = (dReal) v[1];
 148         if( v[2] < AABBMin[2] ) AABBMin[2] = (dReal) v[2];
 149         verts += VertexStide;
 150         }
 151     }
 152     AABBCenter[0] = (AABBMin[0] + AABBMax[0]) * REAL(0.5);
 153     AABBCenter[1] = (AABBMin[1] + AABBMax[1]) * REAL(0.5);
 154     AABBCenter[2] = (AABBMin[2] + AABBMax[2]) * REAL(0.5);
 155     AABBExtents[0] = AABBMax[0] - AABBCenter[0];
 156     AABBExtents[1] = AABBMax[1] - AABBCenter[1];
 157     AABBExtents[2] = AABBMax[2] - AABBCenter[2];
 158
 159     // user data (not used by OPCODE)
 160     Normals = (dReal *) in_Normals;
 161
 162         UseFlags = 0;
 163
 164 #endif // dTRIMESH_ENABLED
 165 }
 166
 167 struct EdgeRecord
 168 {
 169         int VertIdx1;   // Index into vertex array for this edges vertices
 170         int VertIdx2;
 171         int TriIdx;             // Index into triangle array for triangle this edge belongs to
 172
 173         uint8 EdgeFlags;
 174         uint8 Vert1Flags;
 175         uint8 Vert2Flags;
 176         bool Concave;
 177 };
 178
 179 // Edge comparison function for qsort
 180 static int EdgeCompare(const void* edge1, const void* edge2)
 181 {
 182         EdgeRecord* e1 = (EdgeRecord*)edge1;
 183         EdgeRecord* e2 = (EdgeRecord*)edge2;
 184
 185         if (e1->VertIdx1 == e2->VertIdx1)
 186                 return e1->VertIdx2 - e2->VertIdx2;
 187         else
 188                 return e1->VertIdx1 - e2->VertIdx1;
 189 }
 190
 191 void SetupEdge(EdgeRecord* edge, int edgeIdx, int triIdx, const dTriIndex* vertIdxs)
 192 {
 193         if (edgeIdx == 0)
 194         {
 195                 edge->EdgeFlags  = dxTriMeshData::kEdge0;
 196                 edge->Vert1Flags = dxTriMeshData::kVert0;
 197                 edge->Vert2Flags = dxTriMeshData::kVert1;
 198                 edge->VertIdx1 = vertIdxs[0];
 199                 edge->VertIdx2 = vertIdxs[1];
 200         }
 201         else if (edgeIdx == 1)
 202         {
 203                 edge->EdgeFlags  = dxTriMeshData::kEdge1;
 204                 edge->Vert1Flags = dxTriMeshData::kVert1;
 205                 edge->Vert2Flags = dxTriMeshData::kVert2;
 206                 edge->VertIdx1 = vertIdxs[1];
 207                 edge->VertIdx2 = vertIdxs[2];
 208         }
 209         else if (edgeIdx == 2)
 210         {
 211                 edge->EdgeFlags  = dxTriMeshData::kEdge2;
 212                 edge->Vert1Flags = dxTriMeshData::kVert2;
 213                 edge->Vert2Flags = dxTriMeshData::kVert0;
 214                 edge->VertIdx1 = vertIdxs[2];
 215                 edge->VertIdx2 = vertIdxs[0];
 216         }
 217
 218         // Make sure vert index 1 is less than index 2 (for easier sorting)
 219         if (edge->VertIdx1 > edge->VertIdx2)
 220         {
 221                 unsigned int tempIdx = edge->VertIdx1;
 222                 edge->VertIdx1 = edge->VertIdx2;
 223                 edge->VertIdx2 = tempIdx;
 224
 225                 uint8 tempFlags = edge->Vert1Flags;
 226                 edge->Vert1Flags = edge->Vert2Flags;
 227                 edge->Vert2Flags = tempFlags;
 228         }
 229
 230         edge->TriIdx = triIdx;
 231         edge->Concave = false;
 232 }
 233
 234 #if dTRIMESH_ENABLED
 235
 236 // Get the vertex opposite this edge in the triangle
 237 inline Point GetOppositeVert(EdgeRecord* edge, const Point* vertices[])
 238 {
 239         if ((edge->Vert1Flags == dxTriMeshData::kVert0 && edge->Vert2Flags == dxTriMeshData::kVert1) ||
 240                 (edge->Vert1Flags == dxTriMeshData::kVert1 && edge->Vert2Flags == dxTriMeshData::kVert0))
 241         {
 242                 return *vertices[2];
 243         }
 244         else if ((edge->Vert1Flags == dxTriMeshData::kVert1 && edge->Vert2Flags == dxTriMeshData::kVert2) ||
 245                 (edge->Vert1Flags == dxTriMeshData::kVert2 && edge->Vert2Flags == dxTriMeshData::kVert1))
 246         {
 247                 return *vertices[0];
 248         }
 249         else
 250                 return *vertices[1];
 251 }
 252
 253 #endif // dTRIMESH_ENABLED
 254
 255 void dxTriMeshData::Preprocess()
 256 {
 257
 258 #if dTRIMESH_ENABLED
 259
 260         // If this mesh has already been preprocessed, exit
 261         if (UseFlags)
 262                 return;
 263
 264         udword numTris = Mesh.GetNbTriangles();
 265         udword numEdges = numTris * 3;
 266
 267         UseFlags = new uint8[numTris];
 268         memset(UseFlags, 0, sizeof(uint8) * numTris);
 269
 270         EdgeRecord* records = new EdgeRecord[numEdges];
 271
 272         // Make a list of every edge in the mesh
 273         const IndexedTriangle* tris = Mesh.GetTris();
 274     for (unsigned int i = 0; i < numTris; i++)
 275         {
 276                 SetupEdge(&records[i*3],   0, i, tris->mVRef);
 277                 SetupEdge(&records[i*3+1], 1, i, tris->mVRef);
 278                 SetupEdge(&records[i*3+2], 2, i, tris->mVRef);
 279
 280                 tris = (const IndexedTriangle*)(((uint8*)tris) + Mesh.GetTriStride());
 281         }
 282
 283         // Sort the edges, so the ones sharing the same verts are beside each other
 284         qsort(records, numEdges, sizeof(EdgeRecord), EdgeCompare);
 285
 286         // Go through the sorted list of edges and flag all the edges and vertices that we need to use
 287         for (unsigned int i = 0; i < numEdges; i++)
 288         {
 289                 EdgeRecord* rec1 = &records[i];
 290                 EdgeRecord* rec2 = 0;
 291                 if (i < numEdges - 1)
 292                         rec2 = &records[i+1];
 293
 294                 if (rec2 &&
 295                         rec1->VertIdx1 == rec2->VertIdx1 &&
 296                         rec1->VertIdx2 == rec2->VertIdx2)
 297                 {
 298                         VertexPointers vp;
 299                         ConversionArea vc;
 300                         Mesh.GetTriangle(vp, rec1->TriIdx, vc);
 301
 302                         // Get the normal of the first triangle
 303                         Point triNorm = (*vp.Vertex[2] - *vp.Vertex[1]) ^ (*vp.Vertex[0] - *vp.Vertex[1]);
 304                         triNorm.Normalize();
 305
 306                         // Get the vert opposite this edge in the first triangle
 307                         Point oppositeVert1 = GetOppositeVert(rec1, vp.Vertex);
 308
 309                         // Get the vert opposite this edge in the second triangle
 310                         Mesh.GetTriangle(vp, rec2->TriIdx, vc);
 311                         Point oppositeVert2 = GetOppositeVert(rec2, vp.Vertex);
 312
 313                         float dot = triNorm.Dot((oppositeVert2 - oppositeVert1).Normalize());
 314
 315                         // We let the dot threshold for concavity get slightly negative to allow for rounding errors
 316                         static const float kConcaveThresh = -0.000001f;
 317
 318                         // This is a concave edge, leave it for the next pass
 319                         if (dot >= kConcaveThresh)
 320                                 rec1->Concave = true;
 321                         // If this is a convex edge, mark its vertices and edge as used
 322                         else
 323                                 UseFlags[rec1->TriIdx] |= rec1->Vert1Flags | rec1->Vert2Flags | rec1->EdgeFlags;
 324
 325                         // Skip the second edge
 326                         i++;
 327                 }
 328                 // This is a boundary edge
 329                 else
 330                 {
 331                         UseFlags[rec1->TriIdx] |= rec1->Vert1Flags | rec1->Vert2Flags | rec1->EdgeFlags;
 332                 }
 333         }
 334
 335         // Go through the list once more, and take any edge we marked as concave and
 336         // clear it's vertices flags in any triangles they're used in
 337         for (unsigned int i = 0; i < numEdges; i++)
 338         {
 339                 EdgeRecord& er = records[i];
 340
 341                 if (er.Concave)
 342                 {
 343                         for (unsigned int j = 0; j < numEdges; j++)
 344                         {
 345                                 EdgeRecord& curER = records[j];
 346
 347                                 if (curER.VertIdx1 == er.VertIdx1 ||
 348                                         curER.VertIdx1 == er.VertIdx2)
 349                                         UseFlags[curER.TriIdx] &= ~curER.Vert1Flags;
 350
 351                                 if (curER.VertIdx2 == er.VertIdx1 ||
 352                                         curER.VertIdx2 == er.VertIdx2)
 353                                         UseFlags[curER.TriIdx] &= ~curER.Vert2Flags;
 354                         }
 355                 }
 356         }
 357
 358         delete [] records;
 359
 360 #endif // dTRIMESH_ENABLED
 361
 362 }
 363
 364 dTriMeshDataID dGeomTriMeshDataCreate(){
 365     return new dxTriMeshData();
 366 }
 367
 368 void dGeomTriMeshDataDestroy(dTriMeshDataID g){
 369     delete g;
 370 }
 371
 372
 373
 374
 375 void dGeomTriMeshSetLastTransform( dxGeom* g, dMatrix4 last_trans )
 376 {
 377         dAASSERT(g)
 378     dUASSERT(g->type == dTriMeshClass, "geom not trimesh");
 379
 380     for (int i=0; i<16; i++)
 381         (((dxTriMesh*)g)->last_trans)[ i ] = last_trans[ i ];
 382
 383     return;
 384 }
 385
 386
 387 dReal* dGeomTriMeshGetLastTransform( dxGeom* g )
 388 {
 389         dAASSERT(g)
 390     dUASSERT(g->type == dTriMeshClass, "geom not trimesh");
 391
 392     return (dReal*)(((dxTriMesh*)g)->last_trans);
 393 }
 394
 395
 396
 397
 398 void dGeomTriMeshDataSet(dTriMeshDataID g, int data_id, void* in_data)
 399 {
 400     dUASSERT(g, "argument not trimesh data");
 401
 402     switch (data_id)
 403         {
 404     case TRIMESH_FACE_NORMALS:
 405                 g->Normals = (dReal *) in_data;
 406                 break;
 407
 408     default:
 409                 dUASSERT(data_id, "invalid data type");
 410                 break;
 411     }
 412
 413     return;
 414 }
 415
 416
 417
 418 void*  dGeomTriMeshDataGet(dTriMeshDataID g, int data_id)
 419 {
 420     dUASSERT(g, "argument not trimesh data");
 421
 422     switch (data_id)
 423         {
 424     case TRIMESH_FACE_NORMALS:
 425         return (void *) g->Normals;
 426         break;
 427
 428         default:
 429         dUASSERT(data_id, "invalid data type");
 430         break;
 431     }
 432
 433     return NULL;
 434 }
 435
 436
 437 void dGeomTriMeshDataBuildSingle1(dTriMeshDataID g,
 438                                   const void* Vertices, int VertexStride, int VertexCount,
 439                                   const void* Indices, int IndexCount, int TriStride,
 440                                   const void* Normals)
 441 {
 442     dUASSERT(g, "argument not trimesh data");
 443
 444     g->Build(Vertices, VertexStride, VertexCount,
 445              Indices, IndexCount, TriStride,
 446              Normals,
 447              true);
 448 }
 449
 450
 451 void dGeomTriMeshDataBuildSingle(dTriMeshDataID g,
 452                                  const void* Vertices, int VertexStride, int VertexCount,
 453                                  const void* Indices, int IndexCount, int TriStride)
 454 {
 455     dGeomTriMeshDataBuildSingle1(g, Vertices, VertexStride, VertexCount,
 456                                  Indices, IndexCount, TriStride, (void*)NULL);
 457 }
 458
 459
 460 void dGeomTriMeshDataBuildDouble1(dTriMeshDataID g,
 461                                   const void* Vertices, int VertexStride, int VertexCount,
 462                                  const void* Indices, int IndexCount, int TriStride,
 463                                  const void* Normals)
 464 {
 465     dUASSERT(g, "argument not trimesh data");
 466
 467     g->Build(Vertices, VertexStride, VertexCount,
 468              Indices, IndexCount, TriStride,
 469              Normals,
 470              false);
 471 }
 472
 473
 474 void dGeomTriMeshDataBuildDouble(dTriMeshDataID g,
 475                                  const void* Vertices, int VertexStride, int VertexCount,
 476                                  const void* Indices, int IndexCount, int TriStride) {
 477     dGeomTriMeshDataBuildDouble1(g, Vertices, VertexStride, VertexCount,
 478                                  Indices, IndexCount, TriStride, NULL);
 479 }
 480
 481
 482 void dGeomTriMeshDataBuildSimple1(dTriMeshDataID g,
 483                                   const dReal* Vertices, int VertexCount,
 484                                  const dTriIndex* Indices, int IndexCount,
 485                                  const int* Normals){
 486 #ifdef dSINGLE
 487     dGeomTriMeshDataBuildSingle1(g,
 488                                 Vertices, 4 * sizeof(dReal), VertexCount,
 489                                 Indices, IndexCount, 3 * sizeof(dTriIndex),
 490                                 Normals);
 491 #else
 492     dGeomTriMeshDataBuildDouble1(g, Vertices, 4 * sizeof(dReal), VertexCount,
 493                                 Indices, IndexCount, 3 * sizeof(dTriIndex),
 494                                 Normals);
 495 #endif
 496 }
 497
 498
 499 void dGeomTriMeshDataBuildSimple(dTriMeshDataID g,
 500                                  const dReal* Vertices, int VertexCount,
 501                                  const dTriIndex* Indices, int IndexCount) {
 502     dGeomTriMeshDataBuildSimple1(g,
 503                                  Vertices, VertexCount, Indices, IndexCount,
 504                                  (const int*)NULL);
 505 }
 506
 507 void dGeomTriMeshDataPreprocess(dTriMeshDataID g)
 508 {
 509     dUASSERT(g, "argument not trimesh data");
 510         g->Preprocess();
 511 }
 512
 513 void dGeomTriMeshDataGetBuffer(dTriMeshDataID g, unsigned char** buf, int* bufLen)
 514 {
 515     dUASSERT(g, "argument not trimesh data");
 516 #if dTRIMESH_ENABLED
 517         *buf = g->UseFlags;
 518         *bufLen = g->Mesh.GetNbTriangles();
 519 #endif // dTRIMESH_ENABLED
 520 }
 521
 522 void dGeomTriMeshDataSetBuffer(dTriMeshDataID g, unsigned char* buf)
 523 {
 524     dUASSERT(g, "argument not trimesh data");
 525         g->UseFlags = buf;
 526 }
 527
 528
 529 dxTriMesh::dxTriMesh(dSpaceID Space, dTriMeshDataID Data) : dxGeom(Space, 1)
 530 {
 531     type = dTriMeshClass;
 532
 533     this->Data = Data;
 534
 535         /* TC has speed/space 'issues' that don't make it a clear
 536            win by default on spheres/boxes. */
 537         this->doSphereTC = false;
 538         this->doBoxTC = false;
 539         this->doCapsuleTC = false;
 540
 541     for (int i=0; i<16; i++)
 542         last_trans[i] = REAL( 0.0 );
 543 }
 544
 545 dxTriMesh::~dxTriMesh(){
 546     //
 547 }
 548
 549 // Cleanup for allocations when shutting down ODE
 550 /*extern */void opcode_collider_cleanup()
 551 {
 552 #if !dTLS_ENABLED
 553 #if dTRIMESH_ENABLED
 554
 555         // Clear TC caches
 556         TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache();
 557         pccColliderCache->Faces.Empty();
 558         pccColliderCache->defaultSphereCache.TouchedPrimitives.Empty();
 559         pccColliderCache->defaultBoxCache.TouchedPrimitives.Empty();
 560         pccColliderCache->defaultCapsuleCache.TouchedPrimitives.Empty();
 561
 562 #endif // dTRIMESH_ENABLED
 563 #endif // dTLS_ENABLED
 564 }
 565
 566
 567
 568 void dxTriMesh::ClearTCCache()
 569 {
 570 #if dTRIMESH_ENABLED
 571   /* dxTriMesh::ClearTCCache uses dArray's setSize(0) to clear the caches -
 572      but the destructor isn't called when doing this, so we would leak.
 573      So, call the previous caches' containers' destructors by hand first. */
 574     int i, n;
 575     n = SphereTCCache.size();
 576     for( i = 0; i < n; ++i ) {
 577         SphereTCCache[i].~SphereTC();
 578     }
 579     SphereTCCache.setSize(0);
 580     n = BoxTCCache.size();
 581     for( i = 0; i < n; ++i ) {
 582         BoxTCCache[i].~BoxTC();
 583     }
 584     BoxTCCache.setSize(0);
 585         n = CapsuleTCCache.size();
 586         for( i = 0; i < n; ++i ) {
 587           CapsuleTCCache[i].~CapsuleTC();
 588         }
 589         CapsuleTCCache.setSize(0);
 590 #endif // dTRIMESH_ENABLED
 591 }
 592
 593
 594 int dxTriMesh::AABBTest(dxGeom* g, dReal aabb[6]){
 595     return 1;
 596 }
 597
 598
 599 void dxTriMesh::computeAABB() {
 600     const dxTriMeshData* d = Data;
 601     dVector3 c;
 602     const dMatrix3& R = final_posr->R;
 603     const dVector3& pos = final_posr->pos;
 604
 605     dMULTIPLY0_331( c, R, d->AABBCenter );
 606
 607     dReal xrange = dFabs(R[0] * Data->AABBExtents[0]) +
 608         dFabs(R[1] * Data->AABBExtents[1]) +
 609         dFabs(R[2] * Data->AABBExtents[2]);
 610     dReal yrange = dFabs(R[4] * Data->AABBExtents[0]) +
 611         dFabs(R[5] * Data->AABBExtents[1]) +
 612         dFabs(R[6] * Data->AABBExtents[2]);
 613     dReal zrange = dFabs(R[8] * Data->AABBExtents[0]) +
 614         dFabs(R[9] * Data->AABBExtents[1]) +
 615         dFabs(R[10] * Data->AABBExtents[2]);
 616
 617     aabb[0] = c[0] + pos[0] - xrange;
 618     aabb[1] = c[0] + pos[0] + xrange;
 619     aabb[2] = c[1] + pos[1] - yrange;
 620     aabb[3] = c[1] + pos[1] + yrange;
 621     aabb[4] = c[2] + pos[2] - zrange;
 622     aabb[5] = c[2] + pos[2] + zrange;
 623 }
 624
 625
 626 void dxTriMeshData::UpdateData()
 627 {
 628 #if  dTRIMESH_ENABLED
 629         BVTree.Refit();
 630 #endif // dTRIMESH_ENABLED
 631 }
 632
 633
 634 dGeomID dCreateTriMesh(dSpaceID space,
 635                        dTriMeshDataID Data,
 636                        dTriCallback* Callback,
 637                        dTriArrayCallback* ArrayCallback,
 638                        dTriRayCallback* RayCallback)
 639 {
 640     dxTriMesh* Geom = new dxTriMesh(space, Data);
 641     Geom->Callback = Callback;
 642     Geom->ArrayCallback = ArrayCallback;
 643     Geom->RayCallback = RayCallback;
 644
 645     return Geom;
 646 }
 647
 648 void dGeomTriMeshSetCallback(dGeomID g, dTriCallback* Callback)
 649 {
 650         dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
 651         ((dxTriMesh*)g)->Callback = Callback;
 652 }
 653
 654 dTriCallback* dGeomTriMeshGetCallback(dGeomID g)
 655 {
 656         dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
 657         return ((dxTriMesh*)g)->Callback;
 658 }
 659
 660 void dGeomTriMeshSetArrayCallback(dGeomID g, dTriArrayCallback* ArrayCallback)
 661 {
 662         dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
 663         ((dxTriMesh*)g)->ArrayCallback = ArrayCallback;
 664 }
 665
 666 dTriArrayCallback* dGeomTriMeshGetArrayCallback(dGeomID g)
 667 {
 668         dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
 669         return ((dxTriMesh*)g)->ArrayCallback;
 670 }
 671
 672 void dGeomTriMeshSetRayCallback(dGeomID g, dTriRayCallback* Callback)
 673 {
 674         dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
 675         ((dxTriMesh*)g)->RayCallback = Callback;
 676 }
 677
 678 dTriRayCallback* dGeomTriMeshGetRayCallback(dGeomID g)
 679 {
 680         dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
 681         return ((dxTriMesh*)g)->RayCallback;
 682 }
 683
 684 void dGeomTriMeshSetData(dGeomID g, dTriMeshDataID Data)
 685 {
 686         dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
 687         ((dxTriMesh*)g)->Data = Data;
 688         // I changed my data -- I know nothing about my own AABB anymore.
 689         ((dxTriMesh*)g)->gflags |= (GEOM_DIRTY|GEOM_AABB_BAD);
 690 }
 691
 692 dTriMeshDataID dGeomTriMeshGetData(dGeomID g)
 693 {
 694   dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
 695   return ((dxTriMesh*)g)->Data;
 696 }
 697
 698
 699
 700 void dGeomTriMeshEnableTC(dGeomID g, int geomClass, int enable)
 701 {
 702         dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
 703
 704         switch (geomClass)
 705         {
 706                 case dSphereClass:
 707                         ((dxTriMesh*)g)->doSphereTC = (1 == enable);
 708                         break;
 709                 case dBoxClass:
 710                         ((dxTriMesh*)g)->doBoxTC = (1 == enable);
 711                         break;
 712                 case dCapsuleClass:
 713                         ((dxTriMesh*)g)->doCapsuleTC = (1 == enable);
 714                         break;
 715         }
 716 }
 717
 718 int dGeomTriMeshIsTCEnabled(dGeomID g, int geomClass)
 719 {
 720         dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
 721
 722         switch (geomClass)
 723         {
 724                 case dSphereClass:
 725                         if (((dxTriMesh*)g)->doSphereTC)
 726                                 return 1;
 727                         break;
 728                 case dBoxClass:
 729                         if (((dxTriMesh*)g)->doBoxTC)
 730                                 return 1;
 731                         break;
 732                 case dCapsuleClass:
 733                         if (((dxTriMesh*)g)->doCapsuleTC)
 734                                 return 1;
 735                         break;
 736         }
 737         return 0;
 738 }
 739
 740 void dGeomTriMeshClearTCCache(dGeomID g){
 741     dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
 742
 743     dxTriMesh* Geom = (dxTriMesh*)g;
 744     Geom->ClearTCCache();
 745 }
 746
 747 /*
 748  * returns the TriMeshDataID
 749  */
 750 dTriMeshDataID
 751 dGeomTriMeshGetTriMeshDataID(dGeomID g)
 752 {
 753     dxTriMesh* Geom = (dxTriMesh*) g;
 754     return Geom->Data;
 755 }
 756
 757 // Getting data
 758 void dGeomTriMeshGetTriangle(dGeomID g, int Index, dVector3* v0, dVector3* v1, dVector3* v2){
 759     dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
 760
 761     dxTriMesh* Geom = (dxTriMesh*)g;
 762
 763     const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
 764     const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
 765
 766     dVector3 v[3];
 767         FetchTriangle(Geom, Index, Position, Rotation, v);
 768
 769     if (v0){
 770         (*v0)[0] = v[0][0];
 771         (*v0)[1] = v[0][1];
 772         (*v0)[2] = v[0][2];
 773         (*v0)[3] = v[0][3];
 774     }
 775     if (v1){
 776         (*v1)[0] = v[1][0];
 777         (*v1)[1] = v[1][1];
 778         (*v1)[2] = v[1][2];
 779         (*v1)[3] = v[1][3];
 780     }
 781     if (v2){
 782         (*v2)[0] = v[2][0];
 783         (*v2)[1] = v[2][1];
 784         (*v2)[2] = v[2][2];
 785         (*v2)[3] = v[2][3];
 786     }
 787 }
 788
 789 void dGeomTriMeshGetPoint(dGeomID g, int Index, dReal u, dReal v, dVector3 Out){
 790     dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
 791
 792     dxTriMesh* Geom = (dxTriMesh*)g;
 793
 794     const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
 795     const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
 796
 797     dVector3 dv[3];
 798     FetchTriangle(Geom, Index, Position, Rotation, dv);
 799
 800     GetPointFromBarycentric(dv, u, v, Out);
 801 }
 802
 803 int dGeomTriMeshGetTriangleCount (dGeomID g)
 804 {
 805     dxTriMesh* Geom = (dxTriMesh*)g;
 806     return FetchTriangleCount(Geom);
 807 }
 808
 809 void dGeomTriMeshDataUpdate(dTriMeshDataID g) {
 810     dUASSERT(g, "argument not trimesh data");
 811     g->UpdateData();
 812 }
 813
 814 #endif // dTRIMESH_OPCODE
 815 #endif // dTRIMESH_ENABLED