13 m_isUnilateral(isUnilateral),
14 m_numDofsFinalized(-1),
15 m_maxAppliedImpulse(100)
48 for (
int i = 0; i < ndof; ++i)
87 if (solverConstraint.
m_linkA < 0)
89 rel_pos1 = posAworld - multiBodyA->
getBasePos();
96 const int ndofA = multiBodyA->
getNumDofs() + 6;
118 for (
int i = 0; i < ndofA; i++)
139 torqueAxis0 = constraintNormalAng;
143 torqueAxis0 = rel_pos1.
cross(constraintNormalLin);
153 torqueAxis0 = constraintNormalAng;
157 torqueAxis0 = rel_pos1.
cross(constraintNormalLin);
166 if (solverConstraint.
m_linkB < 0)
168 rel_pos2 = posBworld - multiBodyB->
getBasePos();
175 const int ndofB = multiBodyB->
getNumDofs() + 6;
192 for (
int i = 0; i < ndofB; i++)
212 torqueAxis1 = constraintNormalAng;
216 torqueAxis1 = rel_pos2.
cross(constraintNormalLin);
226 torqueAxis1 = constraintNormalAng;
230 torqueAxis1 = rel_pos2.
cross(constraintNormalLin);
251 for (
int i = 0; i < ndofA; ++i)
273 const int ndofB = multiBodyB->
getNumDofs() + 6;
276 for (
int i = 0; i < ndofB; ++i)
310 btScalar penetration = isFriction ? 0 : posError;
321 for (
int i = 0; i < ndofA; ++i)
333 for (
int i = 0; i < ndofB; ++i)
350 btScalar velocityError = desiredVelocity - rel_vel;
357 erp = infoGlobal.
m_erp;
360 positionalError = -penetration * erp / infoGlobal.
m_timeStep;
370 solverConstraint.
m_rhs = penetrationImpulse + velocityImpulse;
381 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())
btAlignedObjectArray< btScalar > m_data
void updateJacobianSizes()
void applyDeltaVee(btMultiBodyJacobianData &data, btScalar *delta_vee, btScalar impulse, int velocityIndex, int ndof)
void allocateJacobiansMultiDof()
virtual ~btMultiBodyConstraint()
btMultiBodyConstraint(btMultiBody *bodyA, btMultiBody *bodyB, int linkA, int linkB, int numRows, bool isUnilateral)
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)
const btVector3 & getBasePos() const
int getCompanionId() const
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 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 setCompanionId(int id)
const btScalar * getVelocityVector() const
The btRigidBody is the main class for rigid body objects.
btScalar getInvMass() const
const btVector3 & getAngularVelocity() const
const btVector3 & getAngularFactor() const
const btMatrix3x3 & getInvInertiaTensorWorld() const
const btVector3 & getLinearVelocity() 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