//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)); }