From ef6abf64903df24e5930243627626ceca8c80cbd Mon Sep 17 00:00:00 2001 From: kubas Date: Thu, 9 Jan 2014 01:09:44 +0100 Subject: [PATCH] unified btMultiBodyConstrained::fillMultiBodyConstraint..(...) mtds + cleaned some of the earlier dirty changes (6DoF grabbing constraint stuff mainly) --- .../Featherstone/btMultiBody.cpp | 121 +--- src/BulletDynamics/Featherstone/btMultiBody.h | 7 +- .../Featherstone/btMultiBodyConstraint.cpp | 676 ++---------------- .../Featherstone/btMultiBodyConstraint.h | 26 +- .../btMultiBodyJointLimitConstraint.cpp | 4 +- .../Featherstone/btMultiBodyJointMotor.cpp | 4 +- .../Featherstone/btMultiBodyPoint2Point.cpp | 341 ++++----- 7 files changed, 254 insertions(+), 925 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 490ebd409..f0f34b506 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -2587,126 +2587,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd } } -void btMultiBody::fillContactJacobianMultiDof(int link, - const btVector3 &contact_point, - const btVector3 &normal, - btScalar *jac, - btAlignedObjectArray &scratch_r, - btAlignedObjectArray &scratch_v, - btAlignedObjectArray &scratch_m) const -{ - // temporary space - int num_links = getNumLinks(); - int m_dofCount = getNumDofs(); - scratch_v.resize(2*num_links + 2); - scratch_m.resize(num_links + 1); - - btVector3 * v_ptr = &scratch_v[0]; - btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1; - btVector3 * n_local = v_ptr; v_ptr += num_links + 1; - btAssert(v_ptr - &scratch_v[0] == scratch_v.size()); - - scratch_r.resize(m_dofCount); - btScalar * results = num_links > 0 ? &scratch_r[0] : 0; - - btMatrix3x3 * rot_from_world = &scratch_m[0]; - - const btVector3 p_minus_com_world = contact_point - m_basePos; - const btVector3 &normal_world = normal; //convenience - - rot_from_world[0] = btMatrix3x3(m_baseQuat); - - // omega coeffients first. - btVector3 omega_coeffs_world; - omega_coeffs_world = p_minus_com_world.cross(normal_world); - jac[0] = omega_coeffs_world[0]; - jac[1] = omega_coeffs_world[1]; - jac[2] = omega_coeffs_world[2]; - // then v coefficients - jac[3] = normal_world[0]; - jac[4] = normal_world[1]; - jac[5] = normal_world[2]; - - //create link-local versions of p_minus_com and normal - p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world; - n_local[0] = rot_from_world[0] * normal_world; - - // Set remaining jac values to zero for now. - for (int i = 6; i < 6 + m_dofCount; ++i) - { - jac[i] = 0; - } - - // Qdot coefficients, if necessary. - if (num_links > 0 && link > -1) { - - // TODO: speed this up -- don't calculate for m_links we don't need. - // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions, - // which is resulting in repeated work being done...) - - // calculate required normals & positions in the local frames. - for (int i = 0; i < num_links; ++i) { - - // transform to local frame - const int parent = m_links[i].m_parent; - const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis); - rot_from_world[i+1] = mtx * rot_from_world[parent+1]; - - n_local[i+1] = mtx * n_local[parent+1]; - p_minus_com_local[i+1] = mtx * p_minus_com_local[parent+1] - m_links[i].m_cachedRVector; - - // calculate the jacobian entry - switch(m_links[i].m_jointType) - { - case btMultibodyLink::eRevolute: - { - results[m_links[i].m_dofOffset] = n_local[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0)); - break; - } - case btMultibodyLink::ePrismatic: - { - results[m_links[i].m_dofOffset] = n_local[i+1].dot(m_links[i].getAxisBottom(0)); - break; - } - case btMultibodyLink::eSpherical: - { - results[m_links[i].m_dofOffset + 0] = n_local[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0)); - results[m_links[i].m_dofOffset + 1] = n_local[i+1].dot(m_links[i].getAxisTop(1).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(1)); - results[m_links[i].m_dofOffset + 2] = n_local[i+1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(2)); - - break; - } -#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS - case btMultibodyLink::ePlanar: - { - results[m_links[i].m_dofOffset + 0] = n_local[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0)); - results[m_links[i].m_dofOffset + 1] = n_local[i+1].dot(m_links[i].getAxisBottom(1)); - results[m_links[i].m_dofOffset + 2] = n_local[i+1].dot(m_links[i].getAxisBottom(2)); - - break; - } -#endif - } - - } - - // Now copy through to output. - //printf("jac[%d] = ", link); - while (link != -1) - { - for(int dof = 0; dof < m_links[link].m_dofCount; ++dof) - { - jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof]; - //printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]); - } - - link = m_links[link].m_parent; - } - //printf("]\n"); - } -} - -void btMultiBody::fillContactJacobianMultiDof_test(int link, +void btMultiBody::filConstraintJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal_ang, const btVector3 &normal_lin, diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index cc86cc4c8..4c99aea4c 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -389,15 +389,18 @@ public: btAlignedObjectArray &scratch_v, btAlignedObjectArray &scratch_m) const; + //multidof version of fillContactJacobian void fillContactJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal, btScalar *jac, btAlignedObjectArray &scratch_r, btAlignedObjectArray &scratch_v, - btAlignedObjectArray &scratch_m) const; + btAlignedObjectArray &scratch_m) const { filConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); } - void fillContactJacobianMultiDof_test(int link, + //a more general version of fillContactJacobianMultiDof which does not assume.. + //.. that the constraint in question is contact or, to be more precise, constrains linear velocity only + void filConstraintJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal_ang, const btVector3 &normal_lin, diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index 9f0b0b9b4..1bb518f94 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -40,579 +40,42 @@ btMultiBodyConstraint::~btMultiBodyConstraint() { } - - -btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow, - btMultiBodyJacobianData& data, - btScalar* jacOrgA,btScalar* jacOrgB, - const btContactSolverInfo& infoGlobal, - btScalar desiredVelocity, - btScalar lowerLimit, - btScalar upperLimit) -{ - - - - constraintRow.m_multiBodyA = m_bodyA; - constraintRow.m_multiBodyB = m_bodyB; - - btMultiBody* multiBodyA = constraintRow.m_multiBodyA; - btMultiBody* multiBodyB = constraintRow.m_multiBodyB; - - if (multiBodyA) - { - - const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; //total dof count of tree A - - constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId(); - - if (constraintRow.m_deltaVelAindex <0) //if this multibody does not have a place allocated in m_deltaVelocities... - { - constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size(); - multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); //=> each constrained tree's dofs are represented in m_deltaVelocities - } 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); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse - btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); - for (int i=0;iisMultiDof()) - multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v); - else - multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v); - } - - if (multiBodyB) - { - const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : 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;iisMultiDof()) - multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v); - else - 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->isMultiDof() ? multiBodyA->getNumDofs() : 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->isMultiDof() ? multiBodyB->getNumDofs() : 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; - } - - } - - - //compute rhs and remaining constraintRow fields - - - - - btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; - { - - btVector3 vel1,vel2; - if (multiBodyA) - { - ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : 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->isMultiDof() ? multiBodyB->getNumDofs() : 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]; - - } - for (int i = 6; i < ndofA ; ++i) - printf("%.4f ", multiBodyA->getVelocityVector()[i]); - printf("\nrel_vel = %.4f\n------------\n", rel_vel); - 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; - - } - return rel_vel; -} - - void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) { for (int i = 0; i < ndof; ++i) data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; } - -void btMultiBodyConstraint::fillMultiBodyConstraintMixed_old(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) -{ - - 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; - - if (bodyA) - rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); - if (bodyB) - rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); - - relaxation = 1.f; - - if (multiBodyA) - { - const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; - - solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); - - if (solverConstraint.m_deltaVelAindex <0) - { - solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size(); - multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); - } else - { - btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); - } - -#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST - solverConstraint.m_jacAindex = data.m_jacobians.size(); - data.m_jacobians.resize(data.m_jacobians.size()+ndofA); -#endif - data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); - btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); - -#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST - btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex]; - if(multiBodyA->isMultiDof()) - multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); - else - multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); -#endif - btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - if(multiBodyA->isMultiDof()) - multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); - else - multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); - } else - { - btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB); - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = contactNormalOnB; - } - - if (multiBodyB) - { - const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; - - solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); - if (solverConstraint.m_deltaVelBindex <0) - { - solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size(); - multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); - } - - solverConstraint.m_jacBindex = data.m_jacobians.size(); - - data.m_jacobians.resize(data.m_jacobians.size()+ndofB); - data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB); - btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); - - if(multiBodyB->isMultiDof()) - multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); - else - multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); - if(multiBodyB->isMultiDof()) - multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v); - else - 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_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; - int ndofA = 0; - if (multiBodyA) - { - ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; - jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; - lambdaA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA; ++i) - { - btScalar j = jacA[i] ; - btScalar l =lambdaA[i]; - denom0 += j*l; - } - } else - { - if (rb0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec); - } - } - if (multiBodyB) - { - const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; - jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; - lambdaB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB; ++i) - { - btScalar j = jacB[i] ; - btScalar l =lambdaB[i]; - denom1 += j*l; - } - - } else - { - if (rb1) - { - 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; - } - - } - - - //compute rhs and remaining solverConstraint fields - - - - btScalar restitution = 0.f; - btScalar penetration = isFriction? 0 : position+infoGlobal.m_linearSlop; - - btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; - { - - btVector3 vel1,vel2; - if (multiBodyA) - { - ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; - btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA ; ++i) - rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; - } else - { - if (rb0) - { - rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); - } - } - if (multiBodyB) - { - ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; - btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB ; ++i) - rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; - - } else - { - if (rb1) - { - 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; - }; - } - - - ///warm starting (or zero if disabled) - /* - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - - if (solverConstraint.m_appliedImpulse) - { - if (multiBodyA) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); - } else - { - if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); - } - if (multiBodyB) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - multiBodyB->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); - } else - { - if (rb1) - bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); - } - } - } else - */ - { - solverConstraint.m_appliedImpulse = 0.f; - } - - solverConstraint.m_appliedPushImpulse = 0.f; - - { - - - btScalar positionalError = 0.f; - btScalar velocityError = restitution - rel_vel;// * damping; - - - btScalar erp = infoGlobal.m_erp2; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - erp = infoGlobal.m_erp; - } - - //commented out on purpose, see below - //if (penetration>0) - //{ - // positionalError = 0; - // velocityError = -penetration / infoGlobal.m_timeStep; - - //} else - //{ - // positionalError = -penetration * erp/infoGlobal.m_timeStep; - //} - - //we cannot assume negative penetration to be the actual penetration and positive - speculative constraint (like for normal contact constraints) - //both are valid in general and definitely so in the case of a point2Point constraint - positionalError = -penetration * erp/infoGlobal.m_timeStep; - - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; - solverConstraint.m_rhsPenetration = 0.f; - - } else - { - //split position and velocity into rhs and m_rhsPenetration - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = penetrationImpulse; - } - - solverConstraint.m_cfm = 0.f; - solverConstraint.m_lowerLimit = -m_maxAppliedImpulse; - solverConstraint.m_upperLimit = m_maxAppliedImpulse; - } - -} - -void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint, +btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint, btMultiBodyJacobianData& data, btScalar* jacOrgA, btScalar* jacOrgB, const btVector3& contactNormalOnB, const btVector3& posAworld, const btVector3& posBworld, - btScalar position, + btScalar posError, const btContactSolverInfo& infoGlobal, - btScalar& relaxation, + 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; - + 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) { @@ -630,7 +93,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); } - //determine jacobian of this 1D constraint + //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); @@ -649,11 +112,12 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); } - //determine the response of the multibody the constraint impulses of this constraint (i.e. multibody's inverse inertia with respect to this 1D constraint) + //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]; - + //determine.. if(multiBodyA->isMultiDof()) multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); else @@ -679,7 +143,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); } - //determine jacobian of this 1D constraint + //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); @@ -697,7 +161,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); } - //determine the response of the multibody the constraint impulses of this constraint (i.e. multibody's inverse inertia with respect to this 1D constraint) + //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()); @@ -716,7 +180,6 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint solverConstraint.m_relpos2CrossNormal = -torqueAxis1; solverConstraint.m_contactNormal2 = -contactNormalOnB; } - { btVector3 vec; @@ -724,78 +187,72 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint 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->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 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 - { - if (rb0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec); - } } + else if(rb0) + { + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec); + } + // if (multiBodyB) { const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 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); + } + //determine the "effective mass" of the constrained multibodyB with respect to this 1D constraint (i.e. 1/A[i,i]) + if (multiBodyA && (multiBodyA==multiBodyB)) + { + // ndof1 == ndof2 in this case + for (int i = 0; i < ndofA; ++i) { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec); + denom1 += jacB[i] * deltaVelA[i]; + denom1 += jacA[i] * deltaVelB[i]; } } - - 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) - { + // + btScalar d = denom0+denom1; + if (btFabs(d)>SIMD_EPSILON) + { - solverConstraint.m_jacDiagABInv = relaxation/(d); - } else - { + solverConstraint.m_jacDiagABInv = relaxation/(d); + } + else + { solverConstraint.m_jacDiagABInv = 1.f; - } - + } } //compute rhs and remaining solverConstraint fields - - - - btScalar restitution = 0.f; - btScalar penetration = isFriction? 0 : position+infoGlobal.m_linearSlop; + btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop; btScalar rel_vel = 0.f; int ndofA = 0; @@ -827,13 +284,6 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint } 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; - }; } @@ -870,17 +320,14 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint } } 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; @@ -889,19 +336,6 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint erp = infoGlobal.m_erp; } - //commented out on purpose, see below - //if (penetration>0) - //{ - // positionalError = 0; - // velocityError = -penetration / infoGlobal.m_timeStep; - - //} else - //{ - // positionalError = -penetration * erp/infoGlobal.m_timeStep; - //} - - //we cannot assume negative penetration to be the actual penetration and positive - speculative constraint (like for normal contact constraints) - //both are valid in general and definitely so in the case of a point2Point constraint positionalError = -penetration * erp/infoGlobal.m_timeStep; btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; @@ -921,8 +355,10 @@ void btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint } 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; + } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 7b27df1c5..d01870485 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -66,32 +66,16 @@ protected: void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof); - void fillMultiBodyConstraintMixed_old(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); - - btScalar fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow, - btMultiBodyJacobianData& data, - btScalar* jacOrgA,btScalar* jacOrgB, - const btContactSolverInfo& infoGlobal, - btScalar desiredVelocity, - btScalar lowerLimit, - btScalar upperLimit); - - void fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, + btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, btMultiBodyJacobianData& data, btScalar* jacOrgA, btScalar* jacOrgB, const btVector3& contactNormalOnB, const btVector3& posAworld, const btVector3& posBworld, - btScalar position, + btScalar posError, const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); + btScalar lowerLimit, btScalar upperLimit, + btScalar relaxation = 1.f, + bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0); public: diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index 21baf3d0e..b9658731b 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -99,8 +99,10 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); constraintRow.m_multiBodyA = m_bodyA; constraintRow.m_multiBodyB = m_bodyB; + const btScalar posError = 0; //why assume it's zero? + const btVector3 dummy(0, 0, 0); - btScalar rel_vel = fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,0,0,m_maxAppliedImpulse); + btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse); { btScalar penetration = getPosition(row); btScalar positionalError = 0.f; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index 527f2c011..2f10118a0 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -82,13 +82,15 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con // directions were set in the ctor and never change. + const btScalar posError = 0; + const btVector3 dummy(0, 0, 0); for (int row=0;rowgetIslandTag(); - - if (m_bodyA) - { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;igetNumLinks();i++) - { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); - } - } - return -1; -} - -int btMultiBodyPoint2Point::getIslandIdB() const -{ - if (m_rigidBodyB) - return m_rigidBodyB->getIslandTag(); - if (m_bodyB) - { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;igetNumLinks();i++) - { - col = m_bodyB->getLink(i).m_collider; - if (col) - return col->getIslandTag(); - } - } - return -1; -} - - - -void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal) -{ - -// int i=1; - for (int i=0;i<3;i++) - { - - btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); - - constraintRow.m_solverBodyIdA = data.m_fixedBodyId; - constraintRow.m_solverBodyIdB = data.m_fixedBodyId; - - - btVector3 contactNormalOnB(0,0,0); - contactNormalOnB[i] = -1; - - btScalar penetration = 0; - - // Convert local points back to world - btVector3 pivotAworld = m_pivotInA; - if (m_rigidBodyA) - { - - constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId(); - pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA; - } else - { - if (m_bodyA) - pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); - } - btVector3 pivotBworld = m_pivotInB; - if (m_rigidBodyB) - { - constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId(); - pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB; - } else - { - if (m_bodyB) - pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); - - } - btScalar position = (pivotAworld-pivotBworld).dot(contactNormalOnB); - btScalar relaxation = 1.f; - fillMultiBodyConstraintMixed_old(constraintRow, data, - contactNormalOnB, - pivotAworld, pivotBworld, - position, - infoGlobal, - relaxation, - false); - constraintRow.m_lowerLimit = -m_maxAppliedImpulse; - constraintRow.m_upperLimit = m_maxAppliedImpulse; - - } -} - + #define BTMBP2PCONSTRAINT_DIM 3 #else - - - -#include "btMultiBodyPoint2Point.h" -#include "btMultiBodyLinkCollider.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" + #define BTMBP2PCONSTRAINT_DIM 6 +#endif btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB) - :btMultiBodyConstraint(body,0,link,-1,6,false), + :btMultiBodyConstraint(body,0,link,-1,BTMBP2PCONSTRAINT_DIM,false), m_rigidBodyA(0), m_rigidBodyB(bodyB), m_pivotInA(pivotInA), @@ -162,7 +35,7 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRi } btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB) - :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,6,false), + :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBP2PCONSTRAINT_DIM,false), m_rigidBodyA(0), m_rigidBodyB(0), m_pivotInA(pivotInA), @@ -223,7 +96,7 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co { // int i=1; - for (int i=0;i<6;i++) + for (int i=0;i= 3) - contactNormalOnB[i-3] = -1; - else - normalAng[i] = -1; - +#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST + contactNormalOnB[i] = -1; +#else + contactNormalOnB[i%3] = -1; +#endif btScalar penetration = 0; @@ -265,30 +136,180 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); } - btScalar position = (pivotAworld-pivotBworld).dot(contactNormalOnB); - btScalar relaxation = 1.f; - if(i < 3) - position = 0; - - constraintRow.m_jacAindex = data.m_jacobians.size(); - data.m_jacobians.resize(data.m_jacobians.size()+m_bodyA->getNumDofs()+6); - btScalar* jac1=&data.m_jacobians[constraintRow.m_jacAindex]; + btScalar posError = i < 3 ? (pivotAworld-pivotBworld).dot(contactNormalOnB) : 0; - m_bodyA->fillContactJacobianMultiDof_test(m_linkA, pivotAworld, normalAng, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); +#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST - - fillMultiBodyConstraintMixed_old(constraintRow, data, - contactNormalOnB, - pivotAworld, pivotBworld, - position, - infoGlobal, - relaxation, - false); - constraintRow.m_lowerLimit = -m_maxAppliedImpulse; - constraintRow.m_upperLimit = m_maxAppliedImpulse; + fillMultiBodyConstraint(constraintRow, data, 0, 0, + contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being" + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse + ); +#else + const btVector3 dummy(0, 0, 0); + + btAssert(m_bodyA->isMultiDof()); + + btScalar* jac1 = jacobianA(i); + const btVector3 &normalAng = i >= 3 ? contactNormalOnB : dummy; + const btVector3 &normalLin = i < 3 ? contactNormalOnB : dummy; + + m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m); + + fillMultiBodyConstraint(constraintRow, data, jac1, 0, + dummy, dummy, dummy, //sucks but let it be this way "for the time being" + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse + ); +#endif } } -#endif \ No newline at end of file + + +//#include "btMultiBodyPoint2Point.h" +//#include "btMultiBodyLinkCollider.h" +//#include "BulletDynamics/Dynamics/btRigidBody.h" +// +//btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB) +// :btMultiBodyConstraint(body,0,link,-1,6,false), +// m_rigidBodyA(0), +// m_rigidBodyB(bodyB), +// m_pivotInA(pivotInA), +// m_pivotInB(pivotInB) +//{ +//} +// +//btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB) +// :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,6,false), +// m_rigidBodyA(0), +// m_rigidBodyB(0), +// m_pivotInA(pivotInA), +// m_pivotInB(pivotInB) +//{ +//} +// +// +//btMultiBodyPoint2Point::~btMultiBodyPoint2Point() +//{ +//} +// +// +//int btMultiBodyPoint2Point::getIslandIdA() const +//{ +// if (m_rigidBodyA) +// return m_rigidBodyA->getIslandTag(); +// +// if (m_bodyA) +// { +// btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); +// if (col) +// return col->getIslandTag(); +// for (int i=0;igetNumLinks();i++) +// { +// if (m_bodyA->getLink(i).m_collider) +// return m_bodyA->getLink(i).m_collider->getIslandTag(); +// } +// } +// return -1; +//} +// +//int btMultiBodyPoint2Point::getIslandIdB() const +//{ +// if (m_rigidBodyB) +// return m_rigidBodyB->getIslandTag(); +// if (m_bodyB) +// { +// btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); +// if (col) +// return col->getIslandTag(); +// +// for (int i=0;igetNumLinks();i++) +// { +// col = m_bodyB->getLink(i).m_collider; +// if (col) +// return col->getIslandTag(); +// } +// } +// return -1; +//} +// +// +// +//void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows, +// btMultiBodyJacobianData& data, +// const btContactSolverInfo& infoGlobal) +//{ +// +//// int i=1; +// for (int i=0;i<6;i++) +// { +// +// btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); +// +// constraintRow.m_solverBodyIdA = data.m_fixedBodyId; +// constraintRow.m_solverBodyIdB = data.m_fixedBodyId; +// +// +// btVector3 contactNormalOnB(0,0,0); +// btVector3 normalAng(0, 0, 0); +// if(i >= 3) +// contactNormalOnB[i-3] = -1; +// else +// normalAng[i] = -1; +// +// +// btScalar penetration = 0; +// +// // Convert local points back to world +// btVector3 pivotAworld = m_pivotInA; +// if (m_rigidBodyA) +// { +// +// constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId(); +// pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA; +// } else +// { +// if (m_bodyA) +// pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); +// } +// btVector3 pivotBworld = m_pivotInB; +// if (m_rigidBodyB) +// { +// constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId(); +// pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB; +// } else +// { +// if (m_bodyB) +// pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); +// +// } +// btScalar position = (pivotAworld-pivotBworld).dot(contactNormalOnB); +// btScalar relaxation = 1.f; +// +// if(i < 3) +// position = 0; +// +// constraintRow.m_jacAindex = data.m_jacobians.size(); +// data.m_jacobians.resize(data.m_jacobians.size()+m_bodyA->getNumDofs()+6); +// btScalar* jac1=&data.m_jacobians[constraintRow.m_jacAindex]; +// +// m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); +// +// +// fillMultiBodyConstraintMixed_old(constraintRow, data, +// contactNormalOnB, +// pivotAworld, pivotBworld, +// position, +// infoGlobal, +// relaxation, +// false); +// constraintRow.m_lowerLimit = -m_maxAppliedImpulse; +// constraintRow.m_upperLimit = m_maxAppliedImpulse; +// +// } +//} \ No newline at end of file