summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/LinearMath
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/LinearMath')
-rw-r--r--thirdparty/bullet/LinearMath/btImplicitQRSVD.h4
-rw-r--r--thirdparty/bullet/LinearMath/btMatrix3x3.h16
-rw-r--r--thirdparty/bullet/LinearMath/btMatrixX.h3
-rw-r--r--thirdparty/bullet/LinearMath/btModifiedGramSchmidt.h83
-rw-r--r--thirdparty/bullet/LinearMath/btReducedVector.cpp170
-rw-r--r--thirdparty/bullet/LinearMath/btReducedVector.h320
6 files changed, 592 insertions, 4 deletions
diff --git a/thirdparty/bullet/LinearMath/btImplicitQRSVD.h b/thirdparty/bullet/LinearMath/btImplicitQRSVD.h
index 7b4cfaf21e..aaedc964f6 100644
--- a/thirdparty/bullet/LinearMath/btImplicitQRSVD.h
+++ b/thirdparty/bullet/LinearMath/btImplicitQRSVD.h
@@ -41,7 +41,7 @@
#ifndef btImplicitQRSVD_h
#define btImplicitQRSVD_h
-
+#include <limits>
#include "btMatrix3x3.h"
class btMatrix2x2
{
@@ -753,7 +753,7 @@ inline int singularValueDecomposition(const btMatrix3x3& A,
btMatrix3x3& V,
btScalar tol = 128*std::numeric_limits<btScalar>::epsilon())
{
- using std::fabs;
+// using std::fabs;
btMatrix3x3 B = A;
U.setIdentity();
V.setIdentity();
diff --git a/thirdparty/bullet/LinearMath/btMatrix3x3.h b/thirdparty/bullet/LinearMath/btMatrix3x3.h
index cc33a68664..9c90fee1d2 100644
--- a/thirdparty/bullet/LinearMath/btMatrix3x3.h
+++ b/thirdparty/bullet/LinearMath/btMatrix3x3.h
@@ -26,10 +26,12 @@ subject to the following restrictions:
#endif
#if defined(BT_USE_SSE)
+#define v0000 (_mm_set_ps(0.0f, 0.0f, 0.0f, 0.0f))
#define v1000 (_mm_set_ps(0.0f, 0.0f, 0.0f, 1.0f))
#define v0100 (_mm_set_ps(0.0f, 0.0f, 1.0f, 0.0f))
#define v0010 (_mm_set_ps(0.0f, 1.0f, 0.0f, 0.0f))
#elif defined(BT_USE_NEON)
+const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0000) = {0.0f, 0.0f, 0.0f, 0.0f};
const btSimdFloat4 ATTRIBUTE_ALIGNED16(v1000) = {1.0f, 0.0f, 0.0f, 0.0f};
const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0100) = {0.0f, 1.0f, 0.0f, 0.0f};
const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0010) = {0.0f, 0.0f, 1.0f, 0.0f};
@@ -330,6 +332,20 @@ public:
btScalar(0.0), btScalar(0.0), btScalar(1.0));
#endif
}
+
+ /**@brief Set the matrix to the identity */
+ void setZero()
+ {
+#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
+ m_el[0] = v0000;
+ m_el[1] = v0000;
+ m_el[2] = v0000;
+#else
+ setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0),
+ btScalar(0.0), btScalar(0.0), btScalar(0.0),
+ btScalar(0.0), btScalar(0.0), btScalar(0.0));
+#endif
+ }
static const btMatrix3x3& getIdentity()
{
diff --git a/thirdparty/bullet/LinearMath/btMatrixX.h b/thirdparty/bullet/LinearMath/btMatrixX.h
index 961c94dc63..bb0f0dd259 100644
--- a/thirdparty/bullet/LinearMath/btMatrixX.h
+++ b/thirdparty/bullet/LinearMath/btMatrixX.h
@@ -346,10 +346,9 @@ struct btMatrixX
T dotProd = 0;
{
{
- int r = rows();
int c = cols();
- for (int k = 0; k < cols(); k++)
+ for (int k = 0; k < c; k++)
{
T w = (*this)(i, k);
if (other(k, j) != 0.f)
diff --git a/thirdparty/bullet/LinearMath/btModifiedGramSchmidt.h b/thirdparty/bullet/LinearMath/btModifiedGramSchmidt.h
new file mode 100644
index 0000000000..33bab8d650
--- /dev/null
+++ b/thirdparty/bullet/LinearMath/btModifiedGramSchmidt.h
@@ -0,0 +1,83 @@
+//
+// btModifiedGramSchmidt.h
+// LinearMath
+//
+// Created by Xuchen Han on 4/4/20.
+//
+
+#ifndef btModifiedGramSchmidt_h
+#define btModifiedGramSchmidt_h
+
+#include "btReducedVector.h"
+#include "btAlignedObjectArray.h"
+#include <iostream>
+#include <cmath>
+template<class TV>
+class btModifiedGramSchmidt
+{
+public:
+ btAlignedObjectArray<TV> m_in;
+ btAlignedObjectArray<TV> m_out;
+
+ btModifiedGramSchmidt(const btAlignedObjectArray<TV>& vecs): m_in(vecs)
+ {
+ m_out.resize(0);
+ }
+
+ void solve()
+ {
+ m_out.resize(m_in.size());
+ for (int i = 0; i < m_in.size(); ++i)
+ {
+// printf("========= starting %d ==========\n", i);
+ TV v(m_in[i]);
+// v.print();
+ for (int j = 0; j < i; ++j)
+ {
+ v = v - v.proj(m_out[j]);
+// v.print();
+ }
+ v.normalize();
+ m_out[i] = v;
+// v.print();
+ }
+ }
+
+ void test()
+ {
+ std::cout << SIMD_EPSILON << std::endl;
+ printf("=======inputs=========\n");
+ for (int i = 0; i < m_out.size(); ++i)
+ {
+ m_in[i].print();
+ }
+ printf("=======output=========\n");
+ for (int i = 0; i < m_out.size(); ++i)
+ {
+ m_out[i].print();
+ }
+ btScalar eps = SIMD_EPSILON;
+ for (int i = 0; i < m_out.size(); ++i)
+ {
+ for (int j = 0; j < m_out.size(); ++j)
+ {
+ if (i == j)
+ {
+ if (std::abs(1.0-m_out[i].dot(m_out[j])) > eps)// && std::abs(m_out[i].dot(m_out[j])) > eps)
+ {
+ printf("vec[%d] is not unit, norm squared = %f\n", i,m_out[i].dot(m_out[j]));
+ }
+ }
+ else
+ {
+ if (std::abs(m_out[i].dot(m_out[j])) > eps)
+ {
+ printf("vec[%d] and vec[%d] is not orthogonal, dot product = %f\n", i, j, m_out[i].dot(m_out[j]));
+ }
+ }
+ }
+ }
+ }
+};
+template class btModifiedGramSchmidt<btReducedVector>;
+#endif /* btModifiedGramSchmidt_h */
diff --git a/thirdparty/bullet/LinearMath/btReducedVector.cpp b/thirdparty/bullet/LinearMath/btReducedVector.cpp
new file mode 100644
index 0000000000..1539584e7e
--- /dev/null
+++ b/thirdparty/bullet/LinearMath/btReducedVector.cpp
@@ -0,0 +1,170 @@
+//
+// btReducedVector.cpp
+// LinearMath
+//
+// Created by Xuchen Han on 4/4/20.
+//
+#include <stdio.h>
+#include "btReducedVector.h"
+#include <cmath>
+
+// returns the projection of this onto other
+btReducedVector btReducedVector::proj(const btReducedVector& other) const
+{
+ btReducedVector ret(m_sz);
+ btScalar other_length2 = other.length2();
+ if (other_length2 < SIMD_EPSILON)
+ {
+ return ret;
+ }
+ return other*(this->dot(other))/other_length2;
+}
+
+void btReducedVector::normalize()
+{
+ if (this->length2() < SIMD_EPSILON)
+ {
+ m_indices.clear();
+ m_vecs.clear();
+ return;
+ }
+ *this /= std::sqrt(this->length2());
+}
+
+bool btReducedVector::testAdd() const
+{
+ int sz = 5;
+ btAlignedObjectArray<int> id1;
+ id1.push_back(1);
+ id1.push_back(3);
+ btAlignedObjectArray<btVector3> v1;
+ v1.push_back(btVector3(1,0,1));
+ v1.push_back(btVector3(3,1,5));
+ btAlignedObjectArray<int> id2;
+ id2.push_back(2);
+ id2.push_back(3);
+ id2.push_back(5);
+ btAlignedObjectArray<btVector3> v2;
+ v2.push_back(btVector3(2,3,1));
+ v2.push_back(btVector3(3,4,9));
+ v2.push_back(btVector3(0,4,0));
+ btAlignedObjectArray<int> id3;
+ id3.push_back(1);
+ id3.push_back(2);
+ id3.push_back(3);
+ id3.push_back(5);
+ btAlignedObjectArray<btVector3> v3;
+ v3.push_back(btVector3(1,0,1));
+ v3.push_back(btVector3(2,3,1));
+ v3.push_back(btVector3(6,5,14));
+ v3.push_back(btVector3(0,4,0));
+ btReducedVector rv1(sz, id1, v1);
+ btReducedVector rv2(sz, id2, v2);
+ btReducedVector ans(sz, id3, v3);
+ bool ret = ((ans == rv1+rv2) && (ans == rv2+rv1));
+ if (!ret)
+ printf("btReducedVector testAdd failed\n");
+ return ret;
+}
+
+bool btReducedVector::testMinus() const
+{
+ int sz = 5;
+ btAlignedObjectArray<int> id1;
+ id1.push_back(1);
+ id1.push_back(3);
+ btAlignedObjectArray<btVector3> v1;
+ v1.push_back(btVector3(1,0,1));
+ v1.push_back(btVector3(3,1,5));
+ btAlignedObjectArray<int> id2;
+ id2.push_back(2);
+ id2.push_back(3);
+ id2.push_back(5);
+ btAlignedObjectArray<btVector3> v2;
+ v2.push_back(btVector3(2,3,1));
+ v2.push_back(btVector3(3,4,9));
+ v2.push_back(btVector3(0,4,0));
+ btAlignedObjectArray<int> id3;
+ id3.push_back(1);
+ id3.push_back(2);
+ id3.push_back(3);
+ id3.push_back(5);
+ btAlignedObjectArray<btVector3> v3;
+ v3.push_back(btVector3(-1,-0,-1));
+ v3.push_back(btVector3(2,3,1));
+ v3.push_back(btVector3(0,3,4));
+ v3.push_back(btVector3(0,4,0));
+ btReducedVector rv1(sz, id1, v1);
+ btReducedVector rv2(sz, id2, v2);
+ btReducedVector ans(sz, id3, v3);
+ bool ret = (ans == rv2-rv1);
+ if (!ret)
+ printf("btReducedVector testMinus failed\n");
+ return ret;
+}
+
+bool btReducedVector::testDot() const
+{
+ int sz = 5;
+ btAlignedObjectArray<int> id1;
+ id1.push_back(1);
+ id1.push_back(3);
+ btAlignedObjectArray<btVector3> v1;
+ v1.push_back(btVector3(1,0,1));
+ v1.push_back(btVector3(3,1,5));
+ btAlignedObjectArray<int> id2;
+ id2.push_back(2);
+ id2.push_back(3);
+ id2.push_back(5);
+ btAlignedObjectArray<btVector3> v2;
+ v2.push_back(btVector3(2,3,1));
+ v2.push_back(btVector3(3,4,9));
+ v2.push_back(btVector3(0,4,0));
+ btReducedVector rv1(sz, id1, v1);
+ btReducedVector rv2(sz, id2, v2);
+ btScalar ans = 58;
+ bool ret = (ans == rv2.dot(rv1) && ans == rv1.dot(rv2));
+ ans = 14+16+9+16+81;
+ ret &= (ans==rv2.dot(rv2));
+
+ if (!ret)
+ printf("btReducedVector testDot failed\n");
+ return ret;
+}
+
+bool btReducedVector::testMultiply() const
+{
+ int sz = 5;
+ btAlignedObjectArray<int> id1;
+ id1.push_back(1);
+ id1.push_back(3);
+ btAlignedObjectArray<btVector3> v1;
+ v1.push_back(btVector3(1,0,1));
+ v1.push_back(btVector3(3,1,5));
+ btScalar s = 2;
+ btReducedVector rv1(sz, id1, v1);
+ btAlignedObjectArray<int> id2;
+ id2.push_back(1);
+ id2.push_back(3);
+ btAlignedObjectArray<btVector3> v2;
+ v2.push_back(btVector3(2,0,2));
+ v2.push_back(btVector3(6,2,10));
+ btReducedVector ans(sz, id2, v2);
+ bool ret = (ans == rv1*s);
+ if (!ret)
+ printf("btReducedVector testMultiply failed\n");
+ return ret;
+}
+
+void btReducedVector::test() const
+{
+ bool ans = testAdd() && testMinus() && testDot() && testMultiply();
+ if (ans)
+ {
+ printf("All tests passed\n");
+ }
+ else
+ {
+ printf("Tests failed\n");
+ }
+}
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-30 07:50:34 +0000