From bbdca8a4e874ac5d7ad8f8839fa271c82659c05f Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Sat, 4 Apr 2020 17:19:57 -0700 Subject: [PATCH] add support for reduced vectors and modified gram schmidt --- .../btDeformableContactProjection.cpp | 2 +- .../btDeformableContactProjection.h | 2 + src/BulletSoftBody/btSoftBody.cpp | 3 - src/LinearMath/CMakeLists.txt | 2 + src/LinearMath/btModifiedGramSchmidt.h | 71 +++++ src/LinearMath/btReducedVector.cpp | 162 +++++++++++ src/LinearMath/btReducedVector.h | 267 ++++++++++++++++++ 7 files changed, 505 insertions(+), 4 deletions(-) create mode 100644 src/LinearMath/btModifiedGramSchmidt.h create mode 100644 src/LinearMath/btReducedVector.cpp create mode 100644 src/LinearMath/btReducedVector.h diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index c9e1a152d..af70ae958 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -109,7 +109,7 @@ btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverI } void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal) -{ +{ BT_PROFILE("setConstraints"); for (int i = 0; i < m_softBodies.size(); ++i) { diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index 32d6c4d75..a59bedda7 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -21,6 +21,8 @@ #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" #include "btDeformableContactConstraint.h" #include "LinearMath/btHashMap.h" +#include "LinearMath/btReducedVector.h" +#include "LinearMath/btModifiedGramSchmidt.h" #include class btDeformableContactProjection { diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 9be9b2009..149cf4f4b 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -3051,9 +3051,6 @@ void btSoftBody::updateArea(bool averageArea) { Face& f = m_faces[i]; f.m_ra = AreaOf(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x); - f.m_rl[0] = (f.m_n[0]->m_x - f.m_n[1]->m_x).safeNorm(); - f.m_rl[1] = (f.m_n[0]->m_x - f.m_n[2]->m_x).safeNorm(); - f.m_rl[2] = (f.m_n[1]->m_x - f.m_n[2]->m_x).safeNorm(); } /* Node area */ diff --git a/src/LinearMath/CMakeLists.txt b/src/LinearMath/CMakeLists.txt index a5491c325..d7864dfc8 100644 --- a/src/LinearMath/CMakeLists.txt +++ b/src/LinearMath/CMakeLists.txt @@ -10,6 +10,7 @@ SET(LinearMath_SRCS btGeometryUtil.cpp btPolarDecomposition.cpp btQuickprof.cpp + btReducedVector.cpp btSerializer.cpp btSerializer64.cpp btThreads.cpp @@ -40,6 +41,7 @@ SET(LinearMath_HDRS btQuadWord.h btQuaternion.h btQuickprof.h + btReducedVector.h btRandom.h btScalar.h btSerializer.h diff --git a/src/LinearMath/btModifiedGramSchmidt.h b/src/LinearMath/btModifiedGramSchmidt.h new file mode 100644 index 000000000..6bf35244b --- /dev/null +++ b/src/LinearMath/btModifiedGramSchmidt.h @@ -0,0 +1,71 @@ +// +// btModifiedGramSchmidt.h +// LinearMath +// +// Created by Xuchen Han on 4/4/20. +// + +#ifndef btModifiedGramSchmidt_h +#define btModifiedGramSchmidt_h + +#include "btReducedVector.h" +#include "btAlignedObjectArray.h" + +template +class btModifiedGramSchmidt +{ +public: + btAlignedObjectArray m_in; + btAlignedObjectArray m_out; + + btModifiedGramSchmidt(const btAlignedObjectArray& 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) + { + 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(); + v.print(); + m_out[i] = v; + printf("===========\n"); + } + } + + void test() + { + 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-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; +#endif /* btModifiedGramSchmidt_h */ diff --git a/src/LinearMath/btReducedVector.cpp b/src/LinearMath/btReducedVector.cpp new file mode 100644 index 000000000..b5665b757 --- /dev/null +++ b/src/LinearMath/btReducedVector.cpp @@ -0,0 +1,162 @@ +// +// btReducedVector.cpp +// LinearMath +// +// Created by Xuchen Han on 4/4/20. +// +#include +#include "btReducedVector.h" + +// 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 == 0) + { + return ret; + } + return other*(this->dot(other) / other_length2); +} + +void btReducedVector::normalize() +{ + if (this->length2() < SIMD_EPSILON) + return; + *this /= btSqrt(this->length2()); +} + +bool btReducedVector::testAdd() const +{ + int sz = 5; + btAlignedObjectArray id1; + id1.push_back(1); + id1.push_back(3); + btAlignedObjectArray v1; + v1.push_back(btVector3(1,0,1)); + v1.push_back(btVector3(3,1,5)); + btAlignedObjectArray id2; + id2.push_back(2); + id2.push_back(3); + id2.push_back(5); + btAlignedObjectArray v2; + v2.push_back(btVector3(2,3,1)); + v2.push_back(btVector3(3,4,9)); + v2.push_back(btVector3(0,4,0)); + btAlignedObjectArray id3; + id3.push_back(1); + id3.push_back(2); + id3.push_back(3); + id3.push_back(5); + btAlignedObjectArray 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 id1; + id1.push_back(1); + id1.push_back(3); + btAlignedObjectArray v1; + v1.push_back(btVector3(1,0,1)); + v1.push_back(btVector3(3,1,5)); + btAlignedObjectArray id2; + id2.push_back(2); + id2.push_back(3); + id2.push_back(5); + btAlignedObjectArray v2; + v2.push_back(btVector3(2,3,1)); + v2.push_back(btVector3(3,4,9)); + v2.push_back(btVector3(0,4,0)); + btAlignedObjectArray id3; + id3.push_back(1); + id3.push_back(2); + id3.push_back(3); + id3.push_back(5); + btAlignedObjectArray 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 id1; + id1.push_back(1); + id1.push_back(3); + btAlignedObjectArray v1; + v1.push_back(btVector3(1,0,1)); + v1.push_back(btVector3(3,1,5)); + btAlignedObjectArray id2; + id2.push_back(2); + id2.push_back(3); + id2.push_back(5); + btAlignedObjectArray 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)); + if (!ret) + printf("btReducedVector testDot failed\n"); + return ret; +} + +bool btReducedVector::testMultiply() const +{ + int sz = 5; + btAlignedObjectArray id1; + id1.push_back(1); + id1.push_back(3); + btAlignedObjectArray v1; + v1.push_back(btVector3(1,0,1)); + v1.push_back(btVector3(3,1,5)); + btScalar s = 2; + btReducedVector rv1(sz, id1, v1); + btAlignedObjectArray id2; + id2.push_back(1); + id2.push_back(3); + btAlignedObjectArray 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/src/LinearMath/btReducedVector.h b/src/LinearMath/btReducedVector.h new file mode 100644 index 000000000..d1b7b9809 --- /dev/null +++ b/src/LinearMath/btReducedVector.h @@ -0,0 +1,267 @@ +// +// 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 +// A helper vector type used for CG projections +class btReducedVector +{ +public: + btAlignedObjectArray m_indices; + btAlignedObjectArray m_vecs; + int m_sz; // all m_indices value < m_sz +public: + btReducedVector():m_sz(0) + { + m_indices.resize(0); + m_vecs.resize(0); + } + + btReducedVector(int sz): m_sz(sz) + { + m_indices.resize(0); + m_vecs.resize(0); + } + + btReducedVector(int sz, const btAlignedObjectArray& indices, const btAlignedObjectArray& vecs): m_sz(sz), m_indices(indices), m_vecs(vecs) + { + } + + void simplify() + { + btAlignedObjectArray old_indices(m_indices); + btAlignedObjectArray old_vecs(m_vecs); + m_indices.resize(0); + m_vecs.resize(0); + 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_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]); + } + } + 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"); + } +}; + +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 */