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
299#ifdef 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
454#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
468#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
481#ifdef 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;
1137btMultiBodySolverConstraint&
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;
1169 return solverConstraint;
1173 btScalar combinedTorsionalFriction,
1176 BT_PROFILE(
"addMultiBodyRollingFrictionConstraint");
1185 bool isFriction =
true;
1209 return solverConstraint;
1213 btScalar combinedTorsionalFriction,
1216 BT_PROFILE(
"addMultiBodyRollingFrictionConstraint");
1223 bool isFriction =
true;
1247 return solverConstraint;
1273 int rollingFriction = 1;
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);
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++)
1735void 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)
@ BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED
@ BT_CONTACT_FLAG_HAS_CONTACT_ERP
@ BT_CONTACT_FLAG_HAS_CONTACT_CFM
@ BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING
@ BT_CONTACT_FLAG_FRICTION_ANCHOR
const T & btMax(const T &a, const T &b)
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btScalar btAtan2(btScalar x, btScalar y)
btScalar btSin(btScalar x)
btScalar btFabs(btScalar x)
btScalar btCos(btScalar x)
static T sum(const btAlignedObjectArray< T > &items)
#define btSimdScalar
Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later,...
void btPlaneSpace1(const T &n, T &p, T &q)
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
T & expandNonInitializing()
btCollisionObject can be used to manage collision detection objects.
@ CF_ANISOTROPIC_ROLLING_FRICTION
@ CF_ANISOTROPIC_FRICTION
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
ManifoldContactPoint collects and maintains persistent contactpoints.
btScalar m_combinedSpinningFriction
btScalar m_combinedRollingFriction
btScalar getDistance() const
btScalar m_combinedContactStiffness1
btScalar m_combinedRestitution
btVector3 m_lateralFrictionDir2
btScalar m_combinedContactDamping1
const btVector3 & getPositionWorldOnB() const
btScalar m_appliedImpulseLateral2
const btVector3 & getPositionWorldOnA() const
btScalar m_appliedImpulse
btScalar m_appliedImpulseLateral1
btVector3 m_normalWorldOnB
btScalar m_combinedFriction
btScalar m_contactMotion2
btVector3 m_lateralFrictionDir1
btScalar m_contactMotion1
btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint &c)
btMultiBodyConstraint ** m_tmpMultiBodyConstraints
void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint &constraint, btScalar deltaTime)
int m_tmpNumMultiBodyConstraints
btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints
void convertContacts(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
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)
void applyDeltaVee(btScalar *deltaV, btScalar impulse, int velocityIndex, int ndof)
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)
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
btMultiBodyConstraintArray m_multiBodySpinningFrictionContactConstraints
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
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)
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)
btMultiBodyJacobianData m_data
btScalar resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint &cA1, const btMultiBodySolverConstraint &cB)
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
void convertMultiBodyContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
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 setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint &solverConstraint, const btVector3 &contactNormal, btManifoldPoint &cp, btScalar combinedTorsionalFriction, const btContactSolverInfo &infoGlobal, btScalar &relaxation, bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0)
btMultiBodyConstraintArray m_multiBodyNonContactConstraints
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 void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)=0
btScalar getAppliedImpulse(int dof)
void internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
btMultiBody * m_multiBody
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
const btVector3 & getBasePos() const
int getCompanionId() const
void addBaseConstraintForce(const btVector3 &f)
void addLinkConstraintForce(int i, const btVector3 &f)
const btMultibodyLink & getLink(int index) const
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instea...
void setPosUpdated(bool updated)
void addLinkConstraintTorque(int i, const btVector3 &t)
bool internalNeedsJointFeedback() const
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
void addBaseConstraintTorque(const btVector3 &t)
void setCompanionId(int id)
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
const btScalar * getVelocityVector() const
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
const btManifoldPoint & getContactPoint(int index) const
const btCollisionObject * getBody0() const
const btCollisionObject * getBody1() const
int getNumContacts() const
btScalar getContactProcessingThreshold() const
The btRigidBody is the main class for rigid body objects.
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
const btVector3 & getTotalTorque() const
btScalar getInvMass() const
const btVector3 & getLinearFactor() const
const btVector3 & getTotalForce() const
const btVector3 & getAngularFactor() const
const btMatrix3x3 & getInvInertiaTensorWorld() const
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
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
static void applyAnisotropicFriction(btCollisionObject *colObj, btVector3 &frictionDirection, int frictionMode)
void convertContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
btAlignedObjectArray< btSolverBody > m_tmpSolverBodyPool
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold)
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
TypedConstraint is the baseclass for Bullet constraints and vehicles.
void setEnabled(bool enabled)
void internalSetAppliedImpulse(btScalar appliedImpulse)
internal method used by the constraint solver, don't use them directly
const btRigidBody & getRigidBodyA() const
btScalar getBreakingImpulseThreshold() const
const btRigidBody & getRigidBodyB() const
const btJointFeedback * getJointFeedback() const
btVector3 can be used to represent 3D points and vectors.
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
btScalar dot(const btVector3 &v) const
Return the dot product.
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
btVector3 m_appliedForceBodyA
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
btAlignedObjectArray< btScalar > m_deltaVelocities
btAlignedObjectArray< btScalar > m_jacobians
btAlignedObjectArray< btSolverBody > * m_solverBodyPool
btAlignedObjectArray< btScalar > scratch_r
btAlignedObjectArray< btMatrix3x3 > scratch_m
btAlignedObjectArray< btVector3 > scratch_v
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btMultiBodyConstraint * m_orgConstraint
btVector3 m_relpos1CrossNormal
void * m_originalContactPoint
btVector3 m_contactNormal2
btVector3 m_contactNormal1
btMultiBody * m_multiBodyB
btVector3 m_angularComponentA
btVector3 m_relpos2CrossNormal
btSimdScalar m_appliedImpulse
btSimdScalar m_appliedPushImpulse
btScalar m_rhsPenetration
btMultiBody * m_multiBodyA
btVector3 m_angularComponentB
btTransform m_cachedWorldTransform
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
btVector3 & internalGetDeltaAngularVelocity()
btVector3 m_angularVelocity
btRigidBody * m_originalBody
btVector3 m_linearVelocity
btVector3 & internalGetDeltaLinearVelocity()
some internal methods, don't use them
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
const btTransform & getWorldTransform() const
const btVector3 & internalGetInvMass() const
btVector3 m_externalForceImpulse