From 75f17509cc9bc537ef0a6f76cec49fea3b937c4b Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Wed, 2 Oct 2013 21:38:40 +0000 Subject: [PATCH] Add a virtual createConstraintRows method, to easier experiment with different kinds of btMultiBodyConstraint --- .../FeatherstoneMultiBodyDemo.cpp | 249 ++++++------ .../FeatherstoneMultiBodyDemo.h | 3 +- .../Featherstone/btMultiBodyConstraint.h | 18 +- .../btMultiBodyConstraintSolver.cpp | 355 +++--------------- .../btMultiBodyConstraintSolver.h | 15 +- .../btMultiBodyJointLimitConstraint.cpp | 259 ++++++++++++- .../btMultiBodyJointLimitConstraint.h | 13 +- .../btMultiBodySolverConstraint.h | 2 + 8 files changed, 463 insertions(+), 451 deletions(-) diff --git a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp index 20253f947..a094327dc 100644 --- a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp +++ b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp @@ -209,156 +209,157 @@ void FeatherstoneMultiBodyDemo::initPhysics() } - createFeatherstoneMultiBody(world, 5, btVector3 (20,29.5,-2), true, true); + btMultiBody* mb = createFeatherstoneMultiBody(world, 1, btVector3 (20,29.5,-2), true, true,true); - createFeatherstoneMultiBody(world, 5, btVector3 (0,29.5,-2), false,false); + + mb = createFeatherstoneMultiBody(world, 5, btVector3 (0,29.5,-2), false,false,true); } -void FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic) +btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic, bool canSleep) { - { - int n_links = numLinks; - float mass = 13.5; - btVector3 inertia(91,344,253); - bool canSleep = true;//false; - btMultiBody * bod = new btMultiBody(n_links, mass, inertia, isFixedBase, canSleep); + int n_links = numLinks; + float mass = 13.5; + btVector3 inertia(91,344,253); + + + btMultiBody * bod = new btMultiBody(n_links, mass, inertia, isFixedBase, canSleep); - //btQuaternion orn(btVector3(0,0,1),-0.25*SIMD_HALF_PI);//0,0,0,1); - btQuaternion orn(0,0,0,1); - bod->setBasePos(basePosition); - bod->setWorldToBaseRot(orn); - btVector3 vel(0,0,0); - bod->setBaseVel(vel); + //btQuaternion orn(btVector3(0,0,1),-0.25*SIMD_HALF_PI);//0,0,0,1); + btQuaternion orn(0,0,0,1); + bod->setBasePos(basePosition); + bod->setWorldToBaseRot(orn); + btVector3 vel(0,0,0); + bod->setBaseVel(vel); - { + { - btVector3 joint_axis_hinge(1,0,0); - btVector3 joint_axis_prismatic(0,0,1); - btQuaternion parent_to_child = orn.inverse(); - btVector3 joint_axis_child_prismatic = quatRotate(parent_to_child ,joint_axis_prismatic); - btVector3 joint_axis_child_hinge = quatRotate(parent_to_child , joint_axis_hinge); + btVector3 joint_axis_hinge(1,0,0); + btVector3 joint_axis_prismatic(0,0,1); + btQuaternion parent_to_child = orn.inverse(); + btVector3 joint_axis_child_prismatic = quatRotate(parent_to_child ,joint_axis_prismatic); + btVector3 joint_axis_child_hinge = quatRotate(parent_to_child , joint_axis_hinge); - int this_link_num = -1; - int link_num_counter = 0; + int this_link_num = -1; + int link_num_counter = 0; - btVector3 pos(0,0,9.0500002); + btVector3 pos(0,0,9.0500002); - btVector3 joint_axis_position(0,0,4.5250001); + btVector3 joint_axis_position(0,0,4.5250001); - for (int i=0;i0) - initial_joint_angle = -0.06f; - - const int child_link_num = link_num_counter++; - - if (usePrismatic && i==(n_links-1)) - { - bod->setupPrismatic(child_link_num, mass, inertia, this_link_num, - parent_to_child, joint_axis_child_prismatic, quatRotate(parent_to_child , pos)); - - } else - { - bod->setupRevolute(child_link_num, mass, inertia, this_link_num,parent_to_child, joint_axis_child_hinge, - joint_axis_position,quatRotate(parent_to_child , (pos - joint_axis_position))); - } - bod->setJointPos(child_link_num, initial_joint_angle); - this_link_num = i; - } - - //add some constraint limit - if (usePrismatic) - { - btMultiBodyConstraint* limit = new btMultiBodyJointLimitConstraint(bod,n_links-1,2,3); - world->addMultiBodyConstraint(limit); - } - } - - //add a collider for the base + for (int i=0;i world_to_local; - world_to_local.resize(n_links+1); + float initial_joint_angle=0.3; + if (i>0) + initial_joint_angle = -0.06f; - btAlignedObjectArray local_origin; - local_origin.resize(n_links+1); - world_to_local[0] = bod->getWorldToBaseRot(); - local_origin[0] = bod->getBasePos(); - //float halfExtents[3]={7.5,0.05,4.5}; - float halfExtents[3]={7.5,0.45,4.5}; + const int child_link_num = link_num_counter++; + + if (usePrismatic && i==(n_links-1)) { - - float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; - float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; + bod->setupPrismatic(child_link_num, mass, inertia, this_link_num, + parent_to_child, joint_axis_child_prismatic, quatRotate(parent_to_child , pos)); - - if (1) - { - btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])); - btRigidBody* body = new btRigidBody(mass,0,box,inertia); - btMultiBodyLinkCollider* multiBody= new btMultiBodyLinkCollider(bod,-1); - - body->setCollisionShape(box); - multiBody->setCollisionShape(box); - - btTransform tr; - tr.setIdentity(); - tr.setOrigin(local_origin[0]); - tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); - body->setWorldTransform(tr); - multiBody->setWorldTransform(tr); - - world->addCollisionObject(multiBody, btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter); - multiBody->setFriction(1); - bod->addLinkCollider(multiBody); - - } - } - - - for (int i=0;igetNumLinks();i++) + } else { - const int parent = bod->getParent(i); - world_to_local[i+1] = bod->getParentToLocalRot(i) * world_to_local[parent+1]; - local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , bod->getRVector(i))); + bod->setupRevolute(child_link_num, mass, inertia, this_link_num,parent_to_child, joint_axis_child_hinge, + joint_axis_position,quatRotate(parent_to_child , (pos - joint_axis_position))); } - - - for (int i=0;igetNumLinks();i++) - { - - btVector3 posr = local_origin[i+1]; - float pos[4]={posr.x(),posr.y(),posr.z(),1}; - - float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; - - btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])); - btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(bod,i); - - col->setCollisionShape(box); - btTransform tr; - tr.setIdentity(); - tr.setOrigin(posr); - tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); - col->setWorldTransform(tr); - col->setFriction(1); - world->addCollisionObject(col,btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter);//,2,1); - - bod->addLinkCollider(col); - //app->drawBox(halfExtents, pos,quat); - } - + bod->setJointPos(child_link_num, initial_joint_angle); + this_link_num = i; + } + + //add some constraint limit + if (usePrismatic) + { + btMultiBodyConstraint* limit = new btMultiBodyJointLimitConstraint(bod,n_links-1,2,3); + world->addMultiBodyConstraint(limit); } - world->addMultiBody(bod); } + //add a collider for the base + { + + btAlignedObjectArray world_to_local; + world_to_local.resize(n_links+1); + + btAlignedObjectArray local_origin; + local_origin.resize(n_links+1); + world_to_local[0] = bod->getWorldToBaseRot(); + local_origin[0] = bod->getBasePos(); + //float halfExtents[3]={7.5,0.05,4.5}; + float halfExtents[3]={7.5,0.45,4.5}; + { + + float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; + float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; + + + if (1) + { + btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])); + btRigidBody* body = new btRigidBody(mass,0,box,inertia); + btMultiBodyLinkCollider* multiBody= new btMultiBodyLinkCollider(bod,-1); + + body->setCollisionShape(box); + multiBody->setCollisionShape(box); + + btTransform tr; + tr.setIdentity(); + tr.setOrigin(local_origin[0]); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + body->setWorldTransform(tr); + multiBody->setWorldTransform(tr); + + world->addCollisionObject(multiBody, btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter); + multiBody->setFriction(1); + bod->addLinkCollider(multiBody); + + } + } + + + for (int i=0;igetNumLinks();i++) + { + const int parent = bod->getParent(i); + world_to_local[i+1] = bod->getParentToLocalRot(i) * world_to_local[parent+1]; + local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , bod->getRVector(i))); + } + + + for (int i=0;igetNumLinks();i++) + { + + btVector3 posr = local_origin[i+1]; + float pos[4]={posr.x(),posr.y(),posr.z(),1}; + + float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; + + btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(bod,i); + + col->setCollisionShape(box); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + col->setWorldTransform(tr); + col->setFriction(1); + world->addCollisionObject(col,btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter);//,2,1); + + bod->addLinkCollider(col); + //app->drawBox(halfExtents, pos,quat); + } + + } + world->addMultiBody(bod); + + return bod; } diff --git a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.h b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.h index 039afb574..7f6c25a08 100644 --- a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.h +++ b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.h @@ -28,6 +28,7 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" +class btMultiBody; class btBroadphaseInterface; class btCollisionShape; class btOverlappingPairCache; @@ -53,7 +54,7 @@ class FeatherstoneMultiBodyDemo : public PlatformDemoApplication btDefaultCollisionConfiguration* m_collisionConfiguration; - void createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic); + btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic, bool canSleep); public: diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index e6bd2b134..99df50c87 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -21,6 +21,20 @@ subject to the following restrictions: #include "btMultiBody.h" class btMultiBody; +struct btSolverInfo; + +#include "btMultiBodySolverConstraint.h" + +struct btMultiBodyJacobianData +{ + btAlignedObjectArray m_jacobians; + btAlignedObjectArray m_deltaVelocitiesUnitImpulse; + btAlignedObjectArray m_deltaVelocities; + btAlignedObjectArray scratch_r; + btAlignedObjectArray scratch_v; + btAlignedObjectArray scratch_m; +}; + class btMultiBodyConstraint { @@ -62,7 +76,9 @@ public: virtual int getIslandIdA() const =0; virtual int getIslandIdB() const =0; - virtual void update()=0; + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal)=0; int getNumRows() const { diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 041779ea9..9645da57a 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -19,6 +19,7 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btSolverBody.h" #include "btMultiBodyConstraint.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" #include "LinearMath/btQuickprof.h" @@ -33,7 +34,8 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl { btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[j]; //if (iteration < constraint.m_overrideNumSolverIterations) - resolveSingleConstraintRowGenericMultiBody(constraint); + //resolveSingleConstraintRowGenericMultiBody(constraint); + resolveSingleConstraintRowGeneric(constraint); } //solve featherstone normal contact @@ -69,9 +71,9 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionOb m_multiBodyNonContactConstraints.resize(0); m_multiBodyNormalContactConstraints.resize(0); m_multiBodyFrictionContactConstraints.resize(0); - m_jacobians.resize(0); - m_deltaVelocitiesUnitImpulse.resize(0); - m_deltaVelocities.resize(0); + m_data.m_jacobians.resize(0); + m_data.m_deltaVelocitiesUnitImpulse.resize(0); + m_data.m_deltaVelocities.resize(0); for (int i=0;igetNumLinks() + 6; for (int i = 0; i < ndofA; ++i) - deltaVelADotn += m_jacobians[c.m_jacAindex+i] * m_deltaVelocities[c.m_deltaVelAindex+i]; + deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i]; } else { bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA]; @@ -119,7 +121,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult { ndofB = c.m_multiBodyB->getNumLinks() + 6; for (int i = 0; i < ndofB; ++i) - deltaVelBDotn += m_jacobians[c.m_jacBindex+i] * m_deltaVelocities[c.m_deltaVelBindex+i]; + deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i]; } else { bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB]; @@ -148,8 +150,8 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult if (c.m_multiBodyA) { - applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); - c.m_multiBodyA->applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); + c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); } else { bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse); @@ -157,8 +159,8 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult } if (c.m_multiBodyB) { - applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); - c.m_multiBodyB->applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); + c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); } else { bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse); @@ -180,14 +182,14 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(con { ndofA = c.m_multiBodyA->getNumLinks() + 6; for (int i = 0; i < ndofA; ++i) - deltaVelADotn += m_jacobians[c.m_jacAindex+i] * m_deltaVelocities[c.m_deltaVelAindex+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->getNumLinks() + 6; for (int i = 0; i < ndofB; ++i) - deltaVelBDotn += m_jacobians[c.m_jacBindex+i] * m_deltaVelocities[c.m_deltaVelBindex+i]; + deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i]; } @@ -212,13 +214,13 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(con if (c.m_multiBodyA) { - applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); - c.m_multiBodyA->applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); + c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); } if (c.m_multiBodyB) { - applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); - c.m_multiBodyB->applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); + c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); } } @@ -261,23 +263,23 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol if (solverConstraint.m_deltaVelAindex <0) { - solverConstraint.m_deltaVelAindex = m_deltaVelocities.size(); + solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size(); multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); - m_deltaVelocities.resize(m_deltaVelocities.size()+ndofA); + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA); } else { - btAssert(m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); + btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); } - solverConstraint.m_jacAindex = m_jacobians.size(); - m_jacobians.resize(m_jacobians.size()+ndofA); - m_deltaVelocitiesUnitImpulse.resize(m_deltaVelocitiesUnitImpulse.size()+ndofA); - btAssert(m_jacobians.size() == m_deltaVelocitiesUnitImpulse.size()); + solverConstraint.m_jacAindex = m_data.m_jacobians.size(); + m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA); + btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); - float* jac1=&m_jacobians[solverConstraint.m_jacAindex]; - multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, scratch_r, scratch_v, scratch_m); - float* delta = &m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->calcAccelerationDeltas(&m_jacobians[solverConstraint.m_jacAindex],delta,scratch_r, scratch_v); + float* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex]; + multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + float* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); } else { btVector3 torqueAxis0 = rel_pos1.cross(contactNormal); @@ -293,19 +295,19 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); if (solverConstraint.m_deltaVelBindex <0) { - solverConstraint.m_deltaVelBindex = m_deltaVelocities.size(); + solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size(); multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); - m_deltaVelocities.resize(m_deltaVelocities.size()+ndofB); + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB); } - solverConstraint.m_jacBindex = m_jacobians.size(); + solverConstraint.m_jacBindex = m_data.m_jacobians.size(); - m_jacobians.resize(m_jacobians.size()+ndofB); - m_deltaVelocitiesUnitImpulse.resize(m_deltaVelocitiesUnitImpulse.size()+ndofB); - btAssert(m_jacobians.size() == m_deltaVelocitiesUnitImpulse.size()); + m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB); + btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); - multiBodyB->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_jacobians[solverConstraint.m_jacBindex], scratch_r, scratch_v, scratch_m); - multiBodyB->calcAccelerationDeltas(&m_jacobians[solverConstraint.m_jacBindex],&m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],scratch_r, scratch_v); + 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); + multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); } else { btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); @@ -327,8 +329,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol if (multiBodyA) { ndofA = multiBodyA->getNumLinks() + 6; - jacA = &m_jacobians[solverConstraint.m_jacAindex]; - lambdaA = &m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; + lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; for (int i = 0; i < ndofA; ++i) { float j = jacA[i] ; @@ -346,8 +348,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol if (multiBodyB) { const int ndofB = multiBodyB->getNumLinks() + 6; - jacB = &m_jacobians[solverConstraint.m_jacBindex]; - lambdaB = &m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; + lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; for (int i = 0; i < ndofB; ++i) { float j = jacB[i] ; @@ -403,7 +405,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol if (multiBodyA) { ndofA = multiBodyA->getNumLinks() + 6; - btScalar* jacA = &m_jacobians[solverConstraint.m_jacAindex]; + btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; for (int i = 0; i < ndofA ; ++i) rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; } else @@ -416,7 +418,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol if (multiBodyB) { ndofB = multiBodyB->getNumLinks() + 6; - btScalar* jacB = &m_jacobians[solverConstraint.m_jacBindex]; + btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; for (int i = 0; i < ndofB ; ++i) rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; @@ -449,7 +451,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol if (multiBodyA) { btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; multiBodyA->applyDeltaVee(deltaV,impulse); applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); } else @@ -460,7 +462,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol if (multiBodyB) { btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; multiBodyB->applyDeltaVee(deltaV,impulse); applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); } else @@ -524,257 +526,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol -void btMultiBodyConstraintSolver::setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, - btScalar* jacOrgA,btScalar* jacOrgB, - btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff, - const btContactSolverInfo& infoGlobal) -{ - - BT_PROFILE("setupMultiBodyContactConstraint"); - - btMultiBody* multiBodyA = constraintRow.m_multiBodyA; - btMultiBody* multiBodyB = constraintRow.m_multiBodyB; - - if (multiBodyA) - { - - const int ndofA = multiBodyA->getNumLinks() + 6; - - constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId(); - - if (constraintRow.m_deltaVelAindex <0) - { - constraintRow.m_deltaVelAindex = m_deltaVelocities.size(); - multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex); - m_deltaVelocities.resize(m_deltaVelocities.size()+ndofA); - } else - { - btAssert(m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA); - } - - constraintRow.m_jacAindex = m_jacobians.size(); - m_jacobians.resize(m_jacobians.size()+ndofA); - m_deltaVelocitiesUnitImpulse.resize(m_deltaVelocitiesUnitImpulse.size()+ndofA); - btAssert(m_jacobians.size() == m_deltaVelocitiesUnitImpulse.size()); - for (int i=0;icalcAccelerationDeltas(&m_jacobians[constraintRow.m_jacAindex],delta,scratch_r, scratch_v); - } - - if (multiBodyB) - { - const int ndofB = multiBodyB->getNumLinks() + 6; - - constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId(); - if (constraintRow.m_deltaVelBindex <0) - { - constraintRow.m_deltaVelBindex = m_deltaVelocities.size(); - multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex); - m_deltaVelocities.resize(m_deltaVelocities.size()+ndofB); - } - - constraintRow.m_jacBindex = m_jacobians.size(); - m_jacobians.resize(m_jacobians.size()+ndofB); - - for (int i=0;icalcAccelerationDeltas(&m_jacobians[constraintRow.m_jacBindex],&m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],scratch_r, scratch_v); - } - { - - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - btScalar* jacB = 0; - btScalar* jacA = 0; - btScalar* lambdaA =0; - btScalar* lambdaB =0; - int ndofA = 0; - if (multiBodyA) - { - ndofA = multiBodyA->getNumLinks() + 6; - jacA = &m_jacobians[constraintRow.m_jacAindex]; - lambdaA = &m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex]; - for (int i = 0; i < ndofA; ++i) - { - float j = jacA[i] ; - float l =lambdaA[i]; - denom0 += j*l; - } - } - if (multiBodyB) - { - const int ndofB = multiBodyB->getNumLinks() + 6; - jacB = &m_jacobians[constraintRow.m_jacBindex]; - lambdaB = &m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex]; - for (int i = 0; i < ndofB; ++i) - { - float j = jacB[i] ; - float 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]; - } - } - - float 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 restitution = 0.f; - - - btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; - { - - btVector3 vel1,vel2; - if (multiBodyA) - { - ndofA = multiBodyA->getNumLinks() + 6; - btScalar* jacA = &m_jacobians[constraintRow.m_jacAindex]; - for (int i = 0; i < ndofA ; ++i) - rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; - } - if (multiBodyB) - { - ndofB = multiBodyB->getNumLinks() + 6; - btScalar* jacB = &m_jacobians[constraintRow.m_jacBindex]; - for (int i = 0; i < ndofB ; ++i) - rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; - - } - - constraintRow.m_friction = combinedFrictionCoeff; - - - restitution = restitutionCurve(rel_vel, combinedRestitutionCoeff); - if (restitution <= btScalar(0.)) - { - restitution = 0.f; - }; - } - - /* - ///warm starting (or zero if disabled) - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - constraintRow.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - - if (constraintRow.m_appliedImpulse) - { - if (multiBodyA) - { - btScalar impulse = constraintRow.m_appliedImpulse; - btScalar* deltaV = &m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex]; - multiBodyA->applyDeltaVee(deltaV,impulse); - applyDeltaVee(deltaV,impulse,constraintRow.m_deltaVelAindex,ndofA); - } - if (multiBodyB) - { - btScalar impulse = constraintRow.m_appliedImpulse; - btScalar* deltaV = &m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex]; - multiBodyB->applyDeltaVee(deltaV,impulse); - applyDeltaVee(deltaV,impulse,constraintRow.m_deltaVelBindex,ndofB); - } - } - } - else - */ - { - constraintRow.m_appliedImpulse = 0.f; - } - - constraintRow.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; - } - - // const btScalar ALLOWED_PENETRATION = btScalar(0.01); - -// float baumgarte_coeff = 0.3; - /// float one_over_dt = 1.f/infoGlobal.m_timeStep; - // btScalar minus_vnew = -penetration * baumgarte_coeff * one_over_dt; - // float myrhs = minus_vnew*solverConstraint.m_jacDiagABInv;//?? - - // solverConstraint.m_rhs = minus_vnew*solverConstraint.m_jacDiagABInv;//?? - //solverConstraint.m_rhsPenetration = 0.f; - - //penetration=0.f; - - if (penetration>0) - { - positionalError = 0; - velocityError = -penetration / infoGlobal.m_timeStep; - - } else - { - positionalError = -penetration * erp/infoGlobal.m_timeStep; - } - - btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv; - - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - //combine position and velocity into rhs - constraintRow.m_rhs = penetrationImpulse+velocityImpulse; - constraintRow.m_rhsPenetration = 0.f; - - } else - { - //split position and velocity into rhs and m_rhsPenetration - constraintRow.m_rhs = velocityImpulse; - constraintRow.m_rhsPenetration = penetrationImpulse; - } - - - - - constraintRow.m_cfm = 0.f; - constraintRow.m_lowerLimit = 0; - constraintRow.m_upperLimit = 1e10f; - } - -} btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { @@ -1013,19 +764,7 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol for (int i=0;iupdate(); - - for (int row=0;rowgetNumRows();row++) - { - - - btMultiBodySolverConstraint& constraintRow = m_multiBodyNonContactConstraints.expandNonInitializing(); - constraintRow.m_multiBodyA = c->getMultiBodyA(); - constraintRow.m_multiBodyB = c->getMultiBodyB(); - - btScalar penetration = c->getPosition(row);//rhs = c->computeRhs(row,infoGlobal.m_timeStep); - setupMultiBodyJointLimitConstraint(constraintRow,c->jacobianA(row),c->jacobianB(row),penetration,0,0,infoGlobal); - } + c->createConstraintRows(m_multiBodyNonContactConstraints,m_data, infoGlobal); } } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h index 8ab334782..28485cf92 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -21,7 +21,10 @@ subject to the following restrictions: class btMultiBody; -class btMultiBodyConstraint; + +#include "btMultiBodyConstraint.h" + + ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver { @@ -33,15 +36,7 @@ protected: btMultiBodyConstraintArray m_multiBodyNormalContactConstraints; btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints; - - btAlignedObjectArray m_jacobians; - btAlignedObjectArray m_deltaVelocitiesUnitImpulse; - btAlignedObjectArray m_deltaVelocities; - - - btAlignedObjectArray scratch_r; - btAlignedObjectArray scratch_v; - btAlignedObjectArray scratch_m; + btMultiBodyJacobianData m_data; //temp storage for multi body constraints for a specific island/group called by 'solveGroup' btMultiBodyConstraint** m_tmpMultiBodyConstraints; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index b6a977ea1..cf5182be4 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -4,15 +4,16 @@ #include "btMultiBodyLinkCollider.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" + btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper) :btMultiBodyConstraint(body,body,link,link,2,true), m_lowerBound(lower), m_upperBound(upper) { - // the jacobians never change, so may as well + // the data.m_jacobians never change, so may as well // initialize them here - // note: we rely on the fact that jacobians are + // note: we rely on the fact that data.m_jacobians are // always initialized to zero by the Constraint ctor // row 0: the lower bound @@ -35,9 +36,12 @@ int btMultiBodyJointLimitConstraint::getIslandIdB() const return m_bodyB->getLinkCollider(0)->getIslandTag(); } -void btMultiBodyJointLimitConstraint::update() + +void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) { - // only positions need to be updated -- jacobians and force + // only positions need to be updated -- data.m_jacobians and force // directions were set in the ctor and never change. // row 0: the lower bound @@ -45,4 +49,249 @@ void btMultiBodyJointLimitConstraint::update() // row 1: the upper bound setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA)); -} \ No newline at end of file + + for (int row=0;rowgetNumLinks() + 6; + + constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId(); + + if (constraintRow.m_deltaVelAindex <0) + { + constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size(); + multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); + } else + { + btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA); + } + + constraintRow.m_jacAindex = data.m_jacobians.size(); + data.m_jacobians.resize(data.m_jacobians.size()+ndofA); + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); + btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); + for (int i=0;icalcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v); + } + + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumLinks() + 6; + + constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId(); + if (constraintRow.m_deltaVelBindex <0) + { + constraintRow.m_deltaVelBindex = data.m_deltaVelocities.size(); + multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); + } + + constraintRow.m_jacBindex = data.m_jacobians.size(); + data.m_jacobians.resize(data.m_jacobians.size()+ndofB); + + for (int i=0;icalcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v); + } + { + + btVector3 vec; + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + btScalar* jacB = 0; + btScalar* jacA = 0; + btScalar* lambdaA =0; + btScalar* lambdaB =0; + int ndofA = 0; + if (multiBodyA) + { + ndofA = multiBodyA->getNumLinks() + 6; + jacA = &data.m_jacobians[constraintRow.m_jacAindex]; + lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + { + float j = jacA[i] ; + float l =lambdaA[i]; + denom0 += j*l; + } + } + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumLinks() + 6; + jacB = &data.m_jacobians[constraintRow.m_jacBindex]; + lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + { + float j = jacB[i] ; + float 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]; + } + } + + float 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->getNumLinks() + 6; + btScalar* jacA = &data.m_jacobians[constraintRow.m_jacAindex]; + for (int i = 0; i < ndofA ; ++i) + rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; + } + if (multiBodyB) + { + ndofB = multiBodyB->getNumLinks() + 6; + btScalar* jacB = &data.m_jacobians[constraintRow.m_jacBindex]; + for (int i = 0; i < ndofB ; ++i) + rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; + + } + + constraintRow.m_friction = combinedFrictionCoeff; + + + + } + + /* + ///warm starting (or zero if disabled) + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + constraintRow.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; + + if (constraintRow.m_appliedImpulse) + { + if (multiBodyA) + { + btScalar impulse = constraintRow.m_appliedImpulse; + btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex]; + multiBodyA->applyDeltaVee(deltaV,impulse); + applyDeltaVee(deltaV,impulse,constraintRow.m_deltaVelAindex,ndofA); + } + if (multiBodyB) + { + btScalar impulse = constraintRow.m_appliedImpulse; + btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex]; + multiBodyB->applyDeltaVee(deltaV,impulse); + applyDeltaVee(deltaV,impulse,constraintRow.m_deltaVelBindex,ndofB); + } + } + } + else + */ + { + constraintRow.m_appliedImpulse = 0.f; + } + + constraintRow.m_appliedPushImpulse = 0.f; + + { + float desiredVelocity = -0.3; + + btScalar positionalError = 0.f; + btScalar velocityError = - rel_vel;// * damping; + + + btScalar erp = infoGlobal.m_erp2; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + erp = infoGlobal.m_erp; + } + + if (penetration>0) + { + positionalError = 0; + velocityError = -penetration / infoGlobal.m_timeStep; + } else + { + positionalError = -penetration * erp/infoGlobal.m_timeStep; + } + + btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv; + + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + //combine position and velocity into rhs + constraintRow.m_rhs = penetrationImpulse+velocityImpulse; + constraintRow.m_rhsPenetration = 0.f; + + } else + { + //split position and velocity into rhs and m_rhsPenetration + constraintRow.m_rhs = velocityImpulse; + constraintRow.m_rhsPenetration = penetrationImpulse; + } + + + + + constraintRow.m_cfm = 0.f; + constraintRow.m_lowerLimit = 0; + constraintRow.m_upperLimit = 1e10f; + } + +} diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h index acdd4db47..bfcc48525 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h @@ -17,6 +17,7 @@ subject to the following restrictions: #define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H #include "btMultiBodyConstraint.h" +struct btSolverInfo; class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint { @@ -24,6 +25,11 @@ protected: btScalar m_lowerBound; btScalar m_upperBound; + void btMultiBodyJointLimitConstraint::fillConstraintRow(btMultiBodySolverConstraint& constraintRow, + btMultiBodyJacobianData& data, + btScalar* jacOrgA,btScalar* jacOrgB, + btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff, + const btContactSolverInfo& infoGlobal1); public: btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper); @@ -32,8 +38,11 @@ public: virtual int getIslandIdA() const; virtual int getIslandIdB() const; - virtual void update(); - + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + }; #endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h index 38531d68d..15d089956 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h @@ -20,6 +20,8 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" class btMultiBody; +#include "BulletDynamics/ConstraintSolver/btSolverBody.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint