10 m_isUnilateral(isUnilateral),
11 m_maxAppliedImpulse(100)
63 for (
int i=0;i<ndofA;i++)
85 for (
int i=0;i<ndofB;i++)
107 for (
int i = 0; i < ndofA; ++i)
119 for (
int i = 0; i < ndofB; ++i)
128 if (multiBodyA && (multiBodyA==multiBodyB))
131 for (
int i = 0; i < ndofA; ++i)
133 denom1 += jacB[i] * lambdaA[i];
134 denom1 += jacA[i] * lambdaB[i];
166 for (
int i = 0; i < ndofA ; ++i)
173 for (
int i = 0; i < ndofB ; ++i)
183 btScalar velocityError = desiredVelocity - rel_vel;
192 constraintRow.
m_rhs = velocityImpulse;
198 constraintRow.
m_rhs = velocityImpulse;
203 constraintRow.
m_cfm = 0.f;
214 for (
int i = 0; i < ndof; ++i)
334 for (
int i = 0; i < ndofA; ++i)
353 for (
int i = 0; i < ndofB; ++i)
369 if (multiBodyA && (multiBodyA==multiBodyB))
372 for (
int i = 0; i < ndofA; ++i)
374 denom1 += jacB[i] * lambdaA[i];
375 denom1 += jacA[i] * lambdaB[i];
409 for (
int i = 0; i < ndofA ; ++i)
422 for (
int i = 0; i < ndofB ; ++i)
436 restitution = restitution * -rel_vel;
487 btScalar velocityError = restitution - rel_vel;
493 erp = infoGlobal.
m_erp;
499 velocityError = -penetration / infoGlobal.
m_timeStep;
503 positionalError = -penetration * erp/infoGlobal.
m_timeStep;
512 solverConstraint.
m_rhs = penetrationImpulse+velocityImpulse;
518 solverConstraint.
m_rhs = velocityImpulse;
522 solverConstraint.
m_cfm = 0.f;
btScalar getInvMass() const
void calcAccelerationDeltas(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
btScalar m_rhsPenetration
void fillMultiBodyConstraintMixed(btMultiBodySolverConstraint &solverConstraint, btMultiBodyJacobianData &data, const btVector3 &contactNormalOnB, const btVector3 &posAworld, const btVector3 &posBworld, btScalar position, const btContactSolverInfo &infoGlobal, btScalar &relaxation, bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0)
btVector3 m_relpos1CrossNormal
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btVector3 m_contactNormal2
btAlignedObjectArray< btScalar > scratch_r
btAlignedObjectArray< btScalar > m_deltaVelocities
const btVector3 & getAngularFactor() const
btAlignedObjectArray< btSolverBody > * m_solverBodyPool
btScalar m_maxAppliedImpulse
const T & at(int n) const
btVector3 m_angularComponentA
btVector3 m_angularComponentB
virtual ~btMultiBodyConstraint()
btAlignedObjectArray< btMatrix3x3 > scratch_m
btScalar dot(const btVector3 &v) const
Return the dot product.
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
int size() const
return the number of elements in the array
btMultiBody * m_multiBodyA
void setCompanionId(int id)
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
btMultiBody * m_multiBodyB
The btRigidBody is the main class for rigid body objects.
btAlignedObjectArray< btScalar > m_data
btAlignedObjectArray< btScalar > m_jacobians
btVector3 can be used to represent 3D points and vectors.
btScalar fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint &constraintRow, btMultiBodyJacobianData &data, btScalar *jacOrgA, btScalar *jacOrgB, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity, btScalar lowerLimit, btScalar upperLimit)
btAlignedObjectArray< btVector3 > scratch_v
btSimdScalar m_appliedImpulse
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
int getCompanionId() const
void resize(int newsize, const T &fillData=T())
btRigidBody * m_originalBody
static float4 cross(const float4 &a, const float4 &b)
const btMatrix3x3 & getInvInertiaTensorWorld() const
void fillContactJacobian(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_relpos2CrossNormal
const btTransform & getWorldTransform() const
btMultiBodyConstraint(btMultiBody *bodyA, btMultiBody *bodyB, int linkA, int linkB, int numRows, bool isUnilateral)
btSimdScalar m_appliedPushImpulse
btVector3 m_contactNormal1
void applyDeltaVee(btMultiBodyJacobianData &data, btScalar *delta_vee, btScalar impulse, int velocityIndex, int ndof)
const btScalar * getVelocityVector() const
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btScalar btFabs(btScalar x)