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: btReducedVector()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 btReducedVector(int sz)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 btReducedVector(int sz,const btAlignedObjectArray<int> & indices,const btAlignedObjectArray<btVector3> & vecs)49 btReducedVector(int sz, const btAlignedObjectArray<int>& indices, const btAlignedObjectArray<btVector3>& vecs): m_sz(sz), m_indices(indices), m_vecs(vecs) 50 { 51 } 52 simplify()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 dot(const btReducedVector & other)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 dot(const btAlignedObjectArray<btVector3> & other)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 length2()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 print()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 sort()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 */ 321