/// <summary>
/// Засунет объект в фрейм, переведя в координаты физики, повернув объект на нужные углы.
/// </summary>
public static void PutInFrame(Entity entity, Frame3D frame, Quaternion inititalOrientation)
{
entity.Position = ToSimUnits(Frame3DToVector3(frame.NewX(-frame.X)));
entity.Orientation = Quaternion.CreateFromYawPitchRoll(
//(float)frame.Pitch.Radian, (float)frame.Roll.Radian, -(float)frame.Yaw.Radian);
(float)frame.Roll.Radian, (float)frame.Pitch.Radian, -(float)frame.Yaw.Radian) * inititalOrientation;
}
/// <summary>
/// Запишет координаты объекта в frame, переведя их в координаты экрана.
/// </summary>
public static Frame3D GetFrame(Entity entity, Quaternion inititalOrientation)
{
inititalOrientation = new Quaternion(inititalOrientation.X, inititalOrientation.Y, inititalOrientation.Z, -inititalOrientation.W);
Frame3D f = Vector3ToFrame3D(ToDisplayUnits(entity.Position));
f = f.NewX(-f.X);
BEPUphysics.MathExtensions.Matrix m;
Quaternion q = entity.Orientation;
BEPUphysics.MathExtensions.Matrix.CreateFromQuaternion(ref q, out m);
f = SetAngles(f, m);
Quaternion orient = entity.Orientation * inititalOrientation;
f.Orientation = new Quat(orient.X, orient.Y, orient.Z, orient.W);
return(f);
}