Bläddra i källkod

Support for steering axis != suspension axis (#488)

Also allows specifying the wheel up and forward axis to enable camber and toe
Jorrit Rouwe 2 år sedan
förälder
incheckning
4269d8bbc7

+ 31 - 11
Jolt/Physics/Vehicle/TrackedVehicleController.cpp

@@ -411,6 +411,8 @@ bool TrackedVehicleController::SolveLongitudinalAndLateralConstraints(float inDe
 
 void TrackedVehicleController::Draw(DebugRenderer *inRenderer) const 
 {
+	float constraint_size = mConstraint.GetDrawConstraintSize();
+
 	// Draw RPM
 	Body *body = mConstraint.GetVehicleBody();
 	Vec3 rpm_meter_up = body->GetRotation() * mConstraint.GetLocalUp();
@@ -423,7 +425,7 @@ void TrackedVehicleController::Draw(DebugRenderer *inRenderer) const
 								 "Gear: %d, Clutch: %.1f, EngineRPM: %.0f, V: %.1f km/h", 
 								 (double)mForwardInput, (double)mLeftRatio, (double)mRightRatio, (double)mBrakeInput, 
 								 mTransmission.GetCurrentGear(), (double)mTransmission.GetClutchFriction(), (double)mEngine.GetCurrentRPM(), (double)body->GetLinearVelocity().Length() * 3.6);
-	inRenderer->DrawText3D(body->GetPosition(), status, Color::sWhite, mConstraint.GetDrawConstraintSize());
+	inRenderer->DrawText3D(body->GetPosition(), status, Color::sWhite, constraint_size);
 
 	for (const VehicleTrack &t : mTracks)
 	{
@@ -433,32 +435,50 @@ void TrackedVehicleController::Draw(DebugRenderer *inRenderer) const
 		// Calculate where the suspension attaches to the body in world space
 		RVec3 ws_position = body->GetCenterOfMassPosition() + body->GetRotation() * (settings->mPosition - body->GetShape()->GetCenterOfMass());
 
-		DebugRenderer::sInstance->DrawText3D(ws_position, StringFormat("W: %.1f", (double)t.mAngularVelocity), Color::sWhite, 0.1f);
+		DebugRenderer::sInstance->DrawText3D(ws_position, StringFormat("W: %.1f", (double)t.mAngularVelocity), Color::sWhite, constraint_size);
 	}
 
+	RMat44 body_transform = body->GetWorldTransform();
+
 	for (const Wheel *w_base : mConstraint.GetWheels())
 	{
 		const WheelTV *w = static_cast<const WheelTV *>(w_base);
 		const WheelSettings *settings = w->GetSettings();
 
 		// Calculate where the suspension attaches to the body in world space
-		RVec3 ws_position = body->GetCenterOfMassPosition() + body->GetRotation() * (settings->mPosition - body->GetShape()->GetCenterOfMass());
+		RVec3 ws_position = body_transform * settings->mPosition;
+		Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection);
+
+		// Draw suspension
+		RVec3 min_suspension_pos = ws_position + ws_direction * settings->mSuspensionMinLength;
+		RVec3 max_suspension_pos = ws_position + ws_direction * settings->mSuspensionMaxLength;
+		inRenderer->DrawLine(ws_position, min_suspension_pos, Color::sRed);
+		inRenderer->DrawLine(min_suspension_pos, max_suspension_pos, Color::sGreen);
+
+		// Draw current length
+		RVec3 wheel_pos = ws_position + ws_direction * w->GetSuspensionLength();
+		inRenderer->DrawMarker(wheel_pos, w->GetSuspensionLength() < settings->mSuspensionMinLength? Color::sRed : Color::sGreen, constraint_size);
+
+		// Draw wheel basis
+		Vec3 wheel_forward, wheel_up, wheel_right;
+		mConstraint.GetWheelLocalBasis(w, wheel_forward, wheel_up, wheel_right);
+		wheel_forward = body_transform.Multiply3x3(wheel_forward);
+		wheel_up = body_transform.Multiply3x3(wheel_up);
+		wheel_right = body_transform.Multiply3x3(wheel_right);
+		Vec3 steering_axis = body_transform.Multiply3x3(settings->mSteeringAxis);
+		inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_forward, Color::sRed);
+		inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_up, Color::sGreen);
+		inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_right, Color::sBlue);
+		inRenderer->DrawLine(wheel_pos, wheel_pos + steering_axis, Color::sYellow);
 
 		if (w->HasContact())
 		{
 			// Draw contact
-			inRenderer->DrawLine(ws_position, w->GetContactPosition(), w->HasHitHardPoint()? Color::sRed : Color::sGreen); // Red if we hit the 'max up' limit
 			inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactNormal(), Color::sYellow);
 			inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLongitudinal(), Color::sRed);
 			inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLateral(), Color::sBlue);
 
