ode/src/collision_trimesh_ray.cpp

   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
  25 #include <ode/collision.h>
  26 #include <ode/rotation.h>
  27 #include "config.h"
  28 #include "matrix.h"
  29 #include "odemath.h"
  30
  31 #if dTRIMESH_ENABLED
  32
  33 #include "collision_util.h"
  34 #include "collision_trimesh_internal.h"
  35
  36 #if dTRIMESH_OPCODE
  37 int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride){
  38     dIASSERT (Stride >= (int)sizeof(dContactGeom));
  39     dIASSERT (g1->type == dTriMeshClass);
  40     dIASSERT (RayGeom->type == dRayClass);
  41     dIASSERT ((Flags & NUMC_MASK) >= 1);
  42
  43     dxTriMesh* TriMesh = (dxTriMesh*)g1;
  44
  45     const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind();
  46     dIASSERT(uiTLSKind == RayGeom->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method
  47     TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind);
  48     RayCollider& Collider = pccColliderCache->m_RayCollider;
  49
  50     dReal Length = dGeomRayGetLength(RayGeom);
  51
  52     int FirstContact = dGeomRayGetFirstContact(RayGeom);
  53     int BackfaceCull = dGeomRayGetBackfaceCull(RayGeom);
  54     int ClosestHit = dGeomRayGetClosestHit(RayGeom);
  55
  56     Collider.SetFirstContact(FirstContact != 0);
  57     Collider.SetClosestHit(ClosestHit != 0);
  58     Collider.SetCulling(BackfaceCull != 0);
  59     Collider.SetMaxDist(Length);
  60
  61     const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
  62     const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
  63
  64     Matrix4x4 MeshMatrix;
  65     const dVector3 ZeroVector3 = { REAL(0.0), };
  66     MakeMatrix(ZeroVector3, TLRotation, MeshMatrix);
  67
  68     dVector3 Origin, Direction;
  69     dGeomRayGet(RayGeom, Origin, Direction);
  70
  71     dVector3 OffsetOrigin;
  72     dSubtractVectors3(OffsetOrigin, Origin, TLPosition);
  73
  74     /* Make Ray */
  75     Ray WorldRay;
  76     WorldRay.mOrig.Set(OffsetOrigin[0], OffsetOrigin[1], OffsetOrigin[2]);
  77     WorldRay.mDir.Set(Direction[0], Direction[1], Direction[2]);
  78
  79     /* Intersect */
  80     int TriCount = 0;
  81     if (Collider.Collide(WorldRay, TriMesh->retrieveMeshBVTreeRef(), &MeshMatrix)) {
  82         TriCount = pccColliderCache->m_Faces.GetNbFaces();
  83     }
  84
  85     if (TriCount == 0) {
  86         return 0;
  87     }
  88
  89     const CollisionFace* Faces = pccColliderCache->m_Faces.GetFaces();
  90
  91     int OutTriCount = 0;
  92     for (int i = 0; i < TriCount; i++) {
  93         if (TriMesh->m_RayCallback == null ||
  94             TriMesh->m_RayCallback(TriMesh, RayGeom, Faces[i].mFaceID,
  95             Faces[i].mU, Faces[i].mV)) {
  96                 const int& TriIndex = Faces[i].mFaceID;
  97                 if (!TriMesh->invokeCallback(RayGeom, TriIndex)) {
  98                     continue;
  99                 }
 100
 101                 dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
 102
 103                 dVector3 dv[3];
 104                 TriMesh->fetchMeshTriangle(dv, TriIndex, TLPosition, TLRotation);
 105
 106                 dVector3 vu;
 107                 vu[0] = dv[1][0] - dv[0][0];
 108                 vu[1] = dv[1][1] - dv[0][1];
 109                 vu[2] = dv[1][2] - dv[0][2];
 110                 vu[3] = REAL(0.0);
 111
 112                 dVector3 vv;
 113                 vv[0] = dv[2][0] - dv[0][0];
 114                 vv[1] = dv[2][1] - dv[0][1];
 115                 vv[2] = dv[2][2] - dv[0][2];
 116                 vv[3] = REAL(0.0);
 117
 118                 dCalcVectorCross3(Contact->normal, vv, vu);     // Reversed
 119
 120                 // Even though all triangles might be initially valid,
 121                 // a triangle may degenerate into a segment after applying
 122                 // space transformation.
 123                 if (dSafeNormalize3(Contact->normal))
 124                 {
 125                     // No sense to save on single type conversion in algorithm of this size.
 126                     // If there would be a custom typedef for distance type it could be used
 127                     // instead of dReal. However using float directly is the loss of abstraction
 128                     // and possible loss of precision in future.
 129                     /*float*/ dReal T = Faces[i].mDistance;
 130                     Contact->pos[0] = Origin[0] + (Direction[0] * T);
 131                     Contact->pos[1] = Origin[1] + (Direction[1] * T);
 132                     Contact->pos[2] = Origin[2] + (Direction[2] * T);
 133                     Contact->pos[3] = REAL(0.0);
 134
 135                     Contact->depth = T;
 136                     Contact->g1 = TriMesh;
 137                     Contact->g2 = RayGeom;
 138                     Contact->side1 = TriIndex;
 139                     Contact->side2 = -1;
 140
 141                     OutTriCount++;
 142
 143                     // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
 144                     if (OutTriCount >= (Flags & NUMC_MASK)) {
 145                         break;
 146                     }
 147                 }
 148         }
 149     }
 150     return OutTriCount;
 151 }
 152 #endif // dTRIMESH_OPCODE
 153
 154 #if dTRIMESH_GIMPACT
 155 int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride)
 156 {
 157     dIASSERT (Stride >= (int)sizeof(dContactGeom));
 158     dIASSERT (g1->type == dTriMeshClass);
 159     dIASSERT (RayGeom->type == dRayClass);
 160     dIASSERT ((Flags & NUMC_MASK) >= 1);
 161
 162     dxTriMesh* TriMesh = (dxTriMesh*)g1;
 163
 164     dReal Length = dGeomRayGetLength(RayGeom);
 165     int FirstContact = dGeomRayGetFirstContact(RayGeom);
 166     int BackfaceCull = dGeomRayGetBackfaceCull(RayGeom);
 167     int ClosestHit = dGeomRayGetClosestHit(RayGeom);
 168     dVector3 Origin, Direction;
 169     dGeomRayGet(RayGeom, Origin, Direction);
 170
 171     char intersect=0;
 172     GIM_TRIANGLE_RAY_CONTACT_DATA contact_data;
 173
 174     if(ClosestHit)
 175     {
 176         intersect = gim_trimesh_ray_closest_collisionODE(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data);
 177     }
 178     else
 179     {
 180         intersect = gim_trimesh_ray_collisionODE(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data);
 181     }
 182
 183     if(intersect == 0)
 184     {
 185         return 0;
 186     }
 187
 188
 189     if(!TriMesh->m_RayCallback ||
 190         TriMesh->m_RayCallback(TriMesh, RayGeom, contact_data.m_face_id, contact_data.u , contact_data.v))
 191     {
 192         dContactGeom* Contact = &( Contacts[ 0 ] );
 193         VEC_COPY(Contact->pos,contact_data.m_point);
 194         VEC_COPY(Contact->normal,contact_data.m_normal);
 195         Contact->depth = contact_data.tparam;
 196         Contact->g1 = TriMesh;
 197         Contact->g2 = RayGeom;
 198         Contact->side1 = contact_data.m_face_id;
 199         Contact->side2 = -1;
 200         return 1;
 201     }
 202
 203     return 0;
 204 }
 205 #endif  // dTRIMESH_GIMPACT
 206
 207 #endif // dTRIMESH_ENABLED