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

11
Demos3/AllBullet2Demos/BulletDemoEntries.h
View File

@@ -22,6 +22,8 @@
#include "../../Demos/SerializeDemo/SerializeSetup.h"
#include "../bullet2/MultiBodyDemo/TestJointTorqueSetup.h"
#include "../bullet2/MultiBodyDemo/MultiBodyVehicle.h"
#include "../bullet2/CollisionDetection/SupportFuncDemo.h"
#include "../bullet2/BasicConcepts/CoordinateSystemDemo.h"
#include "../../Demos3/FiniteElementMethod/FiniteElementDemo.h"
@@ -31,7 +33,11 @@ static BulletDemoInterface* TestJointTorqueCreateFunc(CommonGraphicsApp* app)
CommonPhysicsSetup* physicsSetup = new TestJointTorqueSetup();
return new BasicDemo(app, physicsSetup);
}
static BulletDemoInterface* MultiBodyVehicleCreateFunc(CommonGraphicsApp* app)
{
CommonPhysicsSetup* physicsSetup = new MultiBodyVehicleSetup();
return new BasicDemo(app, physicsSetup);
}
static BulletDemoInterface* LuaDemoCreateFunc(CommonGraphicsApp* app)
{
CommonPhysicsSetup* physicsSetup = new LuaPhysicsSetup(app);
@@ -116,7 +122,7 @@ static BulletDemoEntry allDemos[]=
{ 1, "URDF", MyImportUrdfCreateFunc },
{ 1, "STL", MyImportSTLCreateFunc},
{ 1, "COLLADA", MyImportColladaCreateFunc},
{0,"Finite Element Method", 0},
{0,"Experiments", 0},
{1, "Finite Element Demo", FiniteElementDemo::CreateFunc},
/* {1,"ChainDemo",ChainDemo::MyCreateFunc},
// {0, "Stress tests", 0 },
@@ -135,6 +141,7 @@ static BulletDemoEntry allDemos[]=
// {"MultiBody2",FeatherstoneDemo2::MyCreateFunc},
{1,"MultiDofDemo",MultiDofDemo::MyCreateFunc},
{1,"TestJointTorque",TestJointTorqueCreateFunc},
{1,"MultiBodyVehicle", MultiBodyVehicleCreateFunc},
};

4
Demos3/AllBullet2Demos/premake4.lua
View File

@@ -44,7 +44,9 @@
"../bullet2/LuaDemo/LuaPhysicsSetup.h",
"../bullet2/MultiBodyDemo/TestJointTorqueSetup.cpp",
"../bullet2/MultiBodyDemo/TestJointTorqueSetup.h",
-- "../DifferentialGearDemo/DifferentialGearSetup.cpp",
"../bullet2/MultiBodyDemo/MultiBodyVehicle.cpp",
"../bullet2/MultiBodyDemo/MultiBodyVehicle.h",
-- "../DifferentialGearDemo/DifferentialGearSetup.cpp",
-- "../DifferentialGearDemo/DifferentialGearSetup.h",
"../FiniteElementMethod/FiniteElementDemo.cpp",
"../../Demos/BasicDemo/BasicDemoPhysicsSetup.cpp",

73
Demos3/FiniteElementMethod/FiniteElementDemo.cpp
View File

@@ -26,6 +26,7 @@ subject to the following restrictions:
#include <OpenTissue/dynamics/fem/fem.h>
#include <OpenTissue/core/containers/t4mesh/util/t4mesh_block_generator.h>
#include "LinearMath/btAlignedObjectArray.h"
#include "Bullet3AppSupport/CommonParameterInterface.h"
//typedef OpenTissue::math::BasicMathTypes<float,size_t> math_types;
typedef OpenTissue::math::BasicMathTypes<double,size_t> math_types;
@@ -34,7 +35,7 @@ typedef math_types::vector3_type vector3_type;
typedef math_types::real_type real_type;
static int fixedNodes = 1;
struct FiniteElementDemoInternalData
{
@@ -48,8 +49,8 @@ struct FiniteElementDemoInternalData
real_type m_gravity;
real_type m_young;// = 500000;
real_type m_poisson;// = 0.33;
btScalar m_young;// = 500000;
btScalar m_poisson;// = 0.33;
real_type m_density;// = 1000;
//--- infinite m_c_yield plasticity settings means that plasticity is turned off
@@ -62,7 +63,8 @@ struct FiniteElementDemoInternalData
{
m_stiffness_warp_on= true;
m_collideGroundPlane = true;
m_fixNodes = true;
m_fixNodes = fixedNodes==1;
fixedNodes=1-fixedNodes;
m_gravity = 9.81;
m_young = 500000;//47863;//100000;
m_poisson = 0.33;
@@ -100,13 +102,37 @@ void FiniteElementDemo::initPhysics()
for (int n=0;n<m_data->m_mesh1.m_nodes.size();n++)
{
m_data->m_mesh1.m_nodes[n].m_coord(1)+=0.1f;
m_data->m_mesh1.m_nodes[n].m_coord(m_app->getUpAxis())+=.5f;
m_data->m_mesh1.m_nodes[n].m_model_coord = m_data->m_mesh1.m_nodes[n].m_coord;
}
OpenTissue::fem::init(m_data->m_mesh1,m_data->m_young,m_data->m_poisson,m_data->m_density,m_data->m_c_yield,m_data->m_c_creep,m_data->m_c_max);
OpenTissue::fem::init(m_data->m_mesh1,double(m_data->m_young),double(m_data->m_poisson),m_data->m_density,m_data->m_c_yield,m_data->m_c_creep,m_data->m_c_max);
}
{
SliderParams slider("Young",&m_data->m_young);
// slider.m_showValues = false;
slider.m_minVal=50000;
slider.m_maxVal=1000000;
m_app->m_parameterInterface->registerSliderFloatParameter(slider);
}
{
SliderParams slider("Poisson",&m_data->m_poisson);
// slider.m_showValues = false;
slider.m_minVal=0.01;
slider.m_maxVal=0.49;
m_app->m_parameterInterface->registerSliderFloatParameter(slider);
}
}
void FiniteElementDemo::exitPhysics()
{
@@ -118,7 +144,10 @@ void FiniteElementDemo::stepSimulation(float deltaTime)
m_y+=0.01f;
m_z+=0.01f;
double dt = 1./60.;//double (deltaTime);
//normal gravity
double poisson =m_data->m_poisson;
OpenTissue::fem::init(m_data->m_mesh1,double(m_data->m_young),poisson,m_data->m_density,m_data->m_c_yield,m_data->m_c_creep,m_data->m_c_max);
for (int n=0;n<m_data->m_mesh1.m_nodes.size();n++)
{
@@ -136,9 +165,39 @@ void FiniteElementDemo::stepSimulation(float deltaTime)
m_data->m_mesh1.m_nodes[n].m_fixed = false;
}
}
if (m_data->m_collideGroundPlane && m_data->m_mesh1.m_nodes[n].m_coord(m_app->getUpAxis())<0.f)
{
float depth = -m_data->m_mesh1.m_nodes[n].m_coord(m_app->getUpAxis());
if (depth>0.1)
depth=0.1;
m_data->m_mesh1.m_nodes[n].m_f_external(m_app->getUpAxis()) = depth*1000;
if (m_data->m_mesh1.m_nodes[n].m_velocity(m_app->getUpAxis()) < 0.f)
{
m_data->m_mesh1.m_nodes[n].m_velocity(m_app->getUpAxis())=0.f;
}
int frictionAxisA=0;
int frictionAxisB=2;
if (m_app->getUpAxis()==1)
{
frictionAxisA=0;
frictionAxisB=2;
} else
{
frictionAxisA=0;
frictionAxisB=1;
}
m_data->m_mesh1.m_nodes[n].m_velocity(frictionAxisA)=0.f;
m_data->m_mesh1.m_nodes[n].m_velocity(frictionAxisB)=0.f;
} else
{
vector3_type gravity = vector3_type(0.0, 0.0 , 0.0);
gravity(m_app->getUpAxis()) = -(m_data->m_mesh1.m_nodes[n].m_mass * m_data->m_gravity);
m_data->m_mesh1.m_nodes[n].m_f_external =gravity;
}
//m_data->m_mesh1.m_nodes[n].m_velocity.clear();
}

253
Demos3/ImportURDFDemo/ImportURDFSetup.cpp
View File

@@ -348,256 +348,6 @@ btCollisionShape* convertVisualToCollisionShape(const Collision* visual, const c
}
return shape;
}
btMultiBody* URDF2BulletMultiBody(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world, URDF2BulletMappings& mappings, const char* pathPrefix, btMultiBody* mb, int totalNumJoints)
{
btScalar mass = 0.f;
btTransform localInertialTransform; localInertialTransform.setIdentity();
btVector3 localInertiaDiagonal(0,0,0);
{
if ((*link).inertial)
{
mass = (*link).inertial->mass;
btMatrix3x3 inertiaMat;
inertiaMat.setIdentity();
inertiaMat.setValue(
(*link).inertial->ixx,(*link).inertial->ixy,(*link).inertial->ixz,
(*link).inertial->ixy,(*link).inertial->iyy,(*link).inertial->iyz,
(*link).inertial->ixz,(*link).inertial->iyz,(*link).inertial->izz);
btScalar threshold = 0.00001f;
int maxSteps=20;
btMatrix3x3 inertia2PrincipalAxis;
inertiaMat.diagonalize(inertia2PrincipalAxis,threshold,maxSteps);
localInertiaDiagonal.setValue(inertiaMat[0][0],inertiaMat[1][1],inertiaMat[2][2]);
btVector3 inertiaLocalCOM((*link).inertial->origin.position.x,(*link).inertial->origin.position.y,(*link).inertial->origin.position.z);
localInertialTransform.setOrigin(inertiaLocalCOM);
btQuaternion inertiaOrn((*link).inertial->origin.rotation.x,(*link).inertial->origin.rotation.y,(*link).inertial->origin.rotation.z,(*link).inertial->origin.rotation.w);
btMatrix3x3 inertiaOrnMat(inertiaOrn);
if (mass > 0 && (localInertiaDiagonal[0]==0.f || localInertiaDiagonal[1] == 0.f
|| localInertiaDiagonal[2] == 0.f))
{
b3Warning("Error: inertia should not be zero if mass is positive\n");
localInertiaDiagonal.setMax(btVector3(0.1,0.1,0.1));
localInertialTransform.setIdentity();//.setBasis(inertiaOrnMat);
}
else
{
localInertialTransform.setBasis(inertiaOrnMat*inertia2PrincipalAxis);
}
}
}
btTransform linkTransformInWorldSpace;
int parentIndex = -1;
const Link* parentLink = (*link).getParent();
if (parentLink)
{
parentIndex = parentLink->m_link_index;
btAssert(parentIndex>=0);
}
int linkIndex = mappings.m_linkMasses.size();
btTransform parent2joint;
if ((*link).parent_joint)
{
const urdf::Vector3 pos = (*link).parent_joint->parent_to_joint_origin_transform.position;
const urdf::Rotation orn = (*link).parent_joint->parent_to_joint_origin_transform.rotation;
parent2joint.setOrigin(btVector3(pos.x,pos.y,pos.z));
parent2joint.setRotation(btQuaternion(orn.x,orn.y,orn.z,orn.w));
linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
} else
{
linkTransformInWorldSpace = parentTransformInWorldSpace;
btAssert(mb==0);
bool multiDof = true;
bool canSleep = false;
bool isFixedBase = (mass==0);//todo: figure out when base is fixed
mb = new btMultiBody(totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof);
}
btAssert(mb);
(*link).m_link_index = linkIndex;
//compute this links center of mass transform, aligned with the principal axis of inertia
{
//btTransform rigidBodyFrameInWorldSpace =linkTransformInWorldSpace*inertialFrame;
mappings.m_linkMasses.push_back(mass);
mappings.m_linkLocalDiagonalInertiaTensors.push_back(localInertiaDiagonal);
mappings.m_linkLocalInertiaTransforms.push_back(localInertialTransform);
if ((*link).parent_joint)
{
btTransform offsetInA,offsetInB;
offsetInA.setIdentity();
//offsetInA = mappings.m_linkLocalInertiaTransforms[parentIndex].inverse()*parent2joint;
offsetInA = parent2joint;
offsetInB.setIdentity();
//offsetInB = localInertialTransform.inverse();
const Joint* pj = (*link).parent_joint.get();
//btVector3 jointAxis(0,0,1);//pj->axis.x,pj->axis.y,pj->axis.z);
btVector3 jointAxis(pj->axis.x,pj->axis.y,pj->axis.z);
mappings.m_jointAxisArray.push_back(jointAxis);
mappings.m_jointOffsetInParent.push_back(offsetInA);
mappings.m_jointOffsetInChild.push_back(offsetInB);
mappings.m_jointTypeArray.push_back(pj->type);
switch (pj->type)
{
case Joint::FIXED:
{
printf("Fixed joint\n");
mb->setupFixed(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),offsetInA.getOrigin(),offsetInB.getOrigin());
break;
}
case Joint::CONTINUOUS:
case Joint::REVOLUTE:
{
printf("Revolute joint\n");
mb->setupRevolute(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),jointAxis,offsetInA.getOrigin(),offsetInB.getOrigin(),true);
mb->finalizeMultiDof();
//mb->setJointVel(linkIndex-1,1);
break;
}
case Joint::PRISMATIC:
{
mb->setupPrismatic(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),jointAxis,offsetInB.getOrigin(),true);
printf("Prismatic joint\n");
break;
}
default:
{
printf("Unknown joint\n");
btAssert(0);
}
};
} else
{
mappings.m_jointAxisArray.push_back(btVector3(0,0,0));
btTransform ident;
ident.setIdentity();
mappings.m_jointOffsetInParent.push_back(ident);
mappings.m_jointOffsetInChild.push_back(ident);
mappings.m_jointTypeArray.push_back(-1);
}
}
//btCompoundShape* compoundShape = new btCompoundShape();
btCollisionShape* shape = 0;
for (int v=0;v<(int)link->collision_array.size();v++)
{
const Collision* visual = link->collision_array[v].get();
shape = convertVisualToCollisionShape(visual,pathPrefix);
if (shape)//childShape)
{
gfxBridge.createCollisionShapeGraphicsObject(shape);//childShape);
btVector3 color = selectColor();
/*
if (visual->material.get())
{
color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
}
*/
btVector3 localInertia(0,0,0);
if (mass)
{
shape->calculateLocalInertia(mass,localInertia);
}
//btRigidBody::btRigidBodyConstructionInfo rbci(mass,0,shape,localInertia);
btVector3 visual_pos(visual->origin.position.x,visual->origin.position.y,visual->origin.position.z);
btQuaternion visual_orn(visual->origin.rotation.x,visual->origin.rotation.y,visual->origin.rotation.z,visual->origin.rotation.w);
btTransform visual_frame;
visual_frame.setOrigin(visual_pos);
visual_frame.setRotation(visual_orn);
btTransform childTransform;
childTransform.setIdentity();//TODO(erwincoumans): compute relative visual/inertial transform
// compoundShape->addChildShape(childTransform,childShape);
}
}
if (shape)//compoundShape->getNumChildShapes()>0)
{
btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(mb, linkIndex-1);
col->setCollisionShape(shape);
btTransform tr;
tr.setIdentity();
tr = linkTransformInWorldSpace;
//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]);
//tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
col->setWorldTransform(tr);
bool isDynamic = true;
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);
btVector3 color(0.0,0.0,0.5);
gfxBridge.createCollisionObjectGraphicsObject(col,color);
btScalar friction = 0.5f;
col->setFriction(friction);
if (parentIndex>=0)
{
mb->getLink(linkIndex-1).m_collider=col;
} else
{
mb->setBaseCollider(col);
}
}
for (std::vector<my_shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
{
if (*child)
{
URDF2BulletMultiBody(*child,gfxBridge, linkTransformInWorldSpace, world,mappings,pathPrefix,mb,totalNumJoints);
}
else
{
std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl;
}
}
return mb;
}
void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btDiscreteDynamicsWorld* world1, URDF2BulletMappings& mappings, const char* pathPrefix)
{
btCollisionShape* shape = 0;
@@ -877,6 +627,7 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
}
//the btMultiBody support is work-in-progress :-)
#if 0
if (0)
{
URDF2BulletMappings mappings;
@@ -889,7 +640,7 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
//m_dynamicsWorld->integrateTransforms(0.f);
}
#endif//
printf("numJoints/DOFS = %d\n", numJoints);

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

