14 m_isUnilateral(isUnilateral),
15 m_numDofsFinalized(-1),
16 m_maxAppliedImpulse(100)
49 for (
int i = 0; i < ndof; ++i)
88 if (solverConstraint.
m_linkA < 0)
90 rel_pos1 = posAworld - multiBodyA->
getBasePos();
97 const int ndofA = multiBodyA->
getNumDofs() + 6;
119 for (
int i = 0; i < ndofA; i++)
140 torqueAxis0 = constraintNormalAng;
144 torqueAxis0 = rel_pos1.
cross(constraintNormalLin);
154 torqueAxis0 = constraintNormalAng;
158 torqueAxis0 = rel_pos1.
cross(constraintNormalLin);
167 if (solverConstraint.
m_linkB < 0)
169 rel_pos2 = posBworld - multiBodyB->
getBasePos();
176 const int ndofB = multiBodyB->
getNumDofs() + 6;
193 for (
int i = 0; i < ndofB; i++)
213 torqueAxis1 = constraintNormalAng;
217 torqueAxis1 = rel_pos2.
cross(constraintNormalLin);
227 torqueAxis1 = constraintNormalAng;
231 torqueAxis1 = rel_pos2.
cross(constraintNormalLin);
252 for (
int i = 0; i < ndofA; ++i)
274 const int ndofB = multiBodyB->
getNumDofs() + 6;
277 for (
int i = 0; i < ndofB; ++i)
311 btScalar penetration = isFriction ? 0 : posError;
322 for (
int i = 0; i < ndofA; ++i)
334 for (
int i = 0; i < ndofB; ++i)
351 btScalar velocityError = desiredVelocity - rel_vel;
358 erp = infoGlobal.
m_erp;
361 positionalError = -penetration * erp / infoGlobal.
m_timeStep;
371 solverConstraint.
m_rhs = penetrationImpulse + velocityImpulse;
382 solverConstraint.
m_cfm = 0.f;
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
const T & at(int n) const
btAlignedObjectArray< btScalar > m_data
void updateJacobianSizes()
btMultiBodyConstraint(btMultiBody *bodyA, btMultiBody *bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type)
void applyDeltaVee(btMultiBodyJacobianData &data, btScalar *delta_vee, btScalar impulse, int velocityIndex, int ndof)
void allocateJacobiansMultiDof()
virtual ~btMultiBodyConstraint()
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint &solverConstraint, btMultiBodyJacobianData &data, btScalar *jacOrgA, btScalar *jacOrgB, const btVector3 &constraintNormalAng, const btVector3 &constraintNormalLin, const btVector3 &posAworld, const btVector3 &posBworld, btScalar posError, const btContactSolverInfo &infoGlobal, btScalar lowerLimit, btScalar upperLimit, bool angConstraint=false, btScalar relaxation=1.f, bool isFriction=false, btScalar desiredVelocity=0, btScalar cfmSlip=0)
int getCompanionId() const
const btVector3 & getBasePos() const
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instea...
const btMultibodyLink & getLink(int index) 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
const btScalar * getVelocityVector() const
void setCompanionId(int id)
The btRigidBody is the main class for rigid body objects.
const btVector3 & getAngularVelocity() const
btScalar getInvMass() const
const btVector3 & getLinearVelocity() const
const btMatrix3x3 & getInvInertiaTensorWorld() const
const btVector3 & getAngularFactor() 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.
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...
btVector3 m_relpos1CrossNormal
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...
btRigidBody * m_originalBody
const btTransform & getWorldTransform() const