diff --git a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp index a094327dc..0d32aabee 100644 --- a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp +++ b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp @@ -213,9 +213,17 @@ void FeatherstoneMultiBodyDemo::initPhysics() mb = createFeatherstoneMultiBody(world, 5, btVector3 (0,29.5,-2), false,false,true); - - - + bool testRemoveLinks = false; + if (testRemoveLinks) + { + while (mb->getNumLinks()) + { + btCollisionObject* col = mb->getLink(mb->getNumLinks()-1).m_collider; + m_dynamicsWorld->removeCollisionObject(col); + delete col; + mb->setNumLinks(mb->getNumLinks()-1); + } + } } btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic, bool canSleep) @@ -304,21 +312,21 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult { 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); + btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(bod,-1); body->setCollisionShape(box); - multiBody->setCollisionShape(box); + col->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); + col->setWorldTransform(tr); - world->addCollisionObject(multiBody, btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter); - multiBody->setFriction(1); - bod->addLinkCollider(multiBody); + world->addCollisionObject(col, btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter); + col->setFriction(1); + bod->setBaseCollider(col); } } @@ -351,8 +359,8 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult col->setWorldTransform(tr); col->setFriction(1); world->addCollisionObject(col,btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter);//,2,1); - - bod->addLinkCollider(col); + + bod->getLink(i).m_collider=col; //app->drawBox(halfExtents, pos,quat); } diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index 74518acb6..b3fffdecd 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://bulletphysics.com/Bullet/ +Copyright (c) 2003-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. @@ -18,13 +18,11 @@ subject to the following restrictions: * @mainpage Bullet Documentation * * @section intro_sec Introduction - * Bullet Collision Detection & Physics SDK - * * Bullet is a Collision Detection and Rigid Body Dynamics Library. The Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ). * * The main documentation is Bullet_User_Manual.pdf, included in the source code distribution. * There is the Physics Forum for feedback and general Collision Detection and Physics discussions. - * Please visit http://www.bulletphysics.com + * Please visit http://www.bulletphysics.org * * @section install_sec Installation * @@ -32,7 +30,16 @@ subject to the following restrictions: * You can download the Bullet Physics Library from the Google Code repository: http://code.google.com/p/bullet/downloads/list * * @subsection step2 Step 2: Building - * Bullet main build system for all platforms is cmake, you can download http://www.cmake.org + * Bullet has multiple build systems, including premake, cmake and autotools. Premake and cmake support all platforms. + * Premake is included in the Bullet/build folder for Windows, Mac OSX and Linux. + * Under Windows you can click on Bullet/build/vs2010.bat to create Microsoft Visual Studio projects. + * On Mac OSX and Linux you can open a terminal and generate Makefile, codeblocks or Xcode4 projects: + * cd Bullet/build + * ./premake4_osx gmake or ./premake4_linux gmake or ./premake4_linux64 gmake or (for Mac) ./premake4_osx xcode4 + * cd Bullet/build/gmake + * make + * + * An alternative to premake is cmake. You can download cmake from http://www.cmake.org * cmake can autogenerate projectfiles for Microsoft Visual Studio, Apple Xcode, KDevelop and Unix Makefiles. * The easiest is to run the CMake cmake-gui graphical user interface and choose the options and generate projectfiles. * You can also use cmake in the command-line. Here are some examples for various platforms: diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 41439fdff..d336f7c1f 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -81,15 +81,15 @@ btMultiBody::btMultiBody(int n_links, const btVector3 &inertia, bool fixed_base_, bool can_sleep_) - : num_links(n_links), - base_quat(0, 0, 0, 1), + : base_quat(0, 0, 0, 1), base_mass(mass), base_inertia(inertia), fixed_base(fixed_base_), awake(true), can_sleep(can_sleep_), - sleep_timer(0) + sleep_timer(0), + m_baseCollider(0) { links.resize(n_links); @@ -146,24 +146,8 @@ void btMultiBody::setupRevolute(int i, links[i].updateCache(); } -void btMultiBody::addLinkCollider(btMultiBodyLinkCollider* collider) -{ - m_colliders.push_back(collider); -} -btMultiBodyLinkCollider* btMultiBody::getLinkCollider(int index) -{ - return m_colliders[index]; -} -const btMultiBodyLinkCollider* btMultiBody::getLinkCollider(int index) const -{ - return m_colliders[index]; -} -int btMultiBody::getNumLinkColliders() const -{ - return m_colliders.size(); -} int btMultiBody::getParent(int i) const @@ -262,6 +246,7 @@ btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const { + int num_links = getNumLinks(); // Calculates the velocities of each link (and the base) in its local frame omega[0] = quatRotate(base_quat ,getBaseOmega()); vel[0] = quatRotate(base_quat ,getBaseVel()); @@ -282,6 +267,7 @@ void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const btScalar btMultiBody::getKineticEnergy() const { + int num_links = getNumLinks(); // TODO: would be better not to allocate memory here btAlignedObjectArray omega;omega.resize(num_links+1); btAlignedObjectArray vel;vel.resize(num_links+1); @@ -301,6 +287,7 @@ btScalar btMultiBody::getKineticEnergy() const btVector3 btMultiBody::getAngularMomentum() const { + int num_links = getNumLinks(); // TODO: would be better not to allocate memory here btAlignedObjectArray omega;omega.resize(num_links+1); btAlignedObjectArray vel;vel.resize(num_links+1); @@ -324,7 +311,7 @@ void btMultiBody::clearForcesAndTorques() base_force.setValue(0, 0, 0); base_torque.setValue(0, 0, 0); - for (int i = 0; i < num_links; ++i) { + for (int i = 0; i < getNumLinks(); ++i) { links[i].applied_force.setValue(0, 0, 0); links[i].applied_torque.setValue(0, 0, 0); links[i].joint_torque = 0; @@ -333,7 +320,7 @@ void btMultiBody::clearForcesAndTorques() void btMultiBody::clearVelocities() { - for (int i = 0; i < 6 + num_links; ++i) + for (int i = 0; i < 6 + getNumLinks(); ++i) { real_buf[i] = 0.f; } @@ -406,6 +393,8 @@ void btMultiBody::stepVelocities(btScalar dt, // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame), // num_links joint acceleration values. + int num_links = getNumLinks(); + const btScalar DAMPING_K1 = btScalar(0.0); //const btScalar DAMPING_K2 = btScalar(0); const btScalar DAMPING_K2 = btScalar(0.0); @@ -670,6 +659,7 @@ void btMultiBody::stepVelocities(btScalar dt, void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const { + int num_links = getNumLinks(); ///solve I * x = rhs, so the result = invI * rhs if (num_links == 0) { @@ -722,7 +712,7 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output { // Temporary matrices/vectors -- use scratch space from caller // so that we don't have to keep reallocating every frame - + int num_links = getNumLinks(); scratch_r.resize(num_links); scratch_v.resize(4*num_links + 4); @@ -843,6 +833,7 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output void btMultiBody::stepPositions(btScalar dt) { + int num_links = getNumLinks(); // step position by adding dt * velocity btVector3 v = getBaseVel(); base_pos += dt * v; @@ -885,6 +876,7 @@ void btMultiBody::fillContactJacobian(int link, btAlignedObjectArray &scratch_m) const { // temporary space + int num_links = getNumLinks(); scratch_v.resize(2*num_links + 2); scratch_m.resize(num_links + 1); @@ -967,6 +959,8 @@ void btMultiBody::goToSleep() void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep) { + int num_links = getNumLinks(); + if (!can_sleep) return; // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 49443fbff..9cc5df73e 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -69,11 +69,31 @@ public: const btVector3 &joint_axis, // in my frame const btVector3 &parent_axis_position, // vector from parent COM to joint axis, in PARENT frame const btVector3 &my_axis_position); // vector from joint axis to my COM, in MY frame + + const btMultibodyLink& getLink(int index) const + { + return links[index]; + } + + btMultibodyLink& getLink(int index) + { + return links[index]; + } + + + void setBaseCollider(btMultiBodyLinkCollider* collider)//collider can be NULL to disable collision for the base + { + m_baseCollider = collider; + } + const btMultiBodyLinkCollider* getBaseCollider() const + { + return m_baseCollider; + } + btMultiBodyLinkCollider* getBaseCollider() + { + return m_baseCollider; + } - void addLinkCollider(btMultiBodyLinkCollider* collider); - btMultiBodyLinkCollider* getLinkCollider(int index); - const btMultiBodyLinkCollider* getLinkCollider(int index) const; - int getNumLinkColliders() const; // // get parent // input: link num from 0 to num_links-1 @@ -86,7 +106,7 @@ public: // get number of links, masses, moments of inertia // - int getNumLinks() const { return num_links; } + int getNumLinks() const { return links.size(); } btScalar getBaseMass() const { return base_mass; } const btVector3 & getBaseInertia() const { return base_inertia; } btScalar getLinkMass(int i) const; @@ -238,12 +258,12 @@ public: // apply a delta-vee directly. used in sequential impulses code. void applyDeltaVee(const btScalar * delta_vee) { - for (int i = 0; i < 6 + num_links; ++i) + for (int i = 0; i < 6 + getNumLinks(); ++i) real_buf[i] += delta_vee[i]; } void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier) { - for (int i = 0; i < 6 + num_links; ++i) + for (int i = 0; i < 6 + getNumLinks(); ++i) real_buf[i] += delta_vee[i] * multiplier; } @@ -290,6 +310,11 @@ public: //printf("for %p setCompanionId(%d)\n",this, id); m_companionId = id; } + + void setNumLinks(int numLinks)//careful: when changing the number of links, make sure to re-initialize or update existing links + { + links.resize(numLinks); + } private: btMultiBody(const btMultiBody &); // not implemented void operator=(const btMultiBody &); // not implemented @@ -300,7 +325,8 @@ private: private: - int num_links; // includes base. + + btMultiBodyLinkCollider* m_baseCollider;//can be NULL btVector3 base_pos; // position of COM of base (world frame) btQuaternion base_quat; // rotates world points into base frame @@ -311,7 +337,7 @@ private: btVector3 base_force; // external force applied to base. World frame. btVector3 base_torque; // external torque applied to base. World frame. - btAlignedObjectArray links; // array of links, excluding the base. index from 0 to num_links-2. + btAlignedObjectArray links; // array of links, excluding the base. index from 0 to num_links-1. btAlignedObjectArray m_colliders; // diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index ae1f4ace9..213a982b7 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -78,17 +78,17 @@ void btMultiBodyDynamicsWorld::calculateSimulationIslands() } } - //merge islands linked by Featherstone links + //merge islands linked by Featherstone link colliders for (int i=0;igetNumLinkColliders()>1) { - btMultiBodyLinkCollider* prev = body->getLinkCollider(0); - for (int b=1;bgetNumLinkColliders();b++) - { - btMultiBodyLinkCollider* cur = body->getLinkCollider(b); + btMultiBodyLinkCollider* prev = body->getBaseCollider(); + for (int b=0;bgetNumLinks();b++) + { + btMultiBodyLinkCollider* cur = body->getLink(b).m_collider; + if (((cur) && (!(cur)->isStaticOrKinematicObject())) && ((prev) && (!(prev)->isStaticOrKinematicObject()))) { @@ -96,7 +96,8 @@ void btMultiBodyDynamicsWorld::calculateSimulationIslands() int tagCur = cur->getIslandTag(); getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur); } - prev = cur; + if (cur && !cur->isStaticOrKinematicObject()) + prev = cur; } } @@ -122,10 +123,16 @@ void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) body->checkMotionAndSleepIfRequired(timeStep); if (!body->isAwake()) { - for (int b=0;bgetNumLinkColliders();b++) + btMultiBodyLinkCollider* col = body->getBaseCollider(); + if (col && col->getActivationState() == ACTIVE_TAG) { - btMultiBodyLinkCollider* col = body->getLinkCollider(b); - if (col->getActivationState() == ACTIVE_TAG) + col->setActivationState( WANTS_DEACTIVATION); + col->setDeactivationTime(0.f); + } + for (int b=0;bgetNumLinks();b++) + { + btMultiBodyLinkCollider* col = body->getLink(b).m_collider; + if (col && col->getActivationState() == ACTIVE_TAG) { col->setActivationState( WANTS_DEACTIVATION); col->setDeactivationTime(0.f); @@ -133,10 +140,14 @@ void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) } } else { - for (int b=0;bgetNumLinkColliders();b++) + btMultiBodyLinkCollider* col = body->getBaseCollider(); + if (col && col->getActivationState() != DISABLE_DEACTIVATION) + col->setActivationState( ACTIVE_TAG ); + + for (int b=0;bgetNumLinks();b++) { - btMultiBodyLinkCollider* col = body->getLinkCollider(b); - if (col->getActivationState() != DISABLE_DEACTIVATION) + btMultiBodyLinkCollider* col = body->getLink(b).m_collider; + if (col && col->getActivationState() != DISABLE_DEACTIVATION) col->setActivationState( ACTIVE_TAG ); } } @@ -415,10 +426,17 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) btMultiBody* bod = m_multiBodies[i]; bool isSleeping = false; - if (bod->getNumLinkColliders() && bod->getLinkCollider(0)->getActivationState()==ISLAND_SLEEPING) + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) { isSleeping = true; } + for (int b=0;bgetNumLinks();b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) + isSleeping = true; + } + if (!isSleeping) { scratch_r.resize(bod->getNumLinks()+1); @@ -458,10 +476,17 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) { btMultiBody* bod = m_multiBodies[b]; bool isSleeping = false; - if (bod->getNumLinkColliders() && bod->getLinkCollider(0)->getActivationState()==ISLAND_SLEEPING) + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) { isSleeping = true; } + for (int b=0;bgetNumLinks();b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) { int nLinks = bod->getNumLinks(); @@ -476,6 +501,20 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) world_to_local[0] = bod->getWorldToBaseRot(); local_origin[0] = bod->getBasePos(); + + if (bod->getBaseCollider()) + { + btVector3 posr = local_origin[0]; + float pos[4]={posr.x(),posr.y(),posr.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()}; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + + bod->getBaseCollider()->setWorldTransform(tr); + + } for (int k=0;kgetNumLinks();k++) { @@ -484,25 +523,27 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , bod->getRVector(k))); } - for (int m=0;mgetNumLinkColliders();m++) + + for (int m=0;mgetNumLinks();m++) { - btMultiBodyLinkCollider* col = bod->getLinkCollider(m); - int link = col->m_link; - int index = link+1; + btMultiBodyLinkCollider* col = bod->getLink(m).m_collider; + if (col) + { + int link = col->m_link; + btAssert(link == m); - float halfExtents[3]={7.5,0.05,4.5}; - btVector3 posr = local_origin[index]; - float pos[4]={posr.x(),posr.y(),posr.z(),1}; - - float quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()}; - //app->drawBox(halfExtents, pos,quat); + int index = link+1; - btTransform tr; - tr.setIdentity(); - tr.setOrigin(posr); - tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + btVector3 posr = local_origin[index]; + float pos[4]={posr.x(),posr.y(),posr.z(),1}; + float quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()}; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); - col->setWorldTransform(tr); + col->setWorldTransform(tr); + } } } else { diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index cf5182be4..78e70362c 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -28,12 +28,30 @@ btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint() int btMultiBodyJointLimitConstraint::getIslandIdA() const { - return m_bodyA->getLinkCollider(0)->getIslandTag(); + 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 btMultiBodyJointLimitConstraint::getIslandIdB() const { - return m_bodyB->getLinkCollider(0)->getIslandTag(); + 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; } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index ccf9a7fcb..12f5b80ab 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -63,6 +63,8 @@ struct btMultibodyLink btVector3 applied_torque; // In WORLD frame btScalar joint_torque; + class btMultiBodyLinkCollider* m_collider; + // ctor: set some sensible defaults btMultibodyLink() : joint_pos(0), @@ -71,7 +73,8 @@ struct btMultibodyLink zero_rot_parent_to_this(1, 0, 0, 0), is_revolute(false), cached_rot_parent_to_this(1, 0, 0, 0), - joint_torque(0) + joint_torque(0), + m_collider(0) { inertia.setValue(1, 1, 1); axis_top.setValue(0, 0, 0);