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; }
550 if((a==ma)&&(b==mb))
return(0);
551 if((a==mb)&&(b==ma))
return(1);
564 static const int maxiterations=16;
573 if(
btFabs(a[p][q])>accuracy)
575 const btScalar w=(a[q][q]-a[p][p])/(2*a[p][q]);
587 }
while((++iterations)<maxiterations);
590 *values=
btVector3(a[0][0],a[1][1],a[2][2]);
597 const btScalar m[2][3]={ {a[p][0],a[p][1],a[p][2]},
598 {a[q][0],a[q][1],a[q][2]}};
601 for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i];
602 for(i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i];
606 const btScalar m[2][3]={ {a[0][p],a[1][p],a[2][p]},
607 {a[0][q],a[1][q],a[2][q]}};
610 for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i];
611 for(i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i];
739 m_colObjWrap = colObWrap;
750 volume.Expand(
btVector3(1,1,1)*m_margin);
766 bool connected=
false;
779 cla->
m_com-clb->m_com,res))
782 if(SolveContact(res,cla,clb,joint))
825 psb->checkContact(m_colObj1Wrap,n.
m_x,m,c.
m_cti))
828 const btScalar imb= m_rigidBody? m_rigidBody->getInvMass() : 0.f;
832 const btTransform& wtr=m_rigidBody?m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
833 static const btMatrix3x3 iwiStatic(0,0,0,0,0,0,0,0,0);
834 const btMatrix3x3& iwi=m_rigidBody?m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
836 const btVector3 va=m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra)*psb->m_sst.sdt :
btVector3(0,0,0);
841 const btScalar fc=psb->m_cfg.kDF*m_colObj1Wrap->getCollisionObject()->getFriction();
845 c.
m_c2 = ima*psb->m_sst.sdt;
847 c.
m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR;
848 psb->m_rcontacts.push_back(c);
850 m_rigidBody->activate();
884 if( (n[0]->m_im<=0)||
900 c.
m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR;
901 c.
m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR;
902 psb[0]->m_scontacts.push_back(c);
911 #endif //_BT_SOFT_BODY_INTERNALS_H static void Orthogonalize(btMatrix3x3 &m)
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
void push_back(const T &_Val)
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &vec) const
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...
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
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)
static T Sign(const T &x)
static btMatrix3x3 Mul(const btMatrix3x3 &a, btScalar b)
const btMatrix3x3 & invWorldInertia() const
static btScalar AreaOf(const btVector3 &x0, const btVector3 &x1, const btVector3 &x2)
void DoNode(btSoftBody::Node &n) const
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)
const btTransform & xform() const
static int PolarDecompose(const btMatrix3x3 &m, btMatrix3x3 &q, btMatrix3x3 &s)
static void EvaluateMedium(const btSoftBodyWorldInfo *wfi, const btVector3 &x, btSoftBody::sMedium &medium)
void ProcessColObj(btSoftBody *ps, const btCollisionObjectWrapper *colObWrap)
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
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)
virtual btScalar getMargin() const
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
const btScalar & x() const
Return the x value.
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()
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...
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
void Process(const btDbvtNode *lnode, const btDbvtNode *lface)
virtual void setMargin(btScalar margin)
void processAllTriangles(btTriangleCallback *, const btVector3 &, const btVector3 &) const
virtual void setLocalScaling(const btVector3 &)
int size() const
return the number of elements in the array
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)
The btTriangleCallback provides a callback for each overlapping triangle when calling processAllTrian...
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
int index(int c, int r) const
static btScalar ClusterMetric(const btVector3 &x, const btVector3 &y)
bool isStaticOrKinematicObject() const
virtual void calculateLocalInertia(btScalar, btVector3 &) const
const btTransform & getWorldTransform() const
static btVector3 Clamp(const btVector3 &v, btScalar maxlength)
The btRigidBody is the main class for rigid body objects.
btScalar length() const
Return the length of the vector.
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)
virtual btVector3 localGetSupportingVertex(const btVector3 &vec) const
static T BaryEval(const T &a, const T &b, const T &c, const btVector3 &coord)
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const
virtual btScalar getMargin() const
const btScalar & y() const
Return the y value.
static bool CompLess(const T &a, const T &b)
static btMatrix3x3 ScaleAlongAxis(const btVector3 &a, btScalar s)
const btCollisionShape * getCollisionShape() const
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
const T & operator()(int c, int r) const
btScalar length2() const
Return the length of the vector squared.
virtual btScalar Eval(const btVector3 &x)=0
virtual void calculateLocalInertia(btScalar mass, btVector3 &inertia) const
static T Cube(const T &x)
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
btVector3 normalized() const
Return a normalized version of this vector.
static void mulPQ(btMatrix3x3 &a, btScalar c, btScalar s, int p, int q)
virtual const char * getName() const
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)
virtual const char * getName() const
void ProcessSoftSoft(btSoftBody *psa, btSoftBody *psb)
static void mulTPQ(btMatrix3x3 &a, btScalar c, btScalar s, int p, int q)
btAlignedObjectArray< T > store
btScalar getFriction() const
btRigidBody * m_rigidBody
virtual btScalar getMargin() const =0
void Process(const btDbvtNode *leaf)
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)
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...
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 btCollisionShape * getCollisionShape() const
DBVT_PREFIX void collideTV(const btDbvtNode *root, const btDbvtVolume &volume, DBVT_IPOLICY) const
unsigned int decompose(const btMatrix3x3 &a, btMatrix3x3 &u, btMatrix3x3 &h) const
Decomposes a matrix into orthogonal and symmetric, positive-definite parts.
void setIdentity()
Set the matrix to the identity.
btVector3 velocity(const btVector3 &rpos) const
static int system(btMatrix3x3 &a, btMatrix3x3 *vectors, btVector3 *values=0)
virtual int getShapeType() const
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 btCollisionObject * getCollisionObject() const
btScalar btFabs(btScalar x)
const btScalar & z() const
Return the z value.