From cd9e2fa0a351fbcd2068d8e841f36d4eb5dba827 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 1 Aug 2022 02:59:22 +0100 Subject: [PATCH] ubode change iceplane rotation; replace more tabs --- .../ubODE-OpenSim/OPCODE/Ice/IceMatrix4x4.cpp | 138 +++--- .../ubODE-OpenSim/OPCODE/Ice/IceMatrix4x4.h | 2 +- .../ubODE-OpenSim/OPCODE/Ice/IcePlane.h | 14 +- .../ubODE-OpenSim/OPCODE/OPC_RayAABBOverlap.h | 78 ++- .../ubODE-OpenSim/OPCODE/OPC_TriBoxOverlap.h | 443 +++++++++--------- 5 files changed, 325 insertions(+), 350 deletions(-) diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IceMatrix4x4.cpp b/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IceMatrix4x4.cpp index 23366ccc..4c797038 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IceMatrix4x4.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IceMatrix4x4.cpp @@ -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; } diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IceMatrix4x4.h b/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IceMatrix4x4.h index 85cab440..8df07b94 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IceMatrix4x4.h +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IceMatrix4x4.h @@ -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. diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IcePlane.h b/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IcePlane.h index 4d470814..521e01a8 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IcePlane.h +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IcePlane.h @@ -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__ diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_RayAABBOverlap.h b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_RayAABBOverlap.h index 55523aca..1cb23b3d 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_RayAABBOverlap.h +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_RayAABBOverlap.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; } diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TriBoxOverlap.h b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TriBoxOverlap.h index de326e66..6a906ec9 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TriBoxOverlap.h +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/OPC_TriBoxOverlap.h @@ -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(x1max) max=x1; \ - if(x2max) max=x2; +#define FINDMINMAX(x0, x1, x2, min, max) \ + min = max = x0; \ + if(x1max) max=x1; \ + if(x2max) 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; }