diff --git a/Demos/CMakeLists.txt b/Demos/CMakeLists.txt index bb756b07b..769ad5267 100644 --- a/Demos/CMakeLists.txt +++ b/Demos/CMakeLists.txt @@ -12,7 +12,7 @@ IF (USE_GLUT) SET(SharedDemoSubdirs OpenGL AllBulletDemos ConvexDecompositionDemo CcdPhysicsDemo BulletXmlImportDemo ConstraintDemo SliderConstraintDemo GenericJointDemo Raytracer - RagdollDemo ForkLiftDemo BasicDemo RollingFrictionDemo RaytestDemo VoronoiFractureDemo + RagdollDemo ForkLiftDemo BasicDemo FeatherstoneMultiBodyDemo RollingFrictionDemo RaytestDemo VoronoiFractureDemo GyroscopicDemo FractureDemo Box2dDemo BspDemo MovingConcaveDemo VehicleDemo UserCollisionAlgorithm CharacterDemo SoftDemo CollisionInterfaceDemo ConcaveConvexcastDemo SimplexDemo DynamicControlDemo @@ -56,6 +56,7 @@ ELSE (USE_GLUT) InternalEdgeDemo GimpactTestDemo GyroscopicDemo + FeatherstoneMultiBodyDemo GenericJointDemo SerializeDemo SoftDemo diff --git a/Demos/FeatherstoneMultiBodyDemo/CMakeLists.txt b/Demos/FeatherstoneMultiBodyDemo/CMakeLists.txt new file mode 100644 index 000000000..12105cab0 --- /dev/null +++ b/Demos/FeatherstoneMultiBodyDemo/CMakeLists.txt @@ -0,0 +1,87 @@ +# This is basically the overall name of the project in Visual Studio this is the name of the Solution File + + +# For every executable you have with a main method you should have an add_executable line below. +# For every add executable line you should list every .cpp and .h file you have associated with that executable. + + +# This is the variable for Windows. I use this to define the root of my directory structure. +SET(GLUT_ROOT ${BULLET_PHYSICS_SOURCE_DIR}/Glut) + +# You shouldn't have to modify anything below this line +######################################################## + +INCLUDE_DIRECTORIES( +${BULLET_PHYSICS_SOURCE_DIR}/src ${BULLET_PHYSICS_SOURCE_DIR}/Demos/OpenGL +) + + + +IF (USE_GLUT) + LINK_LIBRARIES( + OpenGLSupport BulletDynamics BulletCollision LinearMath ${GLUT_glut_LIBRARY} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} + ) + +IF (WIN32) +ADD_EXECUTABLE(AppFeatherstoneMultiBodyDemo + main.cpp + FeatherstoneMultiBodyDemo.cpp + FeatherstoneMultiBodyDemo.h + ${BULLET_PHYSICS_SOURCE_DIR}/build/bullet.rc + ) +ELSE() + ADD_EXECUTABLE(AppFeatherstoneMultiBodyDemo + main.cpp + FeatherstoneMultiBodyDemo.cpp + FeatherstoneMultiBodyDemo.h + ) +ENDIF() + + + + + IF (WIN32) + IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) + IF (CMAKE_CL_64) + ADD_CUSTOM_COMMAND( + TARGET AppFeatherstoneMultiBodyDemo + POST_BUILD + COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/glut64.dll ${CMAKE_CURRENT_BINARY_DIR} + ) + ELSE(CMAKE_CL_64) + ADD_CUSTOM_COMMAND( + TARGET AppFeatherstoneMultiBodyDemo + POST_BUILD +# COMMAND copy /Y ${BULLET_PHYSICS_SOURCE_DIR}/GLUT32.DLL ${CMAKE_CURRENT_BINARY_DIR} + COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/GLUT32.DLL ${CMAKE_CURRENT_BINARY_DIR} + ) + ENDIF(CMAKE_CL_64) + ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) + + ENDIF(WIN32) +ELSE (USE_GLUT) + + + + LINK_LIBRARIES( + OpenGLSupport BulletDynamics BulletCollision LinearMath ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} + ) + + + ADD_EXECUTABLE(AppFeatherstoneMultiBodyDemo + WIN32 + ../OpenGL/Win32AppMain.cpp + Win32FeatherstoneMultiBodyDemo.cpp + FeatherstoneMultiBodyDemo.cpp + FeatherstoneMultiBodyDemo.h + ${BULLET_PHYSICS_SOURCE_DIR}/build/bullet.rc + ) + + +ENDIF (USE_GLUT) + +IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) + SET_TARGET_PROPERTIES(AppBasicDemo PROPERTIES DEBUG_POSTFIX "_Debug") + SET_TARGET_PROPERTIES(AppBasicDemo PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel") + SET_TARGET_PROPERTIES(AppBasicDemo PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo") +ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) \ No newline at end of file diff --git a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp new file mode 100644 index 000000000..b9776e8fc --- /dev/null +++ b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp @@ -0,0 +1,398 @@ +/* +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. +*/ + +///experimental support for Featherstone multi body (articulated hierarchies) + + +///create 125 (5x5x5) dynamic object +#define ARRAY_SIZE_X 5 +#define ARRAY_SIZE_Y 5 +#define ARRAY_SIZE_Z 5 + +//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 +#define START_POS_Z -3 + +#include "FeatherstoneMultiBodyDemo.h" + +#include "BulletDynamics/Featherstone/btMultiBody.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyLink.h" + + + +#include "GlutStuff.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" + +#include //printf debugging +#include "GLDebugDrawer.h" +#include "LinearMath/btAabbUtil2.h" + +static GLDebugDrawer gDebugDraw; + + +void FeatherstoneMultiBodyDemo::clientMoveAndDisplay() +{ + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + //simple dynamics world doesn't handle fixed-time-stepping + float ms = getDeltaTimeMicroseconds(); + + ///step the simulation + if (m_dynamicsWorld) + { + m_dynamicsWorld->stepSimulation(ms / 1000000.f); + //optional but useful: debug drawing + m_dynamicsWorld->debugDrawWorld(); + + btVector3 aabbMin(1,1,1); + btVector3 aabbMax(2,2,2); + + + } + + renderme(); + + glFlush(); + + swapBuffers(); + +} + + + +void FeatherstoneMultiBodyDemo::displayCallback(void) { + + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + renderme(); + + //optional but useful: debug drawing to detect problems + if (m_dynamicsWorld) + m_dynamicsWorld->debugDrawWorld(); + + glFlush(); + swapBuffers(); +} + + + + + +void FeatherstoneMultiBodyDemo::initPhysics() +{ + //m_idle=true; + setTexturing(true); + setShadows(true); + + setCameraDistance(btScalar(SCALING*50.)); + + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btDefaultCollisionConfiguration(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + + //Use the btMultiBodyConstraintSolver for Featherstone btMultiBody support + btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver; + m_solver = sol; + + //use btMultiBodyDynamicsWorld for Featherstone btMultiBody support + btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher,m_broadphase,sol,m_collisionConfiguration); + m_dynamicsWorld = world; + m_dynamicsWorld->setDebugDrawer(&gDebugDraw); + + m_dynamicsWorld->setGravity(btVector3(0,-10,0)); + + ///create a few basic rigid bodies + btBoxShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); + //groundShape->initializePolyhedralFeatures(); +// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0,-50,0)); + + //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)); + //btCollisionShape* colShape = new btSphereShape(btScalar(1.)); + m_collisionShapes.push_back(colShape); + + /// Create Dynamic Objects + btTransform startTransform; + startTransform.setIdentity(); + + btScalar mass(1.f); + + //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) + colShape->calculateLocalInertia(mass,localInertia); + + float start_x = START_POS_X - ARRAY_SIZE_X/2; + float start_y = START_POS_Y; + float start_z = START_POS_Z - ARRAY_SIZE_Z/2; + + for (int k=0;kaddRigidBody(body);//,1,1+2); + } + } + } + } + + + { + int n_links = 5; + float mass = 13.5; + btVector3 inertia(91,344,253); + bool isFixedBase = false; + bool canSleep = true;//false; + + btMultiBody * bod = new btMultiBody(n_links, mass, inertia, isFixedBase, canSleep); + btVector3 pos(0,9.5,-2); + //btQuaternion orn(btVector3(0,0,1),-0.25*SIMD_HALF_PI);//0,0,0,1); + btQuaternion orn(0,0,0,1); + bod->setBasePos(pos); + bod->setWorldToBaseRot(orn); + btVector3 vel(0,0,0); + bod->setBaseVel(vel); + + { + + btVector3 joint_axis_hinge(1,0,0); + btVector3 joint_axis_prismatic(0,0,1); + btQuaternion parent_to_child = orn.inverse(); + btVector3 joint_axis_child_prismatic = quatRotate(parent_to_child ,joint_axis_prismatic); + btVector3 joint_axis_child_hinge = quatRotate(parent_to_child , joint_axis_hinge); + + int this_link_num = -1; + int link_num_counter = 0; + + + + btVector3 pos(0,0,9.0500002); + + btVector3 joint_axis_position(0,0,4.5250001); + + for (int i=0;i0) + initial_joint_angle = -0.06f; + + const int child_link_num = link_num_counter++; + + if (0)//i==(n_links-1)) + { + bod->setupPrismatic(child_link_num, mass, inertia, this_link_num, + parent_to_child, joint_axis_child_prismatic, quatRotate(parent_to_child , pos)); + } else + { + bod->setupRevolute(child_link_num, mass, inertia, this_link_num,parent_to_child, joint_axis_child_hinge, + joint_axis_position,quatRotate(parent_to_child , (pos - joint_axis_position))); + } + bod->setJointPos(child_link_num, initial_joint_angle); + this_link_num = i; + } + } + + //add a collider for the base + { + + btAlignedObjectArray world_to_local; + world_to_local.resize(n_links+1); + + btAlignedObjectArray local_origin; + local_origin.resize(n_links+1); + world_to_local[0] = bod->getWorldToBaseRot(); + local_origin[0] = bod->getBasePos(); + //float halfExtents[3]={7.5,0.05,4.5}; + float halfExtents[3]={7.5,0.45,4.5}; + { + + float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; + float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; + + + if (1) + { + btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])); + btRigidBody* body = new btRigidBody(mass,0,box,inertia); + btMultiBodyLinkCollider* multiBody= new btMultiBodyLinkCollider(bod,-1); + + body->setCollisionShape(box); + multiBody->setCollisionShape(box); + + btTransform tr; + tr.setIdentity(); + tr.setOrigin(local_origin[0]); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + body->setWorldTransform(tr); + multiBody->setWorldTransform(tr); + + world->addCollisionObject(multiBody, btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter); + multiBody->setFriction(1); + bod->addLinkCollider(multiBody); + + } + } + + + for (int i=0;igetNumLinks();i++) + { + const int parent = bod->getParent(i); + world_to_local[i+1] = bod->getParentToLocalRot(i) * world_to_local[parent+1]; + local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , bod->getRVector(i))); + } + + + for (int i=0;igetNumLinks();i++) + { + + btVector3 posr = local_origin[i+1]; + float pos[4]={posr.x(),posr.y(),posr.z(),1}; + + float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; + + btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(bod,i); + + col->setCollisionShape(box); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + col->setWorldTransform(tr); + col->setFriction(1); + world->addCollisionObject(col,btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter);//,2,1); + + bod->addLinkCollider(col); + //app->drawBox(halfExtents, pos,quat); + } + + } + world->addMultiBody(bod); + } + + + + +} +void FeatherstoneMultiBodyDemo::clientResetScene() +{ + exitPhysics(); + initPhysics(); +} + + +void FeatherstoneMultiBodyDemo::exitPhysics() +{ + + //cleanup in the reverse order of creation/initialization + + //remove the rigidbodies from the dynamics world and delete them + int i; + for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--) + { + btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject( obj ); + delete obj; + } + + //delete collision shapes + for (int j=0;j m_collisionShapes; + + btBroadphaseInterface* m_broadphase; + + btCollisionDispatcher* m_dispatcher; + + btConstraintSolver* m_solver; + + btDefaultCollisionConfiguration* m_collisionConfiguration; + + public: + + FeatherstoneMultiBodyDemo() + { + } + virtual ~FeatherstoneMultiBodyDemo() + { + exitPhysics(); + } + void initPhysics(); + + void exitPhysics(); + + virtual void clientMoveAndDisplay(); + + virtual void displayCallback(); + virtual void clientResetScene(); + + static DemoApplication* Create() + { + FeatherstoneMultiBodyDemo* demo = new FeatherstoneMultiBodyDemo; + demo->myinit(); + demo->initPhysics(); + return demo; + } + + +}; + +#endif //FEATHERSTONE_MULTIBODY_DEMO_H + diff --git a/Demos/FeatherstoneMultiBodyDemo/Makefile.am b/Demos/FeatherstoneMultiBodyDemo/Makefile.am new file mode 100644 index 000000000..c5324dc9d --- /dev/null +++ b/Demos/FeatherstoneMultiBodyDemo/Makefile.am @@ -0,0 +1,5 @@ +noinst_PROGRAMS=FeatherstoneMultiBodyDemo + +FeatherstoneMultiBodyDemo_SOURCES=FeatherstoneMultiBodyDemo.cpp FeatherstoneMultiBodyDemo.h main.cpp +FeatherstoneMultiBodyDemo_CXXFLAGS=-I@top_builddir@/src -I@top_builddir@/Demos/OpenGL $(CXXFLAGS) +FeatherstoneMultiBodyDemo_LDADD=-L../OpenGL -lbulletopenglsupport -L../../src -lBulletDynamics -lBulletCollision -lLinearMath @opengl_LIBS@ diff --git a/Demos/FeatherstoneMultiBodyDemo/Win32FeatherstoneMultiBodyDemo.cpp b/Demos/FeatherstoneMultiBodyDemo/Win32FeatherstoneMultiBodyDemo.cpp new file mode 100644 index 000000000..0c78e3eb9 --- /dev/null +++ b/Demos/FeatherstoneMultiBodyDemo/Win32FeatherstoneMultiBodyDemo.cpp @@ -0,0 +1,28 @@ +#ifdef _WINDOWS +/* +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. +*/ + +///experimental support for Featherstone multi body (articulated hierarchies) + + +#include "FeatherstoneMultiBodyDemo.h" + +///The 'createDemo' function is called from Bullet/Demos/OpenGL/Win32AppMain.cpp to instantiate this particular demo +DemoApplication* createDemo() +{ + return new FeatherstoneMultiBodyDemo(); +} + +#endif diff --git a/Demos/FeatherstoneMultiBodyDemo/main.cpp b/Demos/FeatherstoneMultiBodyDemo/main.cpp new file mode 100644 index 000000000..8733a5f07 --- /dev/null +++ b/Demos/FeatherstoneMultiBodyDemo/main.cpp @@ -0,0 +1,42 @@ +/* +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. +*/ + +///experimental support for Featherstone multi body (articulated hierarchies) + + +#include "FeatherstoneMultiBodyDemo.h" +#include "GlutStuff.h" +#include "btBulletDynamicsCommon.h" +#include "LinearMath/btHashMap.h" + + + +int main(int argc,char** argv) +{ + + FeatherstoneMultiBodyDemo demo; + demo.initPhysics(); + + +#ifdef CHECK_MEMORY_LEAKS + ccdDemo.exitPhysics(); +#else + return glutmain(argc, argv,1024,600,"Bullet Physics Demo. http://bulletphysics.org",&demo); +#endif + + //default glut doesn't return from mainloop + return 0; +} + diff --git a/Demos/Makefile.am b/Demos/Makefile.am index cb7f84101..5091e00a7 100644 --- a/Demos/Makefile.am +++ b/Demos/Makefile.am @@ -1,5 +1,5 @@ if CONDITIONAL_BUILD_MULTITHREADED -SUBDIRS=OpenGL BasicDemo TerrainDemo VehicleDemo CcdPhysicsDemo MultiThreadedDemo SoftDemo AllBulletDemos +SUBDIRS=OpenGL BasicDemo FeatherstoneMultiBodyDemo TerrainDemo VehicleDemo CcdPhysicsDemo MultiThreadedDemo SoftDemo AllBulletDemos else -SUBDIRS=OpenGL BasicDemo TerrainDemo VehicleDemo CcdPhysicsDemo SoftDemo AllBulletDemos +SUBDIRS=OpenGL BasicDemo FeatherstoneMultiBodyDemo TerrainDemo VehicleDemo CcdPhysicsDemo SoftDemo AllBulletDemos endif diff --git a/Demos/MovingConcaveDemo/ConcavePhysicsDemo.cpp b/Demos/MovingConcaveDemo/ConcavePhysicsDemo.cpp index 68c91c774..0a6cec258 100644 --- a/Demos/MovingConcaveDemo/ConcavePhysicsDemo.cpp +++ b/Demos/MovingConcaveDemo/ConcavePhysicsDemo.cpp @@ -1906,12 +1906,10 @@ void ConcaveDemo::clientMoveAndDisplay() float dt = float(getDeltaTimeMicroseconds()) * 0.000001f; - extern int MyTTcound; + m_dynamicsWorld->stepSimulation(1./60.,0);//dt,0,1./60.); CProfileManager::dumpAll(); - printf("MyTTcound=%d\n",MyTTcound); - MyTTcound=0; - + //optional but useful: debug drawing m_dynamicsWorld->debugDrawWorld(); diff --git a/Demos/premake4.lua b/Demos/premake4.lua index afa6766b5..9a8bf80c3 100644 --- a/Demos/premake4.lua +++ b/Demos/premake4.lua @@ -60,6 +60,7 @@ end "DynamicControlDemo", "EPAPenDepthDemo", "ForkLiftDemo", + "FeatherstoneMultiBodyDemo", "FractureDemo", "GenericJointDemo", "GimpactTestDemo", diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 9bb4d12ab..89cad1682 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -142,7 +142,8 @@ public: CO_GHOST_OBJECT=4, CO_SOFT_BODY=8, CO_HF_FLUID=16, - CO_USER_TYPE=32 + CO_USER_TYPE=32, + CO_FEATHERSTONE_LINK=64 }; enum AnisotropicFrictionFlags diff --git a/src/BulletDynamics/CMakeLists.txt b/src/BulletDynamics/CMakeLists.txt index 100cd7a88..c329f7940 100644 --- a/src/BulletDynamics/CMakeLists.txt +++ b/src/BulletDynamics/CMakeLists.txt @@ -23,6 +23,9 @@ SET(BulletDynamics_SRCS Dynamics/Bullet-C-API.cpp Vehicle/btRaycastVehicle.cpp Vehicle/btWheelInfo.cpp + Featherstone/btMultiBody.cpp + Featherstone/btMultiBodyConstraintSolver.cpp + Featherstone/btMultiBodyDynamicsWorld.cpp ) SET(Root_HDRS @@ -62,6 +65,14 @@ SET(Vehicle_HDRS Vehicle/btWheelInfo.h ) +SET(Featherstone_HDRS + Featherstone/btMultiBody.h + Featherstone/btMultiBodyConstraintSolver.h + Featherstone/btMultiBodyDynamicsWorld.h + Featherstone/btMultiBodyLink.h + Featherstone/btMultiBodyLinkCollider.h + Featherstone/btMultiBodySolverConstraint.h +) SET(Character_HDRS Character/btCharacterControllerInterface.h Character/btKinematicCharacterController.h @@ -75,6 +86,7 @@ SET(BulletDynamics_HDRS ${Dynamics_HDRS} ${Vehicle_HDRS} ${Character_HDRS} + ${Featherstone_HDRS} ) @@ -108,7 +120,7 @@ DESTINATION ${INCLUDE_INSTALL_DIR}/BulletDynamics) SET_PROPERTY(SOURCE ${Dynamics_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Dynamics) SET_PROPERTY(SOURCE ${Vehicle_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Vehicle) SET_PROPERTY(SOURCE ${Character_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Character) - + SET_PROPERTY(SOURCE ${Featherstone_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Featherstone) ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) ENDIF (INSTALL_LIBS) diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 2b72f9f4f..aa8b87062 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -331,8 +331,7 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, -static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode); -static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode) +void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode) { @@ -923,6 +922,20 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m } } +void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal) +{ + int i; + btPersistentManifold* manifold = 0; +// btCollisionObject* colObj0=0,*colObj1=0; + + + for (i=0;i, 2011-2013 + * Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix) + + 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. + + */ + + +#include "btMultiBody.h" +#include "btMultiBodyLink.h" +#include "btMultiBodyLinkCollider.h" + +// #define INCLUDE_GYRO_TERM + +namespace { + const btScalar SLEEP_EPSILON = btScalar(0.01); // this is a squared velocity (m^2 s^-2) + const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds +} + + + + +// +// Various spatial helper functions +// + +namespace { + void SpatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame + const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates + const btVector3 &top_in, // top part of input vector + const btVector3 &bottom_in, // bottom part of input vector + btVector3 &top_out, // top part of output vector + btVector3 &bottom_out) // bottom part of output vector + { + top_out = rotation_matrix * top_in; + bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in; + } + + void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix, + const btVector3 &displacement, + const btVector3 &top_in, + const btVector3 &bottom_in, + btVector3 &top_out, + btVector3 &bottom_out) + { + top_out = rotation_matrix.transpose() * top_in; + bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in)); + } + + btScalar SpatialDotProduct(const btVector3 &a_top, + const btVector3 &a_bottom, + const btVector3 &b_top, + const btVector3 &b_bottom) + { + return a_bottom.dot(b_top) + a_top.dot(b_bottom); + } +} + + +// +// Implementation of class btMultiBody +// + +btMultiBody::btMultiBody(int n_links, + btScalar mass, + const btVector3 &inertia, + bool fixed_base_, + bool can_sleep_) + : num_links(n_links), + 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) +{ + links.resize(n_links); + + vector_buf.resize(2*n_links); + matrix_buf.resize(n_links + 1); + 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); +} + +btMultiBody::~btMultiBody() +{ +} + +void btMultiBody::setupPrismatic(int i, + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rot_parent_to_this, + const btVector3 &joint_axis, + const btVector3 &r_vector_when_q_zero) +{ + links[i].mass = mass; + links[i].inertia = inertia; + links[i].parent = parent; + links[i].zero_rot_parent_to_this = rot_parent_to_this; + links[i].axis_top.setValue(0,0,0); + links[i].axis_bottom = joint_axis; + links[i].e_vector = r_vector_when_q_zero; + links[i].is_revolute = false; + links[i].cached_rot_parent_to_this = rot_parent_to_this; + links[i].updateCache(); +} + +void btMultiBody::setupRevolute(int i, + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &zero_rot_parent_to_this, + const btVector3 &joint_axis, + const btVector3 &parent_axis_position, + const btVector3 &my_axis_position) +{ + links[i].mass = mass; + links[i].inertia = inertia; + links[i].parent = parent; + links[i].zero_rot_parent_to_this = zero_rot_parent_to_this; + links[i].axis_top = joint_axis; + links[i].axis_bottom = joint_axis.cross(my_axis_position); + links[i].d_vector = my_axis_position; + links[i].e_vector = parent_axis_position; + links[i].is_revolute = true; + 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 +{ + return links[i].parent; +} + +btScalar btMultiBody::getLinkMass(int i) const +{ + return links[i].mass; +} + +const btVector3 & btMultiBody::getLinkInertia(int i) const +{ + return links[i].inertia; +} + +btScalar btMultiBody::getJointPos(int i) const +{ + return links[i].joint_pos; +} + +btScalar btMultiBody::getJointVel(int i) const +{ + return real_buf[6 + i]; +} + +void btMultiBody::setJointPos(int i, btScalar q) +{ + links[i].joint_pos = q; + links[i].updateCache(); +} + +void btMultiBody::setJointVel(int i, btScalar qdot) +{ + real_buf[6 + i] = qdot; +} + +const btVector3 & btMultiBody::getRVector(int i) const +{ + return links[i].cached_r_vector; +} + +const btQuaternion & btMultiBody::getParentToLocalRot(int i) const +{ + return links[i].cached_rot_parent_to_this; +} + +btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const +{ + btVector3 result = local_pos; + while (i != -1) { + // 'result' is in frame i. transform it to frame parent(i) + result += getRVector(i); + result = quatRotate(getParentToLocalRot(i).inverse(),result); + i = getParent(i); + } + + // 'result' is now in the base frame. transform it to world frame + result = quatRotate(getWorldToBaseRot().inverse() ,result); + result += getBasePos(); + + return result; +} + +btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const +{ + if (i == -1) { + // world to base + return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos())); + } else { + // find position in parent frame, then transform to current frame + return quatRotate(getParentToLocalRot(i),worldPosToLocal(getParent(i), world_pos)) - getRVector(i); + } +} + +btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const +{ + btVector3 result = local_dir; + while (i != -1) { + result = quatRotate(getParentToLocalRot(i).inverse() , result); + i = getParent(i); + } + result = quatRotate(getWorldToBaseRot().inverse() , result); + return result; +} + +btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const +{ + if (i == -1) { + return quatRotate(getWorldToBaseRot(), world_dir); + } else { + return quatRotate(getParentToLocalRot(i) ,worldDirToLocal(getParent(i), world_dir)); + } +} + +void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const +{ + // 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()); + + for (int i = 0; i < num_links; ++i) { + const int parent = links[i].parent; + + // transform parent vel into this frame, store in omega[i+1], vel[i+1] + SpatialTransform(btMatrix3x3(links[i].cached_rot_parent_to_this), links[i].cached_r_vector, + omega[parent+1], vel[parent+1], + omega[i+1], vel[i+1]); + + // now add qidot * shat_i + omega[i+1] += getJointVel(i) * links[i].axis_top; + vel[i+1] += getJointVel(i) * links[i].axis_bottom; + } +} + +btScalar btMultiBody::getKineticEnergy() const +{ + // TODO: would be better not to allocate memory here + btAlignedObjectArray omega;omega.resize(num_links+1); + btAlignedObjectArray vel;vel.resize(num_links+1); + compTreeLinkVelocities(&omega[0], &vel[0]); + + // we will do the factor of 0.5 at the end + btScalar result = base_mass * vel[0].dot(vel[0]); + result += omega[0].dot(base_inertia * omega[0]); + + for (int i = 0; i < num_links; ++i) { + result += links[i].mass * vel[i+1].dot(vel[i+1]); + result += omega[i+1].dot(links[i].inertia * omega[i+1]); + } + + return 0.5f * result; +} + +btVector3 btMultiBody::getAngularMomentum() const +{ + // TODO: would be better not to allocate memory here + btAlignedObjectArray omega;omega.resize(num_links+1); + btAlignedObjectArray vel;vel.resize(num_links+1); + btAlignedObjectArray rot_from_world;rot_from_world.resize(num_links+1); + compTreeLinkVelocities(&omega[0], &vel[0]); + + rot_from_world[0] = base_quat; + btVector3 result = quatRotate(rot_from_world[0].inverse() , (base_inertia * omega[0])); + + for (int i = 0; i < num_links; ++i) { + rot_from_world[i+1] = links[i].cached_rot_parent_to_this * rot_from_world[links[i].parent+1]; + result += (quatRotate(rot_from_world[i+1].inverse() , (links[i].inertia * omega[i+1]))); + } + + return result; +} + + +void btMultiBody::clearForcesAndTorques() +{ + base_force.setValue(0, 0, 0); + base_torque.setValue(0, 0, 0); + + for (int i = 0; i < num_links; ++i) { + links[i].applied_force.setValue(0, 0, 0); + links[i].applied_torque.setValue(0, 0, 0); + links[i].joint_torque = 0; + } +} + +void btMultiBody::clearVelocities() +{ + for (int i = 0; i < 6 + num_links; ++i) + { + real_buf[i] = 0.f; + } +} +void btMultiBody::addLinkForce(int i, const btVector3 &f) +{ + links[i].applied_force += f; +} + +void btMultiBody::addLinkTorque(int i, const btVector3 &t) +{ + links[i].applied_torque += t; +} + +void btMultiBody::addJointTorque(int i, btScalar Q) +{ + links[i].joint_torque += Q; +} + +const btVector3 & btMultiBody::getLinkForce(int i) const +{ + return links[i].applied_force; +} + +const btVector3 & btMultiBody::getLinkTorque(int i) const +{ + return links[i].applied_torque; +} + +btScalar btMultiBody::getJointTorque(int i) const +{ + return links[i].joint_torque; +} + + +inline btMatrix3x3 vecMulVecTranspose(const btVector3& v0, const btVector3& v1Transposed) +{ + btVector3 row0 = btVector3( + v0.x() * v1Transposed.x(), + v0.x() * v1Transposed.y(), + v0.x() * v1Transposed.z()); + btVector3 row1 = btVector3( + v0.y() * v1Transposed.x(), + v0.y() * v1Transposed.y(), + v0.y() * v1Transposed.z()); + btVector3 row2 = btVector3( + v0.z() * v1Transposed.x(), + v0.z() * v1Transposed.y(), + v0.z() * v1Transposed.z()); + + btMatrix3x3 m(row0[0],row0[1],row0[2], + row1[0],row1[1],row1[2], + row2[0],row2[1],row2[2]); + return m; +} + + +void btMultiBody::stepVelocities(btScalar dt, + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v, + btAlignedObjectArray &scratch_m) +{ + // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot) + // and the base linear & angular accelerations. + + // We apply damping forces in this routine as well as any external forces specified by the + // caller (via addBaseForce etc). + + // output should point to an array of 6 + num_links reals. + // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame), + // num_links joint acceleration values. + + const btScalar DAMPING_K1 = btScalar(0.0); + //const btScalar DAMPING_K2 = btScalar(0); + const btScalar DAMPING_K2 = btScalar(0.0); + + btVector3 base_vel = getBaseVel(); + btVector3 base_omega = getBaseOmega(); + + // Temporary matrices/vectors -- use scratch space from caller + // so that we don't have to keep reallocating every frame + + scratch_r.resize(2*num_links + 6); + scratch_v.resize(8*num_links + 6); + scratch_m.resize(4*num_links + 4); + + btScalar * r_ptr = &scratch_r[0]; + btScalar * output = &scratch_r[num_links]; // "output" holds the q_double_dot results + 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; + + // 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; + + // 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; + + // top left, top right and bottom left blocks of Ihat_i^A. + // bottom right block = transpose of top left block and is not stored. + // Note: the top right and bottom left blocks are always symmetric matrices, but we don't make use of this fact currently. + btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1]; + btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2]; + btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3]; + + // Cached 3x3 rotation matrices from parent frame to this frame. + btMatrix3x3 * rot_from_parent = &matrix_buf[0]; + btMatrix3x3 * rot_from_world = &scratch_m[0]; + + // hhat_i, ahat_i + // hhat is NOT stored for the base (but ahat is) + btVector3 * h_top = num_links > 0 ? &vector_buf[0] : 0; + btVector3 * h_bottom = num_links > 0 ? &vector_buf[num_links] : 0; + btVector3 * accel_top = v_ptr; v_ptr += num_links + 1; + btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1; + + // Y_i, D_i + btScalar * Y = r_ptr; r_ptr += num_links; + btScalar * D = num_links > 0 ? &real_buf[6 + num_links] : 0; + + // ptr to the joint accel part of the output + btScalar * joint_accel = output + 6; + + + // Start of the algorithm proper. + + // First 'upward' loop. + // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. + + 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; + + if (fixed_base) { + zero_acc_top[0] = zero_acc_bottom[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 + - (rot_from_parent[0] * base_torque); + zero_acc_bottom[0] += base_inertia * vel_top[0] * (DAMPING_K1 + DAMPING_K2*vel_top[0].norm()); + } + inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero(); + + + inertia_top_right[0].setValue(base_mass, 0, 0, + 0, base_mass, 0, + 0, 0, base_mass); + inertia_bottom_left[0].setValue(base_inertia[0], 0, 0, + 0, base_inertia[1], 0, + 0, 0, base_inertia[2]); + + rot_from_world[0] = rot_from_parent[0]; + + for (int i = 0; i < num_links; ++i) { + const int parent = links[i].parent; + rot_from_parent[i+1] = btMatrix3x3(links[i].cached_rot_parent_to_this); + + + rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; + + // 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]); + + // 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); + 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); + } else { + coriolis_top[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; + + // 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_bottom[i+1] = +#ifdef INCLUDE_GYRO_TERM + vel_top[i+1].cross( links[i].inertia.cwise() * vel_top[i+1] ) +#endif + - (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()); + + // calculate Ihat_i^A + inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero(); + inertia_top_right[i+1].setValue(links[i].mass, 0, 0, + 0, links[i].mass, 0, + 0, 0, links[i].mass); + inertia_bottom_left[i+1].setValue(links[i].inertia[0], 0, 0, + 0, links[i].inertia[1], 0, + 0, 0, links[i].inertia[2]); + } + + + // 'Downward' loop. + // (part of TreeForwardDynamics in Mirtich.) + for (int i = num_links - 1; i >= 0; --i) { + + 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]); + + const int parent = links[i].parent; + + + // Ip += pXi * (Ii - hi hi' / Di) * iXp + const btScalar one_over_di = 1.0f / D[i]; + + + + + const btMatrix3x3 TL = inertia_top_left[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_bottom[i]); + const btMatrix3x3 TR = inertia_top_right[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_top[i]); + const btMatrix3x3 BL = inertia_bottom_left[i+1]- vecMulVecTranspose(one_over_di * h_bottom[i] , h_bottom[i]); + + + btMatrix3x3 r_cross; + r_cross.setValue( + 0, -links[i].cached_r_vector[2], links[i].cached_r_vector[1], + links[i].cached_r_vector[2], 0, -links[i].cached_r_vector[0], + -links[i].cached_r_vector[1], links[i].cached_r_vector[0], 0); + + inertia_top_left[parent+1] += rot_from_parent[i+1].transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1]; + inertia_top_right[parent+1] += rot_from_parent[i+1].transpose() * TR * rot_from_parent[i+1]; + inertia_bottom_left[parent+1] += rot_from_parent[i+1].transpose() * + (r_cross * (TL - TR * r_cross) + BL - TL.transpose() * r_cross) * rot_from_parent[i+1]; + + + // 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] + + 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] + + 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; + } + + + // Second 'upward' loop + // (part of TreeForwardDynamics in Mirtich) + + if (fixed_base) + { + accel_top[0] = accel_bottom[0] = btVector3(0,0,0); + } + else + { + if (num_links > 0) + { + //Matrix Imatrix; + //Imatrix.block<3,3>(0,0) = inertia_top_left[0]; + //Imatrix.block<3,3>(3,0) = inertia_bottom_left[0]; + //Imatrix.block<3,3>(0,3) = inertia_top_right[0]; + //Imatrix.block<3,3>(3,3) = inertia_top_left[0].transpose(); + //cached_imatrix_lu.reset(new Eigen::LU >(Imatrix)); // TODO: Avoid memory allocation here? + + cached_inertia_top_left = inertia_top_left[0]; + cached_inertia_top_right = inertia_top_right[0]; + cached_inertia_lower_left = inertia_bottom_left[0]; + 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]); + float result[6]; + + solveImatrix(rhs_top, rhs_bot, result); +// printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]); + for (int i = 0; i < 3; ++i) { + accel_top[0][i] = -result[i]; + accel_bottom[0][i] = -result[i+3]; + } + + } + + // now do the loop over the links + for (int i = 0; i < num_links; ++i) { + const int parent = links[i].parent; + SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector, + 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; + } + + // transform base accelerations back to the world frame. + btVector3 omegadot_out = rot_from_parent[0].transpose() * accel_top[0]; + output[0] = omegadot_out[0]; + output[1] = omegadot_out[1]; + output[2] = omegadot_out[2]; + + btVector3 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0]; + output[3] = vdot_out[0]; + output[4] = vdot_out[1]; + output[5] = vdot_out[2]; + // Final step: add the accelerations (times dt) to the velocities. + applyDeltaVee(output, dt); + + +} + + + +void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const +{ + ///solve I * x = rhs, so the result = invI * rhs + if (num_links == 0) + { + // in the case of 0 links (i.e. a plain rigid body, not a multibody) rhs * invI is easier + result[0] = rhs_bot[0] / base_inertia[0]; + result[1] = rhs_bot[1] / base_inertia[1]; + result[2] = rhs_bot[2] / base_inertia[2]; + result[3] = rhs_top[0] / base_mass; + result[4] = rhs_top[1] / base_mass; + result[5] = rhs_top[2] / base_mass; + } else + { + /// Special routine for calculating the inverse of a spatial inertia matrix + ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices + btMatrix3x3 Binv = cached_inertia_top_right.inverse()*-1.f; + btMatrix3x3 tmp = cached_inertia_lower_right * Binv; + btMatrix3x3 invIupper_right = (tmp * cached_inertia_top_left + cached_inertia_lower_left).inverse(); + tmp = invIupper_right * cached_inertia_lower_right; + btMatrix3x3 invI_upper_left = (tmp * Binv); + btMatrix3x3 invI_lower_right = (invI_upper_left).transpose(); + tmp = cached_inertia_top_left * invI_upper_left; + tmp[0][0]-= 1.0; + tmp[1][1]-= 1.0; + tmp[2][2]-= 1.0; + btMatrix3x3 invI_lower_left = (Binv * tmp); + + //multiply result = invI * rhs + { + btVector3 vtop = invI_upper_left*rhs_top; + btVector3 tmp; + tmp = invIupper_right * rhs_bot; + vtop += tmp; + btVector3 vbot = invI_lower_left*rhs_top; + tmp = invI_lower_right * rhs_bot; + vbot += tmp; + result[0] = vtop[0]; + result[1] = vtop[1]; + result[2] = vtop[2]; + result[3] = vbot[0]; + result[4] = vbot[1]; + result[5] = vbot[2]; + } + + } +} + + +void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output, + btAlignedObjectArray &scratch_r, btAlignedObjectArray &scratch_v) const +{ + // Temporary matrices/vectors -- use scratch space from caller + // so that we don't have to keep reallocating every frame + + scratch_r.resize(num_links); + scratch_v.resize(4*num_links + 4); + + btScalar * r_ptr = num_links == 0 ? 0 : &scratch_r[0]; + 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; + + // rot_from_parent (cached from calcAccelerations) + const btMatrix3x3 * rot_from_parent = &matrix_buf[0]; + + // hhat (cached), accel (scratch) + const btVector3 * h_top = num_links > 0 ? &vector_buf[0] : 0; + const btVector3 * h_bottom = num_links > 0 ? &vector_buf[num_links] : 0; + btVector3 * accel_top = v_ptr; v_ptr += num_links + 1; + btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1; + + // 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; + + btAssert(num_links == 0 || r_ptr - &scratch_r[0] == scratch_r.size()); + btAssert(v_ptr - &scratch_v[0] == scratch_v.size()); + + + + // First 'upward' loop. + // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. + + btVector3 input_force(force[3],force[4],force[5]); + btVector3 input_torque(force[0],force[1],force[2]); + + // Fill in zero_acc + // -- set to force/torque on the base, zero otherwise + if (fixed_base) + { + zero_acc_top[0] = zero_acc_bottom[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); + } + for (int i = 0; i < num_links; ++i) + { + zero_acc_top[i+1] = zero_acc_bottom[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] += force[6 + i]; // add joint torque + + const int parent = links[i].parent; + + // 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]; + 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; + } + + // ptr to the joint accel part of the output + btScalar * joint_accel = output + 6; + + // Second 'upward' loop + if (fixed_base) + { + 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]); + + float result[6]; + solveImatrix(rhs_top,rhs_bot, result); + // printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]); + + for (int i = 0; i < 3; ++i) { + accel_top[0][i] = -result[i]; + accel_bottom[0][i] = -result[i+3]; + } + + } + + // now do the loop over the links + for (int i = 0; i < num_links; ++i) { + const int parent = links[i].parent; + SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector, + 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] += joint_accel[i] * links[i].axis_top; + accel_bottom[i+1] += joint_accel[i] * links[i].axis_bottom; + } + + // transform base accelerations back to the world frame. + btVector3 omegadot_out; + omegadot_out = rot_from_parent[0].transpose() * accel_top[0]; + output[0] = omegadot_out[0]; + output[1] = omegadot_out[1]; + output[2] = omegadot_out[2]; + + btVector3 vdot_out; + vdot_out = rot_from_parent[0].transpose() * accel_bottom[0]; + + output[3] = vdot_out[0]; + output[4] = vdot_out[1]; + output[5] = vdot_out[2]; +} + +void btMultiBody::stepPositions(btScalar dt) +{ + // step position by adding dt * velocity + btVector3 v = getBaseVel(); + base_pos += dt * v; + + // "exponential map" method for the rotation + btVector3 base_omega = getBaseOmega(); + const btScalar omega_norm = base_omega.norm(); + const btScalar omega_times_dt = omega_norm * dt; + const btScalar SMALL_ROTATION_ANGLE = 0.02f; // Theoretically this should be ~ pow(FLT_EPSILON,0.25) which is ~ 0.0156 + if (fabs(omega_times_dt) < SMALL_ROTATION_ANGLE) + { + const btScalar xsq = omega_times_dt * omega_times_dt; // |omega|^2 * dt^2 + const btScalar sin_term = dt * (xsq / 48.0f - 0.5f); // -sin(0.5*dt*|omega|) / |omega| + const btScalar cos_term = 1.0f - xsq / 8.0f; // cos(0.5*dt*|omega|) + base_quat = base_quat * btQuaternion(sin_term * base_omega[0],sin_term * base_omega[1],sin_term * base_omega[2],cos_term); + } else + { + base_quat = base_quat * btQuaternion(base_omega / omega_norm,-omega_times_dt); + } + + // Make sure the quaternion represents a valid rotation. + // (Not strictly necessary, but helps prevent any round-off errors from building up.) + base_quat.normalize(); + + // Finally we can update joint_pos for each of the links + for (int i = 0; i < num_links; ++i) + { + float jointVel = getJointVel(i); + links[i].joint_pos += dt * jointVel; + links[i].updateCache(); + } +} + +void btMultiBody::fillContactJacobian(int link, + const btVector3 &contact_point, + const btVector3 &normal, + btScalar *jac, + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v, + btAlignedObjectArray &scratch_m) const +{ + // temporary space + scratch_v.resize(2*num_links + 2); + scratch_m.resize(num_links + 1); + + btVector3 * v_ptr = &scratch_v[0]; + btVector3 * p_minus_com = v_ptr; v_ptr += num_links + 1; + btVector3 * n_local = v_ptr; v_ptr += num_links + 1; + btAssert(v_ptr - &scratch_v[0] == scratch_v.size()); + + scratch_r.resize(num_links); + btScalar * results = num_links > 0 ? &scratch_r[0] : 0; + + btMatrix3x3 * rot_from_world = &scratch_m[0]; + + const btVector3 p_minus_com_world = contact_point - base_pos; + + rot_from_world[0] = btMatrix3x3(base_quat); + + p_minus_com[0] = rot_from_world[0] * p_minus_com_world; + n_local[0] = rot_from_world[0] * normal; + + // omega coeffients first. + btVector3 omega_coeffs; + omega_coeffs = p_minus_com_world.cross(normal); + jac[0] = omega_coeffs[0]; + jac[1] = omega_coeffs[1]; + jac[2] = omega_coeffs[2]; + // then v coefficients + jac[3] = normal[0]; + jac[4] = normal[1]; + jac[5] = normal[2]; + + // Set remaining jac values to zero for now. + for (int i = 6; i < 6 + num_links; ++i) { + jac[i] = 0; + } + + // Qdot coefficients, if necessary. + if (num_links > 0 && link > -1) { + + // TODO: speed this up -- don't calculate for links we don't need. + // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions, + // which is resulting in repeated work being done...) + + // calculate required normals & positions in the local frames. + for (int i = 0; i < num_links; ++i) { + + // transform to local frame + const int parent = links[i].parent; + const btMatrix3x3 mtx(links[i].cached_rot_parent_to_this); + rot_from_world[i+1] = mtx * rot_from_world[parent+1]; + + n_local[i+1] = mtx * n_local[parent+1]; + p_minus_com[i+1] = mtx * p_minus_com[parent+1] - links[i].cached_r_vector; + + // calculate the jacobian entry + if (links[i].is_revolute) { + results[i] = n_local[i+1].dot( links[i].axis_top.cross(p_minus_com[i+1]) + links[i].axis_bottom ); + } else { + results[i] = n_local[i+1].dot( links[i].axis_bottom ); + } + } + + // Now copy through to output. + while (link != -1) { + jac[6 + link] = results[link]; + link = links[link].parent; + } + } +} + +void btMultiBody::wakeUp() +{ + awake = true; +} + +void btMultiBody::goToSleep() +{ + awake = false; +} + +void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep) +{ + if (!can_sleep) 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]; + } + + if (motion < SLEEP_EPSILON) { + sleep_timer += timestep; + if (sleep_timer > SLEEP_TIMEOUT) { + goToSleep(); + } + } else { + sleep_timer = 0; + if (!awake) + wakeUp(); + } +} diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h new file mode 100644 index 000000000..98849fc10 --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -0,0 +1,353 @@ +/* + * PURPOSE: + * Class representing an articulated rigid body. Stores the body's + * current state, allows forces and torques to be set, handles + * timestepping and implements Featherstone's algorithm. + * + * COPYRIGHT: + * Copyright (C) Stephen Thompson, , 2011-2013 + * Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix) + + 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. + + */ + + +#ifndef BT_MULTIBODY_H +#define BT_MULTIBODY_H + +#include "LinearMath/btScalar.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btQuaternion.h" +#include "LinearMath/btMatrix3x3.h" +#include "LinearMath/btAlignedObjectArray.h" + + +#include "btMultibodyLink.h" +class btMultiBodyLinkCollider; + +class btMultiBody +{ +public: + + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + // + // initialization + // + + btMultiBody(int n_links, // NOT including the base + btScalar mass, // mass of base + const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal + bool fixed_base_, // whether the base is fixed (true) or can move (false) + bool can_sleep_); + + ~btMultiBody(); + + void setupPrismatic(int i, // 0 to num_links-1 + btScalar mass, + const btVector3 &inertia, // in my frame; assumed diagonal + int parent, + const btQuaternion &rot_parent_to_this, // rotate points in parent frame to my frame. + const btVector3 &joint_axis, // in my frame + const btVector3 &r_vector_when_q_zero); // vector from parent COM to my COM, in my frame, when q = 0. + + void setupRevolute(int i, // 0 to num_links-1 + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &zero_rot_parent_to_this, // rotate points in parent frame to this frame, when q = 0 + 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 + + 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 + // output: link num from 0 to num_links-1, OR -1 to mean the base. + // + int getParent(int link_num) const; + + + // + // get number of links, masses, moments of inertia + // + + int getNumLinks() const { return num_links; } + btScalar getBaseMass() const { return base_mass; } + const btVector3 & getBaseInertia() const { return base_inertia; } + btScalar getLinkMass(int i) const; + const btVector3 & getLinkInertia(int i) const; + + + // + // change mass (incomplete: can only change base mass and inertia at present) + // + + void setBaseMass(btScalar mass) { base_mass = mass; } + void setBaseInertia(const btVector3 &inertia) { base_inertia = inertia; } + + + // + // get/set pos/vel/rot/omega for the base link + // + + const btVector3 & getBasePos() const { return base_pos; } // in world frame + btVector3 getBaseVel() const { + return btVector3(real_buf[3],real_buf[4],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 + + void setBasePos(const btVector3 &pos) + { + base_pos = pos; + } + void setBaseVel(const btVector3 &vel) + { + real_buf[3]=vel[0]; real_buf[4]=vel[1]; 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]; } + + + // + // get/set pos/vel for child links (i = 0 to num_links-1) + // + + btScalar getJointPos(int i) const; + btScalar getJointVel(int i) const; + + void setJointPos(int i, btScalar q); + void setJointVel(int i, btScalar qdot); + + // + // direct access to velocities as a vector of 6 + num_links elements. + // (omega first, then v, then joint velocities.) + // + const btScalar * getVelocityVector() const + { + return &real_buf[0]; + } + btScalar * getVelocityVector() + { + return &real_buf[0]; + } + + + // + // get the frames of reference (positions and orientations) of the child links + // (i = 0 to num_links-1) + // + + const btVector3 & getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords + const btQuaternion & getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. + + + // + // transform vectors in local frame of link i to world frame (or vice versa) + // + btVector3 localPosToWorld(int i, const btVector3 &vec) const; + btVector3 localDirToWorld(int i, const btVector3 &vec) const; + btVector3 worldPosToLocal(int i, const btVector3 &vec) const; + btVector3 worldDirToLocal(int i, const btVector3 &vec) const; + + + // + // calculate kinetic energy and angular momentum + // useful for debugging. + // + + btScalar getKineticEnergy() const; + btVector3 getAngularMomentum() const; + + + // + // set external forces and torques. Note all external forces/torques are given in the WORLD frame. + // + + void clearForcesAndTorques(); + void clearVelocities(); + + void addBaseForce(const btVector3 &f) + { + base_force += f; + } + void addBaseTorque(const btVector3 &t) { base_torque += t; } + void addLinkForce(int i, const btVector3 &f); + void addLinkTorque(int i, const btVector3 &t); + void addJointTorque(int i, btScalar Q); + + const btVector3 & getBaseForce() const { return base_force; } + const btVector3 & getBaseTorque() const { return base_torque; } + const btVector3 & getLinkForce(int i) const; + const btVector3 & getLinkTorque(int i) const; + btScalar getJointTorque(int i) const; + + + // + // dynamics routines. + // + + // timestep the velocities (given the external forces/torques set using addBaseForce etc). + // also sets up caches for calcAccelerationDeltas. + // + // Note: the caller must provide three vectors which are used as + // temporary scratch space. The idea here is to reduce dynamic + // memory allocation: the same scratch vectors can be re-used + // again and again for different Multibodies, instead of each + // btMultiBody allocating (and then deallocating) their own + // individual scratch buffers. This gives a considerable speed + // improvement, at least on Windows (where dynamic memory + // allocation appears to be fairly slow). + // + void stepVelocities(btScalar dt, + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v, + btAlignedObjectArray &scratch_m); + + // calcAccelerationDeltas + // input: force vector (in same format as jacobian, i.e.: + // 3 torque values, 3 force values, num_links joint torque values) + // output: 3 omegadot values, 3 vdot values, num_links q_double_dot values + // (existing contents of output array are replaced) + // stepVelocities must have been called first. + void calcAccelerationDeltas(const btScalar *force, btScalar *output, + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v) const; + + // 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) + real_buf[i] += delta_vee[i]; + } + void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier) + { + for (int i = 0; i < 6 + num_links; ++i) + real_buf[i] += delta_vee[i] * multiplier; + } + + // timestep the positions (given current velocities). + void stepPositions(btScalar dt); + + + // + // contacts + // + + // This routine fills out a contact constraint jacobian for this body. + // the 'normal' supplied must be -n for body1 or +n for body2 of the contact. + // 'normal' & 'contact_point' are both given in world coordinates. + void fillContactJacobian(int link, + const btVector3 &contact_point, + const btVector3 &normal, + btScalar *jac, + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v, + btAlignedObjectArray &scratch_m) const; + + + // + // sleeping + // + + bool isAwake() const { return awake; } + void wakeUp(); + void goToSleep(); + void checkMotionAndSleepIfRequired(btScalar timestep); + + bool hasFixedBase() const + { + return fixed_base; + } + + int getCompanionId() const + { + return m_companionId; + } + void setCompanionId(int id) + { + m_companionId = id; + } +private: + btMultiBody(const btMultiBody &); // not implemented + void operator=(const btMultiBody &); // not implemented + + void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const; + + void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const; + + +private: + int num_links; // includes base. + + btVector3 base_pos; // position of COM of base (world frame) + btQuaternion base_quat; // rotates world points into base frame + + btScalar base_mass; // mass of the base + btVector3 base_inertia; // inertia of the base (in local frame; diagonal) + + 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 m_colliders; + + // + // real_buf: + // offset size array + // 0 6 + num_links v (base_omega; base_vel; joint_vels) + // 6+num_links num_links D + // + // vector_buf: + // offset size array + // 0 num_links h_top + // num_links num_links h_bottom + // + // matrix_buf: + // offset size array + // 0 num_links+1 rot_from_parent + // + + btAlignedObjectArray real_buf; + btAlignedObjectArray vector_buf; + btAlignedObjectArray matrix_buf; + + //std::auto_ptr > > cached_imatrix_lu; + + btMatrix3x3 cached_inertia_top_left; + btMatrix3x3 cached_inertia_top_right; + btMatrix3x3 cached_inertia_lower_left; + btMatrix3x3 cached_inertia_lower_right; + + bool fixed_base; + + // Sleep parameters. + bool awake; + bool can_sleep; + btScalar sleep_timer; + + int m_companionId; +}; + +#endif diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp new file mode 100644 index 000000000..a8c8b92c2 --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -0,0 +1,694 @@ +/* +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. +*/ + +#include "btMultiBodyConstraintSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "btMultiBodyLinkCollider.h" + +#include "BulletDynamics/ConstraintSolver/btSolverBody.h" + + + +btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +{ + btScalar val = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); + + //solve featherstone non-contact constraints + + for (int j=0;jm_multiBodyFrictionContactConstraints.size();j++) + { + if (iteration < infoGlobal.m_numIterations) + { + btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j]; + btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; + //adjust friction limits here + if (totalImpulse>btScalar(0)) + { + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; + resolveSingleConstraintRowGeneric(frictionConstraint); + } + } + } + return val; +} + +btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +{ + m_multiBodyNonContactConstraints.resize(0); + m_multiBodyNormalContactConstraints.resize(0); + m_multiBodyFrictionContactConstraints.resize(0); + m_jacobians.resize(0); + m_deltaVelocitiesUnitImpulse.resize(0); + m_deltaVelocities.resize(0); + + for (int i=0;im_multiBody->setCompanionId(-1); + } + } + + btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer); + + return val; +} + +void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) +{ + for (int i = 0; i < ndof; ++i) + m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; +} + +void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c) +{ + + btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; + btScalar deltaVelADotn=0; + btScalar deltaVelBDotn=0; + btSolverBody* bodyA = 0; + btSolverBody* bodyB = 0; + int ndofA=0; + int ndofB=0; + + if (c.m_multiBodyA) + { + ndofA = c.m_multiBodyA->getNumLinks() + 6; + for (int i = 0; i < ndofA; ++i) + deltaVelADotn += m_jacobians[c.m_jacAindex+i] * m_deltaVelocities[c.m_deltaVelAindex+i]; + } else + { + bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA]; + deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); + } + + if (c.m_multiBodyB) + { + ndofB = c.m_multiBodyB->getNumLinks() + 6; + for (int i = 0; i < ndofB; ++i) + deltaVelBDotn += m_jacobians[c.m_jacBindex+i] * m_deltaVelocities[c.m_deltaVelBindex+i]; + } else + { + bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB]; + deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); + } + + + deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom + deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv; + const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; + + if (sum < c.m_lowerLimit) + { + deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_lowerLimit; + } + else if (sum > c.m_upperLimit) + { + deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_upperLimit; + } + else + { + c.m_appliedImpulse = sum; + } + + if (c.m_multiBodyA) + { + applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); + c.m_multiBodyA->applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); + } else + { + bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + + } + if (c.m_multiBodyB) + { + applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); + c.m_multiBodyB->applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); + } else + { + bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + } + +} + + + +void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, + const btVector3& contactNormal, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) +{ + + btVector3 rel_pos1; + btVector3 rel_pos2; + + btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; + btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; + + const btVector3& pos1 = cp.getPositionWorldOnA(); + const btVector3& pos2 = cp.getPositionWorldOnB(); + + btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA]; + btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB]; + + btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; + btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; + +// btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); +// btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); + if (bodyA) + rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + if (bodyB) + rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); + + relaxation = 1.f; + + if (multiBodyA) + { + //solverConstraint.m_jacA = + const int ndofA = multiBodyA->getNumLinks() + 6; + + solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); + if (solverConstraint.m_deltaVelAindex <0) + { + solverConstraint.m_deltaVelAindex = m_deltaVelocities.size(); + multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); + m_deltaVelocities.resize(m_deltaVelocities.size()+ndofA); + } + + solverConstraint.m_jacAindex = m_jacobians.size(); + m_jacobians.resize(m_jacobians.size()+ndofA); + m_deltaVelocitiesUnitImpulse.resize(m_deltaVelocitiesUnitImpulse.size()+ndofA); + btAssert(m_jacobians.size() == m_deltaVelocitiesUnitImpulse.size()); + + float* jac1=&m_jacobians[solverConstraint.m_jacAindex]; + multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, scratch_r, scratch_v, scratch_m); + float* delta = &m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + multiBodyA->calcAccelerationDeltas(&m_jacobians[solverConstraint.m_jacAindex],delta,scratch_r, scratch_v); + } else + { + btVector3 torqueAxis0 = rel_pos1.cross(contactNormal); + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = contactNormal; + } + + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumLinks() + 6; + + solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); + if (solverConstraint.m_deltaVelBindex <0) + { + solverConstraint.m_deltaVelBindex = m_deltaVelocities.size(); + multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); + m_deltaVelocities.resize(m_deltaVelocities.size()+ndofB); + } + + solverConstraint.m_jacBindex = m_jacobians.size(); + + m_jacobians.resize(m_jacobians.size()+ndofB); + m_deltaVelocitiesUnitImpulse.resize(m_deltaVelocitiesUnitImpulse.size()+ndofB); + btAssert(m_jacobians.size() == m_deltaVelocitiesUnitImpulse.size()); + + multiBodyB->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_jacobians[solverConstraint.m_jacBindex], scratch_r, scratch_v, scratch_m); + multiBodyB->calcAccelerationDeltas(&m_jacobians[solverConstraint.m_jacBindex],&m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],scratch_r, scratch_v); + } else + { + btVector3 torqueAxis1 = rel_pos2.cross(contactNormal); + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + solverConstraint.m_contactNormal2 = -contactNormal; + } + + { + + btVector3 vec; + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + if (multiBodyA) + { + const int ndofA = multiBodyA->getNumLinks() + 6; + btScalar* jacA = &m_jacobians[solverConstraint.m_jacAindex]; + btScalar* lambdaA = &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() + contactNormal.dot(vec); + } + } + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumLinks() + 6; + btScalar* jacB = &m_jacobians[solverConstraint.m_jacBindex]; + btScalar* lambdaB = &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() + contactNormal.dot(vec); + } + } + + btScalar denom = relaxation/(denom0+denom1); + solverConstraint.m_jacDiagABInv = denom; + } + + + //compute rhs and remaining solverConstraint fields + + + + btScalar restitution = 0.f; + btScalar penetration = isFriction? 0 : cp.getDistance()+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 = &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 = &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 = cp.m_combinedFriction; + + + restitution = 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 = &m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + multiBodyA->applyDeltaVee(deltaV,impulse); + applyDeltaVee(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 = &m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + multiBodyB->applyDeltaVee(deltaV,impulse); + applyDeltaVee(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; + } + + // const btScalar ALLOWED_PENETRATION = btScalar(0.01); + +// float baumgarte_coeff = 0.3; + /// float one_over_dt = 1.f/infoGlobal.m_timeStep; + // btScalar minus_vnew = -penetration * baumgarte_coeff * one_over_dt; + // float myrhs = minus_vnew*solverConstraint.m_jacDiagABInv;//?? + + // solverConstraint.m_rhs = minus_vnew*solverConstraint.m_jacDiagABInv;//?? + //solverConstraint.m_rhsPenetration = 0.f; + + //penetration=0.f; +#if 1 + 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_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv; + //solverConstraint.m_rhs = velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv; + + solverConstraint.m_rhsPenetration = 0.f; + + } else + { + //split position and velocity into rhs and m_rhsPenetration + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = penetrationImpulse; + } +#endif + + solverConstraint.m_cfm = 0.f; + solverConstraint.m_lowerLimit = 0; + solverConstraint.m_upperLimit = 1e10f; + } + +} + +btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +{ + btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing(); + solverConstraint.m_frictionIndex = frictionIndex; + bool isFriction = true; + + const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); + const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + + btMultiBody* mbA = fcA? fcA->m_multiBody : 0; + btMultiBody* mbB = fcB? fcB->m_multiBody : 0; + + int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0); + int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1); + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_multiBodyA = mbA; + if (mbA) + solverConstraint.m_linkA = fcA->m_link; + + solverConstraint.m_multiBodyB = mbB; + if (mbB) + solverConstraint.m_linkB = fcB->m_link; + + solverConstraint.m_originalContactPoint = &cp; + + setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip); + return solverConstraint; +} + +void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) +{ + const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); + const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + + btMultiBody* mbA = fcA? fcA->m_multiBody : 0; + btMultiBody* mbB = fcB? fcB->m_multiBody : 0; + + btCollisionObject* colObj0=0,*colObj1=0; + + colObj0 = (btCollisionObject*)manifold->getBody0(); + colObj1 = (btCollisionObject*)manifold->getBody1(); + + int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0); + int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1); + + btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB]; + + + ///avoid collision response between two static objects +// if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero()))) + // return; + + int rollingFriction=1; + + for (int j=0;jgetNumContacts();j++) + { + + btManifoldPoint& cp = manifold->getContactPoint(j); + + if (cp.getDistance() <= manifold->getContactProcessingThreshold()) + { + + btScalar relaxation; + + int frictionIndex = m_multiBodyNormalContactConstraints.size(); + + btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing(); + + btRigidBody* rb0 = btRigidBody::upcast(colObj0); + btRigidBody* rb1 = btRigidBody::upcast(colObj1); + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_multiBodyA = mbA; + if (mbA) + solverConstraint.m_linkA = fcA->m_link; + + solverConstraint.m_multiBodyB = mbB; + if (mbB) + solverConstraint.m_linkB = fcB->m_link; + + solverConstraint.m_originalContactPoint = &cp; + + bool isFriction = false; + setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction); + +// const btVector3& pos1 = cp.getPositionWorldOnA(); +// const btVector3& pos2 = cp.getPositionWorldOnB(); + + /////setup the friction constraints +#define ENABLE_FRICTION +#ifdef ENABLE_FRICTION + solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); +#if ROLLING_FRICTION + btVector3 angVelA(0,0,0),angVelB(0,0,0); + if (rb0) + angVelA = rb0->getAngularVelocity(); + if (rb1) + angVelB = rb1->getAngularVelocity(); + btVector3 relAngVel = angVelB-angVelA; + + if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) + { + //only a single rollingFriction per manifold + rollingFriction--; + if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold) + { + relAngVel.normalize(); + applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + if (relAngVel.length()>0.001) + addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + + } else + { + addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + btVector3 axis0,axis1; + btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1); + applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + if (axis0.length()>0.001) + addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + if (axis1.length()>0.001) + addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + + } + } +#endif //ROLLING_FRICTION + + ///Bullet has several options to set the friction directions + ///By default, each contact has only a single friction direction that is recomputed automatically very frame + ///based on the relative linear velocity. + ///If the relative velocity it zero, it will automatically compute a friction direction. + + ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS. + ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction. + /// + ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity. + /// + ///The user can manually override the friction directions for certain contacts using a contact callback, + ///and set the cp.m_lateralFrictionInitialized to true + ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) + ///this will give a conveyor belt effect + /// + if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) + {/* + cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; + btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); + if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) + { + cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel); + if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); + cp.m_lateralFrictionDir2.normalize();//?? + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + + } + + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + + } else + */ + { + btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); + } + + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); + + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) + { + cp.m_lateralFrictionInitialized = true; + } + } + + } else + { + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1); + + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2); + + //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); + //todo: + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedPushImpulse = 0.f; + } + + +#endif //ENABLE_FRICTION + + } + } +} + +void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal) +{ + btPersistentManifold* manifold = 0; + + for (int i=0;igetBody0()); + const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + if (!fcA && !fcB) + { + //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case + convertContact(manifold,infoGlobal); + } else + { + convertMultiBodyContact(manifold,infoGlobal); + } + } +} + + + +btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) +{ + return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher); +} \ No newline at end of file diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h new file mode 100644 index 000000000..bddf55b4f --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -0,0 +1,79 @@ +/* +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. +*/ + +#ifndef BT_MULTIBODY_CONSTRAINT_SOLVER_H +#define BT_MULTIBODY_CONSTRAINT_SOLVER_H + +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" +#include "btMultiBodySolverConstraint.h" + + +class btMultiBody; + + +ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver +{ + +protected: + + btMultiBodyConstraintArray m_multiBodyNonContactConstraints; + + btMultiBodyConstraintArray m_multiBodyNormalContactConstraints; + btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints; + + + btAlignedObjectArray m_jacobians; + btAlignedObjectArray m_deltaVelocitiesUnitImpulse; + btAlignedObjectArray m_deltaVelocities; + + + btAlignedObjectArray scratch_r; + btAlignedObjectArray scratch_v; + btAlignedObjectArray scratch_m; + +// virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); +// virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal); + + void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c); + void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal); + btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0); + + + void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, + const btVector3& contactNormal, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); + + void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); + virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); +// virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + + virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + void applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof); + +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); +}; + + + + + +#endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H + diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp new file mode 100644 index 000000000..bdeff61a1 --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -0,0 +1,265 @@ +/* +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. +*/ + +#include "btMultiBodyDynamicsWorld.h" +#include "btMultiBodyConstraintSolver.h" +#include "btMultiBody.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" +#include "LinearMath/btQuickprof.h" + +btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) + :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration) +{ + //split impulse is not yet supported for Featherstone hierarchies + getSolverInfo().m_splitImpulse = false; + getSolverInfo().m_solverMode |=SOLVER_USE_2_FRICTION_DIRECTIONS; +} + +btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld () +{ + +} + + +void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask) +{ + m_multiBodies.push_back(body); + +} + +void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body) +{ + m_multiBodies.remove(body); +} + +void btMultiBodyDynamicsWorld::calculateSimulationIslands() +{ + BT_PROFILE("calculateSimulationIslands"); + + getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); + + { + //merge islands based on speculative contact manifolds too + for (int i=0;im_predictiveManifolds.size();i++) + { + btPersistentManifold* manifold = m_predictiveManifolds[i]; + + const btCollisionObject* colObj0 = manifold->getBody0(); + const btCollisionObject* colObj1 = manifold->getBody1(); + + if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && + ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) + { + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); + } + } + } + + { + int i; + int numConstraints = int(m_constraints.size()); + for (i=0;i< numConstraints ; i++ ) + { + btTypedConstraint* constraint = m_constraints[i]; + if (constraint->isEnabled()) + { + const btRigidBody* colObj0 = &constraint->getRigidBodyA(); + const btRigidBody* colObj1 = &constraint->getRigidBodyB(); + + if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && + ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) + { + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); + } + } + } + } + + //merge islands linked by Featherstone links + for (int i=0;igetNumLinkColliders()>1) + { + btMultiBodyLinkCollider* prev = body->getLinkCollider(0); + for (int b=1;bgetNumLinkColliders();b++) + { + btMultiBodyLinkCollider* cur = body->getLinkCollider(b); + + if (((cur) && (!(cur)->isStaticOrKinematicObject())) && + ((prev) && (!(prev)->isStaticOrKinematicObject()))) + { + int tagPrev = prev->getIslandTag(); + int tagCur = cur->getIslandTag(); + getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur); + } + prev = cur; + + } + } + } + + //Store the island id in each body + getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); + +} + + +void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) +{ + BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState"); + + + + for ( int i=0;icheckMotionAndSleepIfRequired(timeStep); + if (!body->isAwake()) + { + for (int b=0;bgetNumLinkColliders();b++) + { + btMultiBodyLinkCollider* col = body->getLinkCollider(b); + if (col->getActivationState() == ACTIVE_TAG) + { + col->setActivationState( WANTS_DEACTIVATION); + col->setDeactivationTime(0.f); + } + } + } else + { + for (int b=0;bgetNumLinkColliders();b++) + { + btMultiBodyLinkCollider* col = body->getLinkCollider(b); + if (col->getActivationState() != DISABLE_DEACTIVATION) + col->setActivationState( ACTIVE_TAG ); + } + } + + } + } + + btDiscreteDynamicsWorld::updateActivationState(timeStep); +} +void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) +{ + btVector3 g = m_gravity; + btAlignedObjectArray scratch_r; + btAlignedObjectArray scratch_v; + btAlignedObjectArray scratch_m; + + + { + BT_PROFILE("btMultiBody addForce and stepVelocities"); + for (int i=0;im_multiBodies.size();i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + if (bod->getNumLinkColliders() && bod->getLinkCollider(0)->getActivationState()==ISLAND_SLEEPING) + { + isSleeping = true; + } + if (!isSleeping) + { + scratch_r.resize(bod->getNumLinks()+1); + scratch_v.resize(bod->getNumLinks()+1); + scratch_m.resize(bod->getNumLinks()+1); + + bod->clearForcesAndTorques(); + bod->addBaseForce(g * bod->getBaseMass()); + + for (int j = 0; j < bod->getNumLinks(); ++j) + { + bod->addLinkForce(j, g * bod->getLinkMass(j)); + } + + bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); + } + } + } + btDiscreteDynamicsWorld::solveConstraints(solverInfo); +} + +void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) +{ + btDiscreteDynamicsWorld::integrateTransforms(timeStep); + + { + BT_PROFILE("btMultiBody stepPositions"); + //integrate and update the Featherstone hierarchies + btAlignedObjectArray world_to_local; + btAlignedObjectArray local_origin; + + for (int b=0;bgetNumLinkColliders() && bod->getLinkCollider(0)->getActivationState()==ISLAND_SLEEPING) + { + isSleeping = true; + } + if (!isSleeping) + { + int nLinks = bod->getNumLinks(); + + ///base + num links + world_to_local.resize(nLinks+1); + local_origin.resize(nLinks+1); + + bod->stepPositions(timeStep); + + + + world_to_local[0] = bod->getWorldToBaseRot(); + local_origin[0] = bod->getBasePos(); + + for (int k=0;kgetNumLinks();k++) + { + const int parent = bod->getParent(k); + world_to_local[k+1] = bod->getParentToLocalRot(k) * world_to_local[parent+1]; + local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , bod->getRVector(k))); + } + + for (int m=0;mgetNumLinkColliders();m++) + { + btMultiBodyLinkCollider* col = bod->getLinkCollider(m); + int link = col->m_link; + int index = link+1; + + 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); + + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + + col->setWorldTransform(tr); + } + } else + { + bod->clearVelocities(); + } + } + } +} diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h new file mode 100644 index 000000000..375f6c94c --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -0,0 +1,48 @@ +/* +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. +*/ + +#ifndef BT_MULTIBODY_DYNAMICS_WORLD_H +#define BT_MULTIBODY_DYNAMICS_WORLD_H + +#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" + + +class btMultiBody; +class btMultiBodyConstraintSolver; + +///The btMultiBodyDynamicsWorld adds Featherstone multi body dynamics to Bullet +///This implementation is still preliminary/experimental. +class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld +{ +protected: + btAlignedObjectArray m_multiBodies; + + virtual void calculateSimulationIslands(); + virtual void updateActivationState(btScalar timeStep); + virtual void solveConstraints(btContactSolverInfo& solverInfo); + virtual void integrateTransforms(btScalar timeStep); +public: + + btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); + + virtual ~btMultiBodyDynamicsWorld (); + + virtual void addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter); + + virtual void removeMultiBody(btMultiBody* body); + + +}; +#endif //BT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h new file mode 100644 index 000000000..ccf9a7fcb --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -0,0 +1,102 @@ +/* +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. +*/ + +#ifndef BT_MULTIBODY_LINK_H +#define BT_MULTIBODY_LINK_H + +#include "LinearMath/btQuaternion.h" +#include "LinearMath/btVector3.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + + +// +// Link struct +// + +struct btMultibodyLink +{ + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btScalar joint_pos; // qi + + btScalar mass; // mass of link + btVector3 inertia; // inertia of link (local frame; diagonal) + + int parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link. + + btQuaternion zero_rot_parent_to_this; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant. + + // "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant. + // for prismatic: axis_top = zero; + // axis_bottom = unit vector along the joint axis. + // for revolute: axis_top = unit vector along the rotation axis (u); + // axis_bottom = u cross d_vector. + btVector3 axis_top; + btVector3 axis_bottom; + + btVector3 d_vector; // vector from the inboard joint pos to this link's COM. (local frame.) constant. set for revolute joints only. + + // e_vector is constant, but depends on the joint type + // prismatic: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.) + // revolute: vector from parent's COM to the pivot point, in PARENT's frame. + btVector3 e_vector; + + bool is_revolute; // true = revolute, false = prismatic + + btQuaternion cached_rot_parent_to_this; // rotates vectors in parent frame to vectors in local frame + btVector3 cached_r_vector; // vector from COM of parent to COM of this link, in local frame. + + btVector3 applied_force; // In WORLD frame + btVector3 applied_torque; // In WORLD frame + btScalar joint_torque; + + // ctor: set some sensible defaults + btMultibodyLink() + : joint_pos(0), + mass(1), + parent(-1), + zero_rot_parent_to_this(1, 0, 0, 0), + is_revolute(false), + cached_rot_parent_to_this(1, 0, 0, 0), + joint_torque(0) + { + inertia.setValue(1, 1, 1); + axis_top.setValue(0, 0, 0); + axis_bottom.setValue(1, 0, 0); + d_vector.setValue(0, 0, 0); + e_vector.setValue(0, 0, 0); + cached_r_vector.setValue(0, 0, 0); + applied_force.setValue( 0, 0, 0); + applied_torque.setValue(0, 0, 0); + } + + // routine to update cached_rot_parent_to_this and cached_r_vector + void updateCache() + { + if (is_revolute) + { + cached_rot_parent_to_this = btQuaternion(axis_top,-joint_pos) * zero_rot_parent_to_this; + cached_r_vector = d_vector + quatRotate(cached_rot_parent_to_this,e_vector); + } else + { + // cached_rot_parent_to_this never changes, so no need to update + cached_r_vector = e_vector + joint_pos * axis_bottom; + } + } +}; + + +#endif //BT_MULTIBODY_LINK_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h new file mode 100644 index 000000000..4e00a7316 --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h @@ -0,0 +1,62 @@ +/* +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. +*/ + +#ifndef BT_FEATHERSTONE_LINK_COLLIDER_H +#define BT_FEATHERSTONE_LINK_COLLIDER_H + +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + +#include "btMultiBody.h" + +class btMultiBodyLinkCollider : public btCollisionObject +{ +//protected: +public: + + btMultiBody* m_multiBody; + int m_link; + + + btMultiBodyLinkCollider (btMultiBody* multiBody,int link) + :m_multiBody(multiBody), + m_link(link) + { + if (link>=0 || (multiBody && !multiBody->hasFixedBase())) + { + m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT); + } else + { + m_collisionFlags |= (btCollisionObject::CF_STATIC_OBJECT); + } + + m_internalType = CO_FEATHERSTONE_LINK; + } + static btMultiBodyLinkCollider* upcast(btCollisionObject* colObj) + { + if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK) + return (btMultiBodyLinkCollider*)colObj; + return 0; + } + static const btMultiBodyLinkCollider* upcast(const btCollisionObject* colObj) + { + if (colObj->getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK) + return (btMultiBodyLinkCollider*)colObj; + return 0; + } + +}; + +#endif //BT_FEATHERSTONE_LINK_COLLIDER_H + diff --git a/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h new file mode 100644 index 000000000..38531d68d --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h @@ -0,0 +1,80 @@ +/* +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. +*/ + +#ifndef BT_MULTIBODY_SOLVER_CONSTRAINT_H +#define BT_MULTIBODY_SOLVER_CONSTRAINT_H + +#include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedObjectArray.h" + +class btMultiBody; + +///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. +ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint +{ + BT_DECLARE_ALIGNED_ALLOCATOR(); + + + int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1 + btVector3 m_relpos1CrossNormal; + btVector3 m_contactNormal1; + int m_jacAindex; + + int m_deltaVelBindex; + btVector3 m_relpos2CrossNormal; + btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always + int m_jacBindex; + + btVector3 m_angularComponentA; + btVector3 m_angularComponentB; + + mutable btSimdScalar m_appliedPushImpulse; + mutable btSimdScalar m_appliedImpulse; + + btScalar m_friction; + btScalar m_jacDiagABInv; + btScalar m_rhs; + btScalar m_cfm; + + btScalar m_lowerLimit; + btScalar m_upperLimit; + btScalar m_rhsPenetration; + union + { + void* m_originalContactPoint; + btScalar m_unusedPadding4; + }; + + int m_overrideNumSolverIterations; + int m_frictionIndex; + + int m_solverBodyIdA; + btMultiBody* m_multiBodyA; + int m_linkA; + + int m_solverBodyIdB; + btMultiBody* m_multiBodyB; + int m_linkB; + + enum btSolverConstraintType + { + BT_SOLVER_CONTACT_1D = 0, + BT_SOLVER_FRICTION_1D + }; +}; + +typedef btAlignedObjectArray btMultiBodyConstraintArray; + +#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_H diff --git a/src/LinearMath/btVector3.h b/src/LinearMath/btVector3.h index 8ad7dd09e..90e7f0de6 100644 --- a/src/LinearMath/btVector3.h +++ b/src/LinearMath/btVector3.h @@ -264,6 +264,12 @@ public: return btSqrt(length2()); } + /**@brief Return the norm (length) of the vector */ + SIMD_FORCE_INLINE btScalar norm() const + { + return length(); + } + /**@brief Return the distance squared between the ends of this and another vector * This is symantically treating the vector like a point */ SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const; diff --git a/src/Makefile.am b/src/Makefile.am index a5d987a25..99d3ef09a 100644 --- a/src/Makefile.am +++ b/src/Makefile.am @@ -360,7 +360,16 @@ libBulletDynamics_la_SOURCES = \ BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h \ BulletDynamics/Vehicle/btVehicleRaycaster.h \ BulletDynamics/Vehicle/btRaycastVehicle.h \ - BulletDynamics/Vehicle/btWheelInfo.h + BulletDynamics/Vehicle/btWheelInfo.h \ + BulletDynamics/Featherstone/btMultiBody.cpp + BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp \ + BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp \ + BulletDynamics/Featherstone/btMultiBody.h \ + BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h \ + BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h \ + BulletDynamics/Featherstone/btMultiBodyLink.h \ + BulletDynamics/Featherstone/btMultiBodyLinkCollider.h \ + BulletDynamics/Featherstone/btMultiBodySolverConstraint.h libBulletSoftBody_la_SOURCES = \ BulletSoftBody/btDefaultSoftBodySolver.cpp \ @@ -421,6 +430,12 @@ nobase_bullet_include_HEADERS += \ BulletDynamics/ConstraintSolver/btSolverBody.h \ BulletDynamics/Character/btCharacterControllerInterface.h \ BulletDynamics/Character/btKinematicCharacterController.h \ + BulletDynamics/Featherstone/btMultiBody.h \ + BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h \ + BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h \ + BulletDynamics/Featherstone/btMultiBodyLink.h \ + BulletDynamics/Featherstone/btMultiBodyLinkCollider.h \ + BulletDynamics/Featherstone/btMultiBodySolverConstraint.h \ BulletCollision/CollisionShapes/btShapeHull.h \ BulletCollision/CollisionShapes/btConcaveShape.h \ BulletCollision/CollisionShapes/btCollisionMargin.h \