Open Dynamics Engine
collision_trimesh_internal.h
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 // Modified for FreeSOLID Compatibility by Rodrigo Hernandez
25 // Trimesh caches separation by Oleh Derevenko
26 
27 
28 #ifndef _ODE_COLLISION_TRIMESH_INTERNAL_H_
29 #define _ODE_COLLISION_TRIMESH_INTERNAL_H_
30 
31 //****************************************************************************
32 // dxTriMesh class
33 
34 
35 #include "collision_kernel.h"
36 #include "collision_trimesh_colliders.h"
37 #include <ode/collision_trimesh.h>
38 
39 #if dTRIMESH_OPCODE
40 #define BAN_OPCODE_AUTOLINK
41 #include "Opcode.h"
42 using namespace Opcode;
43 #endif // dTRIMESH_OPCODE
44 
45 #if dTRIMESH_GIMPACT
46 #include <GIMPACT/gimpact.h>
47 #endif
48 
49 #if dTLS_ENABLED
50 #include "odetls.h"
51 #endif
52 
53 
54 
55 
56 #if dTRIMESH_OPCODE
57 #if !dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER
58 
59 // New trimesh collider hash table types
60 enum
61 {
62  MAXCONTACT_X_NODE = 4,
63  CONTACTS_HASHSIZE = 256
64 };
65 
66 struct CONTACT_KEY
67 {
68  dContactGeom * m_contact;
69  unsigned int m_key;
70 };
71 
72 struct CONTACT_KEY_HASH_NODE
73 {
74  CONTACT_KEY m_keyarray[MAXCONTACT_X_NODE];
75  int m_keycount;
76 };
77 
78 struct CONTACT_KEY_HASH_TABLE
79 {
80 public:
81  CONTACT_KEY_HASH_NODE &operator[](unsigned int index) { return m_storage[index]; }
82 
83 private:
84  CONTACT_KEY_HASH_NODE m_storage[CONTACTS_HASHSIZE];
85 };
86 
87 #endif // !dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER
88 #endif // dTRIMESH_OPCODE
89 
91 {
92 public:
93  VertexUseCache(): m_VertexUseBits(NULL), m_VertexUseElements(0) {}
94  ~VertexUseCache() { FreeVertexUSEDFlags(); }
95 
96  bool ResizeAndResetVertexUSEDFlags(unsigned VertexCount)
97  {
98  bool Result = false;
99  size_t VertexNewElements = (VertexCount + 7) / 8;
100  if (VertexNewElements <= m_VertexUseElements || ReallocVertexUSEDFlags(VertexNewElements)) {
101  memset(m_VertexUseBits, 0, VertexNewElements);
102  Result = true;
103  }
104  return Result;
105  }
106 
107  bool GetVertexUSEDFlag(unsigned VertexIndex) const { return (m_VertexUseBits[VertexIndex / 8] & (1 << (VertexIndex % 8))) != 0; }
108  void SetVertexUSEDFlag(unsigned VertexIndex) { m_VertexUseBits[VertexIndex / 8] |= (1 << (VertexIndex % 8)); }
109 
110 private:
111  bool ReallocVertexUSEDFlags(size_t VertexNewElements)
112  {
113  bool Result = false;
114  uint8 *VertexNewBits = (uint8 *)dRealloc(m_VertexUseBits, m_VertexUseElements * sizeof(m_VertexUseBits[0]), VertexNewElements * sizeof(m_VertexUseBits[0]));
115  if (VertexNewBits) {
116  m_VertexUseBits = VertexNewBits;
117  m_VertexUseElements = VertexNewElements;
118  Result = true;
119  }
120  return Result;
121  }
122 
123  void FreeVertexUSEDFlags()
124  {
125  dFree(m_VertexUseBits, m_VertexUseElements * sizeof(m_VertexUseBits[0]));
126  m_VertexUseBits = NULL;
127  m_VertexUseElements = 0;
128  }
129 
130 private:
131  uint8 *m_VertexUseBits;
132  size_t m_VertexUseElements;
133 };
134 
135 
137 {
139  {
140 #if dTRIMESH_OPCODE
141  InitOPCODECaches();
142 #endif // dTRIMESH_OPCODE
143  }
144 
145 #if dTRIMESH_OPCODE
146 
147  void InitOPCODECaches();
148 
149 
150  // Collider caches
151  BVTCache ColCache;
152 
153 #if !dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER
154  CONTACT_KEY_HASH_TABLE _hashcontactset;
155 #endif
156 
157  // Colliders
158  /* -- not used -- also uncomment in InitOPCODECaches()
159  PlanesCollider _PlanesCollider; -- not used
160  */
161  SphereCollider _SphereCollider;
162  OBBCollider _OBBCollider;
163  RayCollider _RayCollider;
164  AABBTreeCollider _AABBTreeCollider;
165  /* -- not used -- also uncomment in InitOPCODECaches()
166  LSSCollider _LSSCollider;
167  */
168  // Trimesh caches
169  CollisionFaces Faces;
170  SphereCache defaultSphereCache;
171  OBBCache defaultBoxCache;
172  LSSCache defaultCapsuleCache;
173 
174  // Trimesh-plane collision vertex use cache
175  VertexUseCache VertexUses;
176 
177 #endif // dTRIMESH_OPCODE
178 };
179 
180 #if dTLS_ENABLED
181 
182 inline TrimeshCollidersCache *GetTrimeshCollidersCache(unsigned uiTLSKind)
183 {
184  EODETLSKIND tkTLSKind = (EODETLSKIND)uiTLSKind;
185  return COdeTls::GetTrimeshCollidersCache(tkTLSKind);
186 }
187 
188 
189 #else // dTLS_ENABLED
190 
191 inline TrimeshCollidersCache *GetTrimeshCollidersCache(unsigned uiTLSKind)
192 {
193  extern TrimeshCollidersCache g_ccTrimeshCollidersCache;
194 
195  return &g_ccTrimeshCollidersCache;
196 }
197 
198 
199 #endif // dTLS_ENABLED
200 
201 
202 
203 
204 
205 struct dxTriMeshData : public dBase
206 {
207  /* Array of flags for which edges and verts should be used on each triangle */
208  enum UseFlags
209  {
210  kEdge0 = 0x1,
211  kEdge1 = 0x2,
212  kEdge2 = 0x4,
213  kVert0 = 0x8,
214  kVert1 = 0x10,
215  kVert2 = 0x20,
216 
217  kUseAll = 0xFF
218  };
219 
220  /* Setup the UseFlags array */
221  void Preprocess();
222  /* For when app changes the vertices */
223  void UpdateData();
224 
225 #if dTRIMESH_OPCODE
226  Model BVTree;
227  MeshInterface Mesh;
228 
229  dxTriMeshData();
230  ~dxTriMeshData();
231 
232  void Build(const void* Vertices, int VertexStide, int VertexCount,
233  const void* Indices, int IndexCount, int TriStride,
234  const void* Normals,
235  bool Single);
236 
237  /* aabb in model space */
238  dVector3 AABBCenter;
239  dVector3 AABBExtents;
240 
241  // data for use in collision resolution
242  const void* Normals;
243  uint8* UseFlags;
244 #endif // dTRIMESH_OPCODE
245 
246 #if dTRIMESH_GIMPACT
247  const char* m_Vertices;
248  int m_VertexStride;
249  int m_VertexCount;
250  const char* m_Indices;
251  int m_TriangleCount;
252  int m_TriStride;
253  bool m_single;
254 
255  dxTriMeshData()
256  {
257  m_Vertices=NULL;
258  m_VertexStride = 12;
259  m_VertexCount = 0;
260  m_Indices = 0;
261  m_TriangleCount = 0;
262  m_TriStride = 12;
263  m_single = true;
264  }
265 
266  void Build(const void* Vertices, int VertexStride, int VertexCount,
267  const void* Indices, int IndexCount, int TriStride,
268  const void* Normals,
269  bool Single)
270  {
271  dIASSERT(Vertices);
272  dIASSERT(Indices);
273  dIASSERT(VertexStride);
274  dIASSERT(TriStride);
275  dIASSERT(IndexCount);
276  m_Vertices=(const char *)Vertices;
277  m_VertexStride = VertexStride;
278  m_VertexCount = VertexCount;
279  m_Indices = (const char *)Indices;
280  m_TriangleCount = IndexCount/3;
281  m_TriStride = TriStride;
282  m_single = Single;
283  }
284 
285  inline void GetVertex(unsigned int i, dVector3 Out)
286  {
287  if(m_single)
288  {
289  const float * fverts = (const float * )(m_Vertices + m_VertexStride*i);
290  Out[0] = fverts[0];
291  Out[1] = fverts[1];
292  Out[2] = fverts[2];
293  Out[3] = 1.0f;
294  }
295  else
296  {
297  const double * dverts = (const double * )(m_Vertices + m_VertexStride*i);
298  Out[0] = (float)dverts[0];
299  Out[1] = (float)dverts[1];
300  Out[2] = (float)dverts[2];
301  Out[3] = 1.0f;
302 
303  }
304  }
305 
306  inline void GetTriIndices(unsigned int itriangle, unsigned int triindices[3])
307  {
308  const unsigned int * ind = (const unsigned int * )(m_Indices + m_TriStride*itriangle);
309  triindices[0] = ind[0];
310  triindices[1] = ind[1];
311  triindices[2] = ind[2];
312  }
313 #endif // dTRIMESH_GIMPACT
314 };
315 
316 struct dxTriMesh : public dxGeom{
317  // Callbacks
318  dTriCallback* Callback;
319  dTriArrayCallback* ArrayCallback;
320  dTriRayCallback* RayCallback;
321  dTriTriMergeCallback* TriMergeCallback;
322 
323  // Data types
324  dxTriMeshData* Data;
325 
326  bool doSphereTC;
327  bool doBoxTC;
328  bool doCapsuleTC;
329 
330  // Functions
331  dxTriMesh(dSpaceID Space, dTriMeshDataID Data);
332  ~dxTriMesh();
333 
334  void ClearTCCache();
335 
336  bool controlGeometry(int controlClass, int controlCode, void *dataValue, int *dataSize);
337 
338  int AABBTest(dxGeom* g, dReal aabb[6]);
339  void computeAABB();
340 
341 #if dTRIMESH_OPCODE
342 
343  enum {
344  MERGE_NORMALS__SPHERE_DEFAULT = DONT_MERGE_CONTACTS
345  };
346  bool controlGeometry_SetMergeSphereContacts(int dataValue);
347  bool controlGeometry_GetMergeSphereContacts(int &returnValue);
348 
349  // Contact merging option
350  dxContactMergeOptions SphereContactsMergeOption;
351  // Instance data for last transform.
352  dMatrix4 last_trans;
353 
354  // Some constants
355  // Temporal coherence
356  struct SphereTC : public SphereCache{
357  dxGeom* Geom;
358  };
359  dArray<SphereTC> SphereTCCache;
360 
361  struct BoxTC : public OBBCache{
362  dxGeom* Geom;
363  };
364  dArray<BoxTC> BoxTCCache;
365 
366  struct CapsuleTC : public LSSCache{
367  dxGeom* Geom;
368  };
369  dArray<CapsuleTC> CapsuleTCCache;
370 #endif // dTRIMESH_OPCODE
371 
372 #if dTRIMESH_GIMPACT
373  GIM_TRIMESH m_collision_trimesh;
374  GBUFFER_MANAGER_DATA m_buffer_managers[G_BUFFER_MANAGER__MAX];
375 #endif // dTRIMESH_GIMPACT
376 };
377 
378 #if 0
379 #include "collision_kernel.h"
380 // Fetches a contact
381 inline dContactGeom* SAFECONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){
382  dIASSERT(Index >= 0 && Index < (Flags & NUMC_MASK));
383  return ((dContactGeom*)(((char*)Contacts) + (Index * Stride)));
384 }
385 #endif
386 
387 #if dTRIMESH_OPCODE
388 
389 inline unsigned FetchTriangleCount(dxTriMesh* TriMesh)
390 {
391  return TriMesh->Data->Mesh.GetNbTriangles();
392 }
393 
394 inline void FetchTriangle(dxTriMesh* TriMesh, int Index, const dVector3 Position, const dMatrix3 Rotation, dVector3 Out[3]){
395  VertexPointers VP;
396  ConversionArea VC;
397  TriMesh->Data->Mesh.GetTriangle(VP, Index, VC);
398  for (int i = 0; i < 3; i++){
399  dVector3 v;
400  v[0] = VP.Vertex[i]->x;
401  v[1] = VP.Vertex[i]->y;
402  v[2] = VP.Vertex[i]->z;
403  v[3] = 0;
404 
405  dMultiply0_331(Out[i], Rotation, v);
406  Out[i][0] += Position[0];
407  Out[i][1] += Position[1];
408  Out[i][2] += Position[2];
409  Out[i][3] = 0;
410  }
411 }
412 
413 inline void FetchTransformedTriangle(dxTriMesh* TriMesh, int Index, dVector3 Out[3]){
414  const dVector3& Position = *(const dVector3*)dGeomGetPosition(TriMesh);
415  const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
416  FetchTriangle(TriMesh, Index, Position, Rotation, Out);
417 }
418 
419 inline Matrix4x4& MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, Matrix4x4& Out){
420  Out.m[0][0] = (float) Rotation[0];
421  Out.m[1][0] = (float) Rotation[1];
422  Out.m[2][0] = (float) Rotation[2];
423 
424  Out.m[0][1] = (float) Rotation[4];
425  Out.m[1][1] = (float) Rotation[5];
426  Out.m[2][1] = (float) Rotation[6];
427 
428  Out.m[0][2] = (float) Rotation[8];
429  Out.m[1][2] = (float) Rotation[9];
430  Out.m[2][2] = (float) Rotation[10];
431 
432  Out.m[3][0] = (float) Position[0];
433  Out.m[3][1] = (float) Position[1];
434  Out.m[3][2] = (float) Position[2];
435 
436  Out.m[0][3] = 0.0f;
437  Out.m[1][3] = 0.0f;
438  Out.m[2][3] = 0.0f;
439  Out.m[3][3] = 1.0f;
440 
441  return Out;
442 }
443 
444 inline Matrix4x4& MakeMatrix(dxGeom* g, Matrix4x4& Out){
445  const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
446  const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
447  return MakeMatrix(Position, Rotation, Out);
448 }
449 #endif // dTRIMESH_OPCODE
450 
451 #if dTRIMESH_GIMPACT
452 
453 #ifdef dDOUBLE
454 // To use GIMPACT with doubles, we need to patch a couple of the GIMPACT functions to
455 // convert arguments to floats before sending them in
456 
457 
459 #define dVECTOR3_VEC3F_COPY(b,a) { \
460  (b)[0] = (a)[0]; \
461  (b)[1] = (a)[1]; \
462  (b)[2] = (a)[2]; \
463  (b)[3] = 0; \
464 }
465 
466 inline void gim_trimesh_get_triangle_verticesODE(GIM_TRIMESH * trimesh, GUINT32 triangle_index, dVector3 v1, dVector3 v2, dVector3 v3) {
467  vec3f src1, src2, src3;
468  gim_trimesh_get_triangle_vertices(trimesh, triangle_index, src1, src2, src3);
469 
470  dVECTOR3_VEC3F_COPY(v1, src1);
471  dVECTOR3_VEC3F_COPY(v2, src2);
472  dVECTOR3_VEC3F_COPY(v3, src3);
473 }
474 
475 // Anything calling gim_trimesh_get_triangle_vertices from within ODE
476 // should be patched through to the dDOUBLE version above
477 
478 #define gim_trimesh_get_triangle_vertices gim_trimesh_get_triangle_verticesODE
479 
480 inline int gim_trimesh_ray_closest_collisionODE( GIM_TRIMESH *mesh, dVector3 origin, dVector3 dir, GREAL tmax, GIM_TRIANGLE_RAY_CONTACT_DATA *contact ) {
481  vec3f dir_vec3f = { dir[ 0 ], dir[ 1 ], dir[ 2 ] };
482  vec3f origin_vec3f = { origin[ 0 ], origin[ 1 ], origin[ 2 ] };
483 
484  return gim_trimesh_ray_closest_collision( mesh, origin_vec3f, dir_vec3f, tmax, contact );
485 }
486 
487 inline int gim_trimesh_ray_collisionODE( GIM_TRIMESH *mesh, dVector3 origin, dVector3 dir, GREAL tmax, GIM_TRIANGLE_RAY_CONTACT_DATA *contact ) {
488  vec3f dir_vec3f = { dir[ 0 ], dir[ 1 ], dir[ 2 ] };
489  vec3f origin_vec3f = { origin[ 0 ], origin[ 1 ], origin[ 2 ] };
490 
491  return gim_trimesh_ray_collision( mesh, origin_vec3f, dir_vec3f, tmax, contact );
492 }
493 
494 #define gim_trimesh_sphere_collisionODE( mesh, Position, Radius, contact ) { \
495  vec3f pos_vec3f = { Position[ 0 ], Position[ 1 ], Position[ 2 ] }; \
496  gim_trimesh_sphere_collision( mesh, pos_vec3f, Radius, contact ); \
497 }
498 
499 #define gim_trimesh_plane_collisionODE( mesh, plane, contact ) { \
500  vec4f plane_vec4f = { plane[ 0 ], plane[ 1 ], plane[ 2 ], plane[ 3 ] }; \
501  gim_trimesh_plane_collision( mesh, plane_vec4f, contact ); \
502 }
503 
504 #define GIM_AABB_COPY( src, dst ) { \
505  dst[ 0 ]= (src) -> minX; \
506  dst[ 1 ]= (src) -> maxX; \
507  dst[ 2 ]= (src) -> minY; \
508  dst[ 3 ]= (src) -> maxY; \
509  dst[ 4 ]= (src) -> minZ; \
510  dst[ 5 ]= (src) -> maxZ; \
511 }
512 
513 #else
514 // With single precision, we can pass native ODE vectors directly to GIMPACT
515 
516 #define gim_trimesh_ray_closest_collisionODE gim_trimesh_ray_closest_collision
517 #define gim_trimesh_ray_collisionODE gim_trimesh_ray_collision
518 #define gim_trimesh_sphere_collisionODE gim_trimesh_sphere_collision
519 #define gim_trimesh_plane_collisionODE gim_trimesh_plane_collision
520 
521 #define GIM_AABB_COPY( src, dst ) memcpy( dst, src, 6 * sizeof( GREAL ) )
522 
523 #endif // dDouble
524 
525 inline unsigned FetchTriangleCount(dxTriMesh* TriMesh)
526 {
527  return gim_trimesh_get_triangle_count(&TriMesh->m_collision_trimesh);
528 }
529 
530 inline void FetchTransformedTriangle(dxTriMesh* TriMesh, int Index, dVector3 Out[3]){
531  gim_trimesh_locks_work_data(&TriMesh->m_collision_trimesh);
532  gim_trimesh_get_triangle_vertices(&TriMesh->m_collision_trimesh, (GUINT32)Index, Out[0], Out[1], Out[2]);
533  gim_trimesh_unlocks_work_data(&TriMesh->m_collision_trimesh);
534 }
535 
536 inline void MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, mat4f m)
537 {
538  m[0][0] = (float) Rotation[0];
539  m[0][1] = (float) Rotation[1];
540  m[0][2] = (float) Rotation[2];
541 
542  m[1][0] = (float) Rotation[4];
543  m[1][1] = (float) Rotation[5];
544  m[1][2] = (float) Rotation[6];
545 
546  m[2][0] = (float) Rotation[8];
547  m[2][1] = (float) Rotation[9];
548  m[2][2] = (float) Rotation[10];
549 
550  m[0][3] = (float) Position[0];
551  m[1][3] = (float) Position[1];
552  m[2][3] = (float) Position[2];
553 
554 }
555 
556 inline void MakeMatrix(dxGeom* g, mat4f Out){
557  const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
558  const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
559  MakeMatrix(Position, Rotation, Out);
560 }
561 #endif // dTRIMESH_GIMPACT
562 
563 // Outputs a matrix to 3 vectors
564 inline void Decompose(const dMatrix3 Matrix, dVector3 Right, dVector3 Up, dVector3 Direction){
565  Right[0] = Matrix[0 * 4 + 0];
566  Right[1] = Matrix[1 * 4 + 0];
567  Right[2] = Matrix[2 * 4 + 0];
568  Right[3] = REAL(0.0);
569  Up[0] = Matrix[0 * 4 + 1];
570  Up[1] = Matrix[1 * 4 + 1];
571  Up[2] = Matrix[2 * 4 + 1];
572  Up[3] = REAL(0.0);
573  Direction[0] = Matrix[0 * 4 + 2];
574  Direction[1] = Matrix[1 * 4 + 2];
575  Direction[2] = Matrix[2 * 4 + 2];
576  Direction[3] = REAL(0.0);
577 }
578 
579 // Outputs a matrix to 3 vectors
580 inline void Decompose(const dMatrix3 Matrix, dVector3 Vectors[3]){
581  Decompose(Matrix, Vectors[0], Vectors[1], Vectors[2]);
582 }
583 
584 // Finds barycentric
585 inline void GetPointFromBarycentric(const dVector3 dv[3], dReal u, dReal v, dVector3 Out){
586  dReal w = REAL(1.0) - u - v;
587 
588  Out[0] = (dv[0][0] * w) + (dv[1][0] * u) + (dv[2][0] * v);
589  Out[1] = (dv[0][1] * w) + (dv[1][1] * u) + (dv[2][1] * v);
590  Out[2] = (dv[0][2] * w) + (dv[1][2] * u) + (dv[2][2] * v);
591  Out[3] = (dv[0][3] * w) + (dv[1][3] * u) + (dv[2][3] * v);
592 }
593 
594 // Performs a callback
595 inline bool Callback(dxTriMesh* TriMesh, dxGeom* Object, int TriIndex){
596  if (TriMesh->Callback != NULL){
597  return (TriMesh->Callback(TriMesh, Object, TriIndex)!=0);
598  }
599  else return true;
600 }
601 
602 // Some utilities
603 template<class T> const T& dcMAX(const T& x, const T& y){
604  return x > y ? x : y;
605 }
606 
607 template<class T> const T& dcMIN(const T& x, const T& y){
608  return x < y ? x : y;
609 }
610 
611 dReal SqrDistancePointTri( const dVector3 p, const dVector3 triOrigin,
612  const dVector3 triEdge1, const dVector3 triEdge2,
613  dReal* pfSParam = 0, dReal* pfTParam = 0 );
614 
615 dReal SqrDistanceSegments( const dVector3 seg1Origin, const dVector3 seg1Direction,
616  const dVector3 seg2Origin, const dVector3 seg2Direction,
617  dReal* pfSegP0 = 0, dReal* pfSegP1 = 0 );
618 
619 dReal SqrDistanceSegTri( const dVector3 segOrigin, const dVector3 segEnd,
620  const dVector3 triOrigin,
621  const dVector3 triEdge1, const dVector3 triEdge2,
622  dReal* t = 0, dReal* u = 0, dReal* v = 0 );
623 
624 inline
625 void Vector3Subtract( const dVector3 left, const dVector3 right, dVector3 result )
626 {
627  result[0] = left[0] - right[0];
628  result[1] = left[1] - right[1];
629  result[2] = left[2] - right[2];
630  result[3] = REAL(0.0);
631 }
632 
633 inline
634 void Vector3Add( const dVector3 left, const dVector3 right, dVector3 result )
635 {
636  result[0] = left[0] + right[0];
637  result[1] = left[1] + right[1];
638  result[2] = left[2] + right[2];
639  result[3] = REAL(0.0);
640 }
641 
642 inline
643 void Vector3Negate( const dVector3 in, dVector3 out )
644 {
645  out[0] = -in[0];
646  out[1] = -in[1];
647  out[2] = -in[2];
648  out[3] = REAL(0.0);
649 }
650 
651 inline
652 void Vector3Copy( const dVector3 in, dVector3 out )
653 {
654  out[0] = in[0];
655  out[1] = in[1];
656  out[2] = in[2];
657  out[3] = REAL(0.0);
658 }
659 
660 inline
661 void Vector3Multiply( const dVector3 in, dReal scalar, dVector3 out )
662 {
663  out[0] = in[0] * scalar;
664  out[1] = in[1] * scalar;
665  out[2] = in[2] * scalar;
666  out[3] = REAL(0.0);
667 }
668 
669 inline
670 void TransformVector3( const dVector3 in,
671  const dMatrix3 orientation, const dVector3 position,
672  dVector3 out )
673 {
674  dMultiply0_331( out, orientation, in );
675  out[0] += position[0];
676  out[1] += position[1];
677  out[2] += position[2];
678 }
679 
680 //------------------------------------------------------------------------------
699 inline
700 bool IntersectCapsuleTri( const dVector3 segOrigin, const dVector3 segEnd,
701  const dReal radius, const dVector3 triOrigin,
702  const dVector3 triEdge0, const dVector3 triEdge1,
703  dReal* dist, dReal* t, dReal* u, dReal* v )
704 {
705  dReal sqrDist = SqrDistanceSegTri( segOrigin, segEnd, triOrigin, triEdge0, triEdge1,
706  t, u, v );
707 
708  if ( dist )
709  *dist = sqrDist;
710 
711  return ( sqrDist <= (radius * radius) );
712 }
713 
714 
715 #endif //_ODE_COLLISION_TRIMESH_INTERNAL_H_
Definition: ode/src/objects.h:57
Describe the contact point between two geoms.
Definition: contact.h:88
ODE_API const dReal * dGeomGetRotation(dGeomID geom)
Get the rotation matrix of a placeable geom.
Definition: collision_trimesh_internal.h:316
Definition: array.h:84
Definition: collision_kernel.h:96
Definition: collision_kernel.h:202
Definition: collision_trimesh_internal.h:205
ODE_API const dReal * dGeomGetPosition(dGeomID geom)
Get the position vector of a placeable geom.
Definition: collision_trimesh_internal.h:136
Definition: collision_trimesh_internal.h:90