diff --git a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp index 0d32aabee..a73d07bea 100644 --- a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp +++ b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp @@ -20,12 +20,13 @@ subject to the following restrictions: #define ARRAY_SIZE_X 5 #define ARRAY_SIZE_Y 5 #define ARRAY_SIZE_Z 5 +float friction = 1.; + //maximum number of objects (and allow user to shoot additional boxes) #define MAX_PROXIES (ARRAY_SIZE_X*ARRAY_SIZE_Y*ARRAY_SIZE_Z + 1024) -///scaling of the objects (0.1 = 20 centimeter boxes ) -#define SCALING 1. + #define START_POS_X -5 //#define START_POS_Y 12 #define START_POS_Y 2 @@ -39,7 +40,8 @@ subject to the following restrictions: #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyLink.h" #include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h" - +#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" +#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" #include "GlutStuff.h" @@ -51,6 +53,8 @@ subject to the following restrictions: #include "LinearMath/btAabbUtil2.h" static GLDebugDrawer gDebugDraw; +//btVector3 scaling(0.1,0.1,0.1); +float scaling = 0.4f; void FeatherstoneMultiBodyDemo::clientMoveAndDisplay() @@ -107,8 +111,8 @@ void FeatherstoneMultiBodyDemo::initPhysics() setTexturing(true); setShadows(true); - setCameraDistance(btScalar(SCALING*50.)); - + setCameraDistance(btScalar(100.*scaling)); + this->m_azi = 130; ///collision configuration contains default setup for memory, collision setup m_collisionConfiguration = new btDefaultCollisionConfiguration(); @@ -129,7 +133,7 @@ void FeatherstoneMultiBodyDemo::initPhysics() m_dynamicsWorld->setGravity(btVector3(0,-10,0)); ///create a few basic rigid bodies - btBoxShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); //groundShape->initializePolyhedralFeatures(); // btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50); @@ -137,34 +141,15 @@ void FeatherstoneMultiBodyDemo::initPhysics() btTransform groundTransform; groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0,-50,0)); + groundTransform.setOrigin(btVector3(0,-50,00)); //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: - { - btScalar mass(0.); - - //rigidbody is dynamic if and only if mass is non zero, otherwise static - bool isDynamic = (mass != 0.f); - - btVector3 localInertia(0,0,0); - if (isDynamic) - groundShape->calculateLocalInertia(mass,localInertia); - - //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects - btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); - btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia); - btRigidBody* body = new btRigidBody(rbInfo); - - //add the body to the dynamics world - m_dynamicsWorld->addRigidBody(body);//,1,1+2); - } - if (1) { //create a few dynamic rigidbodies // Re-using the same collision is better for memory usage and performance - btBoxShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1)); + btBoxShape* colShape = new btBoxShape(btVector3(1,1,1)); //btCollisionShape* colShape = new btSphereShape(btScalar(1.)); m_collisionShapes.push_back(colShape); @@ -191,7 +176,7 @@ void FeatherstoneMultiBodyDemo::initPhysics() { for(int j = 0;jworldPosToLocal(linkA, pivotInAworld); + btVector3 pivotInBlocal = mbB->worldPosToLocal(linkB, pivotInAworld); + btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(mbA,linkA,mbB,linkB,pivotInAlocal,pivotInBlocal); + world->addMultiBodyConstraint(p2p); + } +#endif bool testRemoveLinks = false; if (testRemoveLinks) { - while (mb->getNumLinks()) + while (mbA->getNumLinks()) { - btCollisionObject* col = mb->getLink(mb->getNumLinks()-1).m_collider; + btCollisionObject* col = mbA->getLink(mbA->getNumLinks()-1).m_collider; m_dynamicsWorld->removeCollisionObject(col); delete col; - mb->setNumLinks(mb->getNumLinks()-1); + mbA->setNumLinks(mbA->getNumLinks()-1); } } + + if (1)//useGroundShape + { + btScalar mass(0.); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0,0,0); + if (isDynamic) + groundShape->calculateLocalInertia(mass,localInertia); + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + + //add the body to the dynamics world + m_dynamicsWorld->addRigidBody(body,1,1+2);//,1,1+2); + } + + } -btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic, bool canSleep) +btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic, bool canSleep, bool createConstraints) { int n_links = numLinks; - float mass = 13.5; - btVector3 inertia(91,344,253); + float mass = 13.5*scaling; + btVector3 inertia = btVector3 (91,344,253)*scaling*scaling; btMultiBody * bod = new btMultiBody(n_links, mass, inertia, isFixedBase, canSleep); @@ -256,9 +285,9 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult - btVector3 pos(0,0,9.0500002); + btVector3 pos = btVector3 (0,0,9.0500002)*scaling; - btVector3 joint_axis_position(0,0,4.5250001); + btVector3 joint_axis_position = btVector3 (0,0,4.5250001)*scaling; for (int i=0;isetupPrismatic(child_link_num, mass, inertia, this_link_num, parent_to_child, joint_axis_child_prismatic, quatRotate(parent_to_child , pos)); @@ -280,13 +309,40 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult } bod->setJointPos(child_link_num, initial_joint_angle); this_link_num = i; - } + + if (0)//!useGroundShape && i==4) + { + btVector3 pivotInAworld(0,20,46); + btVector3 pivotInAlocal = bod->worldPosToLocal(i, pivotInAworld); + btVector3 pivotInBworld = pivotInAworld; + btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(bod,i,&btTypedConstraint::getFixedBody(),pivotInAlocal,pivotInBworld); + world->addMultiBodyConstraint(p2p); + } + //add some constraint limit + if (usePrismatic) + { + // btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,n_links-1,2,3); + + if (0) + { + btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod,i,1,500000); + world->addMultiBodyConstraint(con); + } + if (createConstraints) + { + btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,i,-1,1); + world->addMultiBodyConstraint(con); + } + + } else + { + if (createConstraints) + { + btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,i,-1,1); + world->addMultiBodyConstraint(con); + } - //add some constraint limit - if (usePrismatic) - { - btMultiBodyConstraint* limit = new btMultiBodyJointLimitConstraint(bod,n_links-1,2,3); - world->addMultiBodyConstraint(limit); + } } } @@ -310,7 +366,7 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult if (1) { - btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])); + btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])*scaling); btRigidBody* body = new btRigidBody(mass,0,box,inertia); btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(bod,-1); @@ -324,8 +380,8 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult body->setWorldTransform(tr); col->setWorldTransform(tr); - world->addCollisionObject(col, btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter); - col->setFriction(1); + world->addCollisionObject(col, 2,1); + col->setFriction(friction); bod->setBaseCollider(col); } @@ -348,7 +404,7 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult 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])); + btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])*scaling); btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(bod,i); col->setCollisionShape(box); @@ -357,8 +413,8 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult 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); + col->setFriction(friction); + world->addCollisionObject(col,2,1); bod->getLink(i).m_collider=col; //app->drawBox(halfExtents, pos,quat); @@ -370,6 +426,80 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult return bod; } +extern btScalar gOldPickingDist; +void FeatherstoneMultiBodyDemo::mouseMotionFunc(int x,int y) +{ + if (m_pickingMultiBodyPoint2Point) + { + //keep it at the same picking distance + + btVector3 newRayTo = getRayTo(x,y); + btVector3 rayFrom; + btVector3 oldPivotInB = m_pickingMultiBodyPoint2Point->getPivotInB(); + btVector3 newPivotB; + if (m_ortho) + { + newPivotB = oldPivotInB; + newPivotB.setX(newRayTo.getX()); + newPivotB.setY(newRayTo.getY()); + } else + { + rayFrom = m_cameraPosition; + btVector3 dir = newRayTo-rayFrom; + dir.normalize(); + dir *= gOldPickingDist; + + newPivotB = rayFrom + dir; + } + m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB); + } + DemoApplication::mouseMotionFunc(x,y); +} + +void FeatherstoneMultiBodyDemo::removePickingConstraint() +{ + if (m_pickingMultiBodyPoint2Point) + { + m_pickingMultiBodyPoint2Point->getMultiBodyA()->setCanSleep(true); + + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_dynamicsWorld; + world->removeMultiBodyConstraint(m_pickingMultiBodyPoint2Point); + delete m_pickingMultiBodyPoint2Point; + m_pickingMultiBodyPoint2Point = 0; + } + + DemoApplication::removePickingConstraint(); + +} + +void FeatherstoneMultiBodyDemo::pickObject(const btVector3& pickPos, const class btCollisionObject* hitObj) +{ + btVector3 pivotInA(0,0,0); + btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(hitObj); + if (multiCol && multiCol->m_multiBody) + { + multiCol->m_multiBody->setCanSleep(false); + + btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos); + + btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody,multiCol->m_link,0,pivotInA,pickPos); + //if you add too much energy to the system, causing high angular velocities, simulation 'explodes' + //see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949 + //so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply + //it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?) + + p2p->setMaxAppliedImpulse(200*scaling); + + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_dynamicsWorld; + world->addMultiBodyConstraint(p2p); + m_pickingMultiBodyPoint2Point =p2p; + } else + { + DemoApplication::pickObject(pickPos,hitObj); + } +} + + void FeatherstoneMultiBodyDemo::clientResetScene() { diff --git a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.h b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.h index 7f6c25a08..0f7a683cf 100644 --- a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.h +++ b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.h @@ -53,12 +53,17 @@ class FeatherstoneMultiBodyDemo : public PlatformDemoApplication btDefaultCollisionConfiguration* m_collisionConfiguration; + virtual void mouseMotionFunc(int x,int y); + virtual void removePickingConstraint(); + virtual void pickObject(const btVector3& pickPos, const class btCollisionObject* hitObj); + class btMultiBodyPoint2Point* m_pickingMultiBodyPoint2Point; - btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic, bool canSleep); + btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic, bool canSleep, bool createConstraints); public: FeatherstoneMultiBodyDemo() + :m_pickingMultiBodyPoint2Point(0) { } virtual ~FeatherstoneMultiBodyDemo() diff --git a/Demos/OpenGL/DemoApplication.cpp b/Demos/OpenGL/DemoApplication.cpp index f2782fd41..8b10a635b 100644 --- a/Demos/OpenGL/DemoApplication.cpp +++ b/Demos/OpenGL/DemoApplication.cpp @@ -773,82 +773,14 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y) if (rayCallback.hasHit()) { + btVector3 pickPos = rayCallback.m_hitPointWorld; + + pickObject(pickPos, rayCallback.m_collisionObject); + + gOldPickingPos = rayTo; + gHitPos = pickPos; - btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject); - if (body) - { - //other exclusions? - if (!(body->isStaticObject() || body->isKinematicObject())) - { - pickedBody = body; - pickedBody->setActivationState(DISABLE_DEACTIVATION); - - - btVector3 pickPos = rayCallback.m_hitPointWorld; - //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ()); - - - btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; - - - - - - - if ((m_modifierKeys& BT_ACTIVE_SHIFT)==0) - { - btTransform tr; - tr.setIdentity(); - tr.setOrigin(localPivot); - btGeneric6DofConstraint* dof6 = new btGeneric6DofConstraint(*body, tr,false); - dof6->setLinearLowerLimit(btVector3(0,0,0)); - dof6->setLinearUpperLimit(btVector3(0,0,0)); - dof6->setAngularLowerLimit(btVector3(0,0,0)); - dof6->setAngularUpperLimit(btVector3(0,0,0)); - - m_dynamicsWorld->addConstraint(dof6,true); - m_pickConstraint = dof6; - - dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,0); - dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,1); - dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,2); - dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,3); - dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,4); - dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,5); - - dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,0); - dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,1); - dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,2); - dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,3); - dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,4); - dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,5); - } else - { - btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body,localPivot); - m_dynamicsWorld->addConstraint(p2p,true); - m_pickConstraint = p2p; - p2p->m_setting.m_impulseClamp = mousePickClamping; - //very weak constraint for picking - p2p->m_setting.m_tau = 0.001f; -/* - p2p->setParam(BT_CONSTRAINT_CFM,0.8,0); - p2p->setParam(BT_CONSTRAINT_CFM,0.8,1); - p2p->setParam(BT_CONSTRAINT_CFM,0.8,2); - p2p->setParam(BT_CONSTRAINT_ERP,0.1,0); - p2p->setParam(BT_CONSTRAINT_ERP,0.1,1); - p2p->setParam(BT_CONSTRAINT_ERP,0.1,2); - */ - - - } - - //save mouse position for dragging - gOldPickingPos = rayTo; - gHitPos = pickPos; - - gOldPickingDist = (pickPos-rayFrom).length(); - } - } + gOldPickingDist = (pickPos-rayFrom).length(); } } @@ -867,6 +799,78 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y) } +void DemoApplication::pickObject(const btVector3& pickPos, const btCollisionObject* hitObj) +{ + + btRigidBody* body = (btRigidBody*)btRigidBody::upcast(hitObj); + if (body) + { + //other exclusions? + if (!(body->isStaticObject() || body->isKinematicObject())) + { + pickedBody = body; + pickedBody->setActivationState(DISABLE_DEACTIVATION); + + + //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ()); + + + btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; + + if ((m_modifierKeys& BT_ACTIVE_SHIFT)==0) + { + btTransform tr; + tr.setIdentity(); + tr.setOrigin(localPivot); + btGeneric6DofConstraint* dof6 = new btGeneric6DofConstraint(*body, tr,false); + dof6->setLinearLowerLimit(btVector3(0,0,0)); + dof6->setLinearUpperLimit(btVector3(0,0,0)); + dof6->setAngularLowerLimit(btVector3(0,0,0)); + dof6->setAngularUpperLimit(btVector3(0,0,0)); + + m_dynamicsWorld->addConstraint(dof6,true); + m_pickConstraint = dof6; + + dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,0); + dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,1); + dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,2); + dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,3); + dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,4); + dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,5); + + dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,0); + dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,1); + dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,2); + dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,3); + dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,4); + dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,5); + } else + { + btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body,localPivot); + m_dynamicsWorld->addConstraint(p2p,true); + m_pickConstraint = p2p; + p2p->m_setting.m_impulseClamp = mousePickClamping; + //very weak constraint for picking + p2p->m_setting.m_tau = 0.001f; + /* + p2p->setParam(BT_CONSTRAINT_CFM,0.8,0); + p2p->setParam(BT_CONSTRAINT_CFM,0.8,1); + p2p->setParam(BT_CONSTRAINT_CFM,0.8,2); + p2p->setParam(BT_CONSTRAINT_ERP,0.1,0); + p2p->setParam(BT_CONSTRAINT_ERP,0.1,1); + p2p->setParam(BT_CONSTRAINT_ERP,0.1,2); + */ + + + } + + //save mouse position for dragging + + } + } + +} + void DemoApplication::removePickingConstraint() { if (m_pickConstraint && m_dynamicsWorld) diff --git a/Demos/OpenGL/DemoApplication.h b/Demos/OpenGL/DemoApplication.h index b28abdd51..a39f30f6e 100644 --- a/Demos/OpenGL/DemoApplication.h +++ b/Demos/OpenGL/DemoApplication.h @@ -57,6 +57,9 @@ protected: virtual void removePickingConstraint(); + virtual void pickObject(const btVector3& pickPos, const class btCollisionObject* hitObj); + + btCollisionShape* m_shootBoxShape; float m_cameraDistance; diff --git a/src/BulletDynamics/CMakeLists.txt b/src/BulletDynamics/CMakeLists.txt index 5a626f851..9bfce7f49 100644 --- a/src/BulletDynamics/CMakeLists.txt +++ b/src/BulletDynamics/CMakeLists.txt @@ -28,6 +28,9 @@ SET(BulletDynamics_SRCS Featherstone/btMultiBodyConstraintSolver.cpp Featherstone/btMultiBodyDynamicsWorld.cpp Featherstone/btMultiBodyJointLimitConstraint.cpp + Featherstone/btMultiBodyConstraint.cpp + Featherstone/btMultiBodyPoint2Point.cpp + Featherstone/btMultiBodyJointMotor.cpp ) SET(Root_HDRS @@ -77,6 +80,9 @@ SET(Featherstone_HDRS Featherstone/btMultiBodySolverConstraint.h Featherstone/btMultiBodyConstraint.h Featherstone/btMultiBodyJointLimitConstraint.h + Featherstone/btMultiBodyConstraint.h + Featherstone/btMultiBodyPoint2Point.h + Featherstone/btMultiBodyJointMotor.h ) SET(Character_HDRS Character/btCharacterControllerInterface.h diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index d336f7c1f..0542b88e8 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -28,7 +28,7 @@ // #define INCLUDE_GYRO_TERM namespace { - const btScalar SLEEP_EPSILON = btScalar(0.01); // this is a squared velocity (m^2 s^-2) + const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2) const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds } @@ -85,17 +85,21 @@ btMultiBody::btMultiBody(int n_links, base_mass(mass), base_inertia(inertia), - fixed_base(fixed_base_), - awake(true), - can_sleep(can_sleep_), - sleep_timer(0), - m_baseCollider(0) + fixed_base(fixed_base_), + awake(true), + can_sleep(can_sleep_), + sleep_timer(0), + m_baseCollider(0), + m_linearDamping(0.04f), + m_angularDamping(0.04f), + m_useGyroTerm(true) + { links.resize(n_links); vector_buf.resize(2*n_links); matrix_buf.resize(n_links + 1); - real_buf.resize(6 + 2*n_links); + m_real_buf.resize(6 + 2*n_links); base_pos.setValue(0, 0, 0); base_force.setValue(0, 0, 0); base_torque.setValue(0, 0, 0); @@ -172,7 +176,7 @@ btScalar btMultiBody::getJointPos(int i) const btScalar btMultiBody::getJointVel(int i) const { - return real_buf[6 + i]; + return m_real_buf[6 + i]; } void btMultiBody::setJointPos(int i, btScalar q) @@ -183,7 +187,7 @@ void btMultiBody::setJointPos(int i, btScalar q) void btMultiBody::setJointVel(int i, btScalar qdot) { - real_buf[6 + i] = qdot; + m_real_buf[6 + i] = qdot; } const btVector3 & btMultiBody::getRVector(int i) const @@ -322,7 +326,7 @@ void btMultiBody::clearVelocities() { for (int i = 0; i < 6 + getNumLinks(); ++i) { - real_buf[i] = 0.f; + m_real_buf[i] = 0.f; } } void btMultiBody::addLinkForce(int i, const btVector3 &f) @@ -395,9 +399,11 @@ void btMultiBody::stepVelocities(btScalar dt, int num_links = getNumLinks(); - const btScalar DAMPING_K1 = btScalar(0.0); - //const btScalar DAMPING_K2 = btScalar(0); - const btScalar DAMPING_K2 = btScalar(0.0); + const btScalar DAMPING_K1_LINEAR = m_linearDamping; + const btScalar DAMPING_K2_LINEAR = m_linearDamping; + + const btScalar DAMPING_K1_ANGULAR = m_angularDamping; + const btScalar DAMPING_K2_ANGULAR= m_angularDamping; btVector3 base_vel = getBaseVel(); btVector3 base_omega = getBaseOmega(); @@ -414,16 +420,16 @@ void btMultiBody::stepVelocities(btScalar dt, btVector3 * v_ptr = &scratch_v[0]; // vhat_i (top = angular, bottom = linear part) - btVector3 * vel_top = v_ptr; v_ptr += num_links + 1; - btVector3 * vel_bottom = v_ptr; v_ptr += num_links + 1; + btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1; + btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1; // zhat_i^A - btVector3 * zero_acc_top = v_ptr; v_ptr += num_links + 1; - btVector3 * zero_acc_bottom = v_ptr; v_ptr += num_links + 1; + btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1; + btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1; // chat_i (note NOT defined for the base) - btVector3 * coriolis_top = v_ptr; v_ptr += num_links; - btVector3 * coriolis_bottom = v_ptr; v_ptr += num_links; + btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links; + btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links; // top left, top right and bottom left blocks of Ihat_i^A. // bottom right block = transpose of top left block and is not stored. @@ -445,7 +451,7 @@ void btMultiBody::stepVelocities(btScalar dt, // Y_i, D_i btScalar * Y = r_ptr; r_ptr += num_links; - btScalar * D = num_links > 0 ? &real_buf[6 + num_links] : 0; + btScalar * D = num_links > 0 ? &m_real_buf[6 + num_links] : 0; // ptr to the joint accel part of the output btScalar * joint_accel = output + 6; @@ -458,21 +464,27 @@ void btMultiBody::stepVelocities(btScalar dt, rot_from_parent[0] = btMatrix3x3(base_quat); - vel_top[0] = rot_from_parent[0] * base_omega; - vel_bottom[0] = rot_from_parent[0] * base_vel; + vel_top_angular[0] = rot_from_parent[0] * base_omega; + vel_bottom_linear[0] = rot_from_parent[0] * base_vel; if (fixed_base) { - zero_acc_top[0] = zero_acc_bottom[0] = btVector3(0,0,0); + zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0); } else { - zero_acc_top[0] = - (rot_from_parent[0] * (base_force - - base_mass*(DAMPING_K1+DAMPING_K2*base_vel.norm())*base_vel)); - zero_acc_bottom[0] = -#ifdef INCLUDE_GYRO_TERM - vel_top[0].cross( base_inertia.cwise() * vel_top[0] ) -#endif + zero_acc_top_angular[0] = - (rot_from_parent[0] * (base_force + - base_mass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel)); + + zero_acc_bottom_linear[0] = - (rot_from_parent[0] * base_torque); - zero_acc_bottom[0] += base_inertia * vel_top[0] * (DAMPING_K1 + DAMPING_K2*vel_top[0].norm()); + + if (m_useGyroTerm) + zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( base_inertia * vel_top_angular[0] ); + + zero_acc_bottom_linear[0] += base_inertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm()); + } + + + inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero(); @@ -494,35 +506,37 @@ void btMultiBody::stepVelocities(btScalar dt, // vhat_i = i_xhat_p(i) * vhat_p(i) SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector, - vel_top[parent+1], vel_bottom[parent+1], - vel_top[i+1], vel_bottom[i+1]); + vel_top_angular[parent+1], vel_bottom_linear[parent+1], + vel_top_angular[i+1], vel_bottom_linear[i+1]); // we can now calculate chat_i // remember vhat_i is really vhat_p(i) (but in current frame) at this point - coriolis_bottom[i] = vel_top[i+1].cross(vel_top[i+1].cross(links[i].cached_r_vector)) - + 2 * vel_top[i+1].cross(links[i].axis_bottom) * getJointVel(i); + coriolis_bottom_linear[i] = vel_top_angular[i+1].cross(vel_top_angular[i+1].cross(links[i].cached_r_vector)) + + 2 * vel_top_angular[i+1].cross(links[i].axis_bottom) * getJointVel(i); if (links[i].is_revolute) { - coriolis_top[i] = vel_top[i+1].cross(links[i].axis_top) * getJointVel(i); - coriolis_bottom[i] += (getJointVel(i) * getJointVel(i)) * links[i].axis_top.cross(links[i].axis_bottom); + coriolis_top_angular[i] = vel_top_angular[i+1].cross(links[i].axis_top) * getJointVel(i); + coriolis_bottom_linear[i] += (getJointVel(i) * getJointVel(i)) * links[i].axis_top.cross(links[i].axis_bottom); } else { - coriolis_top[i] = btVector3(0,0,0); + coriolis_top_angular[i] = btVector3(0,0,0); } // now set vhat_i to its true value by doing // vhat_i += qidot * shat_i - vel_top[i+1] += getJointVel(i) * links[i].axis_top; - vel_bottom[i+1] += getJointVel(i) * links[i].axis_bottom; + vel_top_angular[i+1] += getJointVel(i) * links[i].axis_top; + vel_bottom_linear[i+1] += getJointVel(i) * links[i].axis_bottom; // calculate zhat_i^A - zero_acc_top[i+1] = - (rot_from_world[i+1] * (links[i].applied_force)); - zero_acc_top[i+1] += links[i].mass * (DAMPING_K1 + DAMPING_K2*vel_bottom[i+1].norm()) * vel_bottom[i+1]; + zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (links[i].applied_force)); + zero_acc_top_angular[i+1] += links[i].mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1]; - zero_acc_bottom[i+1] = -#ifdef INCLUDE_GYRO_TERM - vel_top[i+1].cross( links[i].inertia.cwise() * vel_top[i+1] ) -#endif + zero_acc_bottom_linear[i+1] = - (rot_from_world[i+1] * links[i].applied_torque); - zero_acc_bottom[i+1] += links[i].inertia * vel_top[i+1] * (DAMPING_K1 + DAMPING_K2*vel_top[i+1].norm()); + if (m_useGyroTerm) + { + zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( links[i].inertia * vel_top_angular[i+1] ); + } + + zero_acc_bottom_linear[i+1] += links[i].inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm()); // calculate Ihat_i^A inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero(); @@ -541,11 +555,11 @@ void btMultiBody::stepVelocities(btScalar dt, h_top[i] = inertia_top_left[i+1] * links[i].axis_top + inertia_top_right[i+1] * links[i].axis_bottom; h_bottom[i] = inertia_bottom_left[i+1] * links[i].axis_top + inertia_top_left[i+1].transpose() * links[i].axis_bottom; - - D[i] = SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, h_top[i], h_bottom[i]); - Y[i] = links[i].joint_torque - - SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top[i+1], zero_acc_bottom[i+1]) - - SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top[i], coriolis_bottom[i]); + btScalar val = SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, h_top[i], h_bottom[i]); + D[i] = val; + Y[i] = links[i].joint_torque + - SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]) + - SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]); const int parent = links[i].parent; @@ -576,18 +590,18 @@ void btMultiBody::stepVelocities(btScalar dt, // Zp += pXi * (Zi + Ii*ci + hi*Yi/Di) btVector3 in_top, in_bottom, out_top, out_bottom; const btScalar Y_over_D = Y[i] * one_over_di; - in_top = zero_acc_top[i+1] - + inertia_top_left[i+1] * coriolis_top[i] - + inertia_top_right[i+1] * coriolis_bottom[i] + in_top = zero_acc_top_angular[i+1] + + inertia_top_left[i+1] * coriolis_top_angular[i] + + inertia_top_right[i+1] * coriolis_bottom_linear[i] + Y_over_D * h_top[i]; - in_bottom = zero_acc_bottom[i+1] - + inertia_bottom_left[i+1] * coriolis_top[i] - + inertia_top_left[i+1].transpose() * coriolis_bottom[i] + in_bottom = zero_acc_bottom_linear[i+1] + + inertia_bottom_left[i+1] * coriolis_top_angular[i] + + inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i] + Y_over_D * h_bottom[i]; InverseSpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector, in_top, in_bottom, out_top, out_bottom); - zero_acc_top[parent+1] += out_top; - zero_acc_bottom[parent+1] += out_bottom; + zero_acc_top_angular[parent+1] += out_top; + zero_acc_bottom_linear[parent+1] += out_bottom; } @@ -615,8 +629,8 @@ void btMultiBody::stepVelocities(btScalar dt, cached_inertia_lower_right= inertia_top_left[0].transpose(); } - btVector3 rhs_top (zero_acc_top[0][0], zero_acc_top[0][1], zero_acc_top[0][2]); - btVector3 rhs_bot (zero_acc_bottom[0][0], zero_acc_bottom[0][1], zero_acc_bottom[0][2]); + btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]); + btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]); float result[6]; solveImatrix(rhs_top, rhs_bot, result); @@ -635,8 +649,8 @@ void btMultiBody::stepVelocities(btScalar dt, accel_top[parent+1], accel_bottom[parent+1], accel_top[i+1], accel_bottom[i+1]); joint_accel[i] = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i]; - accel_top[i+1] += coriolis_top[i] + joint_accel[i] * links[i].axis_top; - accel_bottom[i+1] += coriolis_bottom[i] + joint_accel[i] * links[i].axis_bottom; + accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] * links[i].axis_top; + accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] * links[i].axis_bottom; } // transform base accelerations back to the world frame. @@ -720,8 +734,8 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output btVector3 * v_ptr = &scratch_v[0]; // zhat_i^A (scratch space) - btVector3 * zero_acc_top = v_ptr; v_ptr += num_links + 1; - btVector3 * zero_acc_bottom = v_ptr; v_ptr += num_links + 1; + btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1; + btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1; // rot_from_parent (cached from calcAccelerations) const btMatrix3x3 * rot_from_parent = &matrix_buf[0]; @@ -734,7 +748,7 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output // Y_i (scratch), D_i (cached) btScalar * Y = r_ptr; r_ptr += num_links; - const btScalar * D = num_links > 0 ? &real_buf[6 + num_links] : 0; + const btScalar * D = num_links > 0 ? &m_real_buf[6 + num_links] : 0; btAssert(num_links == 0 || r_ptr - &scratch_r[0] == scratch_r.size()); btAssert(v_ptr - &scratch_v[0] == scratch_v.size()); @@ -751,22 +765,22 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output // -- set to force/torque on the base, zero otherwise if (fixed_base) { - zero_acc_top[0] = zero_acc_bottom[0] = btVector3(0,0,0); + zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0); } else { - zero_acc_top[0] = - (rot_from_parent[0] * input_force); - zero_acc_bottom[0] = - (rot_from_parent[0] * input_torque); + zero_acc_top_angular[0] = - (rot_from_parent[0] * input_force); + zero_acc_bottom_linear[0] = - (rot_from_parent[0] * input_torque); } for (int i = 0; i < num_links; ++i) { - zero_acc_top[i+1] = zero_acc_bottom[i+1] = btVector3(0,0,0); + zero_acc_top_angular[i+1] = zero_acc_bottom_linear[i+1] = btVector3(0,0,0); } // 'Downward' loop. for (int i = num_links - 1; i >= 0; --i) { - Y[i] = - SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top[i+1], zero_acc_bottom[i+1]); + Y[i] = - SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]); Y[i] += force[6 + i]; // add joint torque const int parent = links[i].parent; @@ -774,12 +788,12 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output // Zp += pXi * (Zi + hi*Yi/Di) btVector3 in_top, in_bottom, out_top, out_bottom; const btScalar Y_over_D = Y[i] / D[i]; - in_top = zero_acc_top[i+1] + Y_over_D * h_top[i]; - in_bottom = zero_acc_bottom[i+1] + Y_over_D * h_bottom[i]; + in_top = zero_acc_top_angular[i+1] + Y_over_D * h_top[i]; + in_bottom = zero_acc_bottom_linear[i+1] + Y_over_D * h_bottom[i]; InverseSpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector, in_top, in_bottom, out_top, out_bottom); - zero_acc_top[parent+1] += out_top; - zero_acc_bottom[parent+1] += out_bottom; + zero_acc_top_angular[parent+1] += out_top; + zero_acc_bottom_linear[parent+1] += out_bottom; } // ptr to the joint accel part of the output @@ -791,8 +805,8 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output accel_top[0] = accel_bottom[0] = btVector3(0,0,0); } else { - btVector3 rhs_top (zero_acc_top[0][0], zero_acc_top[0][1], zero_acc_top[0][2]); - btVector3 rhs_bot (zero_acc_bottom[0][0], zero_acc_bottom[0][1], zero_acc_bottom[0][2]); + btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]); + btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]); float result[6]; solveImatrix(rhs_top,rhs_bot, result); @@ -960,13 +974,18 @@ void btMultiBody::goToSleep() void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep) { int num_links = getNumLinks(); - - if (!can_sleep) return; + extern bool gDisableDeactivation; + if (!can_sleep || gDisableDeactivation) + { + awake = true; + sleep_timer = 0; + return; + } // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities) btScalar motion = 0; for (int i = 0; i < 6 + num_links; ++i) { - motion += real_buf[i] * real_buf[i]; + motion += m_real_buf[i] * m_real_buf[i]; } if (motion < SLEEP_EPSILON) { diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 9cc5df73e..3b6e7a87e 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -126,14 +126,15 @@ public: // const btVector3 & getBasePos() const { return base_pos; } // in world frame - btVector3 getBaseVel() const { - return btVector3(real_buf[3],real_buf[4],real_buf[5]); + const btVector3 getBaseVel() const + { + return btVector3(m_real_buf[3],m_real_buf[4],m_real_buf[5]); } // in world frame const btQuaternion & getWorldToBaseRot() const { return base_quat; } // rotates world vectors into base frame - btVector3 getBaseOmega() const { return btVector3(real_buf[0],real_buf[1],real_buf[2]); } // in world frame + btVector3 getBaseOmega() const { return btVector3(m_real_buf[0],m_real_buf[1],m_real_buf[2]); } // in world frame void setBasePos(const btVector3 &pos) { @@ -141,13 +142,19 @@ public: } void setBaseVel(const btVector3 &vel) { - real_buf[3]=vel[0]; real_buf[4]=vel[1]; real_buf[5]=vel[2]; + + m_real_buf[3]=vel[0]; m_real_buf[4]=vel[1]; m_real_buf[5]=vel[2]; } void setWorldToBaseRot(const btQuaternion &rot) { base_quat = rot; } - void setBaseOmega(const btVector3 &omega) { real_buf[0]=omega[0]; real_buf[1]=omega[1]; real_buf[2]=omega[2]; } + void setBaseOmega(const btVector3 &omega) + { + m_real_buf[0]=omega[0]; + m_real_buf[1]=omega[1]; + m_real_buf[2]=omega[2]; + } // @@ -166,13 +173,13 @@ public: // const btScalar * getVelocityVector() const { - return &real_buf[0]; + return &m_real_buf[0]; } - btScalar * getVelocityVector() +/* btScalar * getVelocityVector() { return &real_buf[0]; } - + */ // // get the frames of reference (positions and orientations) of the child links @@ -258,13 +265,38 @@ public: // apply a delta-vee directly. used in sequential impulses code. void applyDeltaVee(const btScalar * delta_vee) { + for (int i = 0; i < 6 + getNumLinks(); ++i) - real_buf[i] += delta_vee[i]; + { + m_real_buf[i] += delta_vee[i]; + } + } void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier) { - for (int i = 0; i < 6 + getNumLinks(); ++i) - real_buf[i] += delta_vee[i] * multiplier; + btScalar sum = 0; + for (int i = 0; i < 6 + getNumLinks(); ++i) + { + sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier; + } + btScalar l = btSqrt(sum); + static btScalar maxl = -1e30f; + if (l>maxl) + { + maxl=l; + printf("maxl=%f\n",maxl); + } + if (l>100) + { + printf("exceeds 100: l=%f\n",maxl); + multiplier *= 100.f/l; + } + + for (int i = 0; i < 6 + getNumLinks(); ++i) + { + sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier; + m_real_buf[i] += delta_vee[i] * multiplier; + } } // timestep the positions (given current velocities). @@ -290,6 +322,10 @@ public: // // sleeping // + void setCanSleep(bool canSleep) + { + can_sleep = canSleep; + } bool isAwake() const { return awake; } void wakeUp(); @@ -315,6 +351,29 @@ public: { links.resize(numLinks); } + + btScalar getLinearDamping() const + { + return m_linearDamping; + } + void setLinearDamping( btScalar damp) + { + m_linearDamping = damp; + } + btScalar getAngularDamping() const + { + return m_angularDamping; + } + + bool getUseGyroTerm() const + { + return m_useGyroTerm; + } + void setUseGyroTerm(bool useGyro) + { + m_useGyroTerm = useGyro; + } + private: btMultiBody(const btMultiBody &); // not implemented void operator=(const btMultiBody &); // not implemented @@ -356,7 +415,7 @@ private: // 0 num_links+1 rot_from_parent // - btAlignedObjectArray real_buf; + btAlignedObjectArray m_real_buf; btAlignedObjectArray vector_buf; btAlignedObjectArray matrix_buf; @@ -375,6 +434,10 @@ private: btScalar sleep_timer; int m_companionId; + btScalar m_linearDamping; + btScalar m_angularDamping; + bool m_useGyroTerm; + }; #endif diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp new file mode 100644 index 000000000..e13c2f33f --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -0,0 +1,526 @@ +#include "btMultiBodyConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" + +btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral) + :m_bodyA(bodyA), + m_bodyB(bodyB), + m_linkA(linkA), + m_linkB(linkB), + m_num_rows(numRows), + m_isUnilateral(isUnilateral) +{ + m_jac_size_A = (6 + bodyA->getNumLinks()); + m_jac_size_both = (m_jac_size_A + (bodyB ? 6 + bodyB->getNumLinks() : 0)); + m_pos_offset = ((1 + m_jac_size_both)*m_num_rows); + m_data.resize((2 + m_jac_size_both) * m_num_rows); +} + +btMultiBodyConstraint::~btMultiBodyConstraint() +{ +} + + + +btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow, + btMultiBodyJacobianData& data, + btScalar* jacOrgA,btScalar* jacOrgB, + const btContactSolverInfo& infoGlobal, + btScalar desiredVelocity, + btScalar lowerLimit, + btScalar upperLimit) +{ + + + + constraintRow.m_multiBodyA = m_bodyA; + constraintRow.m_multiBodyB = m_bodyB; + + btMultiBody* multiBodyA = constraintRow.m_multiBodyA; + btMultiBody* multiBodyB = constraintRow.m_multiBodyB; + + if (multiBodyA) + { + + const int ndofA = multiBodyA->getNumLinks() + 6; + + constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId(); + + if (constraintRow.m_deltaVelAindex <0) + { + constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size(); + multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); + } else + { + btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA); + } + + constraintRow.m_jacAindex = data.m_jacobians.size(); + data.m_jacobians.resize(data.m_jacobians.size()+ndofA); + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); + btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); + for (int i=0;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 = 0.f; + + constraintRow.m_appliedImpulse = 0.f; + constraintRow.m_appliedPushImpulse = 0.f; + + btScalar velocityError = desiredVelocity - rel_vel;// * damping; + + btScalar erp = infoGlobal.m_erp2; + + btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv; + + if (!infoGlobal.m_splitImpulse) + { + //combine position and velocity into rhs + constraintRow.m_rhs = velocityImpulse; + constraintRow.m_rhsPenetration = 0.f; + + } else + { + //split position and velocity into rhs and m_rhsPenetration + constraintRow.m_rhs = velocityImpulse; + constraintRow.m_rhsPenetration = 0.f; + } + + + constraintRow.m_cfm = 0.f; + constraintRow.m_lowerLimit = lowerLimit; + constraintRow.m_upperLimit = upperLimit; + + } + return rel_vel; +} + + +void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) +{ + for (int i = 0; i < ndof; ++i) + data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; +} + + +void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint, + btMultiBodyJacobianData& data, + const btVector3& contactNormalOnB, + const btVector3& posAworld, const btVector3& posBworld, + btScalar position, + const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) +{ + + + btVector3 rel_pos1 = posAworld; + btVector3 rel_pos2 = posBworld; + + solverConstraint.m_multiBodyA = m_bodyA; + solverConstraint.m_multiBodyB = m_bodyB; + solverConstraint.m_linkA = m_linkA; + solverConstraint.m_linkB = m_linkB; + + + btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; + btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; + + const btVector3& pos1 = posAworld; + const btVector3& pos2 = posBworld; + + btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA); + btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB); + + btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; + btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; + + if (bodyA) + rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + if (bodyB) + rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); + + relaxation = 1.f; + + if (multiBodyA) + { + const int ndofA = multiBodyA->getNumLinks() + 6; + + solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); + + if (solverConstraint.m_deltaVelAindex <0) + { + solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size(); + multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); + } else + { + btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); + } + + solverConstraint.m_jacAindex = data.m_jacobians.size(); + data.m_jacobians.resize(data.m_jacobians.size()+ndofA); + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); + btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); + + float* jac1=&data.m_jacobians[solverConstraint.m_jacAindex]; + multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); + float* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); + } else + { + btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB); + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = contactNormalOnB; + } + + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumLinks() + 6; + + solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); + if (solverConstraint.m_deltaVelBindex <0) + { + solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size(); + multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); + } + + solverConstraint.m_jacBindex = data.m_jacobians.size(); + + data.m_jacobians.resize(data.m_jacobians.size()+ndofB); + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB); + btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); + + multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); + multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v); + } else + { + btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + solverConstraint.m_contactNormal2 = -contactNormalOnB; + } + + { + + btVector3 vec; + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + btScalar* jacB = 0; + btScalar* jacA = 0; + btScalar* lambdaA =0; + btScalar* lambdaB =0; + int ndofA = 0; + if (multiBodyA) + { + ndofA = multiBodyA->getNumLinks() + 6; + jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; + lambdaA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + { + float j = jacA[i] ; + float l =lambdaA[i]; + denom0 += j*l; + } + } else + { + if (rb0) + { + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec); + } + } + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumLinks() + 6; + jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; + lambdaB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + { + float j = jacB[i] ; + float l =lambdaB[i]; + denom1 += j*l; + } + + } else + { + if (rb1) + { + vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec); + } + } + + if (multiBodyA && (multiBodyA==multiBodyB)) + { + // ndof1 == ndof2 in this case + for (int i = 0; i < ndofA; ++i) + { + denom1 += jacB[i] * lambdaA[i]; + denom1 += jacA[i] * lambdaB[i]; + } + } + + float d = denom0+denom1; + if (btFabs(d)>SIMD_EPSILON) + { + + solverConstraint.m_jacDiagABInv = relaxation/(d); + } else + { + solverConstraint.m_jacDiagABInv = 1.f; + } + + } + + + //compute rhs and remaining solverConstraint fields + + + + btScalar restitution = 0.f; + btScalar penetration = isFriction? 0 : position+infoGlobal.m_linearSlop; + + btScalar rel_vel = 0.f; + int ndofA = 0; + int ndofB = 0; + { + + btVector3 vel1,vel2; + if (multiBodyA) + { + ndofA = multiBodyA->getNumLinks() + 6; + btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA ; ++i) + rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; + } else + { + if (rb0) + { + rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); + } + } + if (multiBodyB) + { + ndofB = multiBodyB->getNumLinks() + 6; + btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB ; ++i) + rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; + + } else + { + if (rb1) + { + rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); + } + } + + solverConstraint.m_friction = 0.f;//cp.m_combinedFriction; + + + restitution = restitution * -rel_vel;//restitutionCurve(rel_vel, cp.m_combinedRestitution); + if (restitution <= btScalar(0.)) + { + restitution = 0.f; + }; + } + + + ///warm starting (or zero if disabled) + /* + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; + + if (solverConstraint.m_appliedImpulse) + { + if (multiBodyA) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + multiBodyA->applyDeltaVee(deltaV,impulse); + applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); + } else + { + if (rb0) + bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); + } + if (multiBodyB) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + multiBodyB->applyDeltaVee(deltaV,impulse); + applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); + } else + { + if (rb1) + bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); + } + } + } else + */ + { + solverConstraint.m_appliedImpulse = 0.f; + } + + solverConstraint.m_appliedPushImpulse = 0.f; + + { + + + btScalar positionalError = 0.f; + btScalar velocityError = restitution - rel_vel;// * damping; + + + btScalar erp = infoGlobal.m_erp2; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + erp = infoGlobal.m_erp; + } + + if (penetration>0) + { + positionalError = 0; + velocityError = -penetration / infoGlobal.m_timeStep; + + } else + { + positionalError = -penetration * erp/infoGlobal.m_timeStep; + } + + btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + //combine position and velocity into rhs + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; + + } else + { + //split position and velocity into rhs and m_rhsPenetration + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = penetrationImpulse; + } + + solverConstraint.m_cfm = 0.f; + solverConstraint.m_lowerLimit = 0; + solverConstraint.m_upperLimit = 1e10f; + } + +} diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 99df50c87..ba68fd305 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -33,6 +33,8 @@ struct btMultiBodyJacobianData btAlignedObjectArray scratch_r; btAlignedObjectArray scratch_v; btAlignedObjectArray scratch_m; + btAlignedObjectArray* m_solverBodyPool; + }; @@ -58,21 +60,32 @@ protected: // positions. (one per row.) btAlignedObjectArray m_data; + void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof); + + void fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint, + btMultiBodyJacobianData& data, + const btVector3& contactNormalOnB, + const btVector3& posAworld, const btVector3& posBworld, + btScalar position, + const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); + + btScalar fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow, + btMultiBodyJacobianData& data, + btScalar* jacOrgA,btScalar* jacOrgB, + const btContactSolverInfo& infoGlobal, + btScalar desiredVelocity, + btScalar lowerLimit, + btScalar upperLimit); + public: - btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral) - :m_bodyA(bodyA), - m_bodyB(bodyB), - m_linkA(linkA), - m_linkB(linkB), - m_num_rows(numRows), - m_isUnilateral(isUnilateral) - { - m_jac_size_A = (6 + bodyA->getNumLinks()); - m_jac_size_both = (m_jac_size_A + (bodyB ? 6 + bodyB->getNumLinks() : 0)); - m_pos_offset = ((1 + m_jac_size_both)*m_num_rows); - m_data.resize((2 + m_jac_size_both) * m_num_rows); - } + btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral); + virtual ~btMultiBodyConstraint(); + + + virtual int getIslandIdA() const =0; virtual int getIslandIdB() const =0; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 9645da57a..2ac606b25 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -764,6 +764,7 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol for (int i=0;icreateConstraintRows(m_multiBodyNonContactConstraints,m_data, infoGlobal); } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 213a982b7..5cf197997 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -103,6 +103,18 @@ void btMultiBodyDynamicsWorld::calculateSimulationIslands() } } + //merge islands linked by multibody constraints + { + for (int i=0;im_multiBodyConstraints.size();i++) + { + btMultiBodyConstraint* c = m_multiBodyConstraints[i]; + int tagA = c->getIslandIdA(); + int tagB = c->getIslandIdB(); + if (tagA>=0 && tagB>=0) + getSimulationIslandManager()->getUnionFind().unite(tagA, tagB); + } + } + //Store the island id in each body getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); @@ -353,7 +365,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0; btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0; btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0; - + m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher); m_bodies.resize(0); m_manifolds.resize(0); @@ -386,7 +398,6 @@ btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld () void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { - btVector3 g = m_gravity; btAlignedObjectArray scratch_r; btAlignedObjectArray scratch_v; btAlignedObjectArray scratch_m; @@ -408,6 +419,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i]; } + m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate()); btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; @@ -444,11 +456,11 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) scratch_m.resize(bod->getNumLinks()+1); bod->clearForcesAndTorques(); - bod->addBaseForce(g * bod->getBaseMass()); + bod->addBaseForce(m_gravity * bod->getBaseMass()); for (int j = 0; j < bod->getNumLinks(); ++j) { - bod->addLinkForce(j, g * bod->getLinkMass(j)); + bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); } bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index 78e70362c..5ef65dc71 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -1,3 +1,19 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans #include "btMultiBodyJointLimitConstraint.h" #include "btMultiBody.h" @@ -73,243 +89,45 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); constraintRow.m_multiBodyA = m_bodyA; constraintRow.m_multiBodyB = m_bodyB; - btScalar penetration = getPosition(row); - fillConstraintRow(constraintRow,data,jacobianA(row),jacobianB(row),penetration,0,0,infoGlobal); + + btScalar rel_vel = fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,0,0,1e30); + { + btScalar penetration = getPosition(row); + 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; + } + } } } -void btMultiBodyJointLimitConstraint::fillConstraintRow(btMultiBodySolverConstraint& constraintRow, - btMultiBodyJacobianData& data, - btScalar* jacOrgA,btScalar* jacOrgB, - btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff, - const btContactSolverInfo& infoGlobal) - -{ - - 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 = 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 bfcc48525..563ef4b2e 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h @@ -25,11 +25,6 @@ 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); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp new file mode 100644 index 000000000..2aeb8018c --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -0,0 +1,89 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#include "btMultiBodyJointMotor.h" +#include "btMultiBody.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + + +btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse) + :btMultiBodyConstraint(body,body,link,link,1,true), + m_desiredVelocity(desiredVelocity), + m_maxMotorImpulse(maxMotorImpulse) +{ + // the data.m_jacobians never change, so may as well + // initialize them here + + // note: we rely on the fact that data.m_jacobians are + // always initialized to zero by the Constraint ctor + + // row 0: the lower bound + jacobianA(0)[6 + link] = 1; +} +btMultiBodyJointMotor::~btMultiBodyJointMotor() +{ +} + +int btMultiBodyJointMotor::getIslandIdA() const +{ + 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 btMultiBodyJointMotor::getIslandIdB() const +{ + 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 btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) +{ + // only positions need to be updated -- data.m_jacobians and force + // directions were set in the ctor and never change. + + + + for (int row=0;rowgetIslandTag(); + + if (m_bodyA) + { + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + for (int i=0;igetNumLinks();i++) + { + if (m_bodyA->getLink(i).m_collider) + return m_bodyA->getLink(i).m_collider->getIslandTag(); + } + } + return -1; +} + +int btMultiBodyPoint2Point::getIslandIdB() const +{ + if (m_rigidBodyB) + return m_rigidBodyB->getIslandTag(); + if (m_bodyB) + { + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); + if (col) + return col->getIslandTag(); + + for (int i=0;igetNumLinks();i++) + { + col = m_bodyB->getLink(i).m_collider; + if (col) + return col->getIslandTag(); + } + } + return -1; +} + + + +void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) +{ + +// int i=1; + for (int i=0;i<3;i++) + { + + btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); + + constraintRow.m_solverBodyIdB = 0; + btVector3 contactNormalOnB(0,0,0); + contactNormalOnB[i] = -1; + + btScalar penetration = 0; + + // Convert local points back to world + btVector3 pivotAworld = m_pivotInA; + if (m_rigidBodyA) + { + 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) + { + pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB; + } else + { + if (m_bodyB) + pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); + } + btScalar position = (pivotAworld-pivotBworld).dot(contactNormalOnB); + btScalar relaxation = 1.f; + fillMultiBodyConstraintMixed(constraintRow, data, + contactNormalOnB, + pivotAworld, pivotBworld, + position, + infoGlobal, + relaxation, + false); + constraintRow.m_lowerLimit = -m_maxAppliedImpulse; + constraintRow.m_upperLimit = m_maxAppliedImpulse; + + } +} diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h new file mode 100644 index 000000000..3cefa957a --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h @@ -0,0 +1,67 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was written by Erwin Coumans + +#ifndef BT_MULTIBODY_POINT2POINT_H +#define BT_MULTIBODY_POINT2POINT_H + +#include "btMultiBodyConstraint.h" + +class btMultiBodyPoint2Point : public btMultiBodyConstraint +{ +protected: + + btRigidBody* m_rigidBodyA; + btRigidBody* m_rigidBodyB; + btVector3 m_pivotInA; + btVector3 m_pivotInB; + btScalar m_maxAppliedImpulse; + +public: + + btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB); + btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB); + + virtual ~btMultiBodyPoint2Point(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + const btVector3& getPivotInB() const + { + return m_pivotInB; + } + + void setPivotInB(const btVector3& pivotInB) + { + m_pivotInB = pivotInB; + } + + btScalar getMaxAppliedImpulse() const + { + return m_maxAppliedImpulse; + } + void setMaxAppliedImpulse(btScalar maxImp) + { + m_maxAppliedImpulse = maxImp; + } +}; + +#endif //BT_MULTIBODY_POINT2POINT_H diff --git a/src/Makefile.am b/src/Makefile.am index 22486bbd5..d65ed32c1 100644 --- a/src/Makefile.am +++ b/src/Makefile.am @@ -370,7 +370,12 @@ libBulletDynamics_la_SOURCES = \ BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h \ BulletDynamics/Featherstone/btMultiBodyLink.h \ BulletDynamics/Featherstone/btMultiBodyLinkCollider.h \ - BulletDynamics/Featherstone/btMultiBodySolverConstraint.h + BulletDynamics/Featherstone/btMultiBodySolverConstraint.h \ + BulletDynamics/Featherstone/btMultiBodyConstraint.h \ + BulletDynamics/Featherstone/btMultiBodyPoint2Point.h \ + BulletDynamics/Featherstone/btMultiBodyConstraint.cpp \ + BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp + libBulletSoftBody_la_SOURCES = \ BulletSoftBody/btDefaultSoftBodySolver.cpp \ @@ -437,6 +442,8 @@ nobase_bullet_include_HEADERS += \ BulletDynamics/Featherstone/btMultiBodyLink.h \ BulletDynamics/Featherstone/btMultiBodyLinkCollider.h \ BulletDynamics/Featherstone/btMultiBodySolverConstraint.h \ + BulletDynamics/Featherstone/btMultiBodyConstraint.h \ + BulletDynamics/Featherstone/btMultiBodyPoint2Point.h \ BulletCollision/CollisionShapes/btShapeHull.h \ BulletCollision/CollisionShapes/btConcaveShape.h \ BulletCollision/CollisionShapes/btCollisionMargin.h \