diff options
Diffstat (limited to 'thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h')
| -rw-r--r-- | thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h | 1086 |
1 files changed, 567 insertions, 519 deletions
diff --git a/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h b/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h index 1ad82616ea..7efe514f38 100644 --- a/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h +++ b/thirdparty/bullet/BulletSoftBody/btSoftBodyInternals.h @@ -19,29 +19,37 @@ subject to the following restrictions: #include "btSoftBody.h" - #include "LinearMath/btQuickprof.h" #include "LinearMath/btPolarDecomposition.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/CollisionShapes/btConvexInternalShape.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" -#include <string.h> //for memset +#include <string.h> //for memset // // btSymMatrix // template <typename T> struct btSymMatrix { - btSymMatrix() : dim(0) {} - btSymMatrix(int n,const T& init=T()) { resize(n,init); } - void resize(int n,const T& init=T()) { dim=n;store.resize((n*(n+1))/2,init); } - int index(int c,int r) const { if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); } - T& operator()(int c,int r) { return(store[index(c,r)]); } - const T& operator()(int c,int r) const { return(store[index(c,r)]); } - btAlignedObjectArray<T> store; - int dim; -}; + btSymMatrix() : dim(0) {} + btSymMatrix(int n, const T& init = T()) { resize(n, init); } + void resize(int n, const T& init = T()) + { + dim = n; + store.resize((n * (n + 1)) / 2, init); + } + int index(int c, int r) const + { + if (c > r) btSwap(c, r); + btAssert(r < dim); + return ((r * (r + 1)) / 2 + c); + } + T& operator()(int c, int r) { return (store[index(c, r)]); } + const T& operator()(int c, int r) const { return (store[index(c, r)]); } + btAlignedObjectArray<T> store; + int dim; +}; // // btSoftBodyCollisionShape @@ -49,67 +57,64 @@ struct btSymMatrix class btSoftBodyCollisionShape : public btConcaveShape { public: - btSoftBody* m_body; + btSoftBody* m_body; btSoftBodyCollisionShape(btSoftBody* backptr) { m_shapeType = SOFTBODY_SHAPE_PROXYTYPE; - m_body=backptr; + m_body = backptr; } virtual ~btSoftBodyCollisionShape() { - } - void processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const + void processAllTriangles(btTriangleCallback* /*callback*/, const btVector3& /*aabbMin*/, const btVector3& /*aabbMax*/) const { //not yet btAssert(0); } ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t. - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { /* t is usually identity, except when colliding against btCompoundShape. See Issue 512 */ - const btVector3 mins=m_body->m_bounds[0]; - const btVector3 maxs=m_body->m_bounds[1]; - const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()), - t*btVector3(maxs.x(),mins.y(),mins.z()), - t*btVector3(maxs.x(),maxs.y(),mins.z()), - t*btVector3(mins.x(),maxs.y(),mins.z()), - t*btVector3(mins.x(),mins.y(),maxs.z()), - t*btVector3(maxs.x(),mins.y(),maxs.z()), - t*btVector3(maxs.x(),maxs.y(),maxs.z()), - t*btVector3(mins.x(),maxs.y(),maxs.z())}; - aabbMin=aabbMax=crns[0]; - for(int i=1;i<8;++i) + const btVector3 mins = m_body->m_bounds[0]; + const btVector3 maxs = m_body->m_bounds[1]; + const btVector3 crns[] = {t * btVector3(mins.x(), mins.y(), mins.z()), + t * btVector3(maxs.x(), mins.y(), mins.z()), + t * btVector3(maxs.x(), maxs.y(), mins.z()), + t * btVector3(mins.x(), maxs.y(), mins.z()), + t * btVector3(mins.x(), mins.y(), maxs.z()), + t * btVector3(maxs.x(), mins.y(), maxs.z()), + t * btVector3(maxs.x(), maxs.y(), maxs.z()), + t * btVector3(mins.x(), maxs.y(), maxs.z())}; + aabbMin = aabbMax = crns[0]; + for (int i = 1; i < 8; ++i) { aabbMin.setMin(crns[i]); aabbMax.setMax(crns[i]); } } - - virtual void setLocalScaling(const btVector3& /*scaling*/) - { + virtual void setLocalScaling(const btVector3& /*scaling*/) + { ///na } virtual const btVector3& getLocalScaling() const { - static const btVector3 dummy(1,1,1); + static const btVector3 dummy(1, 1, 1); return dummy; } - virtual void calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const + virtual void calculateLocalInertia(btScalar /*mass*/, btVector3& /*inertia*/) const { ///not yet btAssert(0); } - virtual const char* getName()const + virtual const char* getName() const { return "SoftBody"; } - }; // @@ -118,48 +123,53 @@ public: class btSoftClusterCollisionShape : public btConvexInternalShape { public: - const btSoftBody::Cluster* m_cluster; + const btSoftBody::Cluster* m_cluster; - btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); } + btSoftClusterCollisionShape(const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); } - - virtual btVector3 localGetSupportingVertex(const btVector3& vec) const + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const { - btSoftBody::Node* const * n=&m_cluster->m_nodes[0]; - btScalar d=btDot(vec,n[0]->m_x); - int j=0; - for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i) + btSoftBody::Node* const* n = &m_cluster->m_nodes[0]; + btScalar d = btDot(vec, n[0]->m_x); + int j = 0; + for (int i = 1, ni = m_cluster->m_nodes.size(); i < ni; ++i) { - const btScalar k=btDot(vec,n[i]->m_x); - if(k>d) { d=k;j=i; } + const btScalar k = btDot(vec, n[i]->m_x); + if (k > d) + { + d = k; + j = i; + } } - return(n[j]->m_x); + return (n[j]->m_x); } - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const { - return(localGetSupportingVertex(vec)); + return (localGetSupportingVertex(vec)); } //notice that the vectors should be unit length - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const - {} - + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors, btVector3* supportVerticesOut, int numVectors) const + { + } - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const - {} + virtual void calculateLocalInertia(btScalar mass, btVector3& inertia) const + { + } - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const - {} + virtual void getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const + { + } - virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; } + virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; } //debugging - virtual const char* getName()const {return "SOFTCLUSTER";} + virtual const char* getName() const { return "SOFTCLUSTER"; } - virtual void setMargin(btScalar margin) + virtual void setMargin(btScalar margin) { btConvexInternalShape::setMargin(margin); } - virtual btScalar getMargin() const + virtual btScalar getMargin() const { return btConvexInternalShape::getMargin(); } @@ -171,229 +181,247 @@ public: // template <typename T> -static inline void ZeroInitialize(T& value) +static inline void ZeroInitialize(T& value) { - memset(&value,0,sizeof(T)); + memset(&value, 0, sizeof(T)); } // template <typename T> -static inline bool CompLess(const T& a,const T& b) -{ return(a<b); } +static inline bool CompLess(const T& a, const T& b) +{ + return (a < b); +} // template <typename T> -static inline bool CompGreater(const T& a,const T& b) -{ return(a>b); } +static inline bool CompGreater(const T& a, const T& b) +{ + return (a > b); +} // template <typename T> -static inline T Lerp(const T& a,const T& b,btScalar t) -{ return(a+(b-a)*t); } +static inline T Lerp(const T& a, const T& b, btScalar t) +{ + return (a + (b - a) * t); +} // template <typename T> -static inline T InvLerp(const T& a,const T& b,btScalar t) -{ return((b+a*t-b*t)/(a*b)); } +static inline T InvLerp(const T& a, const T& b, btScalar t) +{ + return ((b + a * t - b * t) / (a * b)); +} // -static inline btMatrix3x3 Lerp( const btMatrix3x3& a, - const btMatrix3x3& b, - btScalar t) +static inline btMatrix3x3 Lerp(const btMatrix3x3& a, + const btMatrix3x3& b, + btScalar t) { - btMatrix3x3 r; - r[0]=Lerp(a[0],b[0],t); - r[1]=Lerp(a[1],b[1],t); - r[2]=Lerp(a[2],b[2],t); - return(r); + btMatrix3x3 r; + r[0] = Lerp(a[0], b[0], t); + r[1] = Lerp(a[1], b[1], t); + r[2] = Lerp(a[2], b[2], t); + return (r); } // -static inline btVector3 Clamp(const btVector3& v,btScalar maxlength) +static inline btVector3 Clamp(const btVector3& v, btScalar maxlength) { - const btScalar sql=v.length2(); - if(sql>(maxlength*maxlength)) - return((v*maxlength)/btSqrt(sql)); + const btScalar sql = v.length2(); + if (sql > (maxlength * maxlength)) + return ((v * maxlength) / btSqrt(sql)); else - return(v); + return (v); } // template <typename T> -static inline T Clamp(const T& x,const T& l,const T& h) -{ return(x<l?l:x>h?h:x); } +static inline T Clamp(const T& x, const T& l, const T& h) +{ + return (x < l ? l : x > h ? h : x); +} // template <typename T> -static inline T Sq(const T& x) -{ return(x*x); } +static inline T Sq(const T& x) +{ + return (x * x); +} // template <typename T> -static inline T Cube(const T& x) -{ return(x*x*x); } +static inline T Cube(const T& x) +{ + return (x * x * x); +} // template <typename T> -static inline T Sign(const T& x) -{ return((T)(x<0?-1:+1)); } +static inline T Sign(const T& x) +{ + return ((T)(x < 0 ? -1 : +1)); +} // template <typename T> -static inline bool SameSign(const T& x,const T& y) -{ return((x*y)>0); } -// -static inline btScalar ClusterMetric(const btVector3& x,const btVector3& y) +static inline bool SameSign(const T& x, const T& y) { - const btVector3 d=x-y; - return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2])); + return ((x * y) > 0); } // -static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s) +static inline btScalar ClusterMetric(const btVector3& x, const btVector3& y) { - const btScalar xx=a.x()*a.x(); - const btScalar yy=a.y()*a.y(); - const btScalar zz=a.z()*a.z(); - const btScalar xy=a.x()*a.y(); - const btScalar yz=a.y()*a.z(); - const btScalar zx=a.z()*a.x(); - btMatrix3x3 m; - m[0]=btVector3(1-xx+xx*s,xy*s-xy,zx*s-zx); - m[1]=btVector3(xy*s-xy,1-yy+yy*s,yz*s-yz); - m[2]=btVector3(zx*s-zx,yz*s-yz,1-zz+zz*s); - return(m); + const btVector3 d = x - y; + return (btFabs(d[0]) + btFabs(d[1]) + btFabs(d[2])); +} +// +static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a, btScalar s) +{ + const btScalar xx = a.x() * a.x(); + const btScalar yy = a.y() * a.y(); + const btScalar zz = a.z() * a.z(); + const btScalar xy = a.x() * a.y(); + const btScalar yz = a.y() * a.z(); + const btScalar zx = a.z() * a.x(); + btMatrix3x3 m; + m[0] = btVector3(1 - xx + xx * s, xy * s - xy, zx * s - zx); + m[1] = btVector3(xy * s - xy, 1 - yy + yy * s, yz * s - yz); + m[2] = btVector3(zx * s - zx, yz * s - yz, 1 - zz + zz * s); + return (m); } // -static inline btMatrix3x3 Cross(const btVector3& v) +static inline btMatrix3x3 Cross(const btVector3& v) { - btMatrix3x3 m; - m[0]=btVector3(0,-v.z(),+v.y()); - m[1]=btVector3(+v.z(),0,-v.x()); - m[2]=btVector3(-v.y(),+v.x(),0); - return(m); + btMatrix3x3 m; + m[0] = btVector3(0, -v.z(), +v.y()); + m[1] = btVector3(+v.z(), 0, -v.x()); + m[2] = btVector3(-v.y(), +v.x(), 0); + return (m); } // -static inline btMatrix3x3 Diagonal(btScalar x) +static inline btMatrix3x3 Diagonal(btScalar x) { - btMatrix3x3 m; - m[0]=btVector3(x,0,0); - m[1]=btVector3(0,x,0); - m[2]=btVector3(0,0,x); - return(m); + btMatrix3x3 m; + m[0] = btVector3(x, 0, 0); + m[1] = btVector3(0, x, 0); + m[2] = btVector3(0, 0, x); + return (m); } // -static inline btMatrix3x3 Add(const btMatrix3x3& a, - const btMatrix3x3& b) +static inline btMatrix3x3 Add(const btMatrix3x3& a, + const btMatrix3x3& b) { - btMatrix3x3 r; - for(int i=0;i<3;++i) r[i]=a[i]+b[i]; - return(r); + btMatrix3x3 r; + for (int i = 0; i < 3; ++i) r[i] = a[i] + b[i]; + return (r); } // -static inline btMatrix3x3 Sub(const btMatrix3x3& a, - const btMatrix3x3& b) +static inline btMatrix3x3 Sub(const btMatrix3x3& a, + const btMatrix3x3& b) { - btMatrix3x3 r; - for(int i=0;i<3;++i) r[i]=a[i]-b[i]; - return(r); + btMatrix3x3 r; + for (int i = 0; i < 3; ++i) r[i] = a[i] - b[i]; + return (r); } // -static inline btMatrix3x3 Mul(const btMatrix3x3& a, - btScalar b) +static inline btMatrix3x3 Mul(const btMatrix3x3& a, + btScalar b) { - btMatrix3x3 r; - for(int i=0;i<3;++i) r[i]=a[i]*b; - return(r); + btMatrix3x3 r; + for (int i = 0; i < 3; ++i) r[i] = a[i] * b; + return (r); } // -static inline void Orthogonalize(btMatrix3x3& m) +static inline void Orthogonalize(btMatrix3x3& m) { - m[2]=btCross(m[0],m[1]).normalized(); - m[1]=btCross(m[2],m[0]).normalized(); - m[0]=btCross(m[1],m[2]).normalized(); + m[2] = btCross(m[0], m[1]).normalized(); + m[1] = btCross(m[2], m[0]).normalized(); + m[0] = btCross(m[1], m[2]).normalized(); } // -static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r) +static inline btMatrix3x3 MassMatrix(btScalar im, const btMatrix3x3& iwi, const btVector3& r) { - const btMatrix3x3 cr=Cross(r); - return(Sub(Diagonal(im),cr*iwi*cr)); + const btMatrix3x3 cr = Cross(r); + return (Sub(Diagonal(im), cr * iwi * cr)); } // -static inline btMatrix3x3 ImpulseMatrix( btScalar dt, - btScalar ima, - btScalar imb, - const btMatrix3x3& iwi, - const btVector3& r) +static inline btMatrix3x3 ImpulseMatrix(btScalar dt, + btScalar ima, + btScalar imb, + const btMatrix3x3& iwi, + const btVector3& r) { - return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse()); + return (Diagonal(1 / dt) * Add(Diagonal(ima), MassMatrix(imb, iwi, r)).inverse()); } // -static inline btMatrix3x3 ImpulseMatrix( btScalar ima,const btMatrix3x3& iia,const btVector3& ra, - btScalar imb,const btMatrix3x3& iib,const btVector3& rb) +static inline btMatrix3x3 ImpulseMatrix(btScalar ima, const btMatrix3x3& iia, const btVector3& ra, + btScalar imb, const btMatrix3x3& iib, const btVector3& rb) { - return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse()); + return (Add(MassMatrix(ima, iia, ra), MassMatrix(imb, iib, rb)).inverse()); } // -static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia, - const btMatrix3x3& iib) +static inline btMatrix3x3 AngularImpulseMatrix(const btMatrix3x3& iia, + const btMatrix3x3& iib) { - return(Add(iia,iib).inverse()); + return (Add(iia, iib).inverse()); } // -static inline btVector3 ProjectOnAxis( const btVector3& v, - const btVector3& a) +static inline btVector3 ProjectOnAxis(const btVector3& v, + const btVector3& a) { - return(a*btDot(v,a)); + return (a * btDot(v, a)); } // -static inline btVector3 ProjectOnPlane( const btVector3& v, - const btVector3& a) +static inline btVector3 ProjectOnPlane(const btVector3& v, + const btVector3& a) { - return(v-ProjectOnAxis(v,a)); + return (v - ProjectOnAxis(v, a)); } // -static inline void ProjectOrigin( const btVector3& a, - const btVector3& b, - btVector3& prj, - btScalar& sqd) +static inline void ProjectOrigin(const btVector3& a, + const btVector3& b, + btVector3& prj, + btScalar& sqd) { - const btVector3 d=b-a; - const btScalar m2=d.length2(); - if(m2>SIMD_EPSILON) - { - const btScalar t=Clamp<btScalar>(-btDot(a,d)/m2,0,1); - const btVector3 p=a+d*t; - const btScalar l2=p.length2(); - if(l2<sqd) + const btVector3 d = b - a; + const btScalar m2 = d.length2(); + if (m2 > SIMD_EPSILON) + { + const btScalar t = Clamp<btScalar>(-btDot(a, d) / m2, 0, 1); + const btVector3 p = a + d * t; + const btScalar l2 = p.length2(); + if (l2 < sqd) { - prj=p; - sqd=l2; + prj = p; + sqd = l2; } } } // -static inline void ProjectOrigin( const btVector3& a, - const btVector3& b, - const btVector3& c, - btVector3& prj, - btScalar& sqd) +static inline void ProjectOrigin(const btVector3& a, + const btVector3& b, + const btVector3& c, + btVector3& prj, + btScalar& sqd) { - const btVector3& q=btCross(b-a,c-a); - const btScalar m2=q.length2(); - if(m2>SIMD_EPSILON) + const btVector3& q = btCross(b - a, c - a); + const btScalar m2 = q.length2(); + if (m2 > SIMD_EPSILON) { - const btVector3 n=q/btSqrt(m2); - const btScalar k=btDot(a,n); - const btScalar k2=k*k; - if(k2<sqd) + const btVector3 n = q / btSqrt(m2); + const btScalar k = btDot(a, n); + const btScalar k2 = k * k; + if (k2 < sqd) { - const btVector3 p=n*k; - if( (btDot(btCross(a-p,b-p),q)>0)&& - (btDot(btCross(b-p,c-p),q)>0)&& - (btDot(btCross(c-p,a-p),q)>0)) - { - prj=p; - sqd=k2; + const btVector3 p = n * k; + if ((btDot(btCross(a - p, b - p), q) > 0) && + (btDot(btCross(b - p, c - p), q) > 0) && + (btDot(btCross(c - p, a - p), q) > 0)) + { + prj = p; + sqd = k2; } else { - ProjectOrigin(a,b,prj,sqd); - ProjectOrigin(b,c,prj,sqd); - ProjectOrigin(c,a,prj,sqd); + ProjectOrigin(a, b, prj, sqd); + ProjectOrigin(b, c, prj, sqd); + ProjectOrigin(c, a, prj, sqd); } } } @@ -401,155 +429,159 @@ static inline void ProjectOrigin( const btVector3& a, // template <typename T> -static inline T BaryEval( const T& a, - const T& b, - const T& c, - const btVector3& coord) +static inline T BaryEval(const T& a, + const T& b, + const T& c, + const btVector3& coord) { - return(a*coord.x()+b*coord.y()+c*coord.z()); + return (a * coord.x() + b * coord.y() + c * coord.z()); } // -static inline btVector3 BaryCoord( const btVector3& a, - const btVector3& b, - const btVector3& c, - const btVector3& p) +static inline btVector3 BaryCoord(const btVector3& a, + const btVector3& b, + const btVector3& c, + const btVector3& p) { - const btScalar w[]={ btCross(a-p,b-p).length(), - btCross(b-p,c-p).length(), - btCross(c-p,a-p).length()}; - const btScalar isum=1/(w[0]+w[1]+w[2]); - return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum)); + const btScalar w[] = {btCross(a - p, b - p).length(), + btCross(b - p, c - p).length(), + btCross(c - p, a - p).length()}; + const btScalar isum = 1 / (w[0] + w[1] + w[2]); + return (btVector3(w[1] * isum, w[2] * isum, w[0] * isum)); } // -inline static btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn, - const btVector3& a, - const btVector3& b, - const btScalar accuracy, - const int maxiterations=256) +inline static btScalar ImplicitSolve(btSoftBody::ImplicitFn* fn, + const btVector3& a, + const btVector3& b, + const btScalar accuracy, + const int maxiterations = 256) { - btScalar span[2]={0,1}; - btScalar values[2]={fn->Eval(a),fn->Eval(b)}; - if(values[0]>values[1]) + btScalar span[2] = {0, 1}; + btScalar values[2] = {fn->Eval(a), fn->Eval(b)}; + if (values[0] > values[1]) { - btSwap(span[0],span[1]); - btSwap(values[0],values[1]); + btSwap(span[0], span[1]); + btSwap(values[0], values[1]); } - if(values[0]>-accuracy) return(-1); - if(values[1]<+accuracy) return(-1); - for(int i=0;i<maxiterations;++i) + if (values[0] > -accuracy) return (-1); + if (values[1] < +accuracy) return (-1); + for (int i = 0; i < maxiterations; ++i) { - const btScalar t=Lerp(span[0],span[1],values[0]/(values[0]-values[1])); - const btScalar v=fn->Eval(Lerp(a,b,t)); - if((t<=0)||(t>=1)) break; - if(btFabs(v)<accuracy) return(t); - if(v<0) - { span[0]=t;values[0]=v; } + const btScalar t = Lerp(span[0], span[1], values[0] / (values[0] - values[1])); + const btScalar v = fn->Eval(Lerp(a, b, t)); + if ((t <= 0) || (t >= 1)) break; + if (btFabs(v) < accuracy) return (t); + if (v < 0) + { + span[0] = t; + values[0] = v; + } else - { span[1]=t;values[1]=v; } + { + span[1] = t; + values[1] = v; + } } - return(-1); + return (-1); } -inline static void EvaluateMedium( const btSoftBodyWorldInfo* wfi, - const btVector3& x, - btSoftBody::sMedium& medium) +inline static void EvaluateMedium(const btSoftBodyWorldInfo* wfi, + const btVector3& x, + btSoftBody::sMedium& medium) { - medium.m_velocity = btVector3(0,0,0); - medium.m_pressure = 0; - medium.m_density = wfi->air_density; - if(wfi->water_density>0) + medium.m_velocity = btVector3(0, 0, 0); + medium.m_pressure = 0; + medium.m_density = wfi->air_density; + if (wfi->water_density > 0) { - const btScalar depth=-(btDot(x,wfi->water_normal)+wfi->water_offset); - if(depth>0) + const btScalar depth = -(btDot(x, wfi->water_normal) + wfi->water_offset); + if (depth > 0) { - medium.m_density = wfi->water_density; - medium.m_pressure = depth*wfi->water_density*wfi->m_gravity.length(); + medium.m_density = wfi->water_density; + medium.m_pressure = depth * wfi->water_density * wfi->m_gravity.length(); } } } - // -static inline btVector3 NormalizeAny(const btVector3& v) +static inline btVector3 NormalizeAny(const btVector3& v) { - const btScalar l=v.length(); - if(l>SIMD_EPSILON) - return(v/l); + const btScalar l = v.length(); + if (l > SIMD_EPSILON) + return (v / l); else - return(btVector3(0,0,0)); + return (btVector3(0, 0, 0)); } // -static inline btDbvtVolume VolumeOf( const btSoftBody::Face& f, - btScalar margin) +static inline btDbvtVolume VolumeOf(const btSoftBody::Face& f, + btScalar margin) { - const btVector3* pts[]={ &f.m_n[0]->m_x, - &f.m_n[1]->m_x, - &f.m_n[2]->m_x}; - btDbvtVolume vol=btDbvtVolume::FromPoints(pts,3); - vol.Expand(btVector3(margin,margin,margin)); - return(vol); + const btVector3* pts[] = {&f.m_n[0]->m_x, + &f.m_n[1]->m_x, + &f.m_n[2]->m_x}; + btDbvtVolume vol = btDbvtVolume::FromPoints(pts, 3); + vol.Expand(btVector3(margin, margin, margin)); + return (vol); } // -static inline btVector3 CenterOf( const btSoftBody::Face& f) +static inline btVector3 CenterOf(const btSoftBody::Face& f) { - return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3); + return ((f.m_n[0]->m_x + f.m_n[1]->m_x + f.m_n[2]->m_x) / 3); } // -static inline btScalar AreaOf( const btVector3& x0, - const btVector3& x1, - const btVector3& x2) +static inline btScalar AreaOf(const btVector3& x0, + const btVector3& x1, + const btVector3& x2) { - const btVector3 a=x1-x0; - const btVector3 b=x2-x0; - const btVector3 cr=btCross(a,b); - const btScalar area=cr.length(); - return(area); + const btVector3 a = x1 - x0; + const btVector3 b = x2 - x0; + const btVector3 cr = btCross(a, b); + const btScalar area = cr.length(); + return (area); } // -static inline btScalar VolumeOf( const btVector3& x0, - const btVector3& x1, - const btVector3& x2, - const btVector3& x3) +static inline btScalar VolumeOf(const btVector3& x0, + const btVector3& x1, + const btVector3& x2, + const btVector3& x3) { - const btVector3 a=x1-x0; - const btVector3 b=x2-x0; - const btVector3 c=x3-x0; - return(btDot(a,btCross(b,c))); + const btVector3 a = x1 - x0; + const btVector3 b = x2 - x0; + const btVector3 c = x3 - x0; + return (btDot(a, btCross(b, c))); } // - // -static inline void ApplyClampedForce( btSoftBody::Node& n, - const btVector3& f, - btScalar dt) +static inline void ApplyClampedForce(btSoftBody::Node& n, + const btVector3& f, + btScalar dt) { - const btScalar dtim=dt*n.m_im; - if((f*dtim).length2()>n.m_v.length2()) - {/* Clamp */ - n.m_f-=ProjectOnAxis(n.m_v,f.normalized())/dtim; + const btScalar dtim = dt * n.m_im; + if ((f * dtim).length2() > n.m_v.length2()) + { /* Clamp */ + n.m_f -= ProjectOnAxis(n.m_v, f.normalized()) / dtim; } else - {/* Apply */ - n.m_f+=f; + { /* Apply */ + n.m_f += f; } } // -static inline int MatchEdge( const btSoftBody::Node* a, - const btSoftBody::Node* b, - const btSoftBody::Node* ma, - const btSoftBody::Node* mb) +static inline int MatchEdge(const btSoftBody::Node* a, + const btSoftBody::Node* b, + const btSoftBody::Node* ma, + const btSoftBody::Node* mb) { - if((a==ma)&&(b==mb)) return(0); - if((a==mb)&&(b==ma)) return(1); - return(-1); + if ((a == ma) && (b == mb)) return (0); + if ((a == mb) && (b == ma)) return (1); + return (-1); } // @@ -557,58 +589,72 @@ static inline int MatchEdge( const btSoftBody::Node* a, // straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html // outputs are NOT sorted. // -struct btEigen +struct btEigen { - static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0) + static int system(btMatrix3x3& a, btMatrix3x3* vectors, btVector3* values = 0) { - static const int maxiterations=16; - static const btScalar accuracy=(btScalar)0.0001; - btMatrix3x3& v=*vectors; - int iterations=0; + static const int maxiterations = 16; + static const btScalar accuracy = (btScalar)0.0001; + btMatrix3x3& v = *vectors; + int iterations = 0; vectors->setIdentity(); - do { - int p=0,q=1; - if(btFabs(a[p][q])<btFabs(a[0][2])) { p=0;q=2; } - if(btFabs(a[p][q])<btFabs(a[1][2])) { p=1;q=2; } - if(btFabs(a[p][q])>accuracy) + do + { + int p = 0, q = 1; + if (btFabs(a[p][q]) < btFabs(a[0][2])) { - const btScalar w=(a[q][q]-a[p][p])/(2*a[p][q]); - const btScalar z=btFabs(w); - const btScalar t=w/(z*(btSqrt(1+w*w)+z)); - if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */ + p = 0; + q = 2; + } + if (btFabs(a[p][q]) < btFabs(a[1][2])) + { + p = 1; + q = 2; + } + if (btFabs(a[p][q]) > accuracy) + { + const btScalar w = (a[q][q] - a[p][p]) / (2 * a[p][q]); + const btScalar z = btFabs(w); + const btScalar t = w / (z * (btSqrt(1 + w * w) + z)); + if (t == t) /* [WARNING] let hope that one does not get thrown aways by some compilers... */ { - const btScalar c=1/btSqrt(t*t+1); - const btScalar s=c*t; - mulPQ(a,c,s,p,q); - mulTPQ(a,c,s,p,q); - mulPQ(v,c,s,p,q); - } else break; - } else break; - } while((++iterations)<maxiterations); - if(values) + const btScalar c = 1 / btSqrt(t * t + 1); + const btScalar s = c * t; + mulPQ(a, c, s, p, q); + mulTPQ(a, c, s, p, q); + mulPQ(v, c, s, p, q); + } + else + break; + } + else + break; + } while ((++iterations) < maxiterations); + if (values) { - *values=btVector3(a[0][0],a[1][1],a[2][2]); + *values = btVector3(a[0][0], a[1][1], a[2][2]); } - return(iterations); + return (iterations); } + private: - static inline void mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q) + static inline void mulTPQ(btMatrix3x3& a, btScalar c, btScalar s, int p, int q) { - const btScalar m[2][3]={ {a[p][0],a[p][1],a[p][2]}, - {a[q][0],a[q][1],a[q][2]}}; + const btScalar m[2][3] = {{a[p][0], a[p][1], a[p][2]}, + {a[q][0], a[q][1], a[q][2]}}; int i; - for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i]; - for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i]; + for (i = 0; i < 3; ++i) a[p][i] = c * m[0][i] - s * m[1][i]; + for (i = 0; i < 3; ++i) a[q][i] = c * m[1][i] + s * m[0][i]; } - static inline void mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q) + static inline void mulPQ(btMatrix3x3& a, btScalar c, btScalar s, int p, int q) { - const btScalar m[2][3]={ {a[0][p],a[1][p],a[2][p]}, - {a[0][q],a[1][q],a[2][q]}}; + const btScalar m[2][3] = {{a[0][p], a[1][p], a[2][p]}, + {a[0][q], a[1][q], a[2][q]}}; int i; - for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i]; - for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i]; + for (i = 0; i < 3; ++i) a[i][p] = c * m[0][i] - s * m[1][i]; + for (i = 0; i < 3; ++i) a[i][q] = c * m[1][i] + s * m[0][i]; } }; @@ -616,9 +662,9 @@ private: // Polar decomposition, // "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986. // -static inline int PolarDecompose( const btMatrix3x3& m,btMatrix3x3& q,btMatrix3x3& s) +static inline int PolarDecompose(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s) { - static const btPolarDecomposition polar; + static const btPolarDecomposition polar; return polar.decompose(m, q, s); } @@ -630,282 +676,284 @@ struct btSoftColliders // // ClusterBase // - struct ClusterBase : btDbvt::ICollide + struct ClusterBase : btDbvt::ICollide { - btScalar erp; - btScalar idt; - btScalar m_margin; - btScalar friction; - btScalar threshold; + btScalar erp; + btScalar idt; + btScalar m_margin; + btScalar friction; + btScalar threshold; ClusterBase() { - erp =(btScalar)1; - idt =0; - m_margin =0; - friction =0; - threshold =(btScalar)0; + erp = (btScalar)1; + idt = 0; + m_margin = 0; + friction = 0; + threshold = (btScalar)0; } - bool SolveContact( const btGjkEpaSolver2::sResults& res, - btSoftBody::Body ba,const btSoftBody::Body bb, - btSoftBody::CJoint& joint) + bool SolveContact(const btGjkEpaSolver2::sResults& res, + btSoftBody::Body ba, const btSoftBody::Body bb, + btSoftBody::CJoint& joint) { - if(res.distance<m_margin) + if (res.distance < m_margin) { btVector3 norm = res.normal; - norm.normalize();//is it necessary? - - const btVector3 ra=res.witnesses[0]-ba.xform().getOrigin(); - const btVector3 rb=res.witnesses[1]-bb.xform().getOrigin(); - const btVector3 va=ba.velocity(ra); - const btVector3 vb=bb.velocity(rb); - const btVector3 vrel=va-vb; - const btScalar rvac=btDot(vrel,norm); - btScalar depth=res.distance-m_margin; - -// printf("depth=%f\n",depth); - const btVector3 iv=norm*rvac; - const btVector3 fv=vrel-iv; - joint.m_bodies[0] = ba; - joint.m_bodies[1] = bb; - joint.m_refs[0] = ra*ba.xform().getBasis(); - joint.m_refs[1] = rb*bb.xform().getBasis(); - joint.m_rpos[0] = ra; - joint.m_rpos[1] = rb; - joint.m_cfm = 1; - joint.m_erp = 1; - joint.m_life = 0; - joint.m_maxlife = 0; - joint.m_split = 1; - - joint.m_drift = depth*norm; - - joint.m_normal = norm; -// printf("normal=%f,%f,%f\n",res.normal.getX(),res.normal.getY(),res.normal.getZ()); - joint.m_delete = false; - joint.m_friction = fv.length2()<(rvac*friction*rvac*friction)?1:friction; - joint.m_massmatrix = ImpulseMatrix( ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0], - bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]); - - return(true); + norm.normalize(); //is it necessary? + + const btVector3 ra = res.witnesses[0] - ba.xform().getOrigin(); + const btVector3 rb = res.witnesses[1] - bb.xform().getOrigin(); + const btVector3 va = ba.velocity(ra); + const btVector3 vb = bb.velocity(rb); + const btVector3 vrel = va - vb; + const btScalar rvac = btDot(vrel, norm); + btScalar depth = res.distance - m_margin; + + // printf("depth=%f\n",depth); + const btVector3 iv = norm * rvac; + const btVector3 fv = vrel - iv; + joint.m_bodies[0] = ba; + joint.m_bodies[1] = bb; + joint.m_refs[0] = ra * ba.xform().getBasis(); + joint.m_refs[1] = rb * bb.xform().getBasis(); + joint.m_rpos[0] = ra; + joint.m_rpos[1] = rb; + joint.m_cfm = 1; + joint.m_erp = 1; + joint.m_life = 0; + joint.m_maxlife = 0; + joint.m_split = 1; + + joint.m_drift = depth * norm; + + joint.m_normal = norm; + // printf("normal=%f,%f,%f\n",res.normal.getX(),res.normal.getY(),res.normal.getZ()); + joint.m_delete = false; + joint.m_friction = fv.length2() < (rvac * friction * rvac * friction) ? 1 : friction; + joint.m_massmatrix = ImpulseMatrix(ba.invMass(), ba.invWorldInertia(), joint.m_rpos[0], + bb.invMass(), bb.invWorldInertia(), joint.m_rpos[1]); + + return (true); } - return(false); + return (false); } }; // // CollideCL_RS // - struct CollideCL_RS : ClusterBase + struct CollideCL_RS : ClusterBase { - btSoftBody* psb; - const btCollisionObjectWrapper* m_colObjWrap; + btSoftBody* psb; + const btCollisionObjectWrapper* m_colObjWrap; - void Process(const btDbvtNode* leaf) + void Process(const btDbvtNode* leaf) { - btSoftBody::Cluster* cluster=(btSoftBody::Cluster*)leaf->data; - btSoftClusterCollisionShape cshape(cluster); - - const btConvexShape* rshape=(const btConvexShape*)m_colObjWrap->getCollisionShape(); + btSoftBody::Cluster* cluster = (btSoftBody::Cluster*)leaf->data; + btSoftClusterCollisionShape cshape(cluster); + + const btConvexShape* rshape = (const btConvexShape*)m_colObjWrap->getCollisionShape(); ///don't collide an anchored cluster with a static/kinematic object - if(m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject() && cluster->m_containsAnchor) + if (m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject() && cluster->m_containsAnchor) return; - btGjkEpaSolver2::sResults res; - if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(), - rshape,m_colObjWrap->getWorldTransform(), - btVector3(1,0,0),res)) + btGjkEpaSolver2::sResults res; + if (btGjkEpaSolver2::SignedDistance(&cshape, btTransform::getIdentity(), + rshape, m_colObjWrap->getWorldTransform(), + btVector3(1, 0, 0), res)) { - btSoftBody::CJoint joint; - if(SolveContact(res,cluster,m_colObjWrap->getCollisionObject(),joint))//prb,joint)) + btSoftBody::CJoint joint; + if (SolveContact(res, cluster, m_colObjWrap->getCollisionObject(), joint)) //prb,joint)) { - btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); - *pj=joint;psb->m_joints.push_back(pj); - if(m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject()) + btSoftBody::CJoint* pj = new (btAlignedAlloc(sizeof(btSoftBody::CJoint), 16)) btSoftBody::CJoint(); + *pj = joint; + psb->m_joints.push_back(pj); + if (m_colObjWrap->getCollisionObject()->isStaticOrKinematicObject()) { - pj->m_erp *= psb->m_cfg.kSKHR_CL; - pj->m_split *= psb->m_cfg.kSK_SPLT_CL; + pj->m_erp *= psb->m_cfg.kSKHR_CL; + pj->m_split *= psb->m_cfg.kSK_SPLT_CL; } else { - pj->m_erp *= psb->m_cfg.kSRHR_CL; - pj->m_split *= psb->m_cfg.kSR_SPLT_CL; + pj->m_erp *= psb->m_cfg.kSRHR_CL; + pj->m_split *= psb->m_cfg.kSR_SPLT_CL; } } } } - void ProcessColObj(btSoftBody* ps,const btCollisionObjectWrapper* colObWrap) + void ProcessColObj(btSoftBody* ps, const btCollisionObjectWrapper* colObWrap) { - psb = ps; - m_colObjWrap = colObWrap; - idt = ps->m_sst.isdt; - m_margin = m_colObjWrap->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin(); + psb = ps; + m_colObjWrap = colObWrap; + idt = ps->m_sst.isdt; + m_margin = m_colObjWrap->getCollisionShape()->getMargin() + psb->getCollisionShape()->getMargin(); ///Bullet rigid body uses multiply instead of minimum to determine combined friction. Some customization would be useful. - friction = btMin(psb->m_cfg.kDF,m_colObjWrap->getCollisionObject()->getFriction()); - btVector3 mins; - btVector3 maxs; - - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; - colObWrap->getCollisionShape()->getAabb(colObWrap->getWorldTransform(),mins,maxs); - volume=btDbvtVolume::FromMM(mins,maxs); - volume.Expand(btVector3(1,1,1)*m_margin); - ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root,volume,*this); - } + friction = btMin(psb->m_cfg.kDF, m_colObjWrap->getCollisionObject()->getFriction()); + btVector3 mins; + btVector3 maxs; + + ATTRIBUTE_ALIGNED16(btDbvtVolume) + volume; + colObWrap->getCollisionShape()->getAabb(colObWrap->getWorldTransform(), mins, maxs); + volume = btDbvtVolume::FromMM(mins, maxs); + volume.Expand(btVector3(1, 1, 1) * m_margin); + ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root, volume, *this); + } }; // // CollideCL_SS // - struct CollideCL_SS : ClusterBase + struct CollideCL_SS : ClusterBase { - btSoftBody* bodies[2]; - void Process(const btDbvtNode* la,const btDbvtNode* lb) + btSoftBody* bodies[2]; + void Process(const btDbvtNode* la, const btDbvtNode* lb) { - btSoftBody::Cluster* cla=(btSoftBody::Cluster*)la->data; - btSoftBody::Cluster* clb=(btSoftBody::Cluster*)lb->data; + btSoftBody::Cluster* cla = (btSoftBody::Cluster*)la->data; + btSoftBody::Cluster* clb = (btSoftBody::Cluster*)lb->data; - - bool connected=false; - if ((bodies[0]==bodies[1])&&(bodies[0]->m_clusterConnectivity.size())) + bool connected = false; + if ((bodies[0] == bodies[1]) && (bodies[0]->m_clusterConnectivity.size())) { - connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex+bodies[0]->m_clusters.size()*clb->m_clusterIndex]; + connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex + bodies[0]->m_clusters.size() * clb->m_clusterIndex]; } if (!connected) { - btSoftClusterCollisionShape csa(cla); - btSoftClusterCollisionShape csb(clb); - btGjkEpaSolver2::sResults res; - if(btGjkEpaSolver2::SignedDistance( &csa,btTransform::getIdentity(), - &csb,btTransform::getIdentity(), - cla->m_com-clb->m_com,res)) + btSoftClusterCollisionShape csa(cla); + btSoftClusterCollisionShape csb(clb); + btGjkEpaSolver2::sResults res; + if (btGjkEpaSolver2::SignedDistance(&csa, btTransform::getIdentity(), + &csb, btTransform::getIdentity(), + cla->m_com - clb->m_com, res)) { - btSoftBody::CJoint joint; - if(SolveContact(res,cla,clb,joint)) + btSoftBody::CJoint joint; + if (SolveContact(res, cla, clb, joint)) { - btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); - *pj=joint;bodies[0]->m_joints.push_back(pj); - pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL); - pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2; + btSoftBody::CJoint* pj = new (btAlignedAlloc(sizeof(btSoftBody::CJoint), 16)) btSoftBody::CJoint(); + *pj = joint; + bodies[0]->m_joints.push_back(pj); + pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL, bodies[1]->m_cfg.kSSHR_CL); + pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL + bodies[1]->m_cfg.kSS_SPLT_CL) / 2; } } - } else + } + else { - static int count=0; + static int count = 0; count++; //printf("count=%d\n",count); - } } - void ProcessSoftSoft(btSoftBody* psa,btSoftBody* psb) + void ProcessSoftSoft(btSoftBody* psa, btSoftBody* psb) { - idt = psa->m_sst.isdt; + idt = psa->m_sst.isdt; //m_margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2; - m_margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin()); - friction = btMin(psa->m_cfg.kDF,psb->m_cfg.kDF); - bodies[0] = psa; - bodies[1] = psb; - psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this); - } + m_margin = (psa->getCollisionShape()->getMargin() + psb->getCollisionShape()->getMargin()); + friction = btMin(psa->m_cfg.kDF, psb->m_cfg.kDF); + bodies[0] = psa; + bodies[1] = psb; + psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root, psb->m_cdbvt.m_root, *this); + } }; // // CollideSDF_RS // - struct CollideSDF_RS : btDbvt::ICollide + struct CollideSDF_RS : btDbvt::ICollide { - void Process(const btDbvtNode* leaf) + void Process(const btDbvtNode* leaf) { - btSoftBody::Node* node=(btSoftBody::Node*)leaf->data; + btSoftBody::Node* node = (btSoftBody::Node*)leaf->data; DoNode(*node); } - void DoNode(btSoftBody::Node& n) const + void DoNode(btSoftBody::Node& n) const { - const btScalar m=n.m_im>0?dynmargin:stamargin; - btSoftBody::RContact c; + const btScalar m = n.m_im > 0 ? dynmargin : stamargin; + btSoftBody::RContact c; - if( (!n.m_battach)&& - psb->checkContact(m_colObj1Wrap,n.m_x,m,c.m_cti)) + if ((!n.m_battach) && + psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti)) { - const btScalar ima=n.m_im; - const btScalar imb= m_rigidBody? m_rigidBody->getInvMass() : 0.f; - const btScalar ms=ima+imb; - if(ms>0) + const btScalar ima = n.m_im; + const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; + const btScalar ms = ima + imb; + if (ms > 0) { - const btTransform& wtr=m_rigidBody?m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); - static const btMatrix3x3 iwiStatic(0,0,0,0,0,0,0,0,0); - const btMatrix3x3& iwi=m_rigidBody?m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; - const btVector3 ra=n.m_x-wtr.getOrigin(); - const btVector3 va=m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra)*psb->m_sst.sdt : btVector3(0,0,0); - const btVector3 vb=n.m_x-n.m_q; - const btVector3 vr=vb-va; - const btScalar dn=btDot(vr,c.m_cti.m_normal); - const btVector3 fv=vr-c.m_cti.m_normal*dn; - const btScalar fc=psb->m_cfg.kDF*m_colObj1Wrap->getCollisionObject()->getFriction(); - c.m_node = &n; - c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra); - c.m_c1 = ra; - c.m_c2 = ima*psb->m_sst.sdt; - c.m_c3 = fv.length2()<(dn*fc*dn*fc)?0:1-fc; - c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR; + const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; + const btVector3 ra = n.m_x - wtr.getOrigin(); + const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0); + const btVector3 vb = n.m_x - n.m_q; + const btVector3 vr = vb - va; + const btScalar dn = btDot(vr, c.m_cti.m_normal); + const btVector3 fv = vr - c.m_cti.m_normal * dn; + const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); + c.m_node = &n; + c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); + c.m_c1 = ra; + c.m_c2 = ima * psb->m_sst.sdt; + c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc; + c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; psb->m_rcontacts.push_back(c); if (m_rigidBody) m_rigidBody->activate(); } } } - btSoftBody* psb; - const btCollisionObjectWrapper* m_colObj1Wrap; - btRigidBody* m_rigidBody; - btScalar dynmargin; - btScalar stamargin; + btSoftBody* psb; + const btCollisionObjectWrapper* m_colObj1Wrap; + btRigidBody* m_rigidBody; + btScalar dynmargin; + btScalar stamargin; }; // // CollideVF_SS // - struct CollideVF_SS : btDbvt::ICollide + struct CollideVF_SS : btDbvt::ICollide { - void Process(const btDbvtNode* lnode, - const btDbvtNode* lface) + void Process(const btDbvtNode* lnode, + const btDbvtNode* lface) { - btSoftBody::Node* node=(btSoftBody::Node*)lnode->data; - btSoftBody::Face* face=(btSoftBody::Face*)lface->data; - btVector3 o=node->m_x; - btVector3 p; - btScalar d=SIMD_INFINITY; - ProjectOrigin( face->m_n[0]->m_x-o, - face->m_n[1]->m_x-o, - face->m_n[2]->m_x-o, - p,d); - const btScalar m=mrg+(o-node->m_q).length()*2; - if(d<(m*m)) + btSoftBody::Node* node = (btSoftBody::Node*)lnode->data; + btSoftBody::Face* face = (btSoftBody::Face*)lface->data; + btVector3 o = node->m_x; + btVector3 p; + btScalar d = SIMD_INFINITY; + ProjectOrigin(face->m_n[0]->m_x - o, + face->m_n[1]->m_x - o, + face->m_n[2]->m_x - o, + p, d); + const btScalar m = mrg + (o - node->m_q).length() * 2; + if (d < (m * m)) { - const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]}; - const btVector3 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o); - const btScalar ma=node->m_im; - btScalar mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w); - if( (n[0]->m_im<=0)|| - (n[1]->m_im<=0)|| - (n[2]->m_im<=0)) + const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]}; + const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o); + const btScalar ma = node->m_im; + btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w); + if ((n[0]->m_im <= 0) || + (n[1]->m_im <= 0) || + (n[2]->m_im <= 0)) { - mb=0; + mb = 0; } - const btScalar ms=ma+mb; - if(ms>0) + const btScalar ms = ma + mb; + if (ms > 0) { - btSoftBody::SContact c; - c.m_normal = p/-btSqrt(d); - c.m_margin = m; - c.m_node = node; - c.m_face = face; - c.m_weights = w; - c.m_friction = btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF); - c.m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR; - c.m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR; + btSoftBody::SContact c; + c.m_normal = p / -btSqrt(d); + c.m_margin = m; + c.m_node = node; + c.m_face = face; + c.m_weights = w; + c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF); + c.m_cfm[0] = ma / ms * psb[0]->m_cfg.kSHR; + c.m_cfm[1] = mb / ms * psb[1]->m_cfg.kSHR; psb[0]->m_scontacts.push_back(c); } - } + } } - btSoftBody* psb[2]; - btScalar mrg; + btSoftBody* psb[2]; + btScalar mrg; }; }; -#endif //_BT_SOFT_BODY_INTERNALS_H +#endif //_BT_SOFT_BODY_INTERNALS_H |