AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-08 08:30:16 +00:00
Code Issues Releases Wiki Activity

Added btMultiBodyPoint2Point, it can be used between btMultiBody vs btMultiBody or btMultiBody vs btRigidBody

Browse Source 
Allow picking of btMultiBody, using a btMultiBodyPoint2Point constraint, with limited strength to avoid adding too much energy to the system (= blowup)
Add btMultiBodyJointMotor, it can be used in combination with joint limit (just add the joint limit after the motor, to avoid jitter)
This commit is contained in:
erwin.coumans@gmail.com 2013-10-05 01:46:32 +00:00
parent 2fb686b937
commit 488dd44835
18 changed files with 1418 additions and 475 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

232
Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp
View File

@ -20,12 +20,13 @@ subject to the following restrictions:
#define ARRAY_SIZE_X 5
#define ARRAY_SIZE_Y 5
#define ARRAY_SIZE_Z 5
float friction = 1.;
//maximum number of objects (and allow user to shoot additional boxes)
#define MAX_PROXIES (ARRAY_SIZE_X*ARRAY_SIZE_Y*ARRAY_SIZE_Z + 1024)
///scaling of the objects (0.1 = 20 centimeter boxes )
#define SCALING 1.
#define START_POS_X -5
//#define START_POS_Y 12
#define START_POS_Y 2
@ -39,7 +40,8 @@ subject to the following restrictions:
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyLink.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "GlutStuff.h"
@ -51,6 +53,8 @@ subject to the following restrictions:
#include "LinearMath/btAabbUtil2.h"
static GLDebugDrawer gDebugDraw;
//btVector3 scaling(0.1,0.1,0.1);
float scaling = 0.4f;
void FeatherstoneMultiBodyDemo::clientMoveAndDisplay()
@ -107,8 +111,8 @@ void FeatherstoneMultiBodyDemo::initPhysics()
setTexturing(true);
setShadows(true);
setCameraDistance(btScalar(SCALING*50.));
setCameraDistance(btScalar(100.*scaling));
this->m_azi = 130;
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btDefaultCollisionConfiguration();
@ -129,7 +133,7 @@ void FeatherstoneMultiBodyDemo::initPhysics()
m_dynamicsWorld->setGravity(btVector3(0,-10,0));
///create a few basic rigid bodies
btBoxShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
//groundShape->initializePolyhedralFeatures();
// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
@ -137,34 +141,15 @@ void FeatherstoneMultiBodyDemo::initPhysics()
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-50,0));
groundTransform.setOrigin(btVector3(0,-50,00));
//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
{
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
groundShape->calculateLocalInertia(mass,localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
//add the body to the dynamics world
m_dynamicsWorld->addRigidBody(body);//,1,1+2);
}
if (1)
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
btBoxShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1));
btBoxShape* colShape = new btBoxShape(btVector3(1,1,1));
//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
m_collisionShapes.push_back(colShape);
@ -191,7 +176,7 @@ void FeatherstoneMultiBodyDemo::initPhysics()
{
for(int j = 0;j<ARRAY_SIZE_Z;j++)
{
startTransform.setOrigin(SCALING*btVector3(
startTransform.setOrigin(btVector3(
btScalar(3.0*i + start_x),
btScalar(3.0*k + start_y),
btScalar(3.0*j + start_z)));
@ -209,29 +194,73 @@ void FeatherstoneMultiBodyDemo::initPhysics()
}
btMultiBody* mb = createFeatherstoneMultiBody(world, 1, btVector3 (20,29.5,-2), true, true,true);
btMultiBody* mbA = createFeatherstoneMultiBody(world, 4, btVector3 (60,29.5,-2)*scaling, false, true,true,true);
mb = createFeatherstoneMultiBody(world, 5, btVector3 (0,29.5,-2), false,false,true);
int numLinks = 10;
btMultiBody* mbB = createFeatherstoneMultiBody(world, numLinks, btVector3 (0,29.5,-numLinks*4.f), true,false,true,true);
btMultiBody* mbC = createFeatherstoneMultiBody(world, numLinks, btVector3 (-20*scaling,29.5*scaling,-numLinks*4.f*scaling), false,false,true,true);
btMultiBody* mbPrim= createFeatherstoneMultiBody(world, numLinks, btVector3 (-20,29.5,-numLinks*4.f), false,true,true,true);
//btMultiBody* mbB = createFeatherstoneMultiBody(world, 15, btVector3 (0,29.5,-2), false,true,true);
#if 0
if (0)//!useGroundShape && i==4)
{
//attach two multibody using a point2point constraint
//btVector3 pivotInAworld(0,20,46);
btVector3 pivotInAworld(-0.3,29,-3.5);
int linkA = -1;
int linkB = -1;
btVector3 pivotInAlocal = mbA->worldPosToLocal(linkA, pivotInAworld);
btVector3 pivotInBlocal = mbB->worldPosToLocal(linkB, pivotInAworld);
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(mbA,linkA,mbB,linkB,pivotInAlocal,pivotInBlocal);
world->addMultiBodyConstraint(p2p);
}
#endif
bool testRemoveLinks = false;
if (testRemoveLinks)
{
while (mb->getNumLinks())
while (mbA->getNumLinks())
{
btCollisionObject* col = mb->getLink(mb->getNumLinks()-1).m_collider;
btCollisionObject* col = mbA->getLink(mbA->getNumLinks()-1).m_collider;
m_dynamicsWorld->removeCollisionObject(col);
delete col;
mb->setNumLinks(mb->getNumLinks()-1);
mbA->setNumLinks(mbA->getNumLinks()-1);
}
}
if (1)//useGroundShape
{
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
groundShape->calculateLocalInertia(mass,localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
//add the body to the dynamics world
m_dynamicsWorld->addRigidBody(body,1,1+2);//,1,1+2);
}
}
btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic, bool canSleep)
btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic, bool canSleep, bool createConstraints)
{
int n_links = numLinks;
float mass = 13.5;
btVector3 inertia(91,344,253);
float mass = 13.5*scaling;
btVector3 inertia = btVector3 (91,344,253)*scaling*scaling;
btMultiBody * bod = new btMultiBody(n_links, mass, inertia, isFixedBase, canSleep);
@ -256,9 +285,9 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult
btVector3 pos(0,0,9.0500002);
btVector3 pos = btVector3 (0,0,9.0500002)*scaling;
btVector3 joint_axis_position(0,0,4.5250001);
btVector3 joint_axis_position = btVector3 (0,0,4.5250001)*scaling;
for (int i=0;i<n_links;i++)
{
@ -268,7 +297,7 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult
const int child_link_num = link_num_counter++;
if (usePrismatic && i==(n_links-1))
if (usePrismatic)// && i==(n_links-1))
{
bod->setupPrismatic(child_link_num, mass, inertia, this_link_num,
parent_to_child, joint_axis_child_prismatic, quatRotate(parent_to_child , pos));
@ -280,13 +309,40 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult
}
bod->setJointPos(child_link_num, initial_joint_angle);
this_link_num = i;
}
if (0)//!useGroundShape && i==4)
{
btVector3 pivotInAworld(0,20,46);
btVector3 pivotInAlocal = bod->worldPosToLocal(i, pivotInAworld);
btVector3 pivotInBworld = pivotInAworld;
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(bod,i,&btTypedConstraint::getFixedBody(),pivotInAlocal,pivotInBworld);
world->addMultiBodyConstraint(p2p);
}
//add some constraint limit
if (usePrismatic)
{
// btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,n_links-1,2,3);
if (0)
{
btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod,i,1,500000);
world->addMultiBodyConstraint(con);
}
if (createConstraints)
{
btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,i,-1,1);
world->addMultiBodyConstraint(con);
}
} else
{
if (createConstraints)
{
btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,i,-1,1);
world->addMultiBodyConstraint(con);
}
//add some constraint limit
if (usePrismatic)
{
btMultiBodyConstraint* limit = new btMultiBodyJointLimitConstraint(bod,n_links-1,2,3);
world->addMultiBodyConstraint(limit);
}
}
}
@ -310,7 +366,7 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult
if (1)
{
btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2]));
btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])*scaling);
btRigidBody* body = new btRigidBody(mass,0,box,inertia);
btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(bod,-1);
@ -324,8 +380,8 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult
body->setWorldTransform(tr);
col->setWorldTransform(tr);
world->addCollisionObject(col, btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter);
col->setFriction(1);
world->addCollisionObject(col, 2,1);
col->setFriction(friction);
bod->setBaseCollider(col);
}
@ -348,7 +404,7 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult
float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2]));
btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])*scaling);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(bod,i);
col->setCollisionShape(box);
@ -357,8 +413,8 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
col->setWorldTransform(tr);
col->setFriction(1);
world->addCollisionObject(col,btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter);//,2,1);
col->setFriction(friction);
world->addCollisionObject(col,2,1);
bod->getLink(i).m_collider=col;
//app->drawBox(halfExtents, pos,quat);
@ -370,6 +426,80 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult
return bod;
}
extern btScalar gOldPickingDist;
void FeatherstoneMultiBodyDemo::mouseMotionFunc(int x,int y)
{
if (m_pickingMultiBodyPoint2Point)
{
//keep it at the same picking distance
btVector3 newRayTo = getRayTo(x,y);
btVector3 rayFrom;
btVector3 oldPivotInB = m_pickingMultiBodyPoint2Point->getPivotInB();
btVector3 newPivotB;
if (m_ortho)
{
newPivotB = oldPivotInB;
newPivotB.setX(newRayTo.getX());
newPivotB.setY(newRayTo.getY());
} else
{
rayFrom = m_cameraPosition;
btVector3 dir = newRayTo-rayFrom;
dir.normalize();
dir *= gOldPickingDist;
newPivotB = rayFrom + dir;
}
m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB);
}
DemoApplication::mouseMotionFunc(x,y);
}
void FeatherstoneMultiBodyDemo::removePickingConstraint()
{
if (m_pickingMultiBodyPoint2Point)
{
m_pickingMultiBodyPoint2Point->getMultiBodyA()->setCanSleep(true);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_dynamicsWorld;
world->removeMultiBodyConstraint(m_pickingMultiBodyPoint2Point);
delete m_pickingMultiBodyPoint2Point;
m_pickingMultiBodyPoint2Point = 0;
}
DemoApplication::removePickingConstraint();
}
void FeatherstoneMultiBodyDemo::pickObject(const btVector3& pickPos, const class btCollisionObject* hitObj)
{
btVector3 pivotInA(0,0,0);
btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(hitObj);
if (multiCol && multiCol->m_multiBody)
{
multiCol->m_multiBody->setCanSleep(false);
btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos);
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody,multiCol->m_link,0,pivotInA,pickPos);
//if you add too much energy to the system, causing high angular velocities, simulation 'explodes'
//see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949
//so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply
//it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?)
p2p->setMaxAppliedImpulse(200*scaling);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_dynamicsWorld;
world->addMultiBodyConstraint(p2p);
m_pickingMultiBodyPoint2Point =p2p;
} else
{
DemoApplication::pickObject(pickPos,hitObj);
}
}
void FeatherstoneMultiBodyDemo::clientResetScene()
{

7
Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.h
View File

@ -53,12 +53,17 @@ class FeatherstoneMultiBodyDemo : public PlatformDemoApplication
btDefaultCollisionConfiguration* m_collisionConfiguration;
virtual void mouseMotionFunc(int x,int y);
virtual void removePickingConstraint();
virtual void pickObject(const btVector3& pickPos, const class btCollisionObject* hitObj);
class btMultiBodyPoint2Point* m_pickingMultiBodyPoint2Point;
btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic, bool canSleep);
btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic, bool canSleep, bool createConstraints);
public:
FeatherstoneMultiBodyDemo()
:m_pickingMultiBodyPoint2Point(0)
{
}
virtual ~FeatherstoneMultiBodyDemo()

