Third Person Camera using JBullet and lwjgl

Question

I am developing a race game in java using jbullet and lwjgl.I am currently having trouble in making my camera follow the vehicle.Here is my code.It follows the car until it rotates.Here is the code.Can someone tell me what i am doing wrong ?!

import static org.lwjgl.opengl.GL11.GL_COLOR_BUFFER_BIT;
import static org.lwjgl.opengl.GL11.GL_DEPTH_BUFFER_BIT;
import static org.lwjgl.opengl.GL11.GL_FRONT_AND_BACK;
import static org.lwjgl.opengl.GL11.GL_LINE;
import static org.lwjgl.opengl.GL11.GL_QUADS;
import static org.lwjgl.opengl.GL11.glBegin;
import static org.lwjgl.opengl.GL11.glCallList;
import static org.lwjgl.opengl.GL11.glClear;
import static org.lwjgl.opengl.GL11.glColor3f;
import static org.lwjgl.opengl.GL11.glEnd;
import static org.lwjgl.opengl.GL11.glLoadIdentity;
import static org.lwjgl.opengl.GL11.glPolygonMode;
import static org.lwjgl.opengl.GL11.glPopMatrix;
import static org.lwjgl.opengl.GL11.glPushMatrix;
import static org.lwjgl.opengl.GL11.glTranslated;
import static org.lwjgl.opengl.GL11.*;

import java.io.File;
import java.io.IOException;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.nio.FloatBuffer;

import javax.vecmath.Matrix4f;
import javax.vecmath.Quat4f;
import javax.vecmath.Vector3f;

import model.Vehicle;

import org.lwjgl.LWJGLException;
import org.lwjgl.input.Keyboard;
import org.lwjgl.opengl.Display;
import org.lwjgl.opengl.DisplayMode;

import utility.LWJGLTimer;
import utility.LookAtCamera;
import utility.Model;
import utility.OBJLoader;
import utility.TerrainLoader;

import com.bulletphysics.collision.broadphase.BroadphaseInterface;
import com.bulletphysics.collision.broadphase.DbvtBroadphase;
import com.bulletphysics.collision.dispatch.CollisionConfiguration;
import com.bulletphysics.collision.dispatch.CollisionDispatcher;
import com.bulletphysics.collision.dispatch.CollisionObject;
import com.bulletphysics.collision.dispatch.CollisionWorld;
import com.bulletphysics.collision.dispatch.DefaultCollisionConfiguration;
import com.bulletphysics.collision.shapes.BoxShape;
import com.bulletphysics.collision.shapes.CollisionShape;
import com.bulletphysics.collision.shapes.SphereShape;
import com.bulletphysics.collision.shapes.StaticPlaneShape;
import com.bulletphysics.dynamics.DiscreteDynamicsWorld;
import com.bulletphysics.dynamics.DynamicsWorld;
import com.bulletphysics.dynamics.RigidBody;
import com.bulletphysics.dynamics.RigidBodyConstructionInfo;
import com.bulletphysics.dynamics.constraintsolver.SequentialImpulseConstraintSolver;
import com.bulletphysics.linearmath.DefaultMotionState;
import com.bulletphysics.linearmath.Transform;

public class PlanoFight {

    private static LookAtCamera camera;

    Vector3f position = new Vector3f(0, 0, 0);

    private DynamicsWorld dynamicsWorld = null;

    public PlanoFight() {       
        setUpDisplay();
        setUpCamera();
        setUpDisplayLists();
        setUpPhysics();
        initializeGL();
        startGameLoop();
    }

    private void setUpDisplayLists() {

    }

    Vehicle vehicle;
    private void startGameLoop() {
        LWJGLTimer timer = new LWJGLTimer();        
        double delta = 0;
        timer.initialize();
        while (!Display.isCloseRequested()) {
            timer.update();
            delta = timer.getElapsedTime();
            dynamicsWorld.stepSimulation(1f / 60f, 10);
            checkInput();
            update(delta);
            render();
            Display.update();
            Display.sync(60);
        }

        cleanup();
        Display.destroy();
        System.exit(0);
    }

