Bullet Collision Detection & Physics Library
|
Go to the documentation of this file.
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) ||
1440 #endif //_BT_SOFT_BODY_INTERNALS_H
virtual int getShapeType() const
btAlignedObjectArray< T > store
btRigidBody * m_rigidBody
btMultiBodyJacobianData jacobianData_t2
const btTransform & xform() const
btAlignedObjectArray< btVector3 > scratch_v
T & operator()(int c, int r)
static btVector3 CenterOf(const btSoftBody::Face &f)
The btRigidBody is the main class for rigid body objects.
virtual void calculateLocalInertia(btScalar mass, btVector3 &inertia) const
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...
static btMatrix3x3 ImpulseMatrix(btScalar dt, btScalar ima, btScalar imb, const btMatrix3x3 &iwi, const btVector3 &r)
DBVT_INLINE void Expand(const btVector3 &e)
btAlignedObjectArray< bool > m_clusterConnectivity
const btCollisionObjectWrapper * m_colObj1Wrap
static T Sign(const T &x)
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
static void mulPQ(btMatrix3x3 &a, btScalar c, btScalar s, int p, int q)
btSymMatrix(int n, const T &init=T())
btScalar length() const
Return the length of the vector.
static btScalar Dot(const btScalar *a, const btScalar *b, int ndof)
virtual btScalar getMargin() const
The btConvexInternalShape is an internal base class, shared by most convex shape implementations.
void Process(const btDbvtNode *lnode, const btDbvtNode *lface)
btAlignedObjectArray< DeformableFaceNodeContact > m_faceNodeContacts
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
static void ZeroInitialize(T &value)
The btConcaveShape class provides an interface for non-moving (static) concave shapes.
const btSoftBody::Cluster * m_cluster
bool checkDeformableContact(const btCollisionObjectWrapper *colObjWrap, const btVector3 &x, btScalar margin, btSoftBody::sCti &cti, bool predict=false) const
btAlignedObjectArray< btScalar > m_jacobians
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
static btMatrix3x3 OuterProduct(const btScalar *v1, const btScalar *v2, const btScalar *v3, const btScalar *u1, const btScalar *u2, const btScalar *u3, int ndof)
const btCollisionObjectWrapper * m_colObj1Wrap
static btDbvtVolume VolumeOf(const btSoftBody::Face &f, btScalar margin)
static btVector3 ProjectOnPlane(const btVector3 &v, const btVector3 &a)
static bool SameSign(const T &x, const T &y)
btScalar dot(const btVector3 &v) const
Return the dot product.
static int PolarDecompose(const btMatrix3x3 &m, btMatrix3x3 &q, btMatrix3x3 &s)
virtual const char * getName() const
static void EvaluateMedium(const btSoftBodyWorldInfo *wfi, const btVector3 &x, btSoftBody::sMedium &medium)
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
#define btAlignedAlloc(size, alignment)
virtual void calculateLocalInertia(btScalar, btVector3 &) const
const btTransform & getWorldTransform() const
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
btVector3 velocity(const btVector3 &rpos) const
static void ApplyClampedForce(btSoftBody::Node &n, const btVector3 &f, btScalar dt)
static btMatrix3x3 Diagonal(btScalar x)
static btVector3 BaryCoord(const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &p)
void DoNode(btSoftBody::Node &n) const
static btScalar AreaOf(const btVector3 &x0, const btVector3 &x1, const btVector3 &x2)
static btScalar SignedDistance(const btVector3 &position, btScalar margin, const btConvexShape *shape, const btTransform &wtrs, sResults &results)
virtual void setLocalScaling(const btVector3 &)
static int system(btMatrix3x3 &a, btMatrix3x3 *vectors, btVector3 *values=0)
const T & btMin(const T &a, const T &b)
const btMatrix3x3 & invWorldInertia() const
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const
static btScalar ClusterMetric(const btVector3 &x, const btVector3 &y)
const T & btMax(const T &a, const T &b)
const btScalar & y() const
Return the y value.
bool isStaticOrKinematicObject() const
const btScalar & getX() const
Return the x value.
btTransform & getWorldTransform()
tSContactArray m_scontacts
btAlignedObjectArray< Node * > m_nodes
static btMatrix3x3 Mul(const btMatrix3x3 &a, btScalar b)
btAlignedObjectArray< DeformableFaceRigidContact > m_faceRigidContacts
const btCollisionObject * m_colObj
virtual btScalar getMargin() const =0
btScalar btFabs(btScalar x)
void DoNode(btSoftBody::Face &f) const
static bool CompGreater(const T &a, const T &b)
btMultiBodyJacobianData jacobianData_normal
static void ProjectOrigin(const btVector3 &a, const btVector3 &b, btVector3 &prj, btScalar &sqd)
static T InvLerp(const T &a, const T &b, btScalar t)
void activate(bool forceActivation=false) const
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instea...
void ProcessColObj(btSoftBody *ps, const btCollisionObjectWrapper *colObWrap)
const btCollisionShape * getCollisionShape() const
static btMatrix3x3 AngularImpulseMatrix(const btMatrix3x3 &iia, const btMatrix3x3 &iib)
void resize(int newsize, const T &fillData=T())
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
btRigidBody * m_rigidBody
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.
DBVT_PREFIX void collideTV(const btDbvtNode *root, const btDbvtVolume &volume, DBVT_IPOLICY) const
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
void ProcessSoftSoft(btSoftBody *psa, btSoftBody *psb)
const T & operator()(int c, int r) const
btMatrix3x3 transpose() const
Return the transpose of the matrix.
The btTriangleCallback provides a callback for each overlapping triangle when calling processAllTrian...
static void findJacobian(const btMultiBodyLinkCollider *multibodyLinkCol, btMultiBodyJacobianData &jacobianData, const btVector3 &contact_point, const btVector3 &dir)
btSoftBody implementation by Nathanael Presson
btScalar getInvMass() const
btAlignedObjectArray< btScalar > scratch_r
const btCollisionObject * getCollisionObject() const
static T Lerp(const T &a, const T &b, btScalar t)
static btVector3 generateUnitOrthogonalVector(const btVector3 &u)
btVector3 can be used to represent 3D points and vectors.
void DoNode(btSoftBody::Node &n) const
void Process(const btDbvtNode *leaf)
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
static int MatchEdge(const btSoftBody::Node *a, const btSoftBody::Node *b, const btSoftBody::Node *ma, const btSoftBody::Node *mb)
void processAllTriangles(btTriangleCallback *, const btVector3 &, const btVector3 &) const
static T BaryEval(const T &a, const T &b, const T &c, const btVector3 &coord)
bool SolveContact(const btGjkEpaSolver2::sResults &res, btSoftBody::Body ba, const btSoftBody::Body bb, btSoftBody::CJoint &joint)
#define ATTRIBUTE_ALIGNED16(a)
This class is used to compute the polar decomposition of a matrix.
virtual btScalar getMargin() const
const btScalar & getZ() const
Return the z value.
virtual void setMargin(btScalar margin)
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
const btScalar & getY() const
Return the y value.
int index(int c, int r) const
virtual btScalar Eval(const btVector3 &x)=0
tRContactArray m_rcontacts
static btMatrix3x3 Add(const btMatrix3x3 &a, const btMatrix3x3 &b)
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
static btVector3 Clamp(const btVector3 &v, btScalar maxlength)
static btDbvtAabbMm FromPoints(const btVector3 *pts, int n)
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3 &vec) const
void Process(const btDbvtNode *leaf)
void setMin(const btVector3 &other)
Set each element to the min of the current values and the values of another btVector3.
static btMatrix3x3 Cross(const btVector3 &v)
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
const btScalar & x() const
Return the x value.
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
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
virtual const btVector3 & getLocalScaling() const
virtual const char * getName() const
bool checkContact(const btCollisionObjectWrapper *colObjWrap, const btVector3 &x, btScalar margin, btSoftBody::sCti &cti) const
void resize(int n, const T &init=T())
btAlignedObjectArray< DeformableNodeRigidContact > m_nodeRigidContacts
static bool CompLess(const T &a, const T &b)
static btMatrix3x3 ScaleAlongAxis(const btVector3 &a, btScalar s)
static btVector3 NormalizeAny(const btVector3 &v)
int getInternalType() const
reserved for Bullet internal usage
void setIdentity()
Set the matrix to the identity.
static btVector3 ProjectOnAxis(const btVector3 &v, const btVector3 &a)
const btMatrix3x3 & getInvInertiaTensorWorld() const
void Process(const btDbvtNode *leaf)
static T Cube(const T &x)
virtual void setMargin(btScalar margin)
btSoftClusterCollisionShape(const btSoftBody::Cluster *cluster)
static bool lineIntersectsTriangle(const btVector3 &rayStart, const btVector3 &rayEnd, const btVector3 &p1, const btVector3 &p2, const btVector3 &p3, btVector3 §, btVector3 &normal)
const btCollisionObjectWrapper * m_colObjWrap
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)
btMultiBody * m_multiBody
const btCollisionObjectWrapper * m_colObj1Wrap
static btMatrix3x3 Sub(const btMatrix3x3 &a, const btMatrix3x3 &b)
virtual ~btSoftBodyCollisionShape()
btScalar norm() const
Return the norm (length) of the vector.
btMultiBodyJacobianData jacobianData_t1
virtual btVector3 localGetSupportingVertex(const btVector3 &vec) const
static void Orthogonalize(btMatrix3x3 &m)
bool checkDeformableFaceContact(const btCollisionObjectWrapper *colObjWrap, Face &f, btVector3 &contact_point, btVector3 &bary, btScalar margin, btSoftBody::sCti &cti, bool predict=false) const
void push_back(const T &_Val)
void Process(const btDbvtNode *lnode, const btDbvtNode *lface)
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
btScalar btSqrt(btScalar y)
void Process(const btDbvtNode *leaf)
btScalar getFriction() const
btSoftBodyCollisionShape(btSoftBody *backptr)
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
btVector3 normalized() const
Return a normalized version of this vector.
btVector3 & safeNormalize()
btAlignedObjectArray< btMatrix3x3 > scratch_m
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.
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
unsigned int decompose(const btMatrix3x3 &a, btMatrix3x3 &u, btMatrix3x3 &h) const
Decomposes a matrix into orthogonal and symmetric, positive-definite parts.
const btScalar & z() const
Return the z value.
void Process(const btDbvntNode *lface1, const btDbvntNode *lface2)
static bool rayIntersectsTriangle(const btVector3 &origin, const btVector3 &dir, const btVector3 &v0, const btVector3 &v1, const btVector3 &v2, btScalar &t)
void Process(const btDbvtNode *la, const btDbvtNode *lb)
int size() const
return the number of elements in the array
static void mulTPQ(btMatrix3x3 &a, btScalar c, btScalar s, int p, int q)
btScalar length2() const
Return the length of the vector squared.
const btCollisionShape * getCollisionShape() const
btRigidBody * m_rigidBody