diff options
Diffstat (limited to 'thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h')
| -rw-r--r-- | thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h | 2108 |
1 files changed, 0 insertions, 2108 deletions
diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h b/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h deleted file mode 100644 index c17bbb5cd4..0000000000 --- a/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h +++ /dev/null @@ -1,2108 +0,0 @@ -/* -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> -#include "poly34.h" - -// 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 SIMD_FORCE_INLINE 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 SIMD_FORCE_INLINE 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; -} - -static SIMD_FORCE_INLINE bool proximityTest(const btVector3& x1, const btVector3& x2, const btVector3& x3, const btVector3& x4, const btVector3& normal, const btScalar& mrg, btVector3& bary) -{ - btVector3 x43 = x4 - x3; - if (std::abs(x43.dot(normal)) > mrg) - return false; - btVector3 x13 = x1 - x3; - btVector3 x23 = x2 - x3; - btScalar a11 = x13.length2(); - btScalar a22 = x23.length2(); - btScalar a12 = x13.dot(x23); - btScalar b1 = x13.dot(x43); - btScalar b2 = x23.dot(x43); - btScalar det = a11 * a22 - a12 * a12; - if (det < SIMD_EPSILON) - return false; - btScalar w1 = (b1 * a22 - b2 * a12) / det; - btScalar w2 = (b2 * a11 - b1 * a12) / det; - btScalar w3 = 1 - w1 - w2; - btScalar delta = mrg / std::sqrt(0.5 * std::abs(x13.cross(x23).safeNorm())); - bary = btVector3(w1, w2, w3); - for (int i = 0; i < 3; ++i) - { - if (bary[i] < -delta || bary[i] > 1 + delta) - return false; - } - return true; -} -static const int KDOP_COUNT = 13; -static btVector3 dop[KDOP_COUNT] = {btVector3(1, 0, 0), - btVector3(0, 1, 0), - btVector3(0, 0, 1), - btVector3(1, 1, 0), - btVector3(1, 0, 1), - btVector3(0, 1, 1), - btVector3(1, -1, 0), - btVector3(1, 0, -1), - btVector3(0, 1, -1), - btVector3(1, 1, 1), - btVector3(1, -1, 1), - btVector3(1, 1, -1), - btVector3(1, -1, -1)}; - -static inline int getSign(const btVector3& n, const btVector3& x) -{ - btScalar d = n.dot(x); - if (d > SIMD_EPSILON) - return 1; - if (d < -SIMD_EPSILON) - return -1; - return 0; -} - -static SIMD_FORCE_INLINE bool hasSeparatingPlane(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) -{ - btVector3 hex[6] = {face->m_n[0]->m_x - node->m_x, - face->m_n[1]->m_x - node->m_x, - face->m_n[2]->m_x - node->m_x, - face->m_n[0]->m_x + dt * face->m_n[0]->m_v - node->m_x, - face->m_n[1]->m_x + dt * face->m_n[1]->m_v - node->m_x, - face->m_n[2]->m_x + dt * face->m_n[2]->m_v - node->m_x}; - btVector3 segment = dt * node->m_v; - for (int i = 0; i < KDOP_COUNT; ++i) - { - int s = getSign(dop[i], segment); - int j = 0; - for (; j < 6; ++j) - { - if (getSign(dop[i], hex[j]) == s) - break; - } - if (j == 6) - return true; - } - return false; -} - -static SIMD_FORCE_INLINE bool nearZero(const btScalar& a) -{ - return (a > -SAFE_EPSILON && a < SAFE_EPSILON); -} -static SIMD_FORCE_INLINE bool sameSign(const btScalar& a, const btScalar& b) -{ - return (nearZero(a) || nearZero(b) || (a > SAFE_EPSILON && b > SAFE_EPSILON) || (a < -SAFE_EPSILON && b < -SAFE_EPSILON)); -} -static SIMD_FORCE_INLINE bool diffSign(const btScalar& a, const btScalar& b) -{ - return !sameSign(a, b); -} -inline btScalar evaluateBezier2(const btScalar& p0, const btScalar& p1, const btScalar& p2, const btScalar& t, const btScalar& s) -{ - btScalar s2 = s * s; - btScalar t2 = t * t; - - return p0 * s2 + p1 * btScalar(2.0) * s * t + p2 * t2; -} -inline btScalar evaluateBezier(const btScalar& p0, const btScalar& p1, const btScalar& p2, const btScalar& p3, const btScalar& t, const btScalar& s) -{ - btScalar s2 = s * s; - btScalar s3 = s2 * s; - btScalar t2 = t * t; - btScalar t3 = t2 * t; - - return p0 * s3 + p1 * btScalar(3.0) * s2 * t + p2 * btScalar(3.0) * s * t2 + p3 * t3; -} -static SIMD_FORCE_INLINE bool getSigns(bool type_c, const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, const btScalar& t1, btScalar& lt0, btScalar& lt1) -{ - if (sameSign(t0, t1)) - { - lt0 = t0; - lt1 = t0; - return true; - } - - if (type_c || diffSign(k0, k3)) - { - btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1); - if (t0 < -0) - ft = -ft; - - if (sameSign(ft, k0)) - { - lt0 = t1; - lt1 = t1; - } - else - { - lt0 = t0; - lt1 = t0; - } - return true; - } - - if (!type_c) - { - btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1); - if (t0 < -0) - ft = -ft; - - if (diffSign(ft, k0)) - { - lt0 = t0; - lt1 = t1; - return true; - } - - btScalar fk = evaluateBezier2(k1 - k0, k2 - k1, k3 - k2, t0, -t1); - - if (sameSign(fk, k1 - k0)) - lt0 = lt1 = t1; - else - lt0 = lt1 = t0; - - return true; - } - return false; -} - -static SIMD_FORCE_INLINE void getBernsteinCoeff(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, btScalar& k0, btScalar& k1, btScalar& k2, btScalar& k3) -{ - const btVector3& n0 = face->m_n0; - const btVector3& n1 = face->m_n1; - btVector3 n_hat = n0 + n1 - face->m_vn; - btVector3 p0ma0 = node->m_x - face->m_n[0]->m_x; - btVector3 p1ma1 = node->m_q - face->m_n[0]->m_q; - k0 = (p0ma0).dot(n0) * 3.0; - k1 = (p0ma0).dot(n_hat) + (p1ma1).dot(n0); - k2 = (p1ma1).dot(n_hat) + (p0ma0).dot(n1); - k3 = (p1ma1).dot(n1) * 3.0; -} - -static SIMD_FORCE_INLINE void polyDecomposition(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& j0, const btScalar& j1, const btScalar& j2, btScalar& u0, btScalar& u1, btScalar& v0, btScalar& v1) -{ - btScalar denom = 4.0 * (j1 - j2) * (j1 - j0) + (j2 - j0) * (j2 - j0); - u0 = (2.0 * (j1 - j2) * (3.0 * k1 - 2.0 * k0 - k3) - (j0 - j2) * (3.0 * k2 - 2.0 * k3 - k0)) / denom; - u1 = (2.0 * (j1 - j0) * (3.0 * k2 - 2.0 * k3 - k0) - (j2 - j0) * (3.0 * k1 - 2.0 * k0 - k3)) / denom; - v0 = k0 - u0 * j0; - v1 = k3 - u1 * j2; -} - -static SIMD_FORCE_INLINE bool rootFindingLemma(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3) -{ - btScalar u0, u1, v0, v1; - btScalar j0 = 3.0 * (k1 - k0); - btScalar j1 = 3.0 * (k2 - k1); - btScalar j2 = 3.0 * (k3 - k2); - polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); - if (sameSign(v0, v1)) - { - btScalar Ypa = j0 * (1.0 - v0) * (1.0 - v0) + 2.0 * j1 * v0 * (1.0 - v0) + j2 * v0 * v0; // Y'(v0) - if (sameSign(Ypa, j0)) - { - return (diffSign(k0, v1)); - } - } - return diffSign(k0, v0); -} - -static SIMD_FORCE_INLINE void getJs(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Node* a, const btSoftBody::Node* b, const btSoftBody::Node* c, const btSoftBody::Node* p, const btScalar& dt, btScalar& j0, btScalar& j1, btScalar& j2) -{ - const btVector3& a0 = a->m_x; - const btVector3& b0 = b->m_x; - const btVector3& c0 = c->m_x; - const btVector3& va = a->m_v; - const btVector3& vb = b->m_v; - const btVector3& vc = c->m_v; - const btVector3 a1 = a0 + dt * va; - const btVector3 b1 = b0 + dt * vb; - const btVector3 c1 = c0 + dt * vc; - btVector3 n0 = (b0 - a0).cross(c0 - a0); - btVector3 n1 = (b1 - a1).cross(c1 - a1); - btVector3 n_hat = n0 + n1 - dt * dt * (vb - va).cross(vc - va); - const btVector3& p0 = p->m_x; - const btVector3& vp = p->m_v; - btVector3 p1 = p0 + dt * vp; - btVector3 m0 = (b0 - p0).cross(c0 - p0); - btVector3 m1 = (b1 - p1).cross(c1 - p1); - btVector3 m_hat = m0 + m1 - dt * dt * (vb - vp).cross(vc - vp); - btScalar l0 = m0.dot(n0); - btScalar l1 = 0.25 * (m0.dot(n_hat) + m_hat.dot(n0)); - btScalar l2 = btScalar(1) / btScalar(6) * (m0.dot(n1) + m_hat.dot(n_hat) + m1.dot(n0)); - btScalar l3 = 0.25 * (m_hat.dot(n1) + m1.dot(n_hat)); - btScalar l4 = m1.dot(n1); - - btScalar k1p = 0.25 * k0 + 0.75 * k1; - btScalar k2p = 0.5 * k1 + 0.5 * k2; - btScalar k3p = 0.75 * k2 + 0.25 * k3; - - btScalar s0 = (l1 * k0 - l0 * k1p) * 4.0; - btScalar s1 = (l2 * k0 - l0 * k2p) * 2.0; - btScalar s2 = (l3 * k0 - l0 * k3p) * btScalar(4) / btScalar(3); - btScalar s3 = l4 * k0 - l0 * k3; - - j0 = (s1 * k0 - s0 * k1) * 3.0; - j1 = (s2 * k0 - s0 * k2) * 1.5; - j2 = (s3 * k0 - s0 * k3); -} - -static SIMD_FORCE_INLINE bool signDetermination1Internal(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& u0, const btScalar& u1, const btScalar& v0, const btScalar& v1) -{ - btScalar Yu0 = k0 * (1.0 - u0) * (1.0 - u0) * (1.0 - u0) + 3.0 * k1 * u0 * (1.0 - u0) * (1.0 - u0) + 3.0 * k2 * u0 * u0 * (1.0 - u0) + k3 * u0 * u0 * u0; // Y(u0) - btScalar Yv0 = k0 * (1.0 - v0) * (1.0 - v0) * (1.0 - v0) + 3.0 * k1 * v0 * (1.0 - v0) * (1.0 - v0) + 3.0 * k2 * v0 * v0 * (1.0 - v0) + k3 * v0 * v0 * v0; // Y(v0) - - btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0; - btScalar L = sameSign(sign_Ytp, k0) ? u1 : u0; - sign_Ytp = (v0 > v1) ? Yv0 : -Yv0; - btScalar K = (sameSign(sign_Ytp, k0)) ? v1 : v0; - return diffSign(L, K); -} - -static SIMD_FORCE_INLINE bool signDetermination2Internal(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& j0, const btScalar& j1, const btScalar& j2, const btScalar& u0, const btScalar& u1, const btScalar& v0, const btScalar& v1) -{ - btScalar Yu0 = k0 * (1.0 - u0) * (1.0 - u0) * (1.0 - u0) + 3.0 * k1 * u0 * (1.0 - u0) * (1.0 - u0) + 3.0 * k2 * u0 * u0 * (1.0 - u0) + k3 * u0 * u0 * u0; // Y(u0) - btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0, L1, L2; - if (diffSign(sign_Ytp, k0)) - { - L1 = u0; - L2 = u1; - } - else - { - btScalar Yp_u0 = j0 * (1.0 - u0) * (1.0 - u0) + 2.0 * j1 * (1.0 - u0) * u0 + j2 * u0 * u0; - if (sameSign(Yp_u0, j0)) - { - L1 = u1; - L2 = u1; - } - else - { - L1 = u0; - L2 = u0; - } - } - btScalar Yv0 = k0 * (1.0 - v0) * (1.0 - v0) * (1.0 - v0) + 3.0 * k1 * v0 * (1.0 - v0) * (1.0 - v0) + 3.0 * k2 * v0 * v0 * (1.0 - v0) + k3 * v0 * v0 * v0; // Y(uv0) - sign_Ytp = (v0 > v1) ? Yv0 : -Yv0; - btScalar K1, K2; - if (diffSign(sign_Ytp, k0)) - { - K1 = v0; - K2 = v1; - } - else - { - btScalar Yp_v0 = j0 * (1.0 - v0) * (1.0 - v0) + 2.0 * j1 * (1.0 - v0) * v0 + j2 * v0 * v0; - if (sameSign(Yp_v0, j0)) - { - K1 = v1; - K2 = v1; - } - else - { - K1 = v0; - K2 = v0; - } - } - return (diffSign(K1, L1) || diffSign(L2, K2)); -} - -static SIMD_FORCE_INLINE bool signDetermination1(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) -{ - btScalar j0, j1, j2, u0, u1, v0, v1; - // p1 - getJs(k0, k1, k2, k3, face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2); - if (nearZero(j0 + j2 - j1 * 2.0)) - { - btScalar lt0, lt1; - getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - return false; - } - else - { - polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); - if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1)) - return false; - } - // p2 - getJs(k0, k1, k2, k3, face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2); - if (nearZero(j0 + j2 - j1 * 2.0)) - { - btScalar lt0, lt1; - getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - return false; - } - else - { - polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); - if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1)) - return false; - } - // p3 - getJs(k0, k1, k2, k3, face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2); - if (nearZero(j0 + j2 - j1 * 2.0)) - { - btScalar lt0, lt1; - getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - return false; - } - else - { - polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); - if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1)) - return false; - } - return true; -} - -static SIMD_FORCE_INLINE bool signDetermination2(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) -{ - btScalar j0, j1, j2, u0, u1, v0, v1; - // p1 - getJs(k0, k1, k2, k3, face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2); - if (nearZero(j0 + j2 - j1 * 2.0)) - { - btScalar lt0, lt1; - bool bt0 = true, bt1 = true; - getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - bt0 = false; - if (lt1 < -SAFE_EPSILON) - bt1 = false; - if (!bt0 && !bt1) - return false; - } - else - { - polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); - if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1)) - return false; - } - // p2 - getJs(k0, k1, k2, k3, face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2); - if (nearZero(j0 + j2 - j1 * 2.0)) - { - btScalar lt0, lt1; - bool bt0 = true, bt1 = true; - getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - bt0 = false; - if (lt1 < -SAFE_EPSILON) - bt1 = false; - if (!bt0 && !bt1) - return false; - } - else - { - polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); - if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1)) - return false; - } - // p3 - getJs(k0, k1, k2, k3, face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2); - if (nearZero(j0 + j2 - j1 * 2.0)) - { - btScalar lt0, lt1; - bool bt0 = true, bt1 = true; - getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1); - if (lt0 < -SAFE_EPSILON) - bt0 = false; - if (lt1 < -SAFE_EPSILON) - bt1 = false; - if (!bt0 && !bt1) - return false; - } - else - { - polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1); - if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1)) - return false; - } - return true; -} - -static SIMD_FORCE_INLINE bool coplanarAndInsideTest(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) -{ - // Coplanar test - if (diffSign(k1 - k0, k3 - k2)) - { - // Case b: - if (sameSign(k0, k3) && !rootFindingLemma(k0, k1, k2, k3)) - return false; - // inside test - return signDetermination2(k0, k1, k2, k3, face, node, dt); - } - else - { - // Case c: - if (sameSign(k0, k3)) - return false; - // inside test - return signDetermination1(k0, k1, k2, k3, face, node, dt); - } - return false; -} -static SIMD_FORCE_INLINE bool conservativeCulling(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& mrg) -{ - if (k0 > mrg && k1 > mrg && k2 > mrg && k3 > mrg) - return true; - if (k0 < -mrg && k1 < -mrg && k2 < -mrg && k3 < -mrg) - return true; - return false; -} - -static SIMD_FORCE_INLINE bool bernsteinVFTest(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& mrg, const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt) -{ - if (conservativeCulling(k0, k1, k2, k3, mrg)) - return false; - return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt); -} - -static SIMD_FORCE_INLINE void deCasteljau(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& t0, btScalar& k10, btScalar& k20, btScalar& k30, btScalar& k21, btScalar& k12) -{ - k10 = k0 * (1.0 - t0) + k1 * t0; - btScalar k11 = k1 * (1.0 - t0) + k2 * t0; - k12 = k2 * (1.0 - t0) + k3 * t0; - k20 = k10 * (1.0 - t0) + k11 * t0; - k21 = k11 * (1.0 - t0) + k12 * t0; - k30 = k20 * (1.0 - t0) + k21 * t0; -} -static SIMD_FORCE_INLINE bool bernsteinVFTest(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg) -{ - btScalar k0, k1, k2, k3; - getBernsteinCoeff(face, node, dt, k0, k1, k2, k3); - if (conservativeCulling(k0, k1, k2, k3, mrg)) - return false; - return true; - if (diffSign(k2 - 2.0 * k1 + k0, k3 - 2.0 * k2 + k1)) - { - btScalar k10, k20, k30, k21, k12; - btScalar t0 = (k2 - 2.0 * k1 + k0) / (k0 - 3.0 * k1 + 3.0 * k2 - k3); - deCasteljau(k0, k1, k2, k3, t0, k10, k20, k30, k21, k12); - return bernsteinVFTest(k0, k10, k20, k30, mrg, face, node, dt) || bernsteinVFTest(k30, k21, k12, k3, mrg, face, node, dt); - } - return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt); -} - -static SIMD_FORCE_INLINE bool continuousCollisionDetection(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg, btVector3& bary) -{ - if (hasSeparatingPlane(face, node, dt)) - return false; - btVector3 x21 = face->m_n[1]->m_x - face->m_n[0]->m_x; - btVector3 x31 = face->m_n[2]->m_x - face->m_n[0]->m_x; - btVector3 x41 = node->m_x - face->m_n[0]->m_x; - btVector3 v21 = face->m_n[1]->m_v - face->m_n[0]->m_v; - btVector3 v31 = face->m_n[2]->m_v - face->m_n[0]->m_v; - btVector3 v41 = node->m_v - face->m_n[0]->m_v; - btVector3 a = x21.cross(x31); - btVector3 b = x21.cross(v31) + v21.cross(x31); - btVector3 c = v21.cross(v31); - btVector3 d = x41; - btVector3 e = v41; - btScalar a0 = a.dot(d); - btScalar a1 = a.dot(e) + b.dot(d); - btScalar a2 = c.dot(d) + b.dot(e); - btScalar a3 = c.dot(e); - btScalar eps = SAFE_EPSILON; - int num_roots = 0; - btScalar roots[3]; - if (std::abs(a3) < eps) - { - // cubic term is zero - if (std::abs(a2) < eps) - { - if (std::abs(a1) < eps) - { - if (std::abs(a0) < eps) - { - num_roots = 2; - roots[0] = 0; - roots[1] = dt; - } - } - else - { - num_roots = 1; - roots[0] = -a0 / a1; - } - } - else - { - num_roots = SolveP2(roots, a1 / a2, a0 / a2); - } - } - else - { - num_roots = SolveP3(roots, a2 / a3, a1 / a3, a0 / a3); - } - // std::sort(roots, roots+num_roots); - if (num_roots > 1) - { - if (roots[0] > roots[1]) - btSwap(roots[0], roots[1]); - } - if (num_roots > 2) - { - if (roots[0] > roots[2]) - btSwap(roots[0], roots[2]); - if (roots[1] > roots[2]) - btSwap(roots[1], roots[2]); - } - for (int r = 0; r < num_roots; ++r) - { - double root = roots[r]; - if (root <= 0) - continue; - if (root > dt + SIMD_EPSILON) - return false; - btVector3 x1 = face->m_n[0]->m_x + root * face->m_n[0]->m_v; - btVector3 x2 = face->m_n[1]->m_x + root * face->m_n[1]->m_v; - btVector3 x3 = face->m_n[2]->m_x + root * face->m_n[2]->m_v; - btVector3 x4 = node->m_x + root * node->m_v; - btVector3 normal = (x2 - x1).cross(x3 - x1); - normal.safeNormalize(); - if (proximityTest(x1, x2, x3, x4, normal, mrg, bary)) - return true; - } - return false; -} -static SIMD_FORCE_INLINE bool bernsteinCCD(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg, btVector3& bary) -{ - if (!bernsteinVFTest(face, node, dt, mrg)) - return false; - if (!continuousCollisionDetection(face, node, dt, 1e-6, bary)) - return false; - return true; -} - -// -// 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 OuterProduct(const btVector3& v1, const btVector3& v2) -{ - btMatrix3x3 m; - btScalar a11 = v1[0] * v2[0]; - btScalar a12 = v1[0] * v2[1]; - btScalar a13 = v1[0] * v2[2]; - - btScalar a21 = v1[1] * v2[0]; - btScalar a22 = v1[1] * v2[1]; - btScalar a23 = v1[1] * v2[2]; - - btScalar a31 = v1[2] * v2[0]; - btScalar a32 = v1[2] * v2[1]; - btScalar a33 = v1[2] * v2[2]; - 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 dt, - const btMatrix3x3& effective_mass_inv, - btScalar imb, - const btMatrix3x3& iwi, - const btVector3& r) -{ - return (Diagonal(1 / dt) * Add(effective_mass_inv, MassMatrix(imb, iwi, r)).inverse()); - // btMatrix3x3 iimb = MassMatrix(imb, iwi, r); - // if (iimb.determinant() == 0) - // return effective_mass_inv.inverse(); - // return effective_mass_inv.inverse() * Add(effective_mass_inv.inverse(), iimb.inverse()).inverse() * iimb.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}^* - if (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; - c.m_c5 = n.m_effectiveMass_inv; - - 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, n.m_effectiveMass_inv, imb, iwi, ra); - // 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 = (n.m_effectiveMass_inv + 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)) - { - 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; - // friction is handled by the nodes to prevent sticking - // const btScalar fc = 0; - 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; - c.m_c5 = Diagonal(ima); - 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); - } - } - // Set caching barycenters to be false after collision detection. - // Only turn on when contact is static. - 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 bary; - if (proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary)) - { - const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]}; - const btVector3 w = bary; - 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; - c.m_normal = face->m_normal; - if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) - c.m_normal = -face->m_normal; - c.m_margin = mrg; - c.m_node = node; - c.m_face = face; - c.m_bary = w; - c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; - 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* f1 = (btSoftBody::Face*)lface1->data; - btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data; - if (f1 != f2) - { - Repel(f1, f2); - Repel(f2, f1); - } - } - void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2) - { - //#define REPEL_NEIGHBOR 1 -#ifndef REPEL_NEIGHBOR - for (int node_id = 0; node_id < 3; ++node_id) - { - btSoftBody::Node* node = f1->m_n[node_id]; - for (int i = 0; i < 3; ++i) - { - if (f2->m_n[i] == node) - return; - } - } -#endif - bool skip = false; - for (int node_id = 0; node_id < 3; ++node_id) - { - btSoftBody::Node* node = f1->m_n[node_id]; -#ifdef REPEL_NEIGHBOR - for (int i = 0; i < 3; ++i) - { - if (f2->m_n[i] == node) - { - skip = true; - break; - } - } - if (skip) - { - skip = false; - continue; - } -#endif - btSoftBody::Face* face = f2; - btVector3 bary; - if (!proximityTest(face->m_n[0]->m_x, face->m_n[1]->m_x, face->m_n[2]->m_x, node->m_x, face->m_normal, mrg, bary)) - continue; - btSoftBody::DeformableFaceNodeContact c; - c.m_normal = face->m_normal; - if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) - c.m_normal = -face->m_normal; - c.m_margin = mrg; - c.m_node = node; - c.m_face = face; - c.m_bary = bary; - c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; - psb[0]->m_faceNodeContacts.push_back(c); - } - } - btSoftBody* psb[2]; - btScalar mrg; - bool useFaceNormal; - }; - - struct CollideCCD : 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 bary; - if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary)) - { - btSoftBody::DeformableFaceNodeContact c; - c.m_normal = face->m_normal; - if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) - c.m_normal = -face->m_normal; - c.m_node = node; - c.m_face = face; - c.m_bary = bary; - c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; - psb[0]->m_faceNodeContacts.push_back(c); - } - } - void Process(const btDbvntNode* lface1, - const btDbvntNode* lface2) - { - btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data; - btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data; - if (f1 != f2) - { - Repel(f1, f2); - Repel(f2, f1); - } - } - void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2) - { - //#define REPEL_NEIGHBOR 1 -#ifndef REPEL_NEIGHBOR - for (int node_id = 0; node_id < 3; ++node_id) - { - btSoftBody::Node* node = f1->m_n[node_id]; - for (int i = 0; i < 3; ++i) - { - if (f2->m_n[i] == node) - return; - } - } -#endif - bool skip = false; - for (int node_id = 0; node_id < 3; ++node_id) - { - btSoftBody::Node* node = f1->m_n[node_id]; -#ifdef REPEL_NEIGHBOR - for (int i = 0; i < 3; ++i) - { - if (f2->m_n[i] == node) - { - skip = true; - break; - } - } - if (skip) - { - skip = false; - continue; - } -#endif - btSoftBody::Face* face = f2; - btVector3 bary; - if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary)) - { - btSoftBody::DeformableFaceNodeContact c; - c.m_normal = face->m_normal; - if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0) - c.m_normal = -face->m_normal; - c.m_node = node; - c.m_face = face; - c.m_bary = bary; - c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF; - psb[0]->m_faceNodeContacts.push_back(c); - } - } - } - btSoftBody* psb[2]; - btScalar dt, mrg; - bool useFaceNormal; - }; -}; -#endif //_BT_SOFT_BODY_INTERNALS_H |