Bullet Collision Detection & Physics Library
|
Go to the documentation of this file.
43 leastSquaredResidual =
btMax(leastSquaredResidual, residual * residual);
64 leastSquaredResidual =
btMax(leastSquaredResidual, residual * residual);
89 leastSquaredResidual =
btMax(leastSquaredResidual, residual * residual);
119 leastSquaredResidual =
btMax(leastSquaredResidual, residual * residual);
154 leastSquaredResidual =
btMax(leastSquaredResidual, residual * residual);
185 leastSquaredResidual =
btMax(leastSquaredResidual, residual * residual);
195 return leastSquaredResidual;
210 for (
int i = 0; i < numBodies; i++)
226 for (
int i = 0; i < ndof; ++i)
243 for (
int i = 0; i < ndofA; ++i)
255 for (
int i = 0; i < ndofB; ++i)
286 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
290 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
299 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
303 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
328 for (
int i = 0; i < ndofA; ++i)
340 for (
int i = 0; i < ndofB; ++i)
365 for (
int i = 0; i < ndofA; ++i)
377 for (
int i = 0; i < ndofB; ++i)
398 if (sumA < -sumAclipped)
403 else if (sumA > sumAclipped)
413 if (sumB < -sumBclipped)
418 else if (sumB > sumBclipped)
441 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
445 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
454 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
458 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
468 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
472 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
481 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
485 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
498 BT_PROFILE(
"setupMultiBodyContactConstraint");
519 relaxation = infoGlobal.
m_sor;
564 if (solverConstraint.
m_linkA < 0)
572 const int ndofA = multiBodyA->
getNumDofs() + 6;
611 if (solverConstraint.
m_linkB < 0)
620 const int ndofB = multiBodyB->
getNumDofs() + 6;
666 for (
int i = 0; i < ndofA; ++i)
683 const int ndofB = multiBodyB->
getNumDofs() + 6;
686 for (
int i = 0; i < ndofB; ++i)
739 for (
int i = 0; i < ndofA; ++i)
756 for (
int i = 0; i < ndofB; ++i)
784 btScalar velocityError = restitution - rel_vel;
787 positionalError = -distance * erp / infoGlobal.
m_timeStep;
794 velocityError -= distance / infoGlobal.
m_timeStep;
798 positionalError = -distance * erp / infoGlobal.
m_timeStep;
810 solverConstraint.
m_rhs = penetrationImpulse + velocityImpulse;
825 solverConstraint.
m_rhs = penetrationImpulse + velocityImpulse;
891 BT_PROFILE(
"setupMultiBodyRollingFrictionConstraint");
912 relaxation = infoGlobal.
m_sor;
918 if (solverConstraint.
m_linkA < 0)
926 const int ndofA = multiBodyA->
getNumDofs() + 6;
951 btVector3 torqueAxis0 = constraintNormal;
957 btVector3 torqueAxis0 = constraintNormal;
965 if (solverConstraint.
m_linkB < 0)
974 const int ndofB = multiBodyB->
getNumDofs() + 6;
993 btVector3 torqueAxis1 = -constraintNormal;
999 btVector3 torqueAxis1 = -constraintNormal;
1019 for (
int i = 0; i < ndofA; ++i)
1036 const int ndofB = multiBodyB->
getNumDofs() + 6;
1039 for (
int i = 0; i < ndofB; ++i)
1081 for (
int i = 0; i < ndofA; ++i)
1096 for (
int i = 0; i < ndofB; ++i)
1108 solverConstraint.
m_friction = combinedTorsionalFriction;
1124 btScalar velocityError = 0 - rel_vel;
1128 solverConstraint.
m_rhs = velocityImpulse;
1137 btMultiBodySolverConstraint&
btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(
const btVector3& normalAxis,
const btScalar& appliedImpulse,
btPersistentManifold* manifold,
int frictionIndex,
btManifoldPoint& cp,
btCollisionObject* colObj0,
btCollisionObject* colObj1,
btScalar relaxation,
const btContactSolverInfo& infoGlobal,
btScalar desiredVelocity,
btScalar cfmSlip)
1139 BT_PROFILE(
"addMultiBodyFrictionConstraint");
1145 bool isFriction =
true;
1164 solverConstraint.
m_linkB = fcB->m_link;
1169 return solverConstraint;
1173 btScalar combinedTorsionalFriction,
1176 BT_PROFILE(
"addMultiBodyRollingFrictionConstraint");
1185 bool isFriction =
true;
1204 solverConstraint.
m_linkB = fcB->m_link;
1209 return solverConstraint;
1213 btScalar combinedTorsionalFriction,
1216 BT_PROFILE(
"addMultiBodyRollingFrictionConstraint");
1223 bool isFriction =
true;
1242 solverConstraint.
m_linkB = fcB->m_link;
1247 return solverConstraint;
1273 int rollingFriction = 1;
1299 solverConstraint.
m_linkB = fcB->m_link;
1303 bool isFriction =
false;
1310 #define ENABLE_FRICTION
1311 #ifdef ENABLE_FRICTION
1334 if (rollingFriction > 0)
1395 addMultiBodyFrictionConstraint(cp.
m_lateralFrictionDir1, cp.
m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.
m_contactMotion1, cp.
m_frictionCFM);
1398 addMultiBodyFrictionConstraint(cp.
m_lateralFrictionDir2, cp.
m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.
m_contactMotion2, cp.
m_frictionCFM);
1403 #endif //ENABLE_FRICTION
1416 for (
int i = 0; i < numManifolds; i++)
1503 forceVector.
resize(numDofsPlusBase);
1504 for (
int i=0;i<numDofsPlusBase;i++)
1506 forceVector[i] = data.
m_jacobians[jacIndex+i]*appliedImpulse;
1509 output.resize(numDofsPlusBase);
1510 bool applyJointFeedback =
true;
1569 #ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
1585 BT_PROFILE(
"btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
1590 for (
int i = 0; i < numPoolConstraints; i++)
1612 for (
int j = 0; j < numPoolConstraints; j++)
1636 for (
int j=0;j<numPoolConstraints;j++)
1707 for (
int i=0;i<numPoolConstraints;i++)
1735 void btMultiBodyConstraintSolver::solveMultiBodyGroup(
btCollisionObject** bodies,
int numBodies,
btPersistentManifold** manifold,
int numManifolds,
btTypedConstraint** constraints,
int numConstraints,
btMultiBodyConstraint** multiBodyConstraints,
int numMultiBodyConstraints,
const btContactSolverInfo& info,
btIDebugDraw* debugDrawer,
btDispatcher* dispatcher)
void internalSetAppliedImpulse(btScalar appliedImpulse)
internal method used by the constraint solver, don't use them directly
btVector3 m_relpos1CrossNormal
void convertMultiBodyContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
TypedConstraint is the baseclass for Bullet constraints and vehicles.
btAlignedObjectArray< btVector3 > scratch_v
btCollisionObject can be used to manage collision detection objects.
btSimdScalar m_appliedPushImpulse
btRigidBody * m_originalBody
The btRigidBody is the main class for rigid body objects.
btVector3 m_lateralFrictionDir2
btScalar m_contactMotion2
const btVector3 & getTotalTorque() const
void addBaseConstraintTorque(const btVector3 &t)
btVector3 m_linearVelocity
virtual btScalar solveGroup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifold, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &info, btIDebugDraw *debugDrawer, btDispatcher *dispatcher)
btSequentialImpulseConstraintSolver Sequentially applies impulses
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
void btPlaneSpace1(const T &n, T &p, T &q)
void addLinkConstraintForce(int i, const btVector3 &f)
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
btVector3 m_angularComponentB
const btVector3 & internalGetInvMass() const
int getCompanionId() const
btMultiBodyConstraint * m_orgConstraint
const btVector3 & getPositionWorldOnA() const
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
const btRigidBody & getRigidBodyA() const
bool internalNeedsJointFeedback() const
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
btScalar getBreakingImpulseThreshold() const
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
btAlignedObjectArray< btScalar > m_jacobians
void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint &constraint, btScalar deltaTime)
btVector3 m_normalWorldOnB
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
btScalar m_rhsPenetration
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold)
const btCollisionObject * getBody0() const
btVector3 m_angularComponentA
static void applyAnisotropicFriction(btCollisionObject *colObj, btVector3 &frictionDirection, int frictionMode)
void internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
const btJointFeedback * getJointFeedback() const
btVector3 m_externalForceImpulse
btScalar dot(const btVector3 &v) const
Return the dot product.
int getNumContacts() const
void addLinkConstraintTorque(int i, const btVector3 &t)
void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint &solverConstraint, const btVector3 &contactNormal, btManifoldPoint &cp, btScalar combinedTorsionalFriction, const btContactSolverInfo &infoGlobal, btScalar &relaxation, bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0)
btMultiBodySolverConstraint & addMultiBodyFrictionConstraint(const btVector3 &normalAxis, const btScalar &appliedImpulse, btPersistentManifold *manifold, int frictionIndex, btManifoldPoint &cp, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0)
btMultiBodyJacobianData m_data
btMultiBodyConstraintArray m_multiBodySpinningFrictionContactConstraints
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
btScalar getDistance() const
btScalar m_combinedRestitution
btMultiBodySolverConstraint & addMultiBodyTorsionalFrictionConstraint(const btVector3 &normalAxis, btPersistentManifold *manifold, int frictionIndex, btManifoldPoint &cp, btScalar combinedTorsionalFriction, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0)
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
virtual btScalar solveGroup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifold, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &info, btIDebugDraw *debugDrawer, btDispatcher *dispatcher)
this method should not be called, it was just used during porting/integration of Featherstone btMulti...
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
void applyDeltaVee(btScalar *deltaV, btScalar impulse, int velocityIndex, int ndof)
const T & btMax(const T &a, const T &b)
btTransform m_cachedWorldTransform
const btRigidBody & getRigidBodyB() const
btVector3 m_angularVelocity
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
btMultiBody * m_multiBodyB
btScalar btSin(btScalar x)
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
const btVector3 & getLinearFactor() const
btScalar btFabs(btScalar x)
btVector3 m_appliedForceBodyA
btScalar m_combinedContactDamping1
int m_tmpNumMultiBodyConstraints
#define btSimdScalar
Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later,...
void convertContacts(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
ManifoldContactPoint collects and maintains persistent contactpoints.
btScalar m_contactMotion1
btScalar m_combinedSpinningFriction
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instea...
void resize(int newsize, const T &fillData=T())
btScalar getContactProcessingThreshold() const
const btVector3 & getAngularFactor() const
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
btScalar btCos(btScalar x)
btVector3 & internalGetDeltaAngularVelocity()
btScalar getInvMass() const
btAlignedObjectArray< btScalar > scratch_r
const btMultibodyLink & getLink(int index) const
btVector3 & internalGetDeltaLinearVelocity()
some internal methods, don't use them
btVector3 can be used to represent 3D points and vectors.
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)=0
const btVector3 & getPositionWorldOnB() const
btScalar m_combinedRollingFriction
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
void convertContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
void setCompanionId(int id)
btMultiBody * m_multiBodyA
btMultiBodyConstraintArray m_multiBodyNonContactConstraints
btScalar m_appliedImpulseLateral1
btAlignedObjectArray< btScalar > m_deltaVelocities
void * m_originalContactPoint
btScalar getAppliedImpulse(int dof)
btAlignedObjectArray< btSolverBody > * m_solverBodyPool
void setPosUpdated(bool updated)
void setupMultiBodyContactConstraint(btMultiBodySolverConstraint &solverConstraint, const btVector3 &contactNormal, const btScalar &appliedImpulse, btManifoldPoint &cp, const btContactSolverInfo &infoGlobal, btScalar &relaxation, bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0)
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
btSimdScalar m_appliedImpulse
btAlignedObjectArray< btSolverBody > m_tmpSolverBodyPool
btVector3 m_contactNormal1
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
btMultiBodyConstraint ** m_tmpMultiBodyConstraints
const btTransform & getWorldTransform() const
const btScalar * getVelocityVector() const
void setEnabled(bool enabled)
const btMatrix3x3 & getInvInertiaTensorWorld() const
void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
void addBaseConstraintForce(const btVector3 &f)
const btManifoldPoint & getContactPoint(int index) const
btMultiBody * m_multiBody
btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints
btScalar btAtan2(btScalar x, btScalar y)
btVector3 m_lateralFrictionDir1
const btCollisionObject * getBody1() const
btScalar m_appliedImpulse
btVector3 m_relpos2CrossNormal
const btVector3 & getTotalForce() const
const btVector3 & getBasePos() const
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint &c)
T & expandNonInitializing()
btMultiBodySolverConstraint & addMultiBodySpinningFrictionConstraint(const btVector3 &normalAxis, btPersistentManifold *manifold, int frictionIndex, btManifoldPoint &cp, btScalar combinedTorsionalFriction, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0)
btVector3 m_contactNormal2
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
btAlignedObjectArray< btMatrix3x3 > scratch_m
static T sum(const btAlignedObjectArray< T > &items)
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
btScalar m_appliedImpulseLateral2
btScalar resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint &cA1, const btMultiBodySolverConstraint &cB)
virtual void solveMultiBodyGroup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifold, int numManifolds, btTypedConstraint **constraints, int numConstraints, btMultiBodyConstraint **multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo &info, btIDebugDraw *debugDrawer, btDispatcher *dispatcher)
void fillConstraintJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal_ang, const btVector3 &normal_lin, btScalar *jac, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m) const
btScalar m_combinedFriction
int size() const
return the number of elements in the array
btScalar m_combinedContactStiffness1