Open Dynamics Engine

collision_trimesh_internal.h

00001 /*************************************************************************
00002  *                                                                       *
00003  * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *
00004  * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
00005  *                                                                       *
00006  * This library is free software; you can redistribute it and/or         *
00007  * modify it under the terms of EITHER:                                  *
00008  *   (1) The GNU Lesser General Public License as published by the Free  *
00009  *       Software Foundation; either version 2.1 of the License, or (at  *
00010  *       your option) any later version. The text of the GNU Lesser      *
00011  *       General Public License is included with this library in the     *
00012  *       file LICENSE.TXT.                                               *
00013  *   (2) The BSD-style license that is included with this library in     *
00014  *       the file LICENSE-BSD.TXT.                                       *
00015  *                                                                       *
00016  * This library is distributed in the hope that it will be useful,       *
00017  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00018  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
00019  * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
00020  *                                                                       *
00021  *************************************************************************/
00022 
00023 // TriMesh code by Erwin de Vries.
00024 // Modified for FreeSOLID Compatibility by Rodrigo Hernandez
00025 // Trimesh caches separation by Oleh Derevenko
00026 
00027 
00028 #ifndef _ODE_COLLISION_TRIMESH_INTERNAL_H_
00029 #define _ODE_COLLISION_TRIMESH_INTERNAL_H_
00030 
00031 //****************************************************************************
00032 // dxTriMesh class
00033 
00034 
00035 #include "collision_kernel.h"
00036 #include "collision_trimesh_colliders.h"
00037 #include <ode/collision_trimesh.h>
00038 
00039 #if dTRIMESH_OPCODE
00040 #define BAN_OPCODE_AUTOLINK
00041 #include "Opcode.h"
00042 using namespace Opcode;
00043 #endif // dTRIMESH_OPCODE
00044 
00045 #if dTRIMESH_GIMPACT
00046 #include <GIMPACT/gimpact.h>
00047 #endif
00048 
00049 #if dTLS_ENABLED
00050 #include "odetls.h"
00051 #endif
00052 
00053 
00054 
00055 
00056 #if dTRIMESH_OPCODE
00057 #if !dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER
00058 
00059 // New trimesh collider hash table types
00060 enum
00061 {
00062    MAXCONTACT_X_NODE = 4,
00063    CONTACTS_HASHSIZE = 256,
00064 };
00065 
00066 struct CONTACT_KEY
00067 {
00068    dContactGeom * m_contact;
00069    unsigned int m_key;
00070 };
00071 
00072 struct CONTACT_KEY_HASH_NODE
00073 {
00074    CONTACT_KEY m_keyarray[MAXCONTACT_X_NODE];
00075    int m_keycount;
00076 };
00077 
00078 struct CONTACT_KEY_HASH_TABLE
00079 {
00080 public:
00081    CONTACT_KEY_HASH_NODE &operator[](unsigned int index) { return m_storage[index]; }
00082 
00083 private:
00084    CONTACT_KEY_HASH_NODE m_storage[CONTACTS_HASHSIZE];
00085 };
00086 
00087 #endif // !dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER
00088 #endif // dTRIMESH_OPCODE
00089 
00090 struct VertexUseCache
00091 {
00092 public:
00093    VertexUseCache(): m_VertexUseBits(NULL), m_VertexUseElements(0) {}
00094    ~VertexUseCache() { FreeVertexUSEDFlags();  }
00095 
00096    bool ResizeAndResetVertexUSEDFlags(unsigned VertexCount)
00097    {
00098       bool Result = false;
00099       size_t VertexNewElements = (VertexCount + 7) / 8;
00100       if (VertexNewElements <= m_VertexUseElements || ReallocVertexUSEDFlags(VertexNewElements)) {
00101          memset(m_VertexUseBits, 0, VertexNewElements);
00102          Result = true;
00103       }
00104       return Result;
00105    }
00106    
00107    bool GetVertexUSEDFlag(unsigned VertexIndex) const { return (m_VertexUseBits[VertexIndex / 8] & (1 << (VertexIndex % 8))) != 0; }
00108    void SetVertexUSEDFlag(unsigned VertexIndex) { m_VertexUseBits[VertexIndex / 8] |= (1 << (VertexIndex % 8)); }
00109 
00110 private:
00111    bool ReallocVertexUSEDFlags(size_t VertexNewElements)
00112    {
00113       bool Result = false;
00114       uint8 *VertexNewBits = (uint8 *)dRealloc(m_VertexUseBits, m_VertexUseElements * sizeof(m_VertexUseBits[0]), VertexNewElements * sizeof(m_VertexUseBits[0]));
00115       if (VertexNewBits) {
00116          m_VertexUseBits = VertexNewBits;
00117          m_VertexUseElements = VertexNewElements;
00118          Result = true;
00119       }
00120       return Result;
00121    }
00122 
00123    void FreeVertexUSEDFlags()
00124    {
00125       dFree(m_VertexUseBits, m_VertexUseElements * sizeof(m_VertexUseBits[0]));
00126       m_VertexUseBits = NULL;
00127       m_VertexUseElements = 0;
00128    }
00129 
00130 private:
00131    uint8 *m_VertexUseBits;
00132    size_t m_VertexUseElements;
00133 };
00134 
00135 
00136 struct TrimeshCollidersCache
00137 {
00138    TrimeshCollidersCache()
00139    {
00140 #if dTRIMESH_OPCODE
00141       InitOPCODECaches();
00142 #endif // dTRIMESH_OPCODE
00143    }
00144 
00145 #if dTRIMESH_OPCODE
00146 
00147    void InitOPCODECaches();
00148 
00149 
00150    // Collider caches
00151    BVTCache ColCache;
00152 
00153 #if !dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER
00154    CONTACT_KEY_HASH_TABLE _hashcontactset;
00155 #endif
00156 
00157    // Colliders
00158 /* -- not used -- also uncomment in InitOPCODECaches()
00159    PlanesCollider _PlanesCollider; -- not used 
00160 */
00161    SphereCollider _SphereCollider;
00162    OBBCollider _OBBCollider;
00163    RayCollider _RayCollider;
00164    AABBTreeCollider _AABBTreeCollider;
00165 /* -- not used -- also uncomment in InitOPCODECaches()
00166    LSSCollider _LSSCollider;
00167 */
00168    // Trimesh caches
00169    CollisionFaces Faces;
00170    SphereCache defaultSphereCache;
00171    OBBCache defaultBoxCache;
00172    LSSCache defaultCapsuleCache;
00173 
00174    // Trimesh-plane collision vertex use cache
00175    VertexUseCache VertexUses;
00176 
00177 #endif // dTRIMESH_OPCODE
00178 };
00179 
00180 #if dTLS_ENABLED
00181 
00182 inline TrimeshCollidersCache *GetTrimeshCollidersCache(unsigned uiTLSKind)
00183 {
00184    EODETLSKIND tkTLSKind = (EODETLSKIND)uiTLSKind;
00185    return COdeTls::GetTrimeshCollidersCache(tkTLSKind);
00186 }
00187 
00188 
00189 #else // dTLS_ENABLED
00190 
00191 inline TrimeshCollidersCache *GetTrimeshCollidersCache(unsigned uiTLSKind)
00192 {
00193    extern TrimeshCollidersCache g_ccTrimeshCollidersCache;
00194 
00195    return &g_ccTrimeshCollidersCache;
00196 }
00197 
00198 
00199 #endif // dTLS_ENABLED
00200 
00201 
00202 
00203 
00204 
00205 struct dxTriMeshData  : public dBase 
00206 {
00207     /* Array of flags for which edges and verts should be used on each triangle */
00208     enum UseFlags
00209     {
00210         kEdge0 = 0x1,
00211         kEdge1 = 0x2,
00212         kEdge2 = 0x4,
00213         kVert0 = 0x8,
00214         kVert1 = 0x10,
00215         kVert2 = 0x20,
00216 
00217         kUseAll = 0xFF,
00218     };
00219 
00220     /* Setup the UseFlags array */
00221     void Preprocess();
00222     /* For when app changes the vertices */
00223     void UpdateData();
00224     
00225 #if dTRIMESH_OPCODE
00226    Model BVTree;
00227    MeshInterface Mesh;
00228 
00229     dxTriMeshData();
00230     ~dxTriMeshData();
00231     
00232    void Build(const void* Vertices, int VertexStide, int VertexCount, 
00233       const void* Indices, int IndexCount, int TriStride, 
00234       const void* Normals, 
00235       bool Single);
00236 
00237    /* aabb in model space */
00238    dVector3 AABBCenter;
00239    dVector3 AABBExtents;
00240 
00241    // data for use in collision resolution
00242    const void* Normals;
00243    uint8* UseFlags;
00244 #endif  // dTRIMESH_OPCODE
00245 
00246 #if dTRIMESH_GIMPACT
00247    const char* m_Vertices;
00248    int m_VertexStride;
00249    int m_VertexCount;
00250    const char* m_Indices;
00251    int m_TriangleCount;
00252    int m_TriStride;
00253    bool m_single;
00254 
00255    dxTriMeshData()
00256    {
00257       m_Vertices=NULL;
00258       m_VertexStride = 12;
00259       m_VertexCount = 0;
00260       m_Indices = 0;
00261       m_TriangleCount = 0;
00262       m_TriStride = 12;
00263       m_single = true;
00264    }
00265 
00266     void Build(const void* Vertices, int VertexStride, int VertexCount,
00267           const void* Indices, int IndexCount, int TriStride,
00268           const void* Normals,
00269          bool Single)
00270    {
00271       dIASSERT(Vertices);
00272       dIASSERT(Indices);
00273       dIASSERT(VertexStride);
00274       dIASSERT(TriStride);
00275       dIASSERT(IndexCount);
00276       m_Vertices=(const char *)Vertices;
00277       m_VertexStride = VertexStride;
00278       m_VertexCount = VertexCount;
00279       m_Indices = (const char *)Indices;
00280       m_TriangleCount = IndexCount/3;
00281       m_TriStride = TriStride;
00282       m_single = Single;
00283    }
00284 
00285    inline void GetVertex(unsigned int i, dVector3 Out)
00286    {
00287       if(m_single)
00288       {
00289          const float * fverts = (const float * )(m_Vertices + m_VertexStride*i);
00290          Out[0] = fverts[0];
00291          Out[1] = fverts[1];
00292          Out[2] = fverts[2];
00293          Out[3] = 1.0f;
00294       }
00295       else
00296       {
00297          const double * dverts = (const double * )(m_Vertices + m_VertexStride*i);
00298          Out[0] = (float)dverts[0];
00299          Out[1] = (float)dverts[1];
00300          Out[2] = (float)dverts[2];
00301          Out[3] = 1.0f;
00302 
00303       }
00304    }
00305 
00306    inline void GetTriIndices(unsigned int itriangle, unsigned int triindices[3])
00307    {
00308       const unsigned int * ind = (const unsigned int * )(m_Indices + m_TriStride*itriangle);
00309       triindices[0] = ind[0];
00310       triindices[1] = ind[1];
00311       triindices[2] = ind[2];
00312    }
00313 #endif  // dTRIMESH_GIMPACT
00314 };
00315 
00316 struct dxTriMesh : public dxGeom{
00317    // Callbacks
00318    dTriCallback* Callback;
00319    dTriArrayCallback* ArrayCallback;
00320    dTriRayCallback* RayCallback;
00321     dTriTriMergeCallback* TriMergeCallback;
00322 
00323    // Data types
00324    dxTriMeshData* Data;
00325 
00326    bool doSphereTC;
00327    bool doBoxTC;
00328    bool doCapsuleTC;
00329 
00330    // Functions
00331    dxTriMesh(dSpaceID Space, dTriMeshDataID Data);
00332    ~dxTriMesh();
00333 
00334    void ClearTCCache();
00335 
00336    bool controlGeometry(int controlClass, int controlCode, void *dataValue, int *dataSize);
00337 
00338    int AABBTest(dxGeom* g, dReal aabb[6]);
00339    void computeAABB();
00340 
00341 #if dTRIMESH_OPCODE
00342 
00343    enum {
00344       MERGE_NORMALS__SPHERE_DEFAULT = DONT_MERGE_CONTACTS,
00345    };
00346    bool controlGeometry_SetMergeSphereContacts(int dataValue);
00347    bool controlGeometry_GetMergeSphereContacts(int &returnValue);
00348 
00349    // Contact merging option
00350    dxContactMergeOptions SphereContactsMergeOption;
00351    // Instance data for last transform.
00352     dMatrix4 last_trans;
00353 
00354    // Some constants
00355    // Temporal coherence
00356    struct SphereTC : public SphereCache{
00357       dxGeom* Geom;
00358    };
00359    dArray<SphereTC> SphereTCCache;
00360 
00361    struct BoxTC : public OBBCache{
00362       dxGeom* Geom;
00363    };
00364    dArray<BoxTC> BoxTCCache;
00365    
00366    struct CapsuleTC : public LSSCache{
00367       dxGeom* Geom;
00368    };
00369    dArray<CapsuleTC> CapsuleTCCache;
00370 #endif // dTRIMESH_OPCODE
00371 
00372 #if dTRIMESH_GIMPACT
00373     GIM_TRIMESH  m_collision_trimesh;
00374    GBUFFER_MANAGER_DATA m_buffer_managers[G_BUFFER_MANAGER__MAX];
00375 #endif  // dTRIMESH_GIMPACT
00376 };
00377 
00378 #if 0
00379 #include "collision_kernel.h"
00380 // Fetches a contact
00381 inline dContactGeom* SAFECONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){
00382    dIASSERT(Index >= 0 && Index < (Flags & NUMC_MASK));
00383    return ((dContactGeom*)(((char*)Contacts) + (Index * Stride)));
00384 }
00385 #endif
00386 
00387 #if dTRIMESH_OPCODE
00388 
00389 inline unsigned FetchTriangleCount(dxTriMesh* TriMesh)
00390 {
00391    return TriMesh->Data->Mesh.GetNbTriangles();
00392 }
00393 
00394 inline void FetchTriangle(dxTriMesh* TriMesh, int Index, const dVector3 Position, const dMatrix3 Rotation, dVector3 Out[3]){
00395    VertexPointers VP;
00396    ConversionArea VC;
00397    TriMesh->Data->Mesh.GetTriangle(VP, Index, VC);
00398    for (int i = 0; i < 3; i++){
00399       dVector3 v;
00400       v[0] = VP.Vertex[i]->x;
00401       v[1] = VP.Vertex[i]->y;
00402       v[2] = VP.Vertex[i]->z;
00403       v[3] = 0;
00404 
00405       dMultiply0_331(Out[i], Rotation, v);
00406       Out[i][0] += Position[0];
00407       Out[i][1] += Position[1];
00408       Out[i][2] += Position[2];
00409       Out[i][3] = 0;
00410    }
00411 }
00412 
00413 inline void FetchTransformedTriangle(dxTriMesh* TriMesh, int Index, dVector3 Out[3]){
00414    const dVector3& Position = *(const dVector3*)dGeomGetPosition(TriMesh);
00415    const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
00416    FetchTriangle(TriMesh, Index, Position, Rotation, Out);
00417 }
00418 
00419 inline Matrix4x4& MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, Matrix4x4& Out){
00420    Out.m[0][0] = (float) Rotation[0];
00421    Out.m[1][0] = (float) Rotation[1];
00422    Out.m[2][0] = (float) Rotation[2];
00423 
00424    Out.m[0][1] = (float) Rotation[4];
00425    Out.m[1][1] = (float) Rotation[5];
00426    Out.m[2][1] = (float) Rotation[6];
00427 
00428    Out.m[0][2] = (float) Rotation[8];
00429    Out.m[1][2] = (float) Rotation[9];
00430    Out.m[2][2] = (float) Rotation[10];
00431 
00432    Out.m[3][0] = (float) Position[0];
00433    Out.m[3][1] = (float) Position[1];
00434    Out.m[3][2] = (float) Position[2];
00435 
00436    Out.m[0][3] = 0.0f;
00437    Out.m[1][3] = 0.0f;
00438    Out.m[2][3] = 0.0f;
00439    Out.m[3][3] = 1.0f;
00440 
00441    return Out;
00442 }
00443 
00444 inline Matrix4x4& MakeMatrix(dxGeom* g, Matrix4x4& Out){
00445    const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
00446    const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
00447    return MakeMatrix(Position, Rotation, Out);
00448 }
00449 #endif // dTRIMESH_OPCODE
00450 
00451 #if dTRIMESH_GIMPACT
00452 
00453    #ifdef dDOUBLE
00454       // To use GIMPACT with doubles, we need to patch a couple of the GIMPACT functions to 
00455       // convert arguments to floats before sending them in
00456 
00457 
00459       #define dVECTOR3_VEC3F_COPY(b,a) {        \
00460          (b)[0] = (a)[0];              \
00461          (b)[1] = (a)[1];              \
00462          (b)[2] = (a)[2];              \
00463          (b)[3] = 0;                   \
00464       }
00465 
00466       inline void gim_trimesh_get_triangle_verticesODE(GIM_TRIMESH * trimesh, GUINT32 triangle_index, dVector3 v1, dVector3 v2, dVector3 v3) {   
00467          vec3f src1, src2, src3;
00468          gim_trimesh_get_triangle_vertices(trimesh, triangle_index, src1, src2, src3);
00469 
00470          dVECTOR3_VEC3F_COPY(v1, src1);
00471          dVECTOR3_VEC3F_COPY(v2, src2);
00472          dVECTOR3_VEC3F_COPY(v3, src3);
00473       }
00474 
00475       // Anything calling gim_trimesh_get_triangle_vertices from within ODE 
00476       // should be patched through to the dDOUBLE version above
00477 
00478       #define gim_trimesh_get_triangle_vertices gim_trimesh_get_triangle_verticesODE
00479 
00480       inline int gim_trimesh_ray_closest_collisionODE( GIM_TRIMESH *mesh, dVector3 origin, dVector3 dir, GREAL tmax, GIM_TRIANGLE_RAY_CONTACT_DATA *contact ) {
00481          vec3f dir_vec3f    = { dir[ 0 ],       dir[ 1 ],    dir[ 2 ] };
00482          vec3f origin_vec3f = { origin[ 0 ], origin[ 1 ], origin[ 2 ] };
00483 
00484          return gim_trimesh_ray_closest_collision( mesh, origin_vec3f, dir_vec3f, tmax, contact );
00485       }
00486 
00487       inline int gim_trimesh_ray_collisionODE( GIM_TRIMESH *mesh, dVector3 origin, dVector3 dir, GREAL tmax, GIM_TRIANGLE_RAY_CONTACT_DATA *contact ) {
00488          vec3f dir_vec3f    = { dir[ 0 ],       dir[ 1 ],    dir[ 2 ] };
00489          vec3f origin_vec3f = { origin[ 0 ], origin[ 1 ], origin[ 2 ] };
00490 
00491          return gim_trimesh_ray_collision( mesh, origin_vec3f, dir_vec3f, tmax, contact );
00492       }
00493 
00494       #define gim_trimesh_sphere_collisionODE( mesh, Position, Radius, contact ) {  \
00495          vec3f pos_vec3f = { Position[ 0 ], Position[ 1 ], Position[ 2 ] };         \
00496          gim_trimesh_sphere_collision( mesh, pos_vec3f, Radius, contact );       \
00497       }
00498 
00499       #define gim_trimesh_plane_collisionODE( mesh, plane, contact ) {        \
00500          vec4f plane_vec4f = { plane[ 0 ], plane[ 1 ], plane[ 2 ], plane[ 3 ] }; \
00501          gim_trimesh_plane_collision( mesh, plane_vec4f, contact );           \
00502       }
00503 
00504       #define GIM_AABB_COPY( src, dst ) {    \
00505          dst[ 0 ]= (src) -> minX;         \
00506          dst[ 1 ]= (src) -> maxX;         \
00507          dst[ 2 ]= (src) -> minY;         \
00508          dst[ 3 ]= (src) -> maxY;         \
00509          dst[ 4 ]= (src) -> minZ;         \
00510          dst[ 5 ]= (src) -> maxZ;         \
00511       }
00512 
00513    #else 
00514       // With single precision, we can pass native ODE vectors directly to GIMPACT
00515 
00516       #define gim_trimesh_ray_closest_collisionODE    gim_trimesh_ray_closest_collision
00517       #define gim_trimesh_ray_collisionODE         gim_trimesh_ray_collision
00518       #define gim_trimesh_sphere_collisionODE      gim_trimesh_sphere_collision
00519       #define gim_trimesh_plane_collisionODE          gim_trimesh_plane_collision
00520 
00521       #define GIM_AABB_COPY( src, dst )   memcpy( dst, src, 6 * sizeof( GREAL ) )
00522 
00523    #endif // dDouble
00524 
00525 inline unsigned FetchTriangleCount(dxTriMesh* TriMesh)
00526 {
00527    return gim_trimesh_get_triangle_count(&TriMesh->m_collision_trimesh);
00528 }
00529 
00530 inline void FetchTransformedTriangle(dxTriMesh* TriMesh, int Index, dVector3 Out[3]){
00531    gim_trimesh_locks_work_data(&TriMesh->m_collision_trimesh); 
00532    gim_trimesh_get_triangle_vertices(&TriMesh->m_collision_trimesh, (GUINT32)Index, Out[0], Out[1], Out[2]);
00533    gim_trimesh_unlocks_work_data(&TriMesh->m_collision_trimesh);
00534 }
00535 
00536 inline void MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, mat4f m)
00537 {
00538    m[0][0] = (float) Rotation[0];
00539    m[0][1] = (float) Rotation[1];
00540    m[0][2] = (float) Rotation[2];
00541 
00542    m[1][0] = (float) Rotation[4];
00543    m[1][1] = (float) Rotation[5];
00544    m[1][2] = (float) Rotation[6];
00545 
00546    m[2][0] = (float) Rotation[8];
00547    m[2][1] = (float) Rotation[9];
00548    m[2][2] = (float) Rotation[10];
00549 
00550    m[0][3] = (float) Position[0];
00551    m[1][3] = (float) Position[1];
00552    m[2][3] = (float) Position[2];
00553 
00554 }
00555 
00556 inline void MakeMatrix(dxGeom* g, mat4f Out){
00557    const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
00558    const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
00559    MakeMatrix(Position, Rotation, Out);
00560 }
00561 #endif // dTRIMESH_GIMPACT
00562 
00563 // Outputs a matrix to 3 vectors
00564 inline void Decompose(const dMatrix3 Matrix, dVector3 Right, dVector3 Up, dVector3 Direction){
00565    Right[0] = Matrix[0 * 4 + 0];
00566    Right[1] = Matrix[1 * 4 + 0];
00567    Right[2] = Matrix[2 * 4 + 0];
00568    Right[3] = REAL(0.0);
00569    Up[0] = Matrix[0 * 4 + 1];
00570    Up[1] = Matrix[1 * 4 + 1];
00571    Up[2] = Matrix[2 * 4 + 1];
00572    Up[3] = REAL(0.0);
00573    Direction[0] = Matrix[0 * 4 + 2];
00574    Direction[1] = Matrix[1 * 4 + 2];
00575    Direction[2] = Matrix[2 * 4 + 2];
00576    Direction[3] = REAL(0.0);
00577 }
00578 
00579 // Outputs a matrix to 3 vectors
00580 inline void Decompose(const dMatrix3 Matrix, dVector3 Vectors[3]){
00581    Decompose(Matrix, Vectors[0], Vectors[1], Vectors[2]);
00582 }
00583 
00584 // Finds barycentric
00585 inline void GetPointFromBarycentric(const dVector3 dv[3], dReal u, dReal v, dVector3 Out){
00586    dReal w = REAL(1.0) - u - v;
00587 
00588    Out[0] = (dv[0][0] * w) + (dv[1][0] * u) + (dv[2][0] * v);
00589    Out[1] = (dv[0][1] * w) + (dv[1][1] * u) + (dv[2][1] * v);
00590    Out[2] = (dv[0][2] * w) + (dv[1][2] * u) + (dv[2][2] * v);
00591    Out[3] = (dv[0][3] * w) + (dv[1][3] * u) + (dv[2][3] * v);
00592 }
00593 
00594 // Performs a callback
00595 inline bool Callback(dxTriMesh* TriMesh, dxGeom* Object, int TriIndex){
00596    if (TriMesh->Callback != NULL){
00597       return (TriMesh->Callback(TriMesh, Object, TriIndex)!=0);
00598    }
00599    else return true;
00600 }
00601 
00602 // Some utilities
00603 template<class T> const T& dcMAX(const T& x, const T& y){
00604    return x > y ? x : y;
00605 }
00606 
00607 template<class T> const T& dcMIN(const T& x, const T& y){
00608    return x < y ? x : y;
00609 }
00610 
00611 dReal SqrDistancePointTri( const dVector3 p, const dVector3 triOrigin, 
00612                            const dVector3 triEdge1, const dVector3 triEdge2,
00613                            dReal* pfSParam = 0, dReal* pfTParam = 0 );
00614 
00615 dReal SqrDistanceSegments( const dVector3 seg1Origin, const dVector3 seg1Direction, 
00616                            const dVector3 seg2Origin, const dVector3 seg2Direction,
00617                            dReal* pfSegP0 = 0, dReal* pfSegP1 = 0 );
00618 
00619 dReal SqrDistanceSegTri( const dVector3 segOrigin, const dVector3 segEnd, 
00620                          const dVector3 triOrigin, 
00621                          const dVector3 triEdge1, const dVector3 triEdge2,
00622                          dReal* t = 0, dReal* u = 0, dReal* v = 0 );
00623 
00624 inline
00625 void Vector3Subtract( const dVector3 left, const dVector3 right, dVector3 result )
00626 {
00627     result[0] = left[0] - right[0];
00628     result[1] = left[1] - right[1];
00629     result[2] = left[2] - right[2];
00630     result[3] = REAL(0.0);
00631 }
00632 
00633 inline
00634 void Vector3Add( const dVector3 left, const dVector3 right, dVector3 result )
00635 {
00636     result[0] = left[0] + right[0];
00637     result[1] = left[1] + right[1];
00638     result[2] = left[2] + right[2];
00639     result[3] = REAL(0.0);
00640 }
00641 
00642 inline
00643 void Vector3Negate( const dVector3 in, dVector3 out )
00644 {
00645     out[0] = -in[0];
00646     out[1] = -in[1];
00647     out[2] = -in[2];
00648     out[3] = REAL(0.0);
00649 }
00650 
00651 inline
00652 void Vector3Copy( const dVector3 in, dVector3 out )
00653 {
00654     out[0] = in[0];
00655     out[1] = in[1];
00656     out[2] = in[2];
00657     out[3] = REAL(0.0);
00658 }
00659 
00660 inline
00661 void Vector3Multiply( const dVector3 in, dReal scalar, dVector3 out )
00662 {
00663     out[0] = in[0] * scalar;
00664     out[1] = in[1] * scalar;
00665     out[2] = in[2] * scalar;
00666     out[3] = REAL(0.0);
00667 }
00668 
00669 inline
00670 void TransformVector3( const dVector3 in, 
00671                        const dMatrix3 orientation, const dVector3 position, 
00672                        dVector3 out )
00673 {
00674     dMultiply0_331( out, orientation, in );
00675     out[0] += position[0];
00676     out[1] += position[1];
00677     out[2] += position[2];
00678 }
00679 
00680 //------------------------------------------------------------------------------
00699 inline
00700 bool IntersectCapsuleTri( const dVector3 segOrigin, const dVector3 segEnd, 
00701                           const dReal radius, const dVector3 triOrigin, 
00702                           const dVector3 triEdge0, const dVector3 triEdge1,
00703                           dReal* dist, dReal* t, dReal* u, dReal* v )
00704 {
00705     dReal sqrDist = SqrDistanceSegTri( segOrigin, segEnd, triOrigin, triEdge0, triEdge1, 
00706                                        t, u, v );
00707   
00708     if ( dist )
00709       *dist = sqrDist;
00710     
00711     return ( sqrDist <= (radius * radius) );
00712 }
00713 
00714 
00715 #endif   //_ODE_COLLISION_TRIMESH_INTERNAL_H_