/// <summary>
/// повернуть фрейм вокруг горизонтальной оси (y)
/// </summary>
/// <param name="frame">положение сонара</param>
/// <param name="angle">угол поворота</param>
/// <returns>вектор - новое направление</returns>
public static Frame3D VerticalFrameRotation(Frame3D frame, Angle angle)
{
Matrix m = frame.GetMatrix();
//var right = new Point3D(m[0, 1], m[1, 1], m[2, 1]);
var right = new Point3D(0,-1,0);
Frame3D rotated = frame.Apply(Frame3D.DoRotate(right, angle));
return rotated;
}
/// <summary>
/// повернуть фрейм вокруг вертикальной оси (z)
/// </summary>
/// <param name="frame">положение сонара</param>
/// <param name="angle">угол поворота</param>
/// <returns>вектор - новое направление</returns>
public static Frame3D HorisontalFrameRotation(Frame3D frame, Angle angle)
{
Matrix m = frame.GetMatrix();
//var up = new Point3D(m[0, 2], m[1, 2], m[2, 2]);
var up = new Point3D(0,0,1);
Frame3D rotated = frame.Apply(Frame3D.DoRotate(up, angle));
return rotated;
}
public static Point3D VerticalRotation(Frame3D frame, Angle angle)
{
Matrix m = frame.GetMatrix();
var right = new Point3D(m[0, 1], m[1, 1], m[2, 1]);
Frame3D rotated = frame.Apply(Frame3D.DoRotate(right, angle));
Matrix n = rotated.GetMatrix();
var rotatedFront = new Point3D(n[0, 0], n[1, 0], n[2, 0]);
return rotatedFront;
}
public static Point3D HorisontalRotation(Frame3D frame, Angle angle)
{
Matrix m = frame.GetMatrix();
var up = new Point3D(m[0, 2], m[1, 2], m[2, 2]);
Frame3D rotated = frame.Apply(Frame3D.DoRotate(up, angle));
Matrix n = rotated.GetMatrix();
var rotatedFront = new Point3D(n[0, 0], n[1, 0], n[2, 0]);
return rotatedFront;
}
private void LogObjectsRecursively(Body body, Frame3D offset)
{
LoggingObject logged;
if(!_loggingObjects.TryGetValue(body, out logged))
{
logged = new LoggingObject(body, _root);
_loggingObjects.Add(body, logged);
}
logged.SaveBody(offset, _totalTime);
_currentlyExistingBodies.Add(body);
foreach(Body child in body.Nested)
LogObjectsRecursively(child, offset.Apply(child.Location));
}