I'm trying to implement Bullet raycast Vehicle (btRayCast Vehicle) in OpensSceneGraph.
I initialized bullet's world and vehicle as (see code below) :-
Bullet World in : initDynamicWorld();
btRayCastVeghicle in : initVehicle(btDynamicsWorld* world,btCylinderShape* wsh,btRigidBody* bb);
After these initializations, i tried to move initialized raycast vehicle using applyEngineForce(force,id) in main function's while loop (simulation loop).
but, if using : carChassis->setActivationState(DISABLE_DEACTIVATION) :-
cout<<vehicle->getCurrentSpeedKmHour() :
output : nan
btTransform btcartrans=vwhicle->getChassisWorldTransform();
cout<< btcartrans.getOrigin().getX()<<", "<<btcartrans.getOrigin().y() <<", "<<btcartrans.getOrigin().z();
output: nan, nan, nan
and,
on commenting : // carChassis->setActivationState(DISABLE_DEACTIVATION)]
cout<<vehicle->getCurrentSpeedKmHour() :
output : 0
btTransform btcartrans=vwhicle->getChassisWorldTransform();
cout<< btcartrans.getOrigin().getX()<<", "<<btcartrans.getOrigin().y() <<", "<<btcartrans.getOrigin().z();
output: 0, 0, 0
below is the code :-
Code: Select all
#include <btBulletDynamicsCommon.h>
#include <btBulletCollisionCommon.h>
#include <osg/Node>
#include <osg/PositionAttitudeTransform>
#include <osgViewer/Viewer>
#include <osgGA/StateSetManipulator>
#include <osgGA/NodeTrackerManipulator>
#include <osgDB/ReadFile>
#include <osgGA/TrackballManipulator>
#include "BULLETutils.h"
#include "testthread.h"
#include "sceneGraphUtil.h"
btDynamicsWorld* initDynamicWorld();
btRaycastVehicle* initVehicle(btDynamicsWorld* world,btCylinderShape* wsh,btRigidBody* bb);
#define FORCE_ZAXIS_UP
#define CUBE_HALF_EXTENTS 1
#ifdef FORCE_ZAXIS_UP
int rightIndex = 0;
int upIndex = 2;
int forwardIndex = 1;
btVector3 wheelDirectionCS0(0,0,-1);
btVector3 wheelAxleCS(1,0,0);
#else
int rightIndex = 0;
int upIndex = 1;
int forwardIndex = 2;
btVector3 wheelDirectionCS0(0,-1,0);
btVector3 wheelAxleCS(-1,0,0);
#endif
int main(int argc, char *argv[])
{
/***** OSG models initializations ******/
osg::ref_ptr<osg::Node> g_node=osgDB::readNodeFile("F:/3D-Database/Idtr_skk/base_idtr_skk 22.flt");
osg::ref_ptr<osg::PositionAttitudeTransform> g_pat=new osg::PositionAttitudeTransform;
g_pat->addChild(g_node);
osg::ref_ptr<osg::Node> c_node=osgDB::readNodeFile("C:/Users/Lokesh/Desktop/OSG(Terrain & 3D Models)/Vechicle/FLT Format/Car Model 1.flt");
osg::ref_ptr<osg::PositionAttitudeTransform> c_pat=new osg::PositionAttitudeTransform;
c_pat->addChild(c_node);
osg::ref_ptr<osg::Group> root=new osg::Group;
root->addChild(g_pat);
root->addChild(c_pat);
/******** osg car wheels******/
osg::ref_ptr<osg::Node> osg_vh_wfl = OSGutil::findNode("rc_lwh",root);
osg::ref_ptr<osg::Node> osg_vh_wfr = OSGutil::findNode("rc_rwh",root);
osg::ref_ptr<osg::Node> osg_vh_wrl = OSGutil::findNode("rc_lwh1",root);
osg::ref_ptr<osg::Node> osg_vh_wrr = OSGutil::findNode("rc_rwh1",root);
osg::ref_ptr<osg::PositionAttitudeTransform> vPat_wfl=new osg::PositionAttitudeTransform;
osg::ref_ptr<osg::PositionAttitudeTransform> vPat_wfr=new osg::PositionAttitudeTransform;
osg::ref_ptr<osg::PositionAttitudeTransform> vPat_wrl=new osg::PositionAttitudeTransform;
osg::ref_ptr<osg::PositionAttitudeTransform> vPat_wrr=new osg::PositionAttitudeTransform;
vPat_wfl->addChild(osg_vh_wfl);
vPat_wfl->addChild(osg_vh_wfr);
vPat_wfl->addChild(osg_vh_wrl);
vPat_wfl->addChild(osg_vh_wrr);
/*********** bullet initializations************/
btDynamicsWorld* world=initDynamicWorld();
btCollisionShape* groundShape =BtUtil::getTriangleMeshShapeOSG(g_node);
btTransform tr(btQuaternion(0,0,0,1),btVector3(0,0,-4));
tr.setOrigin(btVector3(0,-4.5f,0));
btRigidBody* groundRigidBody = BtUtil::createRigidBody(0,tr,groundShape,btVector3(0,0,0));
world->addRigidBody(groundRigidBody);
btCylinderShape* whsh=new btCylinderShapeX(btVector3(0.4f,0.5f,0.5f));
btRigidBody* carcass;
btRaycastVehicle* vwhicle=initVehicle(world,whsh,carcass);
world->addVehicle(vwhicle);
/****** init osg viewer*******/
osgViewer::Viewer viewer;
viewer.setSceneData(root.get());
viewer.setCameraManipulator( new osgGA::TrackballManipulator );
viewer.addEventHandler( new osgGA::StateSetManipulator(viewer.getCamera()->getOrCreateStateSet()));
viewer.realize();
btScalar m[16];
btScalar mm[4][16];
double prevSimTime = 0.;
/********* simulation loop********/
while(!viewer.done())
{
const double currSimTime = viewer.getFrameStamp()->getSimulationTime();
double elapsed( currSimTime - prevSimTime );
if( viewer.getFrameStamp()->getFrameNumber() < 3 )
double elapsed = 1./60.;
world->stepSimulation( elapsed, 4, elapsed/4. );
prevSimTime = currSimTime;
vwhicle->applyEngineForce(50,0);
vwhicle->applyEngineForce(50,1);
vwhicle->applyEngineForce(50,2);
vwhicle->applyEngineForce(50,3);
for (int i=0;i<vwhicle->getNumWheels();i++)
{
vwhicle->updateWheelTransform(i,true);
vwhicle->getWheelInfo(i).m_worldTransform.getOpenGLMatrix(mm[i]);
}
std::cout<<"car speed in kmph : "<<vwhicle->getCurrentSpeedKmHour()<<std::endl;
btTransform btcartrans=vwhicle->getChassisWorldTransform();
btcartrans.getOpenGLMatrix(m);
/********************************************** my problem below i.e positions : nan nan nan **************************************************/
std::cout<<"bullet car pos : "<< btcartrans.getOrigin().getX()<<", "<<btcartrans.getOrigin().y() <<", "<<btcartrans.getOrigin().z()<<std::endl;
std::cout<<"bullet car rot : "<< btcartrans.getRotation().getX()<<", "<<btcartrans.getRotation().getX() <<", "<<btcartrans.getRotation().getX()<<std::endl;
/**** update OSG Car PAT*****/
osg::Matrixf mat2.set(m);
c_pat->setPosition(mat2.getTrans());
c_pat->setAttitude(mat2.getRotate());
viewer.frame();
}
return 0;
}
btDynamicsWorld* initDynamicWorld()
{
btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
btCollisionDispatcher* dispatcher = new btCollisionDispatcher( collisionConfiguration );
btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver();
btVector3 worldAabbMin=btVector3( -10000, -10000, -10000);
btVector3 worldAabbMax=btVector3(10000, 10000, 10000);
btBroadphaseInterface* brodAlgointer = new btAxisSweep3(worldAabbMin,worldAabbMax);
btDiscreteDynamicsWorld* dynamicsWorld = new btDiscreteDynamicsWorld( dispatcher, brodAlgointer, solver,collisionConfiguration);
btVector3 gravity=btVector3(0,0,-10);
dynamicsWorld->setGravity(gravity);
return dynamicsWorld;
}
btRaycastVehicle* initVehicle(btDynamicsWorld* wrld, btRigidBody* carChassis)
{
float gEngineForce = 0.f;
float gBreakingForce = 0.f;
float maxEngineForce = 1000.f;//this should be engine/velocity dependent
float maxBreakingForce = 100.f;
float gVehicleSteering = 0.f;
float steeringIncrement = 0.04f;
float steeringClamp = 0.3f;
float wheelRadius = 0.5f; //;
float wheelWidth = 0.4f;
float wheelFriction = 1000;//BT_LARGE_FLOAT;
float suspensionStiffness = 20.f;
float suspensionDamping = 2.3f;
float suspensionCompression = 4.4f;
float rollInfluence = 0.1f;//1.0f;
btScalar suspensionRestLength=0.6;
btCollisionShape* chassisShape = new btBoxShape(btVector3(1.f,2.f, 0.5f));
#ifdef FORCE_ZAXIS_UP
btCompoundShape* compound = new btCompoundShape();
btTransform localTrans;
localTrans.setIdentity();
//localTrans effectively shifts the center of mass with respect to the chassis
localTrans.setOrigin(btVector3(0,0,1));
#else
this->collisionShapes.push_back(chassisShape);
btCompoundShape* compound = new btCompoundShape();
this->btsimulationWorld->collisionShapes.push_back(compound);
btTransform localTrans;
localTrans.setIdentity();
//localTrans effectively shifts the center of mass with respect to the chassis
localTrans.setOrigin(btVector3(0,1,0));
#endif
compound->addChildShape(localTrans,chassisShape);
btTransform tr;
tr.setOrigin(btVector3(0,0.f,0));
carChassis=BtUtil::createRigidBody(800,tr,compound,btVector3(0,0,0));
carChassis->setActivationState(DISABLE_DEACTIVATION);
//m_carChassis->setDamping(0.2,0.2);
wrld->addRigidBody(carChassis);
btRaycastVehicle::btVehicleTuning tunings;
btVehicleRaycaster* vehicleRayCaster = new btDefaultVehicleRaycaster(wrld);
btRaycastVehicle* m_vehicle = new btRaycastVehicle(tunings,carChassis,vehicleRayCaster);
float connectionHeight = 1.0f;
bool isFrontWheel=true;
m_vehicle->setCoordinateSystem(forwardIndex,upIndex,rightIndex);
/*====== 1st wheel======= */
#ifdef FORCE_ZAXIS_UP
btVector3 connectionPointCS0(CUBE_HALF_EXTENTS-(0.3*wheelWidth),2*CUBE_HALF_EXTENTS-wheelRadius, connectionHeight);
#else
btVector3 connectionPointCS0(CUBE_HALF_EXTENTS-(0.3*wheelWidth),connectionHeight,2*CUBE_HALF_EXTENTS-wheelRadius);
#endif
m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,tunings,isFrontWheel);
/*====== 2nd wheel======= */
#ifdef FORCE_ZAXIS_UP
connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),2*CUBE_HALF_EXTENTS-wheelRadius, connectionHeight);
#else
connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),connectionHeight,2*CUBE_HALF_EXTENTS-wheelRadius);
#endif
m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,tunings,isFrontWheel);
/*====== 3rd wheel======= */
#ifdef FORCE_ZAXIS_UP
connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),-2*CUBE_HALF_EXTENTS+wheelRadius,connectionHeight);
#else
connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),connectionHeight,-2*CUBE_HALF_EXTENTS+wheelRadius);
#endif //FORCE_ZAXIS_UP
isFrontWheel = false;
m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,tunings,isFrontWheel);
/*====== 4th wheel======= */
#ifdef FORCE_ZAXIS_UP
connectionPointCS0 = btVector3(CUBE_HALF_EXTENTS-(0.3*wheelWidth),-2*CUBE_HALF_EXTENTS+wheelRadius, connectionHeight);
#else
connectionPointCS0 = btVector3(CUBE_HALF_EXTENTS-(0.3*wheelWidth),connectionHeight,-2*CUBE_HALF_EXTENTS+wheelRadius);
#endif
m_vehicle->addWheel(connectionPointCS0,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,wheelRadius,tunings,isFrontWheel);
/*===== add all wheels to the vehicl ====*/
for (int i=0;i<m_vehicle->getNumWheels();i++)
{
btWheelInfo& wheel = m_vehicle->getWheelInfo(i);
wheel.m_suspensionStiffness = suspensionStiffness;
wheel.m_wheelsDampingRelaxation = suspensionDamping;
wheel.m_wheelsDampingCompression = suspensionCompression;
wheel.m_frictionSlip = wheelFriction;
wheel.m_rollInfluence = rollInfluence;
}
return m_vehicle;
}
in any case i'm not able to move my vehicle. I think i have initialed my bullet world and/or btRayCastVehicle wrong.
please help me.
Regards
-Lokesh