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