public static void HoldAttitude(FlightCtrlState fcs, Processor p, FlightAttitude fa)
{
Vessel v = p.parentVessel;
var forward = Vector3.zero;
var up = Vector3.zero;
bool ignoreRoll = false;
switch (fa.frame)
{
case ReferenceFrame.Orbit:
ignoreRoll = true;
forward = v.GetObtVelocity();
up = (v.mainBody.position - v.CoM);
break;
case ReferenceFrame.Surface:
ignoreRoll = true;
forward = v.GetSrfVelocity();
up = (v.mainBody.position - v.CoM);
break;
case ReferenceFrame.Maneuver:
ignoreRoll = true;
if (v.patchedConicSolver.maneuverNodes.Count != 0)
{
forward = v.patchedConicSolver.maneuverNodes[0].GetBurnVector(v.orbit);
up = (v.mainBody.position - v.CoM);
}
else
{
forward = v.GetObtVelocity();
up = (v.mainBody.position - v.CoM);
}
break;
case ReferenceFrame.North:
up = (v.mainBody.position - v.CoM);
forward = Vector3.ProjectOnPlane(v.mainBody.position + v.mainBody.transform.up * (float)v.mainBody.Radius - v.CoM, up);
break;
}
Vector3.OrthoNormalize(ref forward, ref up);
Quaternion rotationReference = Quaternion.LookRotation(forward, up);
switch (fa.attitude)
{
case RelativeAttitude.Prograde:
break;
case RelativeAttitude.Retrograde:
rotationReference = rotationReference * Quaternion.AngleAxis(180, Vector3.up);
break;
case RelativeAttitude.Vertical:
ignoreRoll = true;
up = v.mainBody.transform.up;
forward = (v.CoM - v.mainBody.position);
Vector3.OrthoNormalize(ref forward, ref up);
rotationReference = Quaternion.LookRotation(forward, up);
if (fa.frame == ReferenceFrame.Surface)
{
forward = rotationReference * v.terrainNormal;
up = v.mainBody.transform.up;
Vector3.OrthoNormalize(ref forward, ref up);
rotationReference = Quaternion.LookRotation(forward, up);
}
break;
case RelativeAttitude.CustomHP:
Quaternion hp = Quaternion.Euler(new Vector3d(Double.IsNaN(fa.Pitch) ? 0 : fa.Pitch,
Double.IsNaN(fa.Hdg) ? 0 : -fa.Hdg,
0));
rotationReference = rotationReference * hp;
ignoreRoll = true;
break;
case RelativeAttitude.CustomHPR:
Quaternion hpr = Quaternion.Euler(new Vector3d(Double.IsNaN(fa.Pitch) ? 0 : fa.Pitch,
Double.IsNaN(fa.Hdg) ? 0 : -fa.Hdg,
Double.IsNaN(fa.Roll) ? 0 : 180 - fa.Roll));
rotationReference = rotationReference * hpr;
break;
}
HoldOrientation(fcs, p, rotationReference, ignoreRoll);
}
public static void HoldAttitude(FlightCtrlState fcs, Processor p, FlightAttitude fa)
{
Vessel v = p.parentVessel;
var forward = Vector3.zero;
var up = Vector3.zero;
bool ignoreRoll = false;
switch (fa.frame)
{
case ReferenceFrame.Orbit:
ignoreRoll = true;
forward = v.GetObtVelocity();
up = (v.mainBody.position - v.CoM);
break;
case ReferenceFrame.Surface:
ignoreRoll = true;
forward = v.GetSrfVelocity();
up = (v.mainBody.position - v.CoM);
break;
case ReferenceFrame.Maneuver:
ignoreRoll = true;
if (v.patchedConicSolver.maneuverNodes.Count != 0)
{
forward = v.patchedConicSolver.maneuverNodes[0].GetBurnVector(v.orbit);
up = (v.mainBody.position - v.CoM);
}
else
{
forward = v.GetObtVelocity();
up = (v.mainBody.position - v.CoM);
}
break;
case ReferenceFrame.North:
up = (v.mainBody.position - v.CoM);
forward = Vector3.ProjectOnPlane(v.mainBody.position + v.mainBody.transform.up * (float)v.mainBody.Radius - v.CoM, up);
break;
}
Vector3.OrthoNormalize(ref forward, ref up);
Quaternion rotationReference = Quaternion.LookRotation(forward, up);
switch (fa.attitude)
{
case RelativeAttitude.Prograde:
break;
case RelativeAttitude.Retrograde:
rotationReference = rotationReference * Quaternion.AngleAxis(180, Vector3.up);
break;
case RelativeAttitude.Vertical:
ignoreRoll = true;
up = v.mainBody.transform.up;
forward = (v.CoM - v.mainBody.position);
Vector3.OrthoNormalize(ref forward, ref up);
rotationReference = Quaternion.LookRotation(forward, up);
if (fa.frame == ReferenceFrame.Surface)
{
forward = rotationReference * v.terrainNormal;
up = v.mainBody.transform.up;
Vector3.OrthoNormalize(ref forward, ref up);
rotationReference = Quaternion.LookRotation(forward, up);
}
break;
case RelativeAttitude.CustomHP:
Quaternion hp = Quaternion.Euler(new Vector3d(Double.IsNaN(fa.Pitch) ? 0 : fa.Pitch,
Double.IsNaN(fa.Hdg) ? 0 : -fa.Hdg,
0));
rotationReference = rotationReference * hp;
ignoreRoll = true;
break;
case RelativeAttitude.CustomHPR:
Quaternion hpr = Quaternion.Euler(new Vector3d(Double.IsNaN(fa.Pitch) ? 0 : fa.Pitch,
Double.IsNaN(fa.Hdg) ? 0 : -fa.Hdg,
Double.IsNaN(fa.Roll) ? 0 : 180 - fa.Roll));
rotationReference = rotationReference * hpr;
break;
}
HoldOrientation(fcs, p, rotationReference, ignoreRoll);
}