@@ -7,24 +7,66 @@ ConstraintPhysicsSetup::~ConstraintPhysicsSetup()
{
}
btScalar val;
btHingeAccumulatedAngleConstraint* spDoorHinge=0;
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;// spDoorHinge->getHingeAngle()*SIMD_DEGS_PER_RAD;
val=spDoorHinge->getAccumulatedHingeAngle()*SIMD_DEGS_PER_RAD;
if (m_dynamicsWorld)
{
m_dynamicsWorld->stepSimulation(deltaTime);
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);
}
}
}
}
#include "Bullet3Common/b3Logging.h"
void ConstraintPhysicsSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
{
b3Printf(__FILE__);
gfxBridge.setUpAxis(1);
createEmptyDynamicsWorld();
@@ -35,12 +77,35 @@ int mode = btIDebugDraw::DBG_DrawWireframe
+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);
{
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));
@@ -51,27 +116,12 @@ slider.m_maxVal=720;
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);
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

5
btgui/Bullet3AppSupport/CommonParameterInterface.h
View File

@@ -16,6 +16,7 @@ struct SliderParams
btScalar* m_paramValuePointer;
void* m_userPointer;
bool m_clampToNotches;
bool m_showValues;
SliderParams(const char* name, btScalar* targetValuePointer)
:m_name(name),
@@ -24,7 +25,8 @@ struct SliderParams
m_callback(0),
m_paramValuePointer(targetValuePointer),
m_userPointer(0),
m_clampToNotches(false)
m_clampToNotches(false),
m_showValues(true)
{
}
@@ -33,6 +35,7 @@ struct SliderParams
struct CommonParameterInterface
{
virtual void registerSliderFloatParameter(SliderParams& params)=0;
virtual void syncParameters()=0;
virtual void removeAllParameters()=0;

8
btgui/Bullet3AppSupport/GwenParameterInterface.cpp
View File

@@ -10,11 +10,13 @@ struct MySliderEventHandler : public Gwen::Event::Handler
Gwen::Controls::Slider* m_pSlider;
char m_variableName[1024];
T* m_targetValue;
bool m_showValue;
MySliderEventHandler(const char* varName, Gwen::Controls::TextBox* label, Gwen::Controls::Slider* pSlider,T* target)
:m_label(label),
m_pSlider(pSlider),
m_targetValue(target)
m_targetValue(target),
m_showValue(true)
{
memcpy(m_variableName,varName,strlen(varName)+1);
}
@@ -45,9 +47,12 @@ struct MySliderEventHandler : public Gwen::Event::Handler
m_pSlider->SetValue(v,true);
(*m_targetValue) = v;
float val = float(v);//todo: specialize on template type
if (m_showValue)
{
char txt[1024];
sprintf(txt,"%s : %.3f", m_variableName,val);
m_label->SetText(txt);
}
}
};
@@ -120,6 +125,7 @@ void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params)
char labelName[1024];
sprintf(labelName,"%s",params.m_name);//axisNames[0]);
MySliderEventHandler<btScalar>* handler = new MySliderEventHandler<btScalar>(labelName,label,pSlider,params.m_paramValuePointer);
handler->m_showValue = params.m_showValues;
m_paramInternalData->m_sliderEventHandlers.push_back(handler);
pSlider->onValueChanged.Add( handler, &MySliderEventHandler<btScalar>::SliderMoved );