diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h index fdb736272..356dda71c 100644 --- a/src/LinearMath/btScalar.h +++ b/src/LinearMath/btScalar.h @@ -418,21 +418,32 @@ SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmod(x,y); } SIMD_FORCE_INLINE btScalar btSqrt(btScalar y) { #ifdef USE_APPROXIMATION +#ifdef __arm__ double x, z, tempf; unsigned long *tfptr = ((unsigned long *)&tempf) + 1; - - tempf = y; - *tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */ - x = tempf; - z = y*btScalar(0.5); - x = (btScalar(1.5)*x)-(x*x)*(x*z); /* iteration formula */ - x = (btScalar(1.5)*x)-(x*x)*(x*z); - x = (btScalar(1.5)*x)-(x*x)*(x*z); - x = (btScalar(1.5)*x)-(x*x)*(x*z); - x = (btScalar(1.5)*x)-(x*x)*(x*z); - return x*y; + tempf = y; + *tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */ + x = tempf; + z = y*btScalar(0.5); + x = (btScalar(1.5)*x)-(x*x)*(x*z); /* iteration formula */ + x = (btScalar(1.5)*x)-(x*x)*(x*z); + x = (btScalar(1.5)*x)-(x*x)*(x*z); + x = (btScalar(1.5)*x)-(x*x)*(x*z); + x = (btScalar(1.5)*x)-(x*x)*(x*z); + return x*y; +#elif __arm64__ + float xhalf = 0.5f*y; + int i = *(int*)&y; + i = 0x5f375a86 - (i>>1); + y = *(float*)&i; + y = y*(1.5f - xhalf*y*y); + y = y*(1.5f - xhalf*y*y); + y = y*(1.5f - xhalf*y*y); + y=1/y; + return y; +#endif #else - return sqrtf(y); + return sqrtf(y); #endif } SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); }