I'm trying to get a simple demo of the btRaycastVehicle class working. i've followed the Forklift example as closely as I can, but I can't get the vehicle to move. I've included a minimal example of what I'm trying to do at the end of this post.
First, I create a typical DyanimcsWorld, adding a floor, gravity, and create a shape for the car body. I then construct the btRaycastVehicle using the Forklift demo as a guide. I then step the world, adding engine force and steering commands to the btRaycastVehicle, but its position does not change.
What am I missing here?
Code: Select all
#include <bullet/btBulletDynamicsCommon.h>
#include <bullet/BulletDynamics/MLCPSolvers/btDantzigSolver.h>
#include <bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.h>
#include <iostream>
#include <vector>
using namespace std;
static const btScalar car_x = 0.787;
static const btScalar car_y = 0.7112;
static const btScalar car_z = 2.5;
static const btScalar mass = 1200.0;
class Test {
protected:
btCollisionConfiguration * config;
btBroadphaseInterface * broadphase;
btDantzigSolver * solver_impl;
btConstraintSolver * solver_interface;
btCollisionDispatcher * dispatcher;
btDynamicsWorld * world;
vector<btCollisionShape*> collision_shapes;
vector<btRigidBody*> rigid_bodies;
btScalar timestep;
btScalar time;
vector<btRaycastVehicle*> cars;
btVehicleRaycaster * raycaster;
btCompoundShape * car_collision_shape;
btRaycastVehicle::btVehicleTuning tuning;
public:
struct carCommand {
btScalar engine_force;
btScalar brake_force;
btScalar steering_angle;
};
Test() :
config(new btDefaultCollisionConfiguration()),
broadphase(new btDbvtBroadphase()),
solver_impl(new btDantzigSolver()),
solver_interface(new btMLCPSolver(solver_impl)),
dispatcher(new btCollisionDispatcher(config)),
world(new btDiscreteDynamicsWorld(dispatcher, broadphase, solver_interface, config)),
timestep(1.0/60.0),
time(0) {
// set gravity
world->setGravity(btVector3(0, -9.80665, 0));
// create long flat collision box for ground
auto ground = new btBoxShape(btVector3(100, 0.5, 100));
collision_shapes.push_back(ground);
// inertia of static objects is trivially (0, 0, 0)
btScalar mass = 0;
btVector3 inertia;
ground->calculateLocalInertia(mass, inertia);
// construct the rigidBody
btRigidBody::btRigidBodyConstructionInfo rb_info(mass, nullptr, ground, inertia);
rb_info.m_restitution = 1;
auto body = new btRigidBody(rb_info);
rigid_bodies.push_back(body);
// translate plane to position (offset so top face is aligned with the xz plane)
btTransform pos;
pos.setIdentity();
pos.setOrigin(btVector3(0, -0.5, 0));
body->setWorldTransform(pos);
// add ground
world->addRigidBody(body);
// init car shapes and raycasting tools
raycaster = new btDefaultVehicleRaycaster(world);
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(0, car_y, 0));
car_collision_shape = new btCompoundShape();
btBoxShape * car_body = new btBoxShape(btVector3(car_x, car_y, car_z));
car_collision_shape->addChildShape(tr, car_body);
collision_shapes.push_back(car_collision_shape);
collision_shapes.push_back(car_body);
}
~Test() {
// clean up rigid bodies and collision shapes
for (auto body : rigid_bodies) {
world->removeRigidBody(body);
delete body;
}
for (auto shape : collision_shapes) {
delete shape;
}
for (auto car : cars) {
world->removeAction(car);
delete car;
}
// delete dynamics world
delete raycaster;
delete world;
delete solver_interface;
delete solver_impl;
delete broadphase;
delete dispatcher;
delete config;
}
btRaycastVehicle& addCar() {
// get params for rigid body construction
btVector3 inertia;
car_collision_shape->calculateLocalInertia(mass, inertia);
btRigidBody::btRigidBodyConstructionInfo info(
/*mass = */mass,
/*motionState = */nullptr,
/*collsisionShape = */car_collision_shape,
/*localInertia = */inertia);
// make car
btRigidBody * body = new btRigidBody(info);
btRaycastVehicle * car = new btRaycastVehicle(tuning, body, raycaster);
// register car and vehicle
rigid_bodies.push_back(body);
cars.push_back(car);
// add vehicle to the world
world->addAction(car);
car->setCoordinateSystem(0, 1, 2); // afaik, z-forward needs to be hardcoded
// calculate wheel positions and add wheels to car
btScalar wheel_width = 0.25;
btScalar suspension_length = 0.5;
btScalar wheel_radius = 0.34;
btScalar zf = car_z - wheel_radius - 0.6;
btScalar zr = -(zf - 0.15);
btScalar x = car_x - wheel_width/2 - 0.05 * wheel_width;
btScalar y = wheel_radius * 0.8;
btVector3 connect_lf(x, y, zf);
btVector3 connect_rf(-x, y, zr);
btVector3 connect_lr(x, y, zr);
btVector3 connect_rr(-x, y, zr);
btVector3 wheel_axl(1, 0, 0);
btVector3 wheel_forward(0, 1, 0);
car->addWheel(connect_lf, wheel_forward, wheel_axl, suspension_length, wheel_radius, tuning, true);
car->addWheel(connect_rf, wheel_forward, wheel_axl, suspension_length, wheel_radius, tuning, true);
car->addWheel(connect_lr, wheel_forward, wheel_axl, suspension_length, wheel_radius, tuning, false);
car->addWheel(connect_rr, wheel_forward, wheel_axl, suspension_length, wheel_radius, tuning, false);
// adjust wheel parameters
for (int i = 0; i < car->getNumWheels(); i++) {
btWheelInfo& wheel = car->getWheelInfo(i);
wheel.m_suspensionStiffness = 20;
wheel.m_wheelsDampingRelaxation = 2.3;
wheel.m_wheelsDampingCompression = 4.4;
wheel.m_frictionSlip = 500;
wheel.m_rollInfluence = 0.1;
}
return *car;
}
void setCarControls(btRaycastVehicle& car, carCommand cmd) {
for (int i = 0; i < car.getNumWheels(); i++) {
btWheelInfo& w = car.getWheelInfo(i);
if(w.m_bIsFrontWheel) {
car.setSteeringValue(cmd.steering_angle, i);
} else {
car.applyEngineForce(cmd.engine_force, i);
}
car.setBrake(cmd.brake_force, i);
}
}
btScalar step() {
// step the simulation
world->stepSimulation(timestep, 1, timestep);
time += timestep;
return time;
}
};
int main() {
Test* test = new Test();
btRaycastVehicle& car = test->addCar();
Test::carCommand turn_left = {1000, 10, 2.355};
Test::carCommand turn_right = {1000, 10, 0.785};
// run simulations
for (btScalar time = test->step(); time < 30; time = test->step()) {
if (time < 5 || (time > 10 && time < 15) || (time > 20 && time < 25)) {
test->setCarControls(car, turn_left);
} else {
test->setCarControls(car, turn_right);
}
btVector3 pos = car.getChassisWorldTransform().getOrigin();
cout << time << "," << pos.getX() << "\n";
}
cout << endl;
delete test;
return 0;
}