-			DebugRenderer::sInstance->DrawText3D(w->GetContactPosition(), StringFormat("S: %.2f", (double)w->GetSuspensionLength()), Color::sWhite, 0.1f);
-		}
-		else
-		{
-			// Draw 'no hit'
-			Vec3 max_droop = body->GetRotation() * settings->mDirection * (settings->mSuspensionMaxLength + settings->mRadius);
-			inRenderer->DrawLine(ws_position, ws_position + max_droop, Color::sYellow);
+			DebugRenderer::sInstance->DrawText3D(w->GetContactPosition(), StringFormat("S: %.2f", (double)w->GetSuspensionLength()), Color::sWhite, constraint_size);
 		}
 	}
 }

+ 37 - 12
Jolt/Physics/Vehicle/VehicleConstraint.cpp

@@ -117,6 +117,17 @@ VehicleConstraint::~VehicleConstraint()
 		delete w;
 }
 
+void VehicleConstraint::GetWheelLocalBasis(const Wheel *inWheel, Vec3 &outForward, Vec3 &outUp, Vec3 &outRight) const
+{
+	const WheelSettings *settings = inWheel->mSettings;
+
+	Quat steer_rotation = Quat::sRotation(settings->mSteeringAxis, inWheel->mSteerAngle);
+	outUp = steer_rotation * settings->mWheelUp;
+	outForward = steer_rotation * settings->mWheelForward;
+	outRight = outForward.Cross(outUp).Normalized();
+	outForward = outUp.Cross(outRight).Normalized();
+}
+
 Mat44 VehicleConstraint::GetWheelLocalTransform(uint inWheelIndex, Vec3Arg inWheelRight, Vec3Arg inWheelUp) const
 {
 	JPH_ASSERT(inWheelIndex < mWheels.size());
@@ -128,10 +139,10 @@ Mat44 VehicleConstraint::GetWheelLocalTransform(uint inWheelIndex, Vec3Arg inWhe
 	Mat44 wheel_to_rotational = Mat44(Vec4(inWheelRight, 0), Vec4(inWheelUp, 0), Vec4(inWheelUp.Cross(inWheelRight), 0), Vec4(0, 0, 0, 1)).Transposed();
 
 	// Calculate the matrix that takes us from the rotational space to vehicle local space
-	Vec3 local_forward = Quat::sRotation(mUp, wheel->mSteerAngle) * mForward;
-	Vec3 local_right = local_forward.Cross(mUp);
-	Vec3 local_wheel_pos = settings->mPosition + settings->mDirection * wheel->mSuspensionLength;
-	Mat44 rotational_to_local(Vec4(local_right, 0), Vec4(mUp, 0), Vec4(local_forward, 0), Vec4(local_wheel_pos, 1));
+	Vec3 local_forward, local_up, local_right;
+	GetWheelLocalBasis(wheel, local_forward, local_up, local_right);
+	Vec3 local_wheel_pos = settings->mPosition + settings->mSuspensionDirection * wheel->mSuspensionLength;
+	Mat44 rotational_to_local(Vec4(local_right, 0), Vec4(local_up, 0), Vec4(local_forward, 0), Vec4(local_wheel_pos, 1));
 
 	// Calculate transform of rotated wheel
 	return rotational_to_local * Mat44::sRotationX(wheel->mAngle) * wheel_to_rotational;
@@ -152,6 +163,8 @@ void VehicleConstraint::OnStep(float inDeltaTime, PhysicsSystem &inPhysicsSystem
 	// Calculate if this constraint is active by checking if our main vehicle body is active or any of the bodies we touch are active
 	mIsActive = mBody->IsActive();
 
+	RMat44 body_transform = mBody->GetWorldTransform();
+
 	// Test collision for wheels
 	for (uint wheel_index = 0; wheel_index < mWheels.size(); ++wheel_index)
 	{
@@ -165,8 +178,8 @@ void VehicleConstraint::OnStep(float inDeltaTime, PhysicsSystem &inPhysicsSystem
 		w->mSuspensionLength = settings->mSuspensionMaxLength;
 
 		// Test collision to find the floor
-		RVec3 ws_origin = mBody->GetCenterOfMassPosition() + mBody->GetRotation() * (settings->mPosition - mBody->GetShape()->GetCenterOfMass());
-		Vec3 ws_direction = mBody->GetRotation() * settings->mDirection;
+		RVec3 ws_origin = body_transform * settings->mPosition;
+		Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection);
 		if (mVehicleCollisionTester->Collide(inPhysicsSystem, *this, wheel_index, ws_origin, ws_direction, mBody->GetID(), w->mContactBody, w->mContactSubShapeID, w->mContactPosition, w->mContactNormal, w->mSuspensionLength))
 		{
 			// Store ID (pointer is not valid outside of the simulation step)
@@ -182,11 +195,23 @@ void VehicleConstraint::OnStep(float inDeltaTime, PhysicsSystem &inPhysicsSystem
 			mIsActive |= w->mContactBody->IsActive();
 
 			// Determine world space forward using steering angle and body rotation
-			Vec3 forward = mBody->GetRotation() * Quat::sRotation(mUp, w->mSteerAngle) * mForward;
+			Vec3 forward, up, right;
+			GetWheelLocalBasis(w, forward, up, right);
+			forward = body_transform.Multiply3x3(forward);
+			right = body_transform.Multiply3x3(right);
+
+			// The longitudinal axis is in the up/forward plane
+			w->mContactLongitudinal = w->mContactNormal.Cross(right);
+
+			// Make sure that the longitudinal axis is aligned with the forward axis
+			if (w->mContactLongitudinal.Dot(forward) < 0.0f)
+				w->mContactLongitudinal = -w->mContactLongitudinal;
+
+			// Normalize it
+			w->mContactLongitudinal = w->mContactLongitudinal.NormalizedOr(w->mContactNormal.GetNormalizedPerpendicular());
 
-			// Calculate frame of reference for the contact
-			w->mContactLateral = forward.Cross(w->mContactNormal).NormalizedOr(Vec3::sZero());
-			w->mContactLongitudinal = w->mContactNormal.Cross(w->mContactLateral);
+			// The lateral axis is perpendicular to contact normal and longitudinal axis
+			w->mContactLateral = w->mContactLongitudinal.Cross(w->mContactNormal).Normalized();
 		}
 	}
 
@@ -375,7 +400,7 @@ void VehicleConstraint::SetupVelocityConstraint(float inDeltaTime)
 				//
 				// Note that we clamp 1 / cos(alpha) to the range [0.1, 1] in order not to increase the stiffness / damping by too much.
 				// We also ensure that the frequency doesn't go over half the simulation frequency to prevent the spring from getting unstable.
-				Vec3 ws_direction = body_transform.Multiply3x3(settings->mDirection);
+				Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection);
 				float sqrt_cos_angle = sqrt(max(0.1f, ws_direction.Dot(neg_contact_normal)));
 				float damping = settings->mSuspensionDamping / sqrt_cos_angle;
 				float frequency = min(0.5f / inDeltaTime, settings->mSuspensionFrequency / sqrt_cos_angle);
@@ -470,7 +495,7 @@ bool VehicleConstraint::SolvePositionConstraint(float inDeltaTime, float inBaumg
 			// We do this by calculating the axle position at minimum suspension length and making sure it does not go through the
 			// plane defined by the contact normal and the axle position when the contact happened
 			// TODO: This assumes that only the vehicle moved and not the ground as we kept the axle contact plane in world space
-			Vec3 ws_direction = body_transform.Multiply3x3(settings->mDirection);
+			Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection);
 			RVec3 ws_position = body_transform * settings->mPosition;
 			RVec3 min_suspension_pos = ws_position + settings->mSuspensionMinLength * ws_direction;
 			float max_up_error = float(RVec3(w->mContactNormal).Dot(min_suspension_pos) - w->mAxlePlaneConstant);

+ 7 - 0
Jolt/Physics/Vehicle/VehicleConstraint.h

@@ -104,6 +104,13 @@ public:
 	Wheel *						GetWheel(uint inIdx)						{ return mWheels[inIdx]; }
 	const Wheel *				GetWheel(uint inIdx) const					{ return mWheels[inIdx]; }
 
+	/// Get the basis vectors for the wheel in local space to the vehicle body (note: basis does not rotate when the wheel rotates arounds its axis)
+	/// @param inWheel Wheel to fetch basis for
+	/// @param outForward Forward vector for the wheel
+	/// @param outUp Up vector for the wheel
+	/// @param outRight Right vector for the wheel
+	void						GetWheelLocalBasis(const Wheel *inWheel, Vec3 &outForward, Vec3 &outUp, Vec3 &outRight) const;
+
 	/// Get the transform of a wheel in local space to the vehicle body, returns a matrix that transforms a cylinder aligned with the Y axis in body space (not COM space)
 	/// @param inWheelIndex Index of the wheel to fetch
 	/// @param inWheelRight Unit vector that indicates right in model space of the wheel (so if you only have 1 wheel model, you probably want to specify the opposite direction for the left and right wheels)

+ 16 - 4
Jolt/Physics/Vehicle/Wheel.cpp

@@ -13,7 +13,10 @@ JPH_NAMESPACE_BEGIN
 JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheelSettings)
 {
 	JPH_ADD_ATTRIBUTE(WheelSettings, mPosition)
-	JPH_ADD_ATTRIBUTE(WheelSettings, mDirection)
+	JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionDirection)
+	JPH_ADD_ATTRIBUTE(WheelSettings, mSteeringAxis)
+	JPH_ADD_ATTRIBUTE(WheelSettings, mWheelForward)
+	JPH_ADD_ATTRIBUTE(WheelSettings, mWheelUp)
 	JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionMinLength)
 	JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionMaxLength)
 	JPH_ADD_ATTRIBUTE(WheelSettings, mSuspensionPreloadLength)
