From c5fc66ee58f2c60f2d226868bb1cf5b91badaf53 Mon Sep 17 00:00:00 2001 From: sanine Date: Sat, 1 Oct 2022 20:59:36 -0500 Subject: add ode --- libs/ode-0.16.1/ode/src/collision_trimesh_ray.cpp | 207 ++++++++++++++++++++++ 1 file changed, 207 insertions(+) create mode 100644 libs/ode-0.16.1/ode/src/collision_trimesh_ray.cpp (limited to 'libs/ode-0.16.1/ode/src/collision_trimesh_ray.cpp') diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_ray.cpp b/libs/ode-0.16.1/ode/src/collision_trimesh_ray.cpp new file mode 100644 index 0000000..866758a --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_trimesh_ray.cpp @@ -0,0 +1,207 @@ +/************************************************************************* + * * + * 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 "config.h" +#include "matrix.h" +#include "odemath.h" + +#if dTRIMESH_ENABLED + +#include "collision_util.h" +#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 unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind(); + dIASSERT(uiTLSKind == RayGeom->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method + TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind); + RayCollider& Collider = pccColliderCache->m_RayCollider; + + dReal Length = dGeomRayGetLength(RayGeom); + + int FirstContact = dGeomRayGetFirstContact(RayGeom); + int BackfaceCull = dGeomRayGetBackfaceCull(RayGeom); + int ClosestHit = dGeomRayGetClosestHit(RayGeom); + + Collider.SetFirstContact(FirstContact != 0); + Collider.SetClosestHit(ClosestHit != 0); + Collider.SetCulling(BackfaceCull != 0); + Collider.SetMaxDist(Length); + + const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh); + const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh); + + Matrix4x4 MeshMatrix; + const dVector3 ZeroVector3 = { REAL(0.0), }; + MakeMatrix(ZeroVector3, TLRotation, MeshMatrix); + + dVector3 Origin, Direction; + dGeomRayGet(RayGeom, Origin, Direction); + + dVector3 OffsetOrigin; + dSubtractVectors3(OffsetOrigin, Origin, TLPosition); + + /* Make Ray */ + Ray WorldRay; + WorldRay.mOrig.Set(OffsetOrigin[0], OffsetOrigin[1], OffsetOrigin[2]); + WorldRay.mDir.Set(Direction[0], Direction[1], Direction[2]); + + /* Intersect */ + int TriCount = 0; + if (Collider.Collide(WorldRay, TriMesh->retrieveMeshBVTreeRef(), &MeshMatrix)) { + TriCount = pccColliderCache->m_Faces.GetNbFaces(); + } + + if (TriCount == 0) { + return 0; + } + + const CollisionFace* Faces = pccColliderCache->m_Faces.GetFaces(); + + int OutTriCount = 0; + for (int i = 0; i < TriCount; i++) { + if (TriMesh->m_RayCallback == null || + TriMesh->m_RayCallback(TriMesh, RayGeom, Faces[i].mFaceID, + Faces[i].mU, Faces[i].mV)) { + const int& TriIndex = Faces[i].mFaceID; + if (!TriMesh->invokeCallback(RayGeom, TriIndex)) { + continue; + } + + dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride); + + dVector3 dv[3]; + TriMesh->fetchMeshTriangle(dv, TriIndex, TLPosition, TLRotation); + + 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); + + dCalcVectorCross3(Contact->normal, vv, vu); // Reversed + + // Even though all triangles might be initially valid, + // a triangle may degenerate into a segment after applying + // space transformation. + if (dSafeNormalize3(Contact->normal)) + { + // 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); + + Contact->depth = T; + Contact->g1 = TriMesh; + Contact->g2 = RayGeom; + Contact->side1 = TriIndex; + Contact->side2 = -1; + + 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 = dGeomRayGetFirstContact(RayGeom); + int BackfaceCull = dGeomRayGetBackfaceCull(RayGeom); + 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_collisionODE(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data); + } + else + { + intersect = gim_trimesh_ray_collisionODE(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data); + } + + if(intersect == 0) + { + return 0; + } + + + if(!TriMesh->m_RayCallback || + TriMesh->m_RayCallback(TriMesh, RayGeom, contact_data.m_face_id, contact_data.u , contact_data.v)) + { + dContactGeom* Contact = &( Contacts[ 0 ] ); + 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; + Contact->side1 = contact_data.m_face_id; + Contact->side2 = -1; + return 1; + } + + return 0; +} +#endif // dTRIMESH_GIMPACT + +#endif // dTRIMESH_ENABLED -- cgit v1.2.1