mirror of
git://opensimulator.org/git/opensim-libs
synced 2026-08-14 00:58:00 +00:00
ubode change iceplane rotation; replace more tabs
This commit is contained in:
UbitUmarov
@@ -9,29 +9,29 @@
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/**
|
||||
* 4x4 matrix.
|
||||
* DirectX-compliant, ie row-column order, ie m[Row][Col].
|
||||
* Same as:
|
||||
* m11 m12 m13 m14 first row.
|
||||
* m21 m22 m23 m24 second row.
|
||||
* m31 m32 m33 m34 third row.
|
||||
* m41 m42 m43 m44 fourth row.
|
||||
* Translation is (m41, m42, m43), (m14, m24, m34, m44) = (0, 0, 0, 1).
|
||||
* Stored in memory as m11 m12 m13 m14 m21...
|
||||
*
|
||||
* Multiplication rules:
|
||||
*
|
||||
* [x'y'z'1] = [xyz1][M]
|
||||
*
|
||||
* x' = x*m11 + y*m21 + z*m31 + m41
|
||||
* y' = x*m12 + y*m22 + z*m32 + m42
|
||||
* z' = x*m13 + y*m23 + z*m33 + m43
|
||||
* 1' = 0 + 0 + 0 + m44
|
||||
*
|
||||
* \class Matrix4x4
|
||||
* \author Pierre Terdiman
|
||||
* \version 1.0
|
||||
*/
|
||||
* 4x4 matrix.
|
||||
* DirectX-compliant, ie row-column order, ie m[Row][Col].
|
||||
* Same as:
|
||||
* m11 m12 m13 m14 first row.
|
||||
* m21 m22 m23 m24 second row.
|
||||
* m31 m32 m33 m34 third row.
|
||||
* m41 m42 m43 m44 fourth row.
|
||||
* Translation is (m41, m42, m43), (m14, m24, m34, m44) = (0, 0, 0, 1).
|
||||
* Stored in memory as m11 m12 m13 m14 m21...
|
||||
*
|
||||
* Multiplication rules:
|
||||
*
|
||||
* [x'y'z'1] = [xyz1][M]
|
||||
*
|
||||
* x' = x*m11 + y*m21 + z*m31 + m41
|
||||
* y' = x*m12 + y*m22 + z*m32 + m42
|
||||
* z' = x*m13 + y*m23 + z*m33 + m43
|
||||
* 1' = 0 + 0 + 0 + m44
|
||||
*
|
||||
* \class Matrix4x4
|
||||
* \author Pierre Terdiman
|
||||
* \version 1.0
|
||||
*/
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
@@ -55,7 +55,7 @@ using namespace IceMaths;
|
||||
#if defined(__AVX__)
|
||||
ICEMATHS_API void IceMaths::InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src)
|
||||
{
|
||||
__m128 zero, s3, t0, t1, m0, m1, m2, m3,k;
|
||||
__m128 zero, s3, t0, t1, m0, m1, m2, m3, k;
|
||||
t0 = _mm_loadu_ps(src.m[0]);
|
||||
t1 = _mm_loadu_ps(src.m[1]);
|
||||
s3 = _mm_loadu_ps(src.m[3]);
|
||||
@@ -91,20 +91,20 @@ ICEMATHS_API void IceMaths::InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src
|
||||
#else
|
||||
ICEMATHS_API void IceMaths::InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src)
|
||||
{
|
||||
dest.m[0][0] = src.m[0][0];
|
||||
dest.m[1][0] = src.m[0][1];
|
||||
dest.m[2][0] = src.m[0][2];
|
||||
dest.m[3][0] = -(src.m[3][0]*src.m[0][0] + src.m[3][1]*src.m[0][1] + src.m[3][2]*src.m[0][2]);
|
||||
dest.m[0][0] = src.m[0][0];
|
||||
dest.m[1][0] = src.m[0][1];
|
||||
dest.m[2][0] = src.m[0][2];
|
||||
dest.m[3][0] = -(src.m[3][0] * src.m[0][0] + src.m[3][1] * src.m[0][1] + src.m[3][2] * src.m[0][2]);
|
||||
|
||||
dest.m[0][1] = src.m[1][0];
|
||||
dest.m[1][1] = src.m[1][1];
|
||||
dest.m[2][1] = src.m[1][2];
|
||||
dest.m[3][1] = -(src.m[3][0]*src.m[1][0] + src.m[3][1]*src.m[1][1] + src.m[3][2]*src.m[1][2]);
|
||||
dest.m[0][1] = src.m[1][0];
|
||||
dest.m[1][1] = src.m[1][1];
|
||||
dest.m[2][1] = src.m[1][2];
|
||||
dest.m[3][1] = -(src.m[3][0] * src.m[1][0] + src.m[3][1] * src.m[1][1] + src.m[3][2] * src.m[1][2]);
|
||||
|
||||
dest.m[0][2] = src.m[2][0];
|
||||
dest.m[1][2] = src.m[2][1];
|
||||
dest.m[2][2] = src.m[2][2];
|
||||
dest.m[3][2] = -(src.m[3][0]*src.m[2][0] + src.m[3][1]*src.m[2][1] + src.m[3][2]*src.m[2][2]);
|
||||
dest.m[0][2] = src.m[2][0];
|
||||
dest.m[1][2] = src.m[2][1];
|
||||
dest.m[2][2] = src.m[2][2];
|
||||
dest.m[3][2] = -(src.m[3][0] * src.m[2][0] + src.m[3][1] * src.m[2][1] + src.m[3][2] * src.m[2][2]);
|
||||
|
||||
dest.m[0][3] = 0.0f;
|
||||
dest.m[1][3] = 0.0f;
|
||||
@@ -117,12 +117,12 @@ ICEMATHS_API void IceMaths::InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
float Matrix4x4::CoFactor(udword row, udword col) const
|
||||
{
|
||||
return (( m[(row+1)&3][(col+1)&3]*m[(row+2)&3][(col+2)&3]*m[(row+3)&3][(col+3)&3] +
|
||||
m[(row+1)&3][(col+2)&3]*m[(row+2)&3][(col+3)&3]*m[(row+3)&3][(col+1)&3] +
|
||||
m[(row+1)&3][(col+3)&3]*m[(row+2)&3][(col+1)&3]*m[(row+3)&3][(col+2)&3])
|
||||
- (m[(row+3)&3][(col+1)&3]*m[(row+2)&3][(col+2)&3]*m[(row+1)&3][(col+3)&3] +
|
||||
m[(row+3)&3][(col+2)&3]*m[(row+2)&3][(col+3)&3]*m[(row+1)&3][(col+1)&3] +
|
||||
m[(row+3)&3][(col+3)&3]*m[(row+2)&3][(col+1)&3]*m[(row+1)&3][(col+2)&3])) * ((row + col) & 1 ? -1.0f : +1.0f);
|
||||
return ((m[(row + 1) & 3][(col + 1) & 3] * m[(row + 2) & 3][(col + 2) & 3] * m[(row + 3) & 3][(col + 3) & 3] +
|
||||
m[(row + 1) & 3][(col + 2) & 3] * m[(row + 2) & 3][(col + 3) & 3] * m[(row + 3) & 3][(col + 1) & 3] +
|
||||
m[(row + 1) & 3][(col + 3) & 3] * m[(row + 2) & 3][(col + 1) & 3] * m[(row + 3) & 3][(col + 2) & 3])
|
||||
- (m[(row + 3) & 3][(col + 1) & 3] * m[(row + 2) & 3][(col + 2) & 3] * m[(row + 1) & 3][(col + 3) & 3] +
|
||||
m[(row + 3) & 3][(col + 2) & 3] * m[(row + 2) & 3][(col + 3) & 3] * m[(row + 1) & 3][(col + 1) & 3] +
|
||||
m[(row + 3) & 3][(col + 3) & 3] * m[(row + 2) & 3][(col + 1) & 3] * m[(row + 1) & 3][(col + 2) & 3])) * ((row + col) & 1 ? -1.0f : +1.0f);
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
@@ -130,10 +130,10 @@ float Matrix4x4::CoFactor(udword row, udword col) const
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
float Matrix4x4::Determinant() const
|
||||
{
|
||||
return m[0][0] * CoFactor(0, 0) +
|
||||
m[0][1] * CoFactor(0, 1) +
|
||||
m[0][2] * CoFactor(0, 2) +
|
||||
m[0][3] * CoFactor(0, 3);
|
||||
return m[0][0] * CoFactor(0, 0) +
|
||||
m[0][1] * CoFactor(0, 1) +
|
||||
m[0][2] * CoFactor(0, 2) +
|
||||
m[0][3] * CoFactor(0, 3);
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
@@ -141,33 +141,33 @@ float Matrix4x4::Determinant() const
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
Matrix4x4& Matrix4x4::Invert()
|
||||
{
|
||||
float Det = Determinant();
|
||||
Matrix4x4 Temp;
|
||||
float Det = Determinant();
|
||||
Matrix4x4 Temp;
|
||||
|
||||
if(fabsf(Det) < MATRIX4X4_EPSILON)
|
||||
return *this; // The matrix is not invertible! Singular case!
|
||||
if (fabsf(Det) < MATRIX4X4_EPSILON)
|
||||
return *this; // The matrix is not invertible! Singular case!
|
||||
|
||||
float IDet = 1.0f / Det;
|
||||
float IDet = 1.0f / Det;
|
||||
|
||||
Temp.m[0][0] = CoFactor(0,0) * IDet;
|
||||
Temp.m[1][0] = CoFactor(0,1) * IDet;
|
||||
Temp.m[2][0] = CoFactor(0,2) * IDet;
|
||||
Temp.m[3][0] = CoFactor(0,3) * IDet;
|
||||
Temp.m[0][1] = CoFactor(1,0) * IDet;
|
||||
Temp.m[1][1] = CoFactor(1,1) * IDet;
|
||||
Temp.m[2][1] = CoFactor(1,2) * IDet;
|
||||
Temp.m[3][1] = CoFactor(1,3) * IDet;
|
||||
Temp.m[0][2] = CoFactor(2,0) * IDet;
|
||||
Temp.m[1][2] = CoFactor(2,1) * IDet;
|
||||
Temp.m[2][2] = CoFactor(2,2) * IDet;
|
||||
Temp.m[3][2] = CoFactor(2,3) * IDet;
|
||||
Temp.m[0][3] = CoFactor(3,0) * IDet;
|
||||
Temp.m[1][3] = CoFactor(3,1) * IDet;
|
||||
Temp.m[2][3] = CoFactor(3,2) * IDet;
|
||||
Temp.m[3][3] = CoFactor(3,3) * IDet;
|
||||
Temp.m[0][0] = CoFactor(0, 0) * IDet;
|
||||
Temp.m[1][0] = CoFactor(0, 1) * IDet;
|
||||
Temp.m[2][0] = CoFactor(0, 2) * IDet;
|
||||
Temp.m[3][0] = CoFactor(0, 3) * IDet;
|
||||
Temp.m[0][1] = CoFactor(1, 0) * IDet;
|
||||
Temp.m[1][1] = CoFactor(1, 1) * IDet;
|
||||
Temp.m[2][1] = CoFactor(1, 2) * IDet;
|
||||
Temp.m[3][1] = CoFactor(1, 3) * IDet;
|
||||
Temp.m[0][2] = CoFactor(2, 0) * IDet;
|
||||
Temp.m[1][2] = CoFactor(2, 1) * IDet;
|
||||
Temp.m[2][2] = CoFactor(2, 2) * IDet;
|
||||
Temp.m[3][2] = CoFactor(2, 3) * IDet;
|
||||
Temp.m[0][3] = CoFactor(3, 0) * IDet;
|
||||
Temp.m[1][3] = CoFactor(3, 1) * IDet;
|
||||
Temp.m[2][3] = CoFactor(3, 2) * IDet;
|
||||
Temp.m[3][3] = CoFactor(3, 3) * IDet;
|
||||
|
||||
*this = Temp;
|
||||
*this = Temp;
|
||||
|
||||
return *this;
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
@@ -88,7 +88,7 @@ public:
|
||||
//! Sets a column.
|
||||
inline_ void SetCol(const udword c, const HPoint& p) { m[0][c] = p.x; m[1][c] = p.y; m[2][c] = p.z; m[3][c] = p.w; }
|
||||
//! Sets a column.
|
||||
inline_ void SetCol(const udword c, const Point& p) { m[0][c] = p.x; m[1][c] = p.y; m[2][c] = p.z; m[3][c] = (c != 3) ? 0.0f : 1.0f; }
|
||||
inline_ void SetCol(const udword c, const Point& p) { m[0][c] = p.x; m[1][c] = p.y; m[2][c] = p.z; m[3][c] = (c != 3) ? 0.0f : 1.0f; }
|
||||
|
||||
// Translation
|
||||
//! Returns the translation part of the matrix.
|
||||
|
||||
@@ -86,11 +86,10 @@
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
inline_ void TransformPlane(Plane& transformed, const Plane& plane, const Matrix4x4& transform)
|
||||
{
|
||||
// Rotate the normal using the rotation part of the 4x4 matrix
|
||||
transformed.n = plane.n * Matrix3x3(transform);
|
||||
|
||||
TransformPoint3x3(transformed.n, plane.n, transform);
|
||||
// Compute new d
|
||||
transformed.d = plane.d - (Point(transform.GetTrans())|transformed.n);
|
||||
float dot = transform.m[3][0] * transformed.n.x + transform.m[3][1] * transformed.n.y + transform.m[3][2] * transformed.n.z;
|
||||
transformed.d = plane.d - dot;
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
@@ -101,13 +100,14 @@
|
||||
* \warning the plane normal must be unit-length
|
||||
*/
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
inline_ void TransformPlane(Plane& plane, const Matrix4x4& transform)
|
||||
/* inline_ void TransformPlane(Plane& plane, const Matrix4x4& transform)
|
||||
{
|
||||
// Rotate the normal using the rotation part of the 4x4 matrix
|
||||
plane.n *= Matrix3x3(transform);
|
||||
|
||||
// Compute new d
|
||||
plane.d -= Point(transform.GetTrans())|plane.n;
|
||||
float dot = transform.m[3][0] * plane.n.x + transform.m[3][1] * plane.n.y + transform.m[3][2] * plane.n.z;
|
||||
plane.d -= dot;
|
||||
}
|
||||
|
||||
*/
|
||||
#endif // __ICEPLANE_H__
|
||||
|
||||
@@ -11,34 +11,28 @@
|
||||
* \param extents [in] AABB extents
|
||||
* \return true on overlap
|
||||
*/
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
inline_ BOOL RayCollider::SegmentAABBOverlap(const Point& center, const Point& extents)
|
||||
{
|
||||
// Stats
|
||||
mNbRayBVTests++;
|
||||
// Stats
|
||||
mNbRayBVTests++;
|
||||
|
||||
float Dx = mData2.x - center.x;
|
||||
if(fabsf(Dx) > extents.x + mFDir.x)
|
||||
return FALSE;
|
||||
float Dy = mData2.y - center.y;
|
||||
if(fabsf(Dy) > extents.y + mFDir.y)
|
||||
return FALSE;
|
||||
float Dz = mData2.z - center.z;
|
||||
if(fabsf(Dz) > extents.z + mFDir.z)
|
||||
return FALSE;
|
||||
float Dx = mData2.x - center.x;
|
||||
if (fabsf(Dx) > extents.x + mFDir.x) return FALSE;
|
||||
float Dy = mData2.y - center.y;
|
||||
if (fabsf(Dy) > extents.y + mFDir.y) return FALSE;
|
||||
float Dz = mData2.z - center.z;
|
||||
if (fabsf(Dz) > extents.z + mFDir.z) return FALSE;
|
||||
|
||||
float f;
|
||||
f = mData.y * Dz - mData.z * Dy;
|
||||
if(fabsf(f) > extents.y * mFDir.z + extents.z * mFDir.y)
|
||||
return FALSE;
|
||||
f = mData.z * Dx - mData.x * Dz;
|
||||
if(fabsf(f) > extents.x * mFDir.z + extents.z * mFDir.x)
|
||||
return FALSE;
|
||||
f = mData.x * Dy - mData.y * Dx;
|
||||
if(fabsf(f) > extents.x * mFDir.y + extents.y * mFDir.x)
|
||||
return FALSE;
|
||||
float f;
|
||||
f = mData.y * Dz - mData.z * Dy;
|
||||
if (fabsf(f) > extents.y * mFDir.z + extents.z * mFDir.y) return FALSE;
|
||||
f = mData.z * Dx - mData.x * Dz;
|
||||
if (fabsf(f) > extents.x * mFDir.z + extents.z * mFDir.x) return FALSE;
|
||||
f = mData.x * Dy - mData.y * Dx;
|
||||
if (fabsf(f) > extents.x * mFDir.y + extents.y * mFDir.x) return FALSE;
|
||||
|
||||
return TRUE;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
@@ -51,29 +45,23 @@ inline_ BOOL RayCollider::SegmentAABBOverlap(const Point& center, const Point& e
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
inline_ BOOL RayCollider::RayAABBOverlap(const Point& center, const Point& extents)
|
||||
{
|
||||
// Stats
|
||||
mNbRayBVTests++;
|
||||
// Stats
|
||||
mNbRayBVTests++;
|
||||
|
||||
float Dx = mOrigin.x - center.x;
|
||||
if(fabsf(Dx) > extents.x && Dx * mDir.x >= 0.0f)
|
||||
return FALSE;
|
||||
float Dy = mOrigin.y - center.y;
|
||||
if(fabsf(Dy) > extents.y && Dy * mDir.y >= 0.0f)
|
||||
return FALSE;
|
||||
float Dz = mOrigin.z - center.z;
|
||||
if(fabsf(Dz) > extents.z && Dz * mDir.z >= 0.0f)
|
||||
return FALSE;
|
||||
float Dx = mOrigin.x - center.x;
|
||||
if (fabsf(Dx) > extents.x && Dx * mDir.x >= 0.0f) return FALSE;
|
||||
float Dy = mOrigin.y - center.y;
|
||||
if (fabsf(Dy) > extents.y && Dy * mDir.y >= 0.0f) return FALSE;
|
||||
float Dz = mOrigin.z - center.z;
|
||||
if (fabsf(Dz) > extents.z && Dz * mDir.z >= 0.0f) return FALSE;
|
||||
|
||||
float f;
|
||||
f = mDir.y * Dz - mDir.z * Dy;
|
||||
if(fabsf(f) > extents.y * mFDir.z + extents.z * mFDir.y)
|
||||
return FALSE;
|
||||
f = mDir.z * Dx - mDir.x * Dz;
|
||||
if(fabsf(f) > extents.x * mFDir.z + extents.z * mFDir.x)
|
||||
return FALSE;
|
||||
f = mDir.x * Dy - mDir.y * Dx;
|
||||
if(fabsf(f) > extents.x * mFDir.y + extents.y * mFDir.x)
|
||||
return FALSE;
|
||||
float f;
|
||||
f = mDir.y * Dz - mDir.z * Dy;
|
||||
if (fabsf(f) > extents.y * mFDir.z + extents.z * mFDir.y) return FALSE;
|
||||
f = mDir.z * Dx - mDir.x * Dz;
|
||||
if (fabsf(f) > extents.x * mFDir.z + extents.z * mFDir.x) return FALSE;
|
||||
f = mDir.x * Dy - mDir.y * Dx;
|
||||
if (fabsf(f) > extents.x * mFDir.y + extents.y * mFDir.x) return FALSE;
|
||||
|
||||
return TRUE;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
@@ -1,116 +1,114 @@
|
||||
|
||||
//! This macro quickly finds the min & max values among 3 variables
|
||||
#define FINDMINMAX(x0, x1, x2, min, max) \
|
||||
min = max = x0; \
|
||||
if(x1<min) min=x1; \
|
||||
if(x1>max) max=x1; \
|
||||
if(x2<min) min=x2; \
|
||||
if(x2>max) max=x2;
|
||||
#define FINDMINMAX(x0, x1, x2, min, max) \
|
||||
min = max = x0; \
|
||||
if(x1<min) min=x1; \
|
||||
if(x1>max) max=x1; \
|
||||
if(x2<min) min=x2; \
|
||||
if(x2>max) max=x2;
|
||||
|
||||
//! TO BE DOCUMENTED
|
||||
inline_ BOOL planeBoxOverlap(const Point& normal, const float d, const Point& maxbox)
|
||||
{
|
||||
Point vmin, vmax;
|
||||
for(int q = 0; q <= 2; q++)
|
||||
{
|
||||
if(normal[q] > 0.0f)
|
||||
Point vmin, vmax;
|
||||
for (int q = 0; q <= 2; q++)
|
||||
{
|
||||
if (normal[q] > 0.0f)
|
||||
{
|
||||
vmin[q] = -maxbox[q];
|
||||
vmax[q] = maxbox[q];
|
||||
}
|
||||
else
|
||||
else
|
||||
{
|
||||
vmin[q] = maxbox[q];
|
||||
vmax[q] = -maxbox[q];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if((normal | vmin) + d > 0.0f)
|
||||
return FALSE;
|
||||
if((normal | vmax) + d >= 0.0f)
|
||||
return TRUE;
|
||||
if ((normal | vmin) + d > 0.0f) return FALSE;
|
||||
if ((normal | vmax) + d >= 0.0f) return TRUE;
|
||||
|
||||
return FALSE;
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
//! TO BE DOCUMENTED
|
||||
#define AXISTEST_X01(a, b, fa, fb) \
|
||||
min = a*v0.y - b*v0.z; \
|
||||
max = a*v2.y - b*v2.z; \
|
||||
if(min>max) {const float tmp=max; max=min; min=tmp; } \
|
||||
rad = fa * extents.y + fb * extents.z; \
|
||||
#define AXISTEST_X01(a, b, fa, fb) \
|
||||
min = a*v0.y - b*v0.z; \
|
||||
max = a*v2.y - b*v2.z; \
|
||||
if(min>max) {const float tmp=max; max=min; min=tmp; } \
|
||||
rad = fa * extents.y + fb * extents.z; \
|
||||
if(min>rad || max<-rad) return FALSE;
|
||||
|
||||
//! TO BE DOCUMENTED
|
||||
#define AXISTEST_X2(a, b, fa, fb) \
|
||||
min = a*v0.y - b*v0.z; \
|
||||
max = a*v1.y - b*v1.z; \
|
||||
if(min>max) {const float tmp=max; max=min; min=tmp; } \
|
||||
rad = fa * extents.y + fb * extents.z; \
|
||||
if(min>rad || max<-rad) return FALSE;
|
||||
|
||||
//! TO BE DOCUMENTED
|
||||
#define AXISTEST_Y02(a, b, fa, fb) \
|
||||
min = b*v0.z - a*v0.x; \
|
||||
max = b*v2.z - a*v2.x; \
|
||||
if(min>max) {const float tmp=max; max=min; min=tmp; } \
|
||||
rad = fa * extents.x + fb * extents.z; \
|
||||
if(min>rad || max<-rad) return FALSE;
|
||||
|
||||
//! TO BE DOCUMENTED
|
||||
#define AXISTEST_Y1(a, b, fa, fb) \
|
||||
min = b*v0.z - a*v0.x; \
|
||||
max = b*v1.z - a*v1.x; \
|
||||
if(min>max) {const float tmp=max; max=min; min=tmp; } \
|
||||
rad = fa * extents.x + fb * extents.z; \
|
||||
if(min>rad || max<-rad) return FALSE;
|
||||
|
||||
//! TO BE DOCUMENTED
|
||||
#define AXISTEST_X2(a, b, fa, fb) \
|
||||
min = a*v0.y - b*v0.z; \
|
||||
max = a*v1.y - b*v1.z; \
|
||||
if(min>max) {const float tmp=max; max=min; min=tmp; } \
|
||||
rad = fa * extents.y + fb * extents.z; \
|
||||
if(min>rad || max<-rad) return FALSE;
|
||||
#define AXISTEST_Z12(a, b, fa, fb) \
|
||||
min = a*v1.x - b*v1.y; \
|
||||
max = a*v2.x - b*v2.y; \
|
||||
if(min>max) {const float tmp=max; max=min; min=tmp; } \
|
||||
rad = fa * extents.x + fb * extents.y; \
|
||||
if(min>rad || max<-rad) return FALSE;
|
||||
|
||||
//! TO BE DOCUMENTED
|
||||
#define AXISTEST_Y02(a, b, fa, fb) \
|
||||
min = b*v0.z - a*v0.x; \
|
||||
max = b*v2.z - a*v2.x; \
|
||||
if(min>max) {const float tmp=max; max=min; min=tmp; } \
|
||||
rad = fa * extents.x + fb * extents.z; \
|
||||
if(min>rad || max<-rad) return FALSE;
|
||||
|
||||
//! TO BE DOCUMENTED
|
||||
#define AXISTEST_Y1(a, b, fa, fb) \
|
||||
min = b*v0.z - a*v0.x; \
|
||||
max = b*v1.z - a*v1.x; \
|
||||
if(min>max) {const float tmp=max; max=min; min=tmp; } \
|
||||
rad = fa * extents.x + fb * extents.z; \
|
||||
if(min>rad || max<-rad) return FALSE;
|
||||
|
||||
//! TO BE DOCUMENTED
|
||||
#define AXISTEST_Z12(a, b, fa, fb) \
|
||||
min = a*v1.x - b*v1.y; \
|
||||
max = a*v2.x - b*v2.y; \
|
||||
if(min>max) {const float tmp=max; max=min; min=tmp; } \
|
||||
rad = fa * extents.x + fb * extents.y; \
|
||||
if(min>rad || max<-rad) return FALSE;
|
||||
|
||||
//! TO BE DOCUMENTED
|
||||
#define AXISTEST_Z0(a, b, fa, fb) \
|
||||
min = a*v0.x - b*v0.y; \
|
||||
max = a*v1.x - b*v1.y; \
|
||||
if(min>max) {const float tmp=max; max=min; min=tmp; } \
|
||||
rad = fa * extents.x + fb * extents.y; \
|
||||
if(min>rad || max<-rad) return FALSE;
|
||||
#define AXISTEST_Z0(a, b, fa, fb) \
|
||||
min = a*v0.x - b*v0.y; \
|
||||
max = a*v1.x - b*v1.y; \
|
||||
if(min>max) {const float tmp=max; max=min; min=tmp; } \
|
||||
rad = fa * extents.x + fb * extents.y; \
|
||||
if(min>rad || max<-rad) return FALSE;
|
||||
|
||||
// compute triangle edges
|
||||
// - edges lazy evaluated to take advantage of early exits
|
||||
// - fabs precomputed (half less work, possible since extents are always >0)
|
||||
// - customized macros to take advantage of the null component
|
||||
// - axis vector discarded, possibly saves useless movs
|
||||
#define IMPLEMENT_CLASS3_TESTS \
|
||||
float rad; \
|
||||
float min, max; \
|
||||
\
|
||||
const float fey0 = fabsf(e0.y); \
|
||||
const float fez0 = fabsf(e0.z); \
|
||||
AXISTEST_X01(e0.z, e0.y, fez0, fey0); \
|
||||
const float fex0 = fabsf(e0.x); \
|
||||
AXISTEST_Y02(e0.z, e0.x, fez0, fex0); \
|
||||
AXISTEST_Z12(e0.y, e0.x, fey0, fex0); \
|
||||
\
|
||||
const float fey1 = fabsf(e1.y); \
|
||||
const float fez1 = fabsf(e1.z); \
|
||||
AXISTEST_X01(e1.z, e1.y, fez1, fey1); \
|
||||
const float fex1 = fabsf(e1.x); \
|
||||
AXISTEST_Y02(e1.z, e1.x, fez1, fex1); \
|
||||
AXISTEST_Z0(e1.y, e1.x, fey1, fex1); \
|
||||
\
|
||||
const Point e2 = mLeafVerts[0] - mLeafVerts[2]; \
|
||||
const float fey2 = fabsf(e2.y); \
|
||||
const float fez2 = fabsf(e2.z); \
|
||||
AXISTEST_X2(e2.z, e2.y, fez2, fey2); \
|
||||
const float fex2 = fabsf(e2.x); \
|
||||
AXISTEST_Y1(e2.z, e2.x, fez2, fex2); \
|
||||
AXISTEST_Z12(e2.y, e2.x, fey2, fex2);
|
||||
#define IMPLEMENT_CLASS3_TESTS \
|
||||
float rad; \
|
||||
float min, max; \
|
||||
\
|
||||
const float fey0 = fabsf(e0.y); \
|
||||
const float fez0 = fabsf(e0.z); \
|
||||
AXISTEST_X01(e0.z, e0.y, fez0, fey0); \
|
||||
const float fex0 = fabsf(e0.x); \
|
||||
AXISTEST_Y02(e0.z, e0.x, fez0, fex0); \
|
||||
AXISTEST_Z12(e0.y, e0.x, fey0, fex0); \
|
||||
\
|
||||
const float fey1 = fabsf(e1.y); \
|
||||
const float fez1 = fabsf(e1.z); \
|
||||
AXISTEST_X01(e1.z, e1.y, fez1, fey1); \
|
||||
const float fex1 = fabsf(e1.x); \
|
||||
AXISTEST_Y02(e1.z, e1.x, fez1, fex1); \
|
||||
AXISTEST_Z0(e1.y, e1.x, fey1, fex1); \
|
||||
\
|
||||
const Point e2 = mLeafVerts[0] - mLeafVerts[2]; \
|
||||
const float fey2 = fabsf(e2.y); \
|
||||
const float fez2 = fabsf(e2.z); \
|
||||
AXISTEST_X2(e2.z, e2.y, fez2, fey2); \
|
||||
const float fex2 = fabsf(e2.x); \
|
||||
AXISTEST_Y1(e2.z, e2.x, fez2, fex2); \
|
||||
AXISTEST_Z12(e2.y, e2.x, fey2, fex2);
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/**
|
||||
@@ -128,19 +126,19 @@ inline_ BOOL planeBoxOverlap(const Point& normal, const float d, const Point& ma
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
inline_ BOOL AABBTreeCollider::TriBoxOverlap(const Point& center, const Point& extents)
|
||||
{
|
||||
// Stats
|
||||
mNbBVPrimTests++;
|
||||
// Stats
|
||||
mNbBVPrimTests++;
|
||||
|
||||
// use separating axis theorem to test overlap between triangle and box
|
||||
// need to test for overlap in these directions:
|
||||
// 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle
|
||||
// we do not even need to test these)
|
||||
// 2) normal of the triangle
|
||||
// 3) crossproduct(edge from tri, {x,y,z}-directin)
|
||||
// this gives 3x3=9 more tests
|
||||
// use separating axis theorem to test overlap between triangle and box
|
||||
// need to test for overlap in these directions:
|
||||
// 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle
|
||||
// we do not even need to test these)
|
||||
// 2) normal of the triangle
|
||||
// 3) crossproduct(edge from tri, {x,y,z}-directin)
|
||||
// this gives 3x3=9 more tests
|
||||
|
||||
// move everything so that the boxcenter is in (0,0,0)
|
||||
Point v0, v1, v2;
|
||||
// move everything so that the boxcenter is in (0,0,0)
|
||||
Point v0, v1, v2;
|
||||
#if defined(__AVX__)
|
||||
v0 = mLeafVerts[0] - center;
|
||||
v1 = mLeafVerts[1] - center;
|
||||
@@ -148,14 +146,11 @@ inline_ BOOL AABBTreeCollider::TriBoxOverlap(const Point& center, const Point& e
|
||||
|
||||
// First, test overlap in the {x,y,z}-directions
|
||||
// find min, max of the triangle in x-direction, and test for overlap in X
|
||||
if (!inExtent(v0.x, v1.x, v2.x, extents.x))
|
||||
return FALSE;
|
||||
if (!inExtent(v0.x, v1.x, v2.x, extents.x)) return FALSE;
|
||||
|
||||
if (!inExtent(v0.y, v1.y, v2.y, extents.y))
|
||||
return FALSE;
|
||||
|
||||
if (!inExtent(v0.z, v1.z, v2.z, extents.z))
|
||||
return FALSE;
|
||||
if (!inExtent(v0.y, v1.y, v2.y, extents.y)) return FALSE;
|
||||
|
||||
if (!inExtent(v0.z, v1.z, v2.z, extents.z)) return FALSE;
|
||||
#else
|
||||
v0.x = mLeafVerts[0].x - center.x;
|
||||
v1.x = mLeafVerts[1].x - center.x;
|
||||
@@ -163,16 +158,119 @@ inline_ BOOL AABBTreeCollider::TriBoxOverlap(const Point& center, const Point& e
|
||||
|
||||
// First, test overlap in the {x,y,z}-directions
|
||||
// find min, max of the triangle in x-direction, and test for overlap in X
|
||||
if (!inExtent(v0.x, v1.x, v2.x, extents.x))
|
||||
return FALSE;
|
||||
if (!inExtent(v0.x, v1.x, v2.x, extents.x)) return FALSE;
|
||||
|
||||
// same for Y
|
||||
v0.y = mLeafVerts[0].y - center.y;
|
||||
v1.y = mLeafVerts[1].y - center.y;
|
||||
v2.y = mLeafVerts[2].y - center.y;
|
||||
|
||||
if (!inExtent(v0.y, v1.y, v2.y, extents.y))
|
||||
return FALSE;
|
||||
if (!inExtent(v0.y, v1.y, v2.y, extents.y)) return FALSE;
|
||||
|
||||
// same for Z
|
||||
v0.z = mLeafVerts[0].z - center.z;
|
||||
v1.z = mLeafVerts[1].z - center.z;
|
||||
v2.z = mLeafVerts[2].z - center.z;
|
||||
|
||||
if (!inExtent(v0.z, v1.z, v2.z, extents.z)) return FALSE;
|
||||
|
||||
#endif
|
||||
// 2) Test if the box intersects the plane of the triangle
|
||||
// compute plane equation of triangle: normal*x+d=0
|
||||
// ### could be precomputed since we use the same leaf triangle several times
|
||||
const Point e0 = v1 - v0;
|
||||
const Point e1 = v2 - v1;
|
||||
const Point normal = e0 ^ e1;
|
||||
const float d = -normal | v0;
|
||||
if (!planeBoxOverlap(normal, d, extents)) return FALSE;
|
||||
|
||||
// 3) "Class III" tests
|
||||
if (mFullPrimBoxTest)
|
||||
{
|
||||
IMPLEMENT_CLASS3_TESTS
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
//! A dedicated version where the box is constant
|
||||
inline_ BOOL OBBCollider::TriBoxOverlap()
|
||||
{
|
||||
// Stats
|
||||
mNbVolumePrimTests++;
|
||||
|
||||
// Hook
|
||||
const Point& extents = mBoxExtents;
|
||||
const Point& v0 = mLeafVerts[0];
|
||||
const Point& v1 = mLeafVerts[1];
|
||||
const Point& v2 = mLeafVerts[2];
|
||||
|
||||
// use separating axis theorem to test overlap between triangle and box
|
||||
// need to test for overlap in these directions:
|
||||
// 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle
|
||||
// we do not even need to test these)
|
||||
// 2) normal of the triangle
|
||||
// 3) crossproduct(edge from tri, {x,y,z}-directin)
|
||||
// this gives 3x3=9 more tests
|
||||
|
||||
// Box center is already in (0,0,0)
|
||||
|
||||
// First, test overlap in the {x,y,z}-directions
|
||||
// find min, max of the triangle in x-direction, and test for overlap in X
|
||||
if (!inExtent(v0.x, v1.x, v2.x, mBoxExtents.x)) return FALSE;
|
||||
if (!inExtent(v0.y, v1.y, v2.y, mBoxExtents.y)) return FALSE;
|
||||
if (!inExtent(v0.z, v1.z, v2.z, mBoxExtents.z)) return FALSE;
|
||||
|
||||
// 2) Test if the box intersects the plane of the triangle
|
||||
// compute plane equation of triangle: normal*x+d=0
|
||||
// ### could be precomputed since we use the same leaf triangle several times
|
||||
const Point e0 = v1 - v0;
|
||||
const Point e1 = v2 - v1;
|
||||
const Point normal = e0 ^ e1;
|
||||
const float d = -normal | v0;
|
||||
if (!planeBoxOverlap(normal, d, mBoxExtents)) return FALSE;
|
||||
|
||||
// 3) "Class III" tests - here we always do full tests since the box is a primitive (not a BV)
|
||||
{
|
||||
IMPLEMENT_CLASS3_TESTS
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
//! ...and another one, jeez
|
||||
inline_ BOOL AABBCollider::TriBoxOverlap()
|
||||
{
|
||||
// Stats
|
||||
mNbVolumePrimTests++;
|
||||
|
||||
// Hook
|
||||
const Point& center = mBox.mCenter;
|
||||
const Point& extents = mBox.mExtents;
|
||||
|
||||
// use separating axis theorem to test overlap between triangle and box
|
||||
// need to test for overlap in these directions:
|
||||
// 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle
|
||||
// we do not even need to test these)
|
||||
// 2) normal of the triangle
|
||||
// 3) crossproduct(edge from tri, {x,y,z}-directin)
|
||||
// this gives 3x3=9 more tests
|
||||
|
||||
// move everything so that the boxcenter is in (0,0,0)
|
||||
Point v0, v1, v2;
|
||||
v0.x = mLeafVerts[0].x - center.x;
|
||||
v1.x = mLeafVerts[1].x - center.x;
|
||||
v2.x = mLeafVerts[2].x - center.x;
|
||||
|
||||
// First, test overlap in the {x,y,z}-directions
|
||||
|
||||
// find min, max of the triangle in x-direction, and test for overlap in X
|
||||
if (!inExtent(v0.x, v1.x, v2.x, extents.x)) return FALSE;
|
||||
|
||||
// same for Y
|
||||
v0.y = mLeafVerts[0].y - center.y;
|
||||
v1.y = mLeafVerts[1].y - center.y;
|
||||
v2.y = mLeafVerts[2].y - center.y;
|
||||
|
||||
if (!inExtent(v0.y, v1.y, v2.y, extents.y)) return FALSE;
|
||||
|
||||
// same for Z
|
||||
v0.z = mLeafVerts[0].z - center.z;
|
||||
@@ -182,129 +280,18 @@ inline_ BOOL AABBTreeCollider::TriBoxOverlap(const Point& center, const Point& e
|
||||
if (!inExtent(v0.z, v1.z, v2.z, extents.z))
|
||||
return FALSE;
|
||||
|
||||
#endif
|
||||
// 2) Test if the box intersects the plane of the triangle
|
||||
// compute plane equation of triangle: normal*x+d=0
|
||||
// ### could be precomputed since we use the same leaf triangle several times
|
||||
const Point e0 = v1 - v0;
|
||||
const Point e1 = v2 - v1;
|
||||
const Point normal = e0 ^ e1;
|
||||
const float d = -normal|v0;
|
||||
if(!planeBoxOverlap(normal, d, extents)) return FALSE;
|
||||
// compute plane equation of triangle: normal*x+d=0
|
||||
// ### could be precomputed since we use the same leaf triangle several times
|
||||
const Point e0 = v1 - v0;
|
||||
const Point e1 = v2 - v1;
|
||||
const Point normal = e0 ^ e1;
|
||||
const float d = -normal | v0;
|
||||
if (!planeBoxOverlap(normal, d, extents)) return FALSE;
|
||||
|
||||
// 3) "Class III" tests
|
||||
if(mFullPrimBoxTest)
|
||||
{
|
||||
IMPLEMENT_CLASS3_TESTS
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
//! A dedicated version where the box is constant
|
||||
inline_ BOOL OBBCollider::TriBoxOverlap()
|
||||
{
|
||||
// Stats
|
||||
mNbVolumePrimTests++;
|
||||
|
||||
// Hook
|
||||
const Point& extents = mBoxExtents;
|
||||
const Point& v0 = mLeafVerts[0];
|
||||
const Point& v1 = mLeafVerts[1];
|
||||
const Point& v2 = mLeafVerts[2];
|
||||
|
||||
// use separating axis theorem to test overlap between triangle and box
|
||||
// need to test for overlap in these directions:
|
||||
// 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle
|
||||
// we do not even need to test these)
|
||||
// 2) normal of the triangle
|
||||
// 3) crossproduct(edge from tri, {x,y,z}-directin)
|
||||
// this gives 3x3=9 more tests
|
||||
|
||||
// Box center is already in (0,0,0)
|
||||
|
||||
// First, test overlap in the {x,y,z}-directions
|
||||
// find min, max of the triangle in x-direction, and test for overlap in X
|
||||
if(!inExtent(v0.x, v1.x, v2.x, mBoxExtents.x))
|
||||
return FALSE;
|
||||
if(!inExtent(v0.y, v1.y, v2.y, mBoxExtents.y))
|
||||
return FALSE;
|
||||
if(!inExtent(v0.z, v1.z, v2.z, mBoxExtents.z))
|
||||
return FALSE;
|
||||
|
||||
// 2) Test if the box intersects the plane of the triangle
|
||||
// compute plane equation of triangle: normal*x+d=0
|
||||
// ### could be precomputed since we use the same leaf triangle several times
|
||||
const Point e0 = v1 - v0;
|
||||
const Point e1 = v2 - v1;
|
||||
const Point normal = e0 ^ e1;
|
||||
const float d = -normal|v0;
|
||||
if(!planeBoxOverlap(normal, d, mBoxExtents)) return FALSE;
|
||||
|
||||
// 3) "Class III" tests - here we always do full tests since the box is a primitive (not a BV)
|
||||
{
|
||||
IMPLEMENT_CLASS3_TESTS
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
//! ...and another one, jeez
|
||||
inline_ BOOL AABBCollider::TriBoxOverlap()
|
||||
{
|
||||
// Stats
|
||||
mNbVolumePrimTests++;
|
||||
|
||||
// Hook
|
||||
const Point& center = mBox.mCenter;
|
||||
const Point& extents = mBox.mExtents;
|
||||
|
||||
// use separating axis theorem to test overlap between triangle and box
|
||||
// need to test for overlap in these directions:
|
||||
// 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle
|
||||
// we do not even need to test these)
|
||||
// 2) normal of the triangle
|
||||
// 3) crossproduct(edge from tri, {x,y,z}-directin)
|
||||
// this gives 3x3=9 more tests
|
||||
|
||||
// move everything so that the boxcenter is in (0,0,0)
|
||||
Point v0, v1, v2;
|
||||
v0.x = mLeafVerts[0].x - center.x;
|
||||
v1.x = mLeafVerts[1].x - center.x;
|
||||
v2.x = mLeafVerts[2].x - center.x;
|
||||
|
||||
// First, test overlap in the {x,y,z}-directions
|
||||
|
||||
// find min, max of the triangle in x-direction, and test for overlap in X
|
||||
if (!inExtent(v0.x, v1.x, v2.x, extents.x))
|
||||
return FALSE;
|
||||
|
||||
// same for Y
|
||||
v0.y = mLeafVerts[0].y - center.y;
|
||||
v1.y = mLeafVerts[1].y - center.y;
|
||||
v2.y = mLeafVerts[2].y - center.y;
|
||||
|
||||
if (!inExtent(v0.y, v1.y, v2.y, extents.y))
|
||||
return FALSE;
|
||||
|
||||
// same for Z
|
||||
v0.z = mLeafVerts[0].z - center.z;
|
||||
v1.z = mLeafVerts[1].z - center.z;
|
||||
v2.z = mLeafVerts[2].z - center.z;
|
||||
|
||||
if (!inExtent(v0.z, v1.z, v2.z, extents.z))
|
||||
return FALSE;
|
||||
|
||||
// 2) Test if the box intersects the plane of the triangle
|
||||
// compute plane equation of triangle: normal*x+d=0
|
||||
// ### could be precomputed since we use the same leaf triangle several times
|
||||
const Point e0 = v1 - v0;
|
||||
const Point e1 = v2 - v1;
|
||||
const Point normal = e0 ^ e1;
|
||||
const float d = -normal|v0;
|
||||
if(!planeBoxOverlap(normal, d, extents)) return FALSE;
|
||||
|
||||
// 3) "Class III" tests - here we always do full tests since the box is a primitive (not a BV)
|
||||
{
|
||||
IMPLEMENT_CLASS3_TESTS
|
||||
}
|
||||
return TRUE;
|
||||
// 3) "Class III" tests - here we always do full tests since the box is a primitive (not a BV)
|
||||
{
|
||||
IMPLEMENT_CLASS3_TESTS
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user