public Direction GetPrograde()
{
var up = (Vessel.findLocalMOI(Vessel.findWorldCenterOfMass()) - Vessel.mainBody.position).normalized;
var d = new Direction {Rotation = Quaternion.LookRotation(Vessel.orbit.GetVel().normalized, up)};
return d;
}
public Direction GetRetrograde()
{
var up = (Target.findLocalMOI(Target.findWorldCenterOfMass()) - Target.mainBody.position).normalized;
var d = new Direction {Rotation = Quaternion.LookRotation(Target.orbit.GetVel().normalized*-1, up)};
return d;
}
public override void Execute(SharedObjects shared)
{
double roll = GetDouble(PopValueAssert(shared));
double yaw = GetDouble(PopValueAssert(shared));
double pitch = GetDouble(PopValueAssert(shared));
AssertArgBottomAndConsume(shared);
var result = new Direction(new Vector3d(pitch, yaw, roll), true);
ReturnValue = result;
}
public override void Execute(SharedObjects shared)
{
double angle = GetDouble(PopValueAssert(shared));
double roll = GetDouble(PopValueAssert(shared));
double yaw = GetDouble(PopValueAssert(shared));
double pitch = GetDouble(PopValueAssert(shared));
AssertArgBottomAndConsume(shared);
var result = new Direction(new UnityEngine.Quaternion((float)pitch, (float)yaw, (float)roll, (float)angle));
ReturnValue = result;
}
public override void Execute(SharedObjects shared)
{
double pitchAboveHorizon = GetDouble(shared.Cpu.PopValue());
double degreesFromNorth = GetDouble(shared.Cpu.PopValue());
Vessel currentVessel = shared.Vessel;
var q = UnityEngine.Quaternion.LookRotation(VesselUtils.GetNorthVector(currentVessel), currentVessel.upAxis);
q *= UnityEngine.Quaternion.Euler(new UnityEngine.Vector3((float)-pitchAboveHorizon, (float)degreesFromNorth, 0));
var result = new Direction(q);
shared.Cpu.PushStack(result);
}
public static void SteerShipToward(Direction targetDir, FlightCtrlState c, Vessel vessel)
{
// I take no credit for this, this is a stripped down, rearranged version of MechJeb's attitude control system
var CoM = vessel.findWorldCenterOfMass();
var MoI = vessel.findLocalMOI(CoM);
var mass = vessel.GetTotalMass();
var up = (CoM - vessel.mainBody.position).normalized;
var target = targetDir.Rotation;
var vesselR = vessel.transform.rotation;
// some validations
if (!Utils.IsValidNumber(c.mainThrottle) ||
!Utils.IsValidVector(CoM) ||
!Utils.IsValidNumber(mass) ||
!Utils.IsValidVector(up) ||
!Utils.IsValidRotation(target) ||
!Utils.IsValidRotation(vesselR))
{
return;
}
Quaternion delta;
delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselR) * target);
Vector3d deltaEuler = ReduceAngles(delta.eulerAngles);
deltaEuler.y *= -1;
Vector3d torque = GetTorque(vessel, c.mainThrottle);
Vector3d inertia = GetEffectiveInertia(vessel, torque);
Vector3d err = deltaEuler * Math.PI / 180.0F;
err += new Vector3d(inertia.x, inertia.z, inertia.y);
//err.Scale(SwapYZ(Vector3d.Scale(MoI, Inverse(torque))));
prev_err = err;
Vector3d act = 120.0f * err;
float precision = Mathf.Clamp((float)torque.x * 20f / MoI.magnitude, 0.5f, 10f);
float drive_limit = Mathf.Clamp01((float)(err.magnitude * 380.0f / precision));
act.x = Mathf.Clamp((float)act.x, -drive_limit, drive_limit);
act.y = Mathf.Clamp((float)act.y, -drive_limit, drive_limit);
act.z = Mathf.Clamp((float)act.z, -drive_limit, drive_limit);
//act = averageVector3d(averagedAct, act, 2);
c.roll = Mathf.Clamp((float)(c.roll + act.z), -drive_limit, drive_limit);
c.pitch = Mathf.Clamp((float)(c.pitch + act.x), -drive_limit, drive_limit);
c.yaw = Mathf.Clamp((float)(c.yaw + act.y), -drive_limit, drive_limit);
/*
// This revised version from 0.6 gave people problems with gravity turns. I've reverted but may try to make it work
var CoM = vessel.findWorldCenterOfMass();
var MoI = vessel.findLocalMOI(CoM);
var mass = vessel.GetTotalMass();
var up = (CoM - vessel.mainBody.position).normalized;
var target = targetDir.Rotation;
var vesselR = vessel.transform.rotation;
Quaternion delta;
delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselR) * target);
Vector3d deltaEuler = ReduceAngles(delta.eulerAngles);
deltaEuler.y *= -1;
Vector3d torque = GetTorque(vessel, c.mainThrottle);
Vector3d inertia = GetEffectiveInertia(vessel, torque);
Vector3d err = deltaEuler * Math.PI / 180.0F;
err += SwapYZ(inertia * 8);
err.Scale(SwapYZ(Vector3d.Scale(MoI * 3, Inverse(torque))));
prev_err = err;
Vector3d act = 400.0f * err;
float precision = Mathf.Clamp((float)torque.x * 20f / MoI.magnitude, 0.5f, 10f);
float drive_limit = Mathf.Clamp01((float)(err.magnitude * 450.0f / precision));
act.x = Mathf.Clamp((float)act.x, -drive_limit, drive_limit);
act.y = Mathf.Clamp((float)act.y, -drive_limit, drive_limit);
act.z = Mathf.Clamp((float)act.z, -drive_limit, drive_limit);
//act = averageVector3d(averagedAct, act, 2);
c.roll = Mathf.Clamp((float)(c.roll + act.z), -drive_limit, drive_limit);
c.pitch = Mathf.Clamp((float)(c.pitch + act.x), -drive_limit, drive_limit);
c.yaw = Mathf.Clamp((float)(c.yaw + act.y), -drive_limit, drive_limit);*/
}
public Direction GetSurfaceRetrograde()
{
Vector3d up = GetUpVector();
OrbitableVelocity vels = GetVelocities();
Vector3d normSrfVec = vels.Surface.Normalized();
var d = new Direction {Rotation = Quaternion.LookRotation(normSrfVec*(-1), up)};
return d;
}
public Direction GetPrograde()
{
Vector3d up = GetUpVector();
OrbitableVelocity vels = GetVelocities();
Vector3d normOrbVec = vels.Orbital.Normalized();
var d = new Direction {Rotation = Quaternion.LookRotation(normOrbVec, up)};
return d;
}
public override void Execute(SharedObjects shared)
{
double pitchAboveHorizon = GetDouble(PopValueAssert(shared));
double degreesFromNorth = GetDouble(PopValueAssert(shared));
AssertArgBottomAndConsume(shared);
Vessel currentVessel = shared.Vessel;
var q = UnityEngine.Quaternion.LookRotation(VesselUtils.GetNorthVector(currentVessel), currentVessel.upAxis);
q *= UnityEngine.Quaternion.Euler(new UnityEngine.Vector3((float)-pitchAboveHorizon, (float)degreesFromNorth, 0));
var result = new Direction(q);
ReturnValue = result;
}
public Direction GetSurfacePrograde()
{
var up = (Vessel.findLocalMOI(Vessel.findWorldCenterOfMass()) - Vessel.mainBody.position).normalized;
var d = new Direction { Rotation = Quaternion.LookRotation(Vessel.srf_velocity.normalized, up) };
return d;
}
public static void SteerShipToward(Direction targetDir, FlightCtrlState c, Vessel vessel)
{
// I take no credit for this, this is a stripped down, rearranged version of MechJeb's attitude control system
var centerOfMass = vessel.findWorldCenterOfMass();
var moi = vessel.findLocalMOI(centerOfMass);
var target = targetDir.Rotation;
var vesselR = vessel.transform.rotation;
Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselR) * target);
Vector3d deltaEuler = ReduceAngles(delta.eulerAngles);
deltaEuler.y *= -1;
Vector3d torque = GetTorque(vessel, c.mainThrottle);
Vector3d inertia = GetEffectiveInertia(vessel, torque);
Vector3d err = deltaEuler * Math.PI / 180.0F;
err += new Vector3d(inertia.x, inertia.z, inertia.y);
Vector3d act = 120.0f * err;
float precision = Mathf.Clamp((float)torque.x * 20f / moi.magnitude, 0.5f, 10f);
float driveLimit = Mathf.Clamp01((float)(err.magnitude * 380.0f / precision));
act.x = Mathf.Clamp((float)act.x, -driveLimit, driveLimit);
act.y = Mathf.Clamp((float)act.y, -driveLimit, driveLimit);
act.z = Mathf.Clamp((float)act.z, -driveLimit, driveLimit);
c.roll = Mathf.Clamp((float)(c.roll + act.z), -driveLimit, driveLimit);
c.pitch = Mathf.Clamp((float)(c.pitch + act.x), -driveLimit, driveLimit);
c.yaw = Mathf.Clamp((float)(c.yaw + act.y), -driveLimit, driveLimit);
}
public override void Execute(SharedObjects shared)
{
double roll = GetDouble(shared.Cpu.PopValue());
double yaw = GetDouble(shared.Cpu.PopValue());
double pitch = GetDouble(shared.Cpu.PopValue());
var result = new Direction(new Vector3d(pitch, yaw, roll), true);
shared.Cpu.PushStack(result);
}
public override void Execute(SharedObjects shared)
{
double angle = GetDouble(shared.Cpu.PopValue());
double roll = GetDouble(shared.Cpu.PopValue());
double yaw = GetDouble(shared.Cpu.PopValue());
double pitch = GetDouble(shared.Cpu.PopValue());
var result = new Direction(new UnityEngine.Quaternion((float)pitch, (float)yaw, (float)roll, (float)angle));
shared.Cpu.PushStack(result);
}
/// <summary>
/// Returns this rotation relative to a starting rotation - ie.. how you would
/// have to rotate from that start rotation to get to this one.
/// </summary>
/// <param name="fromDir">start rotation.</param>
/// <returns>new Direction representing such a rotation.</returns>
public Direction RelativeFrom(Direction fromDir)
{
return new Direction(Quaternion.RotateTowards(fromDir.rotation, rotation, 99999.0f));
}
