From eb296e6f171b1b9c388a568a1022f2d0ec798ffd Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Mon, 25 Jul 2022 22:33:43 +0100 Subject: [PATCH] some more changes on ubode; remove some math hacks, outdated by fpus --- .../ubODE-OpenSim/OPCODE/Ice/IceAABB.h | 26 +++--- .../ubODE-OpenSim/OPCODE/Ice/IceFPU.h | 79 +------------------ .../ubODE-OpenSim/OPCODE/Ice/IcePoint.cpp | 14 ---- .../ubODE-OpenSim/OPCODE/Ice/IcePoint.h | 41 ++-------- .../ubODE-OpenSim/include/ode/odemath.h | 15 ++++ .../ubODE-OpenSim/ode/src/joints/slider.cpp | 1 + 6 files changed, 39 insertions(+), 137 deletions(-) diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IceAABB.h b/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IceAABB.h index c317516e..dbb243a3 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IceAABB.h +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IceAABB.h @@ -377,9 +377,9 @@ /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// inline_ BOOL Intersect(const AABB& a) const { - float tx = mCenter.x - a.mCenter.x; float ex = a.mExtents.x + mExtents.x; if(AIR(tx) > IR(ex)) return FALSE; - float ty = mCenter.y - a.mCenter.y; float ey = a.mExtents.y + mExtents.y; if(AIR(ty) > IR(ey)) return FALSE; - float tz = mCenter.z - a.mCenter.z; float ez = a.mExtents.z + mExtents.z; if(AIR(tz) > IR(ez)) return FALSE; + float tx = mCenter.x - a.mCenter.x; float ex = a.mExtents.x + mExtents.x; if(fabsf(tx) > ex) return FALSE; + float ty = mCenter.y - a.mCenter.y; float ey = a.mExtents.y + mExtents.y; if(fabsf(ty) > ey) return FALSE; + float tz = mCenter.z - a.mCenter.z; float ez = a.mExtents.z + mExtents.z; if(fabsf(tz) > ez) return FALSE; return TRUE; } @@ -410,7 +410,7 @@ { float t = mCenter[axis] - a.mCenter[axis]; float e = a.mExtents[axis] + mExtents[axis]; - if(AIR(t) > IR(e)) return FALSE; + if(fabsf(t) > e) return FALSE; return TRUE; } @@ -429,21 +429,21 @@ // Compute new extents. FPU code & CPU code have been interleaved for improved performance. Point Ex(mtx.m[0][0] * mExtents.x, mtx.m[0][1] * mExtents.x, mtx.m[0][2] * mExtents.x); //IR(Ex.x)&=0x7fffffff; IR(Ex.y)&=0x7fffffff; IR(Ex.z)&=0x7fffffff; - Ex.x = FR( AIR(Ex.x) ); - Ex.y = FR( AIR(Ex.y) ); - Ex.z = FR( AIR(Ex.z) ); + Ex.x = fabsf(Ex.x); + Ex.y = fabsf(Ex.y); + Ex.z = fabsf(Ex.z); Point Ey(mtx.m[1][0] * mExtents.y, mtx.m[1][1] * mExtents.y, mtx.m[1][2] * mExtents.y); //IR(Ey.x)&=0x7fffffff; IR(Ey.y)&=0x7fffffff; IR(Ey.z)&=0x7fffffff; - Ey.x = FR( AIR(Ey.x) ); - Ey.y = FR( AIR(Ey.y) ); - Ey.z = FR( AIR(Ey.z) ); + Ey.x = fabsf(Ey.x); + Ey.y = fabsf(Ey.y); + Ey.z = fabsf(Ey.z); Point Ez(mtx.m[2][0] * mExtents.z, mtx.m[2][1] * mExtents.z, mtx.m[2][2] * mExtents.z); //IR(Ez.x)&=0x7fffffff; IR(Ez.y)&=0x7fffffff; IR(Ez.z)&=0x7fffffff; - Ez.x = FR( AIR(Ez.x) ); - Ez.y = FR( AIR(Ez.y) ); - Ez.z = FR( AIR(Ez.z) ); + Ez.x = fabsf(Ez.x); + Ez.y = fabsf(Ez.y); + Ez.z = fabsf(Ez.z); aabb.mExtents.x = Ex.x + Ey.x + Ez.x; aabb.mExtents.y = Ex.y + Ey.y + Ez.y; diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IceFPU.h b/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IceFPU.h index 78f104b8..cc880409 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IceFPU.h +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IceFPU.h @@ -28,12 +28,9 @@ //#define SIR(x) ((sdword&)(x)) static inline sdword SIR(float x) { float_sdword fs; fs.f = x; return fs.s; } - //! Absolute integer representation of a floating-point value - #define AIR(x) (IR(x)&0x7fffffff) - //! Floating-point representation of an integer value. //#define FR(x) ((float&)(x)) - static inline float FR(unsigned x) { float_udword fu; fu.u = x; return fu.f; } + //static inline float FR(unsigned x) { float_udword fu; fu.u = x; return fu.f; } //! Integer-based comparison of a floating point value. //! Don't use it blindly, it can be faster or slower than the FPU comparison, depends on the context. @@ -44,8 +41,7 @@ //! Don't use it blindy, it can be faster or slower than the FPU comparison, depends on the context. inline_ float FastFabs(float x) { - udword FloatBits = IR(x)&0x7fffffff; - return FR(FloatBits); + return fabsf(x); } //! Fast square root for floating-point values. @@ -54,70 +50,6 @@ return (float)sqrt(square); } - //! Saturates positive to zero. - inline_ float fsat(float f) - { - udword y = IR(f) & ~(SIR(f) >>31); - return FR(y); - } - - //! Computes 1.0f / sqrtf(x). - inline_ float frsqrt(float f) - { - float x = f * 0.5f; - udword y = 0x5f3759df - (IR(f) >> 1); - // Iteration... - const float fy = FR(y); - const float result = fy * ( 1.5f - ( x * fy * fy ) ); - // Result - return result; - } - - //! Computes 1.0f / sqrtf(x). Comes from NVIDIA. - inline_ float InvSqrt(const float& x) - { - const udword tmp = (udword(IEEE_1_0 << 1) + IEEE_1_0 - IR(x)) >> 1; - const float y = FR(tmp); - return y * (1.47f - 0.47f * x * y * y); - } - - //! Computes 1.0f / sqrtf(x). Comes from Quake3. Looks like the first one I had above. - //! See http://www.magic-software.com/3DGEDInvSqrt.html - inline_ float RSqrt(float number) - { - int i; - float x2, y; - const float threehalfs = 1.5f; - - x2 = number * 0.5f; - y = number; - i = IR(y); - i = 0x5f3759df - (i >> 1); - y = FR(i); - y = y * (threehalfs - (x2 * y * y)); - - return y; - } - - //! TO BE DOCUMENTED - inline_ float fsqrt(float f) - { - udword y = ( ( SIR(f) - 0x3f800000 ) >> 1 ) + 0x3f800000; - // Iteration...? - // (float&)y = (3.0f - ((float&)y * (float&)y) / f) * (float&)y * 0.5f; - // Result - return FR(y); - } - - //! Returns the float ranged espilon value. - inline_ float fepsilon(float f) - { - udword b = IR(f) & 0xff800000; - udword a = b | 0x00000001; - // Result - return FR(a) - FR(b); - } - //! Is the float valid ? inline_ bool IsNAN(float value) { return (IR(value)&0x7f800000) == 0x7f800000; } inline_ bool IsIndeterminate(float value) { return IR(value) == 0xffc00000; } @@ -166,13 +98,6 @@ } } */ - //! This function computes the slowest possible floating-point value (you can also directly use FLT_EPSILON) - inline_ float ComputeFloatEpsilon() - { - const float f = FR( IR(1.0f) ^ 1 ); - return f - 1.0f; // You can check it's the same as FLT_EPSILON - } - inline_ bool IsFloatZero(float x, float epsilon=1e-6f) { return x*x < epsilon; diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IcePoint.cpp b/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IcePoint.cpp index 572269a1..99ca350b 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IcePoint.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IcePoint.cpp @@ -125,20 +125,6 @@ void Point::ProjectToScreen(float halfrenderwidth, float halfrenderheight, const projected.y *= -halfrenderheight; projected.y += halfrenderheight; } -void Point::SetNotUsed() -{ - // We use a particular integer pattern : 0xffffffff everywhere. This is a NAN. - x = y = z = FR(0xffffffff); -} - -BOOL Point::IsNotUsed() const -{ - if(IR(x)!=0xffffffff) return FALSE; - if(IR(y)!=0xffffffff) return FALSE; - if(IR(z)!=0xffffffff) return FALSE; - return TRUE; -} - #if defined(__AVX__) Point& Point::Mult(const Matrix3x3& mat, const Point& a) { diff --git a/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IcePoint.h b/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IcePoint.h index ae7867c9..c6dc8d7e 100644 --- a/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IcePoint.h +++ b/trunk/unmanaged/ubODE-OpenSim/OPCODE/Ice/IcePoint.h @@ -453,9 +453,12 @@ inline_ void iceStore3f(float *res, __m128 ma) //! Clamps each element inline_ Point& Clamp(float min, float max) { - if(xmax) x=max; - if(ymax) y=max; - if(zmax) z=max; + if(xmax) x=max; + if(ymax) y=max; + if(zmax) z=max; return *this; } @@ -508,34 +511,6 @@ inline_ void iceStore3f(float *res, __m128 ma) return TRUE; } - //! Slighty moves the point - void Tweak(udword coord_mask, udword tweak_mask) - { - if(coord_mask&1) { udword Dummy = IR(x); Dummy^=tweak_mask; x = FR(Dummy); } - if(coord_mask&2) { udword Dummy = IR(y); Dummy^=tweak_mask; y = FR(Dummy); } - if(coord_mask&4) { udword Dummy = IR(z); Dummy^=tweak_mask; z = FR(Dummy); } - } - - #define TWEAKMASK 0x3fffff - #define TWEAKNOTMASK ~TWEAKMASK - //! Slighty moves the point out - inline_ void TweakBigger() - { - udword Dummy; - Dummy = (IR(x)&TWEAKNOTMASK); if (x >= 0) Dummy += TWEAKMASK + 1; x = FR(Dummy); - Dummy = (IR(y)&TWEAKNOTMASK); if (y >= 0) Dummy+=TWEAKMASK+1; y = FR(Dummy); - Dummy = (IR(z)&TWEAKNOTMASK); if (z >= 0) Dummy+=TWEAKMASK+1; z = FR(Dummy); - } - - //! Slighty moves the point in - inline_ void TweakSmaller() - { - udword Dummy; - Dummy = (IR(x)&TWEAKNOTMASK); if (x < 0) Dummy += TWEAKMASK + 1; x = FR(Dummy); - Dummy = (IR(y)&TWEAKNOTMASK); if (y < 0) Dummy+=TWEAKMASK+1; y = FR(Dummy); - Dummy = (IR(z)&TWEAKNOTMASK); if (z < 0) Dummy+=TWEAKMASK+1; z = FR(Dummy); - } - //! Normalizes the vector #if defined(__AVX__) inline_ Point& Normalize() @@ -704,8 +679,8 @@ inline_ void iceStore3f(float *res, __m128 ma) { const float* Vals = &x; PointComponent m = X; - if(AIR(Vals[Y]) > AIR(Vals[m])) m = Y; - if(AIR(Vals[Z]) > AIR(Vals[m])) m = Z; + if(fabsf(Vals[Y]) > fabsf(Vals[m])) m = Y; + if(fabsf(Vals[Z]) > fabsf(Vals[m])) m = Z; return m; } diff --git a/trunk/unmanaged/ubODE-OpenSim/include/ode/odemath.h b/trunk/unmanaged/ubODE-OpenSim/include/ode/odemath.h index 0f5da307..c4850b27 100644 --- a/trunk/unmanaged/ubODE-OpenSim/include/ode/odemath.h +++ b/trunk/unmanaged/ubODE-OpenSim/include/ode/odemath.h @@ -114,6 +114,21 @@ ODE_PURE_INLINE void dZeroVector3(dReal *res) } #endif +#if defined(__AVX__) +ODE_PURE_INLINE void dZeroVector4(dReal *res) +{ + __m128 ma = _mm_setzero_ps(); + _mm_storeu_ps(res, ma); (res, ma); +} +#else +ODE_PURE_INLINE void dZeroVector4(dReal *res) +{ + res[0] = 0; + res[1] = 0; + res[2] = 0; + res[4] = 0; +} +#endif #if defined(__AVX__) ODE_PURE_INLINE void dZeroVector3r4(dReal *res) { diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/joints/slider.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/joints/slider.cpp index 51230af1..fe650fcf 100644 --- a/trunk/unmanaged/ubODE-OpenSim/ode/src/joints/slider.cpp +++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/joints/slider.cpp @@ -170,6 +170,7 @@ dxJointSlider::getInfo2 ( dReal worldFPS, dReal worldERP, const Info2Descr *info { pos2 = 0; R2 = 0; + dZeroVector4(c); } // 3 rows to make body rotations equal