dFloat dPolygonRayCast (const dVector& l0, const dVector& l1, int indexCount, const dFloat* const vertex, int strideInBytes, const int* const indices)
{
int stride = strideInBytes / sizeof (dFloat);
dBigVector line0 (l0);
dBigVector line1(l1);
dBigVector segment (line1 - line0);
dBigVector normal (dPolygonNormal (indexCount, vertex, strideInBytes, indices));
double den = normal % segment;
if (dAbs(den) < 1.0e-6) {
return 1.2f;
}
double sign = (den < 0.0f) ? 1.0f : -1.0f;
int index = indices[indexCount - 1] * stride;
dBigVector v0 (vertex[index], vertex[index + 1], vertex[index + 2], 0.0f);
dBigVector p0v0 (v0 - line0);
for (int i = 0; i < indexCount; i ++) {
index = indices[i] * stride;
dBigVector v1 (vertex[index], vertex[index + 1], vertex[index + 2], 0.0f);
dBigVector p0v1 (v1 - line0);
double alpha = sign * ((p0v1 * p0v0) % segment);
if (alpha < 1.0e-3f) {
return 1.2f;
}
p0v0 = p0v1;
}
double t = - ((line0 - v0) % normal) / den;
if ((t < 0.0f) || (t > 1.0f)) {
return 1.2f;
}
return dFloat (t);
}
dgFloat32 FastRayTest::PolygonIntersect (const dgVector& normal, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount) const
{
_ASSERTE (m_p0.m_w == m_p1.m_w);
dgFloat32 dist = normal % m_diff;
if (dist < m_dirError) {
dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32));
dgVector v0 (&polygon[indexArray[indexCount - 1] * stride]);
dgVector p0v0 (v0 - m_p0);
dgFloat32 tOut = normal % p0v0;
// this only work for convex polygons and for single side faces
// walk the polygon around the edges and calculate the volume
if ((tOut < dgFloat32 (0.0f)) && (tOut > dist)) {
for (dgInt32 i = 0; i < indexCount; i ++) {
dgInt32 i2 = indexArray[i] * stride;
dgVector v1 (&polygon[i2]);
dgVector p0v1 (v1 - m_p0);
// calculate the volume formed by the line and the edge of the polygon
dgFloat32 alpha = (m_diff * p0v1) % p0v0;
// if a least one volume is negative it mean the line cross the polygon outside this edge and do not hit the face
if (alpha < DG_RAY_TOL_ERROR) {
return 1.2f;
}
p0v0 = p0v1;
}
//the line is to the left of all the polygon edges,
//then the intersection is the point we the line intersect the plane of the polygon
tOut = tOut / dist;
_ASSERTE (tOut >= dgFloat32 (0.0f));
_ASSERTE (tOut <= dgFloat32 (1.0f));
return tOut;
}
}
return dgFloat32 (1.2f);
}
float dgFastRayTest::PolygonIntersect (const dgVector& normal, const float* const polygon, int32_t strideInBytes, const int32_t* const indexArray, int32_t indexCount) const
{
HACD_ASSERT (m_p0.m_w == m_p1.m_w);
#ifndef __USE_DOUBLE_PRECISION__
float unrealible = float (1.0e10f);
#endif
float dist = normal % m_diff;
if (dist < m_dirError) {
int32_t stride = int32_t (strideInBytes / sizeof (float));
dgVector v0 (&polygon[indexArray[indexCount - 1] * stride]);
dgVector p0v0 (v0 - m_p0);
float tOut = normal % p0v0;
// this only work for convex polygons and for single side faces
// walk the polygon around the edges and calculate the volume
if ((tOut < float (0.0f)) && (tOut > dist)) {
for (int32_t i = 0; i < indexCount; i ++) {
int32_t i2 = indexArray[i] * stride;
dgVector v1 (&polygon[i2]);
dgVector p0v1 (v1 - m_p0);
// calculate the volume formed by the line and the edge of the polygon
float alpha = (m_diff * p0v1) % p0v0;
// if a least one volume is negative it mean the line cross the polygon outside this edge and do not hit the face
if (alpha < DG_RAY_TOL_ERROR) {
#ifdef __USE_DOUBLE_PRECISION__
return 1.2f;
#else
unrealible = alpha;
break;
#endif
}
p0v0 = p0v1;
}
#ifndef __USE_DOUBLE_PRECISION__
if ((unrealible < float (0.0f)) && (unrealible > (DG_RAY_TOL_ERROR * float (10.0f)))) {
// the edge is too close to an edge float is not reliable, do the calculation with double
dgBigVector v0_ (v0);
dgBigVector m_p0_ (m_p0);
//dgBigVector m_p1_ (m_p1);
dgBigVector p0v0_ (v0_ - m_p0_);
dgBigVector normal_ (normal);
dgBigVector diff_ (m_diff);
double tOut_ = normal_ % p0v0_;
//double dist_ = normal_ % diff_;
if ((tOut < double (0.0f)) && (tOut > dist)) {
for (int32_t i = 0; i < indexCount; i ++) {
int32_t i2 = indexArray[i] * stride;
dgBigVector v1 (&polygon[i2]);
dgBigVector p0v1_ (v1 - m_p0_);
// calculate the volume formed by the line and the edge of the polygon
double alpha = (diff_ * p0v1_) % p0v0_;
// if a least one volume is negative it mean the line cross the polygon outside this edge and do not hit the face
if (alpha < DG_RAY_TOL_ERROR) {
return 1.2f;
}
p0v0_ = p0v1_;
}
tOut = float (tOut_);
}
}
#endif
//the line is to the left of all the polygon edges,
//then the intersection is the point we the line intersect the plane of the polygon
tOut = tOut / dist;
HACD_ASSERT (tOut >= float (0.0f));
HACD_ASSERT (tOut <= float (1.0f));
return tOut;
}
}
return float (1.2f);
}
dgFloat32 FastRayTest::PolygonIntersectSimd (const dgVector& normal, const dgFloat32* const polygon, dgInt32 strideInBytes, const dgInt32* const indexArray, dgInt32 indexCount) const
{
_ASSERTE (m_p0.m_w == m_p1.m_w);
simd_128 normal1 ((simd_128&)normal & simd_128 (-1, -1, -1, 0));
simd_128 dist (((simd_128&)normal1).DotProduct((simd_128&)m_diff));
if ((dist < simd_128(m_dirError)).GetSignMask() & 1) {
dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32));
simd_128 v0 (&polygon[indexArray[indexCount - 1] * stride]);
simd_128 p0v0 (v0 - (simd_128&)m_p0);
simd_128 tOut = normal1.DotProduct(p0v0);
simd_128 zero (dgFloat32 (0.0f));
// this only work for convex polygons and for single side faces
// walk the polygon around the edges and calculate the volume
simd_128 test ((tOut < zero) & (tOut > dist));
if (test.GetSignMask()) {
dgInt32 i3 = indexCount - 1;
dgInt32 i2 = indexCount - 2;
dgInt32 i1 = indexCount - 3;
dgInt32 i0 = (indexCount > 3) ? indexCount - 4 : 2;
for (dgInt32 i4 = 0; i4 < indexCount; i4 += 4) {
simd_128 v0 (&polygon[indexArray[i0] * stride]);
simd_128 v1 (&polygon[indexArray[i1] * stride]);
simd_128 v2 (&polygon[indexArray[i2] * stride]);
simd_128 v3 (&polygon[indexArray[i3] * stride]);
simd_128 v4 (&polygon[indexArray[i4] * stride]);
simd_128 p0v0 (v0 - (simd_128&)m_p0);
simd_128 p0v1 (v1 - (simd_128&)m_p0);
simd_128 p0v2 (v2 - (simd_128&)m_p0);
simd_128 p0v3 (v3 - (simd_128&)m_p0);
simd_128 p0v4 (v4 - (simd_128&)m_p0);
simd_128 p0v0_x;
simd_128 p0v0_y;
simd_128 p0v0_z;
simd_128 p0v1_x;
simd_128 p0v1_y;
simd_128 p0v1_z;
Transpose4x4Simd_128 (p0v0_x, p0v0_y, p0v0_z, test, p0v0, p0v1, p0v2, p0v3);
Transpose4x4Simd_128 (p0v1_x, p0v1_y, p0v1_z, test, p0v1, p0v2, p0v3, p0v4);
simd_128 volume = (m_ray_yyyy * p0v1_z - m_ray_zzzz * p0v1_y) * p0v0_x +
(m_ray_zzzz * p0v1_x - m_ray_xxxx * p0v1_z) * p0v0_y +
(m_ray_xxxx * p0v1_y - m_ray_yyyy * p0v1_x) * p0v0_z;
// if a least one volume is negative it mean the line cross the polygon outside this edge and do not hit the face
if ((volume < (simd_128&)m_tolerance).GetSignMask()) {
return 1.2f;
}
i3 = i4 + 3;
i2 = i4 + 2;
i1 = i4 + 1;
i0 = i4 + 0;
}
//the line is to the left of all the polygon edges,
//then the intersection is the point we the line intersect the plane of the polygon
tOut = tOut / dist;
dgFloat32 ret;
tOut.StoreScalar(&ret);
_ASSERTE (ret >= dgFloat32 (0.0f));
_ASSERTE (ret <= dgFloat32 (1.0f));
return ret;
}
}
return dgFloat32 (1.2f);
}
