41 leastSquaredResidual =
btMax(leastSquaredResidual, residual * residual);
62 leastSquaredResidual =
btMax(leastSquaredResidual, residual * residual);
87 leastSquaredResidual =
btMax(leastSquaredResidual, residual * residual);
117 leastSquaredResidual =
btMax(leastSquaredResidual, residual * residual);
148 leastSquaredResidual =
btMax(leastSquaredResidual, residual * residual);
158 return leastSquaredResidual;
172 for (
int i = 0; i < numBodies; i++)
188 for (
int i = 0; i < ndof; ++i)
205 for (
int i = 0; i < ndofA; ++i)
217 for (
int i = 0; i < ndofB; ++i)
248 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 252 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 261 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 265 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 290 for (
int i = 0; i < ndofA; ++i)
302 for (
int i = 0; i < ndofB; ++i)
327 for (
int i = 0; i < ndofA; ++i)
339 for (
int i = 0; i < ndofB; ++i)
360 if (sumA < -sumAclipped)
365 else if (sumA > sumAclipped)
375 if (sumB < -sumBclipped)
380 else if (sumB > sumBclipped)
403 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 407 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 416 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 420 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 430 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 434 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 443 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 447 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 464 BT_PROFILE(
"setupMultiBodyContactConstraint");
485 relaxation = infoGlobal.
m_sor;
530 if (solverConstraint.
m_linkA < 0)
538 const int ndofA = multiBodyA->
getNumDofs() + 6;
577 if (solverConstraint.
m_linkB < 0)
586 const int ndofB = multiBodyB->
getNumDofs() + 6;
632 for (
int i = 0; i < ndofA; ++i)
649 const int ndofB = multiBodyB->
getNumDofs() + 6;
652 for (
int i = 0; i < ndofB; ++i)
705 for (
int i = 0; i < ndofA; ++i)
722 for (
int i = 0; i < ndofB; ++i)
792 btScalar velocityError = restitution - rel_vel;
795 positionalError = -distance * erp / infoGlobal.
m_timeStep;
802 velocityError -= distance / infoGlobal.
m_timeStep;
806 positionalError = -distance * erp / infoGlobal.
m_timeStep;
818 solverConstraint.
m_rhs = penetrationImpulse + velocityImpulse;
833 solverConstraint.
m_rhs = penetrationImpulse + velocityImpulse;
851 BT_PROFILE(
"setupMultiBodyRollingFrictionConstraint");
872 relaxation = infoGlobal.
m_sor;
878 if (solverConstraint.
m_linkA < 0)
886 const int ndofA = multiBodyA->
getNumDofs() + 6;
911 btVector3 torqueAxis0 = -constraintNormal;
917 btVector3 torqueAxis0 = -constraintNormal;
925 if (solverConstraint.
m_linkB < 0)
934 const int ndofB = multiBodyB->
getNumDofs() + 6;
953 btVector3 torqueAxis1 = constraintNormal;
959 btVector3 torqueAxis1 = constraintNormal;
979 for (
int i = 0; i < ndofA; ++i)
996 const int ndofB = multiBodyB->
getNumDofs() + 6;
999 for (
int i = 0; i < ndofB; ++i)
1041 for (
int i = 0; i < ndofA; ++i)
1056 for (
int i = 0; i < ndofB; ++i)
1068 solverConstraint.
m_friction = combinedTorsionalFriction;
1084 btScalar velocityError = 0 - rel_vel;
1088 solverConstraint.
m_rhs = velocityImpulse;
1099 BT_PROFILE(
"addMultiBodyFrictionConstraint");
1105 bool isFriction =
true;
1124 solverConstraint.
m_linkB = fcB->m_link;
1129 return solverConstraint;
1133 btScalar combinedTorsionalFriction,
1136 BT_PROFILE(
"addMultiBodyRollingFrictionConstraint");
1145 bool isFriction =
true;
1164 solverConstraint.
m_linkB = fcB->m_link;
1169 return solverConstraint;
1196 int rollingFriction = 1;
1222 solverConstraint.
m_linkB = fcB->m_link;
1226 bool isFriction =
false;
1233 #define ENABLE_FRICTION 1234 #ifdef ENABLE_FRICTION 1257 if (rollingFriction > 0)
1332 #endif //ENABLE_FRICTION 1341 for (
int i = 0; i < numManifolds; i++)
1383 forceVector.
resize(numDofsPlusBase);
1384 for (
int i=0;i<numDofsPlusBase;i++)
1386 forceVector[i] = data.
m_jacobians[jacIndex+i]*appliedImpulse;
1389 output.resize(numDofsPlusBase);
1390 bool applyJointFeedback =
true;
1449 #ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 1465 BT_PROFILE(
"btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
1470 for (
int i = 0; i < numPoolConstraints; i++)
1492 for (
int j = 0; j < numPoolConstraints; j++)
1516 for (
int j=0;j<numPoolConstraints;j++)
1587 for (
int i=0;i<numPoolConstraints;i++)
1615 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)
static T sum(const btAlignedObjectArray< T > &items)
btVector3 m_linearVelocity
btVector3 m_angularVelocity
btScalar m_rhsPenetration
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
const btVector3 & getBasePos() const
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
void setupMultiBodyContactConstraint(btMultiBodySolverConstraint &solverConstraint, const btVector3 &contactNormal, btManifoldPoint &cp, const btContactSolverInfo &infoGlobal, btScalar &relaxation, bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0)
btVector3 m_relpos1CrossNormal
bool internalNeedsJointFeedback() const
const btVector3 & getPositionWorldOnA() const
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btVector3 m_contactNormal2
btScalar m_combinedContactStiffness1
btVector3 m_lateralFrictionDir1
btAlignedObjectArray< btScalar > scratch_r
const btVector3 & getTotalTorque() const
btAlignedObjectArray< btScalar > m_deltaVelocities
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)=0
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
btAlignedObjectArray< btSolverBody > * m_solverBodyPool
btScalar m_appliedImpulseLateral1
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
btVector3 m_angularComponentA
btScalar btSin(btScalar x)
void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint &constraint, btScalar deltaTime)
btVector3 m_angularComponentB
btScalar m_combinedRestitution
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
btMultiBodyConstraint * m_orgConstraint
void internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
const btRigidBody & getRigidBodyA() const
int getNumContacts() const
void btPlaneSpace1(const T &n, T &p, T &q)
btScalar m_appliedImpulse
void addLinkConstraintForce(int i, const btVector3 &f)
const btVector3 & getLinearFactor() const
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)
int getCompanionId() const
const btVector3 & internalGetInvMass() const
const btJointFeedback * getJointFeedback() const
ManifoldContactPoint collects and maintains persistent contactpoints.
btScalar m_contactMotion1
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instea...
int m_tmpNumMultiBodyConstraints
const btRigidBody & getRigidBodyB() const
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
btScalar getBreakingImpulseThreshold() const
const btVector3 & getAngularFactor() const
btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint &c)
btAlignedObjectArray< btMatrix3x3 > scratch_m
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
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...
btMultiBodyConstraintArray m_multiBodyNonContactConstraints
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
const btManifoldPoint & getContactPoint(int index) const
btVector3 m_externalForceImpulse
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
void addLinkConstraintTorque(int i, const btVector3 &t)
#define btSimdScalar
Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later...
btScalar m_combinedRollingFriction
btAlignedObjectArray< btSolverBody > m_tmpSolverBodyPool
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)
btVector3 m_normalWorldOnB
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)
btVector3 m_appliedForceBodyA
btMultiBody * m_multiBodyA
const btVector3 & getPositionWorldOnB() const
const btCollisionObject * getBody0() const
void setCompanionId(int id)
btScalar m_appliedImpulseLateral2
btScalar getInvMass() const
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.
btScalar btAtan2(btScalar x, btScalar y)
void convertContacts(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
btCollisionObject can be used to manage collision detection objects.
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
btMultiBody * m_multiBodyB
The btRigidBody is the main class for rigid body objects.
btScalar getContactProcessingThreshold() const
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints
btAlignedObjectArray< btScalar > m_jacobians
void * m_originalContactPoint
const btMultibodyLink & getLink(int index) const
btVector3 can be used to represent 3D points and vectors.
int size() const
return the number of elements in the array
void convertContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
btAlignedObjectArray< btVector3 > scratch_v
btSimdScalar m_appliedImpulse
btMultiBodySolverConstraint & addMultiBodyFrictionConstraint(const btVector3 &normalAxis, btPersistentManifold *manifold, int frictionIndex, btManifoldPoint &cp, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0)
btScalar m_combinedContactDamping1
btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
const btTransform & getWorldTransform() const
TypedConstraint is the baseclass for Bullet constraints and vehicles.
void resize(int newsize, const T &fillData=T())
btRigidBody * m_originalBody
btVector3 & internalGetDeltaLinearVelocity()
some internal methods, don't use them
const btCollisionObject * getBody1() const
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold)
void setEnabled(bool enabled)
void convertMultiBodyContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
void applyDeltaVeeMultiDof2(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)
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
void setPosUpdated(bool updated)
void applyDeltaVee(btScalar *deltaV, btScalar impulse, int velocityIndex, int ndof)
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints
btScalar m_contactMotion2
void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint &solverConstraint, const btVector3 &contactNormal, btManifoldPoint &cp, btScalar combinedTorsionalFriction, const btContactSolverInfo &infoGlobal, btScalar &relaxation, bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0)
const T & btMax(const T &a, const T &b)
btScalar m_combinedFriction
btMultiBodyConstraint ** m_tmpMultiBodyConstraints
btVector3 m_relpos2CrossNormal
btTransform m_cachedWorldTransform
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
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
const btVector3 & getTotalForce() const
btVector3 & internalGetDeltaAngularVelocity()
const btMatrix3x3 & getInvInertiaTensorWorld() const
void internalSetAppliedImpulse(btScalar appliedImpulse)
internal method used by the constraint solver, don't use them directly
void addBaseConstraintForce(const btVector3 &f)
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
btVector3 m_lateralFrictionDir2
btMultiBody * m_multiBody
btSimdScalar m_appliedPushImpulse
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
T & expandNonInitializing()
btVector3 m_contactNormal1
void addBaseConstraintTorque(const btVector3 &t)
const btScalar * getVelocityVector() const
static void applyAnisotropicFriction(btCollisionObject *colObj, btVector3 &frictionDirection, int frictionMode)
btScalar getDistance() const
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btScalar m_combinedSpinningFriction
btMultiBodyJacobianData m_data
btScalar btCos(btScalar x)
btScalar length() const
Return the length of the vector.
btScalar btFabs(btScalar x)