Welcome to mirror list, hosted at ThFree Co, Russian Federation.

git.blender.org/blender.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
path: root/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp')
-rw-r--r--extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp466
1 files changed, 157 insertions, 309 deletions
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
index 44e04c3a132..12997d2e374 100644
--- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
@@ -1,263 +1,101 @@
#include "btMultiBodyConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
+
+
btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
:m_bodyA(bodyA),
m_bodyB(bodyB),
m_linkA(linkA),
m_linkB(linkB),
- m_num_rows(numRows),
+ m_numRows(numRows),
+ m_jacSizeA(0),
+ m_jacSizeBoth(0),
m_isUnilateral(isUnilateral),
+ m_numDofsFinalized(-1),
m_maxAppliedImpulse(100)
{
- m_jac_size_A = (6 + bodyA->getNumLinks());
- m_jac_size_both = (m_jac_size_A + (bodyB ? 6 + bodyB->getNumLinks() : 0));
- m_pos_offset = ((1 + m_jac_size_both)*m_num_rows);
- m_data.resize((2 + m_jac_size_both) * m_num_rows);
-}
-btMultiBodyConstraint::~btMultiBodyConstraint()
-{
}
-
-
-btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow,
- btMultiBodyJacobianData& data,
- btScalar* jacOrgA,btScalar* jacOrgB,
- const btContactSolverInfo& infoGlobal,
- btScalar desiredVelocity,
- btScalar lowerLimit,
- btScalar upperLimit)
+void btMultiBodyConstraint::updateJacobianSizes()
{
-
-
-
- constraintRow.m_multiBodyA = m_bodyA;
- constraintRow.m_multiBodyB = m_bodyB;
-
- btMultiBody* multiBodyA = constraintRow.m_multiBodyA;
- btMultiBody* multiBodyB = constraintRow.m_multiBodyB;
-
- if (multiBodyA)
+ if(m_bodyA)
{
-
- const int ndofA = multiBodyA->getNumLinks() + 6;
-
- constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId();
-
- if (constraintRow.m_deltaVelAindex <0)
- {
- constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size();
- multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex);
- data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
- } else
- {
- btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA);
- }
-
- constraintRow.m_jacAindex = data.m_jacobians.size();
- data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
- data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
- btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
- for (int i=0;i<ndofA;i++)
- data.m_jacobians[constraintRow.m_jacAindex+i] = jacOrgA[i];
-
- btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
- multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v);
- }
-
- if (multiBodyB)
- {
- const int ndofB = multiBodyB->getNumLinks() + 6;
-
- constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId();
- if (constraintRow.m_deltaVelBindex <0)
- {
- constraintRow.m_deltaVelBindex = data.m_deltaVelocities.size();
- multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex);
- data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
- }
-
- constraintRow.m_jacBindex = data.m_jacobians.size();
- data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
-
- for (int i=0;i<ndofB;i++)
- data.m_jacobians[constraintRow.m_jacBindex+i] = jacOrgB[i];
-
- data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
- btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
- multiBodyB->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v);
- }
- {
-
- btVector3 vec;
- btScalar denom0 = 0.f;
- btScalar denom1 = 0.f;
- btScalar* jacB = 0;
- btScalar* jacA = 0;
- btScalar* lambdaA =0;
- btScalar* lambdaB =0;
- int ndofA = 0;
- if (multiBodyA)
- {
- ndofA = multiBodyA->getNumLinks() + 6;
- jacA = &data.m_jacobians[constraintRow.m_jacAindex];
- lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
- for (int i = 0; i < ndofA; ++i)
- {
- btScalar j = jacA[i] ;
- btScalar l =lambdaA[i];
- denom0 += j*l;
- }
- }
- if (multiBodyB)
- {
- const int ndofB = multiBodyB->getNumLinks() + 6;
- jacB = &data.m_jacobians[constraintRow.m_jacBindex];
- lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
- for (int i = 0; i < ndofB; ++i)
- {
- btScalar j = jacB[i] ;
- btScalar l =lambdaB[i];
- denom1 += j*l;
- }
-
- }
-
- if (multiBodyA && (multiBodyA==multiBodyB))
- {
- // ndof1 == ndof2 in this case
- for (int i = 0; i < ndofA; ++i)
- {
- denom1 += jacB[i] * lambdaA[i];
- denom1 += jacA[i] * lambdaB[i];
- }
- }
-
- btScalar d = denom0+denom1;
- if (btFabs(d)>SIMD_EPSILON)
- {
-
- constraintRow.m_jacDiagABInv = 1.f/(d);
- } else
- {
- constraintRow.m_jacDiagABInv = 1.f;
- }
-
+ m_jacSizeA = (6 + m_bodyA->getNumDofs());
}
-
- //compute rhs and remaining constraintRow fields
-
-
-
-
- btScalar rel_vel = 0.f;
- int ndofA = 0;
- int ndofB = 0;
+ if(m_bodyB)
{
+ m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs();
+ }
+ else
+ m_jacSizeBoth = m_jacSizeA;
+}
- btVector3 vel1,vel2;
- if (multiBodyA)
- {
- ndofA = multiBodyA->getNumLinks() + 6;
- btScalar* jacA = &data.m_jacobians[constraintRow.m_jacAindex];
- for (int i = 0; i < ndofA ; ++i)
- rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
- }
- if (multiBodyB)
- {
- ndofB = multiBodyB->getNumLinks() + 6;
- btScalar* jacB = &data.m_jacobians[constraintRow.m_jacBindex];
- for (int i = 0; i < ndofB ; ++i)
- rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
-
- }
-
- constraintRow.m_friction = 0.f;
-
- constraintRow.m_appliedImpulse = 0.f;
- constraintRow.m_appliedPushImpulse = 0.f;
-
- btScalar velocityError = desiredVelocity - rel_vel;// * damping;
-
- btScalar erp = infoGlobal.m_erp2;
-
- btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
-
- if (!infoGlobal.m_splitImpulse)
- {
- //combine position and velocity into rhs
- constraintRow.m_rhs = velocityImpulse;
- constraintRow.m_rhsPenetration = 0.f;
-
- } else
- {
- //split position and velocity into rhs and m_rhsPenetration
- constraintRow.m_rhs = velocityImpulse;
- constraintRow.m_rhsPenetration = 0.f;
- }
-
-
- constraintRow.m_cfm = 0.f;
- constraintRow.m_lowerLimit = lowerLimit;
- constraintRow.m_upperLimit = upperLimit;
+void btMultiBodyConstraint::allocateJacobiansMultiDof()
+{
+ updateJacobianSizes();
- }
- return rel_vel;
+ m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
+ m_data.resize((2 + m_jacSizeBoth) * m_numRows);
}
+btMultiBodyConstraint::~btMultiBodyConstraint()
+{
+}
void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
{
- for (int i = 0; i < ndof; ++i)
+ for (int i = 0; i < ndof; ++i)
data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
}
-
-void btMultiBodyConstraint::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, btScalar cfmSlip)
+btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint,
+ btMultiBodyJacobianData& data,
+ btScalar* jacOrgA, btScalar* jacOrgB,
+ const btVector3& contactNormalOnB,
+ const btVector3& posAworld, const btVector3& posBworld,
+ btScalar posError,
+ const btContactSolverInfo& infoGlobal,
+ btScalar lowerLimit, btScalar upperLimit,
+ btScalar relaxation,
+ bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
{
-
-
- btVector3 rel_pos1 = posAworld;
- btVector3 rel_pos2 = posBworld;
+
solverConstraint.m_multiBodyA = m_bodyA;
solverConstraint.m_multiBodyB = m_bodyB;
solverConstraint.m_linkA = m_linkA;
solverConstraint.m_linkB = m_linkB;
-
btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
- const btVector3& pos1 = posAworld;
- const btVector3& pos2 = posBworld;
-
btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
+ btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
if (bodyA)
- rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
+ rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
if (bodyB)
- rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
-
- relaxation = 1.f;
+ rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
if (multiBodyA)
{
- const int ndofA = multiBodyA->getNumLinks() + 6;
+ if (solverConstraint.m_linkA<0)
+ {
+ rel_pos1 = posAworld - multiBodyA->getBasePos();
+ } else
+ {
+ rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
+ }
+
+ const int ndofA = multiBodyA->getNumDofs() + 6;
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
@@ -271,16 +109,35 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
}
+ //determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
+ //resize..
solverConstraint.m_jacAindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
- data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
- btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
+ //copy/determine
+ if(jacOrgA)
+ {
+ for (int i=0;i<ndofA;i++)
+ data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
+ }
+ else
+ {
+ btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
+ multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+ }
- btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
- multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+ //determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
+ //resize..
+ data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
+ btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
- multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
- } else
+ //determine..
+ multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
+
+ btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
+ solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+ solverConstraint.m_contactNormal1 = contactNormalOnB;
+ }
+ else //if(rb0)
{
btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
@@ -290,7 +147,15 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
if (multiBodyB)
{
- const int ndofB = multiBodyB->getNumLinks() + 6;
+ if (solverConstraint.m_linkB<0)
+ {
+ rel_pos2 = posBworld - multiBodyB->getBasePos();
+ } else
+ {
+ rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
+ }
+
+ const int ndofB = multiBodyB->getNumDofs() + 6;
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
if (solverConstraint.m_deltaVelBindex <0)
@@ -300,144 +165,136 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
}
+ //determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
+ //resize..
solverConstraint.m_jacBindex = data.m_jacobians.size();
-
data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
+ //copy/determine..
+ if(jacOrgB)
+ {
+ for (int i=0;i<ndofB;i++)
+ data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
+ }
+ else
+ {
+ multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
+ }
+
+ //determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
+ //resize..
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
+ btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+ //determine..
+ multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
- multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
- multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v);
- } else
+ btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
+ solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+ solverConstraint.m_contactNormal2 = -contactNormalOnB;
+
+ }
+ else //if(rb1)
{
- btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
+ btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -contactNormalOnB;
}
-
{
-
+
btVector3 vec;
btScalar denom0 = 0.f;
btScalar denom1 = 0.f;
btScalar* jacB = 0;
btScalar* jacA = 0;
- btScalar* lambdaA =0;
- btScalar* lambdaB =0;
+ btScalar* deltaVelA = 0;
+ btScalar* deltaVelB = 0;
int ndofA = 0;
+ //determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
if (multiBodyA)
{
- ndofA = multiBodyA->getNumLinks() + 6;
+ ndofA = multiBodyA->getNumDofs() + 6;
jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
- lambdaA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+ deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
{
btScalar j = jacA[i] ;
- btScalar l =lambdaA[i];
+ btScalar l = deltaVelA[i];
denom0 += j*l;
}
- } else
+ }
+ else if(rb0)
{
- if (rb0)
- {
- vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
- denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
- }
+ vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+ denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
}
+ //
if (multiBodyB)
{
- const int ndofB = multiBodyB->getNumLinks() + 6;
+ const int ndofB = multiBodyB->getNumDofs() + 6;
jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
- lambdaB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+ deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
{
btScalar j = jacB[i] ;
- btScalar l =lambdaB[i];
+ btScalar l = deltaVelB[i];
denom1 += j*l;
}
- } else
+ }
+ else if(rb1)
{
- if (rb1)
- {
- vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
- denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
- }
+ vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
+ denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
}
- if (multiBodyA && (multiBodyA==multiBodyB))
- {
- // ndof1 == ndof2 in this case
- for (int i = 0; i < ndofA; ++i)
- {
- denom1 += jacB[i] * lambdaA[i];
- denom1 += jacA[i] * lambdaB[i];
- }
- }
-
- btScalar d = denom0+denom1;
- if (btFabs(d)>SIMD_EPSILON)
- {
-
- solverConstraint.m_jacDiagABInv = relaxation/(d);
- } else
- {
- solverConstraint.m_jacDiagABInv = 1.f;
- }
-
+ //
+ btScalar d = denom0+denom1;
+ if (d>SIMD_EPSILON)
+ {
+ solverConstraint.m_jacDiagABInv = relaxation/(d);
+ }
+ else
+ {
+ //disable the constraint row to handle singularity/redundant constraint
+ solverConstraint.m_jacDiagABInv = 0.f;
+ }
}
-
- //compute rhs and remaining solverConstraint fields
-
-
- btScalar restitution = 0.f;
- btScalar penetration = isFriction? 0 : position+infoGlobal.m_linearSlop;
+ //compute rhs and remaining solverConstraint fields
+ btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
btScalar rel_vel = 0.f;
int ndofA = 0;
int ndofB = 0;
{
-
btVector3 vel1,vel2;
if (multiBodyA)
{
- ndofA = multiBodyA->getNumLinks() + 6;
+ ndofA = multiBodyA->getNumDofs() + 6;
btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
- for (int i = 0; i < ndofA ; ++i)
+ for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
- } else
+ }
+ else if(rb0)
{
- if (rb0)
- {
- rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
- }
+ rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
}
if (multiBodyB)
{
- ndofB = multiBodyB->getNumLinks() + 6;
+ ndofB = multiBodyB->getNumDofs() + 6;
btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
- for (int i = 0; i < ndofB ; ++i)
+ for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
- } else
+ }
+ else if(rb1)
{
- if (rb1)
- {
- rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
- }
+ rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
}
solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
-
-
- restitution = restitution * -rel_vel;//restitutionCurve(rel_vel, cp.m_combinedRestitution);
- if (restitution <= btScalar(0.))
- {
- restitution = 0.f;
- };
}
@@ -474,18 +331,15 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
}
} else
*/
- {
- solverConstraint.m_appliedImpulse = 0.f;
- }
+ solverConstraint.m_appliedImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f;
{
-
btScalar positionalError = 0.f;
- btScalar velocityError = restitution - rel_vel;// * damping;
-
+ btScalar velocityError = desiredVelocity - rel_vel;// * damping;
+
btScalar erp = infoGlobal.m_erp2;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
@@ -493,15 +347,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
erp = infoGlobal.m_erp;
}
- if (penetration>0)
- {
- positionalError = 0;
- velocityError = -penetration / infoGlobal.m_timeStep;
-
- } else
- {
- positionalError = -penetration * erp/infoGlobal.m_timeStep;
- }
+ positionalError = -penetration * erp/infoGlobal.m_timeStep;
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
@@ -520,8 +366,10 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
}
solverConstraint.m_cfm = 0.f;
- solverConstraint.m_lowerLimit = -m_maxAppliedImpulse;
- solverConstraint.m_upperLimit = m_maxAppliedImpulse;
+ solverConstraint.m_lowerLimit = lowerLimit;
+ solverConstraint.m_upperLimit = upperLimit;
}
+ return rel_vel;
+
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-06-15 18:17:48 +0300