49 for (
int i = 0; i < ndof; ++i)
50 data.m_deltaVelocities[velocityIndex + i] += delta_vee[i] * impulse;
66 solverConstraint.m_multiBodyA = m_bodyA;
67 solverConstraint.m_multiBodyB =
m_bodyB;
68 solverConstraint.m_linkA =
m_linkA;
69 solverConstraint.m_linkB =
m_linkB;
71 btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
72 btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
74 btSolverBody* bodyA = multiBodyA ? 0 : &
data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
75 btSolverBody* bodyB = multiBodyB ? 0 : &
data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
77 btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
78 btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
82 rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
84 rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
88 if (solverConstraint.m_linkA < 0)
90 rel_pos1 = posAworld - multiBodyA->getBasePos();
94 rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
97 const int ndofA = multiBodyA->getNumDofs() + 6;
99 solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
101 if (solverConstraint.m_deltaVelAindex < 0)
103 solverConstraint.m_deltaVelAindex =
data.m_deltaVelocities.size();
104 multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
105 data.m_deltaVelocities.resize(
data.m_deltaVelocities.size() + ndofA);
109 btAssert(
data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA);
114 solverConstraint.m_jacAindex =
data.m_jacobians.size();
115 data.m_jacobians.resize(
data.m_jacobians.size() + ndofA);
119 for (
int i = 0; i < ndofA; i++)
120 data.m_jacobians[solverConstraint.m_jacAindex + i] = jacOrgA[i];
124 btScalar* jac1 = &
data.m_jacobians[solverConstraint.m_jacAindex];
126 multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalAng, constraintNormalLin, jac1,
data.scratch_r,
data.scratch_v,
data.scratch_m);
131 data.m_deltaVelocitiesUnitImpulse.resize(
data.m_deltaVelocitiesUnitImpulse.size() + ndofA);
132 btAssert(
data.m_jacobians.size() ==
data.m_deltaVelocitiesUnitImpulse.size());
133 btScalar* delta = &
data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
135 multiBodyA->calcAccelerationDeltasMultiDof(&
data.m_jacobians[solverConstraint.m_jacAindex], delta,
data.scratch_r,
data.scratch_v);
140 torqueAxis0 = constraintNormalAng;
144 torqueAxis0 = rel_pos1.cross(constraintNormalLin);
146 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
147 solverConstraint.m_contactNormal1 = constraintNormalLin;
154 torqueAxis0 = constraintNormalAng;
158 torqueAxis0 = rel_pos1.cross(constraintNormalLin);
161 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
162 solverConstraint.m_contactNormal1 = constraintNormalLin;
167 if (solverConstraint.m_linkB < 0)
169 rel_pos2 = posBworld - multiBodyB->getBasePos();
173 rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
176 const int ndofB = multiBodyB->getNumDofs() + 6;
178 solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
179 if (solverConstraint.m_deltaVelBindex < 0)
181 solverConstraint.m_deltaVelBindex =
data.m_deltaVelocities.size();
182 multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
183 data.m_deltaVelocities.resize(
data.m_deltaVelocities.size() + ndofB);
188 solverConstraint.m_jacBindex =
data.m_jacobians.size();
189 data.m_jacobians.resize(
data.m_jacobians.size() + ndofB);
193 for (
int i = 0; i < ndofB; i++)
194 data.m_jacobians[solverConstraint.m_jacBindex + i] = jacOrgB[i];
199 multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalAng, -constraintNormalLin, &
data.m_jacobians[solverConstraint.m_jacBindex],
data.scratch_r,
data.scratch_v,
data.scratch_m);
204 data.m_deltaVelocitiesUnitImpulse.resize(
data.m_deltaVelocitiesUnitImpulse.size() + ndofB);
205 btAssert(
data.m_jacobians.size() ==
data.m_deltaVelocitiesUnitImpulse.size());
206 btScalar* delta = &
data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
208 multiBodyB->calcAccelerationDeltasMultiDof(&
data.m_jacobians[solverConstraint.m_jacBindex], delta,
data.scratch_r,
data.scratch_v);
213 torqueAxis1 = constraintNormalAng;
217 torqueAxis1 = rel_pos2.cross(constraintNormalLin);
219 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
220 solverConstraint.m_contactNormal2 = -constraintNormalLin;
227 torqueAxis1 = constraintNormalAng;
231 torqueAxis1 = rel_pos2.cross(constraintNormalLin);
234 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
235 solverConstraint.m_contactNormal2 = -constraintNormalLin;
249 ndofA = multiBodyA->getNumDofs() + 6;
250 jacA = &
data.m_jacobians[solverConstraint.m_jacAindex];
251 deltaVelA = &
data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
252 for (
int i = 0; i < ndofA; ++i)
261 vec = (solverConstraint.m_angularComponentA).
cross(rel_pos1);
264 denom0 = constraintNormalAng.dot(solverConstraint.m_angularComponentA);
268 denom0 = rb0->
getInvMass() + constraintNormalLin.dot(vec);
274 const int ndofB = multiBodyB->getNumDofs() + 6;
275 jacB = &
data.m_jacobians[solverConstraint.m_jacBindex];
276 deltaVelB = &
data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
277 for (
int i = 0; i < ndofB; ++i)
286 vec = (-solverConstraint.m_angularComponentB).
cross(rel_pos2);
289 denom1 = constraintNormalAng.dot(-solverConstraint.m_angularComponentB);
293 denom1 = rb1->
getInvMass() + constraintNormalLin.dot(vec);
301 solverConstraint.m_jacDiagABInv = relaxation / (d);
306 solverConstraint.m_jacDiagABInv = 0.f;
311 btScalar penetration = isFriction ? 0 : posError;
320 ndofA = multiBodyA->getNumDofs() + 6;
321 btScalar* jacA = &
data.m_jacobians[solverConstraint.m_jacAindex];
322 for (
int i = 0; i < ndofA; ++i)
323 rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
332 ndofB = multiBodyB->getNumDofs() + 6;
333 btScalar* jacB = &
data.m_jacobians[solverConstraint.m_jacBindex];
334 for (
int i = 0; i < ndofB; ++i)
335 rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
343 solverConstraint.m_friction = 0.f;
346 solverConstraint.m_appliedImpulse = 0.f;
347 solverConstraint.m_appliedPushImpulse = 0.f;
351 btScalar velocityError = desiredVelocity - rel_vel;
363 btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
364 btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
371 solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
372 solverConstraint.m_rhsPenetration = 0.f;
382 solverConstraint.m_cfm = 0.f;
383 solverConstraint.m_lowerLimit = lowerLimit;
384 solverConstraint.m_upperLimit = upperLimit;
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum type
ATTR_WARN_UNUSED_RESULT const BMLoop * l
void updateJacobianSizes()
void applyDeltaVee(btMultiBodyJacobianData &data, btScalar *delta_vee, btScalar impulse, int velocityIndex, int ndof)
btAlignedObjectArray< btScalar > m_data
bool isUnilateral() const
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)
btScalar m_maxAppliedImpulse
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int const btContactSolverInfo & infoGlobal
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
const btVector3 & getAngularVelocity() const
btScalar getInvMass() const
const btVector3 & getLinearVelocity() const
const btMatrix3x3 & getInvInertiaTensorWorld() const
const btVector3 & getAngularFactor() const
vec_base< T, 3 > cross(const vec_base< T, 3 > &a, const vec_base< T, 3 > &b)