diff options
Diffstat (limited to 'thirdparty/bullet/Bullet3Common/shared/b3Quat.h')
| -rw-r--r-- | thirdparty/bullet/Bullet3Common/shared/b3Quat.h | 71 |
1 files changed, 34 insertions, 37 deletions
diff --git a/thirdparty/bullet/Bullet3Common/shared/b3Quat.h b/thirdparty/bullet/Bullet3Common/shared/b3Quat.h index f262d5e08f..940610c77b 100644 --- a/thirdparty/bullet/Bullet3Common/shared/b3Quat.h +++ b/thirdparty/bullet/Bullet3Common/shared/b3Quat.h @@ -5,35 +5,34 @@ #include "Bullet3Common/shared/b3Float4.h" #ifdef __cplusplus - #include "Bullet3Common/b3Quaternion.h" - #include "Bullet3Common/b3Transform.h" +#include "Bullet3Common/b3Quaternion.h" +#include "Bullet3Common/b3Transform.h" - #define b3Quat b3Quaternion - #define b3QuatConstArg const b3Quaternion& - inline b3Quat b3QuatInverse(b3QuatConstArg orn) - { - return orn.inverse(); - } +#define b3Quat b3Quaternion +#define b3QuatConstArg const b3Quaternion& +inline b3Quat b3QuatInverse(b3QuatConstArg orn) +{ + return orn.inverse(); +} - inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation) - { - b3Transform tr; - tr.setOrigin(translation); - tr.setRotation(orientation); - return tr(point); - } +inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation) +{ + b3Transform tr; + tr.setOrigin(translation); + tr.setRotation(orientation); + return tr(point); +} #else - typedef float4 b3Quat; - #define b3QuatConstArg const b3Quat - - +typedef float4 b3Quat; +#define b3QuatConstArg const b3Quat + inline float4 b3FastNormalize4(float4 v) { - v = (float4)(v.xyz,0.f); + v = (float4)(v.xyz, 0.f); return fast_normalize(v); } - + inline b3Quat b3QuatMul(b3Quat a, b3Quat b); inline b3Quat b3QuatNormalized(b3QuatConstArg in); inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec); @@ -43,20 +42,20 @@ inline b3Quat b3QuatInverse(b3QuatConstArg q); inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b) { b3Quat ans; - ans = b3Cross3( a, b ); - ans += a.w*b+b.w*a; -// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z); - ans.w = a.w*b.w - b3Dot3F4(a, b); + ans = b3Cross3(a, b); + ans += a.w * b + b.w * a; + // ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z); + ans.w = a.w * b.w - b3Dot3F4(a, b); return ans; } inline b3Quat b3QuatNormalized(b3QuatConstArg in) { b3Quat q; - q=in; + q = in; //return b3FastNormalize4(in); float len = native_sqrt(dot(q, q)); - if(len > 0.f) + if (len > 0.f) { q *= 1.f / len; } @@ -69,15 +68,13 @@ inline b3Quat b3QuatNormalized(b3QuatConstArg in) } inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec) { - b3Quat qInv = b3QuatInvert( q ); + b3Quat qInv = b3QuatInvert(q); float4 vcpy = vec; vcpy.w = 0.f; - float4 out = b3QuatMul(b3QuatMul(q,vcpy),qInv); + float4 out = b3QuatMul(b3QuatMul(q, vcpy), qInv); return out; } - - inline b3Quat b3QuatInverse(b3QuatConstArg q) { return (b3Quat)(-q.xyz, q.w); @@ -90,14 +87,14 @@ inline b3Quat b3QuatInvert(b3QuatConstArg q) inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec) { - return b3QuatRotate( b3QuatInvert( q ), vec ); + return b3QuatRotate(b3QuatInvert(q), vec); } -inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation) +inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation) { - return b3QuatRotate( orientation, point ) + (translation); + return b3QuatRotate(orientation, point) + (translation); } - -#endif -#endif //B3_QUAT_H +#endif + +#endif //B3_QUAT_H |