TorqueGameEngines/Torque3D
1
0
Fork 0
mirror of https://github.com/TorqueGameEngines/Torque3D.git synced 2026-07-12 15:14:35 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

update bullet so it actually works

Browse source 
Moved the addSourceDirectory for physics/Bullet into the Engine/Source/CMakeLists.txt file that way it can actually appear where we expect it to in the solution explorer.
This commit is contained in:
marauder2k7 2026-06-03 15:08:51 +01:00
parent c7be48130a
commit 13fa178cf6
5986 changed files with 1811270 additions and 453803 deletions
Download patch file Download diff file Expand all files Collapse all files

510
Engine/lib/bullet/examples/MultiBody/InvertedPendulumPDControl.cpp
View file

@ -3,7 +3,7 @@
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
#include "../Utils/b3ResourcePath.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
@ -14,413 +14,380 @@ static btScalar maxForce = 100;
struct InvertedPendulumPDControl : public CommonMultiBodyBase
{
btMultiBody* m_multiBody;
btMultiBody* m_multiBody;
btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks;
bool m_once;
bool m_once;
int m_frameCount;
public:
InvertedPendulumPDControl(struct GUIHelperInterface* helper);
virtual ~InvertedPendulumPDControl();
InvertedPendulumPDControl(struct GUIHelperInterface* helper);
virtual ~InvertedPendulumPDControl();
virtual void initPhysics();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
virtual void stepSimulation(float deltaTime);
virtual void resetCamera()
{
float dist = 5;
float pitch = 270;
float yaw = 21;
float targetPos[3]={-1.34,1.4,3.44};
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
float pitch = -21;
float yaw = 270;
float targetPos[3] = {-1.34, 1.4, 3.44};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
};
InvertedPendulumPDControl::InvertedPendulumPDControl(struct GUIHelperInterface* helper)
:CommonMultiBodyBase(helper),
m_once(true),
m_frameCount(0)
: CommonMultiBodyBase(helper),
m_once(true),
m_frameCount(0)
{
}
InvertedPendulumPDControl::~InvertedPendulumPDControl()
{
}
///this is a temporary global, until we determine if we need the option or not
extern bool gJointFeedbackInWorldSpace;
extern bool gJointFeedbackInJointFrame;
btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GUIHelperInterface* guiHelper, const btTransform& baseWorldTrans, bool fixedBase)
{
btVector4 colors[4] =
{
btVector4(1,0,0,1),
btVector4(0,1,0,1),
btVector4(0,1,1,1),
btVector4(1,1,0,1),
};
int curColor = 0;
{
btVector4(1, 0, 0, 1),
btVector4(0, 1, 0, 1),
btVector4(0, 1, 1, 1),
btVector4(1, 1, 0, 1),
};
int curColor = 0;
bool damping = false;
bool gyro = false;
int numLinks = 2;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false;
bool selfCollide = false;
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 baseHalfExtents(0.04, 0.35, 0.08);
bool damping = false;
bool gyro = false;
int numLinks = 2;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false;
bool selfCollide = false;
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 baseHalfExtents(0.04, 0.35, 0.08);
//mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = fixedBase ? 0.f : 10.f;
//mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = fixedBase ? 0.f : 10.f;
if(baseMass)
{
//btCollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
btCollisionShape *shape = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
shape->calculateLocalInertia(baseMass, baseInertiaDiag);
delete shape;
}
if (baseMass)
{
//btCollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
btCollisionShape* shape = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
shape->calculateLocalInertia(baseMass, baseInertiaDiag);
delete shape;
}
btMultiBody* pMultiBody = new btMultiBody(numLinks, 0, baseInertiaDiag, fixedBase, canSleep);
btMultiBody *pMultiBody = new btMultiBody(numLinks, 0, baseInertiaDiag, fixedBase, canSleep);
pMultiBody->setBaseWorldTransform(baseWorldTrans);
btVector3 vel(0, 0, 0);
// pMultiBody->setBaseVel(vel);
btVector3 vel(0, 0, 0);
// pMultiBody->setBaseVel(vel);
//init the links
btVector3 hingeJointAxis(1, 0, 0);
//y-axis assumed up
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
//init the links
btVector3 hingeJointAxis(1, 0, 0);
//////
btScalar q0 = 1.f * SIMD_PI/ 180.f;
btQuaternion quat0(btVector3(1, 0, 0).normalized(), q0);
quat0.normalize();
/////
//y-axis assumed up
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
for(int i = 0; i < numLinks; ++i)
{
//////
btScalar q0 = 1.f * SIMD_PI / 180.f;
btQuaternion quat0(btVector3(1, 0, 0).normalized(), q0);
quat0.normalize();
/////
for (int i = 0; i < numLinks; ++i)
{
float linkMass = 1.f;
//if (i==3 || i==2)
// linkMass= 1000;
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
btCollisionShape* shape = 0;
if (i==0)
if (i == 0)
{
shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//
} else
shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2])); //
}
else
{
shape = new btSphereShape(radius);
}
shape->calculateLocalInertia(linkMass, linkInertiaDiag);
delete shape;
if(!spherical)
if (!spherical)
{
//pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
if (i==0)
{
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
hingeJointAxis,
parentComToCurrentPivot,
currentPivotToCurrentCom, false);
} else
{
btVector3 parentComToCurrentCom(0, -radius * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -radius, 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
//pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
parentComToCurrentPivot,
currentPivotToCurrentCom);
if (i == 0)
{
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
hingeJointAxis,
parentComToCurrentPivot,
currentPivotToCurrentCom, false);
}
else
{
btVector3 parentComToCurrentCom(0, -radius * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -radius, 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
parentComToCurrentPivot,
currentPivotToCurrentCom);
}
//pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false);
}
else
else
{
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
}
}
}
pMultiBody->finalizeMultiDof();
pMultiBody->finalizeMultiDof();
///
world->addMultiBody(pMultiBody);
btMultiBody* mbC = pMultiBody;
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
mbC->setUseGyroTerm(gyro);
//
if (!damping)
{
mbC->setLinearDamping(0.f);
mbC->setAngularDamping(0.f);
}
else
{
mbC->setLinearDamping(0.1f);
mbC->setAngularDamping(0.9f);
}
//
///
world->addMultiBody(pMultiBody);
btMultiBody* mbC = pMultiBody;
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
mbC->setUseGyroTerm(gyro);
//
if(!damping)
{
mbC->setLinearDamping(0.f);
mbC->setAngularDamping(0.f);
}else
{ mbC->setLinearDamping(0.1f);
mbC->setAngularDamping(0.9f);
}
//
//////////////////////////////////////////////
if(numLinks > 0)
{
btScalar q0 = 180.f * SIMD_PI/ 180.f;
if(!spherical)
//////////////////////////////////////////////
if (numLinks > 0)
{
btScalar q0 = 180.f * SIMD_PI / 180.f;
if (!spherical)
{
mbC->setJointPosMultiDof(0, &q0);
}
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
quat0.normalize();
mbC->setJointPosMultiDof(0, quat0);
}
}
///
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
quat0.normalize();
mbC->setJointPosMultiDof(0, quat0);
}
}
///
btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1);
btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1);
btAlignedObjectArray<btVector3> local_origin;
local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos();
double friction = 1;
{
btAlignedObjectArray<btVector3> local_origin;
local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos();
// double friction = 1;
{
// 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()};
// 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* shape = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); //new btSphereShape(baseHalfExtents[0]);
guiHelper->createCollisionShapeGraphicsObject(shape);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(shape);
if (1)
{
btCollisionShape* shape = new btBoxShape(btVector3(baseHalfExtents[0],baseHalfExtents[1],baseHalfExtents[2]));//new btSphereShape(baseHalfExtents[0]);
guiHelper->createCollisionShapeGraphicsObject(shape);
btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(shape);
btTransform tr;
tr.setIdentity();
//if we don't set the initial pose of the btCollisionObject, the simulator will do this
btTransform tr;
tr.setIdentity();
//if we don't set the initial pose of the btCollisionObject, the simulator will do this
//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
tr.setOrigin(local_origin[0]);
btQuaternion orn(btVector3(0,0,1),0.25*3.1415926538);
tr.setRotation(orn);
col->setWorldTransform(tr);
tr.setOrigin(local_origin[0]);
btQuaternion orn(btVector3(0, 0, 1), 0.25 * 3.1415926538);
tr.setRotation(orn);
col->setWorldTransform(tr);
bool isDynamic = (baseMass > 0 && !fixedBase);
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
world->addCollisionObject(col, collisionFilterGroup, collisionFilterMask); //, 2,1+2);
world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2);
btVector4 color(0.0, 0.0, 0.5, 1);
guiHelper->createCollisionObjectGraphicsObject(col, color);
btVector3 color(0.0,0.0,0.5);
guiHelper->createCollisionObjectGraphicsObject(col,color);
// col->setFriction(friction);
pMultiBody->setBaseCollider(col);
}
}
// col->setFriction(friction);
pMultiBody->setBaseCollider(col);
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
const int parent = pMultiBody->getParent(i);
world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1];
local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i)));
}
}
}
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
btVector3 posr = local_origin[i + 1];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
const btScalar 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* shape = 0;
for (int i=0; i < pMultiBody->getNumLinks(); ++i)
{
const int parent = pMultiBody->getParent(i);
world_to_local[i+1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent+1];
local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , pMultiBody->getRVector(i)));
}
for (int i=0; i < pMultiBody->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* shape =0;
if (i==0)
if (i == 0)
{
shape = new btBoxShape(btVector3(linkHalfExtents[0],linkHalfExtents[1],linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]);
} else
shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2])); //btSphereShape(linkHalfExtents[0]);
}
else
{
shape = new btSphereShape(radius);
}
guiHelper->createCollisionShapeGraphicsObject(shape);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
guiHelper->createCollisionShapeGraphicsObject(shape);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
col->setCollisionShape(shape);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
col->setWorldTransform(tr);
// col->setFriction(friction);
bool isDynamic = 1;//(linkMass > 0);
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
col->setCollisionShape(shape);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
// col->setFriction(friction);
bool isDynamic = 1; //(linkMass > 0);
int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
//if (i==0||i>numLinks-2)
{
world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//,2,1+2);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
guiHelper->createCollisionObjectGraphicsObject(col,color);
world->addCollisionObject(col, collisionFilterGroup, collisionFilterMask); //,2,1+2);
btVector4 color = colors[curColor];
curColor++;
curColor &= 3;
guiHelper->createCollisionObjectGraphicsObject(col, color);
pMultiBody->getLink(i).m_collider=col;
pMultiBody->getLink(i).m_collider = col;
}
}
}
return pMultiBody;
}
void InvertedPendulumPDControl::initPhysics()
{
{
SliderParams slider("Kp",&kp);
slider.m_minVal=-200;
slider.m_maxVal=200;
SliderParams slider("Kp", &kp);
slider.m_minVal = -200;
slider.m_maxVal = 200;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
}
{
SliderParams slider("Kd",&kd);
slider.m_minVal=-50;
slider.m_maxVal=50;
SliderParams slider("Kd", &kd);
slider.m_minVal = -50;
slider.m_maxVal = 50;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
}
{
SliderParams slider("max force",&maxForce);
slider.m_minVal=0;
slider.m_maxVal=100;
SliderParams slider("max force", &maxForce);
slider.m_minVal = 0;
slider.m_maxVal = 100;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
}
int upAxis = 1;
int upAxis = 1;
gJointFeedbackInWorldSpace = true;
gJointFeedbackInJointFrame = true;
m_guiHelper->setUpAxis(upAxis);
this->createEmptyDynamicsWorld();
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
//btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawWireframe
+btIDebugDraw::DBG_DrawContactPoints
+btIDebugDraw::DBG_DrawAabb
);//+btIDebugDraw::DBG_DrawConstraintLimits);
m_dynamicsWorld->setGravity(btVector3(0,-10,0));
m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace = true;
m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame = true;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
//btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb); //+btIDebugDraw::DBG_DrawConstraintLimits);
m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
btTransform baseWorldTrans;
baseWorldTrans.setIdentity();
baseWorldTrans.setOrigin(btVector3(1,2,3));
baseWorldTrans.setOrigin(btVector3(1, 2, 3));
m_multiBody = createInvertedPendulumMultiBody(m_dynamicsWorld, m_guiHelper, baseWorldTrans, true);
//for (int i=pMultiBody->getNumLinks()-1;i>=0;i--)//
for (int i=0;i<m_multiBody->getNumLinks();i++)
for (int i = 0; i < m_multiBody->getNumLinks(); i++)
{
btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback();
m_multiBody->getLink(i).m_jointFeedback = fb;
m_jointFeedbacks.push_back(fb);
//break;
}
}
char fileName[1024];
static btAlignedObjectArray<btScalar> qDesiredArray;
void InvertedPendulumPDControl::stepSimulation(float deltaTime)
{
static btScalar offset = -0.1*SIMD_PI;
static btScalar offset = -0.1 * SIMD_PI;
m_frameCount++;
if ((m_frameCount&0xff)==0 )
if ((m_frameCount & 0xff) == 0)
{
offset = -offset;
}
btScalar target= SIMD_PI+offset;
btScalar target = SIMD_PI + offset;
qDesiredArray.resize(0);
qDesiredArray.resize(m_multiBody->getNumLinks(),target);
qDesiredArray.resize(m_multiBody->getNumLinks(), target);
for (int joint = 0; joint<m_multiBody->getNumLinks();joint++)
for (int joint = 0; joint < m_multiBody->getNumLinks(); joint++)
{
int dof1 = 0;
btScalar qActual = m_multiBody->getJointPosMultiDof(joint)[dof1];
btScalar qdActual = m_multiBody->getJointVelMultiDof(joint)[dof1];
btScalar positionError = (qDesiredArray[joint]-qActual);
btScalar positionError = (qDesiredArray[joint] - qActual);
double desiredVelocity = 0;
btScalar velocityError = (desiredVelocity-qdActual);
btScalar force = kp * positionError + kd*velocityError;
btClamp(force,-maxForce,maxForce);
btScalar velocityError = (desiredVelocity - qdActual);
btScalar force = kp * positionError + kd * velocityError;
btClamp(force, -maxForce, maxForce);
m_multiBody->addJointTorque(joint, force);
}
if (m_frameCount==100)
if (m_frameCount == 100)
{
const char* gPngFileName = "pendulum";
if (gPngFileName)
{
{
//printf("gPngFileName=%s\n",gPngFileName);
//printf("gPngFileName=%s\n",gPngFileName);
sprintf(fileName,"%s%d.png",gPngFileName,m_frameCount);
b3Printf("Made screenshot %s",fileName);
sprintf(fileName, "%s%d.png", gPngFileName, m_frameCount);
b3Printf("Made screenshot %s", fileName);
this->m_guiHelper->getAppInterface()->dumpNextFrameToPng(fileName);
}
}
}
m_dynamicsWorld->stepSimulation(1./60.,0);//240,0);
m_dynamicsWorld->stepSimulation(1. / 60., 0); //240,0);
static int count = 0;
if ((count& 0x0f)==0)
if ((count & 0x0f) == 0)
{
#if 0
for (int i=0;i<m_jointFeedbacks.size();i++)
@ -443,23 +410,18 @@ void InvertedPendulumPDControl::stepSimulation(float deltaTime)
}
count++;
/*
b3Printf("base angvel = %f,%f,%f",m_multiBody->getBaseOmega()[0],
m_multiBody->getBaseOmega()[1],
m_multiBody->getBaseOmega()[2]
);
*/
btScalar jointVel =m_multiBody->getJointVel(0);
// b3Printf("child angvel = %f",jointVel);
// btScalar jointVel =m_multiBody->getJointVel(0);
// b3Printf("child angvel = %f",jointVel);
}
class CommonExampleInterface* InvertedPendulumPDControlCreateFunc(struct CommonExampleOptions& options)
class CommonExampleInterface* InvertedPendulumPDControlCreateFunc(struct CommonExampleOptions& options)
{
return new InvertedPendulumPDControl(options.m_guiHelper);
}

