From d48ea5bb797037069d641da41da0f195f0124491 Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:20:48 +0000 Subject: one more for the gipper --- .../ode-0.9/ode/src/collision_trimesh_ray.cpp | 198 +++++++++++++++++++++ 1 file changed, 198 insertions(+) create mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_ray.cpp (limited to 'libraries/ode-0.9\/ode/src/collision_trimesh_ray.cpp') diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_ray.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_ray.cpp new file mode 100644 index 0000000..c0b97ca --- /dev/null +++ b/libraries/ode-0.9/ode/src/collision_trimesh_ray.cpp @@ -0,0 +1,198 @@ +/************************************************************************* + * * + * 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. + +#include +#include +#include +#include + +#if dTRIMESH_ENABLED + +#include "collision_util.h" + +#define TRIMESH_INTERNAL +#include "collision_trimesh_internal.h" + +#if dTRIMESH_OPCODE +int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride){ + dIASSERT (Stride >= (int)sizeof(dContactGeom)); + dIASSERT (g1->type == dTriMeshClass); + dIASSERT (RayGeom->type == dRayClass); + dIASSERT ((Flags & NUMC_MASK) >= 1); + + dxTriMesh* TriMesh = (dxTriMesh*)g1; + + const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh); + const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh); + + RayCollider& Collider = TriMesh->_RayCollider; + + dReal Length = dGeomRayGetLength(RayGeom); + + int FirstContact, BackfaceCull; + dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull); + int ClosestHit = dGeomRayGetClosestHit(RayGeom); + + Collider.SetFirstContact(FirstContact != 0); + Collider.SetClosestHit(ClosestHit != 0); + Collider.SetCulling(BackfaceCull != 0); + Collider.SetMaxDist(Length); + + dVector3 Origin, Direction; + dGeomRayGet(RayGeom, Origin, Direction); + + /* Make Ray */ + Ray WorldRay; + WorldRay.mOrig.x = Origin[0]; + WorldRay.mOrig.y = Origin[1]; + WorldRay.mOrig.z = Origin[2]; + WorldRay.mDir.x = Direction[0]; + WorldRay.mDir.y = Direction[1]; + WorldRay.mDir.z = Direction[2]; + + /* Intersect */ + Matrix4x4 amatrix; + int TriCount = 0; + if (Collider.Collide(WorldRay, TriMesh->Data->BVTree, &MakeMatrix(TLPosition, TLRotation, amatrix))) { + TriCount = TriMesh->Faces.GetNbFaces(); + } + + if (TriCount == 0) { + return 0; + } + + const CollisionFace* Faces = TriMesh->Faces.GetFaces(); + + int OutTriCount = 0; + for (int i = 0; i < TriCount; i++) { + if (TriMesh->RayCallback == null || + TriMesh->RayCallback(TriMesh, RayGeom, Faces[i].mFaceID, + Faces[i].mU, Faces[i].mV)) { + const int& TriIndex = Faces[i].mFaceID; + if (!Callback(TriMesh, RayGeom, TriIndex)) { + continue; + } + + dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride); + + dVector3 dv[3]; + FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv); + + // No sense to save on single type conversion in algorithm of this size. + // If there would be a custom typedef for distance type it could be used + // instead of dReal. However using float directly is the loss of abstraction + // and possible loss of precision in future. + /*float*/ dReal T = Faces[i].mDistance; + Contact->pos[0] = Origin[0] + (Direction[0] * T); + Contact->pos[1] = Origin[1] + (Direction[1] * T); + Contact->pos[2] = Origin[2] + (Direction[2] * T); + Contact->pos[3] = REAL(0.0); + + dVector3 vu; + vu[0] = dv[1][0] - dv[0][0]; + vu[1] = dv[1][1] - dv[0][1]; + vu[2] = dv[1][2] - dv[0][2]; + vu[3] = REAL(0.0); + + dVector3 vv; + vv[0] = dv[2][0] - dv[0][0]; + vv[1] = dv[2][1] - dv[0][1]; + vv[2] = dv[2][2] - dv[0][2]; + vv[3] = REAL(0.0); + + dCROSS(Contact->normal, =, vv, vu); // Reversed + + dNormalize3(Contact->normal); + + Contact->depth = T; + Contact->g1 = TriMesh; + Contact->g2 = RayGeom; + + OutTriCount++; + + // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue" + if (OutTriCount >= (Flags & NUMC_MASK)) { + break; + } + } + } + return OutTriCount; +} +#endif // dTRIMESH_OPCODE + +#if dTRIMESH_GIMPACT +int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride) +{ + dIASSERT (Stride >= (int)sizeof(dContactGeom)); + dIASSERT (g1->type == dTriMeshClass); + dIASSERT (RayGeom->type == dRayClass); + dIASSERT ((Flags & NUMC_MASK) >= 1); + + dxTriMesh* TriMesh = (dxTriMesh*)g1; + + dReal Length = dGeomRayGetLength(RayGeom); + int FirstContact, BackfaceCull; + dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull); + int ClosestHit = dGeomRayGetClosestHit(RayGeom); + dVector3 Origin, Direction; + dGeomRayGet(RayGeom, Origin, Direction); + + char intersect=0; + GIM_TRIANGLE_RAY_CONTACT_DATA contact_data; + + if(ClosestHit) + { + intersect = gim_trimesh_ray_closest_collision(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data); + } + else + { + intersect = gim_trimesh_ray_collision(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data); + } + + if(intersect == 0) + { + return 0; + } + + int OutTriCount = 0; + + if(!TriMesh->RayCallback || + TriMesh->RayCallback(TriMesh, RayGeom, contact_data.m_face_id, contact_data.u , contact_data.v)) + { + dContactGeom* Contact = SAFECONTACT(Flags, Contacts, (OutTriCount-1), Stride); + VEC_COPY(Contact->pos,contact_data.m_point); + VEC_COPY(Contact->normal,contact_data.m_normal); + Contact->depth = contact_data.tparam; + Contact->g1 = TriMesh; + Contact->g2 = RayGeom; + + OutTriCount = 1; + } + + return OutTriCount; +} +#endif // dTRIMESH_GIMPACT + +#endif // dTRIMESH_ENABLED + -- cgit v1.1