@@ -26,7 +29,10 @@ JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(WheelSettings)
 void WheelSettings::SaveBinaryState(StreamOut &inStream) const
 {
 	inStream.Write(mPosition);
-	inStream.Write(mDirection);
+	inStream.Write(mSuspensionDirection);
+	inStream.Write(mSteeringAxis);
+	inStream.Write(mWheelForward);
+	inStream.Write(mWheelUp);
 	inStream.Write(mSuspensionMinLength);
 	inStream.Write(mSuspensionMaxLength);
 	inStream.Write(mSuspensionPreloadLength);
@@ -39,7 +45,10 @@ void WheelSettings::SaveBinaryState(StreamOut &inStream) const
 void WheelSettings::RestoreBinaryState(StreamIn &inStream)
 {
 	inStream.Read(mPosition);
-	inStream.Read(mDirection);
+	inStream.Read(mSuspensionDirection);
+	inStream.Read(mSteeringAxis);
+	inStream.Read(mWheelForward);
+	inStream.Read(mWheelUp);
 	inStream.Read(mSuspensionMinLength);
 	inStream.Read(mSuspensionMaxLength);
 	inStream.Read(mSuspensionPreloadLength);
@@ -53,7 +62,10 @@ Wheel::Wheel(const WheelSettings &inSettings) :
 	mSettings(&inSettings),
 	mSuspensionLength(inSettings.mSuspensionMaxLength)
 {
-	JPH_ASSERT(inSettings.mDirection.IsNormalized());
+	JPH_ASSERT(inSettings.mSuspensionDirection.IsNormalized());
+	JPH_ASSERT(inSettings.mSteeringAxis.IsNormalized());
+	JPH_ASSERT(inSettings.mWheelForward.IsNormalized());
+	JPH_ASSERT(inSettings.mWheelUp.IsNormalized());
 	JPH_ASSERT(inSettings.mSuspensionMinLength >= 0.0f);
 	JPH_ASSERT(inSettings.mSuspensionMaxLength >= inSettings.mSuspensionMinLength);
 	JPH_ASSERT(inSettings.mSuspensionPreloadLength >= 0.0f);

+ 4 - 1
Jolt/Physics/Vehicle/Wheel.h

@@ -27,7 +27,10 @@ public:
 	virtual void			RestoreBinaryState(StreamIn &inStream);
 
 	Vec3					mPosition { 0, 0, 0 };						///< Attachment point of wheel suspension in local space of the body
-	Vec3					mDirection { 0, -1, 0 };					///< Direction of the suspension in local space of the body
+	Vec3					mSuspensionDirection { 0, -1, 0 };			///< Direction of the suspension in local space of the body, should point down
+	Vec3					mSteeringAxis { 0, 1, 0 };					///< Direction of the steering axis in local space of the body, should point up (e.g. for a bike would be -mSuspensionDirection)
+	Vec3					mWheelUp { 0, 1, 0 };						///< Up direction when the wheel is in the neutral steering position (usually VehicleConstraintSettings::mUp but can be used to give the wheel camber or for a bike would be -mSuspensionDirection)
+	Vec3					mWheelForward { 0, 0, 1 };					///< Forward direction when the wheel is in the neutral steering position (usually VehicleConstraintSettings::mForward but can be used to give the wheel toe, does not need to be perpendicular to mWheelUp)
 	float					mSuspensionMinLength = 0.3f;				///< How long the suspension is in max raised position relative to the attachment point (m)
 	float					mSuspensionMaxLength = 0.5f;				///< How long the suspension is in max droop position relative to the attachment point (m)
 	float					mSuspensionPreloadLength = 0.0f;			///< The natural length (m) of the suspension spring is defined as mSuspensionMaxLength + mSuspensionPreloadLength. Can be used to preload the suspension as the spring is compressed by mSuspensionPreloadLength when the suspension is in max droop position. Note that this means when the vehicle touches the ground there is a discontinuity so it will also make the vehicle more bouncy as we're updating with discrete time steps.

+ 20 - 6
Jolt/Physics/Vehicle/WheeledVehicleController.cpp

@@ -654,14 +654,16 @@ void WheeledVehicleController::Draw(DebugRenderer *inRenderer) const
 								 mTransmission.GetCurrentGear(), (double)mTransmission.GetClutchFriction(), (double)mEngine.GetCurrentRPM(), (double)body->GetLinearVelocity().Length() * 3.6);
 	inRenderer->DrawText3D(body->GetPosition(), status, Color::sWhite, constraint_size);
 
+	RMat44 body_transform = body->GetWorldTransform();
+
 	for (const Wheel *w_base : mConstraint.GetWheels())
 	{
 		const WheelWV *w = static_cast<const WheelWV *>(w_base);
 		const WheelSettings *settings = w->GetSettings();
 
 		// Calculate where the suspension attaches to the body in world space
-		RVec3 ws_position = body->GetCenterOfMassPosition() + body->GetRotation() * (settings->mPosition - body->GetShape()->GetCenterOfMass());
-		Vec3 ws_direction = body->GetRotation() * settings->mDirection;
+		RVec3 ws_position = body_transform * settings->mPosition;
+		Vec3 ws_direction = body_transform.Multiply3x3(settings->mSuspensionDirection);
 
 		// Draw suspension
 		RVec3 min_suspension_pos = ws_position + ws_direction * settings->mSuspensionMinLength;
@@ -670,8 +672,20 @@ void WheeledVehicleController::Draw(DebugRenderer *inRenderer) const
 		inRenderer->DrawLine(min_suspension_pos, max_suspension_pos, Color::sGreen);
 
 		// Draw current length
-		RVec3 cur_suspension_pos = ws_position + ws_direction * w->GetSuspensionLength();
-		inRenderer->DrawMarker(cur_suspension_pos, w->GetSuspensionLength() < settings->mSuspensionMinLength? Color::sRed : Color::sGreen, constraint_size);
+		RVec3 wheel_pos = ws_position + ws_direction * w->GetSuspensionLength();
+		inRenderer->DrawMarker(wheel_pos, w->GetSuspensionLength() < settings->mSuspensionMinLength? Color::sRed : Color::sGreen, constraint_size);
+
+		// Draw wheel basis
+		Vec3 wheel_forward, wheel_up, wheel_right;
+		mConstraint.GetWheelLocalBasis(w, wheel_forward, wheel_up, wheel_right);
+		wheel_forward = body_transform.Multiply3x3(wheel_forward);
+		wheel_up = body_transform.Multiply3x3(wheel_up);
+		wheel_right = body_transform.Multiply3x3(wheel_right);
+		Vec3 steering_axis = body_transform.Multiply3x3(settings->mSteeringAxis);
+		inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_forward, Color::sRed);
+		inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_up, Color::sGreen);
+		inRenderer->DrawLine(wheel_pos, wheel_pos + wheel_right, Color::sBlue);
+		inRenderer->DrawLine(wheel_pos, wheel_pos + steering_axis, Color::sYellow);
 
 		if (w->HasContact())
 		{
@@ -680,12 +694,12 @@ void WheeledVehicleController::Draw(DebugRenderer *inRenderer) const
 			inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLongitudinal(), Color::sRed);
 			inRenderer->DrawLine(w->GetContactPosition(), w->GetContactPosition() + w->GetContactLateral(), Color::sBlue);
 
-			DebugRenderer::sInstance->DrawText3D(cur_suspension_pos, StringFormat("W: %.1f, S: %.2f\nSlip: %.2f, FrLateral: %.1f, FrLong: %.1f", (double)w->GetAngularVelocity(), (double)w->GetSuspensionLength(), (double)w->mLongitudinalSlip, (double)w->mCombinedLateralFriction, (double)w->mCombinedLongitudinalFriction), Color::sWhite, constraint_size);
+			DebugRenderer::sInstance->DrawText3D(wheel_pos, StringFormat("W: %.1f, S: %.2f\nSlip: %.2f, FrLateral: %.1f, FrLong: %.1f", (double)w->GetAngularVelocity(), (double)w->GetSuspensionLength(), (double)w->mLongitudinalSlip, (double)w->mCombinedLateralFriction, (double)w->mCombinedLongitudinalFriction), Color::sWhite, constraint_size);
 		}
 		else
 		{
 			// Draw 'no hit'
-			DebugRenderer::sInstance->DrawText3D(cur_suspension_pos, StringFormat("W: %.1f", (double)w->GetAngularVelocity()), Color::sRed, constraint_size);
+			DebugRenderer::sInstance->DrawText3D(wheel_pos, StringFormat("W: %.1f", (double)w->GetAngularVelocity()), Color::sRed, constraint_size);
 		}
 	}
 }

