dali-physics/third-party/bullet3/src/BulletSoftBody/btSoftBodyInternals.h

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 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.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15 ///btSoftBody implementation by Nathanael Presson
  16
  17 #ifndef _BT_SOFT_BODY_INTERNALS_H
  18 #define _BT_SOFT_BODY_INTERNALS_H
  19
  20 #include "btSoftBody.h"
  21 #include "LinearMath/btQuickprof.h"
  22 #include "LinearMath/btPolarDecomposition.h"
  23 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
  24 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
  25 #include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
  26 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
  27 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
  28 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
  29 #include <string.h>  //for memset
  30 #include <cmath>
  31 #include "poly34.h"
  32
  33 // 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
  34 static SIMD_FORCE_INLINE void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
  35                                                                                    btMultiBodyJacobianData& jacobianData,
  36                                                                                    const btVector3& contact_point,
  37                                                                                    const btVector3& dir)
  38 {
  39         const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
  40         jacobianData.m_jacobians.resize(ndof);
  41         jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
  42         btScalar* jac = &jacobianData.m_jacobians[0];
  43
  44         multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
  45         multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v);
  46 }
  47 static SIMD_FORCE_INLINE btVector3 generateUnitOrthogonalVector(const btVector3& u)
  48 {
  49         btScalar ux = u.getX();
  50         btScalar uy = u.getY();
  51         btScalar uz = u.getZ();
  52         btScalar ax = std::abs(ux);
  53         btScalar ay = std::abs(uy);
  54         btScalar az = std::abs(uz);
  55         btVector3 v;
  56         if (ax <= ay && ax <= az)
  57                 v = btVector3(0, -uz, uy);
  58         else if (ay <= ax && ay <= az)
  59                 v = btVector3(-uz, 0, ux);
  60         else
  61                 v = btVector3(-uy, ux, 0);
  62         v.normalize();
  63         return v;
  64 }
  65
  66 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)
  67 {
  68         btVector3 x43 = x4 - x3;
  69         if (std::abs(x43.dot(normal)) > mrg)
  70                 return false;
  71         btVector3 x13 = x1 - x3;
  72         btVector3 x23 = x2 - x3;
  73         btScalar a11 = x13.length2();
  74         btScalar a22 = x23.length2();
  75         btScalar a12 = x13.dot(x23);
  76         btScalar b1 = x13.dot(x43);
  77         btScalar b2 = x23.dot(x43);
  78         btScalar det = a11 * a22 - a12 * a12;
  79         if (det < SIMD_EPSILON)
  80                 return false;
  81         btScalar w1 = (b1 * a22 - b2 * a12) / det;
  82         btScalar w2 = (b2 * a11 - b1 * a12) / det;
  83         btScalar w3 = 1 - w1 - w2;
  84         btScalar delta = mrg / std::sqrt(0.5 * std::abs(x13.cross(x23).safeNorm()));
  85         bary = btVector3(w1, w2, w3);
  86         for (int i = 0; i < 3; ++i)
  87         {
  88                 if (bary[i] < -delta || bary[i] > 1 + delta)
  89                         return false;
  90         }
  91         return true;
  92 }
  93 static const int KDOP_COUNT = 13;
  94 static btVector3 dop[KDOP_COUNT] = {btVector3(1, 0, 0),
  95                                                                         btVector3(0, 1, 0),
  96                                                                         btVector3(0, 0, 1),
  97                                                                         btVector3(1, 1, 0),
  98                                                                         btVector3(1, 0, 1),
  99                                                                         btVector3(0, 1, 1),
 100                                                                         btVector3(1, -1, 0),
 101                                                                         btVector3(1, 0, -1),
 102                                                                         btVector3(0, 1, -1),
 103                                                                         btVector3(1, 1, 1),
 104                                                                         btVector3(1, -1, 1),
 105                                                                         btVector3(1, 1, -1),
 106                                                                         btVector3(1, -1, -1)};
 107
 108 static inline int getSign(const btVector3& n, const btVector3& x)
 109 {
 110         btScalar d = n.dot(x);
 111         if (d > SIMD_EPSILON)
 112                 return 1;
 113         if (d < -SIMD_EPSILON)
 114                 return -1;
 115         return 0;
 116 }
 117
 118 static SIMD_FORCE_INLINE bool hasSeparatingPlane(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt)
 119 {
 120         btVector3 hex[6] = {face->m_n[0]->m_x - node->m_x,
 121                                                 face->m_n[1]->m_x - node->m_x,
 122                                                 face->m_n[2]->m_x - node->m_x,
 123                                                 face->m_n[0]->m_x + dt * face->m_n[0]->m_v - node->m_x,
 124                                                 face->m_n[1]->m_x + dt * face->m_n[1]->m_v - node->m_x,
 125                                                 face->m_n[2]->m_x + dt * face->m_n[2]->m_v - node->m_x};
 126         btVector3 segment = dt * node->m_v;
 127         for (int i = 0; i < KDOP_COUNT; ++i)
 128         {
 129                 int s = getSign(dop[i], segment);
 130                 int j = 0;
 131                 for (; j < 6; ++j)
 132                 {
 133                         if (getSign(dop[i], hex[j]) == s)
 134                                 break;
 135                 }
 136                 if (j == 6)
 137                         return true;
 138         }
 139         return false;
 140 }
 141
 142 static SIMD_FORCE_INLINE bool nearZero(const btScalar& a)
 143 {
 144         return (a > -SAFE_EPSILON && a < SAFE_EPSILON);
 145 }
 146 static SIMD_FORCE_INLINE bool sameSign(const btScalar& a, const btScalar& b)
 147 {
 148         return (nearZero(a) || nearZero(b) || (a > SAFE_EPSILON && b > SAFE_EPSILON) || (a < -SAFE_EPSILON && b < -SAFE_EPSILON));
 149 }
 150 static SIMD_FORCE_INLINE bool diffSign(const btScalar& a, const btScalar& b)
 151 {
 152         return !sameSign(a, b);
 153 }
 154 inline btScalar evaluateBezier2(const btScalar& p0, const btScalar& p1, const btScalar& p2, const btScalar& t, const btScalar& s)
 155 {
 156         btScalar s2 = s * s;
 157         btScalar t2 = t * t;
 158
 159         return p0 * s2 + p1 * btScalar(2.0) * s * t + p2 * t2;
 160 }
 161 inline btScalar evaluateBezier(const btScalar& p0, const btScalar& p1, const btScalar& p2, const btScalar& p3, const btScalar& t, const btScalar& s)
 162 {
 163         btScalar s2 = s * s;
 164         btScalar s3 = s2 * s;
 165         btScalar t2 = t * t;
 166         btScalar t3 = t2 * t;
 167
 168         return p0 * s3 + p1 * btScalar(3.0) * s2 * t + p2 * btScalar(3.0) * s * t2 + p3 * t3;
 169 }
 170 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)
 171 {
 172         if (sameSign(t0, t1))
 173         {
 174                 lt0 = t0;
 175                 lt1 = t0;
 176                 return true;
 177         }
 178
 179         if (type_c || diffSign(k0, k3))
 180         {
 181                 btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1);
 182                 if (t0 < -0)
 183                         ft = -ft;
 184
 185                 if (sameSign(ft, k0))
 186                 {
 187                         lt0 = t1;
 188                         lt1 = t1;
 189                 }
 190                 else
 191                 {
 192                         lt0 = t0;
 193                         lt1 = t0;
 194                 }
 195                 return true;
 196         }
 197
 198         if (!type_c)
 199         {
 200                 btScalar ft = evaluateBezier(k0, k1, k2, k3, t0, -t1);
 201                 if (t0 < -0)
 202                         ft = -ft;
 203
 204                 if (diffSign(ft, k0))
 205                 {
 206                         lt0 = t0;
 207                         lt1 = t1;
 208                         return true;
 209                 }
 210
 211                 btScalar fk = evaluateBezier2(k1 - k0, k2 - k1, k3 - k2, t0, -t1);
 212
 213                 if (sameSign(fk, k1 - k0))
 214                         lt0 = lt1 = t1;
 215                 else
 216                         lt0 = lt1 = t0;
 217
 218                 return true;
 219         }
 220         return false;
 221 }
 222
 223 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)
 224 {
 225         const btVector3& n0 = face->m_n0;
 226         const btVector3& n1 = face->m_n1;
 227         btVector3 n_hat = n0 + n1 - face->m_vn;
 228         btVector3 p0ma0 = node->m_x - face->m_n[0]->m_x;
 229         btVector3 p1ma1 = node->m_q - face->m_n[0]->m_q;
 230         k0 = (p0ma0).dot(n0) * 3.0;
 231         k1 = (p0ma0).dot(n_hat) + (p1ma1).dot(n0);
 232         k2 = (p1ma1).dot(n_hat) + (p0ma0).dot(n1);
 233         k3 = (p1ma1).dot(n1) * 3.0;
 234 }
 235
 236 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)
 237 {
 238         btScalar denom = 4.0 * (j1 - j2) * (j1 - j0) + (j2 - j0) * (j2 - j0);
 239         u0 = (2.0 * (j1 - j2) * (3.0 * k1 - 2.0 * k0 - k3) - (j0 - j2) * (3.0 * k2 - 2.0 * k3 - k0)) / denom;
 240         u1 = (2.0 * (j1 - j0) * (3.0 * k2 - 2.0 * k3 - k0) - (j2 - j0) * (3.0 * k1 - 2.0 * k0 - k3)) / denom;
 241         v0 = k0 - u0 * j0;
 242         v1 = k3 - u1 * j2;
 243 }
 244
 245 static SIMD_FORCE_INLINE bool rootFindingLemma(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3)
 246 {
 247         btScalar u0, u1, v0, v1;
 248         btScalar j0 = 3.0 * (k1 - k0);
 249         btScalar j1 = 3.0 * (k2 - k1);
 250         btScalar j2 = 3.0 * (k3 - k2);
 251         polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
 252         if (sameSign(v0, v1))
 253         {
 254                 btScalar Ypa = j0 * (1.0 - v0) * (1.0 - v0) + 2.0 * j1 * v0 * (1.0 - v0) + j2 * v0 * v0;  // Y'(v0)
 255                 if (sameSign(Ypa, j0))
 256                 {
 257                         return (diffSign(k0, v1));
 258                 }
 259         }
 260         return diffSign(k0, v0);
 261 }
 262
 263 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)
 264 {
 265         const btVector3& a0 = a->m_x;
 266         const btVector3& b0 = b->m_x;
 267         const btVector3& c0 = c->m_x;
 268         const btVector3& va = a->m_v;
 269         const btVector3& vb = b->m_v;
 270         const btVector3& vc = c->m_v;
 271         const btVector3 a1 = a0 + dt * va;
 272         const btVector3 b1 = b0 + dt * vb;
 273         const btVector3 c1 = c0 + dt * vc;
 274         btVector3 n0 = (b0 - a0).cross(c0 - a0);
 275         btVector3 n1 = (b1 - a1).cross(c1 - a1);
 276         btVector3 n_hat = n0 + n1 - dt * dt * (vb - va).cross(vc - va);
 277         const btVector3& p0 = p->m_x;
 278         const btVector3& vp = p->m_v;
 279         btVector3 p1 = p0 + dt * vp;
 280         btVector3 m0 = (b0 - p0).cross(c0 - p0);
 281         btVector3 m1 = (b1 - p1).cross(c1 - p1);
 282         btVector3 m_hat = m0 + m1 - dt * dt * (vb - vp).cross(vc - vp);
 283         btScalar l0 = m0.dot(n0);
 284         btScalar l1 = 0.25 * (m0.dot(n_hat) + m_hat.dot(n0));
 285         btScalar l2 = btScalar(1) / btScalar(6) * (m0.dot(n1) + m_hat.dot(n_hat) + m1.dot(n0));
 286         btScalar l3 = 0.25 * (m_hat.dot(n1) + m1.dot(n_hat));
 287         btScalar l4 = m1.dot(n1);
 288
 289         btScalar k1p = 0.25 * k0 + 0.75 * k1;
 290         btScalar k2p = 0.5 * k1 + 0.5 * k2;
 291         btScalar k3p = 0.75 * k2 + 0.25 * k3;
 292
 293         btScalar s0 = (l1 * k0 - l0 * k1p) * 4.0;
 294         btScalar s1 = (l2 * k0 - l0 * k2p) * 2.0;
 295         btScalar s2 = (l3 * k0 - l0 * k3p) * btScalar(4) / btScalar(3);
 296         btScalar s3 = l4 * k0 - l0 * k3;
 297
 298         j0 = (s1 * k0 - s0 * k1) * 3.0;
 299         j1 = (s2 * k0 - s0 * k2) * 1.5;
 300         j2 = (s3 * k0 - s0 * k3);
 301 }
 302
 303 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)
 304 {
 305         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)
 306         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)
 307
 308         btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0;
 309         btScalar L = sameSign(sign_Ytp, k0) ? u1 : u0;
 310         sign_Ytp = (v0 > v1) ? Yv0 : -Yv0;
 311         btScalar K = (sameSign(sign_Ytp, k0)) ? v1 : v0;
 312         return diffSign(L, K);
 313 }
 314
 315 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)
 316 {
 317         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)
 318         btScalar sign_Ytp = (u0 > u1) ? Yu0 : -Yu0, L1, L2;
 319         if (diffSign(sign_Ytp, k0))
 320         {
 321                 L1 = u0;
 322                 L2 = u1;
 323         }
 324         else
 325         {
 326                 btScalar Yp_u0 = j0 * (1.0 - u0) * (1.0 - u0) + 2.0 * j1 * (1.0 - u0) * u0 + j2 * u0 * u0;
 327                 if (sameSign(Yp_u0, j0))
 328                 {
 329                         L1 = u1;
 330                         L2 = u1;
 331                 }
 332                 else
 333                 {
 334                         L1 = u0;
 335                         L2 = u0;
 336                 }
 337         }
 338         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)
 339         sign_Ytp = (v0 > v1) ? Yv0 : -Yv0;
 340         btScalar K1, K2;
 341         if (diffSign(sign_Ytp, k0))
 342         {
 343                 K1 = v0;
 344                 K2 = v1;
 345         }
 346         else
 347         {
 348                 btScalar Yp_v0 = j0 * (1.0 - v0) * (1.0 - v0) + 2.0 * j1 * (1.0 - v0) * v0 + j2 * v0 * v0;
 349                 if (sameSign(Yp_v0, j0))
 350                 {
 351                         K1 = v1;
 352                         K2 = v1;
 353                 }
 354                 else
 355                 {
 356                         K1 = v0;
 357                         K2 = v0;
 358                 }
 359         }
 360         return (diffSign(K1, L1) || diffSign(L2, K2));
 361 }
 362
 363 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)
 364 {
 365         btScalar j0, j1, j2, u0, u1, v0, v1;
 366         // p1
 367         getJs(k0, k1, k2, k3, face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2);
 368         if (nearZero(j0 + j2 - j1 * 2.0))
 369         {
 370                 btScalar lt0, lt1;
 371                 getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1);
 372                 if (lt0 < -SAFE_EPSILON)
 373                         return false;
 374         }
 375         else
 376         {
 377                 polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
 378                 if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1))
 379                         return false;
 380         }
 381         // p2
 382         getJs(k0, k1, k2, k3, face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2);
 383         if (nearZero(j0 + j2 - j1 * 2.0))
 384         {
 385                 btScalar lt0, lt1;
 386                 getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1);
 387                 if (lt0 < -SAFE_EPSILON)
 388                         return false;
 389         }
 390         else
 391         {
 392                 polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
 393                 if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1))
 394                         return false;
 395         }
 396         // p3
 397         getJs(k0, k1, k2, k3, face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2);
 398         if (nearZero(j0 + j2 - j1 * 2.0))
 399         {
 400                 btScalar lt0, lt1;
 401                 getSigns(true, k0, k1, k2, k3, j0, j2, lt0, lt1);
 402                 if (lt0 < -SAFE_EPSILON)
 403                         return false;
 404         }
 405         else
 406         {
 407                 polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
 408                 if (!signDetermination1Internal(k0, k1, k2, k3, u0, u1, v0, v1))
 409                         return false;
 410         }
 411         return true;
 412 }
 413
 414 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)
 415 {
 416         btScalar j0, j1, j2, u0, u1, v0, v1;
 417         // p1
 418         getJs(k0, k1, k2, k3, face->m_n[0], face->m_n[1], face->m_n[2], node, dt, j0, j1, j2);
 419         if (nearZero(j0 + j2 - j1 * 2.0))
 420         {
 421                 btScalar lt0, lt1;
 422                 bool bt0 = true, bt1 = true;
 423                 getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1);
 424                 if (lt0 < -SAFE_EPSILON)
 425                         bt0 = false;
 426                 if (lt1 < -SAFE_EPSILON)
 427                         bt1 = false;
 428                 if (!bt0 && !bt1)
 429                         return false;
 430         }
 431         else
 432         {
 433                 polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
 434                 if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1))
 435                         return false;
 436         }
 437         // p2
 438         getJs(k0, k1, k2, k3, face->m_n[1], face->m_n[2], face->m_n[0], node, dt, j0, j1, j2);
 439         if (nearZero(j0 + j2 - j1 * 2.0))
 440         {
 441                 btScalar lt0, lt1;
 442                 bool bt0 = true, bt1 = true;
 443                 getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1);
 444                 if (lt0 < -SAFE_EPSILON)
 445                         bt0 = false;
 446                 if (lt1 < -SAFE_EPSILON)
 447                         bt1 = false;
 448                 if (!bt0 && !bt1)
 449                         return false;
 450         }
 451         else
 452         {
 453                 polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
 454                 if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1))
 455                         return false;
 456         }
 457         // p3
 458         getJs(k0, k1, k2, k3, face->m_n[2], face->m_n[0], face->m_n[1], node, dt, j0, j1, j2);
 459         if (nearZero(j0 + j2 - j1 * 2.0))
 460         {
 461                 btScalar lt0, lt1;
 462                 bool bt0 = true, bt1 = true;
 463                 getSigns(false, k0, k1, k2, k3, j0, j2, lt0, lt1);
 464                 if (lt0 < -SAFE_EPSILON)
 465                         bt0 = false;
 466                 if (lt1 < -SAFE_EPSILON)
 467                         bt1 = false;
 468                 if (!bt0 && !bt1)
 469                         return false;
 470         }
 471         else
 472         {
 473                 polyDecomposition(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1);
 474                 if (!signDetermination2Internal(k0, k1, k2, k3, j0, j1, j2, u0, u1, v0, v1))
 475                         return false;
 476         }
 477         return true;
 478 }
 479
 480 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)
 481 {
 482         // Coplanar test
 483         if (diffSign(k1 - k0, k3 - k2))
 484         {
 485                 // Case b:
 486                 if (sameSign(k0, k3) && !rootFindingLemma(k0, k1, k2, k3))
 487                         return false;
 488                 // inside test
 489                 return signDetermination2(k0, k1, k2, k3, face, node, dt);
 490         }
 491         else
 492         {
 493                 // Case c:
 494                 if (sameSign(k0, k3))
 495                         return false;
 496                 // inside test
 497                 return signDetermination1(k0, k1, k2, k3, face, node, dt);
 498         }
 499         return false;
 500 }
 501 static SIMD_FORCE_INLINE bool conservativeCulling(const btScalar& k0, const btScalar& k1, const btScalar& k2, const btScalar& k3, const btScalar& mrg)
 502 {
 503         if (k0 > mrg && k1 > mrg && k2 > mrg && k3 > mrg)
 504                 return true;
 505         if (k0 < -mrg && k1 < -mrg && k2 < -mrg && k3 < -mrg)
 506                 return true;
 507         return false;
 508 }
 509
 510 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)
 511 {
 512         if (conservativeCulling(k0, k1, k2, k3, mrg))
 513                 return false;
 514         return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt);
 515 }
 516
 517 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)
 518 {
 519         k10 = k0 * (1.0 - t0) + k1 * t0;
 520         btScalar k11 = k1 * (1.0 - t0) + k2 * t0;
 521         k12 = k2 * (1.0 - t0) + k3 * t0;
 522         k20 = k10 * (1.0 - t0) + k11 * t0;
 523         k21 = k11 * (1.0 - t0) + k12 * t0;
 524         k30 = k20 * (1.0 - t0) + k21 * t0;
 525 }
 526 static SIMD_FORCE_INLINE bool bernsteinVFTest(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg)
 527 {
 528         btScalar k0, k1, k2, k3;
 529         getBernsteinCoeff(face, node, dt, k0, k1, k2, k3);
 530         if (conservativeCulling(k0, k1, k2, k3, mrg))
 531                 return false;
 532         return true;
 533         if (diffSign(k2 - 2.0 * k1 + k0, k3 - 2.0 * k2 + k1))
 534         {
 535                 btScalar k10, k20, k30, k21, k12;
 536                 btScalar t0 = (k2 - 2.0 * k1 + k0) / (k0 - 3.0 * k1 + 3.0 * k2 - k3);
 537                 deCasteljau(k0, k1, k2, k3, t0, k10, k20, k30, k21, k12);
 538                 return bernsteinVFTest(k0, k10, k20, k30, mrg, face, node, dt) || bernsteinVFTest(k30, k21, k12, k3, mrg, face, node, dt);
 539         }
 540         return coplanarAndInsideTest(k0, k1, k2, k3, face, node, dt);
 541 }
 542
 543 static SIMD_FORCE_INLINE bool continuousCollisionDetection(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg, btVector3& bary)
 544 {
 545         if (hasSeparatingPlane(face, node, dt))
 546                 return false;
 547         btVector3 x21 = face->m_n[1]->m_x - face->m_n[0]->m_x;
 548         btVector3 x31 = face->m_n[2]->m_x - face->m_n[0]->m_x;
 549         btVector3 x41 = node->m_x - face->m_n[0]->m_x;
 550         btVector3 v21 = face->m_n[1]->m_v - face->m_n[0]->m_v;
 551         btVector3 v31 = face->m_n[2]->m_v - face->m_n[0]->m_v;
 552         btVector3 v41 = node->m_v - face->m_n[0]->m_v;
 553         btVector3 a = x21.cross(x31);
 554         btVector3 b = x21.cross(v31) + v21.cross(x31);
 555         btVector3 c = v21.cross(v31);
 556         btVector3 d = x41;
 557         btVector3 e = v41;
 558         btScalar a0 = a.dot(d);
 559         btScalar a1 = a.dot(e) + b.dot(d);
 560         btScalar a2 = c.dot(d) + b.dot(e);
 561         btScalar a3 = c.dot(e);
 562         btScalar eps = SAFE_EPSILON;
 563         int num_roots = 0;
 564         btScalar roots[3];
 565         if (std::abs(a3) < eps)
 566         {
 567                 // cubic term is zero
 568                 if (std::abs(a2) < eps)
 569                 {
 570                         if (std::abs(a1) < eps)
 571                         {
 572                                 if (std::abs(a0) < eps)
 573                                 {
 574                                         num_roots = 2;
 575                                         roots[0] = 0;
 576                                         roots[1] = dt;
 577                                 }
 578                         }
 579                         else
 580                         {
 581                                 num_roots = 1;
 582                                 roots[0] = -a0 / a1;
 583                         }
 584                 }
 585                 else
 586                 {
 587                         num_roots = SolveP2(roots, a1 / a2, a0 / a2);
 588                 }
 589         }
 590         else
 591         {
 592                 num_roots = SolveP3(roots, a2 / a3, a1 / a3, a0 / a3);
 593         }
 594         //    std::sort(roots, roots+num_roots);
 595         if (num_roots > 1)
 596         {
 597                 if (roots[0] > roots[1])
 598                         btSwap(roots[0], roots[1]);
 599         }
 600         if (num_roots > 2)
 601         {
 602                 if (roots[0] > roots[2])
 603                         btSwap(roots[0], roots[2]);
 604                 if (roots[1] > roots[2])
 605                         btSwap(roots[1], roots[2]);
 606         }
 607         for (int r = 0; r < num_roots; ++r)
 608         {
 609                 double root = roots[r];
 610                 if (root <= 0)
 611                         continue;
 612                 if (root > dt + SIMD_EPSILON)
 613                         return false;
 614                 btVector3 x1 = face->m_n[0]->m_x + root * face->m_n[0]->m_v;
 615                 btVector3 x2 = face->m_n[1]->m_x + root * face->m_n[1]->m_v;
 616                 btVector3 x3 = face->m_n[2]->m_x + root * face->m_n[2]->m_v;
 617                 btVector3 x4 = node->m_x + root * node->m_v;
 618                 btVector3 normal = (x2 - x1).cross(x3 - x1);
 619                 normal.safeNormalize();
 620                 if (proximityTest(x1, x2, x3, x4, normal, mrg, bary))
 621                         return true;
 622         }
 623         return false;
 624 }
 625 static SIMD_FORCE_INLINE bool bernsteinCCD(const btSoftBody::Face* face, const btSoftBody::Node* node, const btScalar& dt, const btScalar& mrg, btVector3& bary)
 626 {
 627         if (!bernsteinVFTest(face, node, dt, mrg))
 628                 return false;
 629         if (!continuousCollisionDetection(face, node, dt, 1e-6, bary))
 630                 return false;
 631         return true;
 632 }
 633
 634 //
 635 // btSymMatrix
 636 //
 637 template <typename T>
 638 struct btSymMatrix
 639 {
 640         btSymMatrix() : dim(0) {}
 641         btSymMatrix(int n, const T& init = T()) { resize(n, init); }
 642         void resize(int n, const T& init = T())
 643         {
 644                 dim = n;
 645                 store.resize((n * (n + 1)) / 2, init);
 646         }
 647         int index(int c, int r) const
 648         {
 649                 if (c > r) btSwap(c, r);
 650                 btAssert(r < dim);
 651                 return ((r * (r + 1)) / 2 + c);
 652         }
 653         T& operator()(int c, int r) { return (store[index(c, r)]); }
 654         const T& operator()(int c, int r) const { return (store[index(c, r)]); }
 655         btAlignedObjectArray<T> store;
 656         int dim;
 657 };
 658
 659 //
 660 // btSoftBodyCollisionShape
 661 //
 662 class btSoftBodyCollisionShape : public btConcaveShape
 663 {
 664 public:
 665         btSoftBody* m_body;
 666
 667         btSoftBodyCollisionShape(btSoftBody* backptr)
 668         {
 669                 m_shapeType = SOFTBODY_SHAPE_PROXYTYPE;
 670                 m_body = backptr;
 671         }
 672
 673         virtual ~btSoftBodyCollisionShape()
 674         {
 675         }
 676
 677         void processAllTriangles(btTriangleCallback* /*callback*/, const btVector3& /*aabbMin*/, const btVector3& /*aabbMax*/) const
 678         {
 679                 //not yet
 680                 btAssert(0);
 681         }
 682
 683         ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
 684         virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
 685         {
 686                 /* t is usually identity, except when colliding against btCompoundShape. See Issue 512 */
 687                 const btVector3 mins = m_body->m_bounds[0];
 688                 const btVector3 maxs = m_body->m_bounds[1];
 689                 const btVector3 crns[] = {t * btVector3(mins.x(), mins.y(), mins.z()),
 690                                                                   t * btVector3(maxs.x(), mins.y(), mins.z()),
 691                                                                   t * btVector3(maxs.x(), maxs.y(), mins.z()),
 692                                                                   t * btVector3(mins.x(), maxs.y(), mins.z()),
 693                                                                   t * btVector3(mins.x(), mins.y(), maxs.z()),
 694                                                                   t * btVector3(maxs.x(), mins.y(), maxs.z()),
 695                                                                   t * btVector3(maxs.x(), maxs.y(), maxs.z()),
 696                                                                   t * btVector3(mins.x(), maxs.y(), maxs.z())};
 697                 aabbMin = aabbMax = crns[0];
 698                 for (int i = 1; i < 8; ++i)
 699                 {
 700                         aabbMin.setMin(crns[i]);
 701                         aabbMax.setMax(crns[i]);
 702                 }
 703         }
 704
 705         virtual void setLocalScaling(const btVector3& /*scaling*/)
 706         {
 707                 ///na
 708         }
 709         virtual const btVector3& getLocalScaling() const
 710         {
 711                 static const btVector3 dummy(1, 1, 1);
 712                 return dummy;
 713         }
 714         virtual void calculateLocalInertia(btScalar /*mass*/, btVector3& /*inertia*/) const
 715         {
 716                 ///not yet
 717                 btAssert(0);
 718         }
 719         virtual const char* getName() const
 720         {
 721                 return "SoftBody";
 722         }
 723 };
 724
 725 //
 726 // btSoftClusterCollisionShape
 727 //
 728 class btSoftClusterCollisionShape : public btConvexInternalShape
 729 {
 730 public:
 731         const btSoftBody::Cluster* m_cluster;
 732
 733         btSoftClusterCollisionShape(const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); }
 734
 735         virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
 736         {
 737                 btSoftBody::Node* const* n = &m_cluster->m_nodes[0];
 738                 btScalar d = btDot(vec, n[0]->m_x);
 739                 int j = 0;
 740                 for (int i = 1, ni = m_cluster->m_nodes.size(); i < ni; ++i)
 741                 {
 742                         const btScalar k = btDot(vec, n[i]->m_x);
 743                         if (k > d)
 744                         {
 745                                 d = k;
 746                                 j = i;
 747                         }
 748                 }
 749                 return (n[j]->m_x);
 750         }
 751         virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const
 752         {
 753                 return (localGetSupportingVertex(vec));
 754         }
 755         //notice that the vectors should be unit length
 756         virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const
 757         {
 758         }
 759
 760         virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const
 761         {
 762         }
 763
 764         virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
 765         {
 766         }
 767
 768         virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; }
 769
 770         //debugging
 771         virtual const char* getName() const { return "SOFTCLUSTER"; }
 772
 773         virtual void setMargin(btScalar margin)
 774         {
 775                 btConvexInternalShape::setMargin(margin);
 776         }
 777         virtual btScalar getMargin() const
 778         {
 779                 return btConvexInternalShape::getMargin();
 780         }
 781 };
 782
 783 //
 784 // Inline's
 785 //
 786
 787 //
 788 template <typename T>
 789 static inline void ZeroInitialize(T& value)
 790 {
 791         memset(&value, 0, sizeof(T));
 792 }
 793 //
 794 template <typename T>
 795 static inline bool CompLess(const T& a, const T& b)
 796 {
 797         return (a < b);
 798 }
 799 //
 800 template <typename T>
 801 static inline bool CompGreater(const T& a, const T& b)
 802 {
 803         return (a > b);
 804 }
 805 //
 806 template <typename T>
 807 static inline T Lerp(const T& a, const T& b, btScalar t)
 808 {
 809         return (a + (b - a) * t);
 810 }
 811 //
 812 template <typename T>
 813 static inline T InvLerp(const T& a, const T& b, btScalar t)
 814 {
 815         return ((b + a * t - b * t) / (a * b));
 816 }
 817 //
 818 static inline btMatrix3x3 Lerp(const btMatrix3x3& a,
 819                                                            const btMatrix3x3& b,
 820                                                            btScalar t)
 821 {
 822         btMatrix3x3 r;
 823         r[0] = Lerp(a[0], b[0], t);
 824         r[1] = Lerp(a[1], b[1], t);
 825         r[2] = Lerp(a[2], b[2], t);
 826         return (r);
 827 }
 828 //
 829 static inline btVector3 Clamp(const btVector3& v, btScalar maxlength)
 830 {
 831         const btScalar sql = v.length2();
 832         if (sql > (maxlength * maxlength))
 833                 return ((v * maxlength) / btSqrt(sql));
 834         else
 835                 return (v);
 836 }
 837 //
 838 template <typename T>
 839 static inline T Clamp(const T& x, const T& l, const T& h)
 840 {
 841         return (x < l ? l : x > h ? h : x);
 842 }
 843 //
 844 template <typename T>
 845 static inline T Sq(const T& x)
 846 {
 847         return (x * x);
 848 }
 849 //
 850 template <typename T>
 851 static inline T Cube(const T& x)
 852 {
 853         return (x * x * x);
 854 }
 855 //
 856 template <typename T>
 857 static inline T Sign(const T& x)
 858 {
 859         return ((T)(x < 0 ? -1 : +1));
 860 }
 861 //
 862 template <typename T>
 863 static inline bool SameSign(const T& x, const T& y)
 864 {
 865         return ((x * y) > 0);
 866 }
 867 //
 868 static inline btScalar ClusterMetric(const btVector3& x, const btVector3& y)
 869 {
 870         const btVector3 d = x - y;
 871         return (btFabs(d[0]) + btFabs(d[1]) + btFabs(d[2]));
 872 }
 873 //
 874 static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a, btScalar s)
 875 {
 876         const btScalar xx = a.x() * a.x();
 877         const btScalar yy = a.y() * a.y();
 878         const btScalar zz = a.z() * a.z();
 879         const btScalar xy = a.x() * a.y();
 880         const btScalar yz = a.y() * a.z();
 881         const btScalar zx = a.z() * a.x();
 882         btMatrix3x3 m;
 883         m[0] = btVector3(1 - xx + xx * s, xy * s - xy, zx * s - zx);
 884         m[1] = btVector3(xy * s - xy, 1 - yy + yy * s, yz * s - yz);
 885         m[2] = btVector3(zx * s - zx, yz * s - yz, 1 - zz + zz * s);
 886         return (m);
 887 }
 888 //
 889 static inline btMatrix3x3 Cross(const btVector3& v)
 890 {
 891         btMatrix3x3 m;
 892         m[0] = btVector3(0, -v.z(), +v.y());
 893         m[1] = btVector3(+v.z(), 0, -v.x());
 894         m[2] = btVector3(-v.y(), +v.x(), 0);
 895         return (m);
 896 }
 897 //
 898 static inline btMatrix3x3 Diagonal(btScalar x)
 899 {
 900         btMatrix3x3 m;
 901         m[0] = btVector3(x, 0, 0);
 902         m[1] = btVector3(0, x, 0);
 903         m[2] = btVector3(0, 0, x);
 904         return (m);
 905 }
 906
 907 static inline btMatrix3x3 Diagonal(const btVector3& v)
 908 {
 909         btMatrix3x3 m;
 910         m[0] = btVector3(v.getX(), 0, 0);
 911         m[1] = btVector3(0, v.getY(), 0);
 912         m[2] = btVector3(0, 0, v.getZ());
 913         return (m);
 914 }
 915
 916 static inline btScalar Dot(const btScalar* a, const btScalar* b, int ndof)
 917 {
 918         btScalar result = 0;
 919         for (int i = 0; i < ndof; ++i)
 920                 result += a[i] * b[i];
 921         return result;
 922 }
 923
 924 static inline btMatrix3x3 OuterProduct(const btScalar* v1, const btScalar* v2, const btScalar* v3,
 925                                                                            const btScalar* u1, const btScalar* u2, const btScalar* u3, int ndof)
 926 {
 927         btMatrix3x3 m;
 928         btScalar a11 = Dot(v1, u1, ndof);
 929         btScalar a12 = Dot(v1, u2, ndof);
 930         btScalar a13 = Dot(v1, u3, ndof);
 931
 932         btScalar a21 = Dot(v2, u1, ndof);
 933         btScalar a22 = Dot(v2, u2, ndof);
 934         btScalar a23 = Dot(v2, u3, ndof);
 935
 936         btScalar a31 = Dot(v3, u1, ndof);
 937         btScalar a32 = Dot(v3, u2, ndof);
 938         btScalar a33 = Dot(v3, u3, ndof);
 939         m[0] = btVector3(a11, a12, a13);
 940         m[1] = btVector3(a21, a22, a23);
 941         m[2] = btVector3(a31, a32, a33);
 942         return (m);
 943 }
 944
 945 static inline btMatrix3x3 OuterProduct(const btVector3& v1, const btVector3& v2)
 946 {
 947         btMatrix3x3 m;
 948         btScalar a11 = v1[0] * v2[0];
 949         btScalar a12 = v1[0] * v2[1];
 950         btScalar a13 = v1[0] * v2[2];
 951
 952         btScalar a21 = v1[1] * v2[0];
 953         btScalar a22 = v1[1] * v2[1];
 954         btScalar a23 = v1[1] * v2[2];
 955
 956         btScalar a31 = v1[2] * v2[0];
 957         btScalar a32 = v1[2] * v2[1];
 958         btScalar a33 = v1[2] * v2[2];
 959         m[0] = btVector3(a11, a12, a13);
 960         m[1] = btVector3(a21, a22, a23);
 961         m[2] = btVector3(a31, a32, a33);
 962         return (m);
 963 }
 964
 965 //
 966 static inline btMatrix3x3 Add(const btMatrix3x3& a,
 967                                                           const btMatrix3x3& b)
 968 {
 969         btMatrix3x3 r;
 970         for (int i = 0; i < 3; ++i) r[i] = a[i] + b[i];
 971         return (r);
 972 }
 973 //
 974 static inline btMatrix3x3 Sub(const btMatrix3x3& a,
 975                                                           const btMatrix3x3& b)
 976 {
 977         btMatrix3x3 r;
 978         for (int i = 0; i < 3; ++i) r[i] = a[i] - b[i];
 979         return (r);
 980 }
 981 //
 982 static inline btMatrix3x3 Mul(const btMatrix3x3& a,
 983                                                           btScalar b)
 984 {
 985         btMatrix3x3 r;
 986         for (int i = 0; i < 3; ++i) r[i] = a[i] * b;
 987         return (r);
 988 }
 989 //
 990 static inline void Orthogonalize(btMatrix3x3& m)
 991 {
 992         m[2] = btCross(m[0], m[1]).normalized();
 993         m[1] = btCross(m[2], m[0]).normalized();
 994         m[0] = btCross(m[1], m[2]).normalized();
 995 }
 996 //
 997 static inline btMatrix3x3 MassMatrix(btScalar im, const btMatrix3x3& iwi, const btVector3& r)
 998 {
 999         const btMatrix3x3 cr = Cross(r);
1000         return (Sub(Diagonal(im), cr * iwi * cr));
1001 }
1002
1003 //
1004 static inline btMatrix3x3 ImpulseMatrix(btScalar dt,
1005                                                                                 btScalar ima,
1006                                                                                 btScalar imb,
1007                                                                                 const btMatrix3x3& iwi,
1008                                                                                 const btVector3& r)
1009 {
1010         return (Diagonal(1 / dt) * Add(Diagonal(ima), MassMatrix(imb, iwi, r)).inverse());
1011 }
1012
1013 //
1014 static inline btMatrix3x3 ImpulseMatrix(btScalar dt,
1015                                                                                 const btMatrix3x3& effective_mass_inv,
1016                                                                                 btScalar imb,
1017                                                                                 const btMatrix3x3& iwi,
1018                                                                                 const btVector3& r)
1019 {
1020         return (Diagonal(1 / dt) * Add(effective_mass_inv, MassMatrix(imb, iwi, r)).inverse());
1021         //    btMatrix3x3 iimb = MassMatrix(imb, iwi, r);
1022         //    if (iimb.determinant() == 0)
1023         //        return effective_mass_inv.inverse();
1024         //    return effective_mass_inv.inverse() *  Add(effective_mass_inv.inverse(), iimb.inverse()).inverse() * iimb.inverse();
1025 }
1026
1027 //
1028 static inline btMatrix3x3 ImpulseMatrix(btScalar ima, const btMatrix3x3& iia, const btVector3& ra,
1029                                                                                 btScalar imb, const btMatrix3x3& iib, const btVector3& rb)
1030 {
1031         return (Add(MassMatrix(ima, iia, ra), MassMatrix(imb, iib, rb)).inverse());
1032 }
1033
1034 //
1035 static inline btMatrix3x3 AngularImpulseMatrix(const btMatrix3x3& iia,
1036                                                                                            const btMatrix3x3& iib)
1037 {
1038         return (Add(iia, iib).inverse());
1039 }
1040
1041 //
1042 static inline btVector3 ProjectOnAxis(const btVector3& v,
1043                                                                           const btVector3& a)
1044 {
1045         return (a * btDot(v, a));
1046 }
1047 //
1048 static inline btVector3 ProjectOnPlane(const btVector3& v,
1049                                                                            const btVector3& a)
1050 {
1051         return (v - ProjectOnAxis(v, a));
1052 }
1053
1054 //
1055 static inline void ProjectOrigin(const btVector3& a,
1056                                                                  const btVector3& b,
1057                                                                  btVector3& prj,
1058                                                                  btScalar& sqd)
1059 {
1060         const btVector3 d = b - a;
1061         const btScalar m2 = d.length2();
1062         if (m2 > SIMD_EPSILON)
1063         {
1064                 const btScalar t = Clamp<btScalar>(-btDot(a, d) / m2, 0, 1);
1065                 const btVector3 p = a + d * t;
1066                 const btScalar l2 = p.length2();
1067                 if (l2 < sqd)
1068                 {
1069                         prj = p;
1070                         sqd = l2;
1071                 }
1072         }
1073 }
1074 //
1075 static inline void ProjectOrigin(const btVector3& a,
1076                                                                  const btVector3& b,
1077                                                                  const btVector3& c,
1078                                                                  btVector3& prj,
1079                                                                  btScalar& sqd)
1080 {
1081         const btVector3& q = btCross(b - a, c - a);
1082         const btScalar m2 = q.length2();
1083         if (m2 > SIMD_EPSILON)
1084         {
1085                 const btVector3 n = q / btSqrt(m2);
1086                 const btScalar k = btDot(a, n);
1087                 const btScalar k2 = k * k;
1088                 if (k2 < sqd)
1089                 {
1090                         const btVector3 p = n * k;
1091                         if ((btDot(btCross(a - p, b - p), q) > 0) &&
1092                                 (btDot(btCross(b - p, c - p), q) > 0) &&
1093                                 (btDot(btCross(c - p, a - p), q) > 0))
1094                         {
1095                                 prj = p;
1096                                 sqd = k2;
1097                         }
1098                         else
1099                         {
1100                                 ProjectOrigin(a, b, prj, sqd);
1101                                 ProjectOrigin(b, c, prj, sqd);
1102                                 ProjectOrigin(c, a, prj, sqd);
1103                         }
1104                 }
1105         }
1106 }
1107
1108 //
1109 static inline bool rayIntersectsTriangle(const btVector3& origin, const btVector3& dir, const btVector3& v0, const btVector3& v1, const btVector3& v2, btScalar& t)
1110 {
1111         btScalar a, f, u, v;
1112
1113         btVector3 e1 = v1 - v0;
1114         btVector3 e2 = v2 - v0;
1115         btVector3 h = dir.cross(e2);
1116         a = e1.dot(h);
1117
1118         if (a > -0.00001 && a < 0.00001)
1119                 return (false);
1120
1121         f = btScalar(1) / a;
1122         btVector3 s = origin - v0;
1123         u = f * s.dot(h);
1124
1125         if (u < 0.0 || u > 1.0)
1126                 return (false);
1127
1128         btVector3 q = s.cross(e1);
1129         v = f * dir.dot(q);
1130         if (v < 0.0 || u + v > 1.0)
1131                 return (false);
1132         // at this stage we can compute t to find out where
1133         // the intersection point is on the line
1134         t = f * e2.dot(q);
1135         if (t > 0)  // ray intersection
1136                 return (true);
1137         else  // this means that there is a line intersection
1138                 // but not a ray intersection
1139                 return (false);
1140 }
1141
1142 static inline bool lineIntersectsTriangle(const btVector3& rayStart, const btVector3& rayEnd, const btVector3& p1, const btVector3& p2, const btVector3& p3, btVector3& sect, btVector3& normal)
1143 {
1144         btVector3 dir = rayEnd - rayStart;
1145         btScalar dir_norm = dir.norm();
1146         if (dir_norm < SIMD_EPSILON)
1147                 return false;
1148         dir.normalize();
1149         btScalar t;
1150         bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t);
1151
1152         if (ret)
1153         {
1154                 if (t <= dir_norm)
1155                 {
1156                         sect = rayStart + dir * t;
1157                 }
1158                 else
1159                 {
1160                         ret = false;
1161                 }
1162         }
1163
1164         if (ret)
1165         {
1166                 btVector3 n = (p3 - p1).cross(p2 - p1);
1167                 n.safeNormalize();
1168                 if (n.dot(dir) < 0)
1169                         normal = n;
1170                 else
1171                         normal = -n;
1172         }
1173         return ret;
1174 }
1175
1176 //
1177 template <typename T>
1178 static inline T BaryEval(const T& a,
1179                                                  const T& b,
1180                                                  const T& c,
1181                                                  const btVector3& coord)
1182 {
1183         return (a * coord.x() + b * coord.y() + c * coord.z());
1184 }
1185 //
1186 static inline btVector3 BaryCoord(const btVector3& a,
1187                                                                   const btVector3& b,
1188                                                                   const btVector3& c,
1189                                                                   const btVector3& p)
1190 {
1191         const btScalar w[] = {btCross(a - p, b - p).length(),
1192                                                   btCross(b - p, c - p).length(),
1193                                                   btCross(c - p, a - p).length()};
1194         const btScalar isum = 1 / (w[0] + w[1] + w[2]);
1195         return (btVector3(w[1] * isum, w[2] * isum, w[0] * isum));
1196 }
1197
1198 //
1199 inline static btScalar ImplicitSolve(btSoftBody::ImplicitFn* fn,
1200                                                                          const btVector3& a,
1201                                                                          const btVector3& b,
1202                                                                          const btScalar accuracy,
1203                                                                          const int maxiterations = 256)
1204 {
1205         btScalar span[2] = {0, 1};
1206         btScalar values[2] = {fn->Eval(a), fn->Eval(b)};
1207         if (values[0] > values[1])
1208         {
1209                 btSwap(span[0], span[1]);
1210                 btSwap(values[0], values[1]);
1211         }
1212         if (values[0] > -accuracy) return (-1);
1213         if (values[1] < +accuracy) return (-1);
1214         for (int i = 0; i < maxiterations; ++i)
1215         {
1216                 const btScalar t = Lerp(span[0], span[1], values[0] / (values[0] - values[1]));
1217                 const btScalar v = fn->Eval(Lerp(a, b, t));
1218                 if ((t <= 0) || (t >= 1)) break;
1219                 if (btFabs(v) < accuracy) return (t);
1220                 if (v < 0)
1221                 {
1222                         span[0] = t;
1223                         values[0] = v;
1224                 }
1225                 else
1226                 {
1227                         span[1] = t;
1228                         values[1] = v;
1229                 }
1230         }
1231         return (-1);
1232 }
1233
1234 inline static void EvaluateMedium(const btSoftBodyWorldInfo* wfi,
1235                                                                   const btVector3& x,
1236                                                                   btSoftBody::sMedium& medium)
1237 {
1238         medium.m_velocity = btVector3(0, 0, 0);
1239         medium.m_pressure = 0;
1240         medium.m_density = wfi->air_density;
1241         if (wfi->water_density > 0)
1242         {
1243                 const btScalar depth = -(btDot(x, wfi->water_normal) + wfi->water_offset);
1244                 if (depth > 0)
1245                 {
1246                         medium.m_density = wfi->water_density;
1247                         medium.m_pressure = depth * wfi->water_density * wfi->m_gravity.length();
1248                 }
1249         }
1250 }
1251
1252 //
1253 static inline btVector3 NormalizeAny(const btVector3& v)
1254 {
1255         const btScalar l = v.length();
1256         if (l > SIMD_EPSILON)
1257                 return (v / l);
1258         else
1259                 return (btVector3(0, 0, 0));
1260 }
1261
1262 //
1263 static inline btDbvtVolume VolumeOf(const btSoftBody::Face& f,
1264                                                                         btScalar margin)
1265 {
1266         const btVector3* pts[] = {&f.m_n[0]->m_x,
1267                                                           &f.m_n[1]->m_x,
1268                                                           &f.m_n[2]->m_x};
1269         btDbvtVolume vol = btDbvtVolume::FromPoints(pts, 3);
1270         vol.Expand(btVector3(margin, margin, margin));
1271         return (vol);
1272 }
1273
1274 //
1275 static inline btVector3 CenterOf(const btSoftBody::Face& f)
1276 {
1277         return ((f.m_n[0]->m_x + f.m_n[1]->m_x + f.m_n[2]->m_x) / 3);
1278 }
1279
1280 //
1281 static inline btScalar AreaOf(const btVector3& x0,
1282                                                           const btVector3& x1,
1283                                                           const btVector3& x2)
1284 {
1285         const btVector3 a = x1 - x0;
1286         const btVector3 b = x2 - x0;
1287         const btVector3 cr = btCross(a, b);
1288         const btScalar area = cr.length();
1289         return (area);
1290 }
1291
1292 //
1293 static inline btScalar VolumeOf(const btVector3& x0,
1294                                                                 const btVector3& x1,
1295                                                                 const btVector3& x2,
1296                                                                 const btVector3& x3)
1297 {
1298         const btVector3 a = x1 - x0;
1299         const btVector3 b = x2 - x0;
1300         const btVector3 c = x3 - x0;
1301         return (btDot(a, btCross(b, c)));
1302 }
1303
1304 //
1305
1306 //
1307 static inline void ApplyClampedForce(btSoftBody::Node& n,
1308                                                                          const btVector3& f,
1309                                                                          btScalar dt)
1310 {
1311         const btScalar dtim = dt * n.m_im;
1312         if ((f * dtim).length2() > n.m_v.length2())
1313         { /* Clamp      */
1314                 n.m_f -= ProjectOnAxis(n.m_v, f.normalized()) / dtim;
1315         }
1316         else
1317         { /* Apply      */
1318                 n.m_f += f;
1319         }
1320 }
1321
1322 //
1323 static inline int MatchEdge(const btSoftBody::Node* a,
1324                                                         const btSoftBody::Node* b,
1325                                                         const btSoftBody::Node* ma,
1326                                                         const btSoftBody::Node* mb)
1327 {
1328         if ((a == ma) && (b == mb)) return (0);
1329         if ((a == mb) && (b == ma)) return (1);
1330         return (-1);
1331 }
1332
1333 //
1334 // btEigen : Extract eigen system,
1335 // straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html
1336 // outputs are NOT sorted.
1337 //
1338 struct btEigen
1339 {
1340         static int system(btMatrix3x3& a, btMatrix3x3* vectors, btVector3* values = 0)
1341         {
1342                 static const int maxiterations = 16;
1343                 static const btScalar accuracy = (btScalar)0.0001;
1344                 btMatrix3x3& v = *vectors;
1345                 int iterations = 0;
1346                 vectors->setIdentity();
1347                 do
1348                 {
1349                         int p = 0, q = 1;
1350                         if (btFabs(a[p][q]) < btFabs(a[0][2]))
1351                         {
1352                                 p = 0;
1353                                 q = 2;
1354                         }
1355                         if (btFabs(a[p][q]) < btFabs(a[1][2]))
1356                         {
1357                                 p = 1;
1358                                 q = 2;
1359                         }
1360                         if (btFabs(a[p][q]) > accuracy)
1361                         {
1362                                 const btScalar w = (a[q][q] - a[p][p]) / (2 * a[p][q]);
1363                                 const btScalar z = btFabs(w);
1364                                 const btScalar t = w / (z * (btSqrt(1 + w * w) + z));
1365                                 if (t == t) /* [WARNING] let hope that one does not get thrown aways by some compilers... */
1366                                 {
1367                                         const btScalar c = 1 / btSqrt(t * t + 1);
1368                                         const btScalar s = c * t;
1369                                         mulPQ(a, c, s, p, q);
1370                                         mulTPQ(a, c, s, p, q);
1371                                         mulPQ(v, c, s, p, q);
1372                                 }
1373                                 else
1374                                         break;
1375                         }
1376                         else
1377                                 break;
1378                 } while ((++iterations) < maxiterations);
1379                 if (values)
1380                 {
1381                         *values = btVector3(a[0][0], a[1][1], a[2][2]);
1382                 }
1383                 return (iterations);
1384         }
1385
1386 private:
1387         static inline void mulTPQ(btMatrix3x3& a, btScalar c, btScalar s, int p, int q)
1388         {
1389                 const btScalar m[2][3] = {{a[p][0], a[p][1], a[p][2]},
1390                                                                   {a[q][0], a[q][1], a[q][2]}};
1391                 int i;
1392
1393                 for (i = 0; i < 3; ++i) a[p][i] = c * m[0][i] - s * m[1][i];
1394                 for (i = 0; i < 3; ++i) a[q][i] = c * m[1][i] + s * m[0][i];
1395         }
1396         static inline void mulPQ(btMatrix3x3& a, btScalar c, btScalar s, int p, int q)
1397         {
1398                 const btScalar m[2][3] = {{a[0][p], a[1][p], a[2][p]},
1399                                                                   {a[0][q], a[1][q], a[2][q]}};
1400                 int i;
1401
1402                 for (i = 0; i < 3; ++i) a[i][p] = c * m[0][i] - s * m[1][i];
1403                 for (i = 0; i < 3; ++i) a[i][q] = c * m[1][i] + s * m[0][i];
1404         }
1405 };
1406
1407 //
1408 // Polar decomposition,
1409 // "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986.
1410 //
1411 static inline int PolarDecompose(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s)
1412 {
1413         static const btPolarDecomposition polar;
1414         return polar.decompose(m, q, s);
1415 }
1416
1417 //
1418 // btSoftColliders
1419 //
1420 struct btSoftColliders
1421 {
1422         //
1423         // ClusterBase
1424         //
1425         struct ClusterBase : btDbvt::ICollide
1426         {
1427                 btScalar erp;
1428                 btScalar idt;
1429                 btScalar m_margin;
1430                 btScalar friction;
1431                 btScalar threshold;
1432                 ClusterBase()
1433                 {
1434                         erp = (btScalar)1;
1435                         idt = 0;
1436                         m_margin = 0;
1437                         friction = 0;
1438                         threshold = (btScalar)0;
1439                 }
1440                 bool SolveContact(const btGjkEpaSolver2::sResults& res,
1441                                                   btSoftBody::Body ba, const btSoftBody::Body bb,
1442                                                   btSoftBody::CJoint& joint)
1443                 {
1444                         if (res.distance < m_margin)
1445                         {
1446                                 btVector3 norm = res.normal;
1447                                 norm.normalize();  //is it necessary?
1448
1449                                 const btVector3 ra = res.witnesses[0] - ba.xform().getOrigin();
1450                                 const btVector3 rb = res.witnesses[1] - bb.xform().getOrigin();
1451                                 const btVector3 va = ba.velocity(ra);
1452                                 const btVector3 vb = bb.velocity(rb);
1453                                 const btVector3 vrel = va - vb;
1454                                 const btScalar rvac = btDot(vrel, norm);
1455                                 btScalar depth = res.distance - m_margin;
1456
1457                                 //                              printf("depth=%f\n",depth);
1458                                 const btVector3 iv = norm * rvac;
1459                                 const btVector3 fv = vrel - iv;
1460                                 joint.m_bodies[0] = ba;
1461                                 joint.m_bodies[1] = bb;
1462                                 joint.m_refs[0] = ra * ba.xform().getBasis();
1463                                 joint.m_refs[1] = rb * bb.xform().getBasis();
1464                                 joint.m_rpos[0] = ra;
1465                                 joint.m_rpos[1] = rb;
1466                                 joint.m_cfm = 1;
1467                                 joint.m_erp = 1;
1468                                 joint.m_life = 0;
1469                                 joint.m_maxlife = 0;
1470                                 joint.m_split = 1;
1471
1472                                 joint.m_drift = depth * norm;
1473
1474                                 joint.m_normal = norm;
1475                                 //                              printf("normal=%f,%f,%f\n",res.normal.getX(),res.normal.getY(),res.normal.getZ());
1476                                 joint.m_delete = false;
1477                                 joint.m_friction = fv.length2() < (rvac * friction * rvac * friction) ? 1 : friction;
1478                                 joint.m_massmatrix = ImpulseMatrix(ba.invMass(), ba.invWorldInertia(), joint.m_rpos[0],
1479                                                                                                    bb.invMass(), bb.invWorldInertia(), joint.m_rpos[1]);
1480
1481                                 return (true);
1482                         }
1483                         return (false);
1484                 }
1485         };
1486         //
1487         // CollideCL_RS
1488         //
1489         struct CollideCL_RS : ClusterBase
1490         {
1491                 btSoftBody* psb;
1492                 const btCollisionObjectWrapper* m_colObjWrap;
1493
1494                 void Process(const btDbvtNode* leaf)
1495                 {
1496                         btSoftBody::Cluster* cluster = (btSoftBody::Cluster*)leaf->data;
1497                         btSoftClusterCollisionShape cshape(cluster);
1498
1499                         const btConvexShape* rshape = (const btConvexShape*)m_colObjWrap->getCollisionShape();
1500
1501                         ///don't collide an anchored cluster with a static/kinematic object
1502                         if (m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject() && cluster->m_containsAnchor)
1503                                 return;
1504
1505                         btGjkEpaSolver2::sResults res;
1506                         if (btGjkEpaSolver2::SignedDistance(&cshape, btTransform::getIdentity(),
1507                                                                                                 rshape, m_colObjWrap->getWorldTransform(),
1508                                                                                                 btVector3(1, 0, 0), res))
1509                         {
1510                                 btSoftBody::CJoint joint;
1511                                 if (SolveContact(res, cluster, m_colObjWrap->getCollisionObject(), joint))  //prb,joint))
1512                                 {
1513                                         btSoftBody::CJoint* pj = new (btAlignedAlloc(sizeof(btSoftBody::CJoint), 16)) btSoftBody::CJoint();
1514                                         *pj = joint;
1515                                         psb->m_joints.push_back(pj);
1516                                         if (m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject())
1517                                         {
1518                                                 pj->m_erp *= psb->m_cfg.kSKHR_CL;
1519                                                 pj->m_split *= psb->m_cfg.kSK_SPLT_CL;
1520                                         }
1521                                         else
1522                                         {
1523                                                 pj->m_erp *= psb->m_cfg.kSRHR_CL;
1524                                                 pj->m_split *= psb->m_cfg.kSR_SPLT_CL;
1525                                         }
1526                                 }
1527                         }
1528                 }
1529                 void ProcessColObj(btSoftBody* ps, const btCollisionObjectWrapper* colObWrap)
1530                 {
1531                         psb = ps;
1532                         m_colObjWrap = colObWrap;
1533                         idt = ps->m_sst.isdt;
1534                         m_margin = m_colObjWrap->getCollisionShape()->getMargin() + psb->getCollisionShape()->getMargin();
1535                         ///Bullet rigid body uses multiply instead of minimum to determine combined friction. Some customization would be useful.
1536                         friction = btMin(psb->m_cfg.kDF, m_colObjWrap->getCollisionObject()->getFriction());
1537                         btVector3 mins;
1538                         btVector3 maxs;
1539
1540                         ATTRIBUTE_ALIGNED16(btDbvtVolume)
1541                         volume;
1542                         colObWrap->getCollisionShape()->getAabb(colObWrap->getWorldTransform(), mins, maxs);
1543                         volume = btDbvtVolume::FromMM(mins, maxs);
1544                         volume.Expand(btVector3(1, 1, 1) * m_margin);
1545                         ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root, volume, *this);
1546                 }
1547         };
1548         //
1549         // CollideCL_SS
1550         //
1551         struct CollideCL_SS : ClusterBase
1552         {
1553                 btSoftBody* bodies[2];
1554                 void Process(const btDbvtNode* la, const btDbvtNode* lb)
1555                 {
1556                         btSoftBody::Cluster* cla = (btSoftBody::Cluster*)la->data;
1557                         btSoftBody::Cluster* clb = (btSoftBody::Cluster*)lb->data;
1558
1559                         bool connected = false;
1560                         if ((bodies[0] == bodies[1]) && (bodies[0]->m_clusterConnectivity.size()))
1561                         {
1562                                 connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex + bodies[0]->m_clusters.size() * clb->m_clusterIndex];
1563                         }
1564
1565                         if (!connected)
1566                         {
1567                                 btSoftClusterCollisionShape csa(cla);
1568                                 btSoftClusterCollisionShape csb(clb);
1569                                 btGjkEpaSolver2::sResults res;
1570                                 if (btGjkEpaSolver2::SignedDistance(&csa, btTransform::getIdentity(),
1571                                                                                                         &csb, btTransform::getIdentity(),
1572                                                                                                         cla->m_com - clb->m_com, res))
1573                                 {
1574                                         btSoftBody::CJoint joint;
1575                                         if (SolveContact(res, cla, clb, joint))
1576                                         {
1577                                                 btSoftBody::CJoint* pj = new (btAlignedAlloc(sizeof(btSoftBody::CJoint), 16)) btSoftBody::CJoint();
1578                                                 *pj = joint;
1579                                                 bodies[0]->m_joints.push_back(pj);
1580                                                 pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL, bodies[1]->m_cfg.kSSHR_CL);
1581                                                 pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL + bodies[1]->m_cfg.kSS_SPLT_CL) / 2;
1582                                         }
1583                                 }
1584                         }
1585                         else
1586                         {
1587                                 static int count = 0;
1588                                 count++;
1589                                 //printf("count=%d\n",count);
1590                         }
1591                 }
1592                 void ProcessSoftSoft(btSoftBody* psa, btSoftBody* psb)
1593                 {
1594                         idt = psa->m_sst.isdt;
1595                         //m_margin              =       (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
1596                         m_margin = (psa->getCollisionShape()->getMargin() + psb->getCollisionShape()->getMargin());
1597                         friction = btMin(psa->m_cfg.kDF, psb->m_cfg.kDF);
1598                         bodies[0] = psa;
1599                         bodies[1] = psb;
1600                         psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root, psb->m_cdbvt.m_root, *this);
1601                 }
1602         };
1603         //
1604         // CollideSDF_RS
1605         //
1606         struct CollideSDF_RS : btDbvt::ICollide
1607         {
1608                 void Process(const btDbvtNode* leaf)
1609                 {
1610                         btSoftBody::Node* node = (btSoftBody::Node*)leaf->data;
1611                         DoNode(*node);
1612                 }
1613                 void DoNode(btSoftBody::Node& n) const
1614                 {
1615                         const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
1616                         btSoftBody::RContact c;
1617
1618                         if ((!n.m_battach) &&
1619                                 psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti))
1620                         {
1621                                 const btScalar ima = n.m_im;
1622                                 const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
1623                                 const btScalar ms = ima + imb;
1624                                 if (ms > 0)
1625                                 {
1626                                         const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
1627                                         static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
1628                                         const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
1629                                         const btVector3 ra = n.m_x - wtr.getOrigin();
1630                                         const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0);
1631                                         const btVector3 vb = n.m_x - n.m_q;
1632                                         const btVector3 vr = vb - va;
1633                                         const btScalar dn = btDot(vr, c.m_cti.m_normal);
1634                                         const btVector3 fv = vr - c.m_cti.m_normal * dn;
1635                                         const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
1636                                         c.m_node = &n;
1637                                         c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);
1638                                         c.m_c1 = ra;
1639                                         c.m_c2 = ima * psb->m_sst.sdt;
1640                                         c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc;
1641                                         c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
1642                                         psb->m_rcontacts.push_back(c);
1643                                         if (m_rigidBody)
1644                                                 m_rigidBody->activate();
1645                                 }
1646                         }
1647                 }
1648                 btSoftBody* psb;
1649                 const btCollisionObjectWrapper* m_colObj1Wrap;
1650                 btRigidBody* m_rigidBody;
1651                 btScalar dynmargin;
1652                 btScalar stamargin;
1653         };
1654
1655         //
1656         // CollideSDF_RD
1657         //
1658         struct CollideSDF_RD : btDbvt::ICollide
1659         {
1660                 void Process(const btDbvtNode* leaf)
1661                 {
1662                         btSoftBody::Node* node = (btSoftBody::Node*)leaf->data;
1663                         DoNode(*node);
1664                 }
1665                 void DoNode(btSoftBody::Node& n) const
1666                 {
1667                         const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
1668                         btSoftBody::DeformableNodeRigidContact c;
1669
1670                         if (!n.m_battach)
1671                         {
1672                                 // check for collision at x_{n+1}^*
1673                                 if (psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true))
1674                                 {
1675                                         const btScalar ima = n.m_im;
1676                                         // todo: collision between multibody and fixed deformable node will be missed.
1677                                         const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
1678                                         const btScalar ms = ima + imb;
1679                                         if (ms > 0)
1680                                         {
1681                                                 // resolve contact at x_n
1682                                                 psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ false);
1683                                                 btSoftBody::sCti& cti = c.m_cti;
1684                                                 c.m_node = &n;
1685                                                 const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
1686                                                 c.m_c2 = ima;
1687                                                 c.m_c3 = fc;
1688                                                 c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
1689                                                 c.m_c5 = n.m_effectiveMass_inv;
1690
1691                                                 if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
1692                                                 {
1693                                                         const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
1694                                                         const btVector3 ra = n.m_x - wtr.getOrigin();
1695
1696                                                         static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
1697                                                         const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
1698                                                         if (psb->m_reducedModel)
1699                                                         {
1700                                                                 c.m_c0 = MassMatrix(imb, iwi, ra); //impulse factor K of the rigid body only (not the inverse)
1701                                                         }
1702                                                         else
1703                                                         {
1704                                                                 c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra);
1705                                                                 //                            c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
1706                                                         }
1707                                                         c.m_c1 = ra;
1708                                                 }
1709                                                 else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
1710                                                 {
1711                                                         btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
1712                                                         if (multibodyLinkCol)
1713                                                         {
1714                                                                 btVector3 normal = cti.m_normal;
1715                                                                 btVector3 t1 = generateUnitOrthogonalVector(normal);
1716                                                                 btVector3 t2 = btCross(normal, t1);
1717                                                                 btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
1718                                                                 findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, normal);
1719                                                                 findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_x, t1);
1720                                                                 findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_x, t2);
1721
1722                                                                 btScalar* J_n = &jacobianData_normal.m_jacobians[0];
1723                                                                 btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
1724                                                                 btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
1725
1726                                                                 btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
1727                                                                 btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
1728                                                                 btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
1729
1730                                                                 btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
1731                                                                                                 t1.getX(), t1.getY(), t1.getZ(),
1732                                                                                                 t2.getX(), t2.getY(), t2.getZ());  // world frame to local frame
1733                                                                 const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
1734
1735                                                                 btMatrix3x3 local_impulse_matrix;
1736                                                                 if (psb->m_reducedModel)
1737                                                                 {
1738                                                                         local_impulse_matrix = OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof);
1739                                                                 }
1740                                                                 else
1741                                                                 {
1742                                                                         local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
1743                                                                 }
1744                                                                 c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
1745                                                                 c.jacobianData_normal = jacobianData_normal;
1746                                                                 c.jacobianData_t1 = jacobianData_t1;
1747                                                                 c.jacobianData_t2 = jacobianData_t2;
1748                                                                 c.t1 = t1;
1749                                                                 c.t2 = t2;
1750                                                         }
1751                                                 }
1752                                                 psb->m_nodeRigidContacts.push_back(c);
1753                                         }
1754                                 }
1755                         }
1756                 }
1757                 btSoftBody* psb;
1758                 const btCollisionObjectWrapper* m_colObj1Wrap;
1759                 btRigidBody* m_rigidBody;
1760                 btScalar dynmargin;
1761                 btScalar stamargin;
1762         };
1763
1764         //
1765         // CollideSDF_RDF
1766         //
1767         struct CollideSDF_RDF : btDbvt::ICollide
1768         {
1769                 void Process(const btDbvtNode* leaf)
1770                 {
1771                         btSoftBody::Face* face = (btSoftBody::Face*)leaf->data;
1772                         DoNode(*face);
1773                 }
1774                 void DoNode(btSoftBody::Face& f) const
1775                 {
1776                         btSoftBody::Node* n0 = f.m_n[0];
1777                         btSoftBody::Node* n1 = f.m_n[1];
1778                         btSoftBody::Node* n2 = f.m_n[2];
1779                         const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0) ? dynmargin : stamargin;
1780                         btSoftBody::DeformableFaceRigidContact c;
1781                         btVector3 contact_point;
1782                         btVector3 bary;
1783                         if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true))
1784                         {
1785                                 btScalar ima = n0->m_im + n1->m_im + n2->m_im;
1786                                 const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
1787                                 // todo: collision between multibody and fixed deformable face will be missed.
1788                                 const btScalar ms = ima + imb;
1789                                 if (ms > 0)
1790                                 {
1791                                         // resolve contact at x_n
1792                                         //                    psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, /*predict = */ false);
1793                                         btSoftBody::sCti& cti = c.m_cti;
1794                                         c.m_contactPoint = contact_point;
1795                                         c.m_bary = bary;
1796                                         // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
1797                                         c.m_weights = btScalar(2) / (btScalar(1) + bary.length2()) * bary;
1798                                         c.m_face = &f;
1799                                         // friction is handled by the nodes to prevent sticking
1800                                         //                    const btScalar fc = 0;
1801                                         const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
1802
1803                                         // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
1804                                         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;
1805                                         c.m_c2 = ima;
1806                                         c.m_c3 = fc;
1807                                         c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
1808                                         c.m_c5 = Diagonal(ima);
1809                                         if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
1810                                         {
1811                                                 const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
1812                                                 static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
1813                                                 const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
1814                                                 const btVector3 ra = contact_point - wtr.getOrigin();
1815
1816                                                 // we do not scale the impulse matrix by dt
1817                                                 c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
1818                                                 c.m_c1 = ra;
1819                                         }
1820                                         else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
1821                                         {
1822                                                 btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
1823                                                 if (multibodyLinkCol)
1824                                                 {
1825                                                         btVector3 normal = cti.m_normal;
1826                                                         btVector3 t1 = generateUnitOrthogonalVector(normal);
1827                                                         btVector3 t2 = btCross(normal, t1);
1828                                                         btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
1829                                                         findJacobian(multibodyLinkCol, jacobianData_normal, contact_point, normal);
1830                                                         findJacobian(multibodyLinkCol, jacobianData_t1, contact_point, t1);
1831                                                         findJacobian(multibodyLinkCol, jacobianData_t2, contact_point, t2);
1832
1833                                                         btScalar* J_n = &jacobianData_normal.m_jacobians[0];
1834                                                         btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
1835                                                         btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
1836
1837                                                         btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
1838                                                         btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
1839                                                         btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
1840
1841                                                         btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
1842                                                                                         t1.getX(), t1.getY(), t1.getZ(),
1843                                                                                         t2.getX(), t2.getY(), t2.getZ());  // world frame to local frame
1844                                                         const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
1845                                                         btMatrix3x3 local_impulse_matrix = (Diagonal(ima) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
1846                                                         c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
1847                                                         c.jacobianData_normal = jacobianData_normal;
1848                                                         c.jacobianData_t1 = jacobianData_t1;
1849                                                         c.jacobianData_t2 = jacobianData_t2;
1850                                                         c.t1 = t1;
1851                                                         c.t2 = t2;
1852                                                 }
1853                                         }
1854                                         psb->m_faceRigidContacts.push_back(c);
1855                                 }
1856                         }
1857                         // Set caching barycenters to be false after collision detection.
1858                         // Only turn on when contact is static.
1859                         f.m_pcontact[3] = 0;
1860                 }
1861                 btSoftBody* psb;
1862                 const btCollisionObjectWrapper* m_colObj1Wrap;
1863                 btRigidBody* m_rigidBody;
1864                 btScalar dynmargin;
1865                 btScalar stamargin;
1866         };
1867
1868         //
1869         // CollideVF_SS
1870         //
1871         struct CollideVF_SS : btDbvt::ICollide
1872         {
1873                 void Process(const btDbvtNode* lnode,
1874                                          const btDbvtNode* lface)
1875                 {
1876                         btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
1877                         btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
1878                         for (int i = 0; i < 3; ++i)
1879                         {
1880                                 if (face->m_n[i] == node)
1881                                         continue;
1882                         }
1883
1884                         btVector3 o = node->m_x;
1885                         btVector3 p;
1886                         btScalar d = SIMD_INFINITY;
1887                         ProjectOrigin(face->m_n[0]->m_x - o,
1888                                                   face->m_n[1]->m_x - o,
1889                                                   face->m_n[2]->m_x - o,
1890                                                   p, d);
1891                         const btScalar m = mrg + (o - node->m_q).length() * 2;
1892                         if (d < (m * m))
1893                         {
1894                                 const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
1895                                 const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o);
1896                                 const btScalar ma = node->m_im;
1897                                 btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
1898                                 if ((n[0]->m_im <= 0) ||
1899                                         (n[1]->m_im <= 0) ||
1900                                         (n[2]->m_im <= 0))
1901                                 {
1902                                         mb = 0;
1903                                 }
1904                                 const btScalar ms = ma + mb;
1905                                 if (ms > 0)
1906                                 {
1907                                         btSoftBody::SContact c;
1908                                         c.m_normal = p / -btSqrt(d);
1909                                         c.m_margin = m;
1910                                         c.m_node = node;
1911                                         c.m_face = face;
1912                                         c.m_weights = w;
1913                                         c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF);
1914                                         c.m_cfm[0] = ma / ms * psb[0]->m_cfg.kSHR;
1915                                         c.m_cfm[1] = mb / ms * psb[1]->m_cfg.kSHR;
1916                                         psb[0]->m_scontacts.push_back(c);
1917                                 }
1918                         }
1919                 }
1920                 btSoftBody* psb[2];
1921                 btScalar mrg;
1922         };
1923
1924         //
1925         // CollideVF_DD
1926         //
1927         struct CollideVF_DD : btDbvt::ICollide
1928         {
1929                 void Process(const btDbvtNode* lnode,
1930                                          const btDbvtNode* lface)
1931                 {
1932                         btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
1933                         btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
1934                         btVector3 bary;
1935                         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))
1936                         {
1937                                 const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
1938                                 const btVector3 w = bary;
1939                                 const btScalar ma = node->m_im;
1940                                 btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
1941                                 if ((n[0]->m_im <= 0) ||
1942                                         (n[1]->m_im <= 0) ||
1943                                         (n[2]->m_im <= 0))
1944                                 {
1945                                         mb = 0;
1946                                 }
1947                                 const btScalar ms = ma + mb;
1948                                 if (ms > 0)
1949                                 {
1950                                         btSoftBody::DeformableFaceNodeContact c;
1951                                         c.m_normal = face->m_normal;
1952                                         if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
1953                                                 c.m_normal = -face->m_normal;
1954                                         c.m_margin = mrg;
1955                                         c.m_node = node;
1956                                         c.m_face = face;
1957                                         c.m_bary = w;
1958                                         c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
1959                                         // Initialize unused fields.
1960                                         c.m_weights = btVector3(0, 0, 0);
1961                                         c.m_imf = 0;
1962                                         c.m_c0 = 0;
1963                                         c.m_colObj = psb[1];
1964                                         psb[0]->m_faceNodeContacts.push_back(c);
1965                                 }
1966                         }
1967                 }
1968                 btSoftBody* psb[2];
1969                 btScalar mrg;
1970                 bool useFaceNormal;
1971         };
1972
1973         //
1974         // CollideFF_DD
1975         //
1976         struct CollideFF_DD : btDbvt::ICollide
1977         {
1978                 void Process(const btDbvntNode* lface1,
1979                                          const btDbvntNode* lface2)
1980                 {
1981                         btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data;
1982                         btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data;
1983                         if (f1 != f2)
1984                         {
1985                                 Repel(f1, f2);
1986                                 Repel(f2, f1);
1987                         }
1988                 }
1989                 void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2)
1990                 {
1991                         //#define REPEL_NEIGHBOR 1
1992 #ifndef REPEL_NEIGHBOR
1993                         for (int node_id = 0; node_id < 3; ++node_id)
1994                         {
1995                                 btSoftBody::Node* node = f1->m_n[node_id];
1996                                 for (int i = 0; i < 3; ++i)
1997                                 {
1998                                         if (f2->m_n[i] == node)
1999                                                 return;
2000                                 }
2001                         }
2002 #endif
2003                         bool skip = false;
2004                         for (int node_id = 0; node_id < 3; ++node_id)
2005                         {
2006                                 btSoftBody::Node* node = f1->m_n[node_id];
2007 #ifdef REPEL_NEIGHBOR
2008                                 for (int i = 0; i < 3; ++i)
2009                                 {
2010                                         if (f2->m_n[i] == node)
2011                                         {
2012                                                 skip = true;
2013                                                 break;
2014                                         }
2015                                 }
2016                                 if (skip)
2017                                 {
2018                                         skip = false;
2019                                         continue;
2020                                 }
2021 #endif
2022                                 btSoftBody::Face* face = f2;
2023                                 btVector3 bary;
2024                                 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))
2025                                         continue;
2026                                 btSoftBody::DeformableFaceNodeContact c;
2027                                 c.m_normal = face->m_normal;
2028                                 if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
2029                                         c.m_normal = -face->m_normal;
2030                                 c.m_margin = mrg;
2031                                 c.m_node = node;
2032                                 c.m_face = face;
2033                                 c.m_bary = bary;
2034                                 c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
2035                                 // Initialize unused fields.
2036                                 c.m_weights = btVector3(0, 0, 0);
2037                                 c.m_imf = 0;
2038                                 c.m_c0 = 0;
2039                                 c.m_colObj = psb[1];
2040                                 psb[0]->m_faceNodeContacts.push_back(c);
2041                         }
2042                 }
2043                 btSoftBody* psb[2];
2044                 btScalar mrg;
2045                 bool useFaceNormal;
2046         };
2047
2048         struct CollideCCD : btDbvt::ICollide
2049         {
2050                 void Process(const btDbvtNode* lnode,
2051                                          const btDbvtNode* lface)
2052                 {
2053                         btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
2054                         btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
2055                         btVector3 bary;
2056                         if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary))
2057                         {
2058                                 btSoftBody::DeformableFaceNodeContact c;
2059                                 c.m_normal = face->m_normal;
2060                                 if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
2061                                         c.m_normal = -face->m_normal;
2062                                 c.m_node = node;
2063                                 c.m_face = face;
2064                                 c.m_bary = bary;
2065                                 c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
2066                                 // Initialize unused fields.
2067                                 c.m_weights = btVector3(0, 0, 0);
2068                                 c.m_margin = mrg;
2069                                 c.m_imf = 0;
2070                                 c.m_c0 = 0;
2071                                 c.m_colObj = psb[1];
2072                                 psb[0]->m_faceNodeContactsCCD.push_back(c);
2073                         }
2074                 }
2075                 void Process(const btDbvntNode* lface1,
2076                                          const btDbvntNode* lface2)
2077                 {
2078                         btSoftBody::Face* f1 = (btSoftBody::Face*)lface1->data;
2079                         btSoftBody::Face* f2 = (btSoftBody::Face*)lface2->data;
2080                         if (f1 != f2)
2081                         {
2082                                 Repel(f1, f2);
2083                                 Repel(f2, f1);
2084                         }
2085                 }
2086                 void Repel(btSoftBody::Face* f1, btSoftBody::Face* f2)
2087                 {
2088                         //#define REPEL_NEIGHBOR 1
2089 #ifndef REPEL_NEIGHBOR
2090                         for (int node_id = 0; node_id < 3; ++node_id)
2091                         {
2092                                 btSoftBody::Node* node = f1->m_n[node_id];
2093                                 for (int i = 0; i < 3; ++i)
2094                                 {
2095                                         if (f2->m_n[i] == node)
2096                                                 return;
2097                                 }
2098                         }
2099 #endif
2100                         bool skip = false;
2101                         for (int node_id = 0; node_id < 3; ++node_id)
2102                         {
2103                                 btSoftBody::Node* node = f1->m_n[node_id];
2104 #ifdef REPEL_NEIGHBOR
2105                                 for (int i = 0; i < 3; ++i)
2106                                 {
2107                                         if (f2->m_n[i] == node)
2108                                         {
2109                                                 skip = true;
2110                                                 break;
2111                                         }
2112                                 }
2113                                 if (skip)
2114                                 {
2115                                         skip = false;
2116                                         continue;
2117                                 }
2118 #endif
2119                                 btSoftBody::Face* face = f2;
2120                                 btVector3 bary;
2121                                 if (bernsteinCCD(face, node, dt, SAFE_EPSILON, bary))
2122                                 {
2123                                         btSoftBody::DeformableFaceNodeContact c;
2124                                         c.m_normal = face->m_normal;
2125                                         if (!useFaceNormal && c.m_normal.dot(node->m_x - face->m_n[2]->m_x) < 0)
2126                                                 c.m_normal = -face->m_normal;
2127                                         c.m_node = node;
2128                                         c.m_face = face;
2129                                         c.m_bary = bary;
2130                                         c.m_friction = psb[0]->m_cfg.kDF * psb[1]->m_cfg.kDF;
2131                                         // Initialize unused fields.
2132                                         c.m_weights = btVector3(0, 0, 0);
2133                                         c.m_margin = mrg;
2134                                         c.m_imf = 0;
2135                                         c.m_c0 = 0;
2136                                         c.m_colObj = psb[1];
2137                                         psb[0]->m_faceNodeContactsCCD.push_back(c);
2138                                 }
2139                         }
2140                 }
2141                 btSoftBody* psb[2];
2142                 btScalar dt, mrg;
2143                 bool useFaceNormal;
2144         };
2145 };
2146 #endif  //_BT_SOFT_BODY_INTERNALS_H