Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/ode/ode-0.9/ode/src/collision_trimesh_opcode.cpp @ 216

Last change on this file since 216 was 216, checked in by mathiask, 16 years ago

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