void btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedTransform)
{
btWheelInfo& wheel = m_wheelInfo[ wheelIndex ];
updateWheelTransformsWS(wheel,interpolatedTransform);
btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS;
btVector3 fwd = up.cross(right);
fwd = fwd.normalize();
// up = right.cross(fwd);
// up.normalize();
//rotate around steering over de wheelAxleWS
btScalar steering = wheel.m_steering;
btQuaternion steeringOrn(up,steering);//wheel.m_steering);
btMatrix3x3 steeringMat(steeringOrn);
btQuaternion rotatingOrn(right,-wheel.m_rotation);
btMatrix3x3 rotatingMat(rotatingOrn);
btMatrix3x3 basis2(
right[0],fwd[0],up[0],
right[1],fwd[1],up[1],
right[2],fwd[2],up[2]
);
wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2);
wheel.m_worldTransform.setOrigin(
wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength
);
}
void VehicleBody::_update_wheel(int p_idx, PhysicsDirectBodyState *s) {
VehicleWheel &wheel = *wheels[p_idx];
_update_wheel_transform(wheel, s);
Vector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS;
const Vector3 &right = wheel.m_raycastInfo.m_wheelAxleWS;
Vector3 fwd = up.cross(right);
fwd = fwd.normalized();
//rotate around steering over de wheelAxleWS
real_t steering = wheel.steers ? m_steeringValue : 0.0;
Basis steeringMat(up, steering);
Basis rotatingMat(right, wheel.m_rotation);
Basis basis2(
right[0], up[0], fwd[0],
right[1], up[1], fwd[1],
right[2], up[2], fwd[2]);
wheel.m_worldTransform.set_basis(steeringMat * rotatingMat * basis2);
//wheel.m_worldTransform.set_basis(basis2 * (steeringMat * rotatingMat));
wheel.m_worldTransform.set_origin(
wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength);
}