154
Demos/OpenGL/DemoApplication.cpp
View File

@ -773,82 +773,14 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
if (rayCallback.hasHit())
{
btVector3 pickPos = rayCallback.m_hitPointWorld;
pickObject(pickPos, rayCallback.m_collisionObject);
gOldPickingPos = rayTo;
gHitPos = pickPos;
btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject);
if (body)
{
//other exclusions?
if (!(body->isStaticObject() || body->isKinematicObject()))
{
pickedBody = body;
pickedBody->setActivationState(DISABLE_DEACTIVATION);
btVector3 pickPos = rayCallback.m_hitPointWorld;
//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
if ((m_modifierKeys& BT_ACTIVE_SHIFT)==0)
{
btTransform tr;
tr.setIdentity();
tr.setOrigin(localPivot);
btGeneric6DofConstraint* dof6 = new btGeneric6DofConstraint(*body, tr,false);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setAngularLowerLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,0));
m_dynamicsWorld->addConstraint(dof6,true);
m_pickConstraint = dof6;
dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,0);
dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,1);
dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,2);
dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,3);
dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,4);
dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,5);
dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,0);
dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,1);
dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,2);
dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,3);
dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,4);
dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,5);
} else
{
btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body,localPivot);
m_dynamicsWorld->addConstraint(p2p,true);
m_pickConstraint = p2p;
p2p->m_setting.m_impulseClamp = mousePickClamping;
//very weak constraint for picking
p2p->m_setting.m_tau = 0.001f;
/*
p2p->setParam(BT_CONSTRAINT_CFM,0.8,0);
p2p->setParam(BT_CONSTRAINT_CFM,0.8,1);
p2p->setParam(BT_CONSTRAINT_CFM,0.8,2);
p2p->setParam(BT_CONSTRAINT_ERP,0.1,0);
p2p->setParam(BT_CONSTRAINT_ERP,0.1,1);
p2p->setParam(BT_CONSTRAINT_ERP,0.1,2);
*/
}
//save mouse position for dragging
gOldPickingPos = rayTo;
gHitPos = pickPos;
gOldPickingDist = (pickPos-rayFrom).length();
}
}
gOldPickingDist = (pickPos-rayFrom).length();
}
}
@ -867,6 +799,78 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
}
void DemoApplication::pickObject(const btVector3& pickPos, const btCollisionObject* hitObj)
{
btRigidBody* body = (btRigidBody*)btRigidBody::upcast(hitObj);
if (body)
{
//other exclusions?
if (!(body->isStaticObject() || body->isKinematicObject()))
{
pickedBody = body;
pickedBody->setActivationState(DISABLE_DEACTIVATION);
//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
if ((m_modifierKeys& BT_ACTIVE_SHIFT)==0)
{
btTransform tr;
tr.setIdentity();
tr.setOrigin(localPivot);
btGeneric6DofConstraint* dof6 = new btGeneric6DofConstraint(*body, tr,false);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setAngularLowerLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,0));
m_dynamicsWorld->addConstraint(dof6,true);
m_pickConstraint = dof6;
dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,0);
dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,1);
dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,2);
dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,3);
dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,4);
dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,5);
dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,0);
dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,1);
dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,2);
dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,3);
dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,4);
dof6->setParam(BT_CONSTRAINT_STOP_ERP,0.1,5);
} else
{
btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body,localPivot);
m_dynamicsWorld->addConstraint(p2p,true);
m_pickConstraint = p2p;
p2p->m_setting.m_impulseClamp = mousePickClamping;
//very weak constraint for picking
p2p->m_setting.m_tau = 0.001f;
/*
p2p->setParam(BT_CONSTRAINT_CFM,0.8,0);
p2p->setParam(BT_CONSTRAINT_CFM,0.8,1);
p2p->setParam(BT_CONSTRAINT_CFM,0.8,2);
p2p->setParam(BT_CONSTRAINT_ERP,0.1,0);
p2p->setParam(BT_CONSTRAINT_ERP,0.1,1);
p2p->setParam(BT_CONSTRAINT_ERP,0.1,2);
*/
}
//save mouse position for dragging
}
}
}
void DemoApplication::removePickingConstraint()
{
if (m_pickConstraint && m_dynamicsWorld)

3
Demos/OpenGL/DemoApplication.h
View File

@ -57,6 +57,9 @@ protected:
virtual void removePickingConstraint();
virtual void pickObject(const btVector3& pickPos, const class btCollisionObject* hitObj);
btCollisionShape* m_shootBoxShape;
float m_cameraDistance;

6
src/BulletDynamics/CMakeLists.txt
View File

@ -28,6 +28,9 @@ SET(BulletDynamics_SRCS
Featherstone/btMultiBodyConstraintSolver.cpp
Featherstone/btMultiBodyDynamicsWorld.cpp
Featherstone/btMultiBodyJointLimitConstraint.cpp
Featherstone/btMultiBodyConstraint.cpp
Featherstone/btMultiBodyPoint2Point.cpp
Featherstone/btMultiBodyJointMotor.cpp
)
SET(Root_HDRS
@ -77,6 +80,9 @@ SET(Featherstone_HDRS
Featherstone/btMultiBodySolverConstraint.h
Featherstone/btMultiBodyConstraint.h
Featherstone/btMultiBodyJointLimitConstraint.h
Featherstone/btMultiBodyConstraint.h
Featherstone/btMultiBodyPoint2Point.h
Featherstone/btMultiBodyJointMotor.h
)
SET(Character_HDRS
Character/btCharacterControllerInterface.h

179
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@ -28,7 +28,7 @@
// #define INCLUDE_GYRO_TERM
namespace {
const btScalar SLEEP_EPSILON = btScalar(0.01); // this is a squared velocity (m^2 s^-2)
const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2)
const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds
}
@ -85,17 +85,21 @@ btMultiBody::btMultiBody(int n_links,
base_mass(mass),
base_inertia(inertia),
fixed_base(fixed_base_),
awake(true),
can_sleep(can_sleep_),
sleep_timer(0),
m_baseCollider(0)
fixed_base(fixed_base_),
awake(true),
can_sleep(can_sleep_),
sleep_timer(0),
m_baseCollider(0),
m_linearDamping(0.04f),
m_angularDamping(0.04f),
m_useGyroTerm(true)
{
links.resize(n_links);
vector_buf.resize(2*n_links);
matrix_buf.resize(n_links + 1);
real_buf.resize(6 + 2*n_links);
m_real_buf.resize(6 + 2*n_links);
base_pos.setValue(0, 0, 0);
base_force.setValue(0, 0, 0);
base_torque.setValue(0, 0, 0);
@ -172,7 +176,7 @@ btScalar btMultiBody::getJointPos(int i) const
btScalar btMultiBody::getJointVel(int i) const
{
return real_buf[6 + i];
return m_real_buf[6 + i];
}
void btMultiBody::setJointPos(int i, btScalar q)
@ -183,7 +187,7 @@ void btMultiBody::setJointPos(int i, btScalar q)
void btMultiBody::setJointVel(int i, btScalar qdot)
{
real_buf[6 + i] = qdot;
m_real_buf[6 + i] = qdot;
}
const btVector3 & btMultiBody::getRVector(int i) const
@ -322,7 +326,7 @@ void btMultiBody::clearVelocities()
{
for (int i = 0; i < 6 + getNumLinks(); ++i)
{
real_buf[i] = 0.f;
m_real_buf[i] = 0.f;
}
}
void btMultiBody::addLinkForce(int i, const btVector3 &f)
@ -395,9 +399,11 @@ void btMultiBody::stepVelocities(btScalar dt,
int num_links = getNumLinks();
const btScalar DAMPING_K1 = btScalar(0.0);
//const btScalar DAMPING_K2 = btScalar(0);
const btScalar DAMPING_K2 = btScalar(0.0);
const btScalar DAMPING_K1_LINEAR = m_linearDamping;
const btScalar DAMPING_K2_LINEAR = m_linearDamping;
const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
btVector3 base_vel = getBaseVel();
btVector3 base_omega = getBaseOmega();
@ -414,16 +420,16 @@ void btMultiBody::stepVelocities(btScalar dt,
btVector3 * v_ptr = &scratch_v[0];
// vhat_i (top = angular, bottom = linear part)
btVector3 * vel_top = v_ptr; v_ptr += num_links + 1;
btVector3 * vel_bottom = v_ptr; v_ptr += num_links + 1;
btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1;
btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1;
// zhat_i^A
btVector3 * zero_acc_top = v_ptr; v_ptr += num_links + 1;
btVector3 * zero_acc_bottom = v_ptr; v_ptr += num_links + 1;
btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
// chat_i (note NOT defined for the base)
btVector3 * coriolis_top = v_ptr; v_ptr += num_links;
btVector3 * coriolis_bottom = v_ptr; v_ptr += num_links;
btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links;
btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links;
// top left, top right and bottom left blocks of Ihat_i^A.
// bottom right block = transpose of top left block and is not stored.
@ -445,7 +451,7 @@ void btMultiBody::stepVelocities(btScalar dt,
// Y_i, D_i
btScalar * Y = r_ptr; r_ptr += num_links;
btScalar * D = num_links > 0 ? &real_buf[6 + num_links] : 0;
btScalar * D = num_links > 0 ? &m_real_buf[6 + num_links] : 0;
// ptr to the joint accel part of the output
btScalar * joint_accel = output + 6;
@ -458,21 +464,27 @@ void btMultiBody::stepVelocities(btScalar dt,
rot_from_parent[0] = btMatrix3x3(base_quat);
vel_top[0] = rot_from_parent[0] * base_omega;
vel_bottom[0] = rot_from_parent[0] * base_vel;
vel_top_angular[0] = rot_from_parent[0] * base_omega;
vel_bottom_linear[0] = rot_from_parent[0] * base_vel;
if (fixed_base) {
zero_acc_top[0] = zero_acc_bottom[0] = btVector3(0,0,0);
zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0);
} else {
zero_acc_top[0] = - (rot_from_parent[0] * (base_force
- base_mass*(DAMPING_K1+DAMPING_K2*base_vel.norm())*base_vel));
zero_acc_bottom[0] =
#ifdef INCLUDE_GYRO_TERM
vel_top[0].cross( base_inertia.cwise() * vel_top[0] )
#endif
zero_acc_top_angular[0] = - (rot_from_parent[0] * (base_force
- base_mass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
zero_acc_bottom_linear[0] =
- (rot_from_parent[0] * base_torque);
zero_acc_bottom[0] += base_inertia * vel_top[0] * (DAMPING_K1 + DAMPING_K2*vel_top[0].norm());
if (m_useGyroTerm)
zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( base_inertia * vel_top_angular[0] );
zero_acc_bottom_linear[0] += base_inertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());
}
inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
@ -494,35 +506,37 @@ void btMultiBody::stepVelocities(btScalar dt,
// vhat_i = i_xhat_p(i) * vhat_p(i)
SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
vel_top[parent+1], vel_bottom[parent+1],
vel_top[i+1], vel_bottom[i+1]);
vel_top_angular[parent+1], vel_bottom_linear[parent+1],
vel_top_angular[i+1], vel_bottom_linear[i+1]);
// we can now calculate chat_i
// remember vhat_i is really vhat_p(i) (but in current frame) at this point
coriolis_bottom[i] = vel_top[i+1].cross(vel_top[i+1].cross(links[i].cached_r_vector))
+ 2 * vel_top[i+1].cross(links[i].axis_bottom) * getJointVel(i);
coriolis_bottom_linear[i] = vel_top_angular[i+1].cross(vel_top_angular[i+1].cross(links[i].cached_r_vector))
+ 2 * vel_top_angular[i+1].cross(links[i].axis_bottom) * getJointVel(i);
if (links[i].is_revolute) {
coriolis_top[i] = vel_top[i+1].cross(links[i].axis_top) * getJointVel(i);
coriolis_bottom[i] += (getJointVel(i) * getJointVel(i)) * links[i].axis_top.cross(links[i].axis_bottom);
coriolis_top_angular[i] = vel_top_angular[i+1].cross(links[i].axis_top) * getJointVel(i);
coriolis_bottom_linear[i] += (getJointVel(i) * getJointVel(i)) * links[i].axis_top.cross(links[i].axis_bottom);
} else {
coriolis_top[i] = btVector3(0,0,0);
coriolis_top_angular[i] = btVector3(0,0,0);
}
// now set vhat_i to its true value by doing
// vhat_i += qidot * shat_i
vel_top[i+1] += getJointVel(i) * links[i].axis_top;
vel_bottom[i+1] += getJointVel(i) * links[i].axis_bottom;
vel_top_angular[i+1] += getJointVel(i) * links[i].axis_top;
vel_bottom_linear[i+1] += getJointVel(i) * links[i].axis_bottom;
// calculate zhat_i^A
zero_acc_top[i+1] = - (rot_from_world[i+1] * (links[i].applied_force));
zero_acc_top[i+1] += links[i].mass * (DAMPING_K1 + DAMPING_K2*vel_bottom[i+1].norm()) * vel_bottom[i+1];
zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (links[i].applied_force));
zero_acc_top_angular[i+1] += links[i].mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1];
zero_acc_bottom[i+1] =
#ifdef INCLUDE_GYRO_TERM
vel_top[i+1].cross( links[i].inertia.cwise() * vel_top[i+1] )
#endif
zero_acc_bottom_linear[i+1] =
- (rot_from_world[i+1] * links[i].applied_torque);
zero_acc_bottom[i+1] += links[i].inertia * vel_top[i+1] * (DAMPING_K1 + DAMPING_K2*vel_top[i+1].norm());
if (m_useGyroTerm)
{
zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( links[i].inertia * vel_top_angular[i+1] );
}
zero_acc_bottom_linear[i+1] += links[i].inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
// calculate Ihat_i^A
inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
@ -541,11 +555,11 @@ void btMultiBody::stepVelocities(btScalar dt,
h_top[i] = inertia_top_left[i+1] * links[i].axis_top + inertia_top_right[i+1] * links[i].axis_bottom;
h_bottom[i] = inertia_bottom_left[i+1] * links[i].axis_top + inertia_top_left[i+1].transpose() * links[i].axis_bottom;
D[i] = SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, h_top[i], h_bottom[i]);
Y[i] = links[i].joint_torque
- SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top[i+1], zero_acc_bottom[i+1])
- SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top[i], coriolis_bottom[i]);
btScalar val = SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, h_top[i], h_bottom[i]);
D[i] = val;
Y[i] = links[i].joint_torque
- SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1])
- SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]);
const int parent = links[i].parent;
@ -576,18 +590,18 @@ void btMultiBody::stepVelocities(btScalar dt,
// Zp += pXi * (Zi + Ii*ci + hi*Yi/Di)
btVector3 in_top, in_bottom, out_top, out_bottom;
const btScalar Y_over_D = Y[i] * one_over_di;
in_top = zero_acc_top[i+1]
+ inertia_top_left[i+1] * coriolis_top[i]
+ inertia_top_right[i+1] * coriolis_bottom[i]
in_top = zero_acc_top_angular[i+1]
+ inertia_top_left[i+1] * coriolis_top_angular[i]
+ inertia_top_right[i+1] * coriolis_bottom_linear[i]
+ Y_over_D * h_top[i];
in_bottom = zero_acc_bottom[i+1]
+ inertia_bottom_left[i+1] * coriolis_top[i]
+ inertia_top_left[i+1].transpose() * coriolis_bottom[i]
in_bottom = zero_acc_bottom_linear[i+1]
+ inertia_bottom_left[i+1] * coriolis_top_angular[i]
+ inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i]
+ Y_over_D * h_bottom[i];
InverseSpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
in_top, in_bottom, out_top, out_bottom);
zero_acc_top[parent+1] += out_top;
zero_acc_bottom[parent+1] += out_bottom;
zero_acc_top_angular[parent+1] += out_top;
zero_acc_bottom_linear[parent+1] += out_bottom;
}
@ -615,8 +629,8 @@ void btMultiBody::stepVelocities(btScalar dt,
cached_inertia_lower_right= inertia_top_left[0].transpose();
}
btVector3 rhs_top (zero_acc_top[0][0], zero_acc_top[0][1], zero_acc_top[0][2]);
btVector3 rhs_bot (zero_acc_bottom[0][0], zero_acc_bottom[0][1], zero_acc_bottom[0][2]);
btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
float result[6];
solveImatrix(rhs_top, rhs_bot, result);
@ -635,8 +649,8 @@ void btMultiBody::stepVelocities(btScalar dt,
accel_top[parent+1], accel_bottom[parent+1],
accel_top[i+1], accel_bottom[i+1]);
joint_accel[i] = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i];
accel_top[i+1] += coriolis_top[i] + joint_accel[i] * links[i].axis_top;
accel_bottom[i+1] += coriolis_bottom[i] + joint_accel[i] * links[i].axis_bottom;
accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] * links[i].axis_top;
accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] * links[i].axis_bottom;
}
// transform base accelerations back to the world frame.
@ -720,8 +734,8 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output
btVector3 * v_ptr = &scratch_v[0];
// zhat_i^A (scratch space)
btVector3 * zero_acc_top = v_ptr; v_ptr += num_links + 1;
btVector3 * zero_acc_bottom = v_ptr; v_ptr += num_links + 1;
btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
// rot_from_parent (cached from calcAccelerations)
const btMatrix3x3 * rot_from_parent = &matrix_buf[0];
@ -734,7 +748,7 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output
// Y_i (scratch), D_i (cached)
btScalar * Y = r_ptr; r_ptr += num_links;
const btScalar * D = num_links > 0 ? &real_buf[6 + num_links] : 0;
const btScalar * D = num_links > 0 ? &m_real_buf[6 + num_links] : 0;
btAssert(num_links == 0 || r_ptr - &scratch_r[0] == scratch_r.size());
btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
@ -751,22 +765,22 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output
// -- set to force/torque on the base, zero otherwise
if (fixed_base)
{
zero_acc_top[0] = zero_acc_bottom[0] = btVector3(0,0,0);
zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0);
} else
{
zero_acc_top[0] = - (rot_from_parent[0] * input_force);
zero_acc_bottom[0] = - (rot_from_parent[0] * input_torque);
zero_acc_top_angular[0] = - (rot_from_parent[0] * input_force);
zero_acc_bottom_linear[0] = - (rot_from_parent[0] * input_torque);
}
for (int i = 0; i < num_links; ++i)
{
zero_acc_top[i+1] = zero_acc_bottom[i+1] = btVector3(0,0,0);
zero_acc_top_angular[i+1] = zero_acc_bottom_linear[i+1] = btVector3(0,0,0);
}
// 'Downward' loop.
for (int i = num_links - 1; i >= 0; --i)
{
Y[i] = - SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top[i+1], zero_acc_bottom[i+1]);
Y[i] = - SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
Y[i] += force[6 + i]; // add joint torque
const int parent = links[i].parent;
@ -774,12 +788,12 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output
// Zp += pXi * (Zi + hi*Yi/Di)
btVector3 in_top, in_bottom, out_top, out_bottom;
const btScalar Y_over_D = Y[i] / D[i];
in_top = zero_acc_top[i+1] + Y_over_D * h_top[i];
in_bottom = zero_acc_bottom[i+1] + Y_over_D * h_bottom[i];
in_top = zero_acc_top_angular[i+1] + Y_over_D * h_top[i];
in_bottom = zero_acc_bottom_linear[i+1] + Y_over_D * h_bottom[i];
InverseSpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
in_top, in_bottom, out_top, out_bottom);
zero_acc_top[parent+1] += out_top;
zero_acc_bottom[parent+1] += out_bottom;
zero_acc_top_angular[parent+1] += out_top;
zero_acc_bottom_linear[parent+1] += out_bottom;
}
// ptr to the joint accel part of the output
@ -791,8 +805,8 @@ void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output
accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
} else
{
btVector3 rhs_top (zero_acc_top[0][0], zero_acc_top[0][1], zero_acc_top[0][2]);
btVector3 rhs_bot (zero_acc_bottom[0][0], zero_acc_bottom[0][1], zero_acc_bottom[0][2]);
btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
float result[6];
solveImatrix(rhs_top,rhs_bot, result);
@ -960,13 +974,18 @@ void btMultiBody::goToSleep()
void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
{
int num_links = getNumLinks();
if (!can_sleep) return;
extern bool gDisableDeactivation;
if (!can_sleep || gDisableDeactivation)
{
awake = true;
sleep_timer = 0;
return;
}
// motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
btScalar motion = 0;
for (int i = 0; i < 6 + num_links; ++i) {
motion += real_buf[i] * real_buf[i];
motion += m_real_buf[i] * m_real_buf[i];
}
if (motion < SLEEP_EPSILON) {

87
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@ -126,14 +126,15 @@ public:
//
const btVector3 & getBasePos() const { return base_pos; } // in world frame
btVector3 getBaseVel() const {
return btVector3(real_buf[3],real_buf[4],real_buf[5]);
const btVector3 getBaseVel() const
{
return btVector3(m_real_buf[3],m_real_buf[4],m_real_buf[5]);
} // in world frame
const btQuaternion & getWorldToBaseRot() const
{
return base_quat;
} // rotates world vectors into base frame
btVector3 getBaseOmega() const { return btVector3(real_buf[0],real_buf[1],real_buf[2]); } // in world frame
btVector3 getBaseOmega() const { return btVector3(m_real_buf[0],m_real_buf[1],m_real_buf[2]); } // in world frame
void setBasePos(const btVector3 &pos)
{
@ -141,13 +142,19 @@ public:
}
void setBaseVel(const btVector3 &vel)
{
real_buf[3]=vel[0]; real_buf[4]=vel[1]; real_buf[5]=vel[2];
m_real_buf[3]=vel[0]; m_real_buf[4]=vel[1]; m_real_buf[5]=vel[2];
}
void setWorldToBaseRot(const btQuaternion &rot)
{
base_quat = rot;
}
void setBaseOmega(const btVector3 &omega) { real_buf[0]=omega[0]; real_buf[1]=omega[1]; real_buf[2]=omega[2]; }
void setBaseOmega(const btVector3 &omega)
{
m_real_buf[0]=omega[0];
m_real_buf[1]=omega[1];
m_real_buf[2]=omega[2];
}
//
@ -166,13 +173,13 @@ public:
//
const btScalar * getVelocityVector() const
{
return &real_buf[0];
return &m_real_buf[0];
}
btScalar * getVelocityVector()
/* btScalar * getVelocityVector()
{
return &real_buf[0];
}
*/
//
// get the frames of reference (positions and orientations) of the child links
@ -258,13 +265,38 @@ public:
// apply a delta-vee directly. used in sequential impulses code.
void applyDeltaVee(const btScalar * delta_vee)
{
for (int i = 0; i < 6 + getNumLinks(); ++i)
real_buf[i] += delta_vee[i];
{
m_real_buf[i] += delta_vee[i];
}
}
void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier)
{
for (int i = 0; i < 6 + getNumLinks(); ++i)
real_buf[i] += delta_vee[i] * multiplier;
btScalar sum = 0;
for (int i = 0; i < 6 + getNumLinks(); ++i)
{
sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
}
btScalar l = btSqrt(sum);
static btScalar maxl = -1e30f;
if (l>maxl)
{
maxl=l;
printf("maxl=%f\n",maxl);
}
if (l>100)
{
printf("exceeds 100: l=%f\n",maxl);
multiplier *= 100.f/l;
}
for (int i = 0; i < 6 + getNumLinks(); ++i)
{
sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
m_real_buf[i] += delta_vee[i] * multiplier;
}
}
// timestep the positions (given current velocities).
@ -290,6 +322,10 @@ public:
//
// sleeping
//
void setCanSleep(bool canSleep)
{
can_sleep = canSleep;
}
bool isAwake() const { return awake; }
void wakeUp();
@ -315,6 +351,29 @@ public:
{
links.resize(numLinks);
}
btScalar getLinearDamping() const
{
return m_linearDamping;
}
void setLinearDamping( btScalar damp)
{
m_linearDamping = damp;
}
btScalar getAngularDamping() const
{
return m_angularDamping;
}
bool getUseGyroTerm() const
{
return m_useGyroTerm;
}
void setUseGyroTerm(bool useGyro)
{
m_useGyroTerm = useGyro;
}
private:
btMultiBody(const btMultiBody &); // not implemented
void operator=(const btMultiBody &); // not implemented
@ -356,7 +415,7 @@ private:
// 0 num_links+1 rot_from_parent
//
btAlignedObjectArray<btScalar> real_buf;
btAlignedObjectArray<btScalar> m_real_buf;
btAlignedObjectArray<btVector3> vector_buf;
btAlignedObjectArray<btMatrix3x3> matrix_buf;
@ -375,6 +434,10 @@ private:
btScalar sleep_timer;
int m_companionId;
btScalar m_linearDamping;
btScalar m_angularDamping;
bool m_useGyroTerm;
};
#endif

526
src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp Normal file
View File

@ -0,0 +1,526 @@
#include "btMultiBodyConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
:m_bodyA(bodyA),
m_bodyB(bodyB),
m_linkA(linkA),
m_linkB(linkB),
m_num_rows(numRows),
m_isUnilateral(isUnilateral)
{
m_jac_size_A = (6 + bodyA->getNumLinks());
m_jac_size_both = (m_jac_size_A + (bodyB ? 6 + bodyB->getNumLinks() : 0));
m_pos_offset = ((1 + m_jac_size_both)*m_num_rows);
m_data.resize((2 + m_jac_size_both) * m_num_rows);
}
btMultiBodyConstraint::~btMultiBodyConstraint()
{
}
btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow,
btMultiBodyJacobianData& data,
btScalar* jacOrgA,btScalar* jacOrgB,
const btContactSolverInfo& infoGlobal,
btScalar desiredVelocity,
btScalar lowerLimit,
btScalar upperLimit)
{
constraintRow.m_multiBodyA = m_bodyA;
constraintRow.m_multiBodyB = m_bodyB;
btMultiBody* multiBodyA = constraintRow.m_multiBodyA;
btMultiBody* multiBodyB = constraintRow.m_multiBodyB;
if (multiBodyA)
{
const int ndofA = multiBodyA->getNumLinks() + 6;
constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId();
if (constraintRow.m_deltaVelAindex <0)
{
constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size();
multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
} else
{
btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA);
}
constraintRow.m_jacAindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
for (int i=0;i<ndofA;i++)
data.m_jacobians[constraintRow.m_jacAindex+i] = jacOrgA[i];
float* delta = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v);
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId();
if (constraintRow.m_deltaVelBindex <0)
{
constraintRow.m_deltaVelBindex = data.m_deltaVelocities.size();
multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
}
constraintRow.m_jacBindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
for (int i=0;i<ndofB;i++)
data.m_jacobians[constraintRow.m_jacBindex+i] = jacOrgB[i];
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
multiBodyB->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v);
}
{
btVector3 vec;
btScalar denom0 = 0.f;
btScalar denom1 = 0.f;
btScalar* jacB = 0;
btScalar* jacA = 0;
btScalar* lambdaA =0;
btScalar* lambdaB =0;
int ndofA = 0;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
jacA = &data.m_jacobians[constraintRow.m_jacAindex];
lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
{
float j = jacA[i] ;
float l =lambdaA[i];
denom0 += j*l;
}
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
jacB = &data.m_jacobians[constraintRow.m_jacBindex];
lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
{
float j = jacB[i] ;
float l =lambdaB[i];
denom1 += j*l;
}
}
if (multiBodyA && (multiBodyA==multiBodyB))
{
// ndof1 == ndof2 in this case
for (int i = 0; i < ndofA; ++i)
{
denom1 += jacB[i] * lambdaA[i];
denom1 += jacA[i] * lambdaB[i];
}
}
float d = denom0+denom1;
if (btFabs(d)>SIMD_EPSILON)
{
constraintRow.m_jacDiagABInv = 1.f/(d);
} else
{
constraintRow.m_jacDiagABInv = 1.f;
}
}
//compute rhs and remaining constraintRow fields
btScalar rel_vel = 0.f;
int ndofA = 0;
int ndofB = 0;
{
btVector3 vel1,vel2;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
btScalar* jacA = &data.m_jacobians[constraintRow.m_jacAindex];
for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
}
if (multiBodyB)
{
ndofB = multiBodyB->getNumLinks() + 6;
btScalar* jacB = &data.m_jacobians[constraintRow.m_jacBindex];
for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
}
constraintRow.m_friction = 0.f;
constraintRow.m_appliedImpulse = 0.f;
constraintRow.m_appliedPushImpulse = 0.f;
btScalar velocityError = desiredVelocity - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
if (!infoGlobal.m_splitImpulse)
{
//combine position and velocity into rhs
constraintRow.m_rhs = velocityImpulse;
constraintRow.m_rhsPenetration = 0.f;
} else
{
//split position and velocity into rhs and m_rhsPenetration
constraintRow.m_rhs = velocityImpulse;
constraintRow.m_rhsPenetration = 0.f;
}
constraintRow.m_cfm = 0.f;
constraintRow.m_lowerLimit = lowerLimit;
constraintRow.m_upperLimit = upperLimit;
}
return rel_vel;
}
void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
{
for (int i = 0; i < ndof; ++i)
data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
}
void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint,
btMultiBodyJacobianData& data,
const btVector3& contactNormalOnB,
const btVector3& posAworld, const btVector3& posBworld,
btScalar position,
const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
{
btVector3 rel_pos1 = posAworld;
btVector3 rel_pos2 = posBworld;
solverConstraint.m_multiBodyA = m_bodyA;
solverConstraint.m_multiBodyB = m_bodyB;
solverConstraint.m_linkA = m_linkA;
solverConstraint.m_linkB = m_linkB;
btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
const btVector3& pos1 = posAworld;
const btVector3& pos2 = posBworld;
btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
if (bodyA)
rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
if (bodyB)
rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
relaxation = 1.f;
if (multiBodyA)
{
const int ndofA = multiBodyA->getNumLinks() + 6;
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
if (solverConstraint.m_deltaVelAindex <0)
{
solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
} else
{
btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
}
solverConstraint.m_jacAindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
float* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
float* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
} else
{
btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = contactNormalOnB;
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
if (solverConstraint.m_deltaVelBindex <0)
{
solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
}
solverConstraint.m_jacBindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v);
} else
{
btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -contactNormalOnB;
}
{
btVector3 vec;
btScalar denom0 = 0.f;
btScalar denom1 = 0.f;
btScalar* jacB = 0;
btScalar* jacA = 0;
btScalar* lambdaA =0;
btScalar* lambdaB =0;
int ndofA = 0;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
lambdaA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
{
float j = jacA[i] ;
float l =lambdaA[i];
denom0 += j*l;
}
} else
{
if (rb0)
{
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
}
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
lambdaB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
{
float j = jacB[i] ;
float l =lambdaB[i];
denom1 += j*l;
}
} else
{
if (rb1)
{
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
}
}
if (multiBodyA && (multiBodyA==multiBodyB))
{
// ndof1 == ndof2 in this case
for (int i = 0; i < ndofA; ++i)
{
denom1 += jacB[i] * lambdaA[i];
denom1 += jacA[i] * lambdaB[i];
}
}
float d = denom0+denom1;
if (btFabs(d)>SIMD_EPSILON)
{
solverConstraint.m_jacDiagABInv = relaxation/(d);
} else
{
solverConstraint.m_jacDiagABInv = 1.f;
}
}
//compute rhs and remaining solverConstraint fields
btScalar restitution = 0.f;
btScalar penetration = isFriction? 0 : position+infoGlobal.m_linearSlop;
btScalar rel_vel = 0.f;
int ndofA = 0;
int ndofB = 0;
{
btVector3 vel1,vel2;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
} else
{
if (rb0)
{
rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
}
}
if (multiBodyB)
{
ndofB = multiBodyB->getNumLinks() + 6;
btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
} else
{
if (rb1)
{
rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
}
}
solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
restitution = restitution * -rel_vel;//restitutionCurve(rel_vel, cp.m_combinedRestitution);
if (restitution <= btScalar(0.))
{
restitution = 0.f;
};
}
///warm starting (or zero if disabled)
/*
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
if (solverConstraint.m_appliedImpulse)
{
if (multiBodyA)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->applyDeltaVee(deltaV,impulse);
applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
} else
{
if (rb0)
bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
}
if (multiBodyB)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
multiBodyB->applyDeltaVee(deltaV,impulse);
applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
} else
{
if (rb1)
bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
}
}
} else
*/
{
solverConstraint.m_appliedImpulse = 0.f;
}
solverConstraint.m_appliedPushImpulse = 0.f;
{
btScalar positionalError = 0.f;
btScalar velocityError = restitution - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
erp = infoGlobal.m_erp;
}
if (penetration>0)
{
positionalError = 0;
velocityError = -penetration / infoGlobal.m_timeStep;
} else
{
positionalError = -penetration * erp/infoGlobal.m_timeStep;
}
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
//combine position and velocity into rhs
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f;
} else
{
//split position and velocity into rhs and m_rhsPenetration
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhsPenetration = penetrationImpulse;
}
solverConstraint.m_cfm = 0.f;
solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e10f;
}
}

