Using bullet physics car and heightfield, car get stuck how can I fix?

Question

I've been working on implementing a car in my game using bullet physics. The physics of the car uses btRaycastVehicle and the code is mostly based on the ForkLift Demo. At this point, the vehicle seems to work properly on a flat ground but then I've started working on a non flat terrain and I saw the class btHeightfieldTerrainShape which take an array of heights to construct a shape.
So I've managed to use this class and the vehicle can be used on it but it sometimes get stuck on terrain's hollows even if the height to climb is really small.

I'm using MLCP constraint solver and tested PGS solver but does not help.
Here is the concerned code :

Vehicle.hpp

#define USE_MLCP_SOLVER
// I removed other #define because all were just floats
 
class Vehicle {
public:
    // theses bools are set to true when the corresponding key is pressed
    bool m_foward = false, m_backward = false, m_leftward = false, m_rightward = false;
    bool m_reset = false;
 
    Vehicle(Vao *chassisVao, Vao *wheelVao, const float *heights, uint32_t gridsize, float amplitude);
 
    ~Vehicle();
 
    // this function runs the logic of the physics simulation, it gets executed each frame
    void stepSimulation(uint32_t frameTime);
 
    // this function instantiate objects to rendered, it gets executed each frame
    // not including definition, not revalent
    void prepareRendering(std::vector<const Entity *> &entities);
 
private:
    // members are declared here --->  <---
 
    // create physics world and vehicle
    void initPhysics(const float *heights, uint32_t gridsize, float amplitude);
 
    // cleanup things
    // not including definition, not revalent
    void exitPhysics(void);
 
    // reset vehicle position, rotation, momentum, etc..
    // not including definition, not revalent
    void resetVehicle(void);
 
    // helper function to create rigid body
    // not including definition, not revalent
    btRigidBody* localCreateRigidBody(btScalar mass, const btTransform& startTransform, btCollisionShape* shape);
};

Vehicle.cpp

#include "Vehicle.hpp"
 
Vehicle::Vehicle(Vao *chassisVao, Vao *wheelVao, const float *heights, uint32_t gridsize, float amplitude) {
    initPhysics(heights, gridsize, amplitude);
 
    if (chassisVao) {
        m_chassisEntity = new Entity(chassisVao);
    }
    for (int i = 0; i < 4; ++i) {
        m_wheelEntities.push_back(Entity(wheelVao));
    }
}
 
Vehicle::~Vehicle() {
    exitPhysics();
 
    if (m_chassisEntity) {
        delete m_chassisEntity;
    }
    m_wheelEntities.clear();
}
 
void Vehicle::initPhysics(const float *heights, uint32_t gridsize, float amplitude) {
    // setup dynamics world
    m_collisionConfiguration = new btDefaultCollisionConfiguration();
    m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
    btVector3 worldMin(-1000, -1000, -1000);
    btVector3 worldMax(1000, 1000, 1000);
    m_overlappingPairCache = new btAxisSweep3(worldMin, worldMax);
 
#ifdef USE_MLCP_SOLVER
    btDantzigSolver* mlcp = new btDantzigSolver();
    // btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel();
    btMLCPSolver* sol = new btMLCPSolver(mlcp);
    m_solver = sol;
#else
    m_solver = new btSequentialImpulseConstraintSolver();
#endif
 
    m_world = new btDiscreteDynamicsWorld(m_dispatcher, m_overlappingPairCache, m_solver, m_collisionConfiguration);
#ifdef USE_MLCP_SOLVER
    m_world->getSolverInfo().m_minimumSolverBatchSize = 1;
#else
    m_world->getSolverInfo().m_minimumSolverBatchSize = 128;
#endif
    m_world->getSolverInfo().m_globalCfm = 0.00001;
 
    // create ground object
    // btVector3 groundExtents(100, 3, 100);
    // btCollisionShape* groundShape = new btBoxShape(groundExtents);
    btCollisionShape* groundShape = new btHeightfieldTerrainShape(gridsize + 1, gridsize + 1, heights, 0.0f, amplitude, 1, false);
    m_collisionShapes.push_back(groundShape);
    btTransform tr;
    tr.setIdentity();
    tr.setOrigin(btVector3(gridsize * 0.5f, WHEEL_RADIUS, gridsize * 0.5f));
    localCreateRigidBody(0, tr, groundShape);
 
 
 
 
    // create vehicle
        // BEGIN - create chassis shape
    btVector3 vehicleExtents(1.76f, 1.1f, 4.0f);
    btCollisionShape* chassisShape = new btBoxShape(vehicleExtents);
    m_collisionShapes.push_back(chassisShape);
 
    btCompoundShape* compound = new btCompoundShape();
    m_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));
    compound->addChildShape(localTrans, chassisShape);
 
    tr.setOrigin(btVector3(0, 0, 0));
    m_carChassis = localCreateRigidBody(800, tr, compound);
        // END - create chassis shape
 
        // BEGIN - create vehicle
    m_vehicleRayCaster = new btDefaultVehicleRaycaster(m_world);
    m_vehicle = new btRaycastVehicle(m_tuning, m_carChassis, m_vehicleRayCaster);
    m_carChassis->setActivationState(DISABLE_DEACTIVATION); // never deactivate the vehicle
    m_world->addVehicle(m_vehicle);
 
    // choose coordinate system
    m_vehicle->setCoordinateSystem(0, 1, 2);
 
    btVector3 wheelDirection(0, -1, 0);
    btVector3 wheelAxis(-1, 0, 0);
    btVector3 connectionPoint(0.5f * vehicleExtents.x(), WHEEL_RADIUS, 0.5f * vehicleExtents.z() - WHEEL_RADIUS);
    m_vehicle->addWheel(connectionPoint, wheelDirection, wheelAxis, SUSPENSION_REST_LENGTH, WHEEL_RADIUS, m_tuning, true);
    connectionPoint = btVector3(-0.5f * vehicleExtents.x(), WHEEL_RADIUS, 0.5f * vehicleExtents.z() - WHEEL_RADIUS);
    m_vehicle->addWheel(connectionPoint, wheelDirection, wheelAxis, SUSPENSION_REST_LENGTH, WHEEL_RADIUS, m_tuning, true);
    connectionPoint = btVector3(0.5f * vehicleExtents.x(), WHEEL_RADIUS, -0.5f * vehicleExtents.z() + WHEEL_RADIUS);
    m_vehicle->addWheel(connectionPoint, wheelDirection, wheelAxis, SUSPENSION_REST_LENGTH, WHEEL_RADIUS, m_tuning, false);
    connectionPoint = btVector3(-0.5f * vehicleExtents.x(), WHEEL_RADIUS, -0.5f * vehicleExtents.z() + WHEEL_RADIUS);
    m_vehicle->addWheel(connectionPoint, wheelDirection, wheelAxis, SUSPENSION_REST_LENGTH, WHEEL_RADIUS, m_tuning, false);
 
    for (int i = 0; i < m_vehicle->getNumWheels(); i++) {
        btWheelInfo& wheel = m_vehicle->getWheelInfo(i);
        wheel.m_suspensionStiffness = SUSPENSION_STIFFNESS;
        wheel.m_wheelsDampingRelaxation = SUSPENSION_DAMPING;
        wheel.m_wheelsDampingCompression = SUSPENSION_COMPRESSION;
        wheel.m_frictionSlip = WHEEL_FRICTION;
        wheel.m_rollInfluence = ROLL_IN_INFLUENCE;
    }
 
    resetVehicle();
}

