Open Dynamics Engine
|
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_