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

   1
   2 #ifndef B3_MAT3x3_H
   3 #define B3_MAT3x3_H
   4
   5 #include "Bullet3Common/shared/b3Quat.h"
   6
   7 #ifdef __cplusplus
   8
   9 #include "Bullet3Common/b3Matrix3x3.h"
  10
  11 #define b3Mat3x3 b3Matrix3x3
  12 #define b3Mat3x3ConstArg const b3Matrix3x3&
  13
  14 inline b3Mat3x3 b3QuatGetRotationMatrix(b3QuatConstArg quat)
  15 {
  16         return b3Mat3x3(quat);
  17 }
  18
  19 inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg mat)
  20 {
  21         return mat.absolute();
  22 }
  23
  24 #define b3GetRow(m, row) m.getRow(row)
  25
  26 __inline b3Float4 mtMul3(b3Float4ConstArg a, b3Mat3x3ConstArg b)
  27 {
  28         return b * a;
  29 }
  30
  31 #else
  32
  33 typedef struct
  34 {
  35         b3Float4 m_row[3];
  36 } b3Mat3x3;
  37
  38 #define b3Mat3x3ConstArg const b3Mat3x3
  39 #define b3GetRow(m, row) (m.m_row[row])
  40
  41 inline b3Mat3x3 b3QuatGetRotationMatrix(b3Quat quat)
  42 {
  43         b3Float4 quat2 = (b3Float4)(quat.x * quat.x, quat.y * quat.y, quat.z * quat.z, 0.f);
  44         b3Mat3x3 out;
  45
  46         out.m_row[0].x = 1 - 2 * quat2.y - 2 * quat2.z;
  47         out.m_row[0].y = 2 * quat.x * quat.y - 2 * quat.w * quat.z;
  48         out.m_row[0].z = 2 * quat.x * quat.z + 2 * quat.w * quat.y;
  49         out.m_row[0].w = 0.f;
  50
  51         out.m_row[1].x = 2 * quat.x * quat.y + 2 * quat.w * quat.z;
  52         out.m_row[1].y = 1 - 2 * quat2.x - 2 * quat2.z;
  53         out.m_row[1].z = 2 * quat.y * quat.z - 2 * quat.w * quat.x;
  54         out.m_row[1].w = 0.f;
  55
  56         out.m_row[2].x = 2 * quat.x * quat.z - 2 * quat.w * quat.y;
  57         out.m_row[2].y = 2 * quat.y * quat.z + 2 * quat.w * quat.x;
  58         out.m_row[2].z = 1 - 2 * quat2.x - 2 * quat2.y;
  59         out.m_row[2].w = 0.f;
  60
  61         return out;
  62 }
  63
  64 inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn)
  65 {
  66         b3Mat3x3 out;
  67         out.m_row[0] = fabs(matIn.m_row[0]);
  68         out.m_row[1] = fabs(matIn.m_row[1]);
  69         out.m_row[2] = fabs(matIn.m_row[2]);
  70         return out;
  71 }
  72
  73 __inline b3Mat3x3 mtZero();
  74
  75 __inline b3Mat3x3 mtIdentity();
  76
  77 __inline b3Mat3x3 mtTranspose(b3Mat3x3 m);
  78
  79 __inline b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);
  80
  81 __inline b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);
  82
  83 __inline b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);
  84
  85 __inline b3Mat3x3 mtZero()
  86 {
  87         b3Mat3x3 m;
  88         m.m_row[0] = (b3Float4)(0.f);
  89         m.m_row[1] = (b3Float4)(0.f);
  90         m.m_row[2] = (b3Float4)(0.f);
  91         return m;
  92 }
  93
  94 __inline b3Mat3x3 mtIdentity()
  95 {
  96         b3Mat3x3 m;
  97         m.m_row[0] = (b3Float4)(1, 0, 0, 0);
  98         m.m_row[1] = (b3Float4)(0, 1, 0, 0);
  99         m.m_row[2] = (b3Float4)(0, 0, 1, 0);
 100         return m;
 101 }
 102
 103 __inline b3Mat3x3 mtTranspose(b3Mat3x3 m)
 104 {
 105         b3Mat3x3 out;
 106         out.m_row[0] = (b3Float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);
 107         out.m_row[1] = (b3Float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);
 108         out.m_row[2] = (b3Float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);
 109         return out;
 110 }
 111
 112 __inline b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)
 113 {
 114         b3Mat3x3 transB;
 115         transB = mtTranspose(b);
 116         b3Mat3x3 ans;
 117         //      why this doesn't run when 0ing in the for{}
 118         a.m_row[0].w = 0.f;
 119         a.m_row[1].w = 0.f;
 120         a.m_row[2].w = 0.f;
 121         for (int i = 0; i < 3; i++)
 122         {
 123                 //      a.m_row[i].w = 0.f;
 124                 ans.m_row[i].x = b3Dot3F4(a.m_row[i], transB.m_row[0]);
 125                 ans.m_row[i].y = b3Dot3F4(a.m_row[i], transB.m_row[1]);
 126                 ans.m_row[i].z = b3Dot3F4(a.m_row[i], transB.m_row[2]);
 127                 ans.m_row[i].w = 0.f;
 128         }
 129         return ans;
 130 }
 131
 132 __inline b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)
 133 {
 134         b3Float4 ans;
 135         ans.x = b3Dot3F4(a.m_row[0], b);
 136         ans.y = b3Dot3F4(a.m_row[1], b);
 137         ans.z = b3Dot3F4(a.m_row[2], b);
 138         ans.w = 0.f;
 139         return ans;
 140 }
 141
 142 __inline b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)
 143 {
 144         b3Float4 colx = b3MakeFloat4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);
 145         b3Float4 coly = b3MakeFloat4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);
 146         b3Float4 colz = b3MakeFloat4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);
 147
 148         b3Float4 ans;
 149         ans.x = b3Dot3F4(a, colx);
 150         ans.y = b3Dot3F4(a, coly);
 151         ans.z = b3Dot3F4(a, colz);
 152         return ans;
 153 }
 154
 155 #endif
 156
 157 #endif  //B3_MAT3x3_H