+ 45 - 15
Samples/Tests/Vehicle/VehicleConstraintTest.cpp

@@ -53,13 +53,23 @@ void VehicleConstraintTest::Initialize()
 	vehicle.mMaxPitchRollAngle = sMaxRollAngle;
 
 	// Suspension direction
-	Vec3 front_dir = Vec3(Tan(sFrontSuspensionKinPinAngle), -1, Tan(sFrontSuspensionCasterAngle)).Normalized();
-	Vec3 rear_dir = Vec3(Tan(sRearSuspensionKingPinAngle), -1, Tan(sRearSuspensionCasterAngle)).Normalized();
-
-	// Wheels
+	Vec3 front_suspension_dir = Vec3(Tan(sFrontSuspensionSidewaysAngle), -1, Tan(sFrontSuspensionForwardAngle)).Normalized();
+	Vec3 front_steering_axis = Vec3(-Tan(sFrontKingPinAngle), 1, -Tan(sFrontCasterAngle)).Normalized();
+	Vec3 front_wheel_up = Vec3(Sin(sFrontCamber), Cos(sFrontCamber), 0);
+	Vec3 front_wheel_forward = Vec3(-Sin(sFrontToe), 0, Cos(sFrontToe));
+	Vec3 rear_suspension_dir = Vec3(Tan(sRearSuspensionSidewaysAngle), -1, Tan(sRearSuspensionForwardAngle)).Normalized();
+	Vec3 rear_steering_axis = Vec3(-Tan(sRearKingPinAngle), 1, -Tan(sRearCasterAngle)).Normalized();
+	Vec3 rear_wheel_up = Vec3(Sin(sRearCamber), Cos(sRearCamber), 0);
+	Vec3 rear_wheel_forward = Vec3(-Sin(sRearToe), 0, Cos(sRearToe));
+	Vec3 flip_x(-1, 1, 1);
+
+	// Wheels, left front
 	WheelSettingsWV *w1 = new WheelSettingsWV;
 	w1->mPosition = Vec3(half_vehicle_width, -0.9f * half_vehicle_height, half_vehicle_length - 2.0f * wheel_radius);
