1440 lines
47 KiB
C++
1440 lines
47 KiB
C++
/*
|
|
Bullet Continuous Collision Detection and Physics Library
|
|
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
|
|
|
This software is provided 'as-is', without any express or implied warranty.
|
|
In no event will the authors be held liable for any damages arising from the use of this software.
|
|
Permission is granted to anyone to use this software for any purpose,
|
|
including commercial applications, and to alter it and redistribute it freely,
|
|
subject to the following restrictions:
|
|
|
|
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
|
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
|
3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
///btSoftBody implementation by Nathanael Presson
|
|
|
|
#ifndef _BT_SOFT_BODY_INTERNALS_H
|
|
#define _BT_SOFT_BODY_INTERNALS_H
|
|
|
|
#include "btSoftBody.h"
|
|
|
|
#include "LinearMath/btQuickprof.h"
|
|
#include "LinearMath/btPolarDecomposition.h"
|
|
#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
|
|
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
|
|
#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
|
|
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
|
|
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
|
|
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
|
|
#include <string.h> //for memset
|
|
#include <cmath>
|
|
|
|
// Given a multibody link, a contact point and a contact direction, fill in the jacobian data needed to calculate the velocity change given an impulse in the contact direction
|
|
static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
|
|
btMultiBodyJacobianData& jacobianData,
|
|
const btVector3& contact_point,
|
|
const btVector3& dir)
|
|
{
|
|
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
|
|
jacobianData.m_jacobians.resize(ndof);
|
|
jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
|
|
btScalar* jac = &jacobianData.m_jacobians[0];
|
|
|
|
multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
|
|
multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v);
|
|
}
|
|
static btVector3 generateUnitOrthogonalVector(const btVector3& u)
|
|
{
|
|
btScalar ux = u.getX();
|
|
btScalar uy = u.getY();
|
|
btScalar uz = u.getZ();
|
|
btScalar ax = std::abs(ux);
|
|
btScalar ay = std::abs(uy);
|
|
btScalar az = std::abs(uz);
|
|
btVector3 v;
|
|
if (ax <= ay && ax <= az)
|
|
v = btVector3(0, -uz, uy);
|
|
else if (ay <= ax && ay <= az)
|
|
v = btVector3(-uz, 0, ux);
|
|
else
|
|
v = btVector3(-uy, ux, 0);
|
|
v.normalize();
|
|
return v;
|
|
}
|
|
//
|
|
// btSymMatrix
|
|
//
|
|
template <typename T>
|
|
struct btSymMatrix
|
|
{
|
|
btSymMatrix() : dim(0) {}
|
|
btSymMatrix(int n, const T& init = T()) { resize(n, init); }
|
|
void resize(int n, const T& init = T())
|
|
{
|
|
dim = n;
|
|
store.resize((n * (n + 1)) / 2, init);
|
|
}
|
|
int index(int c, int r) const
|
|
{
|
|
if (c > r) btSwap(c, r);
|
|
btAssert(r < dim);
|
|
return ((r * (r + 1)) / 2 + c);
|
|
}
|
|
T& operator()(int c, int r) { return (store[index(c, r)]); }
|
|
const T& operator()(int c, int r) const { return (store[index(c, r)]); }
|
|
btAlignedObjectArray<T> store;
|
|
int dim;
|
|
};
|
|
|
|
//
|
|
// btSoftBodyCollisionShape
|
|
//
|
|
class btSoftBodyCollisionShape : public btConcaveShape
|
|
{
|
|
public:
|
|
btSoftBody* m_body;
|
|
|
|
btSoftBodyCollisionShape(btSoftBody* backptr)
|
|
{
|
|
m_shapeType = SOFTBODY_SHAPE_PROXYTYPE;
|
|
m_body = backptr;
|
|
}
|
|
|
|
virtual ~btSoftBodyCollisionShape()
|
|
{
|
|
}
|
|
|
|
void processAllTriangles(btTriangleCallback* /*callback*/, const btVector3& /*aabbMin*/, const btVector3& /*aabbMax*/) const
|
|
{
|
|
//not yet
|
|
btAssert(0);
|
|
}
|
|
|
|
///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
|
|
virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
|
|
{
|
|
/* t is usually identity, except when colliding against btCompoundShape. See Issue 512 */
|
|
const btVector3 mins = m_body->m_bounds[0];
|
|
const btVector3 maxs = m_body->m_bounds[1];
|
|
const btVector3 crns[] = {t * btVector3(mins.x(), mins.y(), mins.z()),
|
|
t * btVector3(maxs.x(), mins.y(), mins.z()),
|
|
t * btVector3(maxs.x(), maxs.y(), mins.z()),
|
|
t * btVector3(mins.x(), maxs.y(), mins.z()),
|
|
t * btVector3(mins.x(), mins.y(), maxs.z()),
|
|
t * btVector3(maxs.x(), mins.y(), maxs.z()),
|
|
t * btVector3(maxs.x(), maxs.y(), maxs.z()),
|
|
t * btVector3(mins.x(), maxs.y(), maxs.z())};
|
|
aabbMin = aabbMax = crns[0];
|
|
for (int i = 1; i < 8; ++i)
|
|
{
|
|
aabbMin.setMin(crns[i]);
|
|
aabbMax.setMax(crns[i]);
|
|
}
|
|
}
|
|
|
|
virtual void setLocalScaling(const btVector3& /*scaling*/)
|
|
{
|
|
///na
|
|
}
|
|
virtual const btVector3& getLocalScaling() const
|
|
{
|
|
static const btVector3 dummy(1, 1, 1);
|
|
return dummy;
|
|
}
|
|
virtual void calculateLocalInertia(btScalar /*mass*/, btVector3& /*inertia*/) const
|
|
{
|
|
///not yet
|
|
btAssert(0);
|
|
}
|
|
virtual const char* getName() const
|
|
{
|
|
return "SoftBody";
|
|
}
|
|
};
|
|
|
|
//
|
|
// btSoftClusterCollisionShape
|
|
//
|
|
class btSoftClusterCollisionShape : public btConvexInternalShape
|
|
{
|
|
public:
|
|
const btSoftBody::Cluster* m_cluster;
|
|
|
|
btSoftClusterCollisionShape(const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); }
|
|
|
|
virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
|
|
{
|
|
btSoftBody::Node* const* n = &m_cluster->m_nodes[0];
|
|
btScalar d = btDot(vec, n[0]->m_x);
|
|
int j = 0;
|
|
for (int i = 1, ni = m_cluster->m_nodes.size(); i < ni; ++i)
|
|
{
|
|
const btScalar k = btDot(vec, n[i]->m_x);
|
|
if (k > d)
|
|
{
|
|
d = k;
|
|
j = i;
|
|
}
|
|
}
|
|
return (n[j]->m_x);
|
|
}
|
|
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const
|
|
{
|
|
return (localGetSupportingVertex(vec));
|
|
}
|
|
//notice that the vectors should be unit length
|
|
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
|
|
{
|
|
}
|
|
|
|
virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const
|
|
{
|
|
}
|
|
|
|
virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
|
|
{
|
|
}
|
|
|
|
virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; }
|
|
|
|
//debugging
|
|
virtual const char* getName() const { return "SOFTCLUSTER"; }
|
|
|
|
virtual void setMargin(btScalar margin)
|
|
{
|
|
btConvexInternalShape::setMargin(margin);
|
|
}
|
|
virtual btScalar getMargin() const
|
|
{
|
|
return btConvexInternalShape::getMargin();
|
|
}
|
|
};
|
|
|
|
//
|
|
// Inline's
|
|
//
|
|
|
|
//
|
|
template <typename T>
|
|
static inline void ZeroInitialize(T& value)
|
|
{
|
|
memset(&value, 0, sizeof(T));
|
|
}
|
|
//
|
|
template <typename T>
|
|
static inline bool CompLess(const T& a, const T& b)
|
|
{
|
|
return (a < b);
|
|
}
|
|
//
|
|
template <typename T>
|
|
static inline bool CompGreater(const T& a, const T& b)
|
|
{
|
|
return (a > b);
|
|
}
|
|
//
|
|
template <typename T>
|
|
static inline T Lerp(const T& a, const T& b, btScalar t)
|
|
{
|
|
return (a + (b - a) * t);
|
|
}
|
|
//
|
|
template <typename T>
|
|
static inline T InvLerp(const T& a, const T& b, btScalar t)
|
|
{
|
|
return ((b + a * t - b * t) / (a * b));
|
|
}
|
|
//
|
|
static inline btMatrix3x3 Lerp(const btMatrix3x3& a,
|
|
const btMatrix3x3& b,
|
|
btScalar t)
|
|
{
|
|
btMatrix3x3 r;
|
|
r[0] = Lerp(a[0], b[0], t);
|
|
r[1] = Lerp(a[1], b[1], t);
|
|
r[2] = Lerp(a[2], b[2], t);
|
|
return (r);
|
|
}
|
|
//
|
|
static inline btVector3 Clamp(const btVector3& v, btScalar maxlength)
|
|
{
|
|
const btScalar sql = v.length2();
|
|
if (sql > (maxlength * maxlength))
|
|
return ((v * maxlength) / btSqrt(sql));
|
|
else
|
|
return (v);
|
|
}
|
|
//
|
|
template <typename T>
|
|
static inline T Clamp(const T& x, const T& l, const T& h)
|
|
{
|
|
return (x < l ? l : x > h ? h : x);
|
|
}
|
|
//
|
|
template <typename T>
|
|
static inline T Sq(const T& x)
|
|
{
|
|
return (x * x);
|
|
}
|
|
//
|
|
template <typename T>
|
|
static inline T Cube(const T& x)
|
|
{
|
|
return (x * x * x);
|
|
}
|
|
//
|
|
template <typename T>
|
|
static inline T Sign(const T& x)
|
|
{
|
|
return ((T)(x < 0 ? -1 : +1));
|
|
}
|
|
//
|
|
template <typename T>
|
|
static inline bool SameSign(const T& x, const T& y)
|
|
{
|
|
return ((x * y) > 0);
|
|
}
|
|
//
|
|
static inline btScalar ClusterMetric(const btVector3& x, const btVector3& y)
|
|
{
|
|
const btVector3 d = x - y;
|
|
return (btFabs(d[0]) + btFabs(d[1]) + btFabs(d[2]));
|
|
}
|
|
//
|
|
static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a, btScalar s)
|
|
{
|
|
const btScalar xx = a.x() * a.x();
|
|
const btScalar yy = a.y() * a.y();
|
|
const btScalar zz = a.z() * a.z();
|
|
const btScalar xy = a.x() * a.y();
|
|
const btScalar yz = a.y() * a.z();
|
|
const btScalar zx = a.z() * a.x();
|
|
btMatrix3x3 m;
|
|
m[0] = btVector3(1 - xx + xx * s, xy * s - xy, zx * s - zx);
|
|
m[1] = btVector3(xy * s - xy, 1 - yy + yy * s, yz * s - yz);
|
|
m[2] = btVector3(zx * s - zx, yz * s - yz, 1 - zz + zz * s);
|
|
return (m);
|
|
}
|
|
//
|
|
static inline btMatrix3x3 Cross(const btVector3& v)
|
|
{
|
|
btMatrix3x3 m;
|
|
m[0] = btVector3(0, -v.z(), +v.y());
|
|
m[1] = btVector3(+v.z(), 0, -v.x());
|
|
m[2] = btVector3(-v.y(), +v.x(), 0);
|
|
return (m);
|
|
}
|
|
//
|
|
static inline btMatrix3x3 Diagonal(btScalar x)
|
|
{
|
|
btMatrix3x3 m;
|
|
m[0] = btVector3(x, 0, 0);
|
|
m[1] = btVector3(0, x, 0);
|
|
m[2] = btVector3(0, 0, x);
|
|
return (m);
|
|
}
|
|
|
|
static inline btMatrix3x3 Diagonal(const btVector3& v)
|
|
{
|
|
btMatrix3x3 m;
|
|
m[0] = btVector3(v.getX(), 0, 0);
|
|
m[1] = btVector3(0, v.getY(), 0);
|
|
m[2] = btVector3(0, 0, v.getZ());
|
|
return (m);
|
|
}
|
|
|
|
static inline btScalar Dot(const btScalar* a,const btScalar* b, int ndof)
|
|
{
|
|
btScalar result = 0;
|
|
for (int i = 0; i < ndof; ++i)
|
|
result += a[i] * b[i];
|
|
return result;
|
|
}
|
|
|
|
static inline btMatrix3x3 OuterProduct(const btScalar* v1,const btScalar* v2,const btScalar* v3,
|
|
const btScalar* u1, const btScalar* u2, const btScalar* u3, int ndof)
|
|
{
|
|
btMatrix3x3 m;
|
|
btScalar a11 = Dot(v1,u1,ndof);
|
|
btScalar a12 = Dot(v1,u2,ndof);
|
|
btScalar a13 = Dot(v1,u3,ndof);
|
|
|
|
btScalar a21 = Dot(v2,u1,ndof);
|
|
btScalar a22 = Dot(v2,u2,ndof);
|
|
btScalar a23 = Dot(v2,u3,ndof);
|
|
|
|
btScalar a31 = Dot(v3,u1,ndof);
|
|
btScalar a32 = Dot(v3,u2,ndof);
|
|
btScalar a33 = Dot(v3,u3,ndof);
|
|
m[0] = btVector3(a11, a12, a13);
|
|
m[1] = btVector3(a21, a22, a23);
|
|
m[2] = btVector3(a31, a32, a33);
|
|
return (m);
|
|
}
|
|
|
|
|
|
//
|
|
static inline btMatrix3x3 Add(const btMatrix3x3& a,
|
|
const btMatrix3x3& b)
|
|
{
|
|
btMatrix3x3 r;
|
|
for (int i = 0; i < 3; ++i) r[i] = a[i] + b[i];
|
|
return (r);
|
|
}
|
|
//
|
|
static inline btMatrix3x3 Sub(const btMatrix3x3& a,
|
|
const btMatrix3x3& b)
|
|
{
|
|
btMatrix3x3 r;
|
|
for (int i = 0; i < 3; ++i) r[i] = a[i] - b[i];
|
|
return (r);
|
|
}
|
|
//
|
|
static inline btMatrix3x3 Mul(const btMatrix3x3& a,
|
|
btScalar b)
|
|
{
|
|
btMatrix3x3 r;
|
|
for (int i = 0; i < 3; ++i) r[i] = a[i] * b;
|
|
return (r);
|
|
}
|
|
//
|
|
static inline void Orthogonalize(btMatrix3x3& m)
|
|
{
|
|
m[2] = btCross(m[0], m[1]).normalized();
|
|
m[1] = btCross(m[2], m[0]).normalized();
|
|
m[0] = btCross(m[1], m[2]).normalized();
|
|
}
|
|
//
|
|
static inline btMatrix3x3 MassMatrix(btScalar im, const btMatrix3x3& iwi, const btVector3& r)
|
|
{
|
|
const btMatrix3x3 cr = Cross(r);
|
|
return (Sub(Diagonal(im), cr * iwi * cr));
|
|
}
|
|
|
|
//
|
|
static inline btMatrix3x3 ImpulseMatrix(btScalar dt,
|
|
btScalar ima,
|
|
btScalar imb,
|
|
const btMatrix3x3& iwi,
|
|
const btVector3& r)
|
|
{
|
|
return (Diagonal(1 / dt) * Add(Diagonal(ima), MassMatrix(imb, iwi, r)).inverse());
|
|
}
|
|
|
|
//
|
|
static inline btMatrix3x3 ImpulseMatrix(btScalar ima, const btMatrix3x3& iia, const btVector3& ra,
|
|
btScalar imb, const btMatrix3x3& iib, const btVector3& rb)
|
|
{
|
|
return (Add(MassMatrix(ima, iia, ra), MassMatrix(imb, iib, rb)).inverse());
|
|
}
|
|
|
|
//
|
|
static inline btMatrix3x3 AngularImpulseMatrix(const btMatrix3x3& iia,
|
|
const btMatrix3x3& iib)
|
|
{
|
|
return (Add(iia, iib).inverse());
|
|
}
|
|
|
|
//
|
|
static inline btVector3 ProjectOnAxis(const btVector3& v,
|
|
const btVector3& a)
|
|
{
|
|
return (a * btDot(v, a));
|
|
}
|
|
//
|
|
static inline btVector3 ProjectOnPlane(const btVector3& v,
|
|
const btVector3& a)
|
|
{
|
|
return (v - ProjectOnAxis(v, a));
|
|
}
|
|
|
|
//
|
|
static inline void ProjectOrigin(const btVector3& a,
|
|
const btVector3& b,
|
|
btVector3& prj,
|
|
btScalar& sqd)
|
|
{
|
|
const btVector3 d = b - a;
|
|
const btScalar m2 = d.length2();
|
|
if (m2 > SIMD_EPSILON)
|
|
{
|
|
const btScalar t = Clamp<btScalar>(-btDot(a, d) / m2, 0, 1);
|
|
const btVector3 p = a + d * t;
|
|
const btScalar l2 = p.length2();
|
|
if (l2 < sqd)
|
|
{
|
|
prj = p;
|
|
sqd = l2;
|
|
}
|
|
}
|
|
}
|
|
//
|
|
static inline void ProjectOrigin(const btVector3& a,
|
|
const btVector3& b,
|
|
const btVector3& c,
|
|
btVector3& prj,
|
|
btScalar& sqd)
|
|
{
|
|
const btVector3& q = btCross(b - a, c - a);
|
|
const btScalar m2 = q.length2();
|
|
if (m2 > SIMD_EPSILON)
|
|
{
|
|
const btVector3 n = q / btSqrt(m2);
|
|
const btScalar k = btDot(a, n);
|
|
const btScalar k2 = k * k;
|
|
if (k2 < sqd)
|
|
{
|
|
const btVector3 p = n * k;
|
|
if ((btDot(btCross(a - p, b - p), q) > 0) &&
|
|
(btDot(btCross(b - p, c - p), q) > 0) &&
|
|
(btDot(btCross(c - p, a - p), q) > 0))
|
|
{
|
|
prj = p;
|
|
sqd = k2;
|
|
}
|
|
else
|
|
{
|
|
ProjectOrigin(a, b, prj, sqd);
|
|
ProjectOrigin(b, c, prj, sqd);
|
|
ProjectOrigin(c, a, prj, sqd);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
//
|
|
static inline bool rayIntersectsTriangle(const btVector3& origin, const btVector3& dir, const btVector3& v0, const btVector3& v1, const btVector3& v2, btScalar& t)
|
|
{
|
|
btScalar a, f, u, v;
|
|
|
|
btVector3 e1 = v1 - v0;
|
|
btVector3 e2 = v2 - v0;
|
|
btVector3 h = dir.cross(e2);
|
|
a = e1.dot(h);
|
|
|
|
if (a > -0.00001 && a < 0.00001)
|
|
return (false);
|
|
|
|
f = btScalar(1) / a;
|
|
btVector3 s = origin - v0;
|
|
u = f * s.dot(h);
|
|
|
|
if (u < 0.0 || u > 1.0)
|
|
return (false);
|
|
|
|
btVector3 q = s.cross(e1);
|
|
v = f * dir.dot(q);
|
|
if (v < 0.0 || u + v > 1.0)
|
|
return (false);
|
|
// at this stage we can compute t to find out where
|
|
// the intersection point is on the line
|
|
t = f * e2.dot(q);
|
|
if (t > 0) // ray intersection
|
|
return (true);
|
|
else // this means that there is a line intersection
|
|
// but not a ray intersection
|
|
return (false);
|
|
}
|
|
|
|
static inline bool lineIntersectsTriangle(const btVector3& rayStart, const btVector3& rayEnd, const btVector3& p1, const btVector3& p2, const btVector3& p3, btVector3& sect, btVector3& normal)
|
|
{
|
|
btVector3 dir = rayEnd - rayStart;
|
|
btScalar dir_norm = dir.norm();
|
|
if (dir_norm < SIMD_EPSILON)
|
|
return false;
|
|
dir.normalize();
|
|
|
|
btScalar t;
|
|
|
|
bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t);
|
|
|
|
if (ret)
|
|
{
|
|
if (t <= dir_norm)
|
|
{
|
|
sect = rayStart + dir * t;
|
|
}
|
|
else
|
|
{
|
|
ret = false;
|
|
}
|
|
}
|
|
|
|
if (ret)
|
|
{
|
|
btVector3 n = (p3-p1).cross(p2-p1);
|
|
n.safeNormalize();
|
|
if (n.dot(dir) < 0)
|
|
normal = n;
|
|
else
|
|
normal = -n;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
|
|
//
|
|
template <typename T>
|
|
static inline T BaryEval(const T& a,
|
|
const T& b,
|
|
const T& c,
|
|
const btVector3& coord)
|
|
{
|
|
return (a * coord.x() + b * coord.y() + c * coord.z());
|
|
}
|
|
//
|
|
static inline btVector3 BaryCoord(const btVector3& a,
|
|
const btVector3& b,
|
|
const btVector3& c,
|
|
const btVector3& p)
|
|
{
|
|
const btScalar w[] = {btCross(a - p, b - p).length(),
|
|
btCross(b - p, c - p).length(),
|
|
btCross(c - p, a - p).length()};
|
|
const btScalar isum = 1 / (w[0] + w[1] + w[2]);
|
|
return (btVector3(w[1] * isum, w[2] * isum, w[0] * isum));
|
|
}
|
|
|
|
//
|
|
inline static btScalar ImplicitSolve(btSoftBody::ImplicitFn* fn,
|
|
const btVector3& a,
|
|
const btVector3& b,
|
|
const btScalar accuracy,
|
|
const int maxiterations = 256)
|
|
{
|
|
btScalar span[2] = {0, 1};
|
|
btScalar values[2] = {fn->Eval(a), fn->Eval(b)};
|
|
if (values[0] > values[1])
|
|
{
|
|
btSwap(span[0], span[1]);
|
|
btSwap(values[0], values[1]);
|
|
}
|
|
if (values[0] > -accuracy) return (-1);
|
|
if (values[1] < +accuracy) return (-1);
|
|
for (int i = 0; i < maxiterations; ++i)
|
|
{
|
|
const btScalar t = Lerp(span[0], span[1], values[0] / (values[0] - values[1]));
|
|
const btScalar v = fn->Eval(Lerp(a, b, t));
|
|
if ((t <= 0) || (t >= 1)) break;
|
|
if (btFabs(v) < accuracy) return (t);
|
|
if (v < 0)
|
|
{
|
|
span[0] = t;
|
|
values[0] = v;
|
|
}
|
|
else
|
|
{
|
|
span[1] = t;
|
|
values[1] = v;
|
|
}
|
|
}
|
|
return (-1);
|
|
}
|
|
|
|
inline static void EvaluateMedium(const btSoftBodyWorldInfo* wfi,
|
|
const btVector3& x,
|
|
btSoftBody::sMedium& medium)
|
|
{
|
|
medium.m_velocity = btVector3(0, 0, 0);
|
|
medium.m_pressure = 0;
|
|
medium.m_density = wfi->air_density;
|
|
if (wfi->water_density > 0)
|
|
{
|
|
const btScalar depth = -(btDot(x, wfi->water_normal) + wfi->water_offset);
|
|
if (depth > 0)
|
|
{
|
|
medium.m_density = wfi->water_density;
|
|
medium.m_pressure = depth * wfi->water_density * wfi->m_gravity.length();
|
|
}
|
|
}
|
|
}
|
|
|
|
//
|
|
static inline btVector3 NormalizeAny(const btVector3& v)
|
|
{
|
|
const btScalar l = v.length();
|
|
if (l > SIMD_EPSILON)
|
|
return (v / l);
|
|
else
|
|
return (btVector3(0, 0, 0));
|
|
}
|
|
|
|
//
|
|
static inline btDbvtVolume VolumeOf(const btSoftBody::Face& f,
|
|
btScalar margin)
|
|
{
|
|
const btVector3* pts[] = {&f.m_n[0]->m_x,
|
|
&f.m_n[1]->m_x,
|
|
&f.m_n[2]->m_x};
|
|
btDbvtVolume vol = btDbvtVolume::FromPoints(pts, 3);
|
|
vol.Expand(btVector3(margin, margin, margin));
|
|
return (vol);
|
|
}
|
|
|
|
//
|
|
static inline btVector3 CenterOf(const btSoftBody::Face& f)
|
|
{
|
|
return ((f.m_n[0]->m_x + f.m_n[1]->m_x + f.m_n[2]->m_x) / 3);
|
|
}
|
|
|
|
//
|
|
static inline btScalar AreaOf(const btVector3& x0,
|
|
const btVector3& x1,
|
|
const btVector3& x2)
|
|
{
|
|
const btVector3 a = x1 - x0;
|
|
const btVector3 b = x2 - x0;
|
|
const btVector3 cr = btCross(a, b);
|
|
const btScalar area = cr.length();
|
|
return (area);
|
|
}
|
|
|
|
//
|
|
static inline btScalar VolumeOf(const btVector3& x0,
|
|
const btVector3& x1,
|
|
const btVector3& x2,
|
|
const btVector3& x3)
|
|
{
|
|
const btVector3 a = x1 - x0;
|
|
const btVector3 b = x2 - x0;
|
|
const btVector3 c = x3 - x0;
|
|
return (btDot(a, btCross(b, c)));
|
|
}
|
|
|
|
//
|
|
|
|
//
|
|
static inline void ApplyClampedForce(btSoftBody::Node& n,
|
|
const btVector3& f,
|
|
btScalar dt)
|
|
{
|
|
const btScalar dtim = dt * n.m_im;
|
|
if ((f * dtim).length2() > n.m_v.length2())
|
|
{ /* Clamp */
|
|
n.m_f -= ProjectOnAxis(n.m_v, f.normalized()) / dtim;
|
|
}
|
|
else
|
|
{ /* Apply */
|
|
n.m_f += f;
|
|
}
|
|
}
|
|
|
|
//
|
|
static inline int MatchEdge(const btSoftBody::Node* a,
|
|
const btSoftBody::Node* b,
|
|
const btSoftBody::Node* ma,
|
|
const btSoftBody::Node* mb)
|
|
{
|
|
if ((a == ma) && (b == mb)) return (0);
|
|
if ((a == mb) && (b == ma)) return (1);
|
|
return (-1);
|
|
}
|
|
|
|
//
|
|
// btEigen : Extract eigen system,
|
|
// straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html
|
|
// outputs are NOT sorted.
|
|
//
|
|
struct btEigen
|
|
{
|
|
static int system(btMatrix3x3& a, btMatrix3x3* vectors, btVector3* values = 0)
|
|
{
|
|
static const int maxiterations = 16;
|
|
static const btScalar accuracy = (btScalar)0.0001;
|
|
btMatrix3x3& v = *vectors;
|
|
int iterations = 0;
|
|
vectors->setIdentity();
|
|
do
|
|
{
|
|
int p = 0, q = 1;
|
|
if (btFabs(a[p][q]) < btFabs(a[0][2]))
|
|
{
|
|
p = 0;
|
|
q = 2;
|
|
}
|
|
if (btFabs(a[p][q]) < btFabs(a[1][2]))
|
|
{
|
|
p = 1;
|
|
q = 2;
|
|
}
|
|
if (btFabs(a[p][q]) > accuracy)
|
|
{
|
|
const btScalar w = (a[q][q] - a[p][p]) / (2 * a[p][q]);
|
|
const btScalar z = btFabs(w);
|
|
const btScalar t = w / (z * (btSqrt(1 + w * w) + z));
|
|
if (t == t) /* [WARNING] let hope that one does not get thrown aways by some compilers... */
|
|
{
|
|
const btScalar c = 1 / btSqrt(t * t + 1);
|
|
const btScalar s = c * t;
|
|
mulPQ(a, c, s, p, q);
|
|
mulTPQ(a, c, s, p, q);
|
|
mulPQ(v, c, s, p, q);
|
|
}
|
|
else
|
|
break;
|
|
}
|
|
else
|
|
break;
|
|
} while ((++iterations) < maxiterations);
|
|
if (values)
|
|
{
|
|
*values = btVector3(a[0][0], a[1][1], a[2][2]);
|
|
}
|
|
return (iterations);
|
|
}
|
|
|
|
private:
|
|
static inline void mulTPQ(btMatrix3x3& a, btScalar c, btScalar s, int p, int q)
|
|
{
|
|
const btScalar m[2][3] = {{a[p][0], a[p][1], a[p][2]},
|
|
{a[q][0], a[q][1], a[q][2]}};
|
|
int i;
|
|
|
|
for (i = 0; i < 3; ++i) a[p][i] = c * m[0][i] - s * m[1][i];
|
|
for (i = 0; i < 3; ++i) a[q][i] = c * m[1][i] + s * m[0][i];
|
|
}
|
|
static inline void mulPQ(btMatrix3x3& a, btScalar c, btScalar s, int p, int q)
|
|
{
|
|
const btScalar m[2][3] = {{a[0][p], a[1][p], a[2][p]},
|
|
{a[0][q], a[1][q], a[2][q]}};
|
|
int i;
|
|
|
|
for (i = 0; i < 3; ++i) a[i][p] = c * m[0][i] - s * m[1][i];
|
|
for (i = 0; i < 3; ++i) a[i][q] = c * m[1][i] + s * m[0][i];
|
|
}
|
|
};
|
|
|
|
//
|
|
// Polar decomposition,
|
|
// "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986.
|
|
//
|
|
static inline int PolarDecompose(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s)
|
|
{
|
|
static const btPolarDecomposition polar;
|
|
return polar.decompose(m, q, s);
|
|
}
|
|
|
|
//
|
|
// btSoftColliders
|
|
//
|
|
struct btSoftColliders
|
|
{
|
|
//
|
|
// ClusterBase
|
|
//
|
|
struct ClusterBase : btDbvt::ICollide
|
|
{
|
|
btScalar erp;
|
|
btScalar idt;
|
|
btScalar m_margin;
|
|
btScalar friction;
|
|
btScalar threshold;
|
|
ClusterBase()
|
|
{
|
|
erp = (btScalar)1;
|
|
idt = 0;
|
|
m_margin = 0;
|
|
friction = 0;
|
|
threshold = (btScalar)0;
|
|
}
|
|
bool SolveContact(const btGjkEpaSolver2::sResults& res,
|
|
btSoftBody::Body ba, const btSoftBody::Body bb,
|
|
btSoftBody::CJoint& joint)
|
|
{
|
|
if (res.distance < m_margin)
|
|
{
|
|
btVector3 norm = res.normal;
|
|
norm.normalize(); //is it necessary?
|
|
|
|
const btVector3 ra = res.witnesses[0] - ba.xform().getOrigin();
|
|
const btVector3 rb = res.witnesses[1] - bb.xform().getOrigin();
|
|
const btVector3 va = ba.velocity(ra);
|
|
const btVector3 vb = bb.velocity(rb);
|
|
const btVector3 vrel = va - vb;
|
|
const btScalar rvac = btDot(vrel, norm);
|
|
btScalar depth = res.distance - m_margin;
|
|
|
|
// printf("depth=%f\n",depth);
|
|
const btVector3 iv = norm * rvac;
|
|
const btVector3 fv = vrel - iv;
|
|
joint.m_bodies[0] = ba;
|
|
joint.m_bodies[1] = bb;
|
|
joint.m_refs[0] = ra * ba.xform().getBasis();
|
|
joint.m_refs[1] = rb * bb.xform().getBasis();
|
|
joint.m_rpos[0] = ra;
|
|
joint.m_rpos[1] = rb;
|
|
joint.m_cfm = 1;
|
|
joint.m_erp = 1;
|
|
joint.m_life = 0;
|
|
joint.m_maxlife = 0;
|
|
joint.m_split = 1;
|
|
|
|
joint.m_drift = depth * norm;
|
|
|
|
joint.m_normal = norm;
|
|
// printf("normal=%f,%f,%f\n",res.normal.getX(),res.normal.getY(),res.normal.getZ());
|
|
joint.m_delete = false;
|
|
joint.m_friction = fv.length2() < (rvac * friction * rvac * friction) ? 1 : friction;
|
|
joint.m_massmatrix = ImpulseMatrix(ba.invMass(), ba.invWorldInertia(), joint.m_rpos[0],
|
|
bb.invMass(), bb.invWorldInertia(), joint.m_rpos[1]);
|
|
|
|
return (true);
|
|
}
|
|
return (false);
|
|
}
|
|
};
|
|
//
|
|
// CollideCL_RS
|
|
//
|
|
struct CollideCL_RS : ClusterBase
|
|
{
|
|
btSoftBody* psb;
|
|
const btCollisionObjectWrapper* m_colObjWrap;
|
|
|
|
void Process(const btDbvtNode* leaf)
|
|
{
|
|
btSoftBody::Cluster* cluster = (btSoftBody::Cluster*)leaf->data;
|
|
btSoftClusterCollisionShape cshape(cluster);
|
|
|
|
const btConvexShape* rshape = (const btConvexShape*)m_colObjWrap->getCollisionShape();
|
|
|
|
///don't collide an anchored cluster with a static/kinematic object
|
|
if (m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject() && cluster->m_containsAnchor)
|
|
return;
|
|
|
|
btGjkEpaSolver2::sResults res;
|
|
if (btGjkEpaSolver2::SignedDistance(&cshape, btTransform::getIdentity(),
|
|
rshape, m_colObjWrap->getWorldTransform(),
|
|
btVector3(1, 0, 0), res))
|
|
{
|
|
btSoftBody::CJoint joint;
|
|
if (SolveContact(res, cluster, m_colObjWrap->getCollisionObject(), joint)) //prb,joint))
|
|
{
|
|
btSoftBody::CJoint* pj = new (btAlignedAlloc(sizeof(btSoftBody::CJoint), 16)) btSoftBody::CJoint();
|
|
*pj = joint;
|
|
psb->m_joints.push_back(pj);
|
|
if (m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject())
|
|
{
|
|
pj->m_erp *= psb->m_cfg.kSKHR_CL;
|
|
pj->m_split *= psb->m_cfg.kSK_SPLT_CL;
|
|
}
|
|
else
|
|
{
|
|
pj->m_erp *= psb->m_cfg.kSRHR_CL;
|
|
pj->m_split *= psb->m_cfg.kSR_SPLT_CL;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
void ProcessColObj(btSoftBody* ps, const btCollisionObjectWrapper* colObWrap)
|
|
{
|
|
psb = ps;
|
|
m_colObjWrap = colObWrap;
|
|
idt = ps->m_sst.isdt;
|
|
m_margin = m_colObjWrap->getCollisionShape()->getMargin() + psb->getCollisionShape()->getMargin();
|
|
///Bullet rigid body uses multiply instead of minimum to determine combined friction. Some customization would be useful.
|
|
friction = btMin(psb->m_cfg.kDF, m_colObjWrap->getCollisionObject()->getFriction());
|
|
btVector3 mins;
|
|
btVector3 maxs;
|
|
|
|
ATTRIBUTE_ALIGNED16(btDbvtVolume)
|
|
volume;
|
|
colObWrap->getCollisionShape()->getAabb(colObWrap->getWorldTransform(), mins, maxs);
|
|
volume = btDbvtVolume::FromMM(mins, maxs);
|
|
volume.Expand(btVector3(1, 1, 1) * m_margin);
|
|
ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root, volume, *this);
|
|
}
|
|
};
|
|
//
|
|
// CollideCL_SS
|
|
//
|
|
struct CollideCL_SS : ClusterBase
|
|
{
|
|
btSoftBody* bodies[2];
|
|
void Process(const btDbvtNode* la, const btDbvtNode* lb)
|
|
{
|
|
btSoftBody::Cluster* cla = (btSoftBody::Cluster*)la->data;
|
|
btSoftBody::Cluster* clb = (btSoftBody::Cluster*)lb->data;
|
|
|
|
bool connected = false;
|
|
if ((bodies[0] == bodies[1]) && (bodies[0]->m_clusterConnectivity.size()))
|
|
{
|
|
connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex + bodies[0]->m_clusters.size() * clb->m_clusterIndex];
|
|
}
|
|
|
|
if (!connected)
|
|
{
|
|
btSoftClusterCollisionShape csa(cla);
|
|
btSoftClusterCollisionShape csb(clb);
|
|
btGjkEpaSolver2::sResults res;
|
|
if (btGjkEpaSolver2::SignedDistance(&csa, btTransform::getIdentity(),
|
|
&csb, btTransform::getIdentity(),
|
|
cla->m_com - clb->m_com, res))
|
|
{
|
|
btSoftBody::CJoint joint;
|
|
if (SolveContact(res, cla, clb, joint))
|
|
{
|
|
btSoftBody::CJoint* pj = new (btAlignedAlloc(sizeof(btSoftBody::CJoint), 16)) btSoftBody::CJoint();
|
|
*pj = joint;
|
|
bodies[0]->m_joints.push_back(pj);
|
|
pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL, bodies[1]->m_cfg.kSSHR_CL);
|
|
pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL + bodies[1]->m_cfg.kSS_SPLT_CL) / 2;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
static int count = 0;
|
|
count++;
|
|
//printf("count=%d\n",count);
|
|
}
|
|
}
|
|
void ProcessSoftSoft(btSoftBody* psa, btSoftBody* psb)
|
|
{
|
|
idt = psa->m_sst.isdt;
|
|
//m_margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
|
|
m_margin = (psa->getCollisionShape()->getMargin() + psb->getCollisionShape()->getMargin());
|
|
friction = btMin(psa->m_cfg.kDF, psb->m_cfg.kDF);
|
|
bodies[0] = psa;
|
|
bodies[1] = psb;
|
|
psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root, psb->m_cdbvt.m_root, *this);
|
|
}
|
|
};
|
|
//
|
|
// CollideSDF_RS
|
|
//
|
|
struct CollideSDF_RS : btDbvt::ICollide
|
|
{
|
|
void Process(const btDbvtNode* leaf)
|
|
{
|
|
btSoftBody::Node* node = (btSoftBody::Node*)leaf->data;
|
|
DoNode(*node);
|
|
}
|
|
void DoNode(btSoftBody::Node& n) const
|
|
{
|
|
const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
|
|
btSoftBody::RContact c;
|
|
|
|
if ((!n.m_battach) &&
|
|
psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti))
|
|
{
|
|
const btScalar ima = n.m_im;
|
|
const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
|
|
const btScalar ms = ima + imb;
|
|
if (ms > 0)
|
|
{
|
|
const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
|
|
static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
|
|
const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
|
|
const btVector3 ra = n.m_x - wtr.getOrigin();
|
|
const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0);
|
|
const btVector3 vb = n.m_x - n.m_q;
|
|
const btVector3 vr = vb - va;
|
|
const btScalar dn = btDot(vr, c.m_cti.m_normal);
|
|
const btVector3 fv = vr - c.m_cti.m_normal * dn;
|
|
const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
|
|
c.m_node = &n;
|
|
c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);
|
|
c.m_c1 = ra;
|
|
c.m_c2 = ima * psb->m_sst.sdt;
|
|
c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc;
|
|
c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
|
|
psb->m_rcontacts.push_back(c);
|
|
if (m_rigidBody)
|
|
m_rigidBody->activate();
|
|
}
|
|
}
|
|
}
|
|
btSoftBody* psb;
|
|
const btCollisionObjectWrapper* m_colObj1Wrap;
|
|
btRigidBody* m_rigidBody;
|
|
btScalar dynmargin;
|
|
btScalar stamargin;
|
|
};
|
|
|
|
//
|
|
// CollideSDF_RD
|
|
//
|
|
struct CollideSDF_RD : btDbvt::ICollide
|
|
{
|
|
void Process(const btDbvtNode* leaf)
|
|
{
|
|
btSoftBody::Node* node = (btSoftBody::Node*)leaf->data;
|
|
DoNode(*node);
|
|
}
|
|
void DoNode(btSoftBody::Node& n) const
|
|
{
|
|
const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
|
|
btSoftBody::DeformableNodeRigidContact c;
|
|
|
|
if (!n.m_battach)
|
|
{
|
|
// check for collision at x_{n+1}^* as well at x_n
|
|
if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true) || psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true))
|
|
{
|
|
const btScalar ima = n.m_im;
|
|
// todo: collision between multibody and fixed deformable node will be missed.
|
|
const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
|
|
const btScalar ms = ima + imb;
|
|
if (ms > 0)
|
|
{
|
|
// resolve contact at x_n
|
|
psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ false);
|
|
btSoftBody::sCti& cti = c.m_cti;
|
|
c.m_node = &n;
|
|
const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
|
|
c.m_c2 = ima;
|
|
c.m_c3 = fc;
|
|
c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
|
|
|
|
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
|
|
{
|
|
const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
|
|
static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
|
|
const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
|
|
const btVector3 ra = n.m_x - wtr.getOrigin();
|
|
|
|
c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
|
|
c.m_c1 = ra;
|
|
}
|
|
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
|
|
{
|
|
btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
|
|
if (multibodyLinkCol)
|
|
{
|
|
btVector3 normal = cti.m_normal;
|
|
btVector3 t1 = generateUnitOrthogonalVector(normal);
|
|
btVector3 t2 = btCross(normal, t1);
|
|
btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
|
|
findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, normal);
|
|
findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_x, t1);
|
|
findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_x, t2);
|
|
|
|
btScalar* J_n = &jacobianData_normal.m_jacobians[0];
|
|
btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
|
|
btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
|
|
|
|
btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
|
|
btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
|
|
btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
|
|
|
|
btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
|
|
t1.getX(), t1.getY(), t1.getZ(),
|
|
t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
|
|
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
|
|
btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
|
|
c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
|
|
c.jacobianData_normal = jacobianData_normal;
|
|
c.jacobianData_t1 = jacobianData_t1;
|
|
c.jacobianData_t2 = jacobianData_t2;
|
|
c.t1 = t1;
|
|
c.t2 = t2;
|
|
}
|
|
}
|
|
psb->m_nodeRigidContacts.push_back(c);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
btSoftBody* psb;
|
|
const btCollisionObjectWrapper* m_colObj1Wrap;
|
|
btRigidBody* m_rigidBody;
|
|
btScalar dynmargin;
|
|
btScalar stamargin;
|
|
};
|
|
|
|
//
|
|
// CollideSDF_RDF
|
|
//
|
|
struct CollideSDF_RDF : btDbvt::ICollide
|
|
{
|
|
void Process(const btDbvtNode* leaf)
|
|
{
|
|
btSoftBody::Face* face = (btSoftBody::Face*)leaf->data;
|
|
DoNode(*face);
|
|
}
|
|
void DoNode(btSoftBody::Face& f) const
|
|
{
|
|
btSoftBody::Node* n0 = f.m_n[0];
|
|
btSoftBody::Node* n1 = f.m_n[1];
|
|
btSoftBody::Node* n2 = f.m_n[2];
|
|
|
|
const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0 )? dynmargin : stamargin;
|
|
btSoftBody::DeformableFaceRigidContact c;
|
|
btVector3 contact_point;
|
|
btVector3 bary;
|
|
if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true))
|
|
{
|
|
f.m_pcontact[3] = 1;
|
|
btScalar ima = n0->m_im + n1->m_im + n2->m_im;
|
|
const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
|
|
// todo: collision between multibody and fixed deformable face will be missed.
|
|
const btScalar ms = ima + imb;
|
|
if (ms > 0)
|
|
{
|
|
// resolve contact at x_n
|
|
psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, /*predict = */ false);
|
|
btSoftBody::sCti& cti = c.m_cti;
|
|
c.m_contactPoint = contact_point;
|
|
c.m_bary = bary;
|
|
// todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
|
|
c.m_weights = btScalar(2)/(btScalar(1) + bary.length2()) * bary;
|
|
c.m_face = &f;
|
|
const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
|
|
|
|
// the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
|
|
ima = bary.getX()*c.m_weights.getX() * n0->m_im + bary.getY()*c.m_weights.getY() * n1->m_im + bary.getZ()*c.m_weights.getZ() * n2->m_im;
|
|
|
|
c.m_c2 = ima;
|
|
c.m_c3 = fc;
|
|
c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
|
|
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
|
|
{
|
|
const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
|
|
static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
|
|
const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
|
|
const btVector3 ra = contact_point - wtr.getOrigin();
|
|
|
|
// we do not scale the impulse matrix by dt
|
|
c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
|
|
c.m_c1 = ra;
|
|
}
|
|
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
|
|
{
|
|
btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
|
|
if (multibodyLinkCol)
|
|
{
|
|
btVector3 normal = cti.m_normal;
|
|
btVector3 t1 = generateUnitOrthogonalVector(normal);
|
|
btVector3 t2 = btCross(normal, t1);
|
|
btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
|
|
findJacobian(multibodyLinkCol, jacobianData_normal, contact_point, normal);
|
|
findJacobian(multibodyLinkCol, jacobianData_t1, contact_point, t1);
|
|
findJacobian(multibodyLinkCol, jacobianData_t2, contact_point, t2);
|
|
|
|
btScalar* J_n = &jacobianData_normal.m_jacobians[0];
|
|
btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
|
|
btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
|
|
|
|
btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
|
|
btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
|
|
btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
|
|
|
|
btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
|
|
t1.getX(), t1.getY(), t1.getZ(),
|
|
t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
|
|
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
|
|
btMatrix3x3 local_impulse_matrix = (Diagonal(ima) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
|
|
c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
|
|
c.jacobianData_normal = jacobianData_normal;
|
|
c.jacobianData_t1 = jacobianData_t1;
|
|
c.jacobianData_t2 = jacobianData_t2;
|
|
c.t1 = t1;
|
|
c.t2 = t2;
|
|
}
|
|
}
|
|
psb->m_faceRigidContacts.push_back(c);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
f.m_pcontact[3] = 0;
|
|
}
|
|
}
|
|
btSoftBody* psb;
|
|
const btCollisionObjectWrapper* m_colObj1Wrap;
|
|
btRigidBody* m_rigidBody;
|
|
btScalar dynmargin;
|
|
btScalar stamargin;
|
|
};
|
|
|
|
//
|
|
// CollideVF_SS
|
|
//
|
|
struct CollideVF_SS : btDbvt::ICollide
|
|
{
|
|
void Process(const btDbvtNode* lnode,
|
|
const btDbvtNode* lface)
|
|
{
|
|
btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
|
|
btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
|
|
for (int i = 0; i < 3; ++i)
|
|
{
|
|
if (face->m_n[i] == node)
|
|
continue;
|
|
}
|
|
|
|
btVector3 o = node->m_x;
|
|
btVector3 p;
|
|
btScalar d = SIMD_INFINITY;
|
|
ProjectOrigin(face->m_n[0]->m_x - o,
|
|
face->m_n[1]->m_x - o,
|
|
face->m_n[2]->m_x - o,
|
|
p, d);
|
|
const btScalar m = mrg + (o - node->m_q).length() * 2;
|
|
if (d < (m * m))
|
|
{
|
|
const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
|
|
const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o);
|
|
const btScalar ma = node->m_im;
|
|
btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
|
|
if ((n[0]->m_im <= 0) ||
|
|
(n[1]->m_im <= 0) ||
|
|
(n[2]->m_im <= 0))
|
|
{
|
|
mb = 0;
|
|
}
|
|
const btScalar ms = ma + mb;
|
|
if (ms > 0)
|
|
{
|
|
btSoftBody::SContact c;
|
|
c.m_normal = p / -btSqrt(d);
|
|
c.m_margin = m;
|
|
c.m_node = node;
|
|
c.m_face = face;
|
|
c.m_weights = w;
|
|
c.m_friction = btMax (psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF);
|
|
c.m_cfm[0] = ma / ms * psb[0]->m_cfg.kSHR;
|
|
c.m_cfm[1] = mb / ms * psb[1]->m_cfg.kSHR;
|
|
psb[0]->m_scontacts.push_back(c);
|
|
}
|
|
}
|
|
}
|
|
btSoftBody* psb[2];
|
|
btScalar mrg;
|
|
};
|
|
|
|
|
|
//
|
|
// CollideVF_DD
|
|
//
|
|
struct CollideVF_DD : btDbvt::ICollide
|
|
{
|
|
void Process(const btDbvtNode* lnode,
|
|
const btDbvtNode* lface)
|
|
{
|
|
btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
|
|
btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
|
|
|
|
btVector3 o = node->m_x;
|
|
btVector3 p;
|
|
btScalar d = SIMD_INFINITY;
|
|
ProjectOrigin(face->m_n[0]->m_x - o,
|
|
face->m_n[1]->m_x - o,
|
|
face->m_n[2]->m_x - o,
|
|
p, d);
|
|
const btScalar m = mrg + (o - node->m_q).safeNorm() * 2;
|
|
if (d < (m * m))
|
|
{
|
|
const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
|
|
const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o);
|
|
const btScalar ma = node->m_im;
|
|
btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
|
|
if ((n[0]->m_im <= 0) ||
|
|
(n[1]->m_im <= 0) ||
|
|
(n[2]->m_im <= 0))
|
|
{
|
|
mb = 0;
|
|
}
|
|
const btScalar ms = ma + mb;
|
|
if (ms > 0)
|
|
{
|
|
btSoftBody::DeformableFaceNodeContact c;
|
|
if (useFaceNormal)
|
|
c.m_normal = face->m_normal;
|
|
else
|
|
c.m_normal = p / -btSqrt(d);
|
|
c.m_margin = mrg;
|
|
c.m_node = node;
|
|
c.m_face = face;
|
|
c.m_bary = w;
|
|
// todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
|
|
c.m_weights = btScalar(2)/(btScalar(1) + w.length2()) * w;
|
|
c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
|
|
// the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
|
|
c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im;
|
|
c.m_c0 = btScalar(1)/(ma + c.m_imf);
|
|
psb[0]->m_faceNodeContacts.push_back(c);
|
|
}
|
|
}
|
|
}
|
|
btSoftBody* psb[2];
|
|
btScalar mrg;
|
|
bool useFaceNormal;
|
|
};
|
|
|
|
//
|
|
// CollideFF_DD
|
|
//
|
|
struct CollideFF_DD : btDbvt::ICollide
|
|
{
|
|
void Process(const btDbvntNode* lface1,
|
|
const btDbvntNode* lface2)
|
|
{
|
|
btSoftBody::Face* f = (btSoftBody::Face*)lface1->data;
|
|
btSoftBody::Face* face = (btSoftBody::Face*)lface2->data;
|
|
for (int node_id = 0; node_id < 3; ++node_id)
|
|
{
|
|
btSoftBody::Node* node = f->m_n[node_id];
|
|
bool skip = false;
|
|
for (int i = 0; i < 3; ++i)
|
|
{
|
|
if (face->m_n[i] == node)
|
|
{
|
|
skip = true;
|
|
break;
|
|
}
|
|
}
|
|
if (skip)
|
|
continue;
|
|
btVector3 o = node->m_x;
|
|
btVector3 p;
|
|
btScalar d = SIMD_INFINITY;
|
|
ProjectOrigin(face->m_n[0]->m_x - o,
|
|
face->m_n[1]->m_x - o,
|
|
face->m_n[2]->m_x - o,
|
|
p, d);
|
|
const btScalar m = mrg + (o - node->m_q).safeNorm() * 2;
|
|
if (d < (m * m))
|
|
{
|
|
const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
|
|
const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o);
|
|
const btScalar ma = node->m_im;
|
|
btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
|
|
if ((n[0]->m_im <= 0) ||
|
|
(n[1]->m_im <= 0) ||
|
|
(n[2]->m_im <= 0))
|
|
{
|
|
mb = 0;
|
|
}
|
|
const btScalar ms = ma + mb;
|
|
if (ms > 0)
|
|
{
|
|
btSoftBody::DeformableFaceNodeContact c;
|
|
if (useFaceNormal)
|
|
c.m_normal = face->m_normal;
|
|
else
|
|
c.m_normal = p / -btSqrt(d);
|
|
c.m_margin = mrg;
|
|
c.m_node = node;
|
|
c.m_face = face;
|
|
c.m_bary = w;
|
|
// todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
|
|
c.m_weights = btScalar(2)/(btScalar(1) + w.length2()) * w;
|
|
c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
|
|
// the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
|
|
c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im;
|
|
c.m_c0 = btScalar(1)/(ma + c.m_imf);
|
|
psb[0]->m_faceNodeContacts.push_back(c);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
btSoftBody* psb[2];
|
|
btScalar mrg;
|
|
bool useFaceNormal;
|
|
};
|
|
};
|
|
|
|
#endif //_BT_SOFT_BODY_INTERNALS_H
|