Vector3f tmp = Stack.alloc(Vector3f.class);
currentVehicleSpeedKmHour = 3.6f * getRigidBody().getLinearVelocity(tmp).length();
Transform chassisTrans = getChassisWorldTransform(Stack.alloc(Transform.class));
Vector3f forwardW = Stack.alloc(Vector3f.class);
forwardW.set(
chassisTrans.basis.getElement(0, indexForwardAxis),
chassisTrans.basis.getElement(1, indexForwardAxis),
chassisTrans.basis.getElement(2, indexForwardAxis));
if (forwardW.dot(getRigidBody().getLinearVelocity(tmp)) < 0f) {
currentVehicleSpeedKmHour *= -1f;
}
//
// simulate suspension
//
int i = 0;
for (i = 0; i < wheelInfo.size(); i++) {
float depth;
depth = rayCast(wheelInfo.getQuick(i));
}
updateSuspension(step);
for (i = 0; i < wheelInfo.size(); i++) {
// apply suspension force
WheelInfo wheel = wheelInfo.getQuick(i);
float suspensionForce = wheel.wheelsSuspensionForce;
if (suspensionForce > wheel.maxSuspensionForce) {
suspensionForce = wheel.maxSuspensionForce;
}
Vector3f impulse = Stack.alloc(Vector3f.class);
impulse.scale(suspensionForce * step, wheel.raycastInfo.contactNormalWS);
Vector3f relpos = Stack.alloc(Vector3f.class);
relpos.sub(wheel.raycastInfo.contactPointWS, getRigidBody().getCenterOfMassPosition(tmp));
getRigidBody().applyImpulse(impulse, relpos);
}
updateFriction(step);
for (i = 0; i < wheelInfo.size(); i++) {
WheelInfo wheel = wheelInfo.getQuick(i);
Vector3f relpos = Stack.alloc(Vector3f.class);
relpos.sub(wheel.raycastInfo.hardPointWS, getRigidBody().getCenterOfMassPosition(tmp));
Vector3f vel = getRigidBody().getVelocityInLocalPoint(relpos, Stack.alloc(Vector3f.class));
if (wheel.raycastInfo.isInContact) {
Transform chassisWorldTransform = getChassisWorldTransform(Stack.alloc(Transform.class));
Vector3f fwd = Stack.alloc(Vector3f.class);
fwd.set(
chassisWorldTransform.basis.getElement(0, indexForwardAxis),
chassisWorldTransform.basis.getElement(1, indexForwardAxis),