    private void cleanup() {

    }

    private void initializeGL() {
    }

    private void setUpDisplay() {
        try {
            Display.setDisplayMode(new DisplayMode(800, 600));
            Display.create();
        } catch (LWJGLException e) {
            e.printStackTrace();
            Display.destroy();
            System.exit(0);
        }
    }

    private void update(double delta) {
        vehicle.update();
    }

    private void render() {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        glEnable(GL_DEPTH_TEST);

        vehicle.updateCamera(camera);
        camera.setMatrices();
        vehicle.render();

    }

    public static void main(String ar[]) {
        new PlanoFight();
    }

    private static void setUpCamera() {
        camera = new LookAtCamera(67, 100f, 0.1f, (float) (Display.getWidth() / Display.getHeight()));
        camera.setPosition(new Vector3f(0,5,0));
    }

    private void checkInput() {
        if (Keyboard.isKeyDown(Keyboard.KEY_DOWN)) {
            vehicle.deacelerate();
        } else if (Keyboard.isKeyDown(Keyboard.KEY_UP)) {
            vehicle.accelerate();
        } else if (Keyboard.isKeyDown(Keyboard.KEY_LEFT)) {
            vehicle.steerLeft();
        } else if (Keyboard.isKeyDown(Keyboard.KEY_RIGHT)) {
            vehicle.steerRight();
        }
    }

    private void setUpPhysics() {

        BroadphaseInterface bInterface = new DbvtBroadphase();
        CollisionConfiguration configuration = new DefaultCollisionConfiguration();
        CollisionDispatcher dispather = new CollisionDispatcher(configuration);
        SequentialImpulseConstraintSolver solver = new SequentialImpulseConstraintSolver();

        dynamicsWorld = new DiscreteDynamicsWorld(dispather, bInterface, solver, configuration);
        dynamicsWorld.setGravity(new Vector3f(0, -10, 0));

        vehicle=new Vehicle(dynamicsWorld);
    }

}

package model;

import static org.lwjgl.opengl.GL11.GL_QUADS;
import static org.lwjgl.opengl.GL11.glBegin;
import static org.lwjgl.opengl.GL11.glCallList;
import static org.lwjgl.opengl.GL11.glColor3f;
import static org.lwjgl.opengl.GL11.glEnd;
import static org.lwjgl.opengl.GL11.glMultMatrix;
import static org.lwjgl.opengl.GL11.glPopMatrix;
import static org.lwjgl.opengl.GL11.glPushMatrix;
import static org.lwjgl.opengl.GL11.glRotated;
import static org.lwjgl.opengl.GL11.glTranslatef;
import static org.lwjgl.opengl.GL11.glVertex3f;

import java.io.File;
import java.io.IOException;
import java.nio.ByteBuffer;
import java.nio.FloatBuffer;

import javax.vecmath.Vector3f;

import org.lwjgl.BufferUtils;
import org.lwjgl.util.glu.Cylinder;

import utility.LookAtCamera;
import utility.Model;
import utility.OBJLoader;

import com.bulletphysics.collision.broadphase.BroadphaseInterface;
import com.bulletphysics.collision.dispatch.CollisionDispatcher;
import com.bulletphysics.collision.dispatch.CollisionObject;
import com.bulletphysics.collision.dispatch.DefaultCollisionConfiguration;
import com.bulletphysics.collision.shapes.BoxShape;
import com.bulletphysics.collision.shapes.CollisionShape;
import com.bulletphysics.collision.shapes.CompoundShape;
import com.bulletphysics.collision.shapes.TriangleIndexVertexArray;
import com.bulletphysics.dynamics.DynamicsWorld;
import com.bulletphysics.dynamics.RigidBody;
import com.bulletphysics.dynamics.RigidBodyConstructionInfo;
import com.bulletphysics.dynamics.constraintsolver.ConstraintSolver;
import com.bulletphysics.dynamics.vehicle.DefaultVehicleRaycaster;
import com.bulletphysics.dynamics.vehicle.RaycastVehicle;
import com.bulletphysics.dynamics.vehicle.VehicleRaycaster;
import com.bulletphysics.dynamics.vehicle.VehicleTuning;
import com.bulletphysics.dynamics.vehicle.WheelInfo;
import com.bulletphysics.linearmath.DefaultMotionState;
import com.bulletphysics.linearmath.Transform;
import com.bulletphysics.util.ObjectArrayList;

