ode/src/collision_trimesh_gimpact.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 // TriMesh storage classes refactoring and face angle computation code by Oleh Derevenko (C) 2016-2022
  27
  28
  29 #ifndef _ODE_COLLISION_TRIMESH_GIMPACT_H_
  30 #define _ODE_COLLISION_TRIMESH_GIMPACT_H_
  31
  32
  33 #if dTRIMESH_ENABLED && dTRIMESH_GIMPACT
  34
  35
  36 //****************************************************************************
  37 // dxTriMesh class
  38
  39
  40 #include "collision_kernel.h"
  41 #include "collision_trimesh_colliders.h"
  42 #include "collision_util.h"
  43 #include <ode/collision_trimesh.h>
  44
  45 #include "collision_trimesh_internal.h"
  46 #include <GIMPACT/gimpact.h>
  47
  48
  49 struct TrimeshCollidersCache // Required for compatibility with OPCODE
  50 {
  51 };
  52
  53
  54 typedef dxTriDataBase dxTriMeshData_Parent;
  55 struct dxTriMeshData:
  56     public dxTriMeshData_Parent
  57 {
  58 public:
  59     dxTriMeshData():
  60         dxTriMeshData_Parent()
  61     {
  62     }
  63
  64     ~dxTriMeshData() { /* Do nothing */ }
  65
  66     using dxTriMeshData_Parent::buildData;
  67
  68     /* Setup the UseFlags array and/or build face angles*/
  69     bool preprocessData(bool buildUseFlags/*=false*/, FaceAngleStorageMethod faceAndgesRequirement/*=ASM__INVALID*/);
  70
  71 private:
  72     bool meaningfulPreprocessData(FaceAngleStorageMethod faceAndgesRequirement/*=ASM__INVALID*/);
  73
  74 public:
  75     /* For when app changes the vertices */
  76     void updateData() { /* Do nothing */ }
  77
  78 public:
  79     const vec3f *retrieveVertexInstances() const { return (const vec3f *)dxTriMeshData_Parent::retrieveVertexInstances(); }
  80     const GUINT32 *retrieveTriangleVertexIndices() const { return (const GUINT32 *)dxTriMeshData_Parent::retrieveTriangleVertexIndices(); }
  81
  82 public:
  83     void assignNormals(const dReal *normals) { dxTriMeshData_Parent::assignNormals(normals); }
  84     const dReal *retrieveNormals() const { return (const dReal *)dxTriMeshData_Parent::retrieveNormals(); }
  85     sizeint calculateNormalsMemoryRequirement() const { return retrieveTriangleCount() * (sizeof(dReal) * dSA__MAX); }
  86 };
  87
  88
  89
  90 #ifdef dDOUBLE
  91 // To use GIMPACT with doubles, we need to patch a couple of the GIMPACT functions to
  92 // convert arguments to floats before sending them in
  93
  94
  95 /// Convert an gimpact vec3f to a ODE dVector3d:   dVector3[i] = vec3f[i]
  96 #define dVECTOR3_VEC3F_COPY(b,a) { \
  97     (b)[0] = (a)[0];              \
  98     (b)[1] = (a)[1];              \
  99     (b)[2] = (a)[2];              \
 100     (b)[3] = 0;                   \
 101 }
 102
 103 static inline
 104 void gim_trimesh_get_triangle_verticesODE(GIM_TRIMESH * trimesh, GUINT32 triangle_index, dVector3 v1, dVector3 v2, dVector3 v3)
 105 {
 106     vec3f src1, src2, src3;
 107     GREAL *psrc1 = v1 != NULL ? src1 : NULL;
 108     GREAL *psrc2 = v2 != NULL ? src2 : NULL;
 109     GREAL *psrc3 = v3 != NULL ? src3 : NULL;
 110     gim_trimesh_get_triangle_vertices(trimesh, triangle_index, psrc1, psrc2, psrc3);
 111
 112     if (v1 != NULL)
 113     {
 114         dVECTOR3_VEC3F_COPY(v1, src1);
 115     }
 116
 117     if (v2 != NULL)
 118     {
 119         dVECTOR3_VEC3F_COPY(v2, src2);
 120     }
 121
 122     if (v3 != NULL)
 123     {
 124         dVECTOR3_VEC3F_COPY(v3, src3);
 125     }
 126 }
 127
 128 // Anything calling gim_trimesh_get_triangle_vertices from within ODE
 129 // should be patched through to the dDOUBLE version above
 130
 131 #define gim_trimesh_get_triangle_vertices gim_trimesh_get_triangle_verticesODE
 132
 133 static inline
 134 int gim_trimesh_ray_closest_collisionODE( GIM_TRIMESH *mesh, dVector3 origin, dVector3 dir, dReal tmax, GIM_TRIANGLE_RAY_CONTACT_DATA *contact )
 135 {
 136     vec3f dir_vec3f    = { (GREAL)dir[ 0 ],    (GREAL)dir[ 1 ],    (GREAL)dir[ 2 ]    };
 137     vec3f origin_vec3f = { (GREAL)origin[ 0 ], (GREAL)origin[ 1 ], (GREAL)origin[ 2 ] };
 138
 139     return gim_trimesh_ray_closest_collision( mesh, origin_vec3f, dir_vec3f, (GREAL)tmax, contact );
 140 }
 141
 142 static inline
 143 int gim_trimesh_ray_collisionODE( GIM_TRIMESH *mesh, const dVector3 origin, const dVector3 dir, dReal tmax, GIM_TRIANGLE_RAY_CONTACT_DATA *contact )
 144 {
 145     vec3f dir_vec3f    = { (GREAL)dir[ 0 ],    (GREAL)dir[ 1 ],    (GREAL)dir[ 2 ]    };
 146     vec3f origin_vec3f = { (GREAL)origin[ 0 ], (GREAL)origin[ 1 ], (GREAL)origin[ 2 ] };
 147
 148     return gim_trimesh_ray_collision( mesh, origin_vec3f, dir_vec3f, (GREAL)tmax, contact );
 149 }
 150
 151 static inline
 152 void gim_trimesh_sphere_collisionODE( GIM_TRIMESH *mesh, const dVector3 Position, dReal Radius, GDYNAMIC_ARRAY *contact )
 153 {
 154     vec3f pos_vec3f = { (GREAL)Position[ 0 ], (GREAL)Position[ 1 ], (GREAL)Position[ 2 ] };
 155     gim_trimesh_sphere_collision( mesh, pos_vec3f, (GREAL)Radius, contact );
 156 }
 157
 158 static inline
 159 void gim_trimesh_plane_collisionODE( GIM_TRIMESH *mesh, const dVector4 plane, GDYNAMIC_ARRAY *contact )
 160 {
 161     vec4f plane_vec4f = { (GREAL)plane[ 0 ], (GREAL)plane[ 1 ], (GREAL)plane[ 2 ], (GREAL)plane[ 3 ] }; \
 162     gim_trimesh_plane_collision( mesh, plane_vec4f, contact );      \
 163 }
 164
 165 #define GIM_AABB_COPY( src, dst ) {             \
 166     (dst)[ 0 ]= (src) -> minX;                  \
 167     (dst)[ 1 ]= (src) -> maxX;                  \
 168     (dst)[ 2 ]= (src) -> minY;                  \
 169     (dst)[ 3 ]= (src) -> maxY;                  \
 170     (dst)[ 4 ]= (src) -> minZ;                  \
 171     (dst)[ 5 ]= (src) -> maxZ;                  \
 172 }
 173
 174
 175 #else // #ifdef !dDOUBLE
 176
 177 // With single precision, we can pass native ODE vectors directly to GIMPACT
 178
 179 #define gim_trimesh_ray_closest_collisionODE    gim_trimesh_ray_closest_collision
 180 #define gim_trimesh_ray_collisionODE                    gim_trimesh_ray_collision
 181 #define gim_trimesh_sphere_collisionODE                 gim_trimesh_sphere_collision
 182 #define gim_trimesh_plane_collisionODE                  gim_trimesh_plane_collision
 183
 184 #define GIM_AABB_COPY( src, dst )       memcpy( dst, src, 6 * sizeof( GREAL ) )
 185
 186
 187 #endif // #ifdef !dDOUBLE
 188
 189
 190 typedef dxMeshBase dxTriMesh_Parent;
 191 struct dxTriMesh:
 192     public dxTriMesh_Parent
 193 {
 194 public:
 195     // Functions
 196     dxTriMesh(dxSpace *Space, dxTriMeshData *Data,
 197         dTriCallback *Callback, dTriArrayCallback *ArrayCallback, dTriRayCallback *RayCallback):
 198         dxTriMesh_Parent(Space, NULL, Callback, ArrayCallback, RayCallback, true) // TC has speed/space 'issues' that don't make it a clear win by default on spheres/boxes.
 199     {
 200         gim_init_buffer_managers(m_buffer_managers);
 201         assignMeshData(Data);
 202     }
 203
 204     ~dxTriMesh();
 205
 206     void clearTCCache() { /* do nothing */ }
 207
 208     virtual void computeAABB();
 209
 210 public:
 211     dxTriMeshData *retrieveMeshData() const { return getMeshData(); }
 212
 213     unsigned getMeshTriangleCount() const { return gim_trimesh_get_triangle_count(const_cast<GIM_TRIMESH *>(&m_collision_trimesh)); }
 214
 215     void fetchMeshTransformedTriangle(dVector3 *const pout_triangle[3], unsigned index)
 216     {
 217         gim_trimesh_locks_work_data(&m_collision_trimesh);
 218         gim_trimesh_get_triangle_vertices(&m_collision_trimesh, (GUINT32)index, *pout_triangle[0], *pout_triangle[1], *pout_triangle[2]);
 219         gim_trimesh_unlocks_work_data(&m_collision_trimesh);
 220     }
 221
 222     void fetchMeshTransformedTriangle(dVector3 out_triangle[3], unsigned index)
 223     {
 224         gim_trimesh_locks_work_data(&m_collision_trimesh);
 225         gim_trimesh_get_triangle_vertices(&m_collision_trimesh, (GUINT32)index, out_triangle[0], out_triangle[1], out_triangle[2]);
 226         gim_trimesh_unlocks_work_data(&m_collision_trimesh);
 227     }
 228
 229 private:
 230     dxTriMeshData *getMeshData() const { return static_cast<dxTriMeshData *>(dxTriMesh_Parent::getMeshData()); }
 231
 232 public:
 233     enum
 234     {
 235         VERTEXINSTANCE_STRIDE = sizeof(vec3f),
 236         TRIANGLEINDEX_STRIDE = sizeof(GUINT32) * dMTV__MAX,
 237     };
 238
 239     void assignMeshData(dxTriMeshData *Data);
 240
 241 public:
 242     GIM_TRIMESH  m_collision_trimesh;
 243     GBUFFER_MANAGER_DATA m_buffer_managers[G_BUFFER_MANAGER__MAX];
 244 };
 245
 246
 247 static inline
 248 void MakeMatrix(const dVector3 position, const dMatrix3 rotation, mat4f m)
 249 {
 250     m[0][0] = (GREAL)rotation[dM3E_XX];
 251     m[0][1] = (GREAL)rotation[dM3E_XY];
 252     m[0][2] = (GREAL)rotation[dM3E_XZ];
 253
 254     m[1][0] = (GREAL)rotation[dM3E_YX];
 255     m[1][1] = (GREAL)rotation[dM3E_YY];
 256     m[1][2] = (GREAL)rotation[dM3E_YZ];
 257
 258     m[2][0] = (GREAL)rotation[dM3E_ZX];
 259     m[2][1] = (GREAL)rotation[dM3E_ZY];
 260     m[2][2] = (GREAL)rotation[dM3E_ZZ];
 261
 262     m[0][3] = (GREAL)position[dV3E_X];
 263     m[1][3] = (GREAL)position[dV3E_Y];
 264     m[2][3] = (GREAL)position[dV3E_Z];
 265 }
 266
 267 static inline
 268 void MakeMatrix(dxGeom *g, mat4f m)
 269 {
 270     const dVector3 &position = g->buildUpdatedPosition();
 271     const dMatrix3 &rotation = g->buildUpdatedRotation();
 272     MakeMatrix(position, rotation, m);
 273 }
 274
 275
 276 #endif // #if dTRIMESH_ENABLED && dTRIMESH_GIMPACT
 277
 278 #endif  //_ODE_COLLISION_TRIMESH_GIMPACT_H_