-	w1->mDirection = front_dir;
+	w1->mSuspensionDirection = front_suspension_dir;
+	w1->mSteeringAxis = front_steering_axis;
+	w1->mWheelUp = front_wheel_up;
+	w1->mWheelForward = front_wheel_forward;
 	w1->mSuspensionMinLength = sFrontSuspensionMinLength;
 	w1->mSuspensionMaxLength = sFrontSuspensionMaxLength;
 	w1->mSuspensionFrequency = sFrontSuspensionFrequency;
@@ -67,9 +77,13 @@ void VehicleConstraintTest::Initialize()
 	w1->mMaxSteerAngle = sMaxSteeringAngle;
 	w1->mMaxHandBrakeTorque = 0.0f; // Front wheel doesn't have hand brake
 
+	// Right front
 	WheelSettingsWV *w2 = new WheelSettingsWV;
 	w2->mPosition = Vec3(-half_vehicle_width, -0.9f * half_vehicle_height, half_vehicle_length - 2.0f * wheel_radius);
-	w2->mDirection = Vec3(-1, 1, 1) * front_dir;
+	w2->mSuspensionDirection = flip_x * front_suspension_dir;
+	w2->mSteeringAxis = flip_x * front_steering_axis;
+	w2->mWheelUp = flip_x * front_wheel_up;
+	w2->mWheelForward = flip_x * front_wheel_forward;
 	w2->mSuspensionMinLength = sFrontSuspensionMinLength;
 	w2->mSuspensionMaxLength = sFrontSuspensionMaxLength;
 	w2->mSuspensionFrequency = sFrontSuspensionFrequency;