39
src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
View File

@ -33,6 +33,8 @@ struct btMultiBodyJacobianData
btAlignedObjectArray<btScalar> scratch_r;
btAlignedObjectArray<btVector3> scratch_v;
btAlignedObjectArray<btMatrix3x3> scratch_m;
btAlignedObjectArray<btSolverBody>* m_solverBodyPool;
};
@ -58,21 +60,32 @@ protected:
// positions. (one per row.)
btAlignedObjectArray<btScalar> m_data;
void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);
void fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint,
btMultiBodyJacobianData& data,
const btVector3& contactNormalOnB,
const btVector3& posAworld, const btVector3& posBworld,
btScalar position,
const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
btScalar fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow,
btMultiBodyJacobianData& data,
btScalar* jacOrgA,btScalar* jacOrgB,
const btContactSolverInfo& infoGlobal,
btScalar desiredVelocity,
btScalar lowerLimit,
btScalar upperLimit);
public:
btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
:m_bodyA(bodyA),
m_bodyB(bodyB),
m_linkA(linkA),
m_linkB(linkB),
m_num_rows(numRows),
m_isUnilateral(isUnilateral)
{
m_jac_size_A = (6 + bodyA->getNumLinks());
m_jac_size_both = (m_jac_size_A + (bodyB ? 6 + bodyB->getNumLinks() : 0));
m_pos_offset = ((1 + m_jac_size_both)*m_num_rows);
m_data.resize((2 + m_jac_size_both) * m_num_rows);
}
btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
virtual ~btMultiBodyConstraint();
virtual int getIslandIdA() const =0;
virtual int getIslandIdB() const =0;

