public CubicSphericalCurve(Vector3D center, MatrixD from, MatrixD to)
{
var tmp = new CubicCurve(from, to);
var avgSpherical = Vector3D.Normalize((from.Translation + to.Translation) / 2 - center);
var quat = QuaternionD.CreateFromTwoVectors(avgSpherical, new Vector3D(1, 0, 0));
var m = MatrixD.CreateFromQuaternion(quat);
m = MatrixD.CreateTranslation(-center) * m;
_matrix = MatrixD.Invert(m);
_curve = new CubicCurve(SphericalExtensions.ToSpherical(Vector3D.Transform(tmp.P0, m)),
SphericalExtensions.ToSpherical(Vector3D.Transform(tmp.P1, m)),
SphericalExtensions.ToSpherical(Vector3D.Transform(tmp.P2, m)),
SphericalExtensions.ToSpherical(Vector3D.Transform(tmp.P3, m)));
}
public void SetOrientation(IMyGyro gyro)
{
if (gyro.Enabled)
{
Vector3D worldRV;
Vector3 pos = cockpit.GetPosition();
Vector3 target = Target - pos;
QuaternionD QRV = QuaternionD.CreateFromTwoVectors(target, cockpit.WorldMatrix.Forward);
Vector3D axis;
double angle;
QRV.GetAxisAngle(out axis, out angle);
worldRV = axis * Math.Log(1 + round0(angle), 2);
Vector3D gyroRV = Vector3D.TransformNormal(worldRV, MatrixD.Transpose(gyro.WorldMatrix));
gyro.Pitch = (float)gyroRV.X;
gyro.Yaw = (float)gyroRV.Y;
gyro.Roll = (float)gyroRV.Z;
}
}
// Rotation operation performed per update
private void PerformRotation()
{
var ray = CreateRayFromCursorPosition();
// Calculate the intersection point
var planeIntersectionPoint = m_rotationPlane.Intersection(ref ray.From, ref ray.Direction);
// Prevent too small steps
if (Vector3D.DistanceSquared(m_rotationStartingPoint, planeIntersectionPoint) < double.Epsilon * 20)
{
return;
}
var fromRotationVector = m_rotationStartingPoint - m_gizmoMatrix.Translation;
var toRotationVector = planeIntersectionPoint - m_gizmoMatrix.Translation;
var rotationMat = MatrixD.CreateFromQuaternion(QuaternionD.CreateFromTwoVectors(fromRotationVector, toRotationVector));
var resultMat = m_storedOrientation * rotationMat * m_storedScale;
resultMat.Translation = m_storedTranslation;
SetWorldMatrix(ref resultMat);
}