27 lines
870 B
C#
27 lines
870 B
C#
using System;
|
|
|
|
namespace VeloX;
|
|
|
|
internal readonly struct Hinge( Vector3 hinge_anchor, Vector3 hinge_arm )
|
|
{
|
|
[Description( "the point that the wheels are rotated around as the suspension compresses" )]
|
|
public readonly Vector3 Anchor = hinge_anchor;
|
|
|
|
[Description( "anchor to wheel vector" )]
|
|
public readonly Vector3 Arm = hinge_arm;
|
|
|
|
[Description( "arm length squared" )]
|
|
public readonly float LengthSquared = hinge_arm.Dot( hinge_arm );
|
|
|
|
[Description( "1 / arm length in hinge axis normal plane" )]
|
|
public readonly float NormXY = 1 / MathF.Sqrt( hinge_arm.x * hinge_arm.x + hinge_arm.y * hinge_arm.y );
|
|
|
|
public readonly Vector3 Rotate( float travel )
|
|
{
|
|
float z = Arm.z + travel;
|
|
float lengthSq = MathF.Max( LengthSquared - z * z, 0.0f );
|
|
float nxy = NormXY * MathF.Sqrt( lengthSq );
|
|
return Anchor + new Vector3( Arm.x * nxy, Arm.y * nxy, z );
|
|
}
|
|
}
|