//Fix64Vec2
public static Fix64Vec2 FindNearestPointOnLine(Fix64Vec2 p, Fix64Vec2 a, Fix64Vec2 b)
{
Fix64Vec2 ba = b - a;
Fix64Vec2 pa = p - a;
Fix64 d = pa.Length();
Fix64 angle = Fix64Vec2.Angle(ba, pa);
if (angle > Fix64.FromDivision(90, 1))
{
angle = Fix64.FromDivision(90, 1);
}
d = d * Fix64.Cos(angle * Fix64.DegreeToRad);
return(a + ba.normalized * d);
}
//https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
public static Fix64Quat FromYawPitchRoll(Fix64 yaw_y, Fix64 pitch_x, Fix64 roll_z)
{
//Fix64 half_yaw = Fix64.Div2(yaw_y);
//Fix64 cy = Fix64.Cos(half_yaw);
//Fix64 sy = Fix64.Sin(half_yaw);
//Fix64 half_pitch = Fix64.Div2(pitch_x);
//Fix64 cp = Fix64.Cos(half_pitch);
//Fix64 sp = Fix64.Sin(half_pitch);
//Fix64 half_roll = Fix64.Div2(roll_z);
//Fix64 cr = Fix64.Cos(half_roll);
//Fix64 sr = Fix64.Sin(half_roll);
//Fix64 w = cr * cp * cy + sr * sp * sy;
//Fix64 x = sr * cp * cy - cr * sp * sy;
//Fix64 y = cr * sp * cy + sr * cp * sy;
//Fix64 z = cr * cp * sy - sr * sp * cy;
//return new Fix64Quat(x, y, z, w);
Fix64 half_roll = Fix64.Div2(roll_z);
Fix64 sr = Fix64.Sin(half_roll);
Fix64 cr = Fix64.Cos(half_roll);
Fix64 half_pitch = Fix64.Div2(pitch_x);
Fix64 sp = Fix64.Sin(half_pitch);
Fix64 cp = Fix64.Cos(half_pitch);
Fix64 half_yaw = Fix64.Div2(yaw_y);
Fix64 sy = Fix64.Sin(half_yaw);
Fix64 cy = Fix64.Cos(half_yaw);
return(new Fix64Quat(
cy * sp * cr + sy * cp * sr,
sy * cp * cr - cy * sp * sr,
cy * cp * sr - sy * sp * cr,
cy * cp * cr + sy * sp * sr));
}
