15 m_isUnilateral(isUnilateral),
16 m_numDofsFinalized(-1),
17 m_maxAppliedImpulse(100)
51 for (
int i = 0; i < ndof; ++i)
92 rel_pos1 = posAworld - multiBodyA->
getBasePos();
98 const int ndofA = multiBodyA->
getNumDofs() + 6;
119 for (
int i=0;i<ndofA;i++)
139 torqueAxis0 = constraintNormalAng;
142 torqueAxis0 = rel_pos1.
cross(constraintNormalLin);
152 torqueAxis0 = constraintNormalAng;
155 torqueAxis0 = rel_pos1.
cross(constraintNormalLin);
164 if (solverConstraint.
m_linkB<0)
166 rel_pos2 = posBworld - multiBodyB->
getBasePos();
172 const int ndofB = multiBodyB->
getNumDofs() + 6;
189 for (
int i=0;i<ndofB;i++)
208 torqueAxis1 = constraintNormalAng;
211 torqueAxis1 = rel_pos2.
cross(constraintNormalLin);
220 torqueAxis1 = constraintNormalAng;
223 torqueAxis1 = rel_pos2.
cross(constraintNormalLin);
245 for (
int i = 0; i < ndofA; ++i)
265 const int ndofB = multiBodyB->
getNumDofs() + 6;
268 for (
int i = 0; i < ndofB; ++i)
302 btScalar penetration = isFriction? 0 : posError;
313 for (
int i = 0; i < ndofA ; ++i)
324 for (
int i = 0; i < ndofB ; ++i)
377 btScalar velocityError = desiredVelocity - rel_vel;
385 erp = infoGlobal.
m_erp;
388 positionalError = -penetration * erp/infoGlobal.
m_timeStep;
398 solverConstraint.
m_rhs = penetrationImpulse+velocityImpulse;
410 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
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.
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
btScalar getInvMass() const
const btVector3 & getAngularFactor() const
const btMatrix3x3 & getInvInertiaTensorWorld() 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