Added a first version of a Featherstone multi body implementation.
The original version was written by Stephen Thompson. I replaced Eigen math by Bullet LinearMath, and added a dedicated 6x6 matrix solver. Also I integrated support for collisions/contact constraints between btMultiBody and btRigidBody, and de-activation support. See Demos/FeatherstoneMultiBodyDemo/Win32FeatherstoneMultiBodyDemo.cpp for example usage. There is currently only support for contact constraints for btMultiBody. Next on the list will be adding support for joint limit constraint for btMultiBody. The implementation is still experimental/untested, the quality will improve in upcoming Bullet releases.
This commit is contained in:
erwin.coumans
@@ -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
|
||||
|
||||
87
Demos/FeatherstoneMultiBodyDemo/CMakeLists.txt
Normal file
87
Demos/FeatherstoneMultiBodyDemo/CMakeLists.txt
Normal file
@@ -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)
|
||||
398
Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp
Normal file
398
Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp
Normal file
@@ -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 <stdio.h> //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;k<ARRAY_SIZE_Y;k++)
|
||||
{
|
||||
for (int i=0;i<ARRAY_SIZE_X;i++)
|
||||
{
|
||||
for(int j = 0;j<ARRAY_SIZE_Z;j++)
|
||||
{
|
||||
startTransform.setOrigin(SCALING*btVector3(
|
||||
btScalar(3.0*i + start_x),
|
||||
btScalar(3.0*k + start_y),
|
||||
btScalar(3.0*j + start_z)));
|
||||
|
||||
|
||||
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
|
||||
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
|
||||
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
|
||||
btRigidBody* body = new btRigidBody(rbInfo);
|
||||
|
||||
|
||||
m_dynamicsWorld->addRigidBody(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;i<n_links;i++)
|
||||
{
|
||||
float initial_joint_angle=0.3;
|
||||
if (i>0)
|
||||
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<btQuaternion> world_to_local;
|
||||
world_to_local.resize(n_links+1);
|
||||
|
||||
btAlignedObjectArray<btVector3> 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;i<bod->getNumLinks();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;i<bod->getNumLinks();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.size();j++)
|
||||
{
|
||||
btCollisionShape* shape = m_collisionShapes[j];
|
||||
delete shape;
|
||||
}
|
||||
m_collisionShapes.clear();
|
||||
|
||||
delete m_dynamicsWorld;
|
||||
|
||||
delete m_solver;
|
||||
|
||||
delete m_broadphase;
|
||||
|
||||
delete m_dispatcher;
|
||||
|
||||
delete m_collisionConfiguration;
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
85
Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.h
Normal file
85
Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.h
Normal file
@@ -0,0 +1,85 @@
|
||||
/*
|
||||
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)
|
||||
|
||||
#ifndef FEATHERSTONE_MULTIBODY_DEMO_H
|
||||
#define FEATHERSTONE_MULTIBODY_DEMO_H
|
||||
|
||||
#ifdef _WINDOWS
|
||||
#include "Win32DemoApplication.h"
|
||||
#define PlatformDemoApplication Win32DemoApplication
|
||||
#else
|
||||
#include "GlutDemoApplication.h"
|
||||
#define PlatformDemoApplication GlutDemoApplication
|
||||
#endif
|
||||
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
|
||||
class btBroadphaseInterface;
|
||||
class btCollisionShape;
|
||||
class btOverlappingPairCache;
|
||||
class btCollisionDispatcher;
|
||||
class btConstraintSolver;
|
||||
struct btCollisionAlgorithmCreateFunc;
|
||||
class btDefaultCollisionConfiguration;
|
||||
|
||||
///FeatherstoneMultiBodyDemo is good starting point for learning the code base and porting.
|
||||
|
||||
class FeatherstoneMultiBodyDemo : public PlatformDemoApplication
|
||||
{
|
||||
|
||||
//keep the collision shapes, for deletion/cleanup
|
||||
btAlignedObjectArray<btCollisionShape*> 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
|
||||
|
||||
5
Demos/FeatherstoneMultiBodyDemo/Makefile.am
Normal file
5
Demos/FeatherstoneMultiBodyDemo/Makefile.am
Normal file
@@ -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@
|
||||
@@ -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
|
||||
42
Demos/FeatherstoneMultiBodyDemo/main.cpp
Normal file
42
Demos/FeatherstoneMultiBodyDemo/main.cpp
Normal file
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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();
|
||||
|
||||
|
||||
@@ -60,6 +60,7 @@ end
|
||||
"DynamicControlDemo",
|
||||
"EPAPenDepthDemo",
|
||||
"ForkLiftDemo",
|
||||
"FeatherstoneMultiBodyDemo",
|
||||
"FractureDemo",
|
||||
"GenericJointDemo",
|
||||
"GimpactTestDemo",
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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<numManifolds;i++)
|
||||
{
|
||||
manifold = manifoldPtr[i];
|
||||
convertContact(manifold,infoGlobal);
|
||||
}
|
||||
}
|
||||
|
||||
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
|
||||
{
|
||||
BT_PROFILE("solveGroupCacheFriendlySetup");
|
||||
@@ -1227,18 +1240,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
int i;
|
||||
btPersistentManifold* manifold = 0;
|
||||
// btCollisionObject* colObj0=0,*colObj1=0;
|
||||
convertContacts(manifoldPtr,numManifolds,infoGlobal);
|
||||
|
||||
|
||||
for (i=0;i<numManifolds;i++)
|
||||
{
|
||||
manifold = manifoldPtr[i];
|
||||
convertContact(manifold,infoGlobal);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// btContactSolverInfo info = infoGlobal;
|
||||
@@ -1397,7 +1400,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
|
||||
for (j=0;j<numPoolConstraints;j++)
|
||||
{
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
|
||||
resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
//resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
|
||||
}
|
||||
|
||||
@@ -1416,7 +1420,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
|
||||
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
|
||||
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
|
||||
|
||||
resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
//resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -56,11 +56,13 @@ protected:
|
||||
btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
|
||||
btSolverConstraint& addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f);
|
||||
|
||||
|
||||
|
||||
void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp,
|
||||
const btContactSolverInfo& infoGlobal, btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
|
||||
btVector3& rel_pos1, btVector3& rel_pos2);
|
||||
|
||||
static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode);
|
||||
|
||||
void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB,
|
||||
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
@@ -70,6 +72,8 @@ protected:
|
||||
|
||||
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
|
||||
|
||||
virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
|
||||
|
||||
|
||||
@@ -98,7 +102,7 @@ protected:
|
||||
|
||||
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);
|
||||
btScalar solveSingleIteration(int iteration, 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);
|
||||
|
||||
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);
|
||||
|
||||
@@ -74,7 +74,7 @@ protected:
|
||||
|
||||
virtual void solveConstraints(btContactSolverInfo& solverInfo);
|
||||
|
||||
void updateActivationState(btScalar timeStep);
|
||||
virtual void updateActivationState(btScalar timeStep);
|
||||
|
||||
void updateActions(btScalar timeStep);
|
||||
|
||||
|
||||
988
src/BulletDynamics/Featherstone/btMultiBody.cpp
Normal file
988
src/BulletDynamics/Featherstone/btMultiBody.cpp
Normal file
@@ -0,0 +1,988 @@
|
||||
/*
|
||||
* 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, <stephen@solarflare.org.uk>, 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<btVector3> omega;omega.resize(num_links+1);
|
||||
btAlignedObjectArray<btVector3> 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<btVector3> omega;omega.resize(num_links+1);
|
||||
btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
|
||||
btAlignedObjectArray<btQuaternion> 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<btScalar> &scratch_r,
|
||||
btAlignedObjectArray<btVector3> &scratch_v,
|
||||
btAlignedObjectArray<btMatrix3x3> &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<btScalar, 6, 6> 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<Matrix<btScalar, 6, 6> >(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<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &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<btScalar> &scratch_r,
|
||||
btAlignedObjectArray<btVector3> &scratch_v,
|
||||
btAlignedObjectArray<btMatrix3x3> &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();
|
||||
}
|
||||
}
|
||||
353
src/BulletDynamics/Featherstone/btMultiBody.h
Normal file
353
src/BulletDynamics/Featherstone/btMultiBody.h
Normal file
@@ -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, <stephen@solarflare.org.uk>, 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<btScalar> &scratch_r,
|
||||
btAlignedObjectArray<btVector3> &scratch_v,
|
||||
btAlignedObjectArray<btMatrix3x3> &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<btScalar> &scratch_r,
|
||||
btAlignedObjectArray<btVector3> &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<btScalar> &scratch_r,
|
||||
btAlignedObjectArray<btVector3> &scratch_v,
|
||||
btAlignedObjectArray<btMatrix3x3> &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<btMultibodyLink> links; // array of links, excluding the base. index from 0 to num_links-2.
|
||||
btAlignedObjectArray<btMultiBodyLinkCollider*> 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<btScalar> real_buf;
|
||||
btAlignedObjectArray<btVector3> vector_buf;
|
||||
btAlignedObjectArray<btMatrix3x3> matrix_buf;
|
||||
|
||||
//std::auto_ptr<Eigen::LU<Eigen::Matrix<btScalar, 6, 6> > > 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
|
||||
694
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
Normal file
694
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
Normal file
@@ -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;j<m_multiBodyNonContactConstraints.size();j++)
|
||||
{
|
||||
btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[j];
|
||||
if (iteration < constraint.m_overrideNumSolverIterations)
|
||||
resolveSingleConstraintRowGeneric(constraint);
|
||||
}
|
||||
|
||||
//solve featherstone normal contact
|
||||
for (int j=0;j<m_multiBodyNormalContactConstraints.size();j++)
|
||||
{
|
||||
btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j];
|
||||
if (iteration < infoGlobal.m_numIterations)
|
||||
resolveSingleConstraintRowGeneric(constraint);
|
||||
}
|
||||
|
||||
//solve featherstone frictional contact
|
||||
|
||||
for (int j=0;j<this->m_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;i<numBodies;i++)
|
||||
{
|
||||
const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(bodies[i]);
|
||||
if (fcA)
|
||||
{
|
||||
fcA->m_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;j<manifold->getNumContacts();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;i<numManifolds;i++)
|
||||
{
|
||||
btPersistentManifold* manifold= manifoldPtr[i];
|
||||
const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
|
||||
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);
|
||||
}
|
||||
@@ -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<btScalar> m_jacobians;
|
||||
btAlignedObjectArray<btScalar> m_deltaVelocitiesUnitImpulse;
|
||||
btAlignedObjectArray<btScalar> m_deltaVelocities;
|
||||
|
||||
|
||||
btAlignedObjectArray<btScalar> scratch_r;
|
||||
btAlignedObjectArray<btVector3> scratch_v;
|
||||
btAlignedObjectArray<btMatrix3x3> 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
|
||||
|
||||
265
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
Normal file
265
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
Normal file
@@ -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;i<this->m_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;i<m_multiBodies.size();i++)
|
||||
{
|
||||
btMultiBody* body = m_multiBodies[i];
|
||||
if (body->getNumLinkColliders()>1)
|
||||
{
|
||||
btMultiBodyLinkCollider* prev = body->getLinkCollider(0);
|
||||
for (int b=1;b<body->getNumLinkColliders();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;i<m_multiBodies.size();i++)
|
||||
{
|
||||
btMultiBody* body = m_multiBodies[i];
|
||||
if (body)
|
||||
{
|
||||
body->checkMotionAndSleepIfRequired(timeStep);
|
||||
if (!body->isAwake())
|
||||
{
|
||||
for (int b=0;b<body->getNumLinkColliders();b++)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = body->getLinkCollider(b);
|
||||
if (col->getActivationState() == ACTIVE_TAG)
|
||||
{
|
||||
col->setActivationState( WANTS_DEACTIVATION);
|
||||
col->setDeactivationTime(0.f);
|
||||
}
|
||||
}
|
||||
} else
|
||||
{
|
||||
for (int b=0;b<body->getNumLinkColliders();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<btScalar> scratch_r;
|
||||
btAlignedObjectArray<btVector3> scratch_v;
|
||||
btAlignedObjectArray<btMatrix3x3> scratch_m;
|
||||
|
||||
|
||||
{
|
||||
BT_PROFILE("btMultiBody addForce and stepVelocities");
|
||||
for (int i=0;i<this->m_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<btQuaternion> world_to_local;
|
||||
btAlignedObjectArray<btVector3> local_origin;
|
||||
|
||||
for (int b=0;b<m_multiBodies.size();b++)
|
||||
{
|
||||
btMultiBody* bod = m_multiBodies[b];
|
||||
bool isSleeping = false;
|
||||
if (bod->getNumLinkColliders() && 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;k<bod->getNumLinks();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;m<bod->getNumLinkColliders();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();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
48
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
Normal file
48
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
Normal file
@@ -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<btMultiBody*> 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
|
||||
102
src/BulletDynamics/Featherstone/btMultiBodyLink.h
Normal file
102
src/BulletDynamics/Featherstone/btMultiBodyLink.h
Normal file
@@ -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
|
||||
62
src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
Normal file
62
src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
Normal file
@@ -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
|
||||
|
||||
@@ -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<btMultiBodySolverConstraint> btMultiBodyConstraintArray;
|
||||
|
||||
#endif //BT_MULTIBODY_SOLVER_CONSTRAINT_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;
|
||||
|
||||
@@ -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 \
|
||||
|
||||
Reference in New Issue
Block a user