5823b5d77d
Modified both MeshInstance tools as well as importer to use it instead of QuickHull.
208 lines
No EOL
5.4 KiB
C++
208 lines
No EOL
5.4 KiB
C++
#include "vhacdRaycastMesh.h"
|
|
#include <math.h>
|
|
#include <assert.h>
|
|
|
|
namespace RAYCAST_MESH
|
|
{
|
|
|
|
/* a = b - c */
|
|
#define vector(a,b,c) \
|
|
(a)[0] = (b)[0] - (c)[0]; \
|
|
(a)[1] = (b)[1] - (c)[1]; \
|
|
(a)[2] = (b)[2] - (c)[2];
|
|
|
|
#define innerProduct(v,q) \
|
|
((v)[0] * (q)[0] + \
|
|
(v)[1] * (q)[1] + \
|
|
(v)[2] * (q)[2])
|
|
|
|
#define crossProduct(a,b,c) \
|
|
(a)[0] = (b)[1] * (c)[2] - (c)[1] * (b)[2]; \
|
|
(a)[1] = (b)[2] * (c)[0] - (c)[2] * (b)[0]; \
|
|
(a)[2] = (b)[0] * (c)[1] - (c)[0] * (b)[1];
|
|
|
|
|
|
static inline bool rayIntersectsTriangle(const double *p,const double *d,const double *v0,const double *v1,const double *v2,double &t)
|
|
{
|
|
double e1[3],e2[3],h[3],s[3],q[3];
|
|
double a,f,u,v;
|
|
|
|
vector(e1,v1,v0);
|
|
vector(e2,v2,v0);
|
|
crossProduct(h,d,e2);
|
|
a = innerProduct(e1,h);
|
|
|
|
if (a > -0.00001 && a < 0.00001)
|
|
return(false);
|
|
|
|
f = 1/a;
|
|
vector(s,p,v0);
|
|
u = f * (innerProduct(s,h));
|
|
|
|
if (u < 0.0 || u > 1.0)
|
|
return(false);
|
|
|
|
crossProduct(q,s,e1);
|
|
v = f * innerProduct(d,q);
|
|
if (v < 0.0 || u + v > 1.0)
|
|
return(false);
|
|
// at this stage we can compute t to find out where
|
|
// the intersection point is on the line
|
|
t = f * innerProduct(e2,q);
|
|
if (t > 0) // ray intersection
|
|
return(true);
|
|
else // this means that there is a line intersection
|
|
// but not a ray intersection
|
|
return (false);
|
|
}
|
|
|
|
static double getPointDistance(const double *p1, const double *p2)
|
|
{
|
|
double dx = p1[0] - p2[0];
|
|
double dy = p1[1] - p2[1];
|
|
double dz = p1[2] - p2[2];
|
|
return sqrt(dx*dx + dy*dy + dz*dz);
|
|
}
|
|
|
|
class MyRaycastMesh : public VHACD::RaycastMesh
|
|
{
|
|
public:
|
|
|
|
template <class T>
|
|
MyRaycastMesh(uint32_t vcount,
|
|
const T *vertices,
|
|
uint32_t tcount,
|
|
const uint32_t *indices)
|
|
{
|
|
mVcount = vcount;
|
|
mVertices = new double[mVcount * 3];
|
|
for (uint32_t i = 0; i < mVcount; i++)
|
|
{
|
|
mVertices[i * 3 + 0] = vertices[0];
|
|
mVertices[i * 3 + 1] = vertices[1];
|
|
mVertices[i * 3 + 2] = vertices[2];
|
|
vertices += 3;
|
|
}
|
|
mTcount = tcount;
|
|
mIndices = new uint32_t[mTcount * 3];
|
|
for (uint32_t i = 0; i < mTcount; i++)
|
|
{
|
|
mIndices[i * 3 + 0] = indices[0];
|
|
mIndices[i * 3 + 1] = indices[1];
|
|
mIndices[i * 3 + 2] = indices[2];
|
|
indices += 3;
|
|
}
|
|
}
|
|
|
|
|
|
~MyRaycastMesh(void)
|
|
{
|
|
delete[]mVertices;
|
|
delete[]mIndices;
|
|
}
|
|
|
|
virtual void release(void)
|
|
{
|
|
delete this;
|
|
}
|
|
|
|
virtual bool raycast(const double *from, // The starting point of the raycast
|
|
const double *to, // The ending point of the raycast
|
|
const double *closestToPoint, // The point to match the nearest hit location (can just be the 'from' location of no specific point)
|
|
double *hitLocation, // The point where the ray hit nearest to the 'closestToPoint' location
|
|
double *hitDistance) final // The distance the ray traveled to the hit location
|
|
{
|
|
bool ret = false;
|
|
|
|
double dir[3];
|
|
|
|
dir[0] = to[0] - from[0];
|
|
dir[1] = to[1] - from[1];
|
|
dir[2] = to[2] - from[2];
|
|
|
|
double distance = sqrt( dir[0]*dir[0] + dir[1]*dir[1]+dir[2]*dir[2] );
|
|
if ( distance < 0.0000000001f ) return false;
|
|
double recipDistance = 1.0f / distance;
|
|
dir[0]*=recipDistance;
|
|
dir[1]*=recipDistance;
|
|
dir[2]*=recipDistance;
|
|
const uint32_t *indices = mIndices;
|
|
const double *vertices = mVertices;
|
|
double nearestDistance = distance;
|
|
|
|
for (uint32_t tri=0; tri<mTcount; tri++)
|
|
{
|
|
uint32_t i1 = indices[tri*3+0];
|
|
uint32_t i2 = indices[tri*3+1];
|
|
uint32_t i3 = indices[tri*3+2];
|
|
|
|
const double *p1 = &vertices[i1*3];
|
|
const double *p2 = &vertices[i2*3];
|
|
const double *p3 = &vertices[i3*3];
|
|
|
|
double t;
|
|
if ( rayIntersectsTriangle(from,dir,p1,p2,p3,t))
|
|
{
|
|
double hitPos[3];
|
|
|
|
hitPos[0] = from[0] + dir[0] * t;
|
|
hitPos[1] = from[1] + dir[1] * t;
|
|
hitPos[2] = from[2] + dir[2] * t;
|
|
|
|
double pointDistance = getPointDistance(hitPos, closestToPoint);
|
|
|
|
if (pointDistance < nearestDistance )
|
|
{
|
|
nearestDistance = pointDistance;
|
|
if ( hitLocation )
|
|
{
|
|
hitLocation[0] = hitPos[0];
|
|
hitLocation[1] = hitPos[1];
|
|
hitLocation[2] = hitPos[2];
|
|
}
|
|
if ( hitDistance )
|
|
{
|
|
*hitDistance = pointDistance;
|
|
}
|
|
ret = true;
|
|
}
|
|
}
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
uint32_t mVcount;
|
|
double *mVertices;
|
|
uint32_t mTcount;
|
|
uint32_t *mIndices;
|
|
};
|
|
|
|
};
|
|
|
|
|
|
|
|
using namespace RAYCAST_MESH;
|
|
|
|
namespace VHACD
|
|
{
|
|
|
|
RaycastMesh * RaycastMesh::createRaycastMesh(uint32_t vcount, // The number of vertices in the source triangle mesh
|
|
const double *vertices, // The array of vertex positions in the format x1,y1,z1..x2,y2,z2.. etc.
|
|
uint32_t tcount, // The number of triangles in the source triangle mesh
|
|
const uint32_t *indices) // The triangle indices in the format of i1,i2,i3 ... i4,i5,i6, ...
|
|
{
|
|
MyRaycastMesh *m = new MyRaycastMesh(vcount, vertices, tcount, indices);
|
|
return static_cast<RaycastMesh *>(m);
|
|
}
|
|
|
|
RaycastMesh * RaycastMesh::createRaycastMesh(uint32_t vcount, // The number of vertices in the source triangle mesh
|
|
const float *vertices, // The array of vertex positions in the format x1,y1,z1..x2,y2,z2.. etc.
|
|
uint32_t tcount, // The number of triangles in the source triangle mesh
|
|
const uint32_t *indices) // The triangle indices in the format of i1,i2,i3 ... i4,i5,i6, ...
|
|
{
|
|
MyRaycastMesh *m = new MyRaycastMesh(vcount, vertices, tcount, indices);
|
|
return static_cast<RaycastMesh *>(m);
|
|
}
|
|
|
|
|
|
} // end of VHACD namespace
|