@@ -77,18 +91,26 @@ void VehicleConstraintTest::Initialize()
 	w2->mMaxSteerAngle = sMaxSteeringAngle;
 	w2->mMaxHandBrakeTorque = 0.0f; // Front wheel doesn't have hand brake
 
+	// Left rear
 	WheelSettingsWV *w3 = new WheelSettingsWV;
 	w3->mPosition = Vec3(half_vehicle_width, -0.9f * half_vehicle_height, -half_vehicle_length + 2.0f * wheel_radius);
-	w3->mDirection = rear_dir;
+	w3->mSuspensionDirection = rear_suspension_dir;
+	w3->mSteeringAxis = rear_steering_axis;
+	w3->mWheelUp = rear_wheel_up;
+	w3->mWheelForward = rear_wheel_forward;
 	w3->mSuspensionMinLength = sRearSuspensionMinLength;
 	w3->mSuspensionMaxLength = sRearSuspensionMaxLength;
 	w3->mSuspensionFrequency = sRearSuspensionFrequency;
 	w3->mSuspensionDamping = sRearSuspensionDamping;
 	w3->mMaxSteerAngle = 0.0f;
 
+	// Right rear
 	WheelSettingsWV *w4 = new WheelSettingsWV;
 	w4->mPosition = Vec3(-half_vehicle_width, -0.9f * half_vehicle_height, -half_vehicle_length + 2.0f * wheel_radius);
-	w4->mDirection = Vec3(-1, 1, 1) * rear_dir;
+	w4->mSuspensionDirection = flip_x * rear_suspension_dir;
+	w4->mSteeringAxis = flip_x * rear_steering_axis;
+	w4->mWheelUp = flip_x * rear_wheel_up;
+	w4->mWheelForward = flip_x * rear_wheel_forward;
 	w4->mSuspensionMinLength = sRearSuspensionMinLength;
 	w4->mSuspensionMaxLength = sRearSuspensionMaxLength;
 	w4->mSuspensionFrequency = sRearSuspensionFrequency;