5
Engine/lib/bullet/examples/MultiBody/InvertedPendulumPDControl.h
View file

@ -1,7 +1,6 @@
#ifndef INVERTED_PENDULUM_PD_CONTROL_H
#define INVERTED_PENDULUM_PD_CONTROL_H
class CommonExampleInterface* InvertedPendulumPDControlCreateFunc(struct CommonExampleOptions& options);
#endif //INVERTED_PENDULUM_PD_CONTROL_H
class CommonExampleInterface* InvertedPendulumPDControlCreateFunc(struct CommonExampleOptions& options);
#endif //INVERTED_PENDULUM_PD_CONTROL_H

201
Engine/lib/bullet/examples/MultiBody/KinematicMultiBodyExample.cpp Normal file
View file

@ -0,0 +1,201 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2015 Google Inc. 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 "KinematicMultiBodyExample.h"
//#define USE_MOTIONSTATE 1
#include "btBulletDynamicsCommon.h"
#include "LinearMath/btTransformUtil.h"
#define ARRAY_SIZE_Y 5
#define ARRAY_SIZE_X 5
#define ARRAY_SIZE_Z 5
#include "LinearMath/btVector3.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "../OpenGLWindow/ShapeData.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
namespace {
void kinematicPreTickCallback(btDynamicsWorld* world, btScalar deltaTime)
{
btMultiBody* groundBody = (btMultiBody*)world->getWorldUserInfo();
btTransform predictedTrans;
btVector3 linearVelocity(0, 0, 0);
btVector3 angularVelocity(0, 0.1, 0);
btTransformUtil::integrateTransform(groundBody->getBaseWorldTransform(), linearVelocity, angularVelocity, deltaTime, predictedTrans);
groundBody->setBaseWorldTransform(predictedTrans);
groundBody->setBaseVel(linearVelocity);
groundBody->setBaseOmega(angularVelocity);
static float time = 0.0;
time += deltaTime;
double old_joint_pos = groundBody->getJointPos(0);
double joint_pos = 0.5 * sin(time * 3.0 - 0.3);
double joint_vel = (joint_pos - old_joint_pos) / deltaTime;
groundBody->setJointPosMultiDof(0, &joint_pos);
groundBody->setJointVelMultiDof(0, &joint_vel);
}
struct KinematicMultiBodyExample : public CommonMultiBodyBase
{
btMultiBody* m_groundBody;
KinematicMultiBodyExample(struct GUIHelperInterface* helper)
: CommonMultiBodyBase(helper),
m_groundBody(0)
{
}
virtual void stepSimulation(float deltaTime)
{
if (m_dynamicsWorld)
{
m_dynamicsWorld->stepSimulation(deltaTime);
}
}
virtual ~KinematicMultiBodyExample() {}
virtual void initPhysics();
void resetCamera()
{
float dist = 4;
float pitch = -30;
float yaw = 50;
float targetPos[3] = {0, 0, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
};
void KinematicMultiBodyExample::initPhysics()
{
m_guiHelper->setUpAxis(1);
createEmptyDynamicsWorld();
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
if (m_dynamicsWorld->getDebugDrawer())
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints);
///create a kinematic multibody
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(10.), btScalar(0.1), btScalar(10.)));
m_collisionShapes.push_back(groundShape);
btBoxShape* secondLevelShape = createBoxShape(btVector3(btScalar(0.5), btScalar(0.1), btScalar(0.5)));
m_collisionShapes.push_back(secondLevelShape);
{
bool floating = false;
int numLinks = 1;
bool canSleep = false;
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 1.f;
btVector3 secondLevelInertiaDiag(0.f, 0.f, 0.f);
float secondLevelMass = 0.1f;
if (baseMass)
{
btCollisionShape* pTempBox = new btBoxShape(btVector3(10, 10, 10));
pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
delete pTempBox;
}
if (secondLevelMass)
{
btCollisionShape* pTempBox = new btBoxShape(btVector3(0.5, 0.5, 0.5));
pTempBox->calculateLocalInertia(secondLevelMass, secondLevelInertiaDiag);
delete pTempBox;
}
btTransform startTransform;
startTransform.setIdentity();
m_groundBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
m_groundBody->setBasePos(startTransform.getOrigin());
m_groundBody->setWorldToBaseRot(startTransform.getRotation());
//init the child link - second level.
btVector3 hingeJointAxis(0, 1, 0);
m_groundBody->setupRevolute(0, secondLevelMass, secondLevelInertiaDiag, -1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, btVector3(0, 0.5, 0), btVector3(0, 0, 0), true);
m_groundBody->finalizeMultiDof();
m_dynamicsWorld->addMultiBody(m_groundBody);
// add collision geometries
bool isDynamic = false; // Kinematic is not treated as dynamic here.
int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(m_groundBody, -1);
col->setCollisionShape(groundShape);
m_dynamicsWorld->addCollisionObject(col, collisionFilterGroup, collisionFilterMask); //, 2,1+2);
m_groundBody->setBaseCollider(col);
m_groundBody->setBaseDynamicType(btCollisionObject::CF_KINEMATIC_OBJECT);
btMultiBodyLinkCollider* secondLevelCol = new btMultiBodyLinkCollider(m_groundBody, 0);
secondLevelCol->setCollisionShape(secondLevelShape);
m_dynamicsWorld->addCollisionObject(secondLevelCol, collisionFilterGroup, collisionFilterMask);
m_groundBody->getLink(0).m_collider = secondLevelCol;
m_groundBody->setLinkDynamicType(0, btCollisionObject::CF_KINEMATIC_OBJECT);
}
m_dynamicsWorld->setInternalTickCallback(kinematicPreTickCallback, m_groundBody, true);
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
btBoxShape* colShape = createBoxShape(btVector3(.1, .1, .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);
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(btVector3(
btScalar(0.2 * i),
btScalar(2 + .2 * k),
btScalar(0.2 * j)));
createRigidBody(mass, startTransform, colShape);
}
}
}
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
}
CommonExampleInterface* KinematicMultiBodyExampleCreateFunc(CommonExampleOptions& options)
{
return new KinematicMultiBodyExample(options.m_guiHelper);
}
B3_STANDALONE_EXAMPLE(KinematicMultiBodyExampleCreateFunc)

21
Engine/lib/bullet/examples/MultiBody/KinematicMultiBodyExample.h Normal file
View file

@ -0,0 +1,21 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2015 Google Inc. 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 KINEMATIC_MULTI_BODY_EXAMPLE_H
#define KINEMATIC_MULTI_BODY_EXAMPLE_H
class CommonExampleInterface* KinematicMultiBodyExampleCreateFunc(struct CommonExampleOptions& options);
#endif //KINEMATIC_MULTI_BODY_EXAMPLE_H

573
Engine/lib/bullet/examples/MultiBody/MultiBodyConstraintFeedback.cpp
View file

