From 460164c30eca3822789464a421bf08eaabd15ef1 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 8 Apr 2020 11:27:26 -0700 Subject: [PATCH] CG with projection + MGS working --- src/BulletSoftBody/btDeformableBodySolver.cpp | 2 +- .../btDeformableContactConstraint.cpp | 6 +- .../btDeformableContactProjection.cpp | 484 ++++++++++-------- .../btDeformableContactProjection.h | 15 +- src/BulletSoftBody/btSoftBody.cpp | 1 - src/LinearMath/btModifiedGramSchmidt.h | 23 +- src/LinearMath/btReducedVector.cpp | 14 +- src/LinearMath/btReducedVector.h | 53 ++ 8 files changed, 360 insertions(+), 238 deletions(-) diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index a334dd443..c5745fba3 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -18,7 +18,7 @@ #include "btDeformableBodySolver.h" #include "btSoftBodyInternals.h" #include "LinearMath/btQuickprof.h" -static const int kMaxConjugateGradientIterations = 50; +static const int kMaxConjugateGradientIterations = 50; btDeformableBodySolver::btDeformableBodySolver() : m_numNodes(0) , m_cg(kMaxConjugateGradientIterations) diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index 389411def..48776a199 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -464,9 +464,9 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1)); btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2)); #endif - v0 += dv0; - v1 += dv1; - v2 += dv2; +// v0 += dv0; +// v1 += dv1; +// v2 += dv2; } void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse) diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index af70ae958..1ed40d7e4 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -17,6 +17,7 @@ #include "btDeformableMultiBodyDynamicsWorld.h" #include #include +static btDeformableFaceRigidContactConstraint* pFaceConstraint; btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal) { btScalar residualSquare = 0; @@ -189,236 +190,293 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in void btDeformableContactProjection::project(TVStack& x) { - const int dim = 3; - for (int index = 0; index < m_projectionsDict.size(); ++index) - { - btAlignedObjectArray& projectionDirs = *m_projectionsDict.getAtIndex(index); - size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1(); - if (projectionDirs.size() >= dim) - { - // static node - x[i].setZero(); - continue; - } - else if (projectionDirs.size() == 2) - { - btVector3 dir0 = projectionDirs[0]; - btVector3 dir1 = projectionDirs[1]; - btVector3 free_dir = btCross(dir0, dir1); - if (free_dir.safeNorm() < SIMD_EPSILON) - { - x[i] -= x[i].dot(dir0) * dir0; - x[i] -= x[i].dot(dir1) * dir1; - } - else - { - free_dir.normalize(); - x[i] = x[i].dot(free_dir) * free_dir; - } - } - else - { - btAssert(projectionDirs.size() == 1); - btVector3 dir0 = projectionDirs[0]; - x[i] -= x[i].dot(dir0) * dir0; - } - } + btReducedVector p(x.size()); + for (int i = 0; i < m_projections.size(); ++i) + { + p += (m_projections[i].dot(x) * m_projections[i]); + } + for (int i = 0; i < p.m_indices.size(); ++i) + { + x[p.m_indices[i]] -= p.m_vecs[i]; + } +// if (pFaceConstraint) +// { +// btVector3 bary = pFaceConstraint->getContact()->m_bary; +// btVector3 p(0,0,0); +// for (int k = 0; k < 3; ++k) +// p += bary[k]*x[pFaceConstraint->m_face->m_n[k]->index]; +// printf("p = %f %f %f \n", p[0], p[1], p[2]); +// } } +//void btDeformableContactProjection::project(TVStack& x) +//{ +// const int dim = 3; +// for (int index = 0; index < m_projectionsDict.size(); ++index) +// { +// btAlignedObjectArray& projectionDirs = *m_projectionsDict.getAtIndex(index); +// size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1(); +// if (projectionDirs.size() >= dim) +// { +// // static node +// x[i].setZero(); +// continue; +// } +// else if (projectionDirs.size() == 2) +// { +// btVector3 dir0 = projectionDirs[0]; +// btVector3 dir1 = projectionDirs[1]; +// btVector3 free_dir = btCross(dir0, dir1); +// if (free_dir.safeNorm() < SIMD_EPSILON) +// { +// x[i] -= x[i].dot(dir0) * dir0; +// x[i] -= x[i].dot(dir1) * dir1; +// } +// else +// { +// free_dir.normalize(); +// x[i] = x[i].dot(free_dir) * free_dir; +// } +// } +// else +// { +// btAssert(projectionDirs.size() == 1); +// btVector3 dir0 = projectionDirs[0]; +// x[i] -= x[i].dot(dir0) * dir0; +// } +// } +//} + void btDeformableContactProjection::setProjection() { - BT_PROFILE("btDeformableContactProjection::setProjection"); - btAlignedObjectArray units; - units.push_back(btVector3(1,0,0)); - units.push_back(btVector3(0,1,0)); - units.push_back(btVector3(0,0,1)); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (!psb->isActive()) - { - continue; - } - for (int j = 0; j < m_staticConstraints[i].size(); ++j) - { - int index = m_staticConstraints[i][j].m_node->index; - m_staticConstraints[i][j].m_node->m_constrained = true; - if (m_projectionsDict.find(index) == NULL) + int dof = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + dof += m_softBodies[i]->m_nodes.size(); + } + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < m_staticConstraints[i].size(); ++j) + { + int index = m_staticConstraints[i][j].m_node->index; + m_staticConstraints[i][j].m_node->m_constrained = true; + btAlignedObjectArray indices; + btAlignedObjectArray vecs1,vecs2,vecs3; + indices.push_back(index); + vecs1.push_back(btVector3(1,0,0)); + vecs2.push_back(btVector3(0,1,0)); + vecs3.push_back(btVector3(0,0,1)); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + m_projections.push_back(btReducedVector(dof, indices, vecs2)); + m_projections.push_back(btReducedVector(dof, indices, vecs3)); + } + + for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) + { + int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; + m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true; + btAlignedObjectArray indices; + btAlignedObjectArray vecs1,vecs2,vecs3; + indices.push_back(index); + vecs1.push_back(btVector3(1,0,0)); + vecs2.push_back(btVector3(0,1,0)); + vecs3.push_back(btVector3(0,0,1)); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + m_projections.push_back(btReducedVector(dof, indices, vecs2)); + m_projections.push_back(btReducedVector(dof, indices, vecs3)); + } + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) + { + int index = m_nodeRigidConstraints[i][j].m_node->index; + m_nodeRigidConstraints[i][j].m_node->m_constrained = true; + btAlignedObjectArray indices; + indices.push_back(index); + btAlignedObjectArray vecs1,vecs2,vecs3; + if (m_nodeRigidConstraints[i][j].m_static) + { + vecs1.push_back(btVector3(1,0,0)); + vecs2.push_back(btVector3(0,1,0)); + vecs3.push_back(btVector3(0,0,1)); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + m_projections.push_back(btReducedVector(dof, indices, vecs2)); + m_projections.push_back(btReducedVector(dof, indices, vecs3)); + } + else + { + vecs1.push_back(m_nodeRigidConstraints[i][j].m_normal); + m_projections.push_back(btReducedVector(dof, indices, vecs1)); + } + } + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + { + const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; + btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary; + if (m_faceRigidConstraints[i][j].m_static) { - m_projectionsDict.insert(index, units); - } - else - { - btAlignedObjectArray& projections = *m_projectionsDict[index]; - for (int k = 0; k < 3; ++k) + if (!pFaceConstraint) + pFaceConstraint = &m_faceRigidConstraints[i][j]; + for (int l = 0; l < 3; ++l) { - projections.push_back(units[k]); - } - } - } - for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) - { - int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; - m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true; - if (m_projectionsDict.find(index) == NULL) - { - m_projectionsDict.insert(index, units); - } - else - { - btAlignedObjectArray& projections = *m_projectionsDict[index]; - for (int k = 0; k < 3; ++k) - { - projections.push_back(units[k]); - } - } - } - for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) - { - int index = m_nodeRigidConstraints[i][j].m_node->index; - m_nodeRigidConstraints[i][j].m_node->m_constrained = true; - if (m_nodeRigidConstraints[i][j].m_static) - { - if (m_projectionsDict.find(index) == NULL) - { - m_projectionsDict.insert(index, units); - } - else - { - btAlignedObjectArray& projections = *m_projectionsDict[index]; + face->m_n[l]->m_constrained = true; + btReducedVector rv(dof); for (int k = 0; k < 3; ++k) { - projections.push_back(units[k]); + rv.m_indices.push_back(face->m_n[k]->index); + btVector3 v(0,0,0); + v[l] = bary[k]; + rv.m_vecs.push_back(v); + rv.sort(); } + m_projections.push_back(rv); } } else { - if (m_projectionsDict.find(index) == NULL) + btReducedVector rv(dof); + for (int k = 0; k < 3; ++k) { - btAlignedObjectArray projections; - projections.push_back(m_nodeRigidConstraints[i][j].m_normal); - m_projectionsDict.insert(index, projections); - } - else - { - btAlignedObjectArray& projections = *m_projectionsDict[index]; - projections.push_back(m_nodeRigidConstraints[i][j].m_normal); + rv.m_indices.push_back(face->m_n[k]->index); + rv.m_vecs.push_back(bary[k] * m_faceRigidConstraints[i][j].m_normal); + rv.sort(); } + m_projections.push_back(rv); } } - for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) - { - const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; - for (int k = 0; k < 3; ++k) - { - btSoftBody::Node* node = face->m_n[k]; - node->m_constrained = true; - int index = node->index; - if (m_faceRigidConstraints[i][j].m_static) - { - if (m_projectionsDict.find(index) == NULL) - { - m_projectionsDict.insert(index, units); - } - else - { - btAlignedObjectArray& projections = *m_projectionsDict[index]; - for (int k = 0; k < 3; ++k) - { - projections.push_back(units[k]); - } - } - } - else - { - if (m_projectionsDict.find(index) == NULL) - { - btAlignedObjectArray projections; - projections.push_back(m_faceRigidConstraints[i][j].m_normal); - m_projectionsDict.insert(index, projections); - } - else - { - btAlignedObjectArray& projections = *m_projectionsDict[index]; - projections.push_back(m_faceRigidConstraints[i][j].m_normal); - } - } - } - } - for (int j = 0; j < m_deformableConstraints[i].size(); ++j) - { - const btSoftBody::Face* face = m_deformableConstraints[i][j].m_face; - for (int k = 0; k < 3; ++k) - { - const btSoftBody::Node* node = face->m_n[k]; - int index = node->index; - if (m_deformableConstraints[i][j].m_static) - { - if (m_projectionsDict.find(index) == NULL) - { - m_projectionsDict.insert(index, units); - } - else - { - btAlignedObjectArray& projections = *m_projectionsDict[index]; - for (int k = 0; k < 3; ++k) - { - projections.push_back(units[k]); - } - } - } - else - { - if (m_projectionsDict.find(index) == NULL) - { - btAlignedObjectArray projections; - projections.push_back(m_deformableConstraints[i][j].m_normal); - m_projectionsDict.insert(index, projections); - } - else - { - btAlignedObjectArray& projections = *m_projectionsDict[index]; - projections.push_back(m_deformableConstraints[i][j].m_normal); - } - } - } - - const btSoftBody::Node* node = m_deformableConstraints[i][j].m_node; - int index = node->index; - if (m_deformableConstraints[i][j].m_static) - { - if (m_projectionsDict.find(index) == NULL) - { - m_projectionsDict.insert(index, units); - } - else - { - btAlignedObjectArray& projections = *m_projectionsDict[index]; - for (int k = 0; k < 3; ++k) - { - projections.push_back(units[k]); - } - } - } - else - { - if (m_projectionsDict.find(index) == NULL) - { - btAlignedObjectArray projections; - projections.push_back(m_deformableConstraints[i][j].m_normal); - m_projectionsDict.insert(index, projections); - } - else - { - btAlignedObjectArray& projections = *m_projectionsDict[index]; - projections.push_back(m_deformableConstraints[i][j].m_normal); - } - } - } - } + } + btModifiedGramSchmidt mgs(m_projections); + mgs.solve(); + m_projections = mgs.m_out; } +//void btDeformableContactProjection::setProjection() +//{ +// BT_PROFILE("btDeformableContactProjection::setProjection"); +// btAlignedObjectArray units; +// units.push_back(btVector3(1,0,0)); +// units.push_back(btVector3(0,1,0)); +// units.push_back(btVector3(0,0,1)); +// for (int i = 0; i < m_softBodies.size(); ++i) +// { +// btSoftBody* psb = m_softBodies[i]; +// if (!psb->isActive()) +// { +// continue; +// } +// for (int j = 0; j < m_staticConstraints[i].size(); ++j) +// { +// int index = m_staticConstraints[i][j].m_node->index; +// m_staticConstraints[i][j].m_node->m_constrained = true; +// if (m_projectionsDict.find(index) == NULL) +// { +// m_projectionsDict.insert(index, units); +// } +// else +// { +// btAlignedObjectArray& projections = *m_projectionsDict[index]; +// for (int k = 0; k < 3; ++k) +// { +// projections.push_back(units[k]); +// } +// } +// } +// for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) +// { +// int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; +// m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true; +// if (m_projectionsDict.find(index) == NULL) +// { +// m_projectionsDict.insert(index, units); +// } +// else +// { +// btAlignedObjectArray& projections = *m_projectionsDict[index]; +// for (int k = 0; k < 3; ++k) +// { +// projections.push_back(units[k]); +// } +// } +// } +// for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) +// { +// int index = m_nodeRigidConstraints[i][j].m_node->index; +// m_nodeRigidConstraints[i][j].m_node->m_constrained = true; +// if (m_nodeRigidConstraints[i][j].m_static) +// { +// if (m_projectionsDict.find(index) == NULL) +// { +// m_projectionsDict.insert(index, units); +// } +// else +// { +// btAlignedObjectArray& projections = *m_projectionsDict[index]; +// for (int k = 0; k < 3; ++k) +// { +// projections.push_back(units[k]); +// } +// } +// } +// else +// { +// if (m_projectionsDict.find(index) == NULL) +// { +// btAlignedObjectArray projections; +// projections.push_back(m_nodeRigidConstraints[i][j].m_normal); +// m_projectionsDict.insert(index, projections); +// } +// else +// { +// btAlignedObjectArray& projections = *m_projectionsDict[index]; +// projections.push_back(m_nodeRigidConstraints[i][j].m_normal); +// } +// } +// } +// for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) +// { +// const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; +// for (int k = 0; k < 3; ++k) +// { +// btSoftBody::Node* node = face->m_n[k]; +// node->m_constrained = true; +// int index = node->index; +// if (m_faceRigidConstraints[i][j].m_static) +// { +// if (m_projectionsDict.find(index) == NULL) +// { +// m_projectionsDict.insert(index, units); +// } +// else +// { +// btAlignedObjectArray& projections = *m_projectionsDict[index]; +// for (int k = 0; k < 3; ++k) +// { +// projections.push_back(units[k]); +// } +// } +// } +// else +// { +// if (m_projectionsDict.find(index) == NULL) +// { +// btAlignedObjectArray projections; +// projections.push_back(m_faceRigidConstraints[i][j].m_normal); +// m_projectionsDict.insert(index, projections); +// } +// else +// { +// btAlignedObjectArray& projections = *m_projectionsDict[index]; +// projections.push_back(m_faceRigidConstraints[i][j].m_normal); +// } +// } +// } +// } +// } +//} + void btDeformableContactProjection::applyDynamicFriction(TVStack& f) { @@ -491,7 +549,9 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated) m_faceRigidConstraints[i].clear(); m_deformableConstraints[i].clear(); } - m_projectionsDict.clear(); +// m_projectionsDict.clear(); + m_projections.clear(); + pFaceConstraint = 0; } diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index a59bedda7..7fc66569c 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -29,23 +29,14 @@ class btDeformableContactProjection public: typedef btAlignedObjectArray TVStack; btAlignedObjectArray& m_softBodies; - -// // map from node index to static constraint -// btHashMap m_staticConstraints; -// // map from node index to node rigid constraint -// btHashMap > m_nodeRigidConstraints; -// // map from node index to face rigid constraint -// btHashMap > m_faceRigidConstraints; -// // map from node index to deformable constraint -// btHashMap > m_deformableConstraints; -// // map from node index to node anchor constraint -// btHashMap m_nodeAnchorConstraints; // all constraints involving face btAlignedObjectArray m_allFaceConstraints; // map from node index to projection directions - btHashMap > m_projectionsDict; +// btHashMap > m_projectionsDict; + + btAlignedObjectArray m_projections; // map from node index to static constraint btAlignedObjectArray > m_staticConstraints; diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 149cf4f4b..27a175b5a 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -4020,7 +4020,6 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap break; case fCollision::SDF_RD: { - btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject()); if (pcoWrap->getCollisionObject()->isActive() || this->isActive()) { diff --git a/src/LinearMath/btModifiedGramSchmidt.h b/src/LinearMath/btModifiedGramSchmidt.h index 6bf35244b..6aee9e1bc 100644 --- a/src/LinearMath/btModifiedGramSchmidt.h +++ b/src/LinearMath/btModifiedGramSchmidt.h @@ -10,7 +10,7 @@ #include "btReducedVector.h" #include "btAlignedObjectArray.h" - +#include template class btModifiedGramSchmidt { @@ -28,22 +28,33 @@ public: 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(); +// v.print(); for (int j = 0; j < i; ++j) { v = v - v.proj(m_out[j]); - v.print(); +// v.print(); } v.normalize(); - v.print(); m_out[i] = v; - printf("===========\n"); +// 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) { @@ -51,7 +62,7 @@ public: { 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) + 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])); } diff --git a/src/LinearMath/btReducedVector.cpp b/src/LinearMath/btReducedVector.cpp index b5665b757..1539584e7 100644 --- a/src/LinearMath/btReducedVector.cpp +++ b/src/LinearMath/btReducedVector.cpp @@ -6,24 +6,29 @@ // #include #include "btReducedVector.h" +#include // 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) + if (other_length2 < SIMD_EPSILON) { return ret; } - return other*(this->dot(other) / other_length2); + return other*(this->dot(other))/other_length2; } void btReducedVector::normalize() { if (this->length2() < SIMD_EPSILON) + { + m_indices.clear(); + m_vecs.clear(); return; - *this /= btSqrt(this->length2()); + } + *this /= std::sqrt(this->length2()); } bool btReducedVector::testAdd() const @@ -119,6 +124,9 @@ bool btReducedVector::testDot() const 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; diff --git a/src/LinearMath/btReducedVector.h b/src/LinearMath/btReducedVector.h index d1b7b9809..83b5e581e 100644 --- a/src/LinearMath/btReducedVector.h +++ b/src/LinearMath/btReducedVector.h @@ -10,6 +10,18 @@ #include "btMatrix3x3.h" #include "btAlignedObjectArray.h" #include +#include +#include +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 { @@ -22,12 +34,16 @@ public: { 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& indices, const btAlignedObjectArray& vecs): m_sz(sz), m_indices(indices), m_vecs(vecs) @@ -40,6 +56,8 @@ public: btAlignedObjectArray 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) @@ -171,6 +189,7 @@ public: { return *this; } + m_sz = other.m_sz; m_indices.copyFromArray(other.m_indices); m_vecs.copyFromArray(other.m_vecs); return *this; @@ -189,11 +208,22 @@ public: 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& 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); @@ -222,6 +252,29 @@ public: } printf("\n"); } + + + void sort() + { + std::vector 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 new_indices; + btAlignedObjectArray 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)