public class Vehicle {

    private static final int rightIndex = 0;
    private static final int upIndex = 1;
    private static final int forwardIndex = 2;
    private static final Vector3f wheelDirectionCS0 = new Vector3f(0, -1, 0);
    private static final Vector3f wheelAxleCS = new Vector3f(-1, 0, 0);

    private static float gEngineForce = 0.f;
    private static float gBreakingForce = 0.f;

    private static float maxEngineForce = 1000.f;// this should be
                                                    // engine/velocity dependent
    private static float maxBreakingForce = 100.f;

    private static float gVehicleSteering = 0.f;
    private static float steeringIncrement = 0.04f;
    private static float steeringClamp = 0.3f;
    private static float wheelRadius = 0.5f;
    private static float wheelWidth = 0.4f;
    private static float wheelFriction = 1000;// 1e30f;
    private static float suspensionStiffness = 20.f;
    private static float suspensionDamping = 2.3f;
    private static float suspensionCompression = 4.4f;
    private static float rollInfluence = 0.1f;// 1.0f;

    private static final float suspensionRestLength = 0.6f;

    private static final int CUBE_HALF_EXTENTS = 1;

    public RigidBody carChassis;
    public ObjectArrayList<CollisionShape> collisionShapes = new ObjectArrayList<CollisionShape>();
    public BroadphaseInterface overlappingPairCache;
    public CollisionDispatcher dispatcher;
    public ConstraintSolver constraintSolver;
    public DefaultCollisionConfiguration collisionConfiguration;
    public TriangleIndexVertexArray indexVertexArrays;

    public ByteBuffer vertices;

    public VehicleTuning tuning = new VehicleTuning();
    public VehicleRaycaster vehicleRayCaster;
    public RaycastVehicle vehicle;

    public float cameraHeight;

    public float minCameraDistance;
    public float maxCameraDistance;

    private DynamicsWorld dynamicsWorld;

    private Cylinder cylinder;

    private int carChassisHandle;

    Vector3f direction;

    public Vehicle(DynamicsWorld world) {
        this.dynamicsWorld = world;
        carChassis = null;
        cameraHeight = 4f;
        minCameraDistance = 3f;
        maxCameraDistance = 10f;
        indexVertexArrays = null;
        vertices = null;
        vehicle = null;
        cylinder = new Cylinder();
        direction = new Vector3f(0, 0, -1);
        initPhysics();

        try {
            Model model = OBJLoader.loadModel(new File("res/chassis.obj"));
            carChassisHandle = OBJLoader.createDisplayList(model);
        } catch (IOException e) {
            e.printStackTrace();
        }

    }