@ -3,429 +3,402 @@
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
static btScalar radius(0.2);
struct MultiBodyConstraintFeedbackSetup : public CommonMultiBodyBase
{
btMultiBody* m_multiBody;
btMultiBody* m_multiBody;
btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks;
btMultiBodyJointMotor* m_motor;
bool m_once;
bool m_once;
public:
MultiBodyConstraintFeedbackSetup(struct GUIHelperInterface* helper);
virtual ~MultiBodyConstraintFeedbackSetup();
MultiBodyConstraintFeedbackSetup(struct GUIHelperInterface* helper);
virtual ~MultiBodyConstraintFeedbackSetup();
virtual void initPhysics();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
virtual void stepSimulation(float deltaTime);
virtual void resetCamera()
{
float dist = 5;
float pitch = 270;
float yaw = 21;
float targetPos[3]={-1.34,3.4,-0.44};
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
float pitch = -21;
float yaw = 270;
float targetPos[3] = {-1.34, 3.4, -0.44};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
};
MultiBodyConstraintFeedbackSetup::MultiBodyConstraintFeedbackSetup(struct GUIHelperInterface* helper)
:CommonMultiBodyBase(helper),
m_motor(0),
m_once(true)
: CommonMultiBodyBase(helper),
m_motor(0),
m_once(true)
{
}
MultiBodyConstraintFeedbackSetup::~MultiBodyConstraintFeedbackSetup()
{
}
///this is a temporary global, until we determine if we need the option or not
extern bool gJointFeedbackInWorldSpace;
extern bool gJointFeedbackInJointFrame;
void MultiBodyConstraintFeedbackSetup::initPhysics()
{
int upAxis = 2;
gJointFeedbackInWorldSpace = true;
gJointFeedbackInJointFrame = true;
int upAxis = 2;
m_guiHelper->setUpAxis(upAxis);
btVector4 colors[4] =
{
btVector4(1,0,0,1),
btVector4(0,1,0,1),
btVector4(0,1,1,1),
btVector4(1,1,0,1),
};
int curColor = 0;
btVector4 colors[4] =
{
btVector4(1, 0, 0, 1),
btVector4(0, 1, 0, 1),
btVector4(0, 1, 1, 1),
btVector4(1, 1, 0, 1),
};
int curColor = 0;
this->createEmptyDynamicsWorld();
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
//btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawWireframe
+btIDebugDraw::DBG_DrawContactPoints
+btIDebugDraw::DBG_DrawAabb
);//+btIDebugDraw::DBG_DrawConstraintLimits);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
//btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb); //+btIDebugDraw::DBG_DrawConstraintLimits);
//create a static ground object
if (1)
{
btVector3 groundHalfExtents(10,10,0.2);
btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures();
m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace = true;
m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame = true;
//create a static ground object
if (1)
{
btVector3 groundHalfExtents(10, 10, 0.2);
btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures();
m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start;
start.setIdentity();
btVector3 groundOrigin(-0.4f, 3.f, 0.f);
//btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
groundOrigin[upAxis] -= .5;
groundOrigin[2] -= 0.6;
start.setOrigin(groundOrigin);
btQuaternion groundOrn(btVector3(0, 1, 0), 0.25 * SIMD_PI);
m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 groundOrigin(-0.4f, 3.f, 0.f);
btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
groundOrigin[upAxis] -=.5;
groundOrigin[2]-=0.6;
start.setOrigin(groundOrigin);
btQuaternion groundOrn(btVector3(0,1,0),0.25*SIMD_PI);
// start.setRotation(groundOrn);
btRigidBody* body = createRigidBody(0,start,box);
body->setFriction(0);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
m_guiHelper->createRigidBodyGraphicsObject(body,color);
}
btRigidBody* body = createRigidBody(0, start, box);
body->setFriction(0);
btVector4 color = colors[curColor];
curColor++;
curColor &= 3;
m_guiHelper->createRigidBodyGraphicsObject(body, color);
}
{
bool floating = false;
bool damping = false;
bool gyro = false;
int numLinks = 2;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false;
bool selfCollide = false;
btVector3 linkHalfExtents(0.05, 0.5, 0.1);
btVector3 baseHalfExtents(0.05, 0.5, 0.1);
{
bool floating = false;
bool damping = false;
bool gyro = false;
int numLinks = 2;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false;
bool selfCollide = false;
btVector3 linkHalfExtents(0.05, 0.5, 0.1);
btVector3 baseHalfExtents(0.05, 0.5, 0.1);
btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
//mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 0.01f;
btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
//mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 0.01f;
if(baseMass)
{
//btCollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
btCollisionShape *shape = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
shape->calculateLocalInertia(baseMass, baseInertiaDiag);
delete shape;
}
if (baseMass)
{
//btCollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
btCollisionShape* shape = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
shape->calculateLocalInertia(baseMass, baseInertiaDiag);
delete shape;
}
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
m_multiBody = pMultiBody;
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
// baseOriQuat.setEulerZYX(-.25*SIMD_PI,0,-1.75*SIMD_PI);
pMultiBody->setBasePos(basePosition);
pMultiBody->setWorldToBaseRot(baseOriQuat);
btVector3 vel(0, 0, 0);
// pMultiBody->setBaseVel(vel);
btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
//init the links
btVector3 hingeJointAxis(1, 0, 0);
//y-axis assumed up
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
m_multiBody = pMultiBody;
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
// baseOriQuat.setEulerZYX(-.25*SIMD_PI,0,-1.75*SIMD_PI);
pMultiBody->setBasePos(basePosition);
pMultiBody->setWorldToBaseRot(baseOriQuat);
btVector3 vel(0, 0, 0);
// pMultiBody->setBaseVel(vel);
//////
btScalar q0 = 0.f * SIMD_PI/ 180.f;
btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
quat0.normalize();
/////
//init the links
btVector3 hingeJointAxis(1, 0, 0);
for(int i = 0; i < numLinks; ++i)
{
float linkMass = i==0? 0.0001 : 1.f;
//y-axis assumed up
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
//////
btScalar q0 = 0.f * SIMD_PI / 180.f;
btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
quat0.normalize();
/////
for (int i = 0; i < numLinks; ++i)
{
float linkMass = i == 0 ? 0.0001 : 1.f;
//if (i==3 || i==2)
// linkMass= 1000;
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
btCollisionShape* shape = 0;
if (i==0)
if (i == 0)
{
shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//
} else
shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2])); //
}
else
{
shape = new btSphereShape(radius);
}
shape->calculateLocalInertia(linkMass, linkInertiaDiag);
delete shape;
if(!spherical)
if (!spherical)
{
//pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
if (i==0)
//pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
if (i == 0)
{
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
hingeJointAxis,
parentComToCurrentPivot,
currentPivotToCurrentCom, false);
} else
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
hingeJointAxis,
parentComToCurrentPivot,
currentPivotToCurrentCom, false);
}
else
{
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1], 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, 0, 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1], 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, 0, 0); //cur body's COM to cur body's PIV offset
//btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
parentComToCurrentPivot,
currentPivotToCurrentCom);
pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
parentComToCurrentPivot,
currentPivotToCurrentCom);
}
//pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false);
}
else
{
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
}
}
pMultiBody->finalizeMultiDof();
//pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false);
}
else
{
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
}
}
pMultiBody->finalizeMultiDof();
//for (int i=pMultiBody->getNumLinks()-1;i>=0;i--)//
for (int i=0;i<pMultiBody->getNumLinks();i++)
for (int i = 0; i < pMultiBody->getNumLinks(); i++)
{
btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback();
pMultiBody->getLink(i).m_jointFeedback = fb;
m_jointFeedbacks.push_back(fb);
//break;
}
btMultiBodyDynamicsWorld* world = m_dynamicsWorld;
btMultiBodyDynamicsWorld* world = m_dynamicsWorld;
///
world->addMultiBody(pMultiBody);
btMultiBody* mbC = pMultiBody;
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
mbC->setUseGyroTerm(gyro);
//
if(!damping)
{
mbC->setLinearDamping(0.f);
mbC->setAngularDamping(0.f);
}else
{ mbC->setLinearDamping(0.1f);
mbC->setAngularDamping(0.9f);
}
//
m_dynamicsWorld->setGravity(btVector3(0,0,-10));
///
world->addMultiBody(pMultiBody);
btMultiBody* mbC = pMultiBody;
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
mbC->setUseGyroTerm(gyro);
//
if (!damping)
{
mbC->setLinearDamping(0.f);
mbC->setAngularDamping(0.f);
}
else
{
mbC->setLinearDamping(0.1f);
mbC->setAngularDamping(0.9f);
}
//
m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
//////////////////////////////////////////////
if(0)//numLinks > 0)
{
btScalar q0 = 45.f * SIMD_PI/ 180.f;
if(!spherical)
//////////////////////////////////////////////
if (/* DISABLES CODE */ (0)) //numLinks > 0)
{
btScalar q0 = 45.f * SIMD_PI / 180.f;
if (!spherical)
{
mbC->setJointPosMultiDof(0, &q0);
mbC->setJointPosMultiDof(0, &q0);
}
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
quat0.normalize();
mbC->setJointPosMultiDof(0, quat0);
}
}
///
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
quat0.normalize();
mbC->setJointPosMultiDof(0, quat0);
}
}
///
btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1);
btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1);
btAlignedObjectArray<btVector3> local_origin;
local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos();
double friction = 1;
{
btAlignedObjectArray<btVector3> local_origin;
local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos();
// double friction = 1;
{
// 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()};
// 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* shape = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); //new btSphereShape(baseHalfExtents[0]);
m_guiHelper->createCollisionShapeGraphicsObject(shape);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(shape);
if (1)
{
btCollisionShape* shape = new btBoxShape(btVector3(baseHalfExtents[0],baseHalfExtents[1],baseHalfExtents[2]));//new btSphereShape(baseHalfExtents[0]);
m_guiHelper->createCollisionShapeGraphicsObject(shape);
btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(shape);
btTransform tr;
tr.setIdentity();
//if we don't set the initial pose of the btCollisionObject, the simulator will do this
btTransform tr;
tr.setIdentity();
//if we don't set the initial pose of the btCollisionObject, the simulator will do this
//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
tr.setOrigin(local_origin[0]);
btQuaternion orn(btVector3(0,0,1),0.25*3.1415926538);
tr.setRotation(orn);
col->setWorldTransform(tr);
tr.setOrigin(local_origin[0]);
btQuaternion orn(btVector3(0, 0, 1), 0.25 * 3.1415926538);
tr.setRotation(orn);
col->setWorldTransform(tr);
bool isDynamic = (baseMass > 0 && floating);
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
world->addCollisionObject(col, collisionFilterGroup, collisionFilterMask); //, 2,1+2);
world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2);
btVector3 color(0.0, 0.0, 0.5);
m_guiHelper->createCollisionObjectGraphicsObject(col, color);
btVector3 color(0.0,0.0,0.5);
m_guiHelper->createCollisionObjectGraphicsObject(col,color);
// col->setFriction(friction);
pMultiBody->setBaseCollider(col);
}
}
// col->setFriction(friction);
pMultiBody->setBaseCollider(col);
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
const int parent = pMultiBody->getParent(i);
world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1];
local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i)));
}
}
}
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
btVector3 posr = local_origin[i + 1];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
const btScalar 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* shape = 0;
for (int i=0; i < pMultiBody->getNumLinks(); ++i)
{
const int parent = pMultiBody->getParent(i);
world_to_local[i+1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent+1];
local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , pMultiBody->getRVector(i)));
}
for (int i=0; i < pMultiBody->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* shape =0;
if (i==0)
if (i == 0)
{
shape = new btBoxShape(btVector3(linkHalfExtents[0],linkHalfExtents[1],linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]);
} else
shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2])); //btSphereShape(linkHalfExtents[0]);
}
else
{
shape = new btSphereShape(radius);
}
m_guiHelper->createCollisionShapeGraphicsObject(shape);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
m_guiHelper->createCollisionShapeGraphicsObject(shape);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
col->setCollisionShape(shape);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
col->setWorldTransform(tr);
// col->setFriction(friction);
bool isDynamic = 1;//(linkMass > 0);
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
col->setCollisionShape(shape);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
// col->setFriction(friction);
bool isDynamic = 1; //(linkMass > 0);
int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
//if (i==0||i>numLinks-2)
{
world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//,2,1+2);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
m_guiHelper->createCollisionObjectGraphicsObject(col,color);
world->addCollisionObject(col, collisionFilterGroup, collisionFilterMask); //,2,1+2);
btVector4 color = colors[curColor];
curColor++;
curColor &= 3;
m_guiHelper->createCollisionObjectGraphicsObject(col, color);
pMultiBody->getLink(i).m_collider=col;
pMultiBody->getLink(i).m_collider = col;
}
}
int link=0;
int targetVelocity=0.f;
btScalar maxForce = 100000;
m_motor = new btMultiBodyJointMotor(pMultiBody,link,targetVelocity,maxForce);
m_dynamicsWorld->addMultiBodyConstraint(m_motor);
}
}
int link = 0;
int targetVelocity = 0.f;
btScalar maxForce = 100000;
m_motor = new btMultiBodyJointMotor(pMultiBody, link, targetVelocity, maxForce);
m_dynamicsWorld->addMultiBodyConstraint(m_motor);
}
}
void MultiBodyConstraintFeedbackSetup::stepSimulation(float deltaTime)
{
//m_multiBody->addLinkForce(0,btVector3(100,100,100));
if (0)//m_once)
{
m_once=false;
m_multiBody->addJointTorque(0, 10.0);
btScalar torque = m_multiBody->getJointTorque(0);
b3Printf("t = %f,%f,%f\n",torque,torque,torque);//[0],torque[1],torque[2]);
}
btScalar timeStep = 1./240.f;
m_dynamicsWorld->stepSimulation(timeStep,0);
if (/* DISABLES CODE */ (0)) //m_once)
{
m_once = false;
m_multiBody->addJointTorque(0, 10.0);
btScalar torque = m_multiBody->getJointTorque(0);
b3Printf("t = %f,%f,%f\n", torque, torque, torque); //[0],torque[1],torque[2]);
}
btScalar timeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(timeStep, 0);
static int count = 0;
if ((count& 0x0f)==0)
if ((count & 0x0f) == 0)
{
if (m_motor)
{
float force = m_motor->getAppliedImpulse(0)/timeStep;
b3Printf("motor applied force = %f\n", force);
}
if (m_motor)
{
float force = m_motor->getAppliedImpulse(0) / timeStep;
b3Printf("motor applied force = %f\n", force);
}
for (int i=0;i<m_jointFeedbacks.size();i++)
{
for (int i = 0; i < m_jointFeedbacks.size(); i++)
{
b3Printf("F_reaction[%i] linear:%f,%f,%f, angular:%f,%f,%f",
i,
m_jointFeedbacks[i]->m_reactionForces.m_topVec[0],
m_jointFeedbacks[i]->m_reactionForces.m_topVec[1],
m_jointFeedbacks[i]->m_reactionForces.m_topVec[2],
i,
m_jointFeedbacks[i]->m_reactionForces.m_topVec[0],
m_jointFeedbacks[i]->m_reactionForces.m_topVec[1],
m_jointFeedbacks[i]->m_reactionForces.m_topVec[2],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[0],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[1],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[2]
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[0],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[1],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[2]
);
}
);
}
}
count++;
/*
b3Printf("base angvel = %f,%f,%f",m_multiBody->getBaseOmega()[0],
m_multiBody->getBaseOmega()[1],
m_multiBody->getBaseOmega()[2]
);
*/
btScalar jointVel =m_multiBody->getJointVel(0);
// b3Printf("child angvel = %f",jointVel);
// btScalar jointVel =m_multiBody->getJointVel(0);
// b3Printf("child angvel = %f",jointVel);
}
class CommonExampleInterface* MultiBodyConstraintFeedbackCreateFunc(struct CommonExampleOptions& options)
class CommonExampleInterface* MultiBodyConstraintFeedbackCreateFunc(struct CommonExampleOptions& options)
{
return new MultiBodyConstraintFeedbackSetup(options.m_guiHelper);
}