1
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@ -764,6 +764,7 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
for (int i=0;i<m_tmpNumMultiBodyConstraints;i++)
{
btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i];
m_data.m_solverBodyPool = &m_tmpSolverBodyPool;
c->createConstraintRows(m_multiBodyNonContactConstraints,m_data, infoGlobal);
}

20
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@ -103,6 +103,18 @@ void btMultiBodyDynamicsWorld::calculateSimulationIslands()
}
}
//merge islands linked by multibody constraints
{
for (int i=0;i<this->m_multiBodyConstraints.size();i++)
{
btMultiBodyConstraint* c = m_multiBodyConstraints[i];
int tagA = c->getIslandIdA();
int tagB = c->getIslandIdB();
if (tagA>=0 && tagB>=0)
getSimulationIslandManager()->getUnionFind().unite(tagA, tagB);
}
}
//Store the island id in each body
getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
@ -353,7 +365,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher);
m_bodies.resize(0);
m_manifolds.resize(0);
@ -386,7 +398,6 @@ btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
btVector3 g = m_gravity;
btAlignedObjectArray<btScalar> scratch_r;
btAlignedObjectArray<btVector3> scratch_v;
btAlignedObjectArray<btMatrix3x3> scratch_m;
@ -408,6 +419,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
}
m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
@ -444,11 +456,11 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
scratch_m.resize(bod->getNumLinks()+1);
bod->clearForcesAndTorques();
bod->addBaseForce(g * bod->getBaseMass());
bod->addBaseForce(m_gravity * bod->getBaseMass());
for (int j = 0; j < bod->getNumLinks(); ++j)
{
bod->addLinkForce(j, g * bod->getLinkMass(j));
bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
}
bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);

