diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 9179ed1b1..490ebd409 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -2706,6 +2706,135 @@ void btMultiBody::fillContactJacobianMultiDof(int link, } } +void btMultiBody::fillContactJacobianMultiDof_test(int link, + const btVector3 &contact_point, + const btVector3 &normal_ang, + const btVector3 &normal_lin, + 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(3*num_links + 3); //(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang + 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_lin = v_ptr; v_ptr += num_links + 1; + btVector3 * n_local_ang = 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_lin_world = normal_lin; //convenience + const btVector3 &normal_ang_world = normal_ang; + + rot_from_world[0] = btMatrix3x3(m_baseQuat); + + // omega coeffients first. + btVector3 omega_coeffs_world; + omega_coeffs_world = p_minus_com_world.cross(normal_lin_world); + jac[0] = omega_coeffs_world[0] + normal_ang_world[0]; + jac[1] = omega_coeffs_world[1] + normal_ang_world[1]; + jac[2] = omega_coeffs_world[2] + normal_ang_world[2]; + // then v coefficients + jac[3] = normal_lin_world[0]; + jac[4] = normal_lin_world[1]; + jac[5] = normal_lin_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_lin[0] = rot_from_world[0] * normal_lin_world; + n_local_ang[0] = rot_from_world[0] * normal_ang_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_lin[i+1] = mtx * n_local_lin[parent+1]; + n_local_ang[i+1] = mtx * n_local_ang[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_lin[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] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0)); + break; + } + case btMultibodyLink::ePrismatic: + { + results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(0)); + break; + } + case btMultibodyLink::eSpherical: + { + results[m_links[i].m_dofOffset + 0] = n_local_lin[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_lin[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_lin[i+1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(2)); + + results[m_links[i].m_dofOffset + 0] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0)); + results[m_links[i].m_dofOffset + 1] += n_local_ang[i+1].dot(m_links[i].getAxisTop(1)); + results[m_links[i].m_dofOffset + 2] += n_local_ang[i+1].dot(m_links[i].getAxisTop(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::fillContactJacobian(int link, const btVector3 &contact_point, const btVector3 &normal, diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 41cb783d3..cc86cc4c8 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -397,6 +397,15 @@ public: btAlignedObjectArray &scratch_v, btAlignedObjectArray &scratch_m) const; + void fillContactJacobianMultiDof_test(int link, + const btVector3 &contact_point, + const btVector3 &normal_ang, + const btVector3 &normal_lin, + btScalar *jac, + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v, + btAlignedObjectArray &scratch_m) const; + // // sleeping diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index 609f92d63..d3045fd3e 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -1,5 +1,6 @@ #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), @@ -296,16 +297,20 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr 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); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp index 029f33449..d4fa1d43b 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp @@ -19,6 +19,8 @@ subject to the following restrictions: #include "btMultiBodyLinkCollider.h" #include "BulletDynamics/Dynamics/btRigidBody.h" +#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST + btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB) :btMultiBodyConstraint(body,0,link,-1,3,false), m_rigidBodyA(0), @@ -141,3 +143,152 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co } } + +#else + + + +#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->fillContactJacobianMultiDof_test(m_linkA, pivotAworld, normalAng, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); + + + fillMultiBodyConstraintMixed(constraintRow, data, + contactNormalOnB, + pivotAworld, pivotBworld, + position, + infoGlobal, + relaxation, + false); + constraintRow.m_lowerLimit = -m_maxAppliedImpulse; + constraintRow.m_upperLimit = m_maxAppliedImpulse; + + } +} + +#endif \ No newline at end of file diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h index 824a57393..cae8fa917 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h @@ -20,6 +20,8 @@ subject to the following restrictions: #include "btMultiBodyConstraint.h" +#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST + class btMultiBodyPoint2Point : public btMultiBodyConstraint { protected: