Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add very basic multibody vehicle

Browse Source 
tweak finite element experiment with parameter
This commit is contained in:
Erwin Coumans
2014-10-31 14:14:45 -07:00
parent 91bcb055db
commit eac8b32782
9 changed files with 486 additions and 348 deletions
Download Patch File Download Diff File Expand all files Collapse all files

204
Demos3/bullet2/ConstraintDemo/ConstraintPhysicsSetup.cpp
View File

@@ -1,77 +1,127 @@
#include "ConstraintPhysicsSetup.h"
ConstraintPhysicsSetup::ConstraintPhysicsSetup()
{
}
ConstraintPhysicsSetup::~ConstraintPhysicsSetup()
{
}
btScalar val;
btHingeAccumulatedAngleConstraint* spDoorHinge=0;
void ConstraintPhysicsSetup::stepSimulation(float deltaTime)
{
val=spDoorHinge->getAccumulatedHingeAngle()*SIMD_DEGS_PER_RAD;// spDoorHinge->getHingeAngle()*SIMD_DEGS_PER_RAD;
if (m_dynamicsWorld)
{
m_dynamicsWorld->stepSimulation(deltaTime);
}
}
#include "Bullet3Common/b3Logging.h"
void ConstraintPhysicsSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
{
b3Printf(__FILE__);
gfxBridge.setUpAxis(1);
createEmptyDynamicsWorld();
gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
int mode = btIDebugDraw::DBG_DrawWireframe
+btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawConstraintLimits;
m_dynamicsWorld->getDebugDrawer()->setDebugMode(mode);
val=1.f;
SliderParams slider("hinge angle",&val);
slider.m_minVal=-720;
slider.m_maxVal=720;
gfxBridge.getParameterInterface()->registerSliderFloatParameter(slider);
{ // create a door using hinge constraint attached to the world
btCollisionShape* pDoorShape = new btBoxShape(btVector3(2.0f, 5.0f, 0.2f));
m_collisionShapes.push_back(pDoorShape);
btTransform doorTrans;
doorTrans.setIdentity();
doorTrans.setOrigin(btVector3(-5.0f, -2.0f, 0.0f));
btRigidBody* pDoorBody = createRigidBody( 1.0, doorTrans, pDoorShape);
pDoorBody->setActivationState(DISABLE_DEACTIVATION);
const btVector3 btPivotA(10.f + 2.1f, -2.0f, 0.0f ); // right next to the door slightly outside
btVector3 btAxisA( 0.0f, 1.0f, 0.0f ); // pointing upwards, aka Y-axis
spDoorHinge = new btHingeAccumulatedAngleConstraint( *pDoorBody, btPivotA, btAxisA );
spDoorHinge->setParam(BT_CONSTRAINT_ERP,0.5);
btScalar erp = spDoorHinge->getParam(BT_CONSTRAINT_ERP);
// spDoorHinge->setLimit( 0.0f, SIMD_PI_2 );
// test problem values
// spDoorHinge->setLimit( -SIMD_PI, SIMD_PI*0.8f);
// spDoorHinge->setLimit( 1.f, -1.f);
// spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI);
// spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI, 0.9f, 0.3f, 0.0f);
// spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI, 0.9f, 0.01f, 0.0f); // "sticky limits"
// spDoorHinge->setLimit( -SIMD_PI * 0.25f, SIMD_PI * 0.25f );
// spDoorHinge->setLimit( 0.0f, 0.0f );
m_dynamicsWorld->addConstraint(spDoorHinge);
spDoorHinge->setDbgDrawSize(btScalar(5.f));
//doorTrans.setOrigin(btVector3(-5.0f, 2.0f, 0.0f));
//btRigidBody* pDropBody = localCreateRigidBody( 10.0, doorTrans, shape);
}
}
#include "ConstraintPhysicsSetup.h"
ConstraintPhysicsSetup::ConstraintPhysicsSetup()
{
}
ConstraintPhysicsSetup::~ConstraintPhysicsSetup()
{
}
static btScalar val;
static btScalar targetVel=0;
static btScalar maxImpulse=10000;
static btHingeAccumulatedAngleConstraint* spDoorHinge=0;
static btScalar actualHingeVelocity=0.f;
static btVector3 btAxisA(0,1,0);
void ConstraintPhysicsSetup::stepSimulation(float deltaTime)
{
val=spDoorHinge->getAccumulatedHingeAngle()*SIMD_DEGS_PER_RAD;
if (m_dynamicsWorld)
{
spDoorHinge->enableAngularMotor(true,targetVel,maxImpulse);
m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
btHingeConstraint* hinge = spDoorHinge;
if (hinge)
{
const btRigidBody& bodyA = hinge->getRigidBodyA();
const btRigidBody& bodyB = hinge->getRigidBodyB();
btTransform trA = bodyA.getWorldTransform();
btVector3 angVelA = bodyA.getAngularVelocity();
btVector3 angVelB = bodyB.getAngularVelocity();
{
btVector3 ax1 = trA.getBasis()*hinge->getFrameOffsetA().getBasis().getColumn(2);
btScalar vel = angVelA.dot(ax1);
vel -= angVelB.dot(ax1);
printf("hinge velocity (q) = %f\n", vel);
actualHingeVelocity=vel;
}
btVector3 ortho0,ortho1;
btPlaneSpace1(btAxisA,ortho0,ortho1);
{
btScalar vel2 = angVelA.dot(ortho0);
vel2 -= angVelB.dot(ortho0);
printf("hinge orthogonal1 velocity (q) = %f\n", vel2);
}
{
btScalar vel0 = angVelA.dot(ortho1);
vel0 -= angVelB.dot(ortho1);
printf("hinge orthogonal0 velocity (q) = %f\n", vel0);
}
}
}
}
void ConstraintPhysicsSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
{
gfxBridge.setUpAxis(1);
createEmptyDynamicsWorld();
gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
int mode = btIDebugDraw::DBG_DrawWireframe
+btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawConstraintLimits;
m_dynamicsWorld->getDebugDrawer()->setDebugMode(mode);
{
SliderParams slider("target vel",&targetVel);
slider.m_minVal=-4;
slider.m_maxVal=4;
gfxBridge.getParameterInterface()->registerSliderFloatParameter(slider);
}
{
SliderParams slider("max impulse",&maxImpulse);
slider.m_minVal=0;
slider.m_maxVal=1000;
gfxBridge.getParameterInterface()->registerSliderFloatParameter(slider);
}
{
SliderParams slider("actual vel",&actualHingeVelocity);
slider.m_minVal=-4;
slider.m_maxVal=4;
gfxBridge.getParameterInterface()->registerSliderFloatParameter(slider);
}
val=1.f;
{
SliderParams slider("angle",&val);
slider.m_minVal=-720;
slider.m_maxVal=720;
gfxBridge.getParameterInterface()->registerSliderFloatParameter(slider);
}
{ // create a door using hinge constraint attached to the world
btCollisionShape* pDoorShape = new btBoxShape(btVector3(2.0f, 5.0f, 0.2f));
m_collisionShapes.push_back(pDoorShape);
btTransform doorTrans;
doorTrans.setIdentity();
doorTrans.setOrigin(btVector3(-5.0f, -2.0f, 0.0f));
btRigidBody* pDoorBody = createRigidBody( 1.0, doorTrans, pDoorShape);
pDoorBody->setActivationState(DISABLE_DEACTIVATION);
const btVector3 btPivotA(10.f + 2.1f, -2.0f, 0.0f ); // right next to the door slightly outside
spDoorHinge = new btHingeAccumulatedAngleConstraint( *pDoorBody, btPivotA, btAxisA );
m_dynamicsWorld->addConstraint(spDoorHinge);
spDoorHinge->setDbgDrawSize(btScalar(5.f));
}
}

