aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src/collision_trimesh_ray.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/ode/src/collision_trimesh_ray.cpp')
-rw-r--r--libraries/ode-0.9/ode/src/collision_trimesh_ray.cpp198
1 files changed, 198 insertions, 0 deletions
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 @@
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
30#if dTRIMESH_ENABLED
31
32#include "collision_util.h"
33
34#define TRIMESH_INTERNAL
35#include "collision_trimesh_internal.h"
36
37#if dTRIMESH_OPCODE
38int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride){
39 dIASSERT (Stride >= (int)sizeof(dContactGeom));
40 dIASSERT (g1->type == dTriMeshClass);
41 dIASSERT (RayGeom->type == dRayClass);
42 dIASSERT ((Flags & NUMC_MASK) >= 1);
43
44 dxTriMesh* TriMesh = (dxTriMesh*)g1;
45
46 const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
47 const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
48
49 RayCollider& Collider = TriMesh->_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 = TriMesh->Faces.GetNbFaces();
79 }
80
81 if (TriCount == 0) {
82 return 0;
83 }
84
85 const CollisionFace* Faces = TriMesh->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 // No sense to save on single type conversion in algorithm of this size.
103 // If there would be a custom typedef for distance type it could be used
104 // instead of dReal. However using float directly is the loss of abstraction
105 // and possible loss of precision in future.
106 /*float*/ dReal T = Faces[i].mDistance;
107 Contact->pos[0] = Origin[0] + (Direction[0] * T);
108 Contact->pos[1] = Origin[1] + (Direction[1] * T);
109 Contact->pos[2] = Origin[2] + (Direction[2] * T);
110 Contact->pos[3] = REAL(0.0);
111
112 dVector3 vu;
113 vu[0] = dv[1][0] - dv[0][0];
114 vu[1] = dv[1][1] - dv[0][1];
115 vu[2] = dv[1][2] - dv[0][2];
116 vu[3] = REAL(0.0);
117
118 dVector3 vv;
119 vv[0] = dv[2][0] - dv[0][0];
120 vv[1] = dv[2][1] - dv[0][1];
121 vv[2] = dv[2][2] - dv[0][2];
122 vv[3] = REAL(0.0);
123
124 dCROSS(Contact->normal, =, vv, vu); // Reversed
125
126 dNormalize3(Contact->normal);
127
128 Contact->depth = T;
129 Contact->g1 = TriMesh;
130 Contact->g2 = RayGeom;
131
132 OutTriCount++;
133
134 // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
135 if (OutTriCount >= (Flags & NUMC_MASK)) {
136 break;
137 }
138 }
139 }
140 return OutTriCount;
141}
142#endif // dTRIMESH_OPCODE
143
144#if dTRIMESH_GIMPACT
145int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride)
146{
147 dIASSERT (Stride >= (int)sizeof(dContactGeom));
148 dIASSERT (g1->type == dTriMeshClass);
149 dIASSERT (RayGeom->type == dRayClass);
150 dIASSERT ((Flags & NUMC_MASK) >= 1);
151
152 dxTriMesh* TriMesh = (dxTriMesh*)g1;
153
154 dReal Length = dGeomRayGetLength(RayGeom);
155 int FirstContact, BackfaceCull;
156 dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull);
157 int ClosestHit = dGeomRayGetClosestHit(RayGeom);
158 dVector3 Origin, Direction;
159 dGeomRayGet(RayGeom, Origin, Direction);
160
161 char intersect=0;
162 GIM_TRIANGLE_RAY_CONTACT_DATA contact_data;
163
164 if(ClosestHit)
165 {
166 intersect = gim_trimesh_ray_closest_collision(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data);
167 }
168 else
169 {
170 intersect = gim_trimesh_ray_collision(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data);
171 }
172
173 if(intersect == 0)
174 {
175 return 0;
176 }
177
178 int OutTriCount = 0;
179
180 if(!TriMesh->RayCallback ||
181 TriMesh->RayCallback(TriMesh, RayGeom, contact_data.m_face_id, contact_data.u , contact_data.v))
182 {
183 dContactGeom* Contact = SAFECONTACT(Flags, Contacts, (OutTriCount-1), Stride);
184 VEC_COPY(Contact->pos,contact_data.m_point);
185 VEC_COPY(Contact->normal,contact_data.m_normal);
186 Contact->depth = contact_data.tparam;
187 Contact->g1 = TriMesh;
188 Contact->g2 = RayGeom;
189
190 OutTriCount = 1;
191 }
192
193 return OutTriCount;
194}
195#endif // dTRIMESH_GIMPACT
196
197#endif // dTRIMESH_ENABLED
198
generated by cgit v1.1 at 2024-09-13 11:39:38 +0000