71 if (constraint->isEnabled())
73 const btRigidBody* colObj0 = &constraint->getRigidBodyA();
74 const btRigidBody* colObj1 = &constraint->getRigidBodyB();
92 for (
int b = 0;
b < body->getNumLinks();
b++)
99 int tagPrev =
prev->getIslandTag();
100 int tagCur = cur->getIslandTag();
103 if (cur && !cur->isStaticOrKinematicObject())
114 int tagA =
c->getIslandIdA();
115 int tagB =
c->getIslandIdB();
116 if (tagA >= 0 && tagB >= 0)
127 BT_PROFILE(
"btMultiBodyDynamicsWorld::updateActivationState");
134 body->checkMotionAndSleepIfRequired(timeStep);
135 if (!body->isAwake())
141 col->setDeactivationTime(0.f);
143 for (
int b = 0;
b < body->getNumLinks();
b++)
149 col->setDeactivationTime(0.f);
159 for (
int b = 0;
b < body->getNumLinks();
b++)
179 m_multiBodyConstraintSolver(constraintSolver)
239 bool isSleeping =
false;
241 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() ==
ISLAND_SLEEPING)
245 for (
int b = 0;
b < bod->getNumLinks();
b++)
247 if (bod->getLink(
b).m_collider && bod->getLink(
b).m_collider->getActivationState() ==
ISLAND_SLEEPING)
258 if (bod->internalNeedsJointFeedback())
260 if (!bod->isUsingRK4Integration())
262 if (bod->internalNeedsJointFeedback())
264 bool isConstraintPass =
true;
266 getSolverInfo().m_jointFeedbackInWorldSpace,
267 getSolverInfo().m_jointFeedbackInJointFrame);
277 bod->processDeltaVeeMultiDof2();
310 #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
317 bool isSleeping =
false;
319 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() ==
ISLAND_SLEEPING)
323 for (
int b = 0;
b < bod->getNumLinks();
b++)
325 if (bod->getLink(
b).m_collider && bod->getLink(
b).m_collider->getActivationState() ==
ISLAND_SLEEPING)
336 bod->addBaseForce(
m_gravity * bod->getBaseMass());
338 for (
int j = 0; j < bod->getNumLinks(); ++j)
340 bod->addLinkForce(j,
m_gravity * bod->getLinkMass(j));
353 bool isSleeping =
false;
355 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() ==
ISLAND_SLEEPING)
359 for (
int b = 0;
b < bod->getNumLinks();
b++)
361 if (bod->getLink(
b).m_collider && bod->getLink(
b).m_collider->getActivationState() ==
ISLAND_SLEEPING)
371 bool doNotUpdatePos =
false;
372 bool isConstraintPass =
false;
374 if (!bod->isUsingRK4Integration())
376 bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.
m_timeStep,
378 getSolverInfo().m_jointFeedbackInWorldSpace,
379 getSolverInfo().m_jointFeedbackInJointFrame);
384 int numDofs = bod->getNumDofs() + 6;
385 int numPosVars = bod->getNumPosVars() + 7;
387 scratch_r2.
resize(2 * numPosVars + 8 * numDofs);
410 btAssert((pMem - (2 * numPosVars + 8 * numDofs)) == &scratch_r2[0]);
414 scratch_q0[0] = bod->getWorldToBaseRot().x();
415 scratch_q0[1] = bod->getWorldToBaseRot().y();
416 scratch_q0[2] = bod->getWorldToBaseRot().z();
417 scratch_q0[3] = bod->getWorldToBaseRot().w();
418 scratch_q0[4] = bod->getBasePos().x();
419 scratch_q0[5] = bod->getBasePos().y();
420 scratch_q0[6] = bod->getBasePos().z();
422 for (
int link = 0; link < bod->getNumLinks(); ++link)
424 for (
int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
425 scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
428 for (
int dof = 0; dof < numDofs; ++dof)
429 scratch_qd0[dof] = bod->getVelocityVector()[dof];
438 for (
int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
439 scratch_qx[dof] = scratch_q0[dof];
441 } pResetQx = {bod, scratch_qx, scratch_q0};
447 for (
int i = 0; i <
size; ++i)
448 pVal[i] = pCurVal[i] + dt * pDer[i];
459 for (
int i = 0; i < pBody->getNumDofs() + 6; ++i)
462 } pCopyToVelocityVector;
468 for (
int i = 0; i <
size; ++i)
469 pDst[i] = pSrc[start + i];
475 #define output &m_scratch_r[bod->getNumDofs()]
478 isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
479 getSolverInfo().m_jointFeedbackInJointFrame);
480 pCopy(
output, scratch_qdd0, 0, numDofs);
483 bod->stepPositionsMultiDof(
btScalar(.5) * h, scratch_qx, scratch_qd0);
485 pEulerIntegrate(
btScalar(.5) * h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
488 pCopyToVelocityVector(bod, scratch_qd1);
490 isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
491 getSolverInfo().m_jointFeedbackInJointFrame);
492 pCopy(
output, scratch_qdd1, 0, numDofs);
495 bod->stepPositionsMultiDof(
btScalar(.5) * h, scratch_qx, scratch_qd1);
497 pEulerIntegrate(
btScalar(.5) * h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
500 pCopyToVelocityVector(bod, scratch_qd2);
502 isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
503 getSolverInfo().m_jointFeedbackInJointFrame);
504 pCopy(
output, scratch_qdd2, 0, numDofs);
507 bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
509 pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
512 pCopyToVelocityVector(bod, scratch_qd3);
514 isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
515 getSolverInfo().m_jointFeedbackInJointFrame);
516 pCopy(
output, scratch_qdd3, 0, numDofs);
525 for (
int i = 0; i < numDofs; ++i)
527 delta_q[i] = h /
btScalar(6.) * (scratch_qd0[i] + 2 * scratch_qd1[i] + 2 * scratch_qd2[i] + scratch_qd3[i]);
528 delta_qd[i] = h /
btScalar(6.) * (scratch_qdd0[i] + 2 * scratch_qdd1[i] + 2 * scratch_qdd2[i] + scratch_qdd3[i]);
533 pCopyToVelocityVector(bod, scratch_qd0);
534 bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
539 pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
541 for (
int i = 0; i < numDofs; ++i)
542 pRealBuf[i] = delta_q[i];
545 bod->setPosUpdated(
true);
550 for (
int link = 0; link < bod->getNumLinks(); ++link)
551 bod->getLink(link).updateCacheMultiDof();
553 isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
554 getSolverInfo().m_jointFeedbackInJointFrame);
559 #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
560 bod->clearForcesAndTorques();
582 bool isSleeping =
false;
583 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() ==
ISLAND_SLEEPING)
587 for (
int b = 0;
b < bod->getNumLinks();
b++)
589 if (bod->getLink(
b).m_collider && bod->getLink(
b).m_collider->getActivationState() ==
ISLAND_SLEEPING)
596 int nLinks = bod->getNumLinks();
599 if (!bod->isPosUpdated())
600 bod->stepPositionsMultiDof(timeStep);
604 pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
606 bod->stepPositionsMultiDof(1, 0, pRealBuf);
607 bod->setPosUpdated(
false);
614 bod->substractSplitV();
618 bod->clearVelocities();
631 bool isSleeping =
false;
632 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() ==
ISLAND_SLEEPING)
636 for (
int b = 0;
b < bod->getNumLinks();
b++)
638 if (bod->getLink(
b).m_collider && bod->getLink(
b).m_collider->getActivationState() ==
ISLAND_SLEEPING)
644 int nLinks = bod->getNumLinks();
645 bod->predictPositionsMultiDof(timeStep);
652 bod->clearVelocities();
669 constraint->debugDraw(getDebugDrawer());
674 BT_PROFILE(
"btMultiBodyDynamicsWorld debugDrawWorld");
678 bool drawConstraints =
false;
679 if (getDebugDrawer())
681 int mode = getDebugDrawer()->getDebugMode();
684 drawConstraints =
true;
704 getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
707 for (
int m = 0; m < bod->getNumLinks(); m++)
709 const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
712 getDebugDrawer()->drawTransform(tr, 0.1);
722 getDebugDrawer()->drawLine(
from, to,
color);
731 getDebugDrawer()->drawLine(
from, to,
color);
740 getDebugDrawer()->drawLine(
from, to,
color);
751 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
757 bool isSleeping =
false;
759 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() ==
ISLAND_SLEEPING)
763 for (
int b = 0;
b < bod->getNumLinks();
b++)
765 if (bod->getLink(
b).m_collider && bod->getLink(
b).m_collider->getActivationState() ==
ISLAND_SLEEPING)
771 bod->addBaseForce(
m_gravity * bod->getBaseMass());
773 for (
int j = 0; j < bod->getNumLinks(); ++j)
775 bod->addLinkForce(j,
m_gravity * bod->getLinkMass(j));
787 bod->clearConstraintForces();
798 bool isSleeping =
false;
800 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() ==
ISLAND_SLEEPING)
804 for (
int b = 0;
b < bod->getNumLinks();
b++)
806 if (bod->getLink(
b).m_collider && bod->getLink(
b).m_collider->getActivationState() ==
ISLAND_SLEEPING)
813 bod->clearForcesAndTorques();
822 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
837 serializeCollisionObjects(serializer);
839 serializeContactManifolds(serializer);
852 int len = mb->calculateSerializeBufferSize();
854 const char* structType = mb->serialize(chunk->
m_oldPtr, serializer);
860 for (i = 0; i < m_collisionObjects.size(); i++)
865 int len = colObj->calculateSerializeBufferSize();
867 const char* structType = colObj->serialize(chunk->
m_oldPtr, serializer);
Group Output data from inside of a node group A color picker Mix two input colors RGB to Convert a color s luminance to a grayscale value Generate a normal vector and a dot product Bright Control the brightness and contrast of the input color Vector Map an input vectors to used to fine tune the interpolation of the input Camera Retrieve information about the camera and how it relates to the current shading point s position Clamp a value between a minimum and a maximum Vector Perform vector math operation Invert a color
SIMD_FORCE_INLINE bool isStaticOrKinematicObject() const
#define DISABLE_DEACTIVATION
#define WANTS_DEACTIVATION
SIMD_FORCE_INLINE int getIslandTag() const
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
virtual void clearForces()
the forces on each rigidbody is accumulating together with gravity. clear this after each timestep.
btSimulationIslandManager * m_islandManager
void serializeRigidBodies(btSerializer *serializer)
void serializeDynamicsWorldInfo(btSerializer *serializer)
virtual void updateActivationState(btScalar timeStep)
btDiscreteDynamicsWorld(btDispatcher *dispatcher, btBroadphaseInterface *pairCache, btConstraintSolver *constraintSolver, btCollisionConfiguration *collisionConfiguration)
this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete thos...
virtual int getNumConstraints() const
virtual void debugDrawWorld()
virtual void predictUnconstraintMotion(btScalar timeStep)
btAlignedObjectArray< btTypedConstraint * > m_constraints
btAlignedObjectArray< btPersistentManifold * > m_predictiveManifolds
virtual void integrateTransforms(btScalar timeStep)
btSimulationIslandManager * getSimulationIslandManager()
btCollisionWorld * getCollisionWorld()
virtual void setConstraintSolver(btConstraintSolver *solver)
btConstraintSolver * m_constraintSolver
virtual void applyGravity()
apply gravity, call this once per timestep
SIMD_FORCE_INLINE btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int numConstraints
#define BT_MULTIBODY_CODE
#define BT_MB_LINKCOLLIDER_CODE
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 int size() const
return the number of elements in the array
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 btConstraintSolverType getSolverType() const =0
virtual void allSolved(const btContactSolverInfo &, class btIDebugDraw *)
virtual void prepareSolve(int, int)
@ DBG_DrawConstraintLimits
btAlignedObjectArray< btMultiBodyConstraint * > m_multiBodyConstraints
virtual void clearForces()
virtual void solveInternalConstraints(btContactSolverInfo &solverInfo)
virtual void updateActivationState(btScalar timeStep)
btAlignedObjectArray< btQuaternion > m_scratch_world_to_local
btAlignedObjectArray< btQuaternion > m_scratch_world_to_local1
btAlignedObjectArray< btMatrix3x3 > m_scratch_m
MultiBodyInplaceSolverIslandCallback * m_solverMultiBodyIslandCallback
virtual void serialize(btSerializer *serializer)
virtual ~btMultiBodyDynamicsWorld()
btAlignedObjectArray< btVector3 > m_scratch_local_origin1
virtual void serializeMultiBodies(btSerializer *serializer)
virtual void addMultiBodyConstraint(btMultiBodyConstraint *constraint)
btAlignedObjectArray< btVector3 > m_scratch_v
virtual void predictUnconstraintMotion(btScalar timeStep)
virtual void calculateSimulationIslands()
virtual void removeMultiBodyConstraint(btMultiBodyConstraint *constraint)
void predictMultiBodyTransforms(btScalar timeStep)
virtual void integrateTransforms(btScalar timeStep)
btMultiBodyConstraintSolver * m_multiBodyConstraintSolver
btAlignedObjectArray< btMultiBody * > m_multiBodies
virtual void getAnalyticsData(btAlignedObjectArray< struct btSolverAnalyticsData > &m_islandAnalyticsData) const
virtual void solveConstraints(btContactSolverInfo &solverInfo)
void integrateMultiBodyTransforms(btScalar timeStep)
virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint *constraint)
virtual void solveExternalForces(btContactSolverInfo &solverInfo)
virtual void setConstraintSolver(btConstraintSolver *solver)
btAlignedObjectArray< btVector3 > m_scratch_local_origin
virtual void clearMultiBodyForces()
btAlignedObjectArray< btScalar > m_scratch_r
virtual void removeMultiBody(btMultiBody *body)
btAlignedObjectArray< btMultiBodyConstraint * > m_sortedMultiBodyConstraints
virtual void addMultiBody(btMultiBody *body, int group=btBroadphaseProxy::DefaultFilter, int mask=btBroadphaseProxy::AllFilter)
btMultiBodyDynamicsWorld(btDispatcher *dispatcher, btBroadphaseInterface *pairCache, btMultiBodyConstraintSolver *constraintSolver, btCollisionConfiguration *collisionConfiguration)
virtual void clearMultiBodyConstraintForces()
virtual void applyGravity()
virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver *solver)
virtual void debugDrawWorld()
virtual btChunk * allocate(size_t size, int numElements)=0
virtual void finishSerialization()=0
virtual void startSerialization()=0
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
virtual void storeIslandActivationState(btCollisionWorld *world)
btUnionFind & getUnionFind()
virtual void updateActivationState(btCollisionWorld *colWorld, btDispatcher *dispatcher)
void buildAndProcessIslands(btDispatcher *dispatcher, btCollisionWorld *collisionWorld, IslandCallback *callback)
ccl_device_inline float4 mask(const int4 &mask, const float4 &a)
SymEdge< T > * prev(const SymEdge< T > *se)
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)
void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver *solver)
btAlignedObjectArray< btSolverAnalyticsData > m_islandAnalyticsData