From 0fc46fc9590912bf6925c899edd02d7a2cdf5f79 Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 04:28:53 +0000 Subject: adding ode source to /libraries --- .../ode/src/collision_trimesh_internal.h" | 495 +++++++++++++++++++++ 1 file changed, 495 insertions(+) create mode 100755 "libraries/ode-0.9\\/ode/src/collision_trimesh_internal.h" (limited to 'libraries/ode-0.9\/ode/src/collision_trimesh_internal.h') diff --git "a/libraries/ode-0.9\\/ode/src/collision_trimesh_internal.h" "b/libraries/ode-0.9\\/ode/src/collision_trimesh_internal.h" new file mode 100755 index 0000000..bf474a2 --- /dev/null +++ "b/libraries/ode-0.9\\/ode/src/collision_trimesh_internal.h" @@ -0,0 +1,495 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +// TriMesh code by Erwin de Vries. +// Modified for FreeSOLID Compatibility by Rodrigo Hernandez + +#ifndef _ODE_COLLISION_TRIMESH_INTERNAL_H_ +#define _ODE_COLLISION_TRIMESH_INTERNAL_H_ + +int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); +int dCollideTrimeshPlane(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); + +int dCollideSTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); +int dCollideBTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); +int dCollideRTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); +int dCollideTTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); +int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip); + +PURE_INLINE int dCollideRayTrimesh( dxGeom *ray, dxGeom *trimesh, int flags, + dContactGeom *contact, int skip ) +{ + // Swapped case, for code that needs it (heightfield initially) + // The other ray-geom colliders take geoms in a swapped order to the + // dCollideRTL function which is annoying when using function pointers. + return dCollideRTL( trimesh, ray, flags, contact, skip ); +} + +//**************************************************************************** +// dxTriMesh class + +#ifdef TRIMESH_INTERNAL + +#include "collision_kernel.h" +#include + +#if dTRIMESH_OPCODE +#define BAN_OPCODE_AUTOLINK +#include "Opcode.h" +using namespace Opcode; +#endif // dTRIMESH_OPCODE + +#if dTRIMESH_GIMPACT +#include +#endif + +struct dxTriMeshData : public dBase +{ + /* Array of flags for which edges and verts should be used on each triangle */ + enum UseFlags + { + kEdge0 = 0x1, + kEdge1 = 0x2, + kEdge2 = 0x4, + kVert0 = 0x8, + kVert1 = 0x10, + kVert2 = 0x20, + + kUseAll = 0xFF, + }; + + /* Setup the UseFlags array */ + void Preprocess(); + /* For when app changes the vertices */ + void UpdateData(); + +#if dTRIMESH_OPCODE + Model BVTree; + MeshInterface Mesh; + + dxTriMeshData(); + ~dxTriMeshData(); + + void Build(const void* Vertices, int VertexStide, int VertexCount, + const void* Indices, int IndexCount, int TriStride, + const void* Normals, + bool Single); + + /* aabb in model space */ + dVector3 AABBCenter; + dVector3 AABBExtents; + + // data for use in collision resolution + const void* Normals; + uint8* UseFlags; +#endif // dTRIMESH_OPCODE + +#if dTRIMESH_GIMPACT + const char* m_Vertices; + int m_VertexStride; + int m_VertexCount; + const char* m_Indices; + int m_TriangleCount; + int m_TriStride; + bool m_single; + + dxTriMeshData() + { + m_Vertices=NULL; + m_VertexStride = 12; + m_VertexCount = 0; + m_Indices = 0; + m_TriangleCount = 0; + m_TriStride = 12; + m_single = true; + } + + void Build(const void* Vertices, int VertexStride, int VertexCount, + const void* Indices, int IndexCount, int TriStride, + const void* Normals, + bool Single) + { + dIASSERT(Vertices); + dIASSERT(Indices); + dIASSERT(VertexStride); + dIASSERT(TriStride); + dIASSERT(IndexCount); + m_Vertices=(const char *)Vertices; + m_VertexStride = VertexStride; + m_VertexCount = VertexCount; + m_Indices = (const char *)Indices; + m_TriangleCount = IndexCount/3; + m_TriStride = TriStride; + m_single = Single; + } + + inline void GetVertex(unsigned int i, dVector3 Out) + { + if(m_single) + { + const float * fverts = (const float * )(m_Vertices + m_VertexStride*i); + Out[0] = fverts[0]; + Out[1] = fverts[1]; + Out[2] = fverts[2]; + Out[3] = 1.0f; + } + else + { + const double * dverts = (const double * )(m_Vertices + m_VertexStride*i); + Out[0] = (float)dverts[0]; + Out[1] = (float)dverts[1]; + Out[2] = (float)dverts[2]; + Out[3] = 1.0f; + + } + } + + inline void GetTriIndices(unsigned int itriangle, unsigned int triindices[3]) + { + const unsigned int * ind = (const unsigned int * )(m_Indices + m_TriStride*itriangle); + triindices[0] = ind[0]; + triindices[1] = ind[1]; + triindices[2] = ind[2]; + } +#endif // dTRIMESH_GIMPACT +}; + + +struct dxTriMesh : public dxGeom{ + // Callbacks + dTriCallback* Callback; + dTriArrayCallback* ArrayCallback; + dTriRayCallback* RayCallback; + + // Data types + dxTriMeshData* Data; + + bool doSphereTC; + bool doBoxTC; + bool doCapsuleTC; + + // Functions + dxTriMesh(dSpaceID Space, dTriMeshDataID Data); + ~dxTriMesh(); + + void ClearTCCache(); + + int AABBTest(dxGeom* g, dReal aabb[6]); + void computeAABB(); + +#if dTRIMESH_OPCODE + // Instance data for last transform. + dMatrix4 last_trans; + + // Colliders + static PlanesCollider _PlanesCollider; + static SphereCollider _SphereCollider; + static OBBCollider _OBBCollider; + static RayCollider _RayCollider; + static AABBTreeCollider _AABBTreeCollider; + static LSSCollider _LSSCollider; + + // Some constants + static CollisionFaces Faces; + // Temporal coherence + struct SphereTC : public SphereCache{ + dxGeom* Geom; + }; + dArray SphereTCCache; + static SphereCache defaultSphereCache; + + struct BoxTC : public OBBCache{ + dxGeom* Geom; + }; + dArray BoxTCCache; + static OBBCache defaultBoxCache; + + struct CapsuleTC : public LSSCache{ + dxGeom* Geom; + }; + dArray CapsuleTCCache; + static LSSCache defaultCapsuleCache; +#endif // dTRIMESH_OPCODE + +#if dTRIMESH_GIMPACT + GIM_TRIMESH m_collision_trimesh; +#endif // dTRIMESH_GIMPACT +}; + +#if 0 +#include "collision_kernel.h" +// Fetches a contact +inline dContactGeom* SAFECONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){ + dIASSERT(Index >= 0 && Index < (Flags & NUMC_MASK)); + return ((dContactGeom*)(((char*)Contacts) + (Index * Stride))); +} +#endif + +#if dTRIMESH_OPCODE +inline void FetchTriangle(dxTriMesh* TriMesh, int Index, dVector3 Out[3]){ + VertexPointers VP; + TriMesh->Data->Mesh.GetTriangle(VP, Index); + for (int i = 0; i < 3; i++){ + Out[i][0] = VP.Vertex[i]->x; + Out[i][1] = VP.Vertex[i]->y; + Out[i][2] = VP.Vertex[i]->z; + Out[i][3] = 0; + } +} + +inline void FetchTriangle(dxTriMesh* TriMesh, int Index, const dVector3 Position, const dMatrix3 Rotation, dVector3 Out[3]){ + VertexPointers VP; + TriMesh->Data->Mesh.GetTriangle(VP, Index); + for (int i = 0; i < 3; i++){ + dVector3 v; + v[0] = VP.Vertex[i]->x; + v[1] = VP.Vertex[i]->y; + v[2] = VP.Vertex[i]->z; + v[3] = 0; + + dMULTIPLY0_331(Out[i], Rotation, v); + Out[i][0] += Position[0]; + Out[i][1] += Position[1]; + Out[i][2] += Position[2]; + Out[i][3] = 0; + } +} + +inline Matrix4x4& MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, Matrix4x4& Out){ + Out.m[0][0] = (float) Rotation[0]; + Out.m[1][0] = (float) Rotation[1]; + Out.m[2][0] = (float) Rotation[2]; + + Out.m[0][1] = (float) Rotation[4]; + Out.m[1][1] = (float) Rotation[5]; + Out.m[2][1] = (float) Rotation[6]; + + Out.m[0][2] = (float) Rotation[8]; + Out.m[1][2] = (float) Rotation[9]; + Out.m[2][2] = (float) Rotation[10]; + + Out.m[3][0] = (float) Position[0]; + Out.m[3][1] = (float) Position[1]; + Out.m[3][2] = (float) Position[2]; + + Out.m[0][3] = 0.0f; + Out.m[1][3] = 0.0f; + Out.m[2][3] = 0.0f; + Out.m[3][3] = 1.0f; + + return Out; +} + +inline Matrix4x4& MakeMatrix(dxGeom* g, Matrix4x4& Out){ + const dVector3& Position = *(const dVector3*)dGeomGetPosition(g); + const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g); + return MakeMatrix(Position, Rotation, Out); +} +#endif // dTRIMESH_OPCODE + +#if dTRIMESH_GIMPACT +inline void FetchTriangle(dxTriMesh* TriMesh, int Index, const dVector3 Position, const dMatrix3 Rotation, dVector3 Out[3]){ + // why is this not implemented? + dAASSERT(false); +} + +inline void MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, mat4f m) +{ + m[0][0] = (float) Rotation[0]; + m[0][1] = (float) Rotation[1]; + m[0][2] = (float) Rotation[2]; + + m[1][0] = (float) Rotation[4]; + m[1][1] = (float) Rotation[5]; + m[1][2] = (float) Rotation[6]; + + m[2][0] = (float) Rotation[8]; + m[2][1] = (float) Rotation[9]; + m[2][2] = (float) Rotation[10]; + + m[0][3] = (float) Position[0]; + m[1][3] = (float) Position[1]; + m[2][3] = (float) Position[2]; + +} + +inline void MakeMatrix(dxGeom* g, mat4f Out){ + const dVector3& Position = *(const dVector3*)dGeomGetPosition(g); + const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g); + MakeMatrix(Position, Rotation, Out); +} +#endif // dTRIMESH_GIMPACT + +// Outputs a matrix to 3 vectors +inline void Decompose(const dMatrix3 Matrix, dVector3 Right, dVector3 Up, dVector3 Direction){ + Right[0] = Matrix[0 * 4 + 0]; + Right[1] = Matrix[1 * 4 + 0]; + Right[2] = Matrix[2 * 4 + 0]; + Right[3] = REAL(0.0); + Up[0] = Matrix[0 * 4 + 1]; + Up[1] = Matrix[1 * 4 + 1]; + Up[2] = Matrix[2 * 4 + 1]; + Up[3] = REAL(0.0); + Direction[0] = Matrix[0 * 4 + 2]; + Direction[1] = Matrix[1 * 4 + 2]; + Direction[2] = Matrix[2 * 4 + 2]; + Direction[3] = REAL(0.0); +} + +// Outputs a matrix to 3 vectors +inline void Decompose(const dMatrix3 Matrix, dVector3 Vectors[3]){ + Decompose(Matrix, Vectors[0], Vectors[1], Vectors[2]); +} + +// Finds barycentric +inline void GetPointFromBarycentric(const dVector3 dv[3], dReal u, dReal v, dVector3 Out){ + dReal w = REAL(1.0) - u - v; + + Out[0] = (dv[0][0] * w) + (dv[1][0] * u) + (dv[2][0] * v); + Out[1] = (dv[0][1] * w) + (dv[1][1] * u) + (dv[2][1] * v); + Out[2] = (dv[0][2] * w) + (dv[1][2] * u) + (dv[2][2] * v); + Out[3] = (dv[0][3] * w) + (dv[1][3] * u) + (dv[2][3] * v); +} + +// Performs a callback +inline bool Callback(dxTriMesh* TriMesh, dxGeom* Object, int TriIndex){ + if (TriMesh->Callback != NULL){ + return (TriMesh->Callback(TriMesh, Object, TriIndex)!=0); + } + else return true; +} + +// Some utilities +template const T& dcMAX(const T& x, const T& y){ + return x > y ? x : y; +} + +template const T& dcMIN(const T& x, const T& y){ + return x < y ? x : y; +} + +dReal SqrDistancePointTri( const dVector3 p, const dVector3 triOrigin, + const dVector3 triEdge1, const dVector3 triEdge2, + dReal* pfSParam = 0, dReal* pfTParam = 0 ); + +dReal SqrDistanceSegments( const dVector3 seg1Origin, const dVector3 seg1Direction, + const dVector3 seg2Origin, const dVector3 seg2Direction, + dReal* pfSegP0 = 0, dReal* pfSegP1 = 0 ); + +dReal SqrDistanceSegTri( const dVector3 segOrigin, const dVector3 segEnd, + const dVector3 triOrigin, + const dVector3 triEdge1, const dVector3 triEdge2, + dReal* t = 0, dReal* u = 0, dReal* v = 0 ); + +inline +void Vector3Subtract( const dVector3 left, const dVector3 right, dVector3 result ) +{ + result[0] = left[0] - right[0]; + result[1] = left[1] - right[1]; + result[2] = left[2] - right[2]; + result[3] = REAL(0.0); +} + +inline +void Vector3Add( const dVector3 left, const dVector3 right, dVector3 result ) +{ + result[0] = left[0] + right[0]; + result[1] = left[1] + right[1]; + result[2] = left[2] + right[2]; + result[3] = REAL(0.0); +} + +inline +void Vector3Negate( const dVector3 in, dVector3 out ) +{ + out[0] = -in[0]; + out[1] = -in[1]; + out[2] = -in[2]; + out[3] = REAL(0.0); +} + +inline +void Vector3Copy( const dVector3 in, dVector3 out ) +{ + out[0] = in[0]; + out[1] = in[1]; + out[2] = in[2]; + out[3] = REAL(0.0); +} + +inline +void Vector3Multiply( const dVector3 in, dReal scalar, dVector3 out ) +{ + out[0] = in[0] * scalar; + out[1] = in[1] * scalar; + out[2] = in[2] * scalar; + out[3] = REAL(0.0); +} + +inline +void TransformVector3( const dVector3 in, + const dMatrix3 orientation, const dVector3 position, + dVector3 out ) +{ + dMULTIPLY0_331( out, orientation, in ); + out[0] += position[0]; + out[1] += position[1]; + out[2] += position[2]; +} + +//------------------------------------------------------------------------------ +/** + @brief Check for intersection between triangle and capsule. + + @param dist [out] Shortest distance squared between the triangle and + the capsule segment (central axis). + @param t [out] t value of point on segment that's the shortest distance + away from the triangle, the coordinates of this point + can be found by (cap.seg.end - cap.seg.start) * t, + or cap.seg.ipol(t). + @param u [out] Barycentric coord on triangle. + @param v [out] Barycentric coord on triangle. + @return True if intersection exists. + + The third Barycentric coord is implicit, ie. w = 1.0 - u - v + The Barycentric coords give the location of the point on the triangle + closest to the capsule (where the distance between the two shapes + is the shortest). +*/ +inline +bool IntersectCapsuleTri( const dVector3 segOrigin, const dVector3 segEnd, + const dReal radius, const dVector3 triOrigin, + const dVector3 triEdge0, const dVector3 triEdge1, + dReal* dist, dReal* t, dReal* u, dReal* v ) +{ + dReal sqrDist = SqrDistanceSegTri( segOrigin, segEnd, triOrigin, triEdge0, triEdge1, + t, u, v ); + + if ( dist ) + *dist = sqrDist; + + return ( sqrDist <= (radius * radius) ); +} + +#endif //TRIMESH_INTERNAL + +#endif //_ODE_COLLISION_TRIMESH_INTERNAL_H_ -- cgit v1.1