@@ -234,23 +256,31 @@ void VehicleConstraintTest::CreateSettingsMenu(DebugUI *inUI, UIElement *inSubMe
 {
 	VehicleTest::CreateSettingsMenu(inUI, inSubMenu);
 
-	inUI->CreateSlider(inSubMenu, "Initial Roll Angle", RadiansToDegrees(sInitialRollAngle), 0.0f, 90.0f, 0.1f, [](float inValue) { sInitialRollAngle = DegreesToRadians(inValue); });
-	inUI->CreateSlider(inSubMenu, "Max Roll Angle", RadiansToDegrees(sMaxRollAngle), 0.0f, 90.0f, 0.1f, [](float inValue) { sMaxRollAngle = DegreesToRadians(inValue); });
-	inUI->CreateSlider(inSubMenu, "Max Steering Angle", RadiansToDegrees(sMaxSteeringAngle), 0.0f, 90.0f, 0.1f, [](float inValue) { sMaxSteeringAngle = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Initial Roll Angle", RadiansToDegrees(sInitialRollAngle), 0.0f, 90.0f, 1.0f, [](float inValue) { sInitialRollAngle = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Max Roll Angle", RadiansToDegrees(sMaxRollAngle), 0.0f, 90.0f, 1.0f, [](float inValue) { sMaxRollAngle = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Max Steering Angle", RadiansToDegrees(sMaxSteeringAngle), 0.0f, 90.0f, 1.0f, [](float inValue) { sMaxSteeringAngle = DegreesToRadians(inValue); });
 	inUI->CreateComboBox(inSubMenu, "Collision Mode", { "Ray", "Cast Sphere", "Cast Cylinder" }, sCollisionMode, [](int inItem) { sCollisionMode = inItem; });
 	inUI->CreateCheckBox(inSubMenu, "4 Wheel Drive", sFourWheelDrive, [](UICheckBox::EState inState) { sFourWheelDrive = inState == UICheckBox::STATE_CHECKED; });
 	inUI->CreateCheckBox(inSubMenu, "Anti Rollbars", sAntiRollbar, [](UICheckBox::EState inState) { sAntiRollbar = inState == UICheckBox::STATE_CHECKED; });
 	inUI->CreateCheckBox(inSubMenu, "Limited Slip Differentials", sLimitedSlipDifferentials, [](UICheckBox::EState inState) { sLimitedSlipDifferentials = inState == UICheckBox::STATE_CHECKED; });
 	inUI->CreateSlider(inSubMenu, "Max Engine Torque", float(sMaxEngineTorque), 100.0f, 2000.0f, 10.0f, [](float inValue) { sMaxEngineTorque = inValue; });
 	inUI->CreateSlider(inSubMenu, "Clutch Strength", float(sClutchStrength), 1.0f, 40.0f, 1.0f, [](float inValue) { sClutchStrength = inValue; });
-	inUI->CreateSlider(inSubMenu, "Front Suspension Caster Angle", RadiansToDegrees(sFrontSuspensionCasterAngle), -89.0f, 89.0f, 1.0f, [](float inValue) { sFrontSuspensionCasterAngle = DegreesToRadians(inValue); });
-	inUI->CreateSlider(inSubMenu, "Front Suspension King Pin Angle", RadiansToDegrees(sFrontSuspensionKinPinAngle), -89.0f, 89.0f, 1.0f, [](float inValue) { sFrontSuspensionKinPinAngle = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Front Caster Angle", RadiansToDegrees(sFrontCasterAngle), -89.0f, 89.0f, 1.0f, [](float inValue) { sFrontCasterAngle = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Front King Pin Angle", RadiansToDegrees(sFrontKingPinAngle), -89.0f, 89.0f, 1.0f, [](float inValue) { sFrontKingPinAngle = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Front Camber", RadiansToDegrees(sFrontCamber), -89.0f, 89.0f, 1.0f, [](float inValue) { sFrontCamber = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Front Toe", RadiansToDegrees(sFrontToe), -89.0f, 89.0f, 1.0f, [](float inValue) { sFrontToe = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Front Suspension Forward Angle", RadiansToDegrees(sFrontSuspensionForwardAngle), -89.0f, 89.0f, 1.0f, [](float inValue) { sFrontSuspensionForwardAngle = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Front Suspension Sideways Angle", RadiansToDegrees(sFrontSuspensionSidewaysAngle), -89.0f, 89.0f, 1.0f, [](float inValue) { sFrontSuspensionSidewaysAngle = DegreesToRadians(inValue); });
 	inUI->CreateSlider(inSubMenu, "Front Suspension Min Length", sFrontSuspensionMinLength, 0.0f, 3.0f, 0.01f, [](float inValue) { sFrontSuspensionMinLength = inValue; });
 	inUI->CreateSlider(inSubMenu, "Front Suspension Max Length", sFrontSuspensionMaxLength, 0.0f, 3.0f, 0.01f, [](float inValue) { sFrontSuspensionMaxLength = inValue; });
 	inUI->CreateSlider(inSubMenu, "Front Suspension Frequency", sFrontSuspensionFrequency, 0.1f, 5.0f, 0.01f, [](float inValue) { sFrontSuspensionFrequency = inValue; });
 	inUI->CreateSlider(inSubMenu, "Front Suspension Damping", sFrontSuspensionDamping, 0.0f, 2.0f, 0.01f, [](float inValue) { sFrontSuspensionDamping = inValue; });
-	inUI->CreateSlider(inSubMenu, "Rear Suspension Caster Angle", RadiansToDegrees(sRearSuspensionCasterAngle), -89.0f, 89.0f, 1.0f, [](float inValue) { sRearSuspensionCasterAngle = DegreesToRadians(inValue); });
-	inUI->CreateSlider(inSubMenu, "Rear Suspension King Pin Angle", RadiansToDegrees(sRearSuspensionKingPinAngle), -89.0f, 89.0f, 1.0f, [](float inValue) { sRearSuspensionKingPinAngle = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Rear Caster Angle", RadiansToDegrees(sRearCasterAngle), -89.0f, 89.0f, 1.0f, [](float inValue) { sRearCasterAngle = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Rear King Pin Angle", RadiansToDegrees(sRearKingPinAngle), -89.0f, 89.0f, 1.0f, [](float inValue) { sRearKingPinAngle = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Rear Camber", RadiansToDegrees(sRearCamber), -89.0f, 89.0f, 1.0f, [](float inValue) { sRearCamber = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Rear Toe", RadiansToDegrees(sRearToe), -89.0f, 89.0f, 1.0f, [](float inValue) { sRearToe = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Rear Suspension Forward Angle", RadiansToDegrees(sRearSuspensionForwardAngle), -89.0f, 89.0f, 1.0f, [](float inValue) { sRearSuspensionForwardAngle = DegreesToRadians(inValue); });
+	inUI->CreateSlider(inSubMenu, "Rear Suspension Sideways Angle", RadiansToDegrees(sRearSuspensionSidewaysAngle), -89.0f, 89.0f, 1.0f, [](float inValue) { sRearSuspensionSidewaysAngle = DegreesToRadians(inValue); });
 	inUI->CreateSlider(inSubMenu, "Rear Suspension Min Length", sRearSuspensionMinLength, 0.0f, 3.0f, 0.01f, [](float inValue) { sRearSuspensionMinLength = inValue; });
 	inUI->CreateSlider(inSubMenu, "Rear Suspension Max Length", sRearSuspensionMaxLength, 0.0f, 3.0f, 0.01f, [](float inValue) { sRearSuspensionMaxLength = inValue; });
 	inUI->CreateSlider(inSubMenu, "Rear Suspension Frequency", sRearSuspensionFrequency, 0.1f, 5.0f, 0.01f, [](float inValue) { sRearSuspensionFrequency = inValue; });

+ 12 - 4
Samples/Tests/Vehicle/VehicleConstraintTest.h

@@ -35,14 +35,22 @@ private:
 	static inline bool			sLimitedSlipDifferentials = true;
 	static inline float			sMaxEngineTorque = 500.0f;
 	static inline float			sClutchStrength = 10.0f;
-	static inline float			sFrontSuspensionCasterAngle = 0.0f;
-	static inline float 		sFrontSuspensionKinPinAngle = 0.0f;
+	static inline float			sFrontCasterAngle = 0.0f;
+	static inline float 		sFrontKingPinAngle = 0.0f;
+	static inline float			sFrontCamber = 0.0f;
+	static inline float			sFrontToe = 0.0f;
+	static inline float			sFrontSuspensionForwardAngle = 0.0f;
+	static inline float			sFrontSuspensionSidewaysAngle = 0.0f;
 	static inline float			sFrontSuspensionMinLength = 0.3f;
 	static inline float			sFrontSuspensionMaxLength = 0.5f;
 	static inline float			sFrontSuspensionFrequency = 1.5f;
 	static inline float			sFrontSuspensionDamping = 0.5f;
-	static inline float 		sRearSuspensionCasterAngle = 0.0f;
-	static inline float 		sRearSuspensionKingPinAngle = 0.0f;
+	static inline float			sRearSuspensionForwardAngle = 0.0f;
+	static inline float			sRearSuspensionSidewaysAngle = 0.0f;
+	static inline float 		sRearCasterAngle = 0.0f;
+	static inline float 		sRearKingPinAngle = 0.0f;
+	static inline float			sRearCamber = 0.0f;
+	static inline float			sRearToe = 0.0f;
 	static inline float			sRearSuspensionMinLength = 0.3f;
 	static inline float			sRearSuspensionMaxLength = 0.5f;
 	static inline float			sRearSuspensionFrequency = 1.5f;