CG with projection + MGS working

This commit is contained in:
Xuchen Han
2020-04-08 11:27:26 -07:00
parent bbdca8a4e8
commit 460164c30e
8 changed files with 360 additions and 238 deletions
@@ -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)
@@ -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)
@@ -17,6 +17,7 @@
#include "btDeformableMultiBodyDynamicsWorld.h"
#include <algorithm>
#include <cmath>
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<btVector3>& 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<btVector3>& 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<btVector3> 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<int> indices;
btAlignedObjectArray<btVector3> 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<int> indices;
btAlignedObjectArray<btVector3> 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<int> indices;
indices.push_back(index);
btAlignedObjectArray<btVector3> 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<btVector3>& 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<btVector3>& 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<btVector3>& 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<btVector3> projections;
projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
m_projectionsDict.insert(index, projections);
}
else
{
btAlignedObjectArray<btVector3>& 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<btVector3>& projections = *m_projectionsDict[index];
for (int k = 0; k < 3; ++k)
{
projections.push_back(units[k]);
}
}
}
else
{
if (m_projectionsDict.find(index) == NULL)
{
btAlignedObjectArray<btVector3> projections;
projections.push_back(m_faceRigidConstraints[i][j].m_normal);
m_projectionsDict.insert(index, projections);
}
else
{
btAlignedObjectArray<btVector3>& 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<btVector3>& projections = *m_projectionsDict[index];
for (int k = 0; k < 3; ++k)
{
projections.push_back(units[k]);
}
}
}
else
{
if (m_projectionsDict.find(index) == NULL)
{
btAlignedObjectArray<btVector3> projections;
projections.push_back(m_deformableConstraints[i][j].m_normal);
m_projectionsDict.insert(index, projections);
}
else
{
btAlignedObjectArray<btVector3>& 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<btVector3>& projections = *m_projectionsDict[index];
for (int k = 0; k < 3; ++k)
{
projections.push_back(units[k]);
}
}
}
else
{
if (m_projectionsDict.find(index) == NULL)
{
btAlignedObjectArray<btVector3> projections;
projections.push_back(m_deformableConstraints[i][j].m_normal);
m_projectionsDict.insert(index, projections);
}
else
{
btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
projections.push_back(m_deformableConstraints[i][j].m_normal);
}
}
}
}
}
btModifiedGramSchmidt<btReducedVector> mgs(m_projections);
mgs.solve();
m_projections = mgs.m_out;
}
//void btDeformableContactProjection::setProjection()
//{
// BT_PROFILE("btDeformableContactProjection::setProjection");
// btAlignedObjectArray<btVector3> 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<btVector3>& 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<btVector3>& 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<btVector3>& projections = *m_projectionsDict[index];
// for (int k = 0; k < 3; ++k)
// {
// projections.push_back(units[k]);
// }
// }
// }
// else
// {
// if (m_projectionsDict.find(index) == NULL)
// {
// btAlignedObjectArray<btVector3> projections;
// projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
// m_projectionsDict.insert(index, projections);
// }
// else
// {
// btAlignedObjectArray<btVector3>& 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<btVector3>& projections = *m_projectionsDict[index];
// for (int k = 0; k < 3; ++k)
// {
// projections.push_back(units[k]);
// }
// }
// }
// else
// {
// if (m_projectionsDict.find(index) == NULL)
// {
// btAlignedObjectArray<btVector3> projections;
// projections.push_back(m_faceRigidConstraints[i][j].m_normal);
// m_projectionsDict.insert(index, projections);
// }
// else
// {
// btAlignedObjectArray<btVector3>& 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;
}
@@ -29,23 +29,14 @@ class btDeformableContactProjection
public:
typedef btAlignedObjectArray<btVector3> TVStack;
btAlignedObjectArray<btSoftBody *>& m_softBodies;
// // map from node index to static constraint
// btHashMap<btHashInt, btDeformableStaticConstraint> m_staticConstraints;
// // map from node index to node rigid constraint
// btHashMap<btHashInt, btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
// // map from node index to face rigid constraint
// btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> > m_faceRigidConstraints;
// // map from node index to deformable constraint
// btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> > m_deformableConstraints;
// // map from node index to node anchor constraint
// btHashMap<btHashInt, btDeformableNodeAnchorConstraint> m_nodeAnchorConstraints;
// all constraints involving face
btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;
// map from node index to projection directions
btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict;
// btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict;
btAlignedObjectArray<btReducedVector> m_projections;
// map from node index to static constraint
btAlignedObjectArray<btAlignedObjectArray<btDeformableStaticConstraint> > m_staticConstraints;
-1
View File
@@ -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())
{
+17 -6
View File
@@ -10,7 +10,7 @@
#include "btReducedVector.h"
#include "btAlignedObjectArray.h"
#include <iostream>
template<class TV>
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]));
}
+11 -3
View File
@@ -6,24 +6,29 @@
//
#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 == 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;
+53
View File
@@ -10,6 +10,18 @@
#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
{
@@ -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<int>& indices, const btAlignedObjectArray<btVector3>& vecs): m_sz(sz), m_indices(indices), m_vecs(vecs)
@@ -40,6 +56,8 @@ public:
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)
@@ -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<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);
@@ -222,6 +252,29 @@ public:
}
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)