284
src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
View File

@ -1,3 +1,19 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///This file was written by Erwin Coumans
#include "btMultiBodyJointLimitConstraint.h"
#include "btMultiBody.h"
@ -73,243 +89,45 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
constraintRow.m_multiBodyA = m_bodyA;
constraintRow.m_multiBodyB = m_bodyB;
btScalar penetration = getPosition(row);
fillConstraintRow(constraintRow,data,jacobianA(row),jacobianB(row),penetration,0,0,infoGlobal);
btScalar rel_vel = fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,0,0,1e30);
{
btScalar penetration = getPosition(row);
btScalar positionalError = 0.f;
btScalar velocityError = - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
erp = infoGlobal.m_erp;
}
if (penetration>0)
{
positionalError = 0;
velocityError = -penetration / infoGlobal.m_timeStep;
} else
{
positionalError = -penetration * erp/infoGlobal.m_timeStep;
}
btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
//combine position and velocity into rhs
constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
constraintRow.m_rhsPenetration = 0.f;
} else
{
//split position and velocity into rhs and m_rhsPenetration
constraintRow.m_rhs = velocityImpulse;
constraintRow.m_rhsPenetration = penetrationImpulse;
}
}
}
}
void btMultiBodyJointLimitConstraint::fillConstraintRow(btMultiBodySolverConstraint& constraintRow,
btMultiBodyJacobianData& data,
btScalar* jacOrgA,btScalar* jacOrgB,
btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
const btContactSolverInfo& infoGlobal)
{
btMultiBody* multiBodyA = constraintRow.m_multiBodyA;
btMultiBody* multiBodyB = constraintRow.m_multiBodyB;
if (multiBodyA)
{
const int ndofA = multiBodyA->getNumLinks() + 6;
constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId();
if (constraintRow.m_deltaVelAindex <0)
{
constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size();
multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
} else
{
btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA);
}
constraintRow.m_jacAindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
for (int i=0;i<ndofA;i++)
data.m_jacobians[constraintRow.m_jacAindex+i] = jacOrgA[i];
float* delta = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v);
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId();
if (constraintRow.m_deltaVelBindex <0)
{
constraintRow.m_deltaVelBindex = data.m_deltaVelocities.size();
multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
}
constraintRow.m_jacBindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
for (int i=0;i<ndofB;i++)
data.m_jacobians[constraintRow.m_jacBindex+i] = jacOrgB[i];
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
multiBodyB->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v);
}
{
btVector3 vec;
btScalar denom0 = 0.f;
btScalar denom1 = 0.f;
btScalar* jacB = 0;
btScalar* jacA = 0;
btScalar* lambdaA =0;
btScalar* lambdaB =0;
int ndofA = 0;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
jacA = &data.m_jacobians[constraintRow.m_jacAindex];
lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
{
float j = jacA[i] ;
float l =lambdaA[i];
denom0 += j*l;
}
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
jacB = &data.m_jacobians[constraintRow.m_jacBindex];
lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
{
float j = jacB[i] ;
float l =lambdaB[i];
denom1 += j*l;
}
}
if (multiBodyA && (multiBodyA==multiBodyB))
{
// ndof1 == ndof2 in this case
for (int i = 0; i < ndofA; ++i)
{
denom1 += jacB[i] * lambdaA[i];
denom1 += jacA[i] * lambdaB[i];
}
}
float d = denom0+denom1;
if (btFabs(d)>SIMD_EPSILON)
{
constraintRow.m_jacDiagABInv = 1.f/(d);
} else
{
constraintRow.m_jacDiagABInv = 1.f;
}
}
//compute rhs and remaining constraintRow fields
btScalar rel_vel = 0.f;
int ndofA = 0;
int ndofB = 0;
{
btVector3 vel1,vel2;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
btScalar* jacA = &data.m_jacobians[constraintRow.m_jacAindex];
for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
}
if (multiBodyB)
{
ndofB = multiBodyB->getNumLinks() + 6;
btScalar* jacB = &data.m_jacobians[constraintRow.m_jacBindex];
for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
}
constraintRow.m_friction = combinedFrictionCoeff;
}
/*
///warm starting (or zero if disabled)
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
constraintRow.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
if (constraintRow.m_appliedImpulse)
{
if (multiBodyA)
{
btScalar impulse = constraintRow.m_appliedImpulse;
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
multiBodyA->applyDeltaVee(deltaV,impulse);
applyDeltaVee(deltaV,impulse,constraintRow.m_deltaVelAindex,ndofA);
}
if (multiBodyB)
{
btScalar impulse = constraintRow.m_appliedImpulse;
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
multiBodyB->applyDeltaVee(deltaV,impulse);
applyDeltaVee(deltaV,impulse,constraintRow.m_deltaVelBindex,ndofB);
}
}
}
else
*/
{
constraintRow.m_appliedImpulse = 0.f;
}
constraintRow.m_appliedPushImpulse = 0.f;
{
float desiredVelocity = -0.3;
btScalar positionalError = 0.f;
btScalar velocityError = - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
erp = infoGlobal.m_erp;
}
if (penetration>0)
{
positionalError = 0;
velocityError = -penetration / infoGlobal.m_timeStep;
} else
{
positionalError = -penetration * erp/infoGlobal.m_timeStep;
}
btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
//combine position and velocity into rhs
constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
constraintRow.m_rhsPenetration = 0.f;
} else
{
//split position and velocity into rhs and m_rhsPenetration
constraintRow.m_rhs = velocityImpulse;
constraintRow.m_rhsPenetration = penetrationImpulse;
}
constraintRow.m_cfm = 0.f;
constraintRow.m_lowerLimit = 0;
constraintRow.m_upperLimit = 1e10f;
}
}