236
Demos3/bullet2/MultiBodyDemo/MultiBodyVehicle.cpp Normal file
View File

@@ -0,0 +1,236 @@
//test addJointTorque
#include "MultiBodyVehicle.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBody.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyLink.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "BulletCollision/CollisionShapes/btBoxShape.h"
btScalar gVehicleBaseMass = 100.f;
btScalar gVehicleWheelMass = 5.f;
float friction = 1.f;
btVector3 gVehicleBaseHalfExtents(1, 0.1, 2);
btVector3 gVehicleWheelHalfExtents(0.2, 0.2, 0.2);
btVector3 gVehicleWheelOffset(0, 0, 0.5);
btVector3 wheelAttachmentPosInWorld[4] = {
btVector3(1, 0, 2.),
btVector3(-1, 0, 2.),
btVector3(1, 0, -2.),
btVector3(-1, 0, -2.)
};
MultiBodyVehicleSetup::MultiBodyVehicleSetup()
{
}
MultiBodyVehicleSetup::~MultiBodyVehicleSetup()
{
}
class btMultiBody* MultiBodyVehicleSetup::createMultiBodyVehicle()
{
class btMultiBodyDynamicsWorld* world = m_dynamicsWorld;
int numWheels = 4;
int totalLinks = numWheels;//number of body parts (links) (in)directly attached to the base, NOT including the base/root itself
btCollisionShape* chassis = new btBoxShape(gVehicleBaseHalfExtents);//CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS));
m_collisionShapes.push_back(chassis);
btCollisionShape* wheel = new btCylinderShapeX(gVehicleWheelHalfExtents);//CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS));
m_collisionShapes.push_back(wheel);
btVector3 baseLocalInertia(0, 0, 0);
chassis->calculateLocalInertia(gVehicleBaseMass, baseLocalInertia);
bool multiDof = false;
bool isFixedBase = false;
bool canSleep = false;
btMultiBody * bod = new btMultiBody(totalLinks, gVehicleBaseMass, baseLocalInertia, isFixedBase, canSleep);// , multiDof);
bod->setHasSelfCollision(false);
btQuaternion baseOrn(0, 0, 0, 1);
btVector3 basePos(0, 0, 0);
bod->setBasePos(basePos);
bod->setWorldToBaseRot(baseOrn);
btVector3 vel(0, 0, 0);
bod->setBaseVel(vel);
{
int linkNum = 0;
btVector3 wheelJointAxisWorld(1, 0, 0);
btQuaternion parent_to_child = baseOrn.inverse();//??
for (int j = 0; j < numWheels; j++, linkNum++)
{
int parent_link_num = -1;
float initial_joint_angle = 0.0;
btVector3 localWheelInertia(0, 0, 0);
wheel->calculateLocalInertia(gVehicleWheelMass, localWheelInertia);
bool disableParentCollision = true;
btVector3 pivotToChildCOM(0, 0, 0.25);
btVector3 pivotToWheelCOM(0, 0, 0);
{
bod->setupRevolute(linkNum, gVehicleWheelMass, localWheelInertia, parent_link_num, parent_to_child, wheelJointAxisWorld,
wheelAttachmentPosInWorld[j], pivotToWheelCOM, disableParentCollision);
}
bod->setJointPos(linkNum, initial_joint_angle);
if (j<2)
{
btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod, linkNum, 1., 50);
world->addMultiBodyConstraint(con);
}
}
}
//add a collider for the base
{
btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(totalLinks + 1);
btAlignedObjectArray<btVector3> local_origin;
local_origin.resize(totalLinks + 1);
world_to_local[0] = bod->getWorldToBaseRot();
local_origin[0] = bod->getBasePos();
{
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)
{
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(bod, -1);
col->setCollisionShape(chassis);
btTransform tr;
tr.setIdentity();
tr.setOrigin(local_origin[0]);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
world->addCollisionObject(col, btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter);// 2, 1 + 2);
col->setFriction(friction);
bod->setBaseCollider(col);
}
}
//initialize local coordinate frames, relative to parent
for (int i = 0; i<bod->getNumLinks(); i++)
{
const int parent = bod->getParent(i);
world_to_local[i + 1] = bod->getParentToLocalRot(i) * world_to_local[parent + 1];
local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), bod->getRVector(i)));
}
int linkIndex = 0;
for (int j = 0; j<numWheels; j++, linkIndex++)
{
btVector3 posr = local_origin[linkIndex + 1];
float pos[4] = { posr.x(), posr.y(), posr.z(), 1 };
float quat[4] = { -world_to_local[linkIndex + 1].x(), -world_to_local[linkIndex + 1].y(), -world_to_local[linkIndex + 1].z(), world_to_local[linkIndex + 1].w() };
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(bod, linkIndex);
col->setCollisionShape(wheel);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
col->setFriction(friction);
world->addCollisionObject(col, btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::AllFilter);// 2, 1 + 2);
bod->getLink(linkIndex).m_collider = col;
}
}
world->addMultiBody(bod);
// world->setGravity(btVector3(0,0,0));
return bod;
}
void MultiBodyVehicleSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
{
int upAxis = 1;
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;
gfxBridge.setUpAxis(upAxis);
this->createEmptyDynamicsWorld();
gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
//btIDebugDraw::DBG_DrawConstraints
+btIDebugDraw::DBG_DrawWireframe
+btIDebugDraw::DBG_DrawContactPoints
+btIDebugDraw::DBG_DrawAabb
);//+btIDebugDraw::DBG_DrawConstraintLimits);
createMultiBodyVehicle();
if (1)
{
btVector3 groundHalfExtents(20,20,20);
groundHalfExtents[upAxis]=1.f;
btBoxShape* box = new btBoxShape(groundHalfExtents);
box->initializePolyhedralFeatures();
gfxBridge.createCollisionShapeGraphicsObject(box);
btTransform start; start.setIdentity();
btVector3 groundOrigin(0,0,0);
groundOrigin[upAxis]=-1.5;
start.setOrigin(groundOrigin);
btRigidBody* body = createRigidBody(0,start,box);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
gfxBridge.createRigidBodyGraphicsObject(body,color);
}
}
void MultiBodyVehicleSetup::stepSimulation(float deltaTime)
{
m_dynamicsWorld->stepSimulation(deltaTime);
}

24
Demos3/bullet2/MultiBodyDemo/MultiBodyVehicle.h Normal file
View File

@@ -0,0 +1,24 @@
#ifndef TEST_MULTIBODY_VEHICLE_SETUP_H
#define TEST_MULTIBODY_VEHICLE_SETUP_H
#include "Bullet3AppSupport/CommonMultiBodySetup.h"
struct MultiBodyVehicleSetup : public CommonMultiBodySetup
{
btMultiBody* m_multiBody;
public:
MultiBodyVehicleSetup();
virtual ~MultiBodyVehicleSetup();
virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge);
virtual void stepSimulation(float deltaTime);
class btMultiBody* createMultiBodyVehicle();
};
#endif //TEST_MULTIBODY_VEHICLE_SETUP_H