/// <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));
}
/// <summary>
/// Compute current angular velocity in pitch,roll,yaw axes
/// </summary>
Vector3 ComputeCurrentAngularVelocity()
{
var worldAngularVelocity = vessel.InternalVessel.GetComponent <Rigidbody> ().angularVelocity;
var localAngularVelocity = ReferenceFrame.AngularVelocityFromWorldSpace(worldAngularVelocity);
//TODO: why does this need to be negative?
return(-vessel.ReferenceFrame.DirectionFromWorldSpace(ReferenceFrame.DirectionToWorldSpace(localAngularVelocity)));
}
/// <summary>
/// Compute current angular velocity in pitch,roll,yaw axes
/// </summary>
Vector3 ComputeCurrentAngularVelocity()
{
var worldAngularVelocity = vessel.InternalVessel.GetComponent <Rigidbody> ().angularVelocity;
var localAngularVelocity = ReferenceFrame.AngularVelocityFromWorldSpace(worldAngularVelocity);
// TODO: why does this need to be negative?
var ret = -vessel.ReferenceFrame.DirectionFromWorldSpace(ReferenceFrame.DirectionToWorldSpace(localAngularVelocity));
if (PlaneMode)
{
var tav = TurnAngularVelocity();
if (vessel.InternalVessel.GetSrfVelocity().magnitude < 1200)
{
var up = new Vector3d(1, 0, 0);
up = vessel.ReferenceFrame.Rotation.Inverse() * vessel.SurfaceReferenceFrame.Rotation * up;
tav = Vector3d.Dot(up, tav) * up;
}
ret = ret - tav;
}
ret = transitionMat.MultiplyVector(ret);
return(ret);
}
public static Tuple3 TransformDirection (Tuple3 direction, ReferenceFrame from, ReferenceFrame to)
{
CheckReferenceFrames (from, to);
return to.DirectionFromWorldSpace (from.DirectionToWorldSpace (direction.ToVector ())).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));
}