17 #ifndef _BT_SOFT_BODY_INTERNALS_H 18 #define _BT_SOFT_BODY_INTERNALS_H 38 void resize(
int n,
const T& init=T()) {
dim=n;
store.resize((n*(n+1))/2,init); }
85 aabbMin=aabbMax=crns[0];
131 for(
int i=1,ni=m_cluster->
m_nodes.size();i<ni;++i)
140 return(localGetSupportingVertex(vec));
156 virtual const char*
getName()
const {
return "SOFTCLUSTER";}
173 template <
typename T>
176 memset(&value,0,
sizeof(T));
179 template <
typename T>
183 template <
typename T>
187 template <
typename T>
189 {
return(a+(b-a)*t); }
191 template <
typename T>
193 {
return((b+a*t-b*t)/(a*b)); }
200 r[0]=
Lerp(a[0],b[0],t);
201 r[1]=
Lerp(a[1],b[1],t);
202 r[2]=
Lerp(a[2],b[2],t);
209 if(sql>(maxlength*maxlength))
210 return((v*maxlength)/
btSqrt(sql));
215 template <
typename T>
216 static inline T
Clamp(
const T& x,
const T& l,
const T& h)
217 {
return(x<l?l:x>h?h:x); }
219 template <
typename T>
220 static inline T
Sq(
const T& x)
223 template <
typename T>
224 static inline T
Cube(
const T& x)
227 template <
typename T>
228 static inline T
Sign(
const T& x)
229 {
return((T)(x<0?-1:+1)); }
231 template <
typename T>
250 m[0]=
btVector3(1-xx+xx*s,xy*s-xy,zx*s-zx);
251 m[1]=
btVector3(xy*s-xy,1-yy+yy*s,yz*s-yz);
252 m[2]=
btVector3(zx*s-zx,yz*s-yz,1-zz+zz*s);
278 for(
int i=0;i<3;++i) r[i]=a[i]+b[i];
286 for(
int i=0;i<3;++i) r[i]=a[i]-b[i];
294 for(
int i=0;i<3;++i) r[i]=a[i]*b;
339 return(a*
btDot(v,a));
403 template <
typename T>
409 return(a*coord.
x()+b*coord.
y()+c*coord.
z());
420 const btScalar isum=1/(w[0]+w[1]+w[2]);
421 return(
btVector3(w[1]*isum,w[2]*isum,w[0]*isum));
429 const int maxiterations=256)
433 if(values[0]>values[1])
436 btSwap(values[0],values[1]);
438 if(values[0]>-accuracy)
return(-1);
439 if(values[1]<+accuracy)
return(-1);
440 for(
int i=0;i<maxiterations;++i)
442 const btScalar t=
Lerp(span[0],span[1],values[0]/(values[0]-values[1]));
444 if((t<=0)||(t>=1))
break;
445 if(
btFabs(v)<accuracy)
return(t);
447 { span[0]=t;values[0]=v; }
449 { span[1]=t;values[1]=v; }
547 if((a==ma)&&(b==mb))
return(0);
548 if((a==mb)&&(b==ma))
return(1);
561 static const int maxiterations=16;
570 if(
btFabs(a[p][q])>accuracy)
572 const btScalar w=(a[q][q]-a[p][p])/(2*a[p][q]);
584 }
while((++iterations)<maxiterations);
587 *values=
btVector3(a[0][0],a[1][1],a[2][2]);
594 const btScalar m[2][3]={ {a[p][0],a[p][1],a[p][2]},
595 {a[q][0],a[q][1],a[q][2]}};
598 for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i];
599 for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i];
603 const btScalar m[2][3]={ {a[0][p],a[1][p],a[2][p]},
604 {a[0][q],a[1][q],a[2][q]}};
607 for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i];
608 for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i];
736 m_colObjWrap = colObWrap;
747 volume.Expand(
btVector3(1,1,1)*m_margin);
763 bool connected=
false;
776 cla->
m_com-clb->m_com,res))
779 if(SolveContact(res,cla,clb,joint))
822 psb->checkContact(m_colObj1Wrap,n.
m_x,m,c.
m_cti))
825 const btScalar imb= m_rigidBody? m_rigidBody->getInvMass() : 0.f;
829 const btTransform& wtr=m_rigidBody?m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
830 static const btMatrix3x3 iwiStatic(0,0,0,0,0,0,0,0,0);
831 const btMatrix3x3& iwi=m_rigidBody?m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
833 const btVector3 va=m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra)*psb->m_sst.sdt :
btVector3(0,0,0);
838 const btScalar fc=psb->m_cfg.kDF*m_colObj1Wrap->getCollisionObject()->getFriction();
842 c.
m_c2 = ima*psb->m_sst.sdt;
844 c.
m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR;
845 psb->m_rcontacts.push_back(c);
847 m_rigidBody->activate();
881 if( (n[0]->m_im<=0)||
897 c.
m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR;
898 c.
m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR;
899 psb[0]->m_scontacts.push_back(c);
908 #endif //_BT_SOFT_BODY_INTERNALS_H static void Orthogonalize(btMatrix3x3 &m)
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
int index(int c, int r) const
void push_back(const T &_Val)
static void ZeroInitialize(T &value)
static btVector3 CenterOf(const btSoftBody::Face &f)
btSoftBody implementation by Nathanael Presson
static btMatrix3x3 ImpulseMatrix(btScalar dt, btScalar ima, btScalar imb, const btMatrix3x3 &iwi, const btVector3 &r)
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
btScalar length2() const
Return the length of the vector squared.
btSymMatrix(int n, const T &init=T())
The btConvexInternalShape is an internal base class, shared by most convex shape implementations.
const btCollisionObjectWrapper * m_colObj1Wrap
T & operator()(int c, int r)
btAlignedObjectArray< bool > m_clusterConnectivity
virtual btVector3 localGetSupportingVertex(const btVector3 &vec) const
static btDbvtAabbMm FromPoints(const btVector3 *pts, int n)
virtual const btVector3 & getLocalScaling() const
btScalar btSqrt(btScalar y)
btAlignedObjectArray< Node * > m_nodes
bool SolveContact(const btGjkEpaSolver2::sResults &res, btSoftBody::Body ba, const btSoftBody::Body bb, btSoftBody::CJoint &joint)
virtual int getShapeType() const
static T Sign(const T &x)
static btMatrix3x3 Mul(const btMatrix3x3 &a, btScalar b)
unsigned int decompose(const btMatrix3x3 &a, btMatrix3x3 &u, btMatrix3x3 &h) const
Decomposes a matrix into orthogonal and symmetric, positive-definite parts.
static btScalar AreaOf(const btVector3 &x0, const btVector3 &x1, const btVector3 &x2)
static bool CompGreater(const T &a, const T &b)
static void ProjectOrigin(const btVector3 &a, const btVector3 &b, btVector3 &prj, btScalar &sqd)
static bool SameSign(const T &x, const T &y)
static T InvLerp(const T &a, const T &b, btScalar t)
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const
getAabb's default implementation is brute force, expected derived classes to implement a fast dedicat...
static int PolarDecompose(const btMatrix3x3 &m, btMatrix3x3 &q, btMatrix3x3 &s)
static void EvaluateMedium(const btSoftBodyWorldInfo *wfi, const btVector3 &x, btSoftBody::sMedium &medium)
const btCollisionShape * getCollisionShape() const
void ProcessColObj(btSoftBody *ps, const btCollisionObjectWrapper *colObWrap)
virtual void calculateLocalInertia(btScalar, btVector3 &) const
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
bool isStaticOrKinematicObject() const
void DoNode(btSoftBody::Node &n) const
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const =0
getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t...
static btMatrix3x3 Diagonal(btScalar x)
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
btVector3 normalized() const
Return a normalized version of this vector.
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
static btDbvtVolume VolumeOf(const btSoftBody::Face &f, btScalar margin)
static btVector3 ProjectOnPlane(const btVector3 &v, const btVector3 &a)
virtual ~btSoftBodyCollisionShape()
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
virtual const char * getName() const
void Process(const btDbvtNode *lnode, const btDbvtNode *lface)
virtual void setMargin(btScalar margin)
virtual void setLocalScaling(const btVector3 &)
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const
getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t...
static void ApplyClampedForce(btSoftBody::Node &n, const btVector3 &f, btScalar dt)
btSoftBodyCollisionShape(btSoftBody *backptr)
static btVector3 BaryCoord(const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &p)
const btScalar & x() const
Return the x value.
The btTriangleCallback provides a callback for each overlapping triangle when calling processAllTrian...
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
const btCollisionObject * getCollisionObject() const
static btScalar ClusterMetric(const btVector3 &x, const btVector3 &y)
void processAllTriangles(btTriangleCallback *, const btVector3 &, const btVector3 &) const
virtual void calculateLocalInertia(btScalar mass, btVector3 &inertia) const
const btScalar & y() const
Return the y value.
const btScalar & z() const
Return the z value.
DBVT_PREFIX void collideTV(const btDbvtNode *root, const btDbvtVolume &volume, DBVT_IPOLICY) const
static btVector3 Clamp(const btVector3 &v, btScalar maxlength)
The btRigidBody is the main class for rigid body objects.
static btMatrix3x3 Cross(const btVector3 &v)
DBVT_INLINE void Expand(const btVector3 &e)
static int MatchEdge(const btSoftBody::Node *a, const btSoftBody::Node *b, const btSoftBody::Node *ma, const btSoftBody::Node *mb)
const btMatrix3x3 & invWorldInertia() const
static T BaryEval(const T &a, const T &b, const T &c, const btVector3 &coord)
static bool CompLess(const T &a, const T &b)
static btMatrix3x3 ScaleAlongAxis(const btVector3 &a, btScalar s)
static btMatrix3x3 AngularImpulseMatrix(const btMatrix3x3 &iia, const btMatrix3x3 &iib)
btVector3 can be used to represent 3D points and vectors.
#define ATTRIBUTE_ALIGNED16(a)
const btSoftBody::Cluster * m_cluster
virtual btScalar Eval(const btVector3 &x)=0
int size() const
return the number of elements in the array
virtual btScalar getMargin() const
static T Cube(const T &x)
btVector3 velocity(const btVector3 &rpos) const
static btMatrix3x3 MassMatrix(btScalar im, const btMatrix3x3 &iwi, const btVector3 &r)
static btScalar ImplicitSolve(btSoftBody::ImplicitFn *fn, const btVector3 &a, const btVector3 &b, const btScalar accuracy, const int maxiterations=256)
const btCollisionObjectWrapper * m_colObjWrap
static void mulPQ(btMatrix3x3 &a, btScalar c, btScalar s, int p, int q)
The btConcaveShape class provides an interface for non-moving (static) concave shapes.
void resize(int n, const T &init=T())
static T Lerp(const T &a, const T &b, btScalar t)
void ProcessSoftSoft(btSoftBody *psa, btSoftBody *psb)
static void mulTPQ(btMatrix3x3 &a, btScalar c, btScalar s, int p, int q)
btAlignedObjectArray< T > store
btRigidBody * m_rigidBody
virtual btScalar getMargin() const =0
const btTransform & getWorldTransform() const
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const
void Process(const btDbvtNode *leaf)
const btTransform & xform() const
const T & btMax(const T &a, const T &b)
static btScalar SignedDistance(const btVector3 &position, btScalar margin, const btConvexShape *shape, const btTransform &wtrs, sResults &results)
static btMatrix3x3 Sub(const btMatrix3x3 &a, const btMatrix3x3 &b)
void Process(const btDbvtNode *leaf)
#define btAlignedAlloc(size, alignment)
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
static btVector3 NormalizeAny(const btVector3 &v)
static btMatrix3x3 Add(const btMatrix3x3 &a, const btMatrix3x3 &b)
static btVector3 ProjectOnAxis(const btVector3 &v, const btVector3 &a)
DBVT_PREFIX void collideTT(const btDbvtNode *root0, const btDbvtNode *root1, DBVT_IPOLICY)
btSoftClusterCollisionShape(const btSoftBody::Cluster *cluster)
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
void Process(const btDbvtNode *la, const btDbvtNode *lb)
virtual void setMargin(btScalar margin)
This class is used to compute the polar decomposition of a matrix.
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
const T & btMin(const T &a, const T &b)
const T & operator()(int c, int r) const
btScalar getFriction() const
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &vec) const
void setIdentity()
Set the matrix to the identity.
static int system(btMatrix3x3 &a, btMatrix3x3 *vectors, btVector3 *values=0)
void setMin(const btVector3 &other)
Set each element to the min of the current values and the values of another btVector3.
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
const btCollisionShape * getCollisionShape() const
virtual const char * getName() const
btScalar length() const
Return the length of the vector.
btScalar btFabs(btScalar x)