void Vehicle::stepSimulation(uint32_t frameTime) {
    float speed = m_vehicle->getCurrentSpeedKmHour();
    
    m_vehicleEngineForce = 0.0f;
    m_vehicleBreakingForce = 0.0f;
    /* --->
    Processing input sets m_vehicleEngineForce, m_vehicleBreakingForce, m_vehicleSteering
    <--- */

 
    m_vehicle->applyEngineForce(m_vehicleEngineForce, 2);
    m_vehicle->setBrake(m_vehicleBreakingForce, 2);
    m_vehicle->applyEngineForce(m_vehicleEngineForce, 3);
    m_vehicle->setBrake(m_vehicleBreakingForce, 3);
 
    m_vehicle->setSteeringValue(m_vehicleSteering, 0);
    m_vehicle->setSteeringValue(m_vehicleSteering, 1);
 
    m_world->stepSimulation(frameTime * 0.001f, 2);
    btMLCPSolver *solver = (btMLCPSolver *) m_world->getConstraintSolver();
    int numFallbacks = solver->getNumFallbacks();
    if (numFallbacks) {
        std::cerr << "MLCP solver failed " << numFallbacks << " times, falling back to btSequentialImpulseSolver" << std::endl;
    }
    solver->setNumFallbacks(0);
}

And here is a video to illustrate : link

Thank you

Answer 1

I finally solve this issue, I used bullet physics debug drawer to view bounding boxes. The problem was the chassis shape colliding on the terrain because btBoxShape takes the half extent, so I multiplied everything by 0.5 and it works well now.

Here is the debugger code written in C++ for modern OpenGL, based on this forum thread :

BulletDebugDrawer.hpp

#ifndef BULLET_DEBUG_DRAWER_H
#define BULLET_DEBUG_DRAWER_H

#include <bullet/LinearMath/btIDebugDraw.h>
#include <vector>

class BulletDebugDrawer : public btIDebugDraw {
private:
    int m_debugMode;

    std::vector<float> m_lines;

public:
    BulletDebugDrawer();

    virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color);

    virtual void reportErrorWarning(const char* warningString);

    virtual void setDebugMode(int debugMode);

    virtual int getDebugMode(void) const;

    virtual void drawContactPoint(const btVector3& PointOnB, const btVector3& normalOnB, btScalar distance, int lifeTime, const btVector3& color) {

    }

    virtual void draw3dText(const btVector3& location, const char* textString) {

    }

    void glfw3_device_create(void);

    void glfw3_device_render(const float *matrix);

    void glfw3_device_destroy(void);

};
#endif

BulletDebugDrawer.cpp

