From 2920d7e61f06da7e47f79c7cb2d7d580e816f0b4 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 5 Nov 2015 21:17:46 -0800 Subject: [PATCH] Only support btMultiBody multi-dof version (remove non-multi-dof path) Use ATTRIBUTE_ALIGNED16 for btMultiBody Always disable parentCollision for btMultiBody::setupFixed --- .../MultiBodyCreationInterface.h | 2 +- .../ImportURDFDemo/MyMultiBodyCreator.cpp | 4 +- .../ImportURDFDemo/MyMultiBodyCreator.h | 2 +- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 6 +- .../MultiBody/InvertedPendulumPDControl.cpp | 13 +- .../MultiBody/MultiBodyConstraintFeedback.cpp | 13 +- examples/MultiBody/MultiDofDemo.cpp | 11 +- examples/MultiBody/TestJointTorqueSetup.cpp | 13 +- .../Featherstone/btMultiBody.cpp | 210 ++---------------- src/BulletDynamics/Featherstone/btMultiBody.h | 25 +-- .../Featherstone/btMultiBodyConstraint.cpp | 42 +--- .../btMultiBodyConstraintSolver.cpp | 113 ++++------ .../Featherstone/btMultiBodyDynamicsWorld.cpp | 25 +-- .../btMultiBodyJointLimitConstraint.cpp | 3 +- .../Featherstone/btMultiBodyJointMotor.cpp | 3 +- .../Featherstone/btMultiBodyLink.h | 14 -- 16 files changed, 125 insertions(+), 374 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h b/examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h index 2cbc2f5e2..696546d90 100644 --- a/examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h +++ b/examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h @@ -15,7 +15,7 @@ public: ///optionally create some graphical representation from a collision object, usually for visual debugging purposes. virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba) = 0; - virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof) =0; + virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep) =0; virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape) = 0; diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp index a92ca2153..dd36baa76 100644 --- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp +++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp @@ -18,12 +18,12 @@ m_guiHelper(guiHelper) } - class btMultiBody* MyMultiBodyCreator::allocateMultiBody(int /* urdfLinkIndex */, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof) + class btMultiBody* MyMultiBodyCreator::allocateMultiBody(int /* urdfLinkIndex */, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep) { // m_urdf2mbLink.resize(totalNumJoints+1,-2); m_mb2urdfLink.resize(totalNumJoints+1,-2); - m_bulletMultiBody = new btMultiBody(totalNumJoints,mass,localInertiaDiagonal,isFixedBase,canSleep,multiDof); + m_bulletMultiBody = new btMultiBody(totalNumJoints,mass,localInertiaDiagonal,isFixedBase,canSleep); return m_bulletMultiBody; } diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h index 99dbc9fc9..b19146022 100644 --- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h +++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h @@ -43,7 +43,7 @@ public: ///optionally create some graphical representation from a collision object, usually for visual debugging purposes. virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba); - virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep, bool multiDof); + virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep); virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape); diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index b1b09eeb9..ff93b7f5a 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -241,11 +241,11 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat { if (cache.m_bulletMultiBody==0) { - bool multiDof = true; + bool canSleep = false; bool isFixedBase = (mass==0);//todo: figure out when base is fixed int totalNumJoints = cache.m_totalNumJoints1; - cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof); + cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep); cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame); } @@ -271,7 +271,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat //b3Printf("Fixed joint (btMultiBody)\n"); btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation(); cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex, - rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin(),disableParentCollision); + rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin()); creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); diff --git a/examples/MultiBody/InvertedPendulumPDControl.cpp b/examples/MultiBody/InvertedPendulumPDControl.cpp index 59c8b5770..371065199 100644 --- a/examples/MultiBody/InvertedPendulumPDControl.cpp +++ b/examples/MultiBody/InvertedPendulumPDControl.cpp @@ -136,8 +136,8 @@ void InvertedPendulumPDControl::initPhysics() delete shape; } - bool isMultiDof = true; - btMultiBody *pMultiBody = new btMultiBody(numLinks, 0, baseInertiaDiag, fixedBase, canSleep, isMultiDof); + + btMultiBody *pMultiBody = new btMultiBody(numLinks, 0, baseInertiaDiag, fixedBase, canSleep); m_multiBody = pMultiBody; btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); @@ -201,7 +201,7 @@ void InvertedPendulumPDControl::initPhysics() pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, - currentPivotToCurrentCom, false); + currentPivotToCurrentCom); } //pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false); @@ -249,10 +249,9 @@ void InvertedPendulumPDControl::initPhysics() { btScalar q0 = 180.f * SIMD_PI/ 180.f; if(!spherical) - if(mbC->isMultiDof()) - mbC->setJointPosMultiDof(0, &q0); - else - mbC->setJointPos(0, q0); + { + mbC->setJointPosMultiDof(0, &q0); + } else { btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0); diff --git a/examples/MultiBody/MultiBodyConstraintFeedback.cpp b/examples/MultiBody/MultiBodyConstraintFeedback.cpp index dc3b79158..a1a787534 100644 --- a/examples/MultiBody/MultiBodyConstraintFeedback.cpp +++ b/examples/MultiBody/MultiBodyConstraintFeedback.cpp @@ -131,8 +131,8 @@ void MultiBodyConstraintFeedbackSetup::initPhysics() delete shape; } - bool isMultiDof = true; - btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof); + + btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep); m_multiBody = pMultiBody; btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); @@ -196,7 +196,7 @@ void MultiBodyConstraintFeedbackSetup::initPhysics() pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, - currentPivotToCurrentCom, false); + currentPivotToCurrentCom); } //pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false); @@ -244,10 +244,9 @@ void MultiBodyConstraintFeedbackSetup::initPhysics() { btScalar q0 = 45.f * SIMD_PI/ 180.f; if(!spherical) - if(mbC->isMultiDof()) - mbC->setJointPosMultiDof(0, &q0); - else - mbC->setJointPos(0, q0); + { + mbC->setJointPosMultiDof(0, &q0); + } else { btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0); diff --git a/examples/MultiBody/MultiDofDemo.cpp b/examples/MultiBody/MultiDofDemo.cpp index fb96e42dc..f4dc2f673 100644 --- a/examples/MultiBody/MultiDofDemo.cpp +++ b/examples/MultiBody/MultiDofDemo.cpp @@ -161,10 +161,9 @@ void MultiDofDemo::initPhysics() { btScalar q0 = 45.f * SIMD_PI/ 180.f; if(!spherical) - if(mbC->isMultiDof()) - mbC->setJointPosMultiDof(0, &q0); - else - mbC->setJointPos(0, q0); + { + mbC->setJointPosMultiDof(0, &q0); + } else { btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0); @@ -260,8 +259,8 @@ btMultiBody* MultiDofDemo::createFeatherstoneMultiBody_testMultiDof(btMultiBodyD } bool canSleep = false; - bool isMultiDof = true; - btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof); + + btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep); btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); pMultiBody->setBasePos(basePosition); diff --git a/examples/MultiBody/TestJointTorqueSetup.cpp b/examples/MultiBody/TestJointTorqueSetup.cpp index ced84a958..8647b8734 100644 --- a/examples/MultiBody/TestJointTorqueSetup.cpp +++ b/examples/MultiBody/TestJointTorqueSetup.cpp @@ -135,8 +135,8 @@ void TestJointTorqueSetup::initPhysics() delete shape; } - bool isMultiDof = true; - btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof); + + btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep); m_multiBody = pMultiBody; btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); @@ -200,7 +200,7 @@ void TestJointTorqueSetup::initPhysics() pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, - currentPivotToCurrentCom, false); + currentPivotToCurrentCom); } //pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false); @@ -248,10 +248,9 @@ void TestJointTorqueSetup::initPhysics() { btScalar q0 = 45.f * SIMD_PI/ 180.f; if(!spherical) - if(mbC->isMultiDof()) - mbC->setJointPosMultiDof(0, &q0); - else - mbC->setJointPos(0, q0); + { + mbC->setJointPosMultiDof(0, &q0); + } else { btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0); diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 1f6f7aa90..96ef4e87e 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -91,8 +91,7 @@ btMultiBody::btMultiBody(int n_links, btScalar mass, const btVector3 &inertia, bool fixedBase, - bool canSleep, - bool multiDof) + bool canSleep) : m_baseCollider(0), m_baseName(0), @@ -112,7 +111,6 @@ btMultiBody::btMultiBody(int n_links, m_maxAppliedImpulse(1000.f), m_maxCoordinateVelocity(100.f), m_hasSelfCollision(true), - m_isMultiDof(multiDof), __posUpdated(false), m_dofCount(0), m_posVarCnt(0), @@ -120,14 +118,6 @@ btMultiBody::btMultiBody(int n_links, m_useGlobalVelocities(false), m_internalNeedsJointFeedback(false) { - - if(!m_isMultiDof) - { - m_vectorBuf.resize(2*n_links); - m_realBuf.resize(6 + 2*n_links); - m_posVarCnt = n_links; - } - m_links.resize(n_links); m_matrixBuf.resize(n_links + 1); @@ -146,11 +136,9 @@ void btMultiBody::setupFixed(int i, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, - const btVector3 &thisPivotToThisComOffset, - bool disableParentCollision) + const btVector3 &thisPivotToThisComOffset) { - btAssert(m_isMultiDof); - + m_links[i].m_mass = mass; m_links[i].m_inertiaLocal = inertia; m_links[i].m_parent = parent; @@ -162,15 +150,10 @@ void btMultiBody::setupFixed(int i, m_links[i].m_dofCount = 0; m_links[i].m_posVarCount = 0; - if (disableParentCollision) - m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; - // + m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + m_links[i].updateCacheMultiDof(); - // - //if(m_isMultiDof) - // resizeInternalMultiDofBuffers(); - // updateLinksDofOffsets(); } @@ -186,12 +169,9 @@ void btMultiBody::setupPrismatic(int i, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision) { - if(m_isMultiDof) - { - m_dofCount += 1; - m_posVarCnt += 1; - } - + m_dofCount += 1; + m_posVarCnt += 1; + m_links[i].m_mass = mass; m_links[i].m_inertiaLocal = inertia; m_links[i].m_parent = parent; @@ -211,13 +191,10 @@ void btMultiBody::setupPrismatic(int i, if (disableParentCollision) m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; // - if(m_isMultiDof) - m_links[i].updateCacheMultiDof(); - else - m_links[i].updateCache(); - // - if(m_isMultiDof) - updateLinksDofOffsets(); + + m_links[i].updateCacheMultiDof(); + + updateLinksDofOffsets(); } void btMultiBody::setupRevolute(int i, @@ -230,12 +207,9 @@ void btMultiBody::setupRevolute(int i, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision) { - if(m_isMultiDof) - { - m_dofCount += 1; - m_posVarCnt += 1; - } - + m_dofCount += 1; + m_posVarCnt += 1; + m_links[i].m_mass = mass; m_links[i].m_inertiaLocal = inertia; m_links[i].m_parent = parent; @@ -254,13 +228,9 @@ void btMultiBody::setupRevolute(int i, if (disableParentCollision) m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; // - if(m_isMultiDof) - m_links[i].updateCacheMultiDof(); - else - m_links[i].updateCache(); + m_links[i].updateCacheMultiDof(); // - if(m_isMultiDof) - updateLinksDofOffsets(); + updateLinksDofOffsets(); } @@ -274,7 +244,7 @@ void btMultiBody::setupSpherical(int i, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision) { - btAssert(m_isMultiDof); + m_dofCount += 3; m_posVarCnt += 4; @@ -315,7 +285,7 @@ void btMultiBody::setupPlanar(int i, const btVector3 &parentComToThisComOffset, bool disableParentCollision) { - btAssert(m_isMultiDof); + m_dofCount += 3; m_posVarCnt += 3; @@ -357,7 +327,6 @@ void btMultiBody::setupPlanar(int i, void btMultiBody::finalizeMultiDof() { - btAssert(m_isMultiDof); m_deltaV.resize(0); m_deltaV.resize(6 + m_dofCount); m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels") @@ -388,7 +357,7 @@ btScalar btMultiBody::getJointPos(int i) const btScalar btMultiBody::getJointVel(int i) const { - return m_realBuf[6 + i]; + return m_realBuf[6 + m_links[i].m_dofOffset]; } btScalar * btMultiBody::getJointPosMultiDof(int i) @@ -415,7 +384,7 @@ const btScalar * btMultiBody::getJointVelMultiDof(int i) const void btMultiBody::setJointPos(int i, btScalar q) { m_links[i].m_jointPos[0] = q; - m_links[i].updateCache(); + m_links[i].updateCacheMultiDof(); } void btMultiBody::setJointPosMultiDof(int i, btScalar *q) @@ -428,7 +397,7 @@ void btMultiBody::setJointPosMultiDof(int i, btScalar *q) void btMultiBody::setJointVel(int i, btScalar qdot) { - m_realBuf[6 + i] = qdot; + m_realBuf[6 + m_links[i].m_dofOffset] = qdot; } void btMultiBody::setJointVelMultiDof(int i, btScalar *qdot) @@ -1939,41 +1908,6 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output //int dummy = 0; } -void btMultiBody::stepPositions(btScalar dt) -{ - int num_links = getNumLinks(); - // step position by adding dt * velocity - btVector3 v = getBaseVel(); - m_basePos += dt * v; - - // "exponential map" method for the rotation - btVector3 base_omega = getBaseOmega(); - const btScalar omega_norm = base_omega.norm(); - const btScalar omega_times_dt = omega_norm * dt; - const btScalar SMALL_ROTATION_ANGLE = 0.02f; // Theoretically this should be ~ pow(FLT_EPSILON,0.25) which is ~ 0.0156 - if (fabs(omega_times_dt) < SMALL_ROTATION_ANGLE) - { - const btScalar xsq = omega_times_dt * omega_times_dt; // |omega|^2 * dt^2 - const btScalar sin_term = dt * (xsq / 48.0f - 0.5f); // -sin(0.5*dt*|omega|) / |omega| - const btScalar cos_term = 1.0f - xsq / 8.0f; // cos(0.5*dt*|omega|) - m_baseQuat = m_baseQuat * btQuaternion(sin_term * base_omega[0],sin_term * base_omega[1],sin_term * base_omega[2],cos_term); - } else - { - m_baseQuat = m_baseQuat * btQuaternion(base_omega / omega_norm,-omega_times_dt); - } - - // Make sure the quaternion represents a valid rotation. - // (Not strictly necessary, but helps prevent any round-off errors from building up.) - m_baseQuat.normalize(); - - // Finally we can update m_jointPos for each of the m_links - for (int i = 0; i < num_links; ++i) - { - float jointVel = getJointVel(i); - m_links[i].m_jointPos[0] += dt * jointVel; - m_links[i].updateCache(); - } -} void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd) { @@ -2238,98 +2172,6 @@ void btMultiBody::filConstraintJacobianMultiDof(int link, } } -void btMultiBody::fillContactJacobian(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(); - scratch_v.resize(2*num_links + 2); - scratch_m.resize(num_links + 1); - - btVector3 * v_ptr = &scratch_v[0]; - btVector3 * p_minus_com = 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(num_links); - 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; - - rot_from_world[0] = btMatrix3x3(m_baseQuat); - - p_minus_com[0] = rot_from_world[0] * p_minus_com_world; - n_local[0] = rot_from_world[0] * normal; - - // omega coeffients first. - if (this->m_fixedBase) - { - for (int i=0;i<6;i++) - { - jac[i]=0; - } - } else - { - btVector3 omega_coeffs; - - omega_coeffs = p_minus_com_world.cross(normal); - jac[0] = omega_coeffs[0]; - jac[1] = omega_coeffs[1]; - jac[2] = omega_coeffs[2]; - // then v coefficients - jac[3] = normal[0]; - jac[4] = normal[1]; - jac[5] = normal[2]; - } - // Set remaining jac values to zero for now. - for (int i = 6; i < 6 + num_links; ++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[i+1] = mtx * p_minus_com[parent+1] - m_links[i].m_cachedRVector; - - // calculate the jacobian entry - if (m_links[i].m_jointType == btMultibodyLink::eRevolute) { - results[i] = n_local[i+1].dot( m_links[i].getAxisTop(0).cross(p_minus_com[i+1]) + m_links[i].getAxisBottom(0) ); - } else { - results[i] = n_local[i+1].dot( m_links[i].getAxisBottom(0) ); - } - } - - // Now copy through to output. - //printf("jac[%d] = ", link); - while (link != -1) - { - jac[6 + link] = results[link]; - //printf("%.2f\t", jac[6 + link]); - link = m_links[link].m_parent; - } - //printf("]\n"); - } -} void btMultiBody::wakeUp() { @@ -2354,17 +2196,11 @@ void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep) // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities) btScalar motion = 0; - if(m_isMultiDof) { for (int i = 0; i < 6 + m_dofCount; ++i) motion += m_realBuf[i] * m_realBuf[i]; } - else - { - for (int i = 0; i < 6 + num_links; ++i) - motion += m_realBuf[i] * m_realBuf[i]; - } - + if (motion < SLEEP_EPSILON) { m_sleepTimer += timestep; diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index b81f6335f..7168b5fe0 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -45,7 +45,7 @@ #include "btMultiBodyLink.h" class btMultiBodyLinkCollider; -class btMultiBody +ATTRIBUTE_ALIGNED16(class) btMultiBody { public: @@ -60,21 +60,19 @@ public: btScalar mass, // mass of base const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal bool fixedBase, // whether the base is fixed (true) or can move (false) - bool canSleep, - bool multiDof = false - ); + bool canSleep); virtual ~btMultiBody(); + //note: fixed link collision with parent is always disabled void setupFixed(int linkIndex, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, - const btVector3 &thisPivotToThisComOffset, - bool disableParentCollision); + const btVector3 &thisPivotToThisComOffset); void setupPrismatic(int i, @@ -447,7 +445,6 @@ void addJointTorque(int i, btScalar Q); // timestep the positions (given current velocities). - void stepPositions(btScalar dt); void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0); @@ -458,15 +455,7 @@ void addJointTorque(int i, btScalar Q); // This routine fills out a contact constraint jacobian for this body. // the 'normal' supplied must be -n for body1 or +n for body2 of the contact. // 'normal' & 'contact_point' are both given in world coordinates. - void fillContactJacobian(int link, - const btVector3 &contact_point, - const btVector3 &normal, - btScalar *jac, - btAlignedObjectArray &scratch_r, - btAlignedObjectArray &scratch_v, - btAlignedObjectArray &scratch_m) const; - - //multidof version of fillContactJacobian + void fillContactJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal, @@ -576,7 +565,7 @@ void addJointTorque(int i, btScalar Q); return m_hasSelfCollision; } - bool isMultiDof() { return m_isMultiDof; } + void finalizeMultiDof(); void useRK4Integration(bool use) { m_useRK4 = use; } @@ -700,7 +689,7 @@ private: btScalar m_maxAppliedImpulse; btScalar m_maxCoordinateVelocity; bool m_hasSelfCollision; - bool m_isMultiDof; + bool __posUpdated; int m_dofCount, m_posVarCnt; bool m_useRK4, m_useGlobalVelocities; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index e9e285f3f..12997d2e3 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -23,18 +23,12 @@ void btMultiBodyConstraint::updateJacobianSizes() { if(m_bodyA) { - if(m_bodyA->isMultiDof()) - m_jacSizeA = (6 + m_bodyA->getNumDofs()); - else - m_jacSizeA = (6 + m_bodyA->getNumLinks()); + m_jacSizeA = (6 + m_bodyA->getNumDofs()); } if(m_bodyB) { - if(m_bodyB->isMultiDof()) - m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs(); - else - m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumLinks(); + m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs(); } else m_jacSizeBoth = m_jacSizeA; @@ -101,7 +95,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); } - const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; + const int ndofA = multiBodyA->getNumDofs() + 6; solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); @@ -128,10 +122,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr else { 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); + multiBodyA->fillContactJacobianMultiDof(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) @@ -140,10 +131,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr 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 - multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); + 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; @@ -167,7 +155,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); } - const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; + const int ndofB = multiBodyB->getNumDofs() + 6; solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); if (solverConstraint.m_deltaVelBindex <0) @@ -189,10 +177,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr } else { - 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); + 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) @@ -201,10 +186,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; //determine.. - if(multiBodyB->isMultiDof()) - multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v); - else - multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v); + multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v); btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); solverConstraint.m_relpos2CrossNormal = -torqueAxis1; @@ -231,7 +213,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr //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; + ndofA = multiBodyA->getNumDofs() + 6; jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; for (int i = 0; i < ndofA; ++i) @@ -249,7 +231,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr // if (multiBodyB) { - const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; + const int ndofB = multiBodyB->getNumDofs() + 6; jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; for (int i = 0; i < ndofB; ++i) @@ -290,7 +272,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr btVector3 vel1,vel2; if (multiBodyA) { - ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; + ndofA = multiBodyA->getNumDofs() + 6; btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; for (int i = 0; i < ndofA ; ++i) rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; @@ -301,7 +283,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr } if (multiBodyB) { - ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; + ndofB = multiBodyB->getNumDofs() + 6; btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; for (int i = 0; i < ndofB ; ++i) rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 1f355eee6..9f2e84aba 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -123,7 +123,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult if (c.m_multiBodyA) { - ndofA = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6; + ndofA = c.m_multiBodyA->getNumDofs() + 6; for (int i = 0; i < ndofA; ++i) deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i]; } else if(c.m_solverBodyIdA >= 0) @@ -134,7 +134,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult if (c.m_multiBodyB) { - ndofB = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6; + ndofB = c.m_multiBodyB->getNumDofs() + 6; for (int i = 0; i < ndofB; ++i) deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i]; } else if(c.m_solverBodyIdB >= 0) @@ -169,10 +169,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - if(c.m_multiBodyA->isMultiDof()) - c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); - else - c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); + c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS } else if(c.m_solverBodyIdA >= 0) { @@ -185,10 +182,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - if(c.m_multiBodyB->isMultiDof()) - c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); - else - c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); + c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS } else if(c.m_solverBodyIdB >= 0) { @@ -209,14 +203,14 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(con if (c.m_multiBodyA) { - ndofA = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6; + ndofA = c.m_multiBodyA->getNumDofs() + 6; for (int i = 0; i < ndofA; ++i) deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i]; } if (c.m_multiBodyB) { - ndofB = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6; + ndofB = c.m_multiBodyB->getNumDofs() + 6; for (int i = 0; i < ndofB; ++i) deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i]; } @@ -296,7 +290,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol { rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); } - const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; + const int ndofA = multiBodyA->getNumDofs() + 6; solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); @@ -316,15 +310,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex]; - if(multiBodyA->isMultiDof()) - multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); - else - multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - if(multiBodyA->isMultiDof()) - multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); - else - multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); + multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); btVector3 torqueAxis0 = rel_pos1.cross(contactNormal); solverConstraint.m_relpos1CrossNormal = torqueAxis0; @@ -349,7 +337,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); } - const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; + const int ndofB = multiBodyB->getNumDofs() + 6; solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); if (solverConstraint.m_deltaVelBindex <0) @@ -365,14 +353,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB); btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); - if(multiBodyB->isMultiDof()) - multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); - else - multiBodyB->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); - if(multiBodyB->isMultiDof()) - multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); - else - multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); + multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); solverConstraint.m_relpos2CrossNormal = -torqueAxis1; @@ -399,7 +381,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol int ndofA = 0; if (multiBodyA) { - ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; + ndofA = multiBodyA->getNumDofs() + 6; jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; for (int i = 0; i < ndofA; ++i) @@ -418,7 +400,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol } if (multiBodyB) { - const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; + const int ndofB = multiBodyB->getNumDofs() + 6; jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; for (int i = 0; i < ndofB; ++i) @@ -467,7 +449,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol btVector3 vel1,vel2; if (multiBodyA) { - ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6; + ndofA = multiBodyA->getNumDofs() + 6; btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; for (int i = 0; i < ndofA ; ++i) rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; @@ -480,7 +462,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol } if (multiBodyB) { - ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6; + ndofB = multiBodyB->getNumDofs() + 6; btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; for (int i = 0; i < ndofB ; ++i) rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; @@ -518,10 +500,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol { btScalar impulse = solverConstraint.m_appliedImpulse; btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - if(multiBodyA->isMultiDof()) - multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse); - else - multiBodyA->applyDeltaVee(deltaV,impulse); + multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse); + applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); } else { @@ -532,10 +512,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol { btScalar impulse = solverConstraint.m_appliedImpulse; btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - if(multiBodyB->isMultiDof()) - multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse); - else - multiBodyB->applyDeltaVee(deltaV,impulse); + multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse); applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); } else { @@ -904,45 +881,39 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv if (c.m_multiBodyA) { - if(c.m_multiBodyA->isMultiDof()) + c.m_multiBodyA->setCompanionId(-1); + btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime); + btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime); + if (c.m_linkA<0) { - c.m_multiBodyA->setCompanionId(-1); - btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime); - btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime); - if (c.m_linkA<0) - { - c.m_multiBodyA->addBaseConstraintForce(force); - c.m_multiBodyA->addBaseConstraintTorque(torque); - } else - { - c.m_multiBodyA->addLinkConstraintForce(c.m_linkA,force); - //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]); - c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA,torque); - } + c.m_multiBodyA->addBaseConstraintForce(force); + c.m_multiBodyA->addBaseConstraintTorque(torque); + } else + { + c.m_multiBodyA->addLinkConstraintForce(c.m_linkA,force); + //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]); + c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA,torque); } } if (c.m_multiBodyB) { - if(c.m_multiBodyB->isMultiDof()) { + c.m_multiBodyB->setCompanionId(-1); + btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime); + btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime); + if (c.m_linkB<0) + { + c.m_multiBodyB->addBaseConstraintForce(force); + c.m_multiBodyB->addBaseConstraintTorque(torque); + } else { - c.m_multiBodyB->setCompanionId(-1); - btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime); - btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime); - if (c.m_linkB<0) { - c.m_multiBodyB->addBaseConstraintForce(force); - c.m_multiBodyB->addBaseConstraintTorque(torque); - } else - { - { - c.m_multiBodyB->addLinkConstraintForce(c.m_linkB,force); - //b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]); - c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB,torque); - } - + c.m_multiBodyB->addLinkConstraintForce(c.m_linkB,force); + //b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]); + c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB,torque); } + } } } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index fa4052d46..13676cc7e 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -505,7 +505,6 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) scratch_m.resize(bod->getNumLinks()+1); bool doNotUpdatePos = false; - if(bod->isMultiDof()) { if(!bod->isUsingRK4Integration()) { @@ -667,10 +666,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) } } - else//if(bod->isMultiDof()) - { - bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); - } + #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY bod->clearForcesAndTorques(); #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY @@ -709,13 +705,13 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) scratch_v.resize(bod->getNumLinks()+1); scratch_m.resize(bod->getNumLinks()+1); - if(bod->isMultiDof()) + + { + if(!bod->isUsingRK4Integration()) { - if(!bod->isUsingRK4Integration()) - { - bool isConstraintPass = true; - bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m, isConstraintPass); - } + bool isConstraintPass = true; + bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m, isConstraintPass); + } } } } @@ -760,7 +756,7 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) ///base + num m_links - if(bod->isMultiDof()) + { if(!bod->isPosUpdated()) bod->stepPositionsMultiDof(timeStep); @@ -773,10 +769,7 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) bod->setPosUpdated(false); } } - else - { - bod->stepPositions(timeStep); - } + world_to_local.resize(nLinks+1); local_origin.resize(nLinks+1); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index b2d864139..3f05aa4d5 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -38,7 +38,7 @@ void btMultiBodyJointLimitConstraint::finalizeMultiDof() allocateJacobiansMultiDof(); - unsigned int offset = 6 + (m_bodyA->isMultiDof() ? m_bodyA->getLink(m_linkA).m_dofOffset : m_linkA); + unsigned int offset = 6 + m_bodyA->getLink(m_linkA).m_dofOffset; // row 0: the lower bound jacobianA(0)[offset] = 1; @@ -124,7 +124,6 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse); - if (m_bodyA->isMultiDof()) { //expect either prismatic or revolute joint type for now btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic)); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index fc718fd0d..50268611b 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -39,7 +39,7 @@ void btMultiBodyJointMotor::finalizeMultiDof() // note: we rely on the fact that data.m_jacobians are // always initialized to zero by the Constraint ctor int linkDoF = 0; - unsigned int offset = 6 + (m_bodyA->isMultiDof() ? m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF : m_linkA); + unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF); // row 0: the lower bound // row 0: the lower bound @@ -118,7 +118,6 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity); constraintRow.m_orgConstraint = this; constraintRow.m_orgDofIndex = row; - if (m_bodyA->isMultiDof()) { //expect either prismatic or revolute joint type for now btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic)); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index 88eda8eeb..909e80e21 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -165,20 +165,6 @@ btVector3 m_appliedConstraintForce; // In WORLD frame } // routine to update m_cachedRotParentToThis and m_cachedRVector - void updateCache() - { - //multidof - if (m_jointType == eRevolute) - { - m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-m_jointPos[0]) * m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); - } else - { - // m_cachedRotParentToThis never changes, so no need to update - m_cachedRVector = m_eVector + m_jointPos[0] * getAxisBottom(0); - } - } - void updateCacheMultiDof(btScalar *pq = 0) { btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);