/// <summary>
/// Поставит углы, схватит gimbal lock у pitch, но пока так, хоть Yaw верный.
/// </summary>
private static Frame3D SetAngles(Frame3D frame, BEPUphysics.MathExtensions.Matrix matrix)
{
AIRLab.Mathematics.Matrix m = new AIRLab.Mathematics.Matrix(4, 4);
m[0, 0] = matrix.M11;
m[0, 1] = matrix.M12;
m[0, 2] = matrix.M13;
m[0, 3] = 0.0f;
m[1, 0] = matrix.M21;
m[1, 1] = matrix.M22;
m[1, 2] = matrix.M23;
m[1, 3] = 0.0f;
m[2, 0] = matrix.M31;
m[2, 1] = matrix.M32;
m[2, 2] = matrix.M33;
m[2, 3] = 0.0f;
m[3, 0] = frame.X;
m[3, 1] = frame.Y;
m[3, 2] = frame.Z;
m[3, 3] = 1.0f;
var yaw = Geometry.Atan2(m[1, 0], m[0, 0]);
var pitch = Geometry.Atan2(-m[2, 0], Geometry.Hypot(m[2, 1], m[2, 2]));
var roll = Geometry.Atan2(m[2, 1], m[2, 2]);
Frame3D fr = new Frame3D(frame.X, frame.Y, frame.Z, pitch, yaw, roll);
return(fr);
}
/// <summary>
/// Поставит углы, схватит gimbal lock у pitch, но пока так, хоть Yaw верный.
/// </summary>
private static Frame3D SetAngles(Frame3D frame, BEPUphysics.MathExtensions.Matrix matrix)
{
AIRLab.Mathematics.Matrix m = new AIRLab.Mathematics.Matrix(4, 4);
m[0, 0] = matrix.M11;
m[0, 1] = matrix.M12;
m[0, 2] = matrix.M13;
m[0, 3] = 0.0f;
m[1, 0] = matrix.M21;
m[1, 1] = matrix.M22;
m[1, 2] = matrix.M23;
m[1, 3] = 0.0f;
m[2, 0] = matrix.M31;
m[2, 1] = matrix.M32;
m[2, 2] = matrix.M33;
m[2, 3] = 0.0f;
m[3, 0] = frame.X;
m[3, 1] = frame.Y;
m[3, 2] = frame.Z;
m[3, 3] = 1.0f;
var yaw = Geometry.Atan2(m[1, 0], m[0, 0]);
var pitch = Geometry.Atan2(-m[2, 0], Geometry.Hypot(m[2, 1], m[2, 2]));
var roll = Geometry.Atan2(m[2, 1], m[2, 2]);
Frame3D fr = new Frame3D(frame.X, frame.Y, frame.Z, pitch, yaw, roll);
return fr;
}