17 #ifndef _BT_SOFT_BODY_INTERNALS_H
18 #define _BT_SOFT_BODY_INTERNALS_H
56 if (ax <= ay && ax <= az)
58 else if (ay <= ax && ay <= az)
73 void resize(
int n,
const T& init = T())
76 store.resize((n * (n + 1)) / 2, init);
82 return ((r * (r + 1)) / 2 + c);
128 aabbMin = aabbMax = crns[0];
129 for (
int i = 1; i < 8; ++i)
202 virtual const char*
getName()
const {
return "SOFTCLUSTER"; }
219 template <
typename T>
222 memset(&value, 0,
sizeof(T));
225 template <
typename T>
226 static inline bool CompLess(
const T& a,
const T& b)
231 template <
typename T>
237 template <
typename T>
240 return (a + (b - a) * t);
243 template <
typename T>
246 return ((b + a * t - b * t) / (a * b));
254 r[0] =
Lerp(a[0], b[0], t);
255 r[1] =
Lerp(a[1], b[1], t);
256 r[2] =
Lerp(a[2], b[2], t);
263 if (sql > (maxlength * maxlength))
264 return ((v * maxlength) /
btSqrt(sql));
269 template <
typename T>
270 static inline T
Clamp(
const T& x,
const T& l,
const T& h)
272 return (x < l ? l : x > h ? h : x);
275 template <
typename T>
276 static inline T
Sq(
const T& x)
281 template <
typename T>
282 static inline T
Cube(
const T& x)
287 template <
typename T>
288 static inline T
Sign(
const T& x)
290 return ((T)(x < 0 ? -1 : +1));
293 template <
typename T>
294 static inline bool SameSign(
const T& x,
const T& y)
296 return ((x * y) > 0);
314 m[0] =
btVector3(1 - xx + xx * s, xy * s - xy, zx * s - zx);
315 m[1] =
btVector3(xy * s - xy, 1 - yy + yy * s, yz * s - yz);
316 m[2] =
btVector3(zx * s - zx, yz * s - yz, 1 - zz + zz * s);
350 for (
int i = 0; i < ndof; ++i)
351 result += a[i] * b[i];
382 for (
int i = 0; i < 3; ++i) r[i] = a[i] + b[i];
390 for (
int i = 0; i < 3; ++i) r[i] = a[i] - b[i];
398 for (
int i = 0; i < 3; ++i) r[i] = a[i] * b;
443 return (a *
btDot(v, a));
516 if (a > -0.00001 && a < 0.00001)
523 if (u < 0.0 || u > 1.0)
528 if (v < 0.0 || u + v > 1.0)
556 sect = rayStart + dir * t;
578 template <
typename T>
584 return (a * coord.
x() + b * coord.
y() + c * coord.
z());
595 const btScalar isum = 1 / (w[0] + w[1] + w[2]);
596 return (
btVector3(w[1] * isum, w[2] * isum, w[0] * isum));
604 const int maxiterations = 256)
608 if (values[0] > values[1])
611 btSwap(values[0], values[1]);
613 if (values[0] > -accuracy)
return (-1);
614 if (values[1] < +accuracy)
return (-1);
615 for (
int i = 0; i < maxiterations; ++i)
617 const btScalar t =
Lerp(span[0], span[1], values[0] / (values[0] - values[1]));
619 if ((t <= 0) || (t >= 1))
break;
620 if (
btFabs(v) < accuracy)
return (t);
729 if ((a == ma) && (b == mb))
return (0);
730 if ((a == mb) && (b == ma))
return (1);
743 static const int maxiterations = 16;
761 if (
btFabs(a[p][q]) > accuracy)
763 const btScalar w = (a[q][q] - a[p][p]) / (2 * a[p][q]);
770 mulPQ(a, c, s, p, q);
772 mulPQ(v, c, s, p, q);
779 }
while ((++iterations) < maxiterations);
782 *values =
btVector3(a[0][0], a[1][1], a[2][2]);
790 const btScalar m[2][3] = {{a[p][0], a[p][1], a[p][2]},
791 {a[q][0], a[q][1], a[q][2]}};
794 for (i = 0; i < 3; ++i) a[p][i] = c * m[0][i] - s * m[1][i];
795 for (i = 0; i < 3; ++i) a[q][i] = c * m[1][i] + s * m[0][i];
799 const btScalar m[2][3] = {{a[0][p], a[1][p], a[2][p]},
800 {a[0][q], a[1][q], a[2][q]}};
803 for (i = 0; i < 3; ++i) a[i][p] = c * m[0][i] - s * m[1][i];
804 for (i = 0; i < 3; ++i) a[i][q] = c * m[1][i] + s * m[0][i];
960 bool connected =
false;
973 cla->
m_com - clb->m_com, res))
988 static int count = 0;
1028 static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
1041 c.
m_c3 = fv.
length2() < (dn * fc * dn * fc) ? 0 : 1 - fc;
1094 static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
1104 if (multibodyLinkCol)
1195 static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
1206 if (multibodyLinkCol)
1212 findJacobian(multibodyLinkCol, jacobianData_normal, contact_point, normal);
1213 findJacobian(multibodyLinkCol, jacobianData_t1, contact_point, t1);
1214 findJacobian(multibodyLinkCol, jacobianData_t2, contact_point, t2);
1262 for (
int i = 0; i < 3; ++i)
1264 if (face->m_n[i] == node)
1272 face->m_n[1]->m_x - o,
1273 face->m_n[2]->m_x - o,
1282 if ((n[0]->m_im <= 0) ||
1283 (n[1]->m_im <= 0) ||
1324 face->m_n[1]->m_x - o,
1325 face->m_n[2]->m_x - o,
1334 if ((n[0]->m_im <= 0) ||
1335 (n[1]->m_im <= 0) ||
1377 for (
int node_id = 0; node_id < 3; ++node_id)
1381 for (
int i = 0; i < 3; ++i)
1383 if (face->m_n[i] == node)
1395 face->m_n[1]->m_x - o,
1396 face->m_n[2]->m_x - o,
1405 if ((n[0]->m_im <= 0) ||
1406 (n[1]->m_im <= 0) ||
#define btAlignedAlloc(size, alignment)
@ SOFTBODY_SHAPE_PROXYTYPE
const T & btMax(const T &a, const T &b)
const T & btMin(const T &a, const T &b)
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
#define ATTRIBUTE_ALIGNED16(a)
btScalar btSqrt(btScalar y)
btScalar btFabs(btScalar x)
static btVector3 ProjectOnAxis(const btVector3 &v, const btVector3 &a)
static bool CompGreater(const T &a, const T &b)
static btMatrix3x3 OuterProduct(const btScalar *v1, const btScalar *v2, const btScalar *v3, const btScalar *u1, const btScalar *u2, const btScalar *u3, int ndof)
static btMatrix3x3 Mul(const btMatrix3x3 &a, btScalar b)
static btScalar ImplicitSolve(btSoftBody::ImplicitFn *fn, const btVector3 &a, const btVector3 &b, const btScalar accuracy, const int maxiterations=256)
static void Orthogonalize(btMatrix3x3 &m)
static btMatrix3x3 Cross(const btVector3 &v)
static btMatrix3x3 MassMatrix(btScalar im, const btMatrix3x3 &iwi, const btVector3 &r)
static btDbvtVolume VolumeOf(const btSoftBody::Face &f, btScalar margin)
static T Lerp(const T &a, const T &b, btScalar t)
static btMatrix3x3 AngularImpulseMatrix(const btMatrix3x3 &iia, const btMatrix3x3 &iib)
static btScalar AreaOf(const btVector3 &x0, const btVector3 &x1, const btVector3 &x2)
static btVector3 generateUnitOrthogonalVector(const btVector3 &u)
static btVector3 NormalizeAny(const btVector3 &v)
static int PolarDecompose(const btMatrix3x3 &m, btMatrix3x3 &q, btMatrix3x3 &s)
static bool lineIntersectsTriangle(const btVector3 &rayStart, const btVector3 &rayEnd, const btVector3 &p1, const btVector3 &p2, const btVector3 &p3, btVector3 §, btVector3 &normal)
static int MatchEdge(const btSoftBody::Node *a, const btSoftBody::Node *b, const btSoftBody::Node *ma, const btSoftBody::Node *mb)
static void findJacobian(const btMultiBodyLinkCollider *multibodyLinkCol, btMultiBodyJacobianData &jacobianData, const btVector3 &contact_point, const btVector3 &dir)
btSoftBody implementation by Nathanael Presson
static bool CompLess(const T &a, const T &b)
static T BaryEval(const T &a, const T &b, const T &c, const btVector3 &coord)
static btVector3 CenterOf(const btSoftBody::Face &f)
static bool SameSign(const T &x, const T &y)
static void EvaluateMedium(const btSoftBodyWorldInfo *wfi, const btVector3 &x, btSoftBody::sMedium &medium)
static btVector3 BaryCoord(const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &p)
static btMatrix3x3 ScaleAlongAxis(const btVector3 &a, btScalar s)
static void ZeroInitialize(T &value)
static btMatrix3x3 Add(const btMatrix3x3 &a, const btMatrix3x3 &b)
static T InvLerp(const T &a, const T &b, btScalar t)
static btVector3 ProjectOnPlane(const btVector3 &v, const btVector3 &a)
static btScalar ClusterMetric(const btVector3 &x, const btVector3 &y)
static btMatrix3x3 ImpulseMatrix(btScalar dt, btScalar ima, btScalar imb, const btMatrix3x3 &iwi, const btVector3 &r)
static void ApplyClampedForce(btSoftBody::Node &n, const btVector3 &f, btScalar dt)
static bool rayIntersectsTriangle(const btVector3 &origin, const btVector3 &dir, const btVector3 &v0, const btVector3 &v1, const btVector3 &v2, btScalar &t)
static void ProjectOrigin(const btVector3 &a, const btVector3 &b, btVector3 &prj, btScalar &sqd)
static btScalar Dot(const btScalar *a, const btScalar *b, int ndof)
static btMatrix3x3 Sub(const btMatrix3x3 &a, const btMatrix3x3 &b)
static T Sign(const T &x)
static btVector3 Clamp(const btVector3 &v, btScalar maxlength)
static btMatrix3x3 Diagonal(btScalar x)
static T Cube(const T &x)
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
void push_back(const T &_Val)
bool isStaticOrKinematicObject() const
btTransform & getWorldTransform()
int getInternalType() const
reserved for Bullet internal usage
void activate(bool forceActivation=false) const
btScalar getFriction() const
const btCollisionShape * getCollisionShape() const
virtual btScalar getMargin() const =0
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.
The btConcaveShape class provides an interface for non-moving (static) concave shapes.
The btConvexInternalShape is an internal base class, shared by most convex shape implementations.
virtual void setMargin(btScalar margin)
virtual btScalar getMargin() const
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
btMatrix3x3 transpose() const
Return the transpose of the matrix.
void setIdentity()
Set the matrix to the identity.
btMultiBody * m_multiBody
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
void fillContactJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal, btScalar *jac, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m) const
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instea...
This class is used to compute the polar decomposition of a matrix.
unsigned int decompose(const btMatrix3x3 &a, btMatrix3x3 &u, btMatrix3x3 &h) const
Decomposes a matrix into orthogonal and symmetric, positive-definite parts.
The btRigidBody is the main class for rigid body objects.
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
btScalar getInvMass() const
const btMatrix3x3 & getInvInertiaTensorWorld() const
virtual void calculateLocalInertia(btScalar, btVector3 &) const
void processAllTriangles(btTriangleCallback *, const btVector3 &, const btVector3 &) const
virtual void setLocalScaling(const btVector3 &)
virtual const char * getName() const
btSoftBodyCollisionShape(btSoftBody *backptr)
virtual const btVector3 & getLocalScaling() const
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.
virtual ~btSoftBodyCollisionShape()
btMultiBodyJacobianData jacobianData_t1
btMultiBodyJacobianData jacobianData_normal
btMultiBodyJacobianData jacobianData_t2
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
btAlignedObjectArray< bool > m_clusterConnectivity
btAlignedObjectArray< DeformableFaceNodeContact > m_faceNodeContacts
bool checkDeformableContact(const btCollisionObjectWrapper *colObjWrap, const btVector3 &x, btScalar margin, btSoftBody::sCti &cti, bool predict=false) const
tRContactArray m_rcontacts
btAlignedObjectArray< DeformableFaceRigidContact > m_faceRigidContacts
bool checkDeformableFaceContact(const btCollisionObjectWrapper *colObjWrap, Face &f, btVector3 &contact_point, btVector3 &bary, btScalar margin, btSoftBody::sCti &cti, bool predict=false) const
btAlignedObjectArray< DeformableNodeRigidContact > m_nodeRigidContacts
tSContactArray m_scontacts
bool checkContact(const btCollisionObjectWrapper *colObjWrap, const btVector3 &x, btScalar margin, btSoftBody::sCti &cti) const
virtual void setMargin(btScalar margin)
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)
virtual int getShapeType() const
virtual void calculateLocalInertia(btScalar mass, btVector3 &inertia) const
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &vec) const
virtual const char * getName() const
virtual btScalar getMargin() const
const btSoftBody::Cluster * m_cluster
virtual btVector3 localGetSupportingVertex(const btVector3 &vec) const
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const
The btTriangleCallback provides a callback for each overlapping triangle when calling processAllTrian...
btVector3 can be used to represent 3D points and vectors.
btVector3 & safeNormalize()
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
const btScalar & y() const
Return the y value.
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
btScalar length() const
Return the length of the vector.
const btScalar & z() const
Return the z value.
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
const btScalar & getX() const
Return the x value.
btScalar dot(const btVector3 &v) const
Return the dot product.
btScalar norm() const
Return the norm (length) of the vector.
const btScalar & getZ() const
Return the z value.
btVector3 normalized() const
Return a normalized version of this vector.
const btScalar & x() const
Return the x value.
const btScalar & getY() const
Return the y value.
btScalar length2() const
Return the length of the vector squared.
void setMin(const btVector3 &other)
Set each element to the min of the current values and the values of another btVector3.
const btCollisionShape * getCollisionShape() const
const btCollisionObject * getCollisionObject() const
const btTransform & getWorldTransform() const
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
static btDbvtAabbMm FromPoints(const btVector3 *pts, int n)
DBVT_INLINE void Expand(const btVector3 &e)
DBVT_PREFIX void collideTV(const btDbvtNode *root, const btDbvtVolume &volume, DBVT_IPOLICY) const
DBVT_PREFIX void collideTT(const btDbvtNode *root0, const btDbvtNode *root1, DBVT_IPOLICY)
static void mulPQ(btMatrix3x3 &a, btScalar c, btScalar s, int p, int q)
static int system(btMatrix3x3 &a, btMatrix3x3 *vectors, btVector3 *values=0)
static void mulTPQ(btMatrix3x3 &a, btScalar c, btScalar s, int p, int q)
static btScalar SignedDistance(const btVector3 &position, btScalar margin, const btConvexShape *shape, const btTransform &wtrs, sResults &results)
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
btAlignedObjectArray< btScalar > m_jacobians
btAlignedObjectArray< btScalar > scratch_r
btAlignedObjectArray< btMatrix3x3 > scratch_m
btAlignedObjectArray< btVector3 > scratch_v
const btTransform & xform() const
btVector3 velocity(const btVector3 &rpos) const
const btMatrix3x3 & invWorldInertia() const
btAlignedObjectArray< Node * > m_nodes
virtual btScalar Eval(const btVector3 &x)=0
const btCollisionObject * m_colObj
bool SolveContact(const btGjkEpaSolver2::sResults &res, btSoftBody::Body ba, const btSoftBody::Body bb, btSoftBody::CJoint &joint)
void Process(const btDbvtNode *leaf)
void ProcessColObj(btSoftBody *ps, const btCollisionObjectWrapper *colObWrap)
const btCollisionObjectWrapper * m_colObjWrap
void Process(const btDbvtNode *la, const btDbvtNode *lb)
void ProcessSoftSoft(btSoftBody *psa, btSoftBody *psb)
void Process(const btDbvntNode *lface1, const btDbvntNode *lface2)
void DoNode(btSoftBody::Face &f) const
void Process(const btDbvtNode *leaf)
btRigidBody * m_rigidBody
const btCollisionObjectWrapper * m_colObj1Wrap
btRigidBody * m_rigidBody
void Process(const btDbvtNode *leaf)
const btCollisionObjectWrapper * m_colObj1Wrap
void DoNode(btSoftBody::Node &n) const
void DoNode(btSoftBody::Node &n) const
void Process(const btDbvtNode *leaf)
btRigidBody * m_rigidBody
const btCollisionObjectWrapper * m_colObj1Wrap
void Process(const btDbvtNode *lnode, const btDbvtNode *lface)
void Process(const btDbvtNode *lnode, const btDbvtNode *lface)
T & operator()(int c, int r)
int index(int c, int r) const
btAlignedObjectArray< T > store
void resize(int n, const T &init=T())
btSymMatrix(int n, const T &init=T())
const T & operator()(int c, int r) const