5
src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h
View File

@ -25,11 +25,6 @@ protected:
btScalar m_lowerBound;
btScalar m_upperBound;
void btMultiBodyJointLimitConstraint::fillConstraintRow(btMultiBodySolverConstraint& constraintRow,
btMultiBodyJacobianData& data,
btScalar* jacOrgA,btScalar* jacOrgB,
btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
const btContactSolverInfo& infoGlobal1);
public:
btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper);

89
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp Normal file
View File

@ -0,0 +1,89 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///This file was written by Erwin Coumans
#include "btMultiBodyJointMotor.h"
#include "btMultiBody.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
:btMultiBodyConstraint(body,body,link,link,1,true),
m_desiredVelocity(desiredVelocity),
m_maxMotorImpulse(maxMotorImpulse)
{
// the data.m_jacobians never change, so may as well
// initialize them here
// note: we rely on the fact that data.m_jacobians are
// always initialized to zero by the Constraint ctor
// row 0: the lower bound
jacobianA(0)[6 + link] = 1;
}
btMultiBodyJointMotor::~btMultiBodyJointMotor()
{
}
int btMultiBodyJointMotor::getIslandIdA() const
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
}
return -1;
}
int btMultiBodyJointMotor::getIslandIdB() const
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
{
col = m_bodyB->getLink(i).m_collider;
if (col)
return col->getIslandTag();
}
return -1;
}
void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
{
// only positions need to be updated -- data.m_jacobians and force
// directions were set in the ctor and never change.
for (int row=0;row<getNumRows();row++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
btScalar penetration = 0;
fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,m_desiredVelocity,-m_maxMotorImpulse,m_maxMotorImpulse);
}
}

