dali-physics/third-party/bullet3/src/Bullet3Common/shared/b3Quat.h

   1 #ifndef B3_QUAT_H
   2 #define B3_QUAT_H
   3
   4 #include "Bullet3Common/shared/b3PlatformDefinitions.h"
   5 #include "Bullet3Common/shared/b3Float4.h"
   6
   7 #ifdef __cplusplus
   8 #include "Bullet3Common/b3Quaternion.h"
   9 #include "Bullet3Common/b3Transform.h"
  10
  11 #define b3Quat b3Quaternion
  12 #define b3QuatConstArg const b3Quaternion&
  13 inline b3Quat b3QuatInverse(b3QuatConstArg orn)
  14 {
  15         return orn.inverse();
  16 }
  17
  18 inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)
  19 {
  20         b3Transform tr;
  21         tr.setOrigin(translation);
  22         tr.setRotation(orientation);
  23         return tr(point);
  24 }
  25
  26 #else
  27 typedef float4 b3Quat;
  28 #define b3QuatConstArg const b3Quat
  29
  30 inline float4 b3FastNormalize4(float4 v)
  31 {
  32         v = (float4)(v.xyz, 0.f);
  33         return fast_normalize(v);
  34 }
  35
  36 inline b3Quat b3QuatMul(b3Quat a, b3Quat b);
  37 inline b3Quat b3QuatNormalized(b3QuatConstArg in);
  38 inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);
  39 inline b3Quat b3QuatInvert(b3QuatConstArg q);
  40 inline b3Quat b3QuatInverse(b3QuatConstArg q);
  41
  42 inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)
  43 {
  44         b3Quat ans;
  45         ans = b3Cross3(a, b);
  46         ans += a.w * b + b.w * a;
  47         //      ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);
  48         ans.w = a.w * b.w - b3Dot3F4(a, b);
  49         return ans;
  50 }
  51
  52 inline b3Quat b3QuatNormalized(b3QuatConstArg in)
  53 {
  54         b3Quat q;
  55         q = in;
  56         //return b3FastNormalize4(in);
  57         float len = native_sqrt(dot(q, q));
  58         if (len > 0.f)
  59         {
  60                 q *= 1.f / len;
  61         }
  62         else
  63         {
  64                 q.x = q.y = q.z = 0.f;
  65                 q.w = 1.f;
  66         }
  67         return q;
  68 }
  69 inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)
  70 {
  71         b3Quat qInv = b3QuatInvert(q);
  72         float4 vcpy = vec;
  73         vcpy.w = 0.f;
  74         float4 out = b3QuatMul(b3QuatMul(q, vcpy), qInv);
  75         return out;
  76 }
  77
  78 inline b3Quat b3QuatInverse(b3QuatConstArg q)
  79 {
  80         return (b3Quat)(-q.xyz, q.w);
  81 }
  82
  83 inline b3Quat b3QuatInvert(b3QuatConstArg q)
  84 {
  85         return (b3Quat)(-q.xyz, q.w);
  86 }
  87
  88 inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)
  89 {
  90         return b3QuatRotate(b3QuatInvert(q), vec);
  91 }
  92
  93 inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)
  94 {
  95         return b3QuatRotate(orientation, point) + (translation);
  96 }
  97
  98 #endif
  99
 100 #endif  //B3_QUAT_H