1#include "vhacdRaycastMesh.h"
2#include <math.h>
3#include <assert.h>
4
5namespace RAYCAST_MESH
6{
7
8/* a = b - c */
9#define vector(a,b,c) \
10 (a)[0] = (b)[0] - (c)[0]; \
11 (a)[1] = (b)[1] - (c)[1]; \
12 (a)[2] = (b)[2] - (c)[2];
13
14#define innerProduct(v,q) \
15 ((v)[0] * (q)[0] + \
16 (v)[1] * (q)[1] + \
17 (v)[2] * (q)[2])
18
19#define crossProduct(a,b,c) \
20 (a)[0] = (b)[1] * (c)[2] - (c)[1] * (b)[2]; \
21 (a)[1] = (b)[2] * (c)[0] - (c)[2] * (b)[0]; \
22 (a)[2] = (b)[0] * (c)[1] - (c)[0] * (b)[1];
23
24
25static inline bool rayIntersectsTriangle(const double *p,const double *d,const double *v0,const double *v1,const double *v2,double &t)
26{
27 double e1[3],e2[3],h[3],s[3],q[3];
28 double a,f,u,v;
29
30 vector(e1,v1,v0);
31 vector(e2,v2,v0);
32 crossProduct(h,d,e2);
33 a = innerProduct(e1,h);
34
35 if (a > -0.00001 && a < 0.00001)
36 return(false);
37
38 f = 1/a;
39 vector(s,p,v0);
40 u = f * (innerProduct(s,h));
41
42 if (u < 0.0 || u > 1.0)
43 return(false);
44
45 crossProduct(q,s,e1);
46 v = f * innerProduct(d,q);
47 if (v < 0.0 || u + v > 1.0)
48 return(false);
49 // at this stage we can compute t to find out where
50 // the intersection point is on the line
51 t = f * innerProduct(e2,q);
52 if (t > 0) // ray intersection
53 return(true);
54 else // this means that there is a line intersection
55 // but not a ray intersection
56 return (false);
57}
58
59static double getPointDistance(const double *p1, const double *p2)
60{
61 double dx = p1[0] - p2[0];
62 double dy = p1[1] - p2[1];
63 double dz = p1[2] - p2[2];
64 return sqrt(dx*dx + dy*dy + dz*dz);
65}
66
67class MyRaycastMesh : public VHACD::RaycastMesh
68{
69public:
70
71 template <class T>
72 MyRaycastMesh(uint32_t vcount,
73 const T *vertices,
74 uint32_t tcount,
75 const uint32_t *indices)
76 {
77 mVcount = vcount;
78 mVertices = new double[mVcount * 3];
79 for (uint32_t i = 0; i < mVcount; i++)
80 {
81 mVertices[i * 3 + 0] = vertices[0];
82 mVertices[i * 3 + 1] = vertices[1];
83 mVertices[i * 3 + 2] = vertices[2];
84 vertices += 3;
85 }
86 mTcount = tcount;
87 mIndices = new uint32_t[mTcount * 3];
88 for (uint32_t i = 0; i < mTcount; i++)
89 {
90 mIndices[i * 3 + 0] = indices[0];
91 mIndices[i * 3 + 1] = indices[1];
92 mIndices[i * 3 + 2] = indices[2];
93 indices += 3;
94 }
95 }
96
97
98 ~MyRaycastMesh(void)
99 {
100 delete[]mVertices;
101 delete[]mIndices;
102 }
103
104 virtual void release(void)
105 {
106 delete this;
107 }
108
109 virtual bool raycast(const double *from, // The starting point of the raycast
110 const double *to, // The ending point of the raycast
111 const double *closestToPoint, // The point to match the nearest hit location (can just be the 'from' location of no specific point)
112 double *hitLocation, // The point where the ray hit nearest to the 'closestToPoint' location
113 double *hitDistance) final // The distance the ray traveled to the hit location
114 {
115 bool ret = false;
116
117 double dir[3];
118
119 dir[0] = to[0] - from[0];
120 dir[1] = to[1] - from[1];
121 dir[2] = to[2] - from[2];
122
123 double distance = sqrt( dir[0]*dir[0] + dir[1]*dir[1]+dir[2]*dir[2] );
124 if ( distance < 0.0000000001f ) return false;
125 double recipDistance = 1.0f / distance;
126 dir[0]*=recipDistance;
127 dir[1]*=recipDistance;
128 dir[2]*=recipDistance;
129 const uint32_t *indices = mIndices;
130 const double *vertices = mVertices;
131 double nearestDistance = distance;
132
133 for (uint32_t tri=0; tri<mTcount; tri++)
134 {
135 uint32_t i1 = indices[tri*3+0];
136 uint32_t i2 = indices[tri*3+1];
137 uint32_t i3 = indices[tri*3+2];
138
139 const double *p1 = &vertices[i1*3];
140 const double *p2 = &vertices[i2*3];
141 const double *p3 = &vertices[i3*3];
142
143 double t;
144 if ( rayIntersectsTriangle(from,dir,p1,p2,p3,t))
145 {
146 double hitPos[3];
147
148 hitPos[0] = from[0] + dir[0] * t;
149 hitPos[1] = from[1] + dir[1] * t;
150 hitPos[2] = from[2] + dir[2] * t;
151
152 double pointDistance = getPointDistance(hitPos, closestToPoint);
153
154 if (pointDistance < nearestDistance )
155 {
156 nearestDistance = pointDistance;
157 if ( hitLocation )
158 {
159 hitLocation[0] = hitPos[0];
160 hitLocation[1] = hitPos[1];
161 hitLocation[2] = hitPos[2];
162 }
163 if ( hitDistance )
164 {
165 *hitDistance = pointDistance;
166 }
167 ret = true;
168 }
169 }
170 }
171 return ret;
172 }
173
174 uint32_t mVcount;
175 double *mVertices;
176 uint32_t mTcount;
177 uint32_t *mIndices;
178};
179
180};
181
182
183
184using namespace RAYCAST_MESH;
185
186namespace VHACD
187{
188
189 RaycastMesh * RaycastMesh::createRaycastMesh(uint32_t vcount, // The number of vertices in the source triangle mesh
190 const double *vertices, // The array of vertex positions in the format x1,y1,z1..x2,y2,z2.. etc.
191 uint32_t tcount, // The number of triangles in the source triangle mesh
192 const uint32_t *indices) // The triangle indices in the format of i1,i2,i3 ... i4,i5,i6, ...
193 {
194 MyRaycastMesh *m = new MyRaycastMesh(vcount, vertices, tcount, indices);
195 return static_cast<RaycastMesh *>(m);
196 }
197
198 RaycastMesh * RaycastMesh::createRaycastMesh(uint32_t vcount, // The number of vertices in the source triangle mesh
199 const float *vertices, // The array of vertex positions in the format x1,y1,z1..x2,y2,z2.. etc.
200 uint32_t tcount, // The number of triangles in the source triangle mesh
201 const uint32_t *indices) // The triangle indices in the format of i1,i2,i3 ... i4,i5,i6, ...
202 {
203 MyRaycastMesh *m = new MyRaycastMesh(vcount, vertices, tcount, indices);
204 return static_cast<RaycastMesh *>(m);
205 }
206
207
208} // end of VHACD namespace