47
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h Normal file
View File

@ -0,0 +1,47 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///This file was written by Erwin Coumans
#ifndef BT_MULTIBODY_JOINT_MOTOR_H
#define BT_MULTIBODY_JOINT_MOTOR_H
#include "btMultiBodyConstraint.h"
struct btSolverInfo;
class btMultiBodyJointMotor : public btMultiBodyConstraint
{
protected:
btScalar m_maxMotorImpulse;
btScalar m_desiredVelocity;
public:
btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse);
virtual ~btMultiBodyJointMotor();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
};
#endif //BT_MULTIBODY_JOINT_MOTOR_H

138
src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp Normal file
View File

@ -0,0 +1,138 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///This file was written by Erwin Coumans
#include "btMultiBodyPoint2Point.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
:btMultiBodyConstraint(body,0,link,-1,3,false),
m_rigidBodyA(0),
m_rigidBodyB(bodyB),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB),
m_maxAppliedImpulse(1e30f)
{
}
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,3,false),
m_rigidBodyA(0),
m_rigidBodyB(0),
m_pivotInA(pivotInA),
m_pivotInB(pivotInB),
m_maxAppliedImpulse(1e30f)
{
}
btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
{
}
int btMultiBodyPoint2Point::getIslandIdA() const
{
if (m_rigidBodyA)
return m_rigidBodyA->getIslandTag();
if (m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
}
}
return -1;
}
int btMultiBodyPoint2Point::getIslandIdB() const
{
if (m_rigidBodyB)
return m_rigidBodyB->getIslandTag();
if (m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
{
col = m_bodyB->getLink(i).m_collider;
if (col)
return col->getIslandTag();
}
}
return -1;
}
void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
{
// int i=1;
for (int i=0;i<3;i++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
constraintRow.m_solverBodyIdB = 0;
btVector3 contactNormalOnB(0,0,0);
contactNormalOnB[i] = -1;
btScalar penetration = 0;
// Convert local points back to world
btVector3 pivotAworld = m_pivotInA;
if (m_rigidBodyA)
{
pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
} else
{
if (m_bodyA)
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
}
btVector3 pivotBworld = m_pivotInB;
if (m_rigidBodyB)
{
pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
} else
{
if (m_bodyB)
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
}
btScalar position = (pivotAworld-pivotBworld).dot(contactNormalOnB);
btScalar relaxation = 1.f;
fillMultiBodyConstraintMixed(constraintRow, data,
contactNormalOnB,
pivotAworld, pivotBworld,
position,
infoGlobal,
relaxation,
false);
constraintRow.m_lowerLimit = -m_maxAppliedImpulse;
constraintRow.m_upperLimit = m_maxAppliedImpulse;
}
}