5
Engine/lib/bullet/examples/MultiBody/MultiBodyConstraintFeedback.h
View file

@ -1,7 +1,6 @@
#ifndef MULTIBODY_CONSTRAINT_FEEDBACK_H
#define MULTIBODY_CONSTRAINT_FEEDBACK_H
class CommonExampleInterface* MultiBodyConstraintFeedbackCreateFunc(struct CommonExampleOptions& options);
#endif //MULTIBODY_CONSTRAINT_FEEDBACK_H
class CommonExampleInterface* MultiBodyConstraintFeedbackCreateFunc(struct CommonExampleOptions& options);
#endif //MULTIBODY_CONSTRAINT_FEEDBACK_H

169
Engine/lib/bullet/examples/MultiBody/MultiBodySoftContact.cpp
View file

@ -3,109 +3,92 @@
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
#include "../Utils/b3ResourcePath.h"
static btScalar radius(0.2);
//static btScalar radius(0.2);
struct MultiBodySoftContact : public CommonMultiBodyBase
{
btMultiBody* m_multiBody;
btMultiBody* m_multiBody;
btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks;
bool m_once;
bool m_once;
public:
MultiBodySoftContact(struct GUIHelperInterface* helper);
virtual ~MultiBodySoftContact();
MultiBodySoftContact(struct GUIHelperInterface* helper);
virtual ~MultiBodySoftContact();
virtual void initPhysics();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
virtual void stepSimulation(float deltaTime);
virtual void resetCamera()
{
float dist = 5;
float pitch = 270;
float yaw = 21;
float targetPos[3]={0,0,0};
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
float pitch = -21;
float yaw = 270;
float targetPos[3] = {0, 0, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
};
MultiBodySoftContact::MultiBodySoftContact(struct GUIHelperInterface* helper)
:CommonMultiBodyBase(helper),
m_once(true)
: CommonMultiBodyBase(helper),
m_once(true)
{
}
MultiBodySoftContact::~MultiBodySoftContact()
{
}
void MultiBodySoftContact::initPhysics()
{
int upAxis = 2;
int upAxis = 2;
m_guiHelper->setUpAxis(upAxis);
btVector4 colors[4] =
{
btVector4(1,0,0,1),
btVector4(0,1,0,1),
btVector4(0,1,1,1),
btVector4(1,1,0,1),
};
int curColor = 0;
btVector4 colors[4] =
{
btVector4(1, 0, 0, 1),
btVector4(0, 1, 0, 1),
btVector4(0, 1, 1, 1),
btVector4(1, 1, 0, 1),
};
int curColor = 0;
this->createEmptyDynamicsWorld();
m_dynamicsWorld->setGravity(btVector3(0,0,-10));
m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
//btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawWireframe
+btIDebugDraw::DBG_DrawContactPoints
+btIDebugDraw::DBG_DrawAabb
);//+btIDebugDraw::DBG_DrawConstraintLimits);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
//btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb); //+btIDebugDraw::DBG_DrawConstraintLimits);
//create a static ground object
if (1)
{
btVector3 groundHalfExtents(50, 50, 50);
btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures();
//create a static ground object
if (1)
{
btVector3 groundHalfExtents(50,50,50);
btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures();
m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 groundOrigin(0,0,-50.5);
start.setOrigin(groundOrigin);
m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start;
start.setIdentity();
btVector3 groundOrigin(0, 0, -50.5);
start.setOrigin(groundOrigin);
// start.setRotation(groundOrn);
btRigidBody* body = createRigidBody(0,start,box);
//setContactStiffnessAndDamping will enable compliant rigid body contact
body->setContactStiffnessAndDamping(300,10);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
m_guiHelper->createRigidBodyGraphicsObject(body,color);
btRigidBody* body = createRigidBody(0, start, box);
}
//setContactStiffnessAndDamping will enable compliant rigid body contact
body->setContactStiffnessAndDamping(300, 10);
btVector4 color = colors[curColor];
curColor++;
curColor &= 3;
m_guiHelper->createRigidBodyGraphicsObject(body, color);
}
{
btCollisionShape* childShape = new btSphereShape(btScalar(0.5));
@ -114,60 +97,52 @@ void MultiBodySoftContact::initPhysics()
btScalar mass = 1;
btVector3 baseInertiaDiag;
bool isFixed = (mass == 0);
childShape->calculateLocalInertia(mass,baseInertiaDiag);
btMultiBody *pMultiBody = new btMultiBody(0, 1, baseInertiaDiag, false, false);
btTransform startTrans;
startTrans.setIdentity();
startTrans.setOrigin(btVector3(0,0,3));
childShape->calculateLocalInertia(mass, baseInertiaDiag);
btMultiBody* pMultiBody = new btMultiBody(0, 1, baseInertiaDiag, false, false);
btTransform startTrans;
startTrans.setIdentity();
startTrans.setOrigin(btVector3(0, 0, 3));
pMultiBody->setBaseWorldTransform(startTrans);
pMultiBody->setBaseWorldTransform(startTrans);
btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(childShape);
pMultiBody->setBaseCollider(col);
bool isDynamic = (mass > 0 && !isFixed);
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
m_dynamicsWorld->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2);
bool isDynamic = (mass > 0 && !isFixed);
int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
m_dynamicsWorld->addCollisionObject(col, collisionFilterGroup, collisionFilterMask); //, 2,1+2);
pMultiBody->finalizeMultiDof();
m_dynamicsWorld->addMultiBody(pMultiBody);
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
pMultiBody->forwardKinematics(scratch_q,scratch_m);
pMultiBody->forwardKinematics(scratch_q, scratch_m);
btAlignedObjectArray<btQuaternion> world_to_local;
btAlignedObjectArray<btVector3> local_origin;
pMultiBody->updateCollisionObjectWorldTransforms(world_to_local,local_origin);
pMultiBody->updateCollisionObjectWorldTransforms(world_to_local, local_origin);
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void MultiBodySoftContact::stepSimulation(float deltaTime)
{
if (0)//m_once)
{
m_once=false;
m_multiBody->addJointTorque(0, 10.0);
btScalar torque = m_multiBody->getJointTorque(0);
b3Printf("t = %f,%f,%f\n",torque,torque,torque);//[0],torque[1],torque[2]);
}
m_dynamicsWorld->stepSimulation(deltaTime);
if (/* DISABLES CODE */ (0)) //m_once)
{
m_once = false;
m_multiBody->addJointTorque(0, 10.0);
btScalar torque = m_multiBody->getJointTorque(0);
b3Printf("t = %f,%f,%f\n", torque, torque, torque); //[0],torque[1],torque[2]);
}
m_dynamicsWorld->stepSimulation(deltaTime);
}
class CommonExampleInterface* MultiBodySoftContactCreateFunc(struct CommonExampleOptions& options)
class CommonExampleInterface* MultiBodySoftContactCreateFunc(struct CommonExampleOptions& options)
{
return new MultiBodySoftContact(options.m_guiHelper);
}

5
Engine/lib/bullet/examples/MultiBody/MultiBodySoftContact.h
View file

@ -1,7 +1,6 @@
#ifndef MULTI_BODY_SOFT_CONTACT_H
#define MULTI_BODY_SOFT_CONTACT_H
class CommonExampleInterface* MultiBodySoftContactCreateFunc(struct CommonExampleOptions& options);
#endif //MULTI_BODY_SOFT_CONTACT_H
class CommonExampleInterface* MultiBodySoftContactCreateFunc(struct CommonExampleOptions& options);
#endif //MULTI_BODY_SOFT_CONTACT_H

347
Engine/lib/bullet/examples/MultiBody/MultiDofDemo.cpp
View file

@ -2,8 +2,13 @@
#include "../OpenGLWindow/SimpleOpenGL3App.h"
#include "btBulletDynamicsCommon.h"
#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
#include "BulletDynamics/MLCPSolvers/btLemkeSolver.h"
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
#include "BulletDynamics/Featherstone/btMultiBody.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyLink.h"
@ -18,57 +23,49 @@
#include "../CommonInterfaces/CommonMultiBodyBase.h"
class MultiDofDemo : public CommonMultiBodyBase
{
public:
MultiDofDemo(GUIHelperInterface* helper);
virtual ~MultiDofDemo();
virtual void initPhysics();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
virtual void stepSimulation(float deltaTime);
virtual void resetCamera()
{
float dist = 1;
float pitch = 50;
float yaw = 35;
float targetPos[3]={-3,2.8,-2.5};
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
float pitch = -35;
float yaw = 50;
float targetPos[3] = {-3, 2.8, -2.5};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
btMultiBody* createFeatherstoneMultiBody_testMultiDof(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents, bool spherical = false, bool floating = false);
void addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBodyDynamicsWorld *pWorld, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents);
btMultiBody* createFeatherstoneMultiBody_testMultiDof(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool floating = false);
void addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
void addBoxes_testMultiDof();
};
static bool g_floatingBase = false;
static bool g_firstInit = true;
static float scaling = 0.4f;
static float friction = 1.;
static int g_constraintSolverType = 0;
#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)
#define MAX_PROXIES (ARRAY_SIZE_X * ARRAY_SIZE_Y * ARRAY_SIZE_Z + 1024)
#define START_POS_X -5
//#define START_POS_Y 12
#define START_POS_Y 2
#define START_POS_Z -3
MultiDofDemo::MultiDofDemo(GUIHelperInterface* helper)
:CommonMultiBodyBase(helper)
: CommonMultiBodyBase(helper)
{
m_guiHelper->setUpAxis(1);
}
@ -76,94 +73,123 @@ MultiDofDemo::~MultiDofDemo()
{
}
void MultiDofDemo::stepSimulation(float deltaTime)
void MultiDofDemo::stepSimulation(float deltaTime)
{
//use a smaller internal timestep, there are stability issues
float internalTimeStep = 1./240.f;
m_dynamicsWorld->stepSimulation(deltaTime,10,internalTimeStep);
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep);
}
void MultiDofDemo::initPhysics()
{
void MultiDofDemo::initPhysics()
{
m_guiHelper->setUpAxis(1);
if(g_firstInit)
if (g_firstInit)
{
m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraDistance(btScalar(10.*scaling));
m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraDistance(btScalar(10. * scaling));
m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraPitch(50);
g_firstInit = false;
}
}
///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_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
//Use the btMultiBodyConstraintSolver for Featherstone btMultiBody support
btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver;
if (g_constraintSolverType == 4)
{
g_constraintSolverType = 0;
g_floatingBase = !g_floatingBase;
}
btMultiBodyConstraintSolver* sol;
btMLCPSolverInterface* mlcp;
switch (g_constraintSolverType++)
{
case 0:
sol = new btMultiBodyConstraintSolver;
b3Printf("Constraint Solver: Sequential Impulse");
break;
case 1:
mlcp = new btSolveProjectedGaussSeidel();
sol = new btMultiBodyMLCPConstraintSolver(mlcp);
b3Printf("Constraint Solver: MLCP + PGS");
break;
case 2:
mlcp = new btDantzigSolver();
sol = new btMultiBodyMLCPConstraintSolver(mlcp);
b3Printf("Constraint Solver: MLCP + Dantzig");
break;
default:
mlcp = new btLemkeSolver();
sol = new btMultiBodyMLCPConstraintSolver(mlcp);
b3Printf("Constraint Solver: MLCP + Lemke");
break;
}
m_solver = sol;
//use btMultiBodyDynamicsWorld for Featherstone btMultiBody support
btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher,m_broadphase,sol,m_collisionConfiguration);
btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration);
m_dynamicsWorld = world;
// m_dynamicsWorld->setDebugDrawer(&gDebugDraw);
// m_dynamicsWorld->setDebugDrawer(&gDebugDraw);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->setGravity(btVector3(0,-10,0));
m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
m_dynamicsWorld->getSolverInfo().m_globalCfm = 1e-3;
///create a few basic rigid bodies
btVector3 groundHalfExtents(50,50,50);
btVector3 groundHalfExtents(50, 50, 50);
btCollisionShape* groundShape = new btBoxShape(groundHalfExtents);
//groundShape->initializePolyhedralFeatures();
// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-50,00));
groundTransform.setOrigin(btVector3(0, -50, 00));
/////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////
bool damping = true;
bool gyro = true;
int numLinks = 5;
bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool multibodyOnly = false;
bool canSleep = true;
bool selfCollide = false;
bool multibodyConstraint = true;
bool canSleep = false;
bool selfCollide = true;
bool multibodyConstraint = false;
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(world, numLinks, btVector3(-0.4f, 3.f, 0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase);
//mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(world, numLinks, btVector3(-0.4f, 3.f, 0.f), baseHalfExtents, linkHalfExtents, spherical, g_floatingBase);
//mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
g_floatingBase = ! g_floatingBase;
mbC->setCanSleep(canSleep);
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
mbC->setUseGyroTerm(gyro);
//
if(!damping)
if (!damping)
{
mbC->setLinearDamping(0.f);
mbC->setAngularDamping(0.f);
}else
{ mbC->setLinearDamping(0.1f);
}
else
{
mbC->setLinearDamping(0.1f);
mbC->setAngularDamping(0.9f);
}
//
m_dynamicsWorld->setGravity(btVector3(0, -9.81 ,0));
m_dynamicsWorld->setGravity(btVector3(0, -9.81, 0));
//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
//////////////////////////////////////////////
if(numLinks > 0)
{
btScalar q0 = 45.f * SIMD_PI/ 180.f;
if(!spherical)
if (numLinks > 0)
{
btScalar q0 = 45.f * SIMD_PI / 180.f;
if (!spherical)
{
mbC->setJointPosMultiDof(0, &q0);
}
@ -171,14 +197,13 @@ void MultiDofDemo::initPhysics()
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
quat0.normalize();
mbC->setJointPosMultiDof(0, quat0);
}
}
mbC->setJointPosMultiDof(0, quat0);
}
}
///
addColliders_testMultiDof(mbC, world, baseHalfExtents, linkHalfExtents);
addColliders_testMultiDof(mbC, world, baseHalfExtents, linkHalfExtents);
/////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////
btScalar groundHeight = -51.55;
if (!multibodyOnly)
{
@ -187,28 +212,24 @@ void MultiDofDemo::initPhysics()
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
btVector3 localInertia(0, 0, 0);
if (isDynamic)
groundShape->calculateLocalInertia(mass,localInertia);
groundShape->calculateLocalInertia(mass, localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,groundHeight,0));
groundTransform.setOrigin(btVector3(0, groundHeight, 0));
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
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);
m_dynamicsWorld->addRigidBody(body, 1, 1 + 2); //,1,1+2);
}
/////////////////////////////////////////////////////////////////
if(!multibodyOnly)
if (!multibodyOnly)
{
btVector3 halfExtents(.5,.5,.5);
btVector3 halfExtents(.5, .5, .5);
btBoxShape* colShape = new btBoxShape(halfExtents);
//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
m_collisionShapes.push_back(colShape);
@ -217,41 +238,41 @@ void MultiDofDemo::initPhysics()
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
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);
btVector3 localInertia(0, 0, 0);
if (isDynamic)
colShape->calculateLocalInertia(mass,localInertia);
colShape->calculateLocalInertia(mass, localInertia);
startTransform.setOrigin(btVector3(
btScalar(0.0),
0.0,
btScalar(0.0)));
btScalar(0.0),
0.0,
btScalar(0.0)));
//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::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, colShape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
m_dynamicsWorld->addRigidBody(body);//,1,1+2);
if (multibodyConstraint) {
btVector3 pointInA = -linkHalfExtents;
btVector3 pointInB = halfExtents;
btMatrix3x3 frameInA;
btMatrix3x3 frameInB;
frameInA.setIdentity();
frameInB.setIdentity();
btVector3 jointAxis(1.0,0.0,0.0);
btMultiBodySliderConstraint* p2p = new btMultiBodySliderConstraint(mbC,numLinks-1,body,pointInA,pointInB,frameInA,frameInB,jointAxis);
//btMultiBodyFixedConstraint* p2p = new btMultiBodyFixedConstraint(mbC,numLinks-1,mbC,numLinks-4,pointInA,pointInA,frameInA,frameInB);
p2p->setMaxAppliedImpulse(2.0);
m_dynamicsWorld->addMultiBodyConstraint(p2p);
}
m_dynamicsWorld->addRigidBody(body); //,1,1+2);
if (multibodyConstraint)
{
btVector3 pointInA = -linkHalfExtents;
// btVector3 pointInB = halfExtents;
btMatrix3x3 frameInA;
btMatrix3x3 frameInB;
frameInA.setIdentity();
frameInB.setIdentity();
btVector3 jointAxis(1.0, 0.0, 0.0);
//btMultiBodySliderConstraint* p2p = new btMultiBodySliderConstraint(mbC,numLinks-1,body,pointInA,pointInB,frameInA,frameInB,jointAxis);
btMultiBodyFixedConstraint* p2p = new btMultiBodyFixedConstraint(mbC, numLinks - 1, mbC, numLinks - 4, pointInA, pointInA, frameInA, frameInB);
p2p->setMaxAppliedImpulse(2.0);
m_dynamicsWorld->addMultiBodyConstraint(p2p);
}
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
@ -259,57 +280,56 @@ void MultiDofDemo::initPhysics()
/////////////////////////////////////////////////////////////////
}
btMultiBody* MultiDofDemo::createFeatherstoneMultiBody_testMultiDof(btMultiBodyDynamicsWorld *pWorld, int numLinks, const btVector3 &basePosition, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents, bool spherical, bool floating)
btMultiBody* MultiDofDemo::createFeatherstoneMultiBody_testMultiDof(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool floating)
{
//init the base
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 1.f;
if(baseMass)
if (baseMass)
{
btCollisionShape *pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
delete pTempBox;
}
bool canSleep = false;
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
pMultiBody->setBasePos(basePosition);
pMultiBody->setWorldToBaseRot(baseOriQuat);
btVector3 vel(0, 0, 0);
// pMultiBody->setBaseVel(vel);
// pMultiBody->setBaseVel(vel);
//init the links
//init the links
btVector3 hingeJointAxis(1, 0, 0);
float linkMass = 1.f;
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
btCollisionShape *pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
delete pTempBox;
//y-axis assumed up
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
//////
btScalar q0 = 0.f * SIMD_PI/ 180.f;
btScalar q0 = 0.f * SIMD_PI / 180.f;
btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
quat0.normalize();
quat0.normalize();
/////
for(int i = 0; i < numLinks; ++i)
for (int i = 0; i < numLinks; ++i)
{
if(!spherical)
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
if (!spherical)
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true);
else
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true);
}
pMultiBody->finalizeMultiDof();
@ -320,9 +340,8 @@ btMultiBody* MultiDofDemo::createFeatherstoneMultiBody_testMultiDof(btMultiBodyD
return pMultiBody;
}
void MultiDofDemo::addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBodyDynamicsWorld *pWorld, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents)
{
void MultiDofDemo::addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents)
{
btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1);
@ -330,51 +349,43 @@ void MultiDofDemo::addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBod
local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos();
{
// float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
{
// float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
btScalar 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(baseHalfExtents);
btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1);
btCollisionShape* box = new btBoxShape(baseHalfExtents);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(local_origin[0]);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
pWorld->addCollisionObject(col, 2,1+2);
pWorld->addCollisionObject(col, 2, 1 + 2);
col->setFriction(friction);
pMultiBody->setBaseCollider(col);
}
}
for (int i=0; i < pMultiBody->getNumLinks(); ++i)
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
const int parent = pMultiBody->getParent(i);
world_to_local[i+1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent+1];
local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , pMultiBody->getRVector(i)));
world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1];
local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i)));
}
for (int i=0; i < pMultiBody->getNumLinks(); ++i)
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
btVector3 posr = local_origin[i+1];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
btScalar 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()};
btVector3 posr = local_origin[i + 1];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
btScalar 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(linkHalfExtents);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
@ -383,13 +394,12 @@ void MultiDofDemo::addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBod
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
col->setFriction(friction);
pWorld->addCollisionObject(col,2,1+2);
pMultiBody->getLink(i).m_collider=col;
pWorld->addCollisionObject(col, 2, 1 + 2);
pMultiBody->getLink(i).m_collider = col;
}
}
@ -398,7 +408,7 @@ void MultiDofDemo::addBoxes_testMultiDof()
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
btBoxShape* colShape = new btBoxShape(btVector3(1,1,1));
btBoxShape* colShape = new btBoxShape(btVector3(1, 1, 1));
//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
m_collisionShapes.push_back(colShape);
@ -406,43 +416,42 @@ void MultiDofDemo::addBoxes_testMultiDof()
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
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);
btVector3 localInertia(0, 0, 0);
if (isDynamic)
colShape->calculateLocalInertia(mass,localInertia);
colShape->calculateLocalInertia(mass, localInertia);
float start_x = START_POS_X - ARRAY_SIZE_X/2;
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;
float start_z = START_POS_Z - ARRAY_SIZE_Z / 2;
for (int k=0;k<ARRAY_SIZE_Y;k++)
for (int k = 0; k < ARRAY_SIZE_Y; k++)
{
for (int i=0;i<ARRAY_SIZE_X;i++)
for (int i = 0; i < ARRAY_SIZE_X; i++)
{
for(int j = 0;j<ARRAY_SIZE_Z;j++)
for (int j = 0; j < ARRAY_SIZE_Z; j++)
{
startTransform.setOrigin(btVector3(
btScalar(3.0*i + start_x),
btScalar(3.0*k + start_y),
btScalar(3.0*j + start_z)));
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::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, colShape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
m_dynamicsWorld->addRigidBody(body);//,1,1+2);
m_dynamicsWorld->addRigidBody(body); //,1,1+2);
}
}
}
}
class CommonExampleInterface* MultiDofCreateFunc(struct CommonExampleOptions& options)
class CommonExampleInterface* MultiDofCreateFunc(struct CommonExampleOptions& options)
{
return new MultiDofDemo(options.m_guiHelper);
}
}

