43 :
btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration),
44 m_deformableBodySolver(deformableBodySolver),
48 m_drawNodeTree =
true;
49 m_drawFaceTree =
false;
50 m_drawClusterTree =
false;
65 m_useProjection =
false;
72 delete m_solverDeformableBodyIslandCallback;
78 if (0 != m_internalPreTickCallback)
80 (*m_internalPreTickCallback)(
this, timeStep);
91 btMultiBodyDynamicsWorld::performDiscreteCollisionDetection();
120 for (
int i = 0; i < m_softBodies.
size(); ++i)
122 m_softBodies[i]->m_softSoftCollision =
true;
125 for (
int i = 0; i < m_softBodies.
size(); ++i)
127 for (
int j = i; j < m_softBodies.
size(); ++j)
129 m_softBodies[i]->defaultCollisionHandler(m_softBodies[j]);
133 for (
int i = 0; i < m_softBodies.
size(); ++i)
135 m_softBodies[i]->m_softSoftCollision =
false;
141 for (
int i = 0; i < m_softBodies.
size(); i++)
165 BT_PROFILE(
"btDeformableMultiBodyDynamicsWorld::applyRepulsionForce");
166 for (
int i = 0; i < m_softBodies.
size(); i++)
178 BT_PROFILE(
"btDeformableMultiBodyDynamicsWorld::performGeometricCollisions");
180 for (
int i = 0; i < m_softBodies.
size(); ++i)
185 m_softBodies[i]->updateFaceTree(
true,
false);
186 m_softBodies[i]->updateNodeTree(
true,
false);
187 for (
int j = 0; j < m_softBodies[i]->m_faces.
size(); ++j)
196 for (
int r = 0;
r < m_ccdIterations; ++
r)
198 for (
int i = 0; i < m_softBodies.
size(); ++i)
221 for (
int i = 0; i < m_softBodies.
size(); ++i)
223 for (
int j = i; j < m_softBodies.
size(); ++j)
227 if (psb1->isActive() && psb2->isActive())
229 m_softBodies[i]->geometricCollisionHandler(m_softBodies[j]);
234 int penetration_count = 0;
235 for (
int i = 0; i < m_softBodies.
size(); ++i)
243 if (penetration_count == 0)
249 for (
int i = 0; i < m_softBodies.
size(); ++i)
262 BT_PROFILE(
"btDeformableMultiBodyDynamicsWorld::softBodySelfCollision");
263 for (
int i = 0; i < m_softBodies.
size(); i++)
285 if (pushVelocity[0] != 0.f || pushVelocity[1] != 0 || pushVelocity[2] != 0 || turnVelocity[0] != 0.f || turnVelocity[1] != 0 || turnVelocity[2] != 0)
288 rb->setWorldTransform(newTransform);
300 for (
int i = 0; i < m_softBodies.
size(); ++i)
307 btScalar clampDeltaV = maxDisplacement / timeStep;
308 for (
int c = 0;
c < 3;
c++)
310 if (
node.m_v[
c] > clampDeltaV)
312 node.m_v[
c] = clampDeltaV;
314 if (
node.m_v[
c] < -clampDeltaV)
316 node.m_v[
c] = -clampDeltaV;
328 n->
m_x =
a.m_cti.m_colObj->getWorldTransform() *
a.m_local;
334 if (multibodyLinkCol)
338 const btTransform& wtr = multibodyLinkCol->getWorldTransform();
340 wtr.invXform(n->
m_x),
344 a.m_cti.m_normal = wtr.getBasis() * nrm;
350 findJacobian(multibodyLinkCol, jacobianData_t1,
a.m_node->m_x, t1);
351 findJacobian(multibodyLinkCol, jacobianData_t2,
a.m_node->m_x, t2);
362 t1.getX(), t1.getY(), t1.getZ(),
363 t2.getX(), t2.getY(), t2.getZ());
364 const int ndof = multibodyLinkCol->
m_multiBody->getNumDofs() + 6;
366 a.m_c0 =
rot.transpose() * local_impulse_matrix *
rot;
367 a.jacobianData_normal = jacobianData_normal;
368 a.jacobianData_t1 = jacobianData_t1;
369 a.jacobianData_t2 = jacobianData_t2;
381 BT_PROFILE(
"btDeformableMultiBodyDynamicsWorld::solveConstraints");
418 m_solverDeformableBodyIslandCallback->
setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints,
m_sortedMultiBodyConstraints.
size(), getDebugDrawer());
458 bool isSleeping =
false;
460 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() ==
ISLAND_SLEEPING)
464 for (
int b = 0;
b < bod->getNumLinks();
b++)
466 if (bod->getLink(
b).m_collider && bod->getLink(
b).m_collider->getActivationState() ==
ISLAND_SLEEPING)
477 if (bod->internalNeedsJointFeedback())
479 if (!bod->isUsingRK4Integration())
481 if (bod->internalNeedsJointFeedback())
483 bool isConstraintPass =
true;
485 getSolverInfo().m_jointFeedbackInWorldSpace,
486 getSolverInfo().m_jointFeedbackInJointFrame);
497 bod->processDeltaVeeMultiDof2();
510 collisionFilterGroup,
511 collisionFilterMask);
523 m_internalTime += timeStep;
526 m_deformableBodySolver->
reinitialize(m_softBodies, timeStep);
527 btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo();
530 dispatchInfo.
m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer();
531 btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep;
582 bool isSleeping =
false;
584 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() ==
ISLAND_SLEEPING)
588 for (
int b = 0;
b < bod->getNumLinks();
b++)
590 if (bod->getLink(
b).m_collider && bod->getLink(
b).m_collider->getActivationState() ==
ISLAND_SLEEPING)
599 bool isConstraintPass =
false;
601 if (!bod->isUsingRK4Integration())
603 bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep,
605 getSolverInfo().m_jointFeedbackInWorldSpace,
606 getSolverInfo().m_jointFeedbackInJointFrame);
610 btAssert(
" RK4Integration is not supported");
627 if (body->isActive())
637 bool isSleeping =
false;
639 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() ==
ISLAND_SLEEPING)
643 for (
int b = 0;
b < bod->getNumLinks();
b++)
645 if (bod->getLink(
b).m_collider && bod->getLink(
b).m_collider->getActivationState() ==
ISLAND_SLEEPING)
651 bod->addBaseForce(-
m_gravity * bod->getBaseMass());
653 for (
int j = 0; j < bod->getNumLinks(); ++j)
655 bod->addLinkForce(j, -
m_gravity * bod->getLinkMass(j));
663 if (0 != m_internalTickCallback)
665 (*m_internalTickCallback)(
this, timeStep);
668 if (0 != m_solverCallback)
670 (*m_solverCallback)(m_internalTime,
this);
676 if (0 != m_solverCallback)
678 (*m_solverCallback)(m_internalTime,
this);
686 for (
int i = 0; i < forces.
size(); ++i)
690 forces[i]->addSoftBody(psb);
706 int removed_index = -1;
707 for (
int i = 0; i < forces.
size(); ++i)
711 forces[i]->removeSoftBody(psb);
712 if (forces[i]->m_softBodies.
size() == 0)
717 if (removed_index >= 0)
724 for (
int i = 0; i < forces.
size(); ++i)
726 forces[i]->removeSoftBody(psb);
733 m_softBodies.
remove(body);
752 int numSimulationSubSteps = 0;
761 numSimulationSubSteps = int(
m_localTime / fixedTimeStep);
762 m_localTime -= numSimulationSubSteps * fixedTimeStep;
768 fixedTimeStep = timeStep;
773 numSimulationSubSteps = 0;
778 numSimulationSubSteps = 1;
784 if (getDebugDrawer())
789 if (numSimulationSubSteps)
792 int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps;
796 for (
int i = 0; i < clampedSimulationSteps; i++)
809 #ifndef BT_NO_PROFILE
810 CProfileManager::Increment_Frame_Counter();
813 return numSimulationSubSteps;
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble y2 _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat y2 _GL_VOID_RET _GL_VOID GLint GLint GLint y2 _GL_VOID_RET _GL_VOID GLshort GLshort GLshort y2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLuint *buffer _GL_VOID_RET _GL_VOID GLdouble t _GL_VOID_RET _GL_VOID GLfloat t _GL_VOID_RET _GL_VOID GLint t _GL_VOID_RET _GL_VOID GLshort t _GL_VOID_RET _GL_VOID GLdouble GLdouble r _GL_VOID_RET _GL_VOID GLfloat GLfloat r _GL_VOID_RET _GL_VOID GLint GLint r _GL_VOID_RET _GL_VOID GLshort GLshort r _GL_VOID_RET _GL_VOID GLdouble GLdouble r
#define DISABLE_DEACTIVATION
#define WANTS_DEACTIVATION
btCollisionShape
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
void updateActions(btScalar timeStep)
btSimulationIslandManager * m_islandManager
virtual void saveKinematicState(btScalar timeStep)
btAlignedObjectArray< btRigidBody * > m_nonStaticRigidBodies
virtual void synchronizeMotionStates()
virtual int getNumConstraints() const
btAlignedObjectArray< btTypedConstraint * > m_constraints
virtual void removeCollisionObject(btCollisionObject *collisionObject)
removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise ca...
btCollisionWorld * getCollisionWorld()
bool m_latencyMotionStateInterpolation
btConstraintSolver * m_constraintSolver
void startProfiling(btScalar timeStep)
btMatrix3x3 inverse() const
Return the inverse of the matrix.
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
bool gDisableDeactivation
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x)
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int const btContactSolverInfo & infoGlobal
static btMatrix3x3 OuterProduct(const btScalar *v1, const btScalar *v2, const btScalar *v3, const btScalar *u1, const btScalar *u2, const btScalar *u3, int ndof)
static SIMD_FORCE_INLINE btVector3 generateUnitOrthogonalVector(const btVector3 &u)
static SIMD_FORCE_INLINE void findJacobian(const btMultiBodyLinkCollider *multibodyLinkCol, btMultiBodyJacobianData &jacobianData, const btVector3 &contact_point, const btVector3 &dir)
btSoftBody implementation by Nathanael Presson
static btMatrix3x3 Diagonal(btScalar x)
btTypedConstraint(btTypedConstraintType type, btRigidBody &rbA)
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
SIMD_FORCE_INLINE btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
void removeAtIndex(int index)
void remove(const T &key)
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
void quickSort(const L &CompareFunc)
SIMD_FORCE_INLINE void push_back(const T &_Val)
virtual void removeCollisionObject(btCollisionObject *collisionObject)
virtual void addCollisionObject(btCollisionObject *collisionObject, int collisionFilterGroup=btBroadphaseProxy::DefaultFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter)
virtual void allSolved(const btContactSolverInfo &, class btIDebugDraw *)
virtual void solveDeformableConstraints(btScalar solverdt)
void setConstraints(const btContactSolverInfo &infoGlobal)
btDeformableBackwardEulerObjective * m_objective
void reinitialize(const btAlignedObjectArray< btSoftBody * > &softBodies, btScalar dt)
void setupDeformableSolve(bool implicit)
void setLineSearch(bool lineSearch)
virtual void predictMotion(btScalar solverdt)
void setImplicit(bool implicit)
void applyRigidBodyGravity(btScalar timeStep)
btDeformableMultiBodyDynamicsWorld(btDispatcher *dispatcher, btBroadphaseInterface *pairCache, btDeformableMultiBodyConstraintSolver *constraintSolver, btCollisionConfiguration *collisionConfiguration, btDeformableBodySolver *deformableBodySolver=0)
void removeCollisionObject(btCollisionObject *collisionObject)
virtual void addSoftBody(btSoftBody *body, int collisionFilterGroup=btBroadphaseProxy::DefaultFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter)
void removeSoftBodyForce(btSoftBody *psb)
void removeSoftBody(btSoftBody *body)
void reinitialize(btScalar timeStep)
void positionCorrection(btScalar timeStep)
virtual void predictUnconstraintMotion(btScalar timeStep)
void afterSolverCallbacks(btScalar timeStep)
void applyRepulsionForce(btScalar timeStep)
virtual void debugDrawWorld()
void solveConstraints(btScalar timeStep)
void addForce(btSoftBody *psb, btDeformableLagrangianForce *force)
void softBodySelfCollision()
btSoftBodyArray & getSoftBodyArray()
void performGeometricCollisions(btScalar timeStep)
virtual void integrateTransforms(btScalar timeStep)
void removeForce(btSoftBody *psb, btDeformableLagrangianForce *force)
virtual void internalSingleStepSimulation(btScalar timeStep)
void updateActivationState(btScalar timeStep)
virtual ~btDeformableMultiBodyDynamicsWorld()
void solveContactConstraints()
virtual int stepSimulation(btScalar timeStep, int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))
void beforeSolverCallbacks(btScalar timeStep)
void performDeformableCollisionDetection()
virtual int getDebugMode() const =0
btAlignedObjectArray< btMultiBodyConstraint * > m_multiBodyConstraints
virtual void clearForces()
virtual void updateActivationState(btScalar timeStep)
btAlignedObjectArray< btMatrix3x3 > m_scratch_m
btAlignedObjectArray< btVector3 > m_scratch_v
virtual void predictUnconstraintMotion(btScalar timeStep)
virtual void calculateSimulationIslands()
virtual void integrateTransforms(btScalar timeStep)
btAlignedObjectArray< btMultiBody * > m_multiBodies
btAlignedObjectArray< btScalar > m_scratch_r
btAlignedObjectArray< btMultiBodyConstraint * > m_sortedMultiBodyConstraints
virtual void clearMultiBodyConstraintForces()
virtual void applyGravity()
virtual void debugDrawWorld()
btMultiBody * m_multiBody
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
void integrateVelocities(btScalar step)
void setTurnVelocity(const btVector3 &v)
void setPushVelocity(const btVector3 &v)
btVector3 getPushVelocity() const
btVector3 getTurnVelocity() const
void processIslands(btDispatcher *dispatcher, btCollisionWorld *collisionWorld, IslandCallback *callback)
void buildIslands(btDispatcher *dispatcher, btCollisionWorld *colWorld)
void interpolateRenderMesh()
void defaultCollisionHandler(const btCollisionObjectWrapper *pcoWrap)
btSoftBodyWorldInfo * getWorldInfo()
btSoftBodyWorldInfo * m_worldInfo
void setSoftBodySolver(btSoftBodySolver *softBodySolver)
btAlignedObjectArray< DeformableFaceNodeContact > m_faceNodeContacts
void updateDeactivation(btScalar timeStep)
void applyRepulsionForce(btScalar timeStep, bool applySpringForce)
btAlignedObjectArray< DeformableNodeRigidAnchor > m_deformableAnchors
static const btSoftBody * upcast(const btCollisionObject *colObj)
IconTextureDrawCall normal
vec_base< T, 3 > cross(const vec_base< T, 3 > &a, const vec_base< T, 3 > &b)
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
virtual void processConstraints(int islandId=-1)
virtual SIMD_FORCE_INLINE void setup(btContactSolverInfo *solverInfo, btTypedConstraint **sortedConstraints, int numConstraints, btMultiBodyConstraint **sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw *debugDrawer)
class btIDebugDraw * m_debugDraw
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
btAlignedObjectArray< btScalar > m_jacobians
static void Draw(btSoftBody *psb, btIDebugDraw *idraw, int drawflags=fDrawFlags::Std)
static void DrawFrame(btSoftBody *psb, btIDebugDraw *idraw)
btDispatcher * m_dispatcher
btSparseSdf< 3 > m_sparsesdf
btScalar m_maxDisplacement
btBroadphaseInterface * m_broadphase
btScalar Evaluate(const btVector3 &x, const btCollisionShape *shape, btVector3 &normal, btScalar margin)
void setDefaultVoxelsz(btScalar sz)
void Initialize(int hashsize=2383, int clampCells=256 *1024)