mirror of
https://github.com/bulletphysics/bullet3.git
synced 2026-11-17 05:06:07 +00:00
CG with projection + MGS working
This commit is contained in:
@@ -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;
|
||||
|
||||
@@ -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())
|
||||
{
|
||||
|
||||
@@ -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]));
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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)
|
||||
|
||||
Reference in New Issue
Block a user