dali-physics/third-party/bullet3/src/LinearMath/btReducedVector.h

   1 //
   2 //  btReducedVectors.h
   3 //  BulletLinearMath
   4 //
   5 //  Created by Xuchen Han on 4/4/20.
   6 //
   7 #ifndef btReducedVectors_h
   8 #define btReducedVectors_h
   9 #include "btVector3.h"
  10 #include "btMatrix3x3.h"
  11 #include "btAlignedObjectArray.h"
  12 #include <stdio.h>
  13 #include <vector>
  14 #include <algorithm>
  15 struct TwoInts
  16 {
  17     int a,b;
  18 };
  19 inline bool operator<(const TwoInts& A, const TwoInts& B)
  20 {
  21     return A.b < B.b;
  22 }
  23
  24
  25 // A helper vector type used for CG projections
  26 class btReducedVector
  27 {
  28 public:
  29     btAlignedObjectArray<int> m_indices;
  30     btAlignedObjectArray<btVector3> m_vecs;
  31     int m_sz; // all m_indices value < m_sz
  32 public:
  33         btReducedVector():m_sz(0)
  34         {
  35                 m_indices.resize(0);
  36                 m_vecs.resize(0);
  37         m_indices.clear();
  38         m_vecs.clear();
  39         }
  40
  41     btReducedVector(int sz): m_sz(sz)
  42     {
  43         m_indices.resize(0);
  44         m_vecs.resize(0);
  45         m_indices.clear();
  46         m_vecs.clear();
  47     }
  48
  49     btReducedVector(int sz, const btAlignedObjectArray<int>& indices, const btAlignedObjectArray<btVector3>& vecs): m_sz(sz), m_indices(indices), m_vecs(vecs)
  50     {
  51     }
  52
  53     void simplify()
  54     {
  55         btAlignedObjectArray<int> old_indices(m_indices);
  56         btAlignedObjectArray<btVector3> old_vecs(m_vecs);
  57         m_indices.resize(0);
  58         m_vecs.resize(0);
  59         m_indices.clear();
  60         m_vecs.clear();
  61         for (int i = 0; i < old_indices.size(); ++i)
  62         {
  63             if (old_vecs[i].length2() > SIMD_EPSILON)
  64             {
  65                 m_indices.push_back(old_indices[i]);
  66                 m_vecs.push_back(old_vecs[i]);
  67             }
  68         }
  69     }
  70
  71     btReducedVector operator+(const btReducedVector& other)
  72     {
  73                 btReducedVector ret(m_sz);
  74                 int i=0, j=0;
  75                 while (i < m_indices.size() && j < other.m_indices.size())
  76                 {
  77                         if (m_indices[i] < other.m_indices[j])
  78                         {
  79                                 ret.m_indices.push_back(m_indices[i]);
  80                                 ret.m_vecs.push_back(m_vecs[i]);
  81                                 ++i;
  82                         }
  83                         else if (m_indices[i] > other.m_indices[j])
  84                         {
  85                                 ret.m_indices.push_back(other.m_indices[j]);
  86                                 ret.m_vecs.push_back(other.m_vecs[j]);
  87                                 ++j;
  88                         }
  89                         else
  90                         {
  91                                 ret.m_indices.push_back(other.m_indices[j]);
  92                                 ret.m_vecs.push_back(m_vecs[i] + other.m_vecs[j]);
  93                                 ++i; ++j;
  94                         }
  95                 }
  96                 while (i < m_indices.size())
  97                 {
  98                         ret.m_indices.push_back(m_indices[i]);
  99                         ret.m_vecs.push_back(m_vecs[i]);
 100                         ++i;
 101                 }
 102                 while (j < other.m_indices.size())
 103                 {
 104                         ret.m_indices.push_back(other.m_indices[j]);
 105                         ret.m_vecs.push_back(other.m_vecs[j]);
 106                         ++j;
 107                 }
 108         ret.simplify();
 109         return ret;
 110     }
 111
 112     btReducedVector operator-()
 113     {
 114         btReducedVector ret(m_sz);
 115         for (int i = 0; i < m_indices.size(); ++i)
 116         {
 117             ret.m_indices.push_back(m_indices[i]);
 118             ret.m_vecs.push_back(-m_vecs[i]);
 119         }
 120         ret.simplify();
 121         return ret;
 122     }
 123
 124     btReducedVector operator-(const btReducedVector& other)
 125     {
 126                 btReducedVector ret(m_sz);
 127                 int i=0, j=0;
 128                 while (i < m_indices.size() && j < other.m_indices.size())
 129                 {
 130                         if (m_indices[i] < other.m_indices[j])
 131                         {
 132                                 ret.m_indices.push_back(m_indices[i]);
 133                                 ret.m_vecs.push_back(m_vecs[i]);
 134                                 ++i;
 135                         }
 136                         else if (m_indices[i] > other.m_indices[j])
 137                         {
 138                                 ret.m_indices.push_back(other.m_indices[j]);
 139                                 ret.m_vecs.push_back(-other.m_vecs[j]);
 140                                 ++j;
 141                         }
 142                         else
 143                         {
 144                                 ret.m_indices.push_back(other.m_indices[j]);
 145                                 ret.m_vecs.push_back(m_vecs[i] - other.m_vecs[j]);
 146                                 ++i; ++j;
 147                         }
 148                 }
 149                 while (i < m_indices.size())
 150                 {
 151                         ret.m_indices.push_back(m_indices[i]);
 152                         ret.m_vecs.push_back(m_vecs[i]);
 153                         ++i;
 154                 }
 155                 while (j < other.m_indices.size())
 156                 {
 157                         ret.m_indices.push_back(other.m_indices[j]);
 158                         ret.m_vecs.push_back(-other.m_vecs[j]);
 159                         ++j;
 160                 }
 161         ret.simplify();
 162                 return ret;
 163     }
 164
 165     bool operator==(const btReducedVector& other) const
 166     {
 167         if (m_sz != other.m_sz)
 168             return false;
 169         if (m_indices.size() != other.m_indices.size())
 170             return false;
 171         for (int i = 0; i < m_indices.size(); ++i)
 172         {
 173             if (m_indices[i] != other.m_indices[i] || m_vecs[i] != other.m_vecs[i])
 174             {
 175                 return false;
 176             }
 177         }
 178         return true;
 179     }
 180
 181     bool operator!=(const btReducedVector& other) const
 182     {
 183         return !(*this == other);
 184     }
 185
 186         btReducedVector& operator=(const btReducedVector& other)
 187         {
 188                 if (this == &other)
 189                 {
 190                         return *this;
 191                 }
 192         m_sz = other.m_sz;
 193                 m_indices.copyFromArray(other.m_indices);
 194                 m_vecs.copyFromArray(other.m_vecs);
 195                 return *this;
 196         }
 197
 198     btScalar dot(const btReducedVector& other) const
 199     {
 200         btScalar ret = 0;
 201         int j = 0;
 202         for (int i = 0; i < m_indices.size(); ++i)
 203         {
 204             while (j < other.m_indices.size() && other.m_indices[j] < m_indices[i])
 205             {
 206                 ++j;
 207             }
 208             if (j < other.m_indices.size() && other.m_indices[j] == m_indices[i])
 209             {
 210                 ret += m_vecs[i].dot(other.m_vecs[j]);
 211 //                ++j;
 212             }
 213         }
 214         return ret;
 215     }
 216
 217     btScalar dot(const btAlignedObjectArray<btVector3>& other) const
 218     {
 219         btScalar ret = 0;
 220         for (int i = 0; i < m_indices.size(); ++i)
 221         {
 222             ret += m_vecs[i].dot(other[m_indices[i]]);
 223         }
 224         return ret;
 225     }
 226
 227     btScalar length2() const
 228     {
 229         return this->dot(*this);
 230     }
 231
 232         void normalize();
 233
 234     // returns the projection of this onto other
 235     btReducedVector proj(const btReducedVector& other) const;
 236
 237     bool testAdd() const;
 238
 239     bool testMinus() const;
 240
 241     bool testDot() const;
 242
 243     bool testMultiply() const;
 244
 245     void test() const;
 246
 247     void print() const
 248     {
 249         for (int i = 0; i < m_indices.size(); ++i)
 250         {
 251             printf("%d: (%f, %f, %f)/", m_indices[i], m_vecs[i][0],m_vecs[i][1],m_vecs[i][2]);
 252         }
 253         printf("\n");
 254     }
 255
 256
 257     void sort()
 258     {
 259         std::vector<TwoInts> tuples;
 260         for (int i = 0; i < m_indices.size(); ++i)
 261         {
 262             TwoInts ti;
 263             ti.a = i;
 264             ti.b = m_indices[i];
 265             tuples.push_back(ti);
 266         }
 267         std::sort(tuples.begin(), tuples.end());
 268         btAlignedObjectArray<int> new_indices;
 269         btAlignedObjectArray<btVector3> new_vecs;
 270         for (size_t i = 0; i < tuples.size(); ++i)
 271         {
 272             new_indices.push_back(tuples[i].b);
 273             new_vecs.push_back(m_vecs[tuples[i].a]);
 274         }
 275         m_indices = new_indices;
 276         m_vecs = new_vecs;
 277     }
 278 };
 279
 280 SIMD_FORCE_INLINE btReducedVector operator*(const btReducedVector& v, btScalar s)
 281 {
 282     btReducedVector ret(v.m_sz);
 283     for (int i = 0; i < v.m_indices.size(); ++i)
 284     {
 285         ret.m_indices.push_back(v.m_indices[i]);
 286         ret.m_vecs.push_back(s*v.m_vecs[i]);
 287     }
 288     ret.simplify();
 289     return ret;
 290 }
 291
 292 SIMD_FORCE_INLINE btReducedVector operator*(btScalar s, const btReducedVector& v)
 293 {
 294     return v*s;
 295 }
 296
 297 SIMD_FORCE_INLINE btReducedVector operator/(const btReducedVector& v, btScalar s)
 298 {
 299         return v * (1.0/s);
 300 }
 301
 302 SIMD_FORCE_INLINE btReducedVector& operator/=(btReducedVector& v, btScalar s)
 303 {
 304         v = v/s;
 305         return v;
 306 }
 307
 308 SIMD_FORCE_INLINE btReducedVector& operator+=(btReducedVector& v1, const btReducedVector& v2)
 309 {
 310         v1 = v1+v2;
 311         return v1;
 312 }
 313
 314 SIMD_FORCE_INLINE btReducedVector& operator-=(btReducedVector& v1, const btReducedVector& v2)
 315 {
 316         v1 = v1-v2;
 317         return v1;
 318 }
 319
 320 #endif /* btReducedVectors_h */