Bullet Collision Detection & Physics Library
|
Go to the documentation of this file.
38 nonContactResidual = 0;
46 nonContactResidual =
btMax(nonContactResidual, residual * residual);
54 leastSquaredResidual =
btMax(leastSquaredResidual, nonContactResidual);
69 leastSquaredResidual =
btMax(leastSquaredResidual, residual * residual);
94 leastSquaredResidual =
btMax(leastSquaredResidual, residual * residual);
124 leastSquaredResidual =
btMax(leastSquaredResidual, residual * residual);
159 leastSquaredResidual =
btMax(leastSquaredResidual, residual * residual);
190 leastSquaredResidual =
btMax(leastSquaredResidual, residual * residual);
200 return leastSquaredResidual;
215 for (
int i = 0; i < numBodies; i++)
231 for (
int i = 0; i < ndof; ++i)
248 for (
int i = 0; i < ndofA; ++i)
260 for (
int i = 0; i < ndofB; ++i)
291 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
295 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
304 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
308 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
333 for (
int i = 0; i < ndofA; ++i)
345 for (
int i = 0; i < ndofB; ++i)
370 for (
int i = 0; i < ndofA; ++i)
382 for (
int i = 0; i < ndofB; ++i)
403 if (sumA < -sumAclipped)
408 else if (sumA > sumAclipped)
418 if (sumB < -sumBclipped)
423 else if (sumB > sumBclipped)
446 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
450 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
459 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
463 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
473 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
477 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
486 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
490 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
503 BT_PROFILE(
"setupMultiBodyContactConstraint");
524 relaxation = infoGlobal.
m_sor;
569 if (solverConstraint.
m_linkA < 0)
577 const int ndofA = multiBodyA->
getNumDofs() + 6;
616 if (solverConstraint.
m_linkB < 0)
625 const int ndofB = multiBodyB->
getNumDofs() + 6;
671 for (
int i = 0; i < ndofA; ++i)
688 const int ndofB = multiBodyB->
getNumDofs() + 6;
691 for (
int i = 0; i < ndofB; ++i)
744 for (
int i = 0; i < ndofA; ++i)
761 for (
int i = 0; i < ndofB; ++i)
789 btScalar velocityError = restitution - rel_vel;
792 positionalError = -distance * erp / infoGlobal.
m_timeStep;
799 velocityError -= distance / infoGlobal.
m_timeStep;
803 positionalError = -distance * erp / infoGlobal.
m_timeStep;
815 solverConstraint.
m_rhs = penetrationImpulse + velocityImpulse;
830 solverConstraint.
m_rhs = penetrationImpulse + velocityImpulse;
896 BT_PROFILE(
"setupMultiBodyRollingFrictionConstraint");
917 relaxation = infoGlobal.
m_sor;
923 if (solverConstraint.
m_linkA < 0)
931 const int ndofA = multiBodyA->
getNumDofs() + 6;
956 btVector3 torqueAxis0 = constraintNormal;
962 btVector3 torqueAxis0 = constraintNormal;
970 if (solverConstraint.
m_linkB < 0)
979 const int ndofB = multiBodyB->
getNumDofs() + 6;
998 btVector3 torqueAxis1 = -constraintNormal;
1004 btVector3 torqueAxis1 = -constraintNormal;
1024 for (
int i = 0; i < ndofA; ++i)
1041 const int ndofB = multiBodyB->
getNumDofs() + 6;
1044 for (
int i = 0; i < ndofB; ++i)
1086 for (
int i = 0; i < ndofA; ++i)
1101 for (
int i = 0; i < ndofB; ++i)
1113 solverConstraint.
m_friction = combinedTorsionalFriction;
1129 btScalar velocityError = 0 - rel_vel;
1133 solverConstraint.
m_rhs = velocityImpulse;
1142 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)
1144 BT_PROFILE(
"addMultiBodyFrictionConstraint");
1150 bool isFriction =
true;
1174 return solverConstraint;
1178 btScalar combinedTorsionalFriction,
1181 BT_PROFILE(
"addMultiBodyRollingFrictionConstraint");
1190 bool isFriction =
true;
1214 return solverConstraint;
1218 btScalar combinedTorsionalFriction,
1221 BT_PROFILE(
"addMultiBodyRollingFrictionConstraint");
1228 bool isFriction =
true;
1252 return solverConstraint;
1278 int rollingFriction = 4;
1308 bool isFriction =
false;
1315 #define ENABLE_FRICTION
1316 #ifdef ENABLE_FRICTION
1339 if (rollingFriction > 0)
1400 addMultiBodyFrictionConstraint(cp.
m_lateralFrictionDir1, cp.
m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.
m_contactMotion1, cp.
m_frictionCFM);
1403 addMultiBodyFrictionConstraint(cp.
m_lateralFrictionDir2, cp.
m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.
m_contactMotion2, cp.
m_frictionCFM);
1408 #endif //ENABLE_FRICTION
1421 for (
int i = 0; i < numManifolds; i++)
1508 forceVector.
resize(numDofsPlusBase);
1509 for (
int i=0;i<numDofsPlusBase;i++)
1511 forceVector[i] = data.
m_jacobians[jacIndex+i]*appliedImpulse;
1514 output.resize(numDofsPlusBase);
1515 bool applyJointFeedback =
true;
1574 #ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
1590 BT_PROFILE(
"btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
1595 for (
int i = 0; i < numPoolConstraints; i++)
1617 for (
int j = 0; j < numPoolConstraints; j++)
1641 for (
int j=0;j<numPoolConstraints;j++)
1712 for (
int i=0;i<numPoolConstraints;i++)
1740 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
@ BT_CONTACT_FLAG_HAS_CONTACT_CFM
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
@ CF_ANISOTROPIC_ROLLING_FRICTION
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)
@ BT_CONTACT_FLAG_FRICTION_ANCHOR
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
@ BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING
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)
@ BT_CONTACT_FLAG_HAS_CONTACT_ERP
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()
@ CF_ANISOTROPIC_FRICTION
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
@ BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED
int size() const
return the number of elements in the array
btScalar m_combinedContactStiffness1