33 lines
933 B
C#
33 lines
933 B
C#
using Sandbox;
|
|
using System;
|
|
|
|
namespace VeloX;
|
|
public class MacPhersonSuspension
|
|
{
|
|
public readonly Vector3 WheelOffset;
|
|
public readonly Vector3 UprightTop;
|
|
public readonly Vector3 UprightAxis;
|
|
private readonly Hinge Hinge;
|
|
|
|
public MacPhersonSuspension( Vector3 wheel, Vector3 strutBody, Vector3 strutWheel, Vector3 hingeBody )
|
|
{
|
|
WheelOffset = wheel - strutWheel;
|
|
UprightTop = strutBody;
|
|
UprightAxis = (strutBody - strutWheel).Normal;
|
|
Hinge = new( hingeBody, strutWheel - hingeBody );
|
|
}
|
|
|
|
public void GetWheelTransform( float travel, out Rotation rotation, out Vector3 position )
|
|
{
|
|
Vector3 hingeEnd = Hinge.Rotate( travel );
|
|
Vector3 uprightAxisNew = (UprightTop - hingeEnd).Normal;
|
|
|
|
rotation = Rotation.FromAxis(
|
|
Vector3.Cross( UprightAxis, uprightAxisNew ),
|
|
MathF.Acos( Vector3.Dot( UprightAxis, uprightAxisNew ) ).RadianToDegree()
|
|
);
|
|
|
|
position = hingeEnd + WheelOffset.Transform( rotation );
|
|
}
|
|
}
|