67
src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h Normal file
View File

@ -0,0 +1,67 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///This file was written by Erwin Coumans
#ifndef BT_MULTIBODY_POINT2POINT_H
#define BT_MULTIBODY_POINT2POINT_H
#include "btMultiBodyConstraint.h"
class btMultiBodyPoint2Point : public btMultiBodyConstraint
{
protected:
btRigidBody* m_rigidBodyA;
btRigidBody* m_rigidBodyB;
btVector3 m_pivotInA;
btVector3 m_pivotInB;
btScalar m_maxAppliedImpulse;
public:
btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB);
btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB);
virtual ~btMultiBodyPoint2Point();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
const btVector3& getPivotInB() const
{
return m_pivotInB;
}
void setPivotInB(const btVector3& pivotInB)
{
m_pivotInB = pivotInB;
}
btScalar getMaxAppliedImpulse() const
{
return m_maxAppliedImpulse;
}
void setMaxAppliedImpulse(btScalar maxImp)
{
m_maxAppliedImpulse = maxImp;
}
};
#endif //BT_MULTIBODY_POINT2POINT_H

9
src/Makefile.am
View File

@ -370,7 +370,12 @@ libBulletDynamics_la_SOURCES = \
BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h \
BulletDynamics/Featherstone/btMultiBodyLink.h \
BulletDynamics/Featherstone/btMultiBodyLinkCollider.h \
BulletDynamics/Featherstone/btMultiBodySolverConstraint.h
BulletDynamics/Featherstone/btMultiBodySolverConstraint.h \
BulletDynamics/Featherstone/btMultiBodyConstraint.h \
BulletDynamics/Featherstone/btMultiBodyPoint2Point.h \
BulletDynamics/Featherstone/btMultiBodyConstraint.cpp \
BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
libBulletSoftBody_la_SOURCES = \
BulletSoftBody/btDefaultSoftBodySolver.cpp \
@ -437,6 +442,8 @@ nobase_bullet_include_HEADERS += \
BulletDynamics/Featherstone/btMultiBodyLink.h \
BulletDynamics/Featherstone/btMultiBodyLinkCollider.h \
BulletDynamics/Featherstone/btMultiBodySolverConstraint.h \
BulletDynamics/Featherstone/btMultiBodyConstraint.h \
BulletDynamics/Featherstone/btMultiBodyPoint2Point.h \
BulletCollision/CollisionShapes/btShapeHull.h \
BulletCollision/CollisionShapes/btConcaveShape.h \
BulletCollision/CollisionShapes/btCollisionMargin.h \