summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletSoftBody/btSoftBody.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletSoftBody/btSoftBody.cpp')
-rw-r--r--thirdparty/bullet/BulletSoftBody/btSoftBody.cpp636
1 files changed, 524 insertions, 112 deletions
diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBody.cpp b/thirdparty/bullet/BulletSoftBody/btSoftBody.cpp
index 2a458b1d80..81b846d7f8 100644
--- a/thirdparty/bullet/BulletSoftBody/btSoftBody.cpp
+++ b/thirdparty/bullet/BulletSoftBody/btSoftBody.cpp
@@ -18,6 +18,7 @@ subject to the following restrictions:
#include "BulletSoftBody/btSoftBodySolvers.h"
#include "btSoftBodyData.h"
#include "LinearMath/btSerializer.h"
+#include "LinearMath/btImplicitQRSVD.h"
#include "LinearMath/btAlignedAllocator.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
@@ -25,6 +26,107 @@ subject to the following restrictions:
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
#include <iostream>
//
+static inline btDbvtNode* buildTreeBottomUp(btAlignedObjectArray<btDbvtNode*>& leafNodes, btAlignedObjectArray<btAlignedObjectArray<int> >& adj)
+{
+ int N = leafNodes.size();
+ if (N == 0)
+ {
+ return NULL;
+ }
+ while (N > 1)
+ {
+ btAlignedObjectArray<bool> marked;
+ btAlignedObjectArray<btDbvtNode*> newLeafNodes;
+ btAlignedObjectArray<std::pair<int,int> > childIds;
+ btAlignedObjectArray<btAlignedObjectArray<int> > newAdj;
+ marked.resize(N);
+ for (int i = 0; i < N; ++i)
+ marked[i] = false;
+
+ // pair adjacent nodes into new(parent) node
+ for (int i = 0; i < N; ++i)
+ {
+ if (marked[i])
+ continue;
+ bool merged = false;
+ for (int j = 0; j < adj[i].size(); ++j)
+ {
+ int n = adj[i][j];
+ if (!marked[adj[i][j]])
+ {
+ btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
+ node->parent = NULL;
+ node->childs[0] = leafNodes[i];
+ node->childs[1] = leafNodes[n];
+ leafNodes[i]->parent = node;
+ leafNodes[n]->parent = node;
+ newLeafNodes.push_back(node);
+ childIds.push_back(std::make_pair(i,n));
+ merged = true;
+ marked[n] = true;
+ break;
+ }
+ }
+ if (!merged)
+ {
+ newLeafNodes.push_back(leafNodes[i]);
+ childIds.push_back(std::make_pair(i,-1));
+ }
+ marked[i] = true;
+ }
+ // update adjacency matrix
+ newAdj.resize(newLeafNodes.size());
+ for (int i = 0; i < newLeafNodes.size(); ++i)
+ {
+ for (int j = i+1; j < newLeafNodes.size(); ++j)
+ {
+ bool neighbor = false;
+ const btAlignedObjectArray<int>& leftChildNeighbors = adj[childIds[i].first];
+ for (int k = 0; k < leftChildNeighbors.size(); ++k)
+ {
+ if (leftChildNeighbors[k] == childIds[j].first || leftChildNeighbors[k] == childIds[j].second)
+ {
+ neighbor = true;
+ break;
+ }
+ }
+ if (!neighbor && childIds[i].second != -1)
+ {
+ const btAlignedObjectArray<int>& rightChildNeighbors = adj[childIds[i].second];
+ for (int k = 0; k < rightChildNeighbors.size(); ++k)
+ {
+ if (rightChildNeighbors[k] == childIds[j].first || rightChildNeighbors[k] == childIds[j].second)
+ {
+ neighbor = true;
+ break;
+ }
+ }
+ }
+ if (neighbor)
+ {
+ newAdj[i].push_back(j);
+ newAdj[j].push_back(i);
+ }
+ }
+ }
+ leafNodes = newLeafNodes;
+ //this assignment leaks memory, the assignment doesn't do a deep copy, for now a manual copy
+ //adj = newAdj;
+ adj.clear();
+ adj.resize(newAdj.size());
+ for (int i = 0; i < newAdj.size(); i++)
+ {
+ for (int j = 0; j < newAdj[i].size(); j++)
+ {
+ adj[i].push_back(newAdj[i][j]);
+ }
+ }
+ N = leafNodes.size();
+ }
+ return leafNodes[0];
+}
+
+//
btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
: m_softBodySolver(0), m_worldInfo(worldInfo)
{
@@ -41,6 +143,7 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btV
/* Nodes */
const btScalar margin = getCollisionShape()->getMargin();
m_nodes.resize(node_count);
+ m_X.resize(node_count);
for (int i = 0, ni = node_count; i < ni; ++i)
{
Node& n = m_nodes[i];
@@ -51,8 +154,11 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btV
n.m_im = n.m_im > 0 ? 1 / n.m_im : 0;
n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n);
n.m_material = pm;
+ m_X[i] = n.m_x;
}
updateBounds();
+ setCollisionQuadrature(3);
+ m_fdbvnt = 0;
}
btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo)
@@ -111,15 +217,18 @@ void btSoftBody::initDefaults()
m_collisionShape = new btSoftBodyCollisionShape(this);
m_collisionShape->setMargin(0.25f);
- m_initialWorldTransform.setIdentity();
+ m_worldTransform.setIdentity();
m_windVelocity = btVector3(0, 0, 0);
m_restLengthScale = btScalar(1.0);
- m_dampingCoefficient = 1;
- m_sleepingThreshold = 0.1;
- m_useFaceContact = true;
+ m_dampingCoefficient = 1.0;
+ m_sleepingThreshold = .4;
m_useSelfCollision = false;
- m_collisionFlags = 0;
+ m_collisionFlags = 0;
+ m_softSoftCollision = false;
+ m_maxSpeedSquared = 0;
+ m_repulsionStiffness = 0.5;
+ m_fdbvnt = 0;
}
//
@@ -134,6 +243,8 @@ btSoftBody::~btSoftBody()
btAlignedFree(m_materials[i]);
for (i = 0; i < m_joints.size(); ++i)
btAlignedFree(m_joints[i]);
+ if (m_fdbvnt)
+ delete m_fdbvnt;
}
//
@@ -897,6 +1008,71 @@ void btSoftBody::setVolumeDensity(btScalar density)
}
//
+btVector3 btSoftBody::getLinearVelocity()
+{
+ btVector3 total_momentum = btVector3(0,0,0);
+ for (int i = 0; i < m_nodes.size(); ++i)
+ {
+ btScalar mass = m_nodes[i].m_im == 0 ? 0 : 1.0/m_nodes[i].m_im;
+ total_momentum += mass * m_nodes[i].m_v;
+ }
+ btScalar total_mass = getTotalMass();
+ return total_mass == 0 ? total_momentum : total_momentum / total_mass;
+}
+
+//
+void btSoftBody::setLinearVelocity(const btVector3& linVel)
+{
+ btVector3 old_vel = getLinearVelocity();
+ btVector3 diff = linVel - old_vel;
+ for (int i = 0; i < m_nodes.size(); ++i)
+ m_nodes[i].m_v += diff;
+}
+
+//
+void btSoftBody::setAngularVelocity(const btVector3& angVel)
+{
+ btVector3 old_vel = getLinearVelocity();
+ btVector3 com = getCenterOfMass();
+ for (int i = 0; i < m_nodes.size(); ++i)
+ {
+ m_nodes[i].m_v = angVel.cross(m_nodes[i].m_x - com) + old_vel;
+ }
+}
+
+//
+btTransform btSoftBody::getRigidTransform()
+{
+ btVector3 t = getCenterOfMass();
+ btMatrix3x3 S;
+ S.setZero();
+ // get rotation that minimizes L2 difference: \sum_i || RX_i + t - x_i ||
+ for (int i = 0; i < m_nodes.size(); ++i)
+ {
+ S += OuterProduct(m_X[i], t-m_nodes[i].m_x);
+ }
+ btVector3 sigma;
+ btMatrix3x3 U,V;
+ singularValueDecomposition(S,U,sigma,V);
+ btMatrix3x3 R = V * U.transpose();
+ btTransform trs;
+ trs.setIdentity();
+ trs.setOrigin(t);
+ trs.setBasis(R);
+ return trs;
+}
+
+//
+void btSoftBody::transformTo(const btTransform& trs)
+{
+ // get the current best rigid fit
+ btTransform current_transform = getRigidTransform();
+ // apply transform in material space
+ btTransform new_transform = trs * current_transform.inverse();
+ transform(new_transform);
+}
+
+//
void btSoftBody::transform(const btTransform& trs)
{
const btScalar margin = getCollisionShape()->getMargin();
@@ -916,7 +1092,6 @@ void btSoftBody::transform(const btTransform& trs)
updateNormals();
updateBounds();
updateConstants();
- m_initialWorldTransform = trs;
}
//
@@ -1834,6 +2009,25 @@ bool btSoftBody::rayTest(const btVector3& rayFrom,
return (rayTest(rayFrom, rayTo, results.fraction, results.feature, results.index, false) != 0);
}
+bool btSoftBody::rayFaceTest(const btVector3& rayFrom,
+ const btVector3& rayTo,
+ sRayCast& results)
+{
+ if (m_faces.size() == 0)
+ return false;
+ else
+ {
+ if (m_fdbvt.empty())
+ initializeFaceTree();
+ }
+
+ results.body = this;
+ results.fraction = 1.f;
+ results.index = -1;
+
+ return (rayFaceTest(rayFrom, rayTo, results.fraction, results.index) != 0);
+}
+
//
void btSoftBody::setSolver(eSolverPresets::_ preset)
{
@@ -2339,15 +2533,160 @@ int btSoftBody::rayTest(const btVector3& rayFrom, const btVector3& rayTo,
return (cnt);
}
+int btSoftBody::rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo,
+ btScalar& mint, int& index) const
+{
+ int cnt = 0;
+ { /* Use dbvt */
+ RayFromToCaster collider(rayFrom, rayTo, mint);
+
+ btDbvt::rayTest(m_fdbvt.m_root, rayFrom, rayTo, collider);
+ if (collider.m_face)
+ {
+ mint = collider.m_mint;
+ index = (int)(collider.m_face - &m_faces[0]);
+ cnt = 1;
+ }
+ }
+ return (cnt);
+}
+
+
//
+static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n)
+{
+ if (n == 0)
+ return 0;
+ btDbvntNode* root = new btDbvntNode(n);
+ if (n->isinternal())
+ {
+ btDbvntNode* c0 = copyToDbvnt(n->childs[0]);
+ root->childs[0] = c0;
+ btDbvntNode* c1 = copyToDbvnt(n->childs[1]);
+ root->childs[1] = c1;
+ }
+ return root;
+}
+
+static inline void calculateNormalCone(btDbvntNode* root)
+{
+ if (!root)
+ return;
+ if (root->isleaf())
+ {
+ const btSoftBody::Face* face = (btSoftBody::Face*)root->data;
+ root->normal = face->m_normal;
+ root->angle = 0;
+ }
+ else
+ {
+ btVector3 n0(0,0,0), n1(0,0,0);
+ btScalar a0 = 0, a1 = 0;
+ if (root->childs[0])
+ {
+ calculateNormalCone(root->childs[0]);
+ n0 = root->childs[0]->normal;
+ a0 = root->childs[0]->angle;
+ }
+ if (root->childs[1])
+ {
+ calculateNormalCone(root->childs[1]);
+ n1 = root->childs[1]->normal;
+ a1 = root->childs[1]->angle;
+ }
+ root->normal = (n0+n1).safeNormalize();
+ root->angle = btMax(a0,a1) + btAngle(n0, n1)*0.5;
+ }
+}
+
void btSoftBody::initializeFaceTree()
{
+ BT_PROFILE("btSoftBody::initializeFaceTree");
m_fdbvt.clear();
+ // create leaf nodes;
+ btAlignedObjectArray<btDbvtNode*> leafNodes;
+ leafNodes.resize(m_faces.size());
for (int i = 0; i < m_faces.size(); ++i)
{
Face& f = m_faces[i];
- f.m_leaf = m_fdbvt.insert(VolumeOf(f, 0), &f);
+ ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = VolumeOf(f, 0);
+ btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
+ node->parent = NULL;
+ node->data = &f;
+ node->childs[1] = 0;
+ node->volume = vol;
+ leafNodes[i] = node;
+ f.m_leaf = node;
}
+ btAlignedObjectArray<btAlignedObjectArray<int> > adj;
+ adj.resize(m_faces.size());
+ // construct the adjacency list for triangles
+ for (int i = 0; i < adj.size(); ++i)
+ {
+ for (int j = i+1; j < adj.size(); ++j)
+ {
+ int dup = 0;
+ for (int k = 0; k < 3; ++k)
+ {
+ for (int l = 0; l < 3; ++l)
+ {
+ if (m_faces[i].m_n[k] == m_faces[j].m_n[l])
+ {
+ ++dup;
+ break;
+ }
+ }
+ if (dup == 2)
+ {
+ adj[i].push_back(j);
+ adj[j].push_back(i);
+ }
+ }
+ }
+ }
+ m_fdbvt.m_root = buildTreeBottomUp(leafNodes, adj);
+ if (m_fdbvnt)
+ delete m_fdbvnt;
+ m_fdbvnt = copyToDbvnt(m_fdbvt.m_root);
+ updateFaceTree(false, false);
+ rebuildNodeTree();
+}
+
+//
+void btSoftBody::rebuildNodeTree()
+{
+ m_ndbvt.clear();
+ btAlignedObjectArray<btDbvtNode*> leafNodes;
+ leafNodes.resize(m_nodes.size());
+ for (int i = 0; i < m_nodes.size(); ++i)
+ {
+ Node& n = m_nodes[i];
+ ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = btDbvtVolume::FromCR(n.m_x, 0);
+ btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
+ node->parent = NULL;
+ node->data = &n;
+ node->childs[1] = 0;
+ node->volume = vol;
+ leafNodes[i] = node;
+ n.m_leaf = node;
+ }
+ btAlignedObjectArray<btAlignedObjectArray<int> > adj;
+ adj.resize(m_nodes.size());
+ btAlignedObjectArray<int> old_id;
+ old_id.resize(m_nodes.size());
+ for (int i = 0; i < m_nodes.size(); ++i)
+ old_id[i] = m_nodes[i].index;
+ for (int i = 0; i < m_nodes.size(); ++i)
+ m_nodes[i].index = i;
+ for (int i = 0; i < m_links.size(); ++i)
+ {
+ Link& l = m_links[i];
+ adj[l.m_n[0]->index].push_back(l.m_n[1]->index);
+ adj[l.m_n[1]->index].push_back(l.m_n[0]->index);
+ }
+ m_ndbvt.m_root = buildTreeBottomUp(leafNodes, adj);
+ for (int i = 0; i < m_nodes.size(); ++i)
+ m_nodes[i].index = old_id[i];
}
//
@@ -2403,10 +2742,9 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr
const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
// use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
// but resolve contact at x_n
-// btTransform wtr = (predict) ?
-// (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
-// : colObjWrap->getWorldTransform();
- const btTransform& wtr = colObjWrap->getWorldTransform();
+ btTransform wtr = (predict) ?
+ (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
+ : colObjWrap->getWorldTransform();
btScalar dst =
m_worldInfo->m_sparsesdf.Evaluate(
wtr.invXform(x),
@@ -2457,7 +2795,6 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
btTransform wtr = (predict) ?
(colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
: colObjWrap->getWorldTransform();
-// const btTransform& wtr = colObjWrap->getWorldTransform();
btScalar dst;
//#define USE_QUADRATURE 1
@@ -2476,6 +2813,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
nrm,
margin);
nrm = wtr.getBasis() * nrm;
+ cti.m_colObj = colObjWrap->getCollisionObject();
// use cached contact point
}
else
@@ -2492,10 +2830,11 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
contact_point = results.witnesses[0];
getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
nrm = results.normal;
+ cti.m_colObj = colObjWrap->getCollisionObject();
for (int i = 0; i < 3; ++i)
f.m_pcontact[i] = bary[i];
}
-
+ return (dst < 0);
#endif
// use collision quadrature point
@@ -2505,7 +2844,11 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
btVector3 local_nrm;
for (int q = 0; q < m_quads.size(); ++q)
{
- btVector3 p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]);
+ btVector3 p;
+ if (predict)
+ p = BaryEval(f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, m_quads[q]);
+ else
+ p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]);
btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate(
wtr.invXform(p),
shp,
@@ -2513,43 +2856,83 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
margin);
if (local_dst < dst)
{
+ if (local_dst < 0 && predict)
+ return true;
dst = local_dst;
contact_point = p;
bary = m_quads[q];
- nrm = wtr.getBasis() * local_nrm;
+ nrm = local_nrm;
+ }
+ if (!predict)
+ {
+ cti.m_colObj = colObjWrap->getCollisionObject();
+ cti.m_normal = wtr.getBasis() * nrm;
+ cti.m_offset = dst;
}
}
+ return (dst < 0);
}
#endif
+// // regular face contact
+// {
+// btGjkEpaSolver2::sResults results;
+// btTransform triangle_transform;
+// triangle_transform.setIdentity();
+// triangle_transform.setOrigin(f.m_n[0]->m_x);
+// btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
+// btVector3 guess(0,0,0);
+// if (predict)
+// {
+// triangle_transform.setOrigin(f.m_n[0]->m_q);
+// triangle = btTriangleShape(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q);
+// }
+// const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
+//// btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
+//// dst = results.distance - margin;
+//// contact_point = results.witnesses[0];
+// btGjkEpaSolver2::Penetration(&triangle, triangle_transform, csh, wtr, guess, results);
+// if (results.status == btGjkEpaSolver2::sResults::Separated)
+// return false;
+// dst = results.distance - margin;
+// contact_point = results.witnesses[1];
+// getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
+// nrm = results.normal;
+// for (int i = 0; i < 3; ++i)
+// f.m_pcontact[i] = bary[i];
+// }
+//
+// if (!predict)
+// {
+// cti.m_colObj = colObjWrap->getCollisionObject();
+// cti.m_normal = nrm;
+// cti.m_offset = dst;
+// }
+//
+
// regular face contact
{
btGjkEpaSolver2::sResults results;
btTransform triangle_transform;
triangle_transform.setIdentity();
- triangle_transform.setOrigin(f.m_n[0]->m_x);
- btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
+ triangle_transform.setOrigin(f.m_n[0]->m_q);
+ btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q);
btVector3 guess(0,0,0);
const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
- dst = results.distance - margin;
+ dst = results.distance-csh->getMargin();
+ dst -= margin;
+ if (dst >= 0)
+ return false;
contact_point = results.witnesses[0];
- getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
+ getBarycentric(contact_point, f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, bary);
+ btVector3 curr = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
nrm = results.normal;
- for (int i = 0; i < 3; ++i)
- f.m_pcontact[i] = bary[i];
- }
-
- if (!predict)
- {
cti.m_colObj = colObjWrap->getCollisionObject();
cti.m_normal = nrm;
- cti.m_offset = dst;
+ cti.m_offset = dst + (curr - contact_point).dot(nrm);
}
-
- if (dst < 0)
- return true;
- return (false);
+ return (dst < 0);
}
//
@@ -3075,6 +3458,7 @@ void btSoftBody::setSpringStiffness(btScalar k)
{
m_links[i].Feature::m_material->m_kLST = k;
}
+ m_repulsionStiffness = k;
}
void btSoftBody::initializeDmInverse()
@@ -3372,18 +3756,39 @@ void btSoftBody::setMaxStress(btScalar maxStress)
//
void btSoftBody::interpolateRenderMesh()
{
- for (int i = 0; i < m_renderNodes.size(); ++i)
- {
- Node& n = m_renderNodes[i];
- n.m_x.setZero();
- for (int j = 0; j < 4; ++j)
- {
- if (m_renderNodesParents[i].size())
+ if (m_z.size() > 0)
+ {
+ for (int i = 0; i < m_renderNodes.size(); ++i)
+ {
+ const Node* p0 = m_renderNodesParents[i][0];
+ const Node* p1 = m_renderNodesParents[i][1];
+ const Node* p2 = m_renderNodesParents[i][2];
+ btVector3 normal = btCross(p1->m_x - p0->m_x, p2->m_x - p0->m_x);
+ btVector3 unit_normal = normal.normalized();
+ Node& n = m_renderNodes[i];
+ n.m_x.setZero();
+ for (int j = 0; j < 3; ++j)
{
n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j];
}
- }
- }
+ n.m_x += m_z[i] * unit_normal;
+ }
+ }
+ else
+ {
+ for (int i = 0; i < m_renderNodes.size(); ++i)
+ {
+ Node& n = m_renderNodes[i];
+ n.m_x.setZero();
+ for (int j = 0; j < 4; ++j)
+ {
+ if (m_renderNodesParents[i].size())
+ {
+ n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j];
+ }
+ }
+ }
+ }
}
void btSoftBody::setCollisionQuadrature(int N)
@@ -3649,13 +4054,10 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
break;
case fCollision::SDF_RD:
{
-
btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject());
if (pcoWrap->getCollisionObject()->isActive() || this->isActive())
{
const btTransform wtr = pcoWrap->getWorldTransform();
-// const btTransform ctr = pcoWrap->getWorldTransform();
-// const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length();
const btScalar timemargin = 0;
const btScalar basemargin = getCollisionShape()->getMargin();
btVector3 mins;
@@ -3667,22 +4069,25 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
maxs);
volume = btDbvtVolume::FromMM(mins, maxs);
volume.Expand(btVector3(basemargin, basemargin, basemargin));
- btSoftColliders::CollideSDF_RD docollideNode;
- docollideNode.psb = this;
- docollideNode.m_colObj1Wrap = pcoWrap;
- docollideNode.m_rigidBody = prb1;
- docollideNode.dynmargin = basemargin + timemargin;
- docollideNode.stamargin = basemargin;
- m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode);
-
- if (this->m_useFaceContact)
+ if (m_cfg.collisions & fCollision::SDF_RDN)
+ {
+ btSoftColliders::CollideSDF_RD docollideNode;
+ docollideNode.psb = this;
+ docollideNode.m_colObj1Wrap = pcoWrap;
+ docollideNode.m_rigidBody = prb1;
+ docollideNode.dynmargin = basemargin + timemargin;
+ docollideNode.stamargin = basemargin;
+ m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode);
+ }
+
+ if (((pcoWrap->getCollisionObject()->getInternalType() == CO_RIGID_BODY) && (m_cfg.collisions & fCollision::SDF_RDF)) || ((pcoWrap->getCollisionObject()->getInternalType() == CO_FEATHERSTONE_LINK) && (m_cfg.collisions & fCollision::SDF_MDF)))
{
btSoftColliders::CollideSDF_RDF docollideFace;
docollideFace.psb = this;
docollideFace.m_colObj1Wrap = pcoWrap;
docollideFace.m_rigidBody = prb1;
- docollideFace.dynmargin = basemargin + timemargin;
- docollideFace.stamargin = basemargin;
+ docollideFace.dynmargin = basemargin + timemargin;
+ docollideFace.stamargin = basemargin;
m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
}
}
@@ -3691,51 +4096,6 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
}
}
-static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n)
-{
- if (n == 0)
- return 0;
- btDbvntNode* root = new btDbvntNode(n);
- if (n->isinternal())
- {
- btDbvntNode* c0 = copyToDbvnt(n->childs[0]);
- root->childs[0] = c0;
- btDbvntNode* c1 = copyToDbvnt(n->childs[1]);
- root->childs[1] = c1;
- }
- return root;
-}
-
-static inline void calculateNormalCone(btDbvntNode* root)
-{
- if (!root)
- return;
- if (root->isleaf())
- {
- const btSoftBody::Face* face = (btSoftBody::Face*)root->data;
- root->normal = face->m_normal;
- root->angle = 0;
- }
- else
- {
- btVector3 n0(0,0,0), n1(0,0,0);
- btScalar a0 = 0, a1 = 0;
- if (root->childs[0])
- {
- calculateNormalCone(root->childs[0]);
- n0 = root->childs[0]->normal;
- a0 = root->childs[0]->angle;
- }
- if (root->childs[1])
- {
- calculateNormalCone(root->childs[1]);
- n1 = root->childs[1]->normal;
- a1 = root->childs[1]->angle;
- }
- root->normal = (n0+n1).safeNormalize();
- root->angle = btMax(a0,a1) + btAngle(n0, n1)*0.5;
- }
-}
//
void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
{
@@ -3779,6 +4139,8 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
break;
case fCollision::VF_DD:
{
+ if (!psb->m_softSoftCollision)
+ return;
if (psb->isActive() || this->isActive())
{
if (this != psb)
@@ -3797,6 +4159,7 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
docollide.psb[1]->m_fdbvt.m_root,
docollide);
+
/* psb1 nodes vs psb0 faces */
if (this->m_tetras.size() > 0)
docollide.useFaceNormal = true;
@@ -3812,20 +4175,17 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
{
if (psb->useSelfCollision())
{
- btSoftColliders::CollideFF_DD docollide;
- docollide.mrg = getCollisionShape()->getMargin() +
- psb->getCollisionShape()->getMargin();
- docollide.psb[0] = this;
- docollide.psb[1] = psb;
- if (this->m_tetras.size() > 0)
- docollide.useFaceNormal = true;
- else
- docollide.useFaceNormal = false;
- /* psb0 faces vs psb0 faces */
- btDbvntNode* root = copyToDbvnt(this->m_fdbvt.m_root);
- calculateNormalCone(root);
- this->m_fdbvt.selfCollideT(root,docollide);
- delete root;
+ btSoftColliders::CollideFF_DD docollide;
+ docollide.mrg = 2*getCollisionShape()->getMargin();
+ docollide.psb[0] = this;
+ docollide.psb[1] = psb;
+ if (this->m_tetras.size() > 0)
+ docollide.useFaceNormal = true;
+ else
+ docollide.useFaceNormal = false;
+ /* psb0 faces vs psb0 faces */
+ calculateNormalCone(this->m_fdbvnt);
+ this->m_fdbvt.selfCollideT(m_fdbvnt,docollide);
}
}
}
@@ -3837,6 +4197,58 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
}
}
+void btSoftBody::geometricCollisionHandler(btSoftBody* psb)
+{
+ if (psb->isActive() || this->isActive())
+ {
+ if (this != psb)
+ {
+ btSoftColliders::CollideCCD docollide;
+ /* common */
+ docollide.mrg = SAFE_EPSILON; // for rounding error instead of actual margin
+ docollide.dt = psb->m_sst.sdt;
+ /* psb0 nodes vs psb1 faces */
+ if (psb->m_tetras.size() > 0)
+ docollide.useFaceNormal = true;
+ else
+ docollide.useFaceNormal = false;
+ docollide.psb[0] = this;
+ docollide.psb[1] = psb;
+ docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
+ docollide.psb[1]->m_fdbvt.m_root,
+ docollide);
+ /* psb1 nodes vs psb0 faces */
+ if (this->m_tetras.size() > 0)
+ docollide.useFaceNormal = true;
+ else
+ docollide.useFaceNormal = false;
+ docollide.psb[0] = psb;
+ docollide.psb[1] = this;
+ docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
+ docollide.psb[1]->m_fdbvt.m_root,
+ docollide);
+ }
+ else
+ {
+ if (psb->useSelfCollision())
+ {
+ btSoftColliders::CollideCCD docollide;
+ docollide.mrg = SAFE_EPSILON;
+ docollide.psb[0] = this;
+ docollide.psb[1] = psb;
+ docollide.dt = psb->m_sst.sdt;
+ if (this->m_tetras.size() > 0)
+ docollide.useFaceNormal = true;
+ else
+ docollide.useFaceNormal = false;
+ /* psb0 faces vs psb0 faces */
+ calculateNormalCone(this->m_fdbvnt); // should compute this outside of this scope
+ this->m_fdbvt.selfCollideT(m_fdbvnt,docollide);
+ }
+ }
+ }
+}
+
void btSoftBody::setWindVelocity(const btVector3& velocity)
{
m_windVelocity = velocity;
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-01 13:30:00 +0000