    public void initPhysics() {

        CollisionShape groundShape = new BoxShape(new Vector3f(50, 3, 50));
        collisionShapes.add(groundShape);
        Transform tr = new Transform();
        tr.setIdentity();

        tr.origin.set(0, -4.5f, 0);

        localCreateRigidBody(0, tr, groundShape);

        CollisionShape chassisShape = new BoxShape(new Vector3f(1.0f, 0.5f, 2.0f));
        collisionShapes.add(chassisShape);

        CompoundShape compound = new CompoundShape();
        collisionShapes.add(compound);
        Transform localTrans = new Transform();
        localTrans.setIdentity();
        localTrans.origin.set(0, 1, 0);

        compound.addChildShape(localTrans, chassisShape);

        tr.origin.set(0, 0, 0);

        carChassis = localCreateRigidBody(800, tr, compound);

        Transform t = new Transform();
        carChassis.getWorldTransform(t);
        Vector3f forward = new Vector3f();
        t.basis.getRow(0, forward);
        System.out.println("0 " + forward);
        t.basis.getRow(1, forward);
        System.out.println("1 " + forward);
        t.basis.getRow(2, forward);
        System.out.println("2 " + forward);

        clientResetScene();

        // create vehicle
        {
            vehicleRayCaster = new DefaultVehicleRaycaster(dynamicsWorld);
            vehicle = new RaycastVehicle(tuning, carChassis, vehicleRayCaster);

            // never deactivate the vehicle
            carChassis.setActivationState(CollisionObject.DISABLE_DEACTIVATION);

            dynamicsWorld.addVehicle(vehicle);

            float connectionHeight = 1.2f;

            boolean isFrontWheel = true;

            vehicle.setCoordinateSystem(rightIndex, upIndex, forwardIndex);
            Vector3f connectionPointCS0 = new Vector3f(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, 2f * CUBE_HALF_EXTENTS - wheelRadius);

            vehicle.addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
            connectionPointCS0.set(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, 2f * CUBE_HALF_EXTENTS - wheelRadius);
            vehicle.addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
            connectionPointCS0.set(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, -2f * CUBE_HALF_EXTENTS + wheelRadius);
            isFrontWheel = false;
            vehicle.addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
            connectionPointCS0.set(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, -2f * CUBE_HALF_EXTENTS + wheelRadius);
            vehicle.addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);

            for (int i = 0; i < vehicle.getNumWheels(); i++) {
                WheelInfo wheel = vehicle.getWheelInfo(i);
                wheel.suspensionStiffness = suspensionStiffness;
                wheel.wheelsDampingRelaxation = suspensionDamping;
                wheel.wheelsDampingCompression = suspensionCompression;
                wheel.frictionSlip = wheelFriction;
                wheel.rollInfluence = rollInfluence;
            }
        }

    }

    public void render() {

        float matrix[] = new float[16];

        glColor3f(0, 0, 1);
        glBegin(GL_QUADS);
        glVertex3f(100, -4.5f, 100);
        glVertex3f(100, -4.5f, -100);
        glVertex3f(-100, -4.5f, -100);
        glVertex3f(-100, -4.5f, 100);
        glEnd();
        FloatBuffer buffer;

        Transform chassisTr = new Transform();
        carChassis.getMotionState().getWorldTransform(chassisTr);

        CollisionShape chassisShape = carChassis.getCollisionShape();
        if (chassisShape instanceof CompoundShape) {
            CollisionShape childShape = ((CompoundShape) chassisShape).getChildShape(0);
            if (childShape instanceof BoxShape) {
                Vector3f size = new Vector3f();
                ((BoxShape) childShape).getHalfExtentsWithoutMargin(size);
                System.out.println(size.x + "  " + size.y + "  " + size.z);
                System.out.println(chassisTr.origin.x + " " + chassisTr.origin.y + " " + chassisTr.origin.z);
            }
        }

        chassisTr.getOpenGLMatrix(matrix);
        buffer = BufferUtils.createFloatBuffer(16 * 4);
        buffer.clear();
        buffer.put(matrix);
        buffer.flip();
        glColor3f(0, 1, 0);
        glPushMatrix();
        glMultMatrix(buffer);
        glTranslatef(0, 1f, 0);
        glCallList(carChassisHandle);
        glPopMatrix();

        for (int i = 0; i < vehicle.getNumWheels(); i++) {
            vehicle.updateWheelTransform(i, true);
            Transform wheelTr = vehicle.getWheelInfo(i).worldTransform;
            wheelTr.getOpenGLMatrix(matrix);
            buffer = BufferUtils.createFloatBuffer(16 * 4);
            buffer.clear();
            buffer.put(matrix);
            buffer.flip();
            glColor3f(1, 0, 0);
            glPushMatrix();
            glMultMatrix(buffer);
            glRotated(90, 0, 1, 0);
            cylinder.draw(wheelRadius, wheelRadius, wheelWidth, 10, 10);
            glPopMatrix();
        }
    }

    public void update() {
        int wheelIndex = 2;
        vehicle.applyEngineForce(gEngineForce, wheelIndex);
        vehicle.setBrake(gBreakingForce, wheelIndex);
        wheelIndex = 3;
        vehicle.applyEngineForce(gEngineForce, wheelIndex);
        vehicle.setBrake(gBreakingForce, wheelIndex);
        wheelIndex = 0;
        vehicle.setSteeringValue(gVehicleSteering, wheelIndex);
        wheelIndex = 1;
        vehicle.setSteeringValue(gVehicleSteering, wheelIndex);
    }

    public void clientResetScene() {
        gVehicleSteering = 0f;
        Transform tr = new Transform();
        tr.setIdentity();
        carChassis.setCenterOfMassTransform(tr);
        carChassis.setLinearVelocity(new Vector3f(0, 0, 0));
        carChassis.setAngularVelocity(new Vector3f(0, 0, 0));
        dynamicsWorld.getBroadphase().getOverlappingPairCache().cleanProxyFromPairs(carChassis.getBroadphaseHandle(), dynamicsWorld.getDispatcher());
        if (vehicle != null) {
            vehicle.resetSuspension();
            for (int i = 0; i < vehicle.getNumWheels(); i++) {
                vehicle.updateWheelTransform(i, true);
            }
        }
    }

    public void steerLeft() {
        System.out.println("steer left");
        gVehicleSteering += steeringIncrement;
        if (gVehicleSteering > steeringIncrement) {
            gVehicleSteering = steeringClamp;
        }
    }

    public void steerRight() {
        gVehicleSteering -= steeringIncrement;
        if (gVehicleSteering < -steeringClamp)
            gVehicleSteering = -steeringClamp;
    }

    public void accelerate() {
        gEngineForce = maxEngineForce;
        gBreakingForce = 0.0f;
    }

    public void deacelerate() {
        gBreakingForce = maxBreakingForce;
        gEngineForce = 0.0f;
    }

    public void updateCamera(LookAtCamera camera) {
        Transform chassisWorldTrans = new Transform();
        carChassis.getMotionState().getWorldTransform(chassisWorldTrans);

        Transform characterWorldTrans = chassisWorldTrans;
        Vector3f up = new Vector3f();
        characterWorldTrans.basis.getRow(1, up);
        Vector3f backward = new Vector3f();
        characterWorldTrans.basis.getRow(2, backward);
        backward.scale(-1);
        up.normalize();
        backward.normalize();

        camera.lookAtVector.set(characterWorldTrans.origin);

        Vector3f cameraPosition = new Vector3f(camera.lookAtVector.x,camera.lookAtVector.y,camera.lookAtVector.z);
        up.scale(4);
        cameraPosition.add(up);
        backward.scale(8);
        cameraPosition.add(backward);

//      System.out.println("lookat" + camera.lookAtVector);
//      System.out.println("position " + cameraPosition + " angle " + cameraPosition.angle(camera.lookAtVector));
//      System.out.println(" " + backward + " " + up);

        camera.position.set(cameraPosition);
    }

    public RigidBody localCreateRigidBody(float mass, Transform startTransform, CollisionShape shape) {
        boolean isDynamic = (mass != 0f);

        Vector3f localInertia = new Vector3f(0f, 0f, 0f);
        if (isDynamic) {
            shape.calculateLocalInertia(mass, localInertia);
        }
        DefaultMotionState myMotionState = new DefaultMotionState(startTransform);

        RigidBodyConstructionInfo cInfo = new RigidBodyConstructionInfo(mass, myMotionState, shape, localInertia);

        RigidBody body = new RigidBody(cInfo);

        dynamicsWorld.addRigidBody(body);

        return body;
    }

        public void printVector(Vector3f origin) {
            System.out.println(origin.x + " " + origin.y + " " + origin.z);
        }
    }