#include "BulletDebugDrawer.hpp"

#include <algorithm>
#include <cstdint>
#include <iostream>
#include <glad/gl.h>

#define MAX_LINES_DRAWCALL 1000

GLuint dev_program;
GLint dev_uniform_proj;
GLint dev_uniform_col;
GLint dev_attrib_pos;

GLuint dev_vao;
GLuint dev_vbo;

BulletDebugDrawer::BulletDebugDrawer() : m_debugMode(0) {

}

void BulletDebugDrawer::drawLine(const btVector3& from,const btVector3& to, const btVector3& color) {
    m_lines.push_back(from.getX());
    m_lines.push_back(from.getY());
    m_lines.push_back(from.getZ());

    m_lines.push_back(to.getX());
    m_lines.push_back(to.getY());
    m_lines.push_back(to.getZ());
}

void BulletDebugDrawer::setDebugMode(int debugMode) {
    m_debugMode = debugMode;
}

int BulletDebugDrawer::getDebugMode() const {
    return m_debugMode;
}

void BulletDebugDrawer::reportErrorWarning(const char* warningString) {
    std::cout << warningString << std::endl;
}

void BulletDebugDrawer::glfw3_device_create(void) {
    GLint status;
    static const GLchar *vertex_shader =
        "#version 150\n"
        "uniform mat4 ProjMtx;\n"
        "in vec3 Position;\n"
        "void main() {\n"
        "   gl_Position = ProjMtx * vec4(Position, 1);\n"
        "}\n";
    static const GLchar *fragment_shader =
        "#version 150\n"
        "uniform vec3 Color;\n"
        "out vec4 Out_Color;\n"
        "void main(){\n"
        "   Out_Color = vec4(Color, 1);\n"
        "}\n";

    dev_program = glCreateProgram();
    GLuint vert_shdr = glCreateShader(GL_VERTEX_SHADER);
    GLuint frag_shdr = glCreateShader(GL_FRAGMENT_SHADER);
    glShaderSource(vert_shdr, 1, &vertex_shader, 0);
    glShaderSource(frag_shdr, 1, &fragment_shader, 0);
    glCompileShader(vert_shdr);
    glCompileShader(frag_shdr);
    glGetShaderiv(vert_shdr, GL_COMPILE_STATUS, &status);
    assert(status == GL_TRUE);
    glGetShaderiv(frag_shdr, GL_COMPILE_STATUS, &status);
    assert(status == GL_TRUE);
    glAttachShader(dev_program, vert_shdr);
    glAttachShader(dev_program, frag_shdr);
    glLinkProgram(dev_program);
    glGetProgramiv(dev_program, GL_LINK_STATUS, &status);
    assert(status == GL_TRUE);
    glDetachShader(dev_program, vert_shdr);
        glDetachShader(dev_program, frag_shdr);
        glDeleteShader(vert_shdr);
        glDeleteShader(frag_shdr);

    dev_uniform_proj = glGetUniformLocation(dev_program, "ProjMtx");
    dev_uniform_col = glGetUniformLocation(dev_program, "Color");
    dev_attrib_pos = glGetAttribLocation(dev_program, "Position");

    {
        /* buffer setup */
        glGenBuffers(1, &dev_vbo);
        glGenVertexArrays(1, &dev_vao);

        glBindVertexArray(dev_vao);
        glBindBuffer(GL_ARRAY_BUFFER, dev_vbo);
        glBufferData(GL_ARRAY_BUFFER, MAX_LINES_DRAWCALL * 24, nullptr, GL_STREAM_DRAW);

        glEnableVertexAttribArray(dev_attrib_pos);

        glVertexAttribPointer(dev_attrib_pos, 3, GL_FLOAT, GL_FALSE, 12, 0);
    }

    glBindBuffer(GL_ARRAY_BUFFER, 0);
    glBindVertexArray(0);
}

void BulletDebugDrawer::glfw3_device_render(const float *matrix) {
    glUseProgram(dev_program);
    glUniformMatrix4fv(dev_uniform_proj, 1, GL_FALSE, matrix);
    glUniform3f(dev_uniform_col, 1.0f, 0.0f, 0.0f);

    glBindVertexArray(dev_vao);
    glBindBuffer(GL_ARRAY_BUFFER, dev_vbo);

    for (int i = 0; i < m_lines.size(); i += 2 * MAX_LINES_DRAWCALL) {
        int batchVertexCount = std::min<int>(m_lines.size() - i, 2 * MAX_LINES_DRAWCALL);
        glBufferSubData(GL_ARRAY_BUFFER, 0, batchVertexCount * 12, reinterpret_cast<void *>(m_lines.data() + i));
        glDrawArrays(GL_LINES, 0, batchVertexCount);
    }

    glBindBuffer(GL_ARRAY_BUFFER, 0);
    glBindVertexArray(0);
    glUseProgram(0);

    m_lines.clear();
}

void BulletDebugDrawer::glfw3_device_destroy(void) {
    glDeleteProgram(dev_program);
    glDeleteBuffers(1, &dev_vbo);
    glDeleteVertexArrays(1, &dev_vao);
}