5
Engine/lib/bullet/examples/MultiBody/MultiDofDemo.h
View file

@ -2,7 +2,6 @@
#ifndef MULTI_DOF_DEMO_H
#define MULTI_DOF_DEMO_H
class CommonExampleInterface* MultiDofCreateFunc(struct CommonExampleOptions& options);
#endif //MULTI_DOF_DEMO_H
class CommonExampleInterface* MultiDofCreateFunc(struct CommonExampleOptions& options);
#endif //MULTI_DOF_DEMO_H

147
Engine/lib/bullet/examples/MultiBody/Pendulum.cpp
View file

@ -14,7 +14,7 @@ subject to the following restrictions:
*/
///Original author: Erwin Coumans, January 2016
///Compare the simulation of a pendulum with
///Compare the simulation of a pendulum with
#ifdef USE_GTEST
#include <gtest/gtest.h>
@ -30,9 +30,8 @@ struct Pendulum : public CommonMultiBodyBase
{
btMultiBody* m_multiBody;
btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks;
public:
Pendulum(struct GUIHelperInterface* helper);
virtual ~Pendulum();
virtual void initPhysics();
@ -40,17 +39,15 @@ public:
virtual void resetCamera()
{
float dist = 5;
float pitch = 270;
float yaw = 21;
float targetPos[3]={0,0,0};
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
float pitch = -21;
float yaw = 270;
float targetPos[3] = {0, 0, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
};
Pendulum::Pendulum(struct GUIHelperInterface* helper)
:CommonMultiBodyBase(helper)
: CommonMultiBodyBase(helper)
{
}
@ -58,24 +55,19 @@ Pendulum::~Pendulum()
{
}
void Pendulum::initPhysics()
{
int upAxis = 1;
m_guiHelper->setUpAxis(upAxis);
this->createEmptyDynamicsWorld();
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
if (m_dynamicsWorld->getDebugDrawer())
{
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
//btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawWireframe
+btIDebugDraw::DBG_DrawContactPoints
+btIDebugDraw::DBG_DrawAabb
);//+btIDebugDraw::DBG_DrawConstraintLimits);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
//btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb); //+btIDebugDraw::DBG_DrawConstraintLimits);
}
{
bool floating = false;
@ -86,24 +78,24 @@ void Pendulum::initPhysics()
bool selfCollide = false;
btVector3 linkHalfExtents(0.05, 0.5, 0.1);
btVector3 baseHalfExtents(0.05, 0.5, 0.1);
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 0.f;
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
//pMultiBody->useRK4Integration(true);
m_multiBody = pMultiBody;
pMultiBody->setBaseWorldTransform(btTransform::getIdentity());
//init the links
btVector3 hingeJointAxis(1, 0, 0);
//y-axis assumed up
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] , 0);
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0);
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1], 0);
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0);
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom;
for(int i = 0; i < numLinks; ++i)
for (int i = 0; i < numLinks; ++i)
{
float linkMass = 10.f;
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
@ -113,114 +105,109 @@ void Pendulum::initPhysics()
}
shape->calculateLocalInertia(linkMass, linkInertiaDiag);
delete shape;
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
hingeJointAxis,
parentComToCurrentPivot,
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
hingeJointAxis,
parentComToCurrentPivot,
currentPivotToCurrentCom, false);
}
pMultiBody->finalizeMultiDof();
btMultiBodyDynamicsWorld* world = m_dynamicsWorld;
world->addMultiBody(pMultiBody);
pMultiBody->setCanSleep(canSleep);
pMultiBody->setHasSelfCollision(selfCollide);
pMultiBody->setUseGyroTerm(gyro);
//
if(!damping)
if (!damping)
{
pMultiBody->setLinearDamping(0.f);
pMultiBody->setAngularDamping(0.f);
}else
{ pMultiBody->setLinearDamping(0.1f);
}
else
{
pMultiBody->setLinearDamping(0.1f);
pMultiBody->setAngularDamping(0.9f);
}
m_dynamicsWorld->setGravity(btVector3(0,-9.81,0));
for (int i=0; i < pMultiBody->getNumLinks(); ++i)
m_dynamicsWorld->setGravity(btVector3(0, -9.81, 0));
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
btCollisionShape* shape =new btSphereShape(radius);
btCollisionShape* shape = new btSphereShape(radius);
m_guiHelper->createCollisionShapeGraphicsObject(shape);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
col->setCollisionShape(shape);
bool isDynamic = 1;
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//,2,1+2);
btVector4 color(1,0,0,1);
m_guiHelper->createCollisionObjectGraphicsObject(col,color);
pMultiBody->getLink(i).m_collider=col;
int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
world->addCollisionObject(col, collisionFilterGroup, collisionFilterMask); //,2,1+2);
btVector4 color(1, 0, 0, 1);
m_guiHelper->createCollisionObjectGraphicsObject(col, color);
pMultiBody->getLink(i).m_collider = col;
}
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
pMultiBody->forwardKinematics(scratch_q,scratch_m);
pMultiBody->forwardKinematics(scratch_q, scratch_m);
btAlignedObjectArray<btQuaternion> world_to_local;
btAlignedObjectArray<btVector3> local_origin;
pMultiBody->updateCollisionObjectWorldTransforms(world_to_local,local_origin);
pMultiBody->updateCollisionObjectWorldTransforms(world_to_local, local_origin);
}
}
void Pendulum::stepSimulation(float deltaTime)
{
m_multiBody->addJointTorque(0, 20.0);
#ifdef USE_GTEST
m_dynamicsWorld->stepSimulation(1./1000.0,0);
#ifdef USE_GTEST
m_dynamicsWorld->stepSimulation(1. / 1000.0, 0);
#else
m_dynamicsWorld->stepSimulation(deltaTime);
m_dynamicsWorld->stepSimulation(deltaTime);
#endif
btVector3 from = m_multiBody->getBaseWorldTransform().getOrigin();
btVector3 to = m_multiBody->getLink(0).m_collider->getWorldTransform().getOrigin();
btVector4 color(1,0,0,1);
btVector4 color(1, 0, 0, 1);
if (m_guiHelper->getRenderInterface())
{
m_guiHelper->getRenderInterface()->drawLine(from,to,color,btScalar(1));
m_guiHelper->getRenderInterface()->drawLine(from, to, color, btScalar(1));
}
}
#ifdef USE_GTEST
TEST(BulletDynamicsTest, pendulum)
TEST(BulletDynamicsTest, pendulum)
{
DummyGUIHelper noGfx;
Pendulum* setup = new Pendulum(&noGfx);
setup->initPhysics();
int numGoldValues = sizeof(sPendulumGold)/sizeof(float);
for (int i=0;i<2000;i++)
int numGoldValues = sizeof(sPendulumGold) / sizeof(float);
for (int i = 0; i < 2000; i++)
{
setup->stepSimulation(0.001);
int index = i*2+1;
ASSERT_LE(index,numGoldValues);
ASSERT_NEAR(setup->m_multiBody->getJointPos(0),sPendulumGold[index],0.005);
int index = i * 2 + 1;
ASSERT_LE(index, numGoldValues);
ASSERT_NEAR(setup->m_multiBody->getJointPos(0), sPendulumGold[index], 0.005);
}
setup->exitPhysics();
delete setup;
}
int main(int argc, char **argv) {
int main(int argc, char** argv)
{
#if _MSC_VER
_CrtSetDbgFlag ( _CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF );
//void *testWhetherMemoryLeakDetectionWorks = malloc(1);
_CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF);
//void *testWhetherMemoryLeakDetectionWorks = malloc(1);
#endif
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
#endif //USE_GTEST
#endif //USE_GTEST
class CommonExampleInterface* TestPendulumCreateFunc(struct CommonExampleOptions& options)
class CommonExampleInterface* TestPendulumCreateFunc(struct CommonExampleOptions& options)
{
return new Pendulum(options.m_guiHelper);
}

5
Engine/lib/bullet/examples/MultiBody/Pendulum.h
View file

@ -1,7 +1,6 @@
#ifndef TEST_PENDULUM_H
#define TEST_PENDULUM_H
class CommonExampleInterface* TestPendulumCreateFunc(struct CommonExampleOptions& options);
#endif //TEST_PENDULUM_H
class CommonExampleInterface* TestPendulumCreateFunc(struct CommonExampleOptions& options);
#endif //TEST_PENDULUM_H

393
Engine/lib/bullet/examples/MultiBody/SerialChains.cpp Normal file
View file

@ -0,0 +1,393 @@
#include "SerialChains.h"
#include "../OpenGLWindow/SimpleOpenGL3App.h"
#include "btBulletDynamicsCommon.h"
#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
#include "BulletDynamics/Featherstone/btMultiBody.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#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 "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
#include "../OpenGLWindow/GLInstancingRenderer.h"
#include "BulletCollision/CollisionShapes/btShapeHull.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
class SerialChains : public CommonMultiBodyBase
{
public:
SerialChains(GUIHelperInterface* helper);
virtual ~SerialChains();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
virtual void resetCamera()
{
float dist = 1;
float pitch = -35;
float yaw = 50;
float targetPos[3] = {-3, 2.8, -2.5};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool fixedBase = false);
void createGround(const btVector3& halfExtents = btVector3(50, 50, 50), btScalar zOffSet = btScalar(-1.55));
void addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
};
static bool g_fixedBase = true;
static bool g_firstInit = true;
static float scaling = 0.4f;
static float friction = 1.;
static int g_constraintSolverType = 0;
SerialChains::SerialChains(GUIHelperInterface* helper)
: CommonMultiBodyBase(helper)
{
m_guiHelper->setUpAxis(1);
}
SerialChains::~SerialChains()
{
// Do nothing
}
void SerialChains::stepSimulation(float deltaTime)
{
//use a smaller internal timestep, there are stability issues
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep);
}
void SerialChains::initPhysics()
{
m_guiHelper->setUpAxis(1);
if (g_firstInit)
{
m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraDistance(btScalar(10. * scaling));
m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraPitch(50);
g_firstInit = false;
}
///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();
if (g_constraintSolverType == 3)
{
g_constraintSolverType = 0;
g_fixedBase = !g_fixedBase;
}
btMLCPSolverInterface* mlcp;
switch (g_constraintSolverType++)
{
case 0:
m_solver = new btMultiBodyConstraintSolver;
b3Printf("Constraint Solver: Sequential Impulse");
break;
case 1:
mlcp = new btSolveProjectedGaussSeidel();
m_solver = new btMultiBodyMLCPConstraintSolver(mlcp);
b3Printf("Constraint Solver: MLCP + PGS");
break;
default:
mlcp = new btDantzigSolver();
m_solver = new btMultiBodyMLCPConstraintSolver(mlcp);
b3Printf("Constraint Solver: MLCP + Dantzig");
break;
}
btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
m_dynamicsWorld = world;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-4); //todo: what value is good?
///create a few basic rigid bodies
btVector3 groundHalfExtents(50, 50, 50);
btCollisionShape* groundShape = new btBoxShape(groundHalfExtents);
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -50, 00));
/////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////
bool damping = true;
bool gyro = true;
int numLinks = 5;
bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool multibodyOnly = true; //false
bool canSleep = true;
bool selfCollide = true;
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
btMultiBody* mbC1 = createFeatherstoneMultiBody(world, numLinks, btVector3(-0.4f, 3.f, 0.f), linkHalfExtents, baseHalfExtents, spherical, g_fixedBase);
btMultiBody* mbC2 = createFeatherstoneMultiBody(world, numLinks, btVector3(-0.4f, 3.0f, 0.5f), linkHalfExtents, baseHalfExtents, spherical, g_fixedBase);
mbC1->setCanSleep(canSleep);
mbC1->setHasSelfCollision(selfCollide);
mbC1->setUseGyroTerm(gyro);
if (!damping)
{
mbC1->setLinearDamping(0.f);
mbC1->setAngularDamping(0.f);
}
else
{
mbC1->setLinearDamping(0.1f);
mbC1->setAngularDamping(0.9f);
}
//
m_dynamicsWorld->setGravity(btVector3(0, -9.81, 0));
//////////////////////////////////////////////
if (numLinks > 0)
{
btScalar q0 = 45.f * SIMD_PI / 180.f;
if (!spherical)
{
mbC1->setJointPosMultiDof(0, &q0);
}
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
quat0.normalize();
mbC1->setJointPosMultiDof(0, quat0);
}
}
///
addColliders(mbC1, world, baseHalfExtents, linkHalfExtents);
mbC2->setCanSleep(canSleep);
mbC2->setHasSelfCollision(selfCollide);
mbC2->setUseGyroTerm(gyro);
//
if (!damping)
{
mbC2->setLinearDamping(0.f);
mbC2->setAngularDamping(0.f);
}
else
{
mbC2->setLinearDamping(0.1f);
mbC2->setAngularDamping(0.9f);
}
//
m_dynamicsWorld->setGravity(btVector3(0, -9.81, 0));
//////////////////////////////////////////////
if (numLinks > 0)
{
btScalar q0 = -45.f * SIMD_PI / 180.f;
if (!spherical)
{
mbC2->setJointPosMultiDof(0, &q0);
}
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
quat0.normalize();
mbC2->setJointPosMultiDof(0, quat0);
}
}
///
addColliders(mbC2, world, baseHalfExtents, linkHalfExtents);
/////////////////////////////////////////////////////////////////
btScalar groundHeight = -51.55;
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
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, groundHeight, 0));
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);
/////////////////////////////////////////////////////////////////
createGround();
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
/////////////////////////////////////////////////////////////////
}
btMultiBody* SerialChains::createFeatherstoneMultiBody(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool fixedBase)
{
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 1.f;
if (baseMass)
{
btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
delete pTempBox;
}
bool canSleep = false;
btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, fixedBase, canSleep);
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
pMultiBody->setBasePos(basePosition);
pMultiBody->setWorldToBaseRot(baseOriQuat);
btVector3 vel(0, 0, 0);
//init the links
btVector3 hingeJointAxis(1, 0, 0);
float linkMass = 1.f;
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
delete pTempBox;
//y-axis assumed up
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
//////
btScalar q0 = 0.f * SIMD_PI / 180.f;
btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
quat0.normalize();
/////
for (int i = 0; i < numLinks; ++i)
{
if (!spherical)
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true);
else
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true);
}
pMultiBody->finalizeMultiDof();
///
pWorld->addMultiBody(pMultiBody);
///
return pMultiBody;
}
void SerialChains::createGround(const btVector3& halfExtents, btScalar zOffSet)
{
btCollisionShape* groundShape = new btBoxShape(halfExtents);
m_collisionShapes.push_back(groundShape);
// rigidbody is dynamic if and only if mass is non zero, otherwise static
btScalar mass(0.);
const 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
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -halfExtents.z() + zOffSet, 0));
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);
}
void SerialChains::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents)
{
btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1);
btAlignedObjectArray<btVector3> local_origin;
local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos();
{
btScalar 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(baseHalfExtents);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(local_origin[0]);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
pWorld->addCollisionObject(col, 2, 1 + 2);
col->setFriction(friction);
pMultiBody->setBaseCollider(col);
}
}
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
const int parent = pMultiBody->getParent(i);
world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1];
local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i)));
}
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
btVector3 posr = local_origin[i + 1];
btScalar 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(linkHalfExtents);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, 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(friction);
pWorld->addCollisionObject(col, 2, 1 + 2);
pMultiBody->getLink(i).m_collider = col;
}
}
CommonExampleInterface* SerialChainsCreateFunc(CommonExampleOptions& options)
{
return new SerialChains(options.m_guiHelper);
}

