summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/LinearMath/btReducedVector.h
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/LinearMath/btReducedVector.h')
-rw-r--r--thirdparty/bullet/LinearMath/btReducedVector.h320
1 files changed, 320 insertions, 0 deletions
diff --git a/thirdparty/bullet/LinearMath/btReducedVector.h b/thirdparty/bullet/LinearMath/btReducedVector.h
new file mode 100644
index 0000000000..83b5e581e5
--- /dev/null
+++ b/thirdparty/bullet/LinearMath/btReducedVector.h
@@ -0,0 +1,320 @@
+//
+// btReducedVectors.h
+// BulletLinearMath
+//
+// Created by Xuchen Han on 4/4/20.
+//
+#ifndef btReducedVectors_h
+#define btReducedVectors_h
+#include "btVector3.h"
+#include "btMatrix3x3.h"
+#include "btAlignedObjectArray.h"
+#include <stdio.h>
+#include <vector>
+#include <algorithm>
+struct TwoInts
+{
+ int a,b;
+};
+inline bool operator<(const TwoInts& A, const TwoInts& B)
+{
+ return A.b < B.b;
+}
+
+
+// A helper vector type used for CG projections
+class btReducedVector
+{
+public:
+ btAlignedObjectArray<int> m_indices;
+ btAlignedObjectArray<btVector3> m_vecs;
+ int m_sz; // all m_indices value < m_sz
+public:
+ btReducedVector():m_sz(0)
+ {
+ m_indices.resize(0);
+ m_vecs.resize(0);
+ m_indices.clear();
+ m_vecs.clear();
+ }
+
+ btReducedVector(int sz): m_sz(sz)
+ {
+ m_indices.resize(0);
+ m_vecs.resize(0);
+ m_indices.clear();
+ m_vecs.clear();
+ }
+
+ btReducedVector(int sz, const btAlignedObjectArray<int>& indices, const btAlignedObjectArray<btVector3>& vecs): m_sz(sz), m_indices(indices), m_vecs(vecs)
+ {
+ }
+
+ void simplify()
+ {
+ btAlignedObjectArray<int> old_indices(m_indices);
+ btAlignedObjectArray<btVector3> old_vecs(m_vecs);
+ m_indices.resize(0);
+ m_vecs.resize(0);
+ m_indices.clear();
+ m_vecs.clear();
+ for (int i = 0; i < old_indices.size(); ++i)
+ {
+ if (old_vecs[i].length2() > SIMD_EPSILON)
+ {
+ m_indices.push_back(old_indices[i]);
+ m_vecs.push_back(old_vecs[i]);
+ }
+ }
+ }
+
+ btReducedVector operator+(const btReducedVector& other)
+ {
+ btReducedVector ret(m_sz);
+ int i=0, j=0;
+ while (i < m_indices.size() && j < other.m_indices.size())
+ {
+ if (m_indices[i] < other.m_indices[j])
+ {
+ ret.m_indices.push_back(m_indices[i]);
+ ret.m_vecs.push_back(m_vecs[i]);
+ ++i;
+ }
+ else if (m_indices[i] > other.m_indices[j])
+ {
+ ret.m_indices.push_back(other.m_indices[j]);
+ ret.m_vecs.push_back(other.m_vecs[j]);
+ ++j;
+ }
+ else
+ {
+ ret.m_indices.push_back(other.m_indices[j]);
+ ret.m_vecs.push_back(m_vecs[i] + other.m_vecs[j]);
+ ++i; ++j;
+ }
+ }
+ while (i < m_indices.size())
+ {
+ ret.m_indices.push_back(m_indices[i]);
+ ret.m_vecs.push_back(m_vecs[i]);
+ ++i;
+ }
+ while (j < other.m_indices.size())
+ {
+ ret.m_indices.push_back(other.m_indices[j]);
+ ret.m_vecs.push_back(other.m_vecs[j]);
+ ++j;
+ }
+ ret.simplify();
+ return ret;
+ }
+
+ btReducedVector operator-()
+ {
+ btReducedVector ret(m_sz);
+ for (int i = 0; i < m_indices.size(); ++i)
+ {
+ ret.m_indices.push_back(m_indices[i]);
+ ret.m_vecs.push_back(-m_vecs[i]);
+ }
+ ret.simplify();
+ return ret;
+ }
+
+ btReducedVector operator-(const btReducedVector& other)
+ {
+ btReducedVector ret(m_sz);
+ int i=0, j=0;
+ while (i < m_indices.size() && j < other.m_indices.size())
+ {
+ if (m_indices[i] < other.m_indices[j])
+ {
+ ret.m_indices.push_back(m_indices[i]);
+ ret.m_vecs.push_back(m_vecs[i]);
+ ++i;
+ }
+ else if (m_indices[i] > other.m_indices[j])
+ {
+ ret.m_indices.push_back(other.m_indices[j]);
+ ret.m_vecs.push_back(-other.m_vecs[j]);
+ ++j;
+ }
+ else
+ {
+ ret.m_indices.push_back(other.m_indices[j]);
+ ret.m_vecs.push_back(m_vecs[i] - other.m_vecs[j]);
+ ++i; ++j;
+ }
+ }
+ while (i < m_indices.size())
+ {
+ ret.m_indices.push_back(m_indices[i]);
+ ret.m_vecs.push_back(m_vecs[i]);
+ ++i;
+ }
+ while (j < other.m_indices.size())
+ {
+ ret.m_indices.push_back(other.m_indices[j]);
+ ret.m_vecs.push_back(-other.m_vecs[j]);
+ ++j;
+ }
+ ret.simplify();
+ return ret;
+ }
+
+ bool operator==(const btReducedVector& other) const
+ {
+ if (m_sz != other.m_sz)
+ return false;
+ if (m_indices.size() != other.m_indices.size())
+ return false;
+ for (int i = 0; i < m_indices.size(); ++i)
+ {
+ if (m_indices[i] != other.m_indices[i] || m_vecs[i] != other.m_vecs[i])
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ bool operator!=(const btReducedVector& other) const
+ {
+ return !(*this == other);
+ }
+
+ btReducedVector& operator=(const btReducedVector& other)
+ {
+ if (this == &other)
+ {
+ return *this;
+ }
+ m_sz = other.m_sz;
+ m_indices.copyFromArray(other.m_indices);
+ m_vecs.copyFromArray(other.m_vecs);
+ return *this;
+ }
+
+ btScalar dot(const btReducedVector& other) const
+ {
+ btScalar ret = 0;
+ int j = 0;
+ for (int i = 0; i < m_indices.size(); ++i)
+ {
+ while (j < other.m_indices.size() && other.m_indices[j] < m_indices[i])
+ {
+ ++j;
+ }
+ if (j < other.m_indices.size() && other.m_indices[j] == m_indices[i])
+ {
+ ret += m_vecs[i].dot(other.m_vecs[j]);
+// ++j;
+ }
+ }
+ return ret;
+ }
+
+ btScalar dot(const btAlignedObjectArray<btVector3>& other) const
+ {
+ btScalar ret = 0;
+ for (int i = 0; i < m_indices.size(); ++i)
+ {
+ ret += m_vecs[i].dot(other[m_indices[i]]);
+ }
+ return ret;
+ }
+
+ btScalar length2() const
+ {
+ return this->dot(*this);
+ }
+
+ void normalize();
+
+ // returns the projection of this onto other
+ btReducedVector proj(const btReducedVector& other) const;
+
+ bool testAdd() const;
+
+ bool testMinus() const;
+
+ bool testDot() const;
+
+ bool testMultiply() const;
+
+ void test() const;
+
+ void print() const
+ {
+ for (int i = 0; i < m_indices.size(); ++i)
+ {
+ printf("%d: (%f, %f, %f)/", m_indices[i], m_vecs[i][0],m_vecs[i][1],m_vecs[i][2]);
+ }
+ printf("\n");
+ }
+
+
+ void sort()
+ {
+ std::vector<TwoInts> tuples;
+ for (int i = 0; i < m_indices.size(); ++i)
+ {
+ TwoInts ti;
+ ti.a = i;
+ ti.b = m_indices[i];
+ tuples.push_back(ti);
+ }
+ std::sort(tuples.begin(), tuples.end());
+ btAlignedObjectArray<int> new_indices;
+ btAlignedObjectArray<btVector3> new_vecs;
+ for (int i = 0; i < tuples.size(); ++i)
+ {
+ new_indices.push_back(tuples[i].b);
+ new_vecs.push_back(m_vecs[tuples[i].a]);
+ }
+ m_indices = new_indices;
+ m_vecs = new_vecs;
+ }
+};
+
+SIMD_FORCE_INLINE btReducedVector operator*(const btReducedVector& v, btScalar s)
+{
+ btReducedVector ret(v.m_sz);
+ for (int i = 0; i < v.m_indices.size(); ++i)
+ {
+ ret.m_indices.push_back(v.m_indices[i]);
+ ret.m_vecs.push_back(s*v.m_vecs[i]);
+ }
+ ret.simplify();
+ return ret;
+}
+
+SIMD_FORCE_INLINE btReducedVector operator*(btScalar s, const btReducedVector& v)
+{
+ return v*s;
+}
+
+SIMD_FORCE_INLINE btReducedVector operator/(const btReducedVector& v, btScalar s)
+{
+ return v * (1.0/s);
+}
+
+SIMD_FORCE_INLINE btReducedVector& operator/=(btReducedVector& v, btScalar s)
+{
+ v = v/s;
+ return v;
+}
+
+SIMD_FORCE_INLINE btReducedVector& operator+=(btReducedVector& v1, const btReducedVector& v2)
+{
+ v1 = v1+v2;
+ return v1;
+}
+
+SIMD_FORCE_INLINE btReducedVector& operator-=(btReducedVector& v1, const btReducedVector& v2)
+{
+ v1 = v1-v2;
+ return v1;
+}
+
+#endif /* btReducedVectors_h */
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-21 01:00:20 +0000