/// <summary>
/// Compute target angular velocity in pitch,roll,yaw axes
/// </summary>
Vector3 ComputeTargetAngularVelocity(Vector3d torque, Vector3d moi)
{
var internalVessel = vessel.InternalVessel;
var currentRotation = ReferenceFrame.RotationFromWorldSpace(internalVessel.ReferenceTransform.rotation);
var currentDirection = ReferenceFrame.DirectionFromWorldSpace(internalVessel.ReferenceTransform.up);
QuaternionD rotation;
if (!double.IsNaN(TargetRoll))
{
// Roll angle set => use rotation from currentRotation -> targetRotation
rotation = targetRotation * currentRotation.Inverse();
}
else
{
// Roll angle not set => use rotation from currentDirection -> targetDirection
//FIXME: QuaternionD.FromToRotation method not available at runtime
rotation = Quaternion.FromToRotation(currentDirection, targetDirection);
}
// Compute angles for the rotation in pitch (x), roll (y), yaw (z) axes
float angleFloat;
Vector3 axisFloat;
//FIXME: QuaternionD.ToAngleAxis method not available at runtime
((Quaternion)rotation).ToAngleAxis(out angleFloat, out axisFloat);
double angle = GeometryExtensions.ClampAngle180(angleFloat);
Vector3d axis = axisFloat;
var angles = axis * angle;
angles = vessel.ReferenceFrame.DirectionFromWorldSpace(ReferenceFrame.DirectionToWorldSpace(angles));
return(AnglesToAngularVelocity(angles, torque, moi));
}
public Tuple4 Rotation(ReferenceFrame referenceFrame)
{
if (ReferenceEquals(referenceFrame, null))
{
throw new ArgumentNullException(nameof(referenceFrame));
}
return(referenceFrame.RotationFromWorldSpace(port.nodeTransform.rotation * Quaternion.Euler(90, 0, 0)).ToTuple());
}
public Tuple4 Rotation(ReferenceFrame referenceFrame)
{
if (ReferenceEquals(referenceFrame, null))
{
throw new ArgumentNullException(nameof(referenceFrame));
}
return(referenceFrame.RotationFromWorldSpace(InternalPart.transform.rotation).ToTuple());
}
public static Tuple4 TransformRotation (Tuple4 rotation, ReferenceFrame from, ReferenceFrame to)
{
CheckReferenceFrames (from, to);
return to.RotationFromWorldSpace (from.RotationToWorldSpace (rotation.ToQuaternion ())).ToTuple ();
}
/// <summary>
/// Compute target angular velocity in pitch,roll,yaw axes
/// </summary>
Vector3 ComputeTargetAngularVelocity(Vector3d torque, Vector3d moi)
{
var internalVessel = vessel.InternalVessel;
var currentRotation = ReferenceFrame.RotationFromWorldSpace(internalVessel.ReferenceTransform.rotation);
var tmpTargetRotation = TargetRotation;
var tmpTargetDirection = TargetDirection;
var phr = DirectionBias;
var right = currentRotation * Vector3d.right;
var forward = currentRotation * Vector3d.forward;
var up = currentRotation * Vector3d.up;
var q = QuaternionD.AngleAxis(phr.x, right);
q = q * QuaternionD.AngleAxis(phr.y, forward);
q = q * QuaternionD.AngleAxis(phr.z, up);
QuaternionD directionBiasQuaternion = q;
currentRotation = directionBiasQuaternion * currentRotation;
var currentDirection = currentRotation * Vector3d.up;
if (PlaneMode)
{
var tmpdirection = internalVessel.GetSrfVelocity();
if (tmpdirection.magnitude <= 0)
{
tmpdirection = ReferenceFrame.Rotation * currentDirection;
}
tmpdirection = vessel.SurfaceReferenceFrame.Rotation.Inverse() * tmpdirection;
var sq = GeometryExtensions.ToQuaternion(tmpdirection.normalized, TargetRoll);
sq = ReferenceFrame.Rotation.Inverse() * vessel.SurfaceReferenceFrame.Rotation * sq;
right = sq * Vector3d.right;
forward = sq * Vector3d.forward;
var tmpq = QuaternionD.AngleAxis(-(float)TargetPitch, right);
tmpTargetRotation = tmpq * sq;
tmpq = QuaternionD.AngleAxis(-(float)TargetHeading, forward);
tmpTargetRotation = tmpq * tmpTargetRotation;
tmpTargetDirection = tmpTargetRotation * Vector3d.up;
}
QuaternionD rotation = Quaternion.FromToRotation(currentDirection, tmpTargetDirection);
// Compute angles for the rotation in pitch (x), roll (y), yaw (z) axes
float angleFloat;
Vector3 axisFloat;
// FIXME: QuaternionD.ToAngleAxis method not available at runtime
((Quaternion)rotation).ToAngleAxis(out angleFloat, out axisFloat);
double angle = GeometryExtensions.ClampAngle180(angleFloat);
Vector3d axis = axisFloat;
var angles = new Vector3d(0, 0, 0);
angles = angles + axis * angle;
if (!double.IsNaN(TargetRoll))
{
// Roll angle set => use rotation from currentRotation -> targetRotation
rotation = rotation.Inverse() * tmpTargetRotation;
rotation = rotation * currentRotation.Inverse();
((Quaternion)rotation).ToAngleAxis(out angleFloat, out axisFloat);
if (!float.IsInfinity(axisFloat.magnitude))
{
angle = GeometryExtensions.ClampAngle180(angleFloat);
axis = axisFloat;
angles = angles + axis * angle;
}
}
// else Roll angle not set => use rotation from currentDirection -> targetDirection
// FIXME: QuaternionD.FromToRotation method not available at runtime
angles = directionBiasQuaternion * angles;
angles = vessel.ReferenceFrame.DirectionFromWorldSpace(ReferenceFrame.DirectionToWorldSpace(angles));
angles = transitionMat.MultiplyVector(angles);
return(AnglesToAngularVelocity(angles, torque, moi));
}