7
Engine/lib/bullet/examples/MultiBody/SerialChains.h Normal file
View file

@ -0,0 +1,7 @@
#ifndef MULTI_BODY_CONSTRAINT_SOLVERS_DEMO_H
#define MULTI_BODY_CONSTRAINT_SOLVERS_DEMO_H
class CommonExampleInterface* SerialChainsCreateFunc(struct CommonExampleOptions& options);
#endif //MULTI_BODY_CONSTRAINT_SOLVERS_DEMO_H

558
Engine/lib/bullet/examples/MultiBody/TestJointTorqueSetup.cpp
View file

@ -3,7 +3,7 @@
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
#include "../Utils/b3ResourcePath.h"
@ -11,367 +11,344 @@ static btScalar radius(0.2);
struct TestJointTorqueSetup : public CommonMultiBodyBase
{
btMultiBody* m_multiBody;
btMultiBody* m_multiBody;
btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks;
bool m_once;
bool m_once;
public:
TestJointTorqueSetup(struct GUIHelperInterface* helper);
virtual ~TestJointTorqueSetup();
TestJointTorqueSetup(struct GUIHelperInterface* helper);
virtual ~TestJointTorqueSetup();
virtual void initPhysics();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
virtual void stepSimulation(float deltaTime);
virtual void resetCamera()
{
float dist = 5;
float pitch = 270;
float yaw = 21;
float targetPos[3]={-1.34,3.4,-0.44};
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
float pitch = -21;
float yaw = 270;
float targetPos[3] = {-1.34, 3.4, -0.44};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
};
TestJointTorqueSetup::TestJointTorqueSetup(struct GUIHelperInterface* helper)
:CommonMultiBodyBase(helper),
m_once(true)
: CommonMultiBodyBase(helper),
m_once(true)
{
}
TestJointTorqueSetup::~TestJointTorqueSetup()
{
}
///this is a temporary global, until we determine if we need the option or not
extern bool gJointFeedbackInWorldSpace;
extern bool gJointFeedbackInJointFrame;
void TestJointTorqueSetup::initPhysics()
{
int upAxis = 1;
gJointFeedbackInWorldSpace = true;
gJointFeedbackInJointFrame = true;
int upAxis = 1;
m_guiHelper->setUpAxis(upAxis);
btVector4 colors[4] =
{
btVector4(1,0,0,1),
btVector4(0,1,0,1),
btVector4(0,1,1,1),
btVector4(1,1,0,1),
};
int curColor = 0;
btVector4 colors[4] =
{
btVector4(1, 0, 0, 1),
btVector4(0, 1, 0, 1),
btVector4(0, 1, 1, 1),
btVector4(1, 1, 0, 1),
};
int curColor = 0;
this->createEmptyDynamicsWorld();
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
//btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawWireframe
+btIDebugDraw::DBG_DrawContactPoints
+btIDebugDraw::DBG_DrawAabb
);//+btIDebugDraw::DBG_DrawConstraintLimits);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
//btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints + btIDebugDraw::DBG_DrawAabb); //+btIDebugDraw::DBG_DrawConstraintLimits);
m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace = true;
m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame = true;
//create a static ground object
if (1)
{
btVector3 groundHalfExtents(1,1,0.2);
groundHalfExtents[upAxis]=1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures();
m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 groundOrigin(-0.4f, 3.f, 0.f);
groundOrigin[upAxis] -=.5;
groundOrigin[2]-=0.6;
start.setOrigin(groundOrigin);
btQuaternion groundOrn(btVector3(0,1,0),0.25*SIMD_PI);
//create a static ground object
if (1)
{
btVector3 groundHalfExtents(1, 1, 0.2);
groundHalfExtents[upAxis] = 1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures();
m_guiHelper->createCollisionShapeGraphicsObject(box);
btTransform start;
start.setIdentity();
btVector3 groundOrigin(-0.4f, 3.f, 0.f);
groundOrigin[upAxis] -= .5;
groundOrigin[2] -= 0.6;
start.setOrigin(groundOrigin);
btQuaternion groundOrn(btVector3(0, 1, 0), 0.25 * SIMD_PI);
// start.setRotation(groundOrn);
btRigidBody* body = createRigidBody(0,start,box);
body->setFriction(0);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
m_guiHelper->createRigidBodyGraphicsObject(body,color);
}
btRigidBody* body = createRigidBody(0, start, box);
body->setFriction(0);
btVector4 color = colors[curColor];
curColor++;
curColor &= 3;
m_guiHelper->createRigidBodyGraphicsObject(body, color);
}
{
bool floating = false;
bool damping = false;
bool gyro = false;
int numLinks = 2;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false;
bool selfCollide = false;
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
{
bool floating = false;
bool damping = false;
bool gyro = false;
int numLinks = 2;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false;
bool selfCollide = false;
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
//mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 1.f;
btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
//mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 1.f;
if(baseMass)
{
//btCollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
btCollisionShape *shape = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
shape->calculateLocalInertia(baseMass, baseInertiaDiag);
delete shape;
}
if (baseMass)
{
//btCollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
btCollisionShape* shape = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
shape->calculateLocalInertia(baseMass, baseInertiaDiag);
delete shape;
}
btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
m_multiBody = pMultiBody;
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
// baseOriQuat.setEulerZYX(-.25*SIMD_PI,0,-1.75*SIMD_PI);
pMultiBody->setBasePos(basePosition);
pMultiBody->setWorldToBaseRot(baseOriQuat);
btVector3 vel(0, 0, 0);
// pMultiBody->setBaseVel(vel);
m_multiBody = pMultiBody;
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
// baseOriQuat.setEulerZYX(-.25*SIMD_PI,0,-1.75*SIMD_PI);
pMultiBody->setBasePos(basePosition);
pMultiBody->setWorldToBaseRot(baseOriQuat);
btVector3 vel(0, 0, 0);
// pMultiBody->setBaseVel(vel);
//init the links
btVector3 hingeJointAxis(1, 0, 0);
//y-axis assumed up
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
//init the links
btVector3 hingeJointAxis(1, 0, 0);
//////
btScalar q0 = 0.f * SIMD_PI/ 180.f;
btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
quat0.normalize();
/////
//y-axis assumed up
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
for(int i = 0; i < numLinks; ++i)
{
//////
btScalar q0 = 0.f * SIMD_PI / 180.f;
btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
quat0.normalize();
/////
for (int i = 0; i < numLinks; ++i)
{
float linkMass = 1.f;
//if (i==3 || i==2)
// linkMass= 1000;
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
btCollisionShape* shape = 0;
if (i==0)
if (i == 0)
{
shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//
} else
shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2])); //
}
else
{
shape = new btSphereShape(radius);
}
shape->calculateLocalInertia(linkMass, linkInertiaDiag);
delete shape;
if(!spherical)
if (!spherical)
{
//pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
if (i==0)
{
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
hingeJointAxis,
parentComToCurrentPivot,
currentPivotToCurrentCom, false);
} else
{
btVector3 parentComToCurrentCom(0, -radius * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -radius, 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
//pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
parentComToCurrentPivot,
currentPivotToCurrentCom);
if (i == 0)
{
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
hingeJointAxis,
parentComToCurrentPivot,
currentPivotToCurrentCom, false);
}
//pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false);
}
else
{
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
}
}
else
{
btVector3 parentComToCurrentCom(0, -radius * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -radius, 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
pMultiBody->finalizeMultiDof();
pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
parentComToCurrentPivot,
currentPivotToCurrentCom);
}
//pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false);
}
else
{
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
}
}
pMultiBody->finalizeMultiDof();
//for (int i=pMultiBody->getNumLinks()-1;i>=0;i--)//
for (int i=0;i<pMultiBody->getNumLinks();i++)
for (int i = 0; i < pMultiBody->getNumLinks(); i++)
{
btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback();
pMultiBody->getLink(i).m_jointFeedback = fb;
m_jointFeedbacks.push_back(fb);
//break;
}
btMultiBodyDynamicsWorld* world = m_dynamicsWorld;
btMultiBodyDynamicsWorld* world = m_dynamicsWorld;
///
world->addMultiBody(pMultiBody);
btMultiBody* mbC = pMultiBody;
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
mbC->setUseGyroTerm(gyro);
//
if(!damping)
{
mbC->setLinearDamping(0.f);
mbC->setAngularDamping(0.f);
}else
{ mbC->setLinearDamping(0.1f);
mbC->setAngularDamping(0.9f);
}
//
m_dynamicsWorld->setGravity(btVector3(0,0,-10));
///
world->addMultiBody(pMultiBody);
btMultiBody* mbC = pMultiBody;
mbC->setCanSleep(canSleep);
mbC->setHasSelfCollision(selfCollide);
mbC->setUseGyroTerm(gyro);
//
if (!damping)
{
mbC->setLinearDamping(0.f);
mbC->setAngularDamping(0.f);
}
else
{
mbC->setLinearDamping(0.1f);
mbC->setAngularDamping(0.9f);
}
//
m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
//////////////////////////////////////////////
if(0)//numLinks > 0)
{
btScalar q0 = 45.f * SIMD_PI/ 180.f;
if(!spherical)
//////////////////////////////////////////////
if (/* DISABLES CODE */ (0)) //numLinks > 0)
{
btScalar q0 = 45.f * SIMD_PI / 180.f;
if (!spherical)
{
mbC->setJointPosMultiDof(0, &q0);
}
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
quat0.normalize();
mbC->setJointPosMultiDof(0, quat0);
}
}
///
else
{
btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
quat0.normalize();
mbC->setJointPosMultiDof(0, quat0);
}
}
///
btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1);
btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1);
btAlignedObjectArray<btVector3> local_origin;
local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos();
// double friction = 1;
{
btAlignedObjectArray<btVector3> local_origin;
local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos();
// double friction = 1;
{
// float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
// btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
// float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
// btScalar 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* shape = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); //new btSphereShape(baseHalfExtents[0]);
m_guiHelper->createCollisionShapeGraphicsObject(shape);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(shape);
if (1)
{
btCollisionShape* shape = new btBoxShape(btVector3(baseHalfExtents[0],baseHalfExtents[1],baseHalfExtents[2]));//new btSphereShape(baseHalfExtents[0]);
m_guiHelper->createCollisionShapeGraphicsObject(shape);
btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(shape);
btTransform tr;
tr.setIdentity();
//if we don't set the initial pose of the btCollisionObject, the simulator will do this
btTransform tr;
tr.setIdentity();
//if we don't set the initial pose of the btCollisionObject, the simulator will do this
//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
tr.setOrigin(local_origin[0]);
btQuaternion orn(btVector3(0,0,1),0.25*3.1415926538);
tr.setRotation(orn);
col->setWorldTransform(tr);
tr.setOrigin(local_origin[0]);
btQuaternion orn(btVector3(0, 0, 1), 0.25 * 3.1415926538);
tr.setRotation(orn);
col->setWorldTransform(tr);
bool isDynamic = (baseMass > 0 && floating);
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
world->addCollisionObject(col, collisionFilterGroup, collisionFilterMask); //, 2,1+2);
world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2);
btVector3 color(0.0, 0.0, 0.5);
m_guiHelper->createCollisionObjectGraphicsObject(col, color);
btVector3 color(0.0,0.0,0.5);
m_guiHelper->createCollisionObjectGraphicsObject(col,color);
// col->setFriction(friction);
pMultiBody->setBaseCollider(col);
}
}
// col->setFriction(friction);
pMultiBody->setBaseCollider(col);
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
const int parent = pMultiBody->getParent(i);
world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1];
local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i)));
}
}
}
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
btVector3 posr = local_origin[i + 1];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
btScalar 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* shape = 0;
for (int i=0; i < pMultiBody->getNumLinks(); ++i)
{
const int parent = pMultiBody->getParent(i);
world_to_local[i+1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent+1];
local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , pMultiBody->getRVector(i)));
}
for (int i=0; i < pMultiBody->getNumLinks(); ++i)
{
btVector3 posr = local_origin[i+1];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
btScalar 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* shape =0;
if (i==0)
if (i == 0)
{
shape = new btBoxShape(btVector3(linkHalfExtents[0],linkHalfExtents[1],linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]);
} else
shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2])); //btSphereShape(linkHalfExtents[0]);
}
else
{
shape = new btSphereShape(radius);
}
m_guiHelper->createCollisionShapeGraphicsObject(shape);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
m_guiHelper->createCollisionShapeGraphicsObject(shape);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
col->setCollisionShape(shape);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
col->setWorldTransform(tr);
// col->setFriction(friction);
bool isDynamic = 1;//(linkMass > 0);
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
col->setCollisionShape(shape);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
// col->setFriction(friction);
bool isDynamic = 1; //(linkMass > 0);
int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
//if (i==0||i>numLinks-2)
{
world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//,2,1+2);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
m_guiHelper->createCollisionObjectGraphicsObject(col,color);
world->addCollisionObject(col, collisionFilterGroup, collisionFilterMask); //,2,1+2);
btVector4 color = colors[curColor];
curColor++;
curColor &= 3;
m_guiHelper->createCollisionObjectGraphicsObject(col, color);
pMultiBody->getLink(i).m_collider=col;
pMultiBody->getLink(i).m_collider = col;
}
}
}
}
}
btSerializer* s = new btDefaultSerializer;
m_dynamicsWorld->serialize(s);
char resourcePath[1024];
if (b3ResourcePath::findResourcePath("multibody.bullet",resourcePath,1024))
if (b3ResourcePath::findResourcePath("multibody.bullet", resourcePath, 1024,0))
{
FILE* f = fopen(resourcePath,"wb");
fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);
FILE* f = fopen(resourcePath, "wb");
fwrite(s->getBufferPointer(), s->getCurrentBufferSize(), 1, f);
fclose(f);
}
}
@ -379,56 +356,49 @@ void TestJointTorqueSetup::initPhysics()
void TestJointTorqueSetup::stepSimulation(float deltaTime)
{
//m_multiBody->addLinkForce(0,btVector3(100,100,100));
if (0)//m_once)
{
m_once=false;
m_multiBody->addJointTorque(0, 10.0);
btScalar torque = m_multiBody->getJointTorque(0);
b3Printf("t = %f,%f,%f\n",torque,torque,torque);//[0],torque[1],torque[2]);
}
m_dynamicsWorld->stepSimulation(1./240,0);
if (/* DISABLES CODE */ (0)) //m_once)
{
m_once = false;
m_multiBody->addJointTorque(0, 10.0);
btScalar torque = m_multiBody->getJointTorque(0);
b3Printf("t = %f,%f,%f\n", torque, torque, torque); //[0],torque[1],torque[2]);
}
m_dynamicsWorld->stepSimulation(1. / 240, 0);
static int count = 0;
if ((count& 0x0f)==0)
{
for (int i=0;i<m_jointFeedbacks.size();i++)
if ((count & 0x0f) == 0)
{
for (int i = 0; i < m_jointFeedbacks.size(); i++)
{
b3Printf("F_reaction[%i] linear:%f,%f,%f, angular:%f,%f,%f",
i,
m_jointFeedbacks[i]->m_reactionForces.m_topVec[0],
m_jointFeedbacks[i]->m_reactionForces.m_topVec[1],
m_jointFeedbacks[i]->m_reactionForces.m_topVec[2],
i,
m_jointFeedbacks[i]->m_reactionForces.m_topVec[0],
m_jointFeedbacks[i]->m_reactionForces.m_topVec[1],
m_jointFeedbacks[i]->m_reactionForces.m_topVec[2],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[0],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[1],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[2]
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[0],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[1],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[2]
);
}
);
}
}
count++;
/*
b3Printf("base angvel = %f,%f,%f",m_multiBody->getBaseOmega()[0],
m_multiBody->getBaseOmega()[1],
m_multiBody->getBaseOmega()[2]
);
*/
// btScalar jointVel =m_multiBody->getJointVel(0);
// b3Printf("child angvel = %f",jointVel);
// btScalar jointVel =m_multiBody->getJointVel(0);
// b3Printf("child angvel = %f",jointVel);
}
class CommonExampleInterface* TestJointTorqueCreateFunc(struct CommonExampleOptions& options)
class CommonExampleInterface* TestJointTorqueCreateFunc(struct CommonExampleOptions& options)
{
return new TestJointTorqueSetup(options.m_guiHelper);
}

5
Engine/lib/bullet/examples/MultiBody/TestJointTorqueSetup.h
View file

@ -1,7 +1,6 @@
#ifndef TEST_JOINT_TORQUE_SETUP_H
#define TEST_JOINT_TORQUE_SETUP_H
class CommonExampleInterface* TestJointTorqueCreateFunc(struct CommonExampleOptions& options);
#endif //TEST_JOINT_TORQUE_SETUP_H
class CommonExampleInterface* TestJointTorqueCreateFunc(struct CommonExampleOptions& options);
#endif //TEST_JOINT_TORQUE_SETUP_H

4007
Engine/lib/bullet/examples/MultiBody/pendulum_gold.h
View file

File diff suppressed because it is too large Load diff