public void RunCalculations(
Vessel vessel,
Quaternion gymbal)
{
ManeuverPresent = false;
// Calculations thanks to Mechjeb
Vector3d CoM = vessel.findWorldCenterOfMass();
Vector3d velocityVesselOrbit = vessel.orbit.GetVel();
Vector3d velocityVesselOrbitUnit = velocityVesselOrbit.normalized;
Vector3d velocityVesselSurface = velocityVesselOrbit - vessel.mainBody.getRFrmVel(CoM);
Vector3d velocityVesselSurfaceUnit = velocityVesselSurface.normalized;
ProgradeOrbit = gymbal * velocityVesselOrbitUnit;
ProgradeSurface = gymbal * velocityVesselSurfaceUnit;
PatchedConicSolver patchedConicSolver = vessel.patchedConicSolver;
if (patchedConicSolver == null)
return;
if (patchedConicSolver.maneuverNodes == null)
return;
if (patchedConicSolver.maneuverNodes.Count > 0)
{
var manuever = patchedConicSolver.maneuverNodes[0];
ManeuverApplied = manuever.DeltaV != Vector3d.zero;
Vector3d burnVector = manuever.GetBurnVector(vessel.orbit);
ManeuverPlus = gymbal * burnVector.normalized;
ManeuverPresent = true;
}
}
/// <summary>
/// Copy Constructor.
/// </summary>
/// <param name="orig"></param>
public SpawnVessel(SpawnVessel orig)
{
deferVesselCreation = orig.deferVesselCreation;
foreach (VesselData vessel in orig.vessels)
{
if (vessel.pqsCity != null)
{
// Generate PQS city coordinates
LoggingUtil.LogVerbose(this, "Generating coordinates from PQS city for Vessel " + vessel.name);
// Translate by the PQS offset (inverse transform of coordinate system)
Vector3d position = vessel.pqsCity.transform.position;
LoggingUtil.LogVerbose(this, " pqs city position = " + position);
Vector3d v = vessel.pqsOffset;
Vector3d i = vessel.pqsCity.transform.right;
Vector3d j = vessel.pqsCity.transform.forward;
Vector3d k = vessel.pqsCity.transform.up;
LoggingUtil.LogVerbose(this, " i, j, k = " + i + ", " + j + "," + k);
Vector3d offsetPos = new Vector3d(
(j.y * k.z - j.z * k.y) * v.x + (i.z * k.y - i.y * k.z) * v.y + (i.y * j.z - i.z * j.y) * v.z,
(j.z * k.x - j.x * k.z) * v.x + (i.x * k.z - i.z * k.x) * v.y + (i.z * j.x - i.x * j.z) * v.z,
(j.x * k.y - j.y * k.x) * v.x + (i.y * k.x - i.x * k.y) * v.y + (i.x * j.y - i.y * j.x) * v.z
);
offsetPos *= (i.x * j.y * k.z) + (i.y * j.z * k.x) + (i.z * j.x * k.y) - (i.z * j.y * k.x) - (i.y * j.x * k.z) - (i.x * j.z * k.y);
vessel.latitude = vessel.body.GetLatitude(position + offsetPos);
vessel.longitude = vessel.body.GetLongitude(position + offsetPos);
LoggingUtil.LogVerbose(this, " resulting lat, lon = (" + vessel.latitude + ", " + vessel.longitude + ")");
}
vessels.Add(new VesselData(vessel));
}
}
public WaypointData(WaypointData orig, Contract contract)
{
type = orig.type;
waypoint.altitude = orig.waypoint.altitude;
waypoint.celestialName = orig.waypoint.celestialName;
waypoint.height = orig.waypoint.height;
waypoint.id = orig.waypoint.id;
waypoint.index = orig.waypoint.index;
waypoint.isClustered = orig.waypoint.isClustered;
waypoint.isExplored = orig.waypoint.isExplored;
waypoint.isOnSurface = orig.waypoint.isOnSurface;
waypoint.landLocked = orig.waypoint.landLocked;
waypoint.latitude = orig.waypoint.latitude;
waypoint.longitude = orig.waypoint.longitude;
waypoint.name = orig.waypoint.name;
waypoint.visible = orig.waypoint.visible;
randomAltitude = orig.randomAltitude;
waterAllowed = orig.waterAllowed;
forceEquatorial = orig.forceEquatorial;
nearIndex = orig.nearIndex;
count = orig.count;
minDistance = orig.minDistance;
maxDistance = orig.maxDistance;
pqsCity = orig.pqsCity;
pqsOffset = orig.pqsOffset;
parameter = orig.parameter.ToList();
randomName = orig.randomName;
underwater = orig.underwater;
SetContract(contract);
}
/// <summary>
/// Converts a Cartesian 3D vector to latitude and longitude.
/// </summary>
/// <param name="vec">Vector3d of indicated position (convention: prime meridian is +x, north pole is +z)</param>
/// <returns>Pair of lat/lon coordinates (in degrees) of indicated position. </returns>
public static double[] Cart2LatLon(Vector3d vec)
{
vec = Vector3d.Normalize (vec);
double lat = Rad2Deg (Math.Asin (vec.z));
double lon = Rad2Deg (Math.Atan2 (vec.x, vec.y));
return new double[] { lat, lon };
}
public static ManeuverNode PlaceManeuverNode(this Vessel vessel, Orbit patch, Vector3d dV, double UT)
{
//placing a maneuver node with bad dV values can really mess up the game, so try to protect against that
//and log an exception if we get a bad dV vector:
for (int i = 0; i < 3; i++)
{
if (double.IsNaN(dV[i]) || double.IsInfinity(dV[i]))
{
throw new Exception("VesselExtensions.PlaceManeuverNode: bad dV: " + dV);
}
}
if (double.IsNaN(UT) || double.IsInfinity(UT))
{
throw new Exception("VesselExtensions.PlaceManeuverNode: bad UT: " + UT);
}
//It seems that sometimes the game can freak out if you place a maneuver node in the past, so this
//protects against that.
UT = Math.Max(UT, Planetarium.GetUniversalTime());
//convert a dV in world coordinates into the coordinate system of the maneuver node,
//which uses (x, y, z) = (radial+, normal-, prograde)
Vector3d nodeDV = patch.DeltaVToManeuverNodeCoordinates(UT, dV);
ManeuverNode mn = vessel.patchedConicSolver.AddManeuverNode(UT);
mn.OnGizmoUpdated(nodeDV, UT);
return mn;
}
public Direction(Quaternion q)
: this()
{
rotation = q;
euler = q.eulerAngles;
vector = rotation * Vector3d.forward;
}
public void AddWeightedCorrection(Vector3d cor)
{
var cm = cor.magnitude;
if(cm > 1e-10) cor *= Math.Sqrt(1/cm);
if(VSL.Physics.G > 1e-10) cor *= Utils.ClampH(Utils.G0/VSL.Physics.G, HSC.MaxCorrectionWeight);
CourseCorrections.Add(cor);
}
public void SetChunk(double size, Vector3d lowerCorner, int iOffset, int jOffset, int kOffset, HashSet<Part> overridingParts)
{
_size = size;
offset = iOffset + 8 * jOffset + 64 * kOffset;
this.lowerCorner = lowerCorner;
this.overridingParts = overridingParts;
}
public void UpdateForceAndTorque(Vector3d com)
{
PartModule gimbal;
VesselState.GimbalExt gimbalExt = VesselState.getGimbalExt(engine.part, out gimbal);
Vessel v = engine.vessel;
float currentMaxThrust, currentMinThrust;
currentMaxThrust = engine.minFuelFlow * engine.flowMultiplier * engine.realIsp * engine.g / engine.thrustTransforms.Count;
currentMinThrust = engine.maxFuelFlow * engine.flowMultiplier * engine.realIsp * engine.g / engine.thrustTransforms.Count;
if (engine.throttleLocked)
{
currentMaxThrust *= thrustRatio;
currentMinThrust = currentMaxThrust;
}
for (int i = 0; i < engine.thrustTransforms.Count; i++)
{
Vector3d thrust_dir = v.transform.rotation.Inverse() * gimbalExt.initialRot(gimbal, engine.thrustTransforms[i], i) * Vector3d.back;
Vector3d pos = v.transform.rotation.Inverse() * (engine.part.transform.position - com);
_maxVariableForce += (currentMaxThrust - currentMinThrust) * thrust_dir;
_constantForce += currentMinThrust * thrust_dir;
_maxVariableTorque += (currentMaxThrust - currentMinThrust) * Vector3d.Cross(pos, thrust_dir);
_constantTorque += currentMinThrust * Vector3d.Cross(pos, thrust_dir);
}
}
//The KSP drag law is dv/dt = -b * v^2 where b is proportional to the air density and
//the ship's drag coefficient. In this equation b has units of inverse length. So 1/b
//is a characteristic length: a ship that travels this distance through air will lose a significant
//fraction of its initial velocity
public static double DragLength(this CelestialBody body, Vector3d pos, double dragCoeffOverMass)
{
double airDensity = FlightGlobals.getAtmDensity(FlightGlobals.getStaticPressure(pos, body));
if (airDensity <= 0)
return Double.MaxValue;
return 1.0 / (0.5 * FlightGlobals.DragMultiplier * airDensity * dragCoeffOverMass);
}
public static Vector3d CartesianToSpherical(Vector3d cartesian)
{
double r = cartesian.magnitude;
double theta = Math.Atan(cartesian.y / cartesian.x) * RadToDeg;
double phi = Math.Acos(cartesian.z / r) * RadToDeg;
return new Vector3d(r, theta, phi);
}
public void Load(ConfigNode node)
{
if (!HighLogic.LoadedSceneIsFlight || FlightGlobals.ActiveVessel == null || parent.vessel == null) {
return;
}
string[] values = node.GetValues("node");
int max = values.Length;
for (int k = 0; k < max; k++) {
string[] info = values[k].Split(delimiter, StringSplitOptions.RemoveEmptyEntries);
Vector3d deltav = new Vector3d();
double d = 0.0;
if (info.Length == 4) {
double.TryParse(info[0], out d);
deltav.x = d;
d = 0.0;
double.TryParse(info[1], out d);
deltav.y = d;
d = 0.0;
double.TryParse(info[2], out d);
deltav.z = d;
d = 0.0;
double.TryParse(info[3], out d);
// at the very least it'll /act/ like a proper maneuver node.
nodes.Add(new NodeState(deltav, d));
}
}
Debug.Log("Node Saver loaded " + max + " nodes.");
}
private void InitializeSuffixes()
{
AddSuffix("X", new SetSuffix<double>(() => X, value => X = value));
AddSuffix("Y", new SetSuffix<double>(() => Y, value => Y = value));
AddSuffix("Z", new SetSuffix<double>(() => Z, value => Z = value));
AddSuffix("MAG", new SetSuffix<double>(Magnitude, value =>
{
double oldMag = new Vector3d(X, Y, Z).magnitude;
if (oldMag == 0) return; // Avoid division by zero
X = X / oldMag * value;
Y = Y / oldMag * value;
Z = Z / oldMag * value;
}));
AddSuffix("VEC", new Suffix<Vector>(() => new Vector(X, Y, Z)));
AddSuffix("NORMALIZED", new Suffix<Vector>(Normalized));
AddSuffix("SQRMAGNITUDE", new Suffix<double>(() => new Vector3d(X, Y, Z).sqrMagnitude));
AddSuffix("DIRECTION", new SetSuffix<Direction>(ToDirection, value =>
{
var newMagnitude = Vector3d.forward * new Vector3d(X, Y, Z).magnitude;
var newVector = value.Rotation * newMagnitude;
X = newVector.x;
Y = newVector.y;
Z = newVector.z;
}));
}
public VesselData(VesselData vd)
{
name = vd.name;
id = vd.id;
craftURL = vd.craftURL;
craftPart = vd.craftPart;
flagURL = vd.flagURL;
vesselType = vd.vesselType;
body = vd.body;
orbit = vd.orbit;
latitude = vd.latitude;
longitude = vd.longitude;
altitude = vd.altitude;
height = vd.height;
orbiting = vd.orbiting;
owned = vd.owned;
pqsCity = vd.pqsCity;
pqsOffset = vd.pqsOffset;
heading = vd.heading;
pitch = vd.pitch;
roll = vd.roll;
foreach (CrewData cd in vd.crew)
{
crew.Add(new CrewData(cd));
}
}
void GgsUpdate(){
if (core.attitude.attitudeAngleFromTarget() < 1)
{
Vector3d ang;
Quaternion rot;
Quaternion ori;
Vector3d gee;
ang = new Vector3d(inpX, inpY, inpZ);
gee = FlightGlobals.getGeeForceAtPosition(this.vessel.transform.position);
rot = Quaternion.LookRotation(gee);
ori = Quaternion.LookRotation(gee);
Quaternion x = Quaternion.AngleAxis(inpX, Vector3.Cross(gee,Vector3.up).normalized);
Quaternion y = Quaternion.AngleAxis(360 - inpY, gee);
if(modo == Modo.Horizonte) {
Quaternion z = Quaternion.AngleAxis(360 - inpZ, this.vessel.transform.InverseTransformDirection(this.vessel.transform.up));
part.vessel.SetRotation(y * x * Quaternion.LookRotation(gee) * z);
}
if (modo == Modo.Prograde) {
Quaternion z = Quaternion.AngleAxis(-(inpZ-90)+180, this.vessel.transform.InverseTransformDirection(this.vessel.transform.up));
Vector3 pro = part.vessel.orbit.GetVel();
//Vector3d gee = FlightGlobals.getGeeForceAtPosition(this.vessel.transform.position);
Vector3 realPro = Vector3.Cross(gee*-1,pro);
x = Quaternion.AngleAxis(inpY, Vector3.Cross(realPro,Vector3.up).normalized);
y = Quaternion.AngleAxis(360 - inpX, realPro);
part.vessel.SetRotation(y * x * Quaternion.LookRotation(realPro,pro) * z);
}
}
}
protected override void _Drive(FlightCtrlState s)
{
Vessel vessel = base.Autopilot.vessel;
AttitudeController attitudeController = base.Autopilot.AutopilotController.AttitudeController;
if (vessel.altitude < 10000) {
attitudeController.Attitude = Attitude.Up;
base.Autopilot.AutopilotController.AutoStagingController.Enable ();
s.mainThrottle = 1.0f;
} else if (vessel.altitude < 80000) {
attitudeController.Pitch = 90;
attitudeController.Yaw = 45;
attitudeController.Attitude = Attitude.UserDefined;
s.mainThrottle = Mathf.Clamp01 (100000.0f - (float)vessel.orbit.ApA);
} else {
double ut = Planetarium.GetUniversalTime () + vessel.orbit.timeToAp;
double deltaV = Math.Sqrt (vessel.mainBody.gravParameter / (100000 + vessel.mainBody.Radius)) - vessel.orbit.getOrbitalVelocityAtUT (ut).magnitude;
Vector3d nodeDV = new Vector3d (0, 0, deltaV);
base.Autopilot.ManeuverNode = vessel.patchedConicSolver.AddManeuverNode (ut);
base.Autopilot.ManeuverNode.OnGizmoUpdated (nodeDV, ut);
base.Autopilot.AutopilotController.Disable ();
base.Autopilot.AutopilotController.AutoStagingController.Disable ();
attitudeController.Attitude = Attitude.None;
}
}
public static Entity CreatePlanetTemplate(uint planetIndex)
{
PlanetData data = PlanetInfo.GetData(planetIndex);
float initialAngle = Random.Range(0f, 360f); // TODO: calculate this from position
double orbitRadius = data.distanceFromSun() * Scales.au2km;
float orbitPeriod = data.orbitalPeriod * 365.25f;
float rotationPeriod = data.rotationPeriod;
double diameter = data.diameter * Scales.earthDiameter;
var position = new Vector3d(0, 0, 0);
var rotation = Quaternion.identity;
var scale = new Vector3d(diameter, diameter, diameter);
var spatialPosition = position / Scales.spatialFactor;
var prefabName = (planetIndex == 5 || planetIndex == 6) ? SimulationSettings.RingedPlanetPrefabName : SimulationSettings.PlanetPrefabName;
var planetTemplate = EntityBuilder.Begin()
.AddPositionComponent((Vector3)spatialPosition, CommonRequirementSets.PhysicsOnly)
.AddMetadataComponent(entityType: prefabName)
.SetPersistence(true)
.SetReadAcl(CommonRequirementSets.PhysicsOrVisual)
.AddComponent(new TransformInfo.Data(position.ToImprobable(), rotation.ToImprobable(), scale.ToImprobable()), CommonRequirementSets.PhysicsOnly)
.AddComponent(new OrbitInfo.Data(initialAngle, orbitRadius, orbitPeriod, rotationPeriod), CommonRequirementSets.PhysicsOnly)
.AddComponent(new PlanetIndex.Data(planetIndex), CommonRequirementSets.PhysicsOnly)
.Build();
return(planetTemplate);
}
public static Entity CreateAsteroidTemplate(float initialAngle, double orbitRadius, double diameter)
{
float orbitPeriod = 365.25f; // TODO: pick a good orbit period for the asteroid belt
float rotationPeriod = 1f / 24f / 60f; // TODO: have different asteroids rotate at different speeds
double x = Mathf.Sin(Mathf.Deg2Rad * initialAngle) * orbitRadius;
double y = Random.Range(-200f, 200f);
double z = Mathf.Cos(Mathf.Deg2Rad * initialAngle) * orbitRadius;
var position = new Vector3d(x, y, z);
var rotation = Quaternion.identity;
var scale = new Vector3d(diameter, diameter, diameter);
var spatialPosition = position / Scales.spatialFactor;
var asteroidTemplate = EntityBuilder.Begin()
.AddPositionComponent((Vector3)spatialPosition, CommonRequirementSets.PhysicsOnly)
.AddMetadataComponent(entityType: SimulationSettings.AsteroidPrefabName)
.SetPersistence(true)
.SetReadAcl(CommonRequirementSets.PhysicsOrVisual)
.AddComponent(new TransformInfo.Data(position.ToImprobable(), rotation.ToImprobable(), scale.ToImprobable()), CommonRequirementSets.PhysicsOnly)
.AddComponent(new OrbitInfo.Data(initialAngle, orbitRadius, orbitPeriod, rotationPeriod), CommonRequirementSets.PhysicsOnly)
.Build();
Debug.Log(orbitRadius.ToString() + " " + spatialPosition.ToString());
return(asteroidTemplate);
}
public void BuildSection(Vector3d lowerInputs, Vector3d upperInputs)
{
//Creates cubic from x,y,dy/dx data
double recipXDiff, recipXDiffSq;
recipXDiff = 1 / (lowerInputs.x - upperInputs.x);
recipXDiffSq = recipXDiff * recipXDiff;
a = 2 * (upperInputs.y - lowerInputs.y) * recipXDiff;
a += upperInputs.z + lowerInputs.z;
a *= recipXDiffSq;
b = 3 * (upperInputs.x + lowerInputs.x) * (lowerInputs.y - upperInputs.y) * recipXDiff;
b -= (lowerInputs.x + 2 * upperInputs.x) * lowerInputs.z;
b -= (2 * lowerInputs.x + upperInputs.x) * upperInputs.z;
b *= recipXDiffSq;
c = 6 * upperInputs.x * lowerInputs.x * (upperInputs.y - lowerInputs.y) * recipXDiff;
c += (2 * lowerInputs.x * upperInputs.x + upperInputs.x * upperInputs.x) * lowerInputs.z;
c += (2 * lowerInputs.x * upperInputs.x + lowerInputs.x * lowerInputs.x) * upperInputs.z;
c *= recipXDiffSq;
d = (3 * lowerInputs.x - upperInputs.x) * upperInputs.x * upperInputs.x * lowerInputs.y;
d += (lowerInputs.x - 3 * upperInputs.x) * lowerInputs.x * lowerInputs.x * upperInputs.y;
d *= recipXDiff;
d -= lowerInputs.x * upperInputs.x * upperInputs.x * lowerInputs.z;
d -= lowerInputs.x * lowerInputs.x * upperInputs.x * upperInputs.z;
d *= recipXDiffSq;
upperLim = upperInputs.x;
lowerLim = lowerInputs.x;
}
public Vector3d Start(Vector3d approachPosition)
{
Vector3d startPos = start.Position();
Vector3d endPos = end.Position();
if (Vector3d.Distance(startPos, approachPosition) < Vector3d.Distance(endPos, approachPosition)) return startPos;
else return endPos;
}
public Vector3d velocity; // velocity in world frame relatively to the reference body
public VesselState(Vessel vessel)
{
referenceBody = vessel.orbit.referenceBody;
time = Planetarium.GetUniversalTime();
position = vessel.GetWorldPos3D() - referenceBody.position;
velocity = vessel.obt_velocity;
}
public bool Execute(Vector3d dV, float MinDeltaV = 0.1f, ManeuverCondition condition = null)
{
THR.Throttle = 0;
dVrem.Value = dV.magnitude;
//end if below the minimum dV
if(dVrem < MinDeltaV) return false;
VSL.Engines.ActivateEngines();
if(VSL.Engines.MaxThrustM.Equals(0)) return true;
//orient along the burning vector
if(dVrem && VSL.Controls.RCSAvailableInDirection(-dV))
CFG.AT.OnIfNot(Attitude.KillRotation);
else
{
CFG.AT.OnIfNot(Attitude.Custom);
ATC.SetThrustDirW(-dV);
}
//check the condition
if(condition != null && !condition((float)dVrem)) return true;
if(VSL.Controls.TranslationAvailable)
{
if(dVrem || VSL.Controls.AttitudeError > GLB.ATCB.AttitudeErrorThreshold)
TRA.AddDeltaV(-VSL.LocalDir(dV));
if(dVrem && CFG.AT[Attitude.KillRotation])
{
var errorF = Utils.ClampL(Vector3.Dot(VSL.Engines.Thrust.normalized, -dV.normalized), 0);
THR.DeltaV = (float)dVrem * errorF*errorF;
}
else THR.DeltaV = (float)dVrem;
}
else THR.DeltaV = (float)dVrem;
// Log("\ndVrem: {}\nAttitudeError {}, DeltaV: {}, Throttle {}, RCS {}",
// dVrem, VSL.Controls.AttitudeError, THR.DeltaV, THR.Throttle, VSL.Controls.RCSAvailableInDirection(-dV));//debug
return true;
}
public static string FormattedGeoPos(Vector3d geoPos, bool altitude)
{
string finalString = string.Empty;
//lat
double lat = geoPos.x;
double latSign = Math.Sign(lat);
double latMajor = latSign * Math.Floor(Math.Abs(lat));
double latMinor = 100 * (Math.Abs(lat) - Math.Abs(latMajor));
string latString = latMajor.ToString("0") + " " + latMinor.ToString("0.000");
finalString += "N:" + latString;
//longi
double longi = geoPos.y;
double longiSign = Math.Sign(longi);
double longiMajor = longiSign * Math.Floor(Math.Abs(longi));
double longiMinor = 100 * (Math.Abs(longi) - Math.Abs(longiMajor));
string longiString = longiMajor.ToString("0") + " " + longiMinor.ToString("0.000");
finalString += " E:" + longiString;
if(altitude)
{
finalString += " ASL:" + geoPos.z.ToString("0.000");
}
return finalString;
}
/// <summary>
/// Orbit foo, this finds the nodes of two orbits
/// </summary>
/// <returns>
/// The true anomaly of the ascending node(descing node is 180degrees off)
/// </returns>
/// <param name='orbit'>
/// Orbit.
/// </param>
/// <param name='tgtorbit'>
/// Target Orbit
/// </param>
public static double FindAN(Orbit orbit, Orbit tgtorbit)
{
double rad = Math.PI/180;
double Lan1 = orbit.LAN;
double inc1 = orbit.inclination;
double Lan2 = tgtorbit.LAN;
double inc2 = tgtorbit.inclination;
//see braeunig.us/space... cross product of two orbital planes gives the node location
var a = new Vector3d(Math.Sin(inc1*rad)*Math.Cos(Lan1*rad), Math.Sin(inc1*rad)*Math.Sin(Lan1*rad),
Math.Cos(inc1*rad));
var b = new Vector3d(Math.Sin(inc2*rad)*Math.Cos(Lan2*rad), Math.Sin(inc2*rad)*Math.Sin(Lan2*rad),
Math.Cos(inc2*rad));
var c = new Vector3d(0, 0, 0);
c = Vector3d.Cross(a, b);
var coord = new double[] {0, 0};
coord = LatLonofVector(c); //get the coordinates lat/lon of the ascending node
double lat = coord[0];
double lon = coord[1];
//go look at the diagrams at braeunig.us/space
double α = lon - Lan1; //its all greek to me
if (α < 0) α += 360;
double λ = Math.Atan(Math.Tan(α*rad)/Math.Cos(inc1*rad))/rad;
double x = 180 + (λ - orbit.argumentOfPeriapsis);
if (x > 360) return 360 - x;
else return x;
}
double tauP; //normalized parabolic transfer time
#endregion Fields
#region Constructors
/// <summary>
/// Initializes a new instance of the <see cref="ThrottleControlledAvionics.LambertSolver"/> class.
/// </summary>
/// <param name="orb">Starting orbit.</param>
/// <param name="destination">Destination radius-vector.</param>
/// <param name="UT">Starting UT.</param>
public LambertSolver(Orbit orb, Vector3d destination, double UT)
{
orbit = orb;
body = orbit.referenceBody;
StartUT = UT;
mu = body.gravParameter;
r1 = orb.getRelativePositionAtUT(UT);
var h = Vector3d.Cross(r1, destination);
if(h.sqrMagnitude < 0.01) h = orb.GetOrbitNormal();
c = destination-r1;
cm = c.magnitude;
var r1m = r1.magnitude;
var r2m = destination.magnitude;
var rrm = r1m+r2m;
m = rrm+cm;
n = rrm-cm;
m3 = m*m*m;
var transfer_angle = Vector3d.Angle(r1, destination)*Mathf.Deg2Rad;
if(h.z < 0) transfer_angle = Utils.TwoPI-transfer_angle;
sigma = Math.Sqrt(n/m);
if(transfer_angle > Math.PI) sigma = -sigma;
sigma2 = sigma*sigma;
sigma3 = sigma2*sigma;
sigma5 = sigma2*sigma3;
tauP = 2/3.0*(1-sigma3);
tauME = Math.Acos(sigma)+sigma*Math.Sqrt(1-sigma2);
}
public override ErrorData Simulate(Transform vesselTransform, Vector3d targetDirection, Vector3d targetDirectionYaw, Vector3 targetPosition, Vector3 upDirection, float upWeighting, Vessel vessel)
{
float pitchError;
float rollError;
float yawError;
float sideslip;
sideslip = (float)Math.Asin(Vector3.Dot(vesselTransform.right, vessel.srf_velocity.normalized)) * Mathf.Rad2Deg;
pitchError = (float)Math.Asin(Vector3d.Dot(Vector3d.back, VectorUtils.Vector3dProjectOnPlane(targetDirection, Vector3d.right))) * Mathf.Rad2Deg;
yawError = (float)Math.Asin(Vector3d.Dot(Vector3d.right, VectorUtils.Vector3dProjectOnPlane(targetDirectionYaw, Vector3d.forward))) * Mathf.Rad2Deg;
//roll
Vector3 currentRoll = -vesselTransform.forward;
Vector3 rollTarget;
rollTarget = (targetPosition + Mathf.Clamp(upWeighting * (100f - Math.Abs(yawError * 1.6f) - (pitchError * 2.8f)), 0, float.PositiveInfinity) * upDirection) - vessel.CoM;
rollTarget = Vector3.ProjectOnPlane(rollTarget, vesselTransform.up);
rollError = VectorUtils.SignedAngle(currentRoll, rollTarget, vesselTransform.right) - sideslip * (float)Math.Sqrt(vessel.srf_velocity.magnitude) / 5;
float pitchDownFactor = pitchError * (10 / ((float)Math.Pow(yawError, 2) + 10f) - 0.1f);
rollError -= Mathf.Clamp(pitchDownFactor, -15, 0);
ErrorData behavior = new ErrorData(pitchError, rollError, yawError);
return behavior;
}
/// <summary>
/// This function should return exactly the same value as Vessel.atmDensity, but is more generic because you don't need an actual vessel updated by KSP to get a value at the desired location.
/// Computations are performed for the current body position, which means it's theoritically wrong if you want to know the temperature in the future, but since body rotation is not used (position is given in sun frame), you should get accurate results up to a few weeks.
/// </summary>
/// <param name="position"></param>
/// <param name="body"></param>
/// <returns></returns>
public static double GetTemperature(Vector3d position, CelestialBody body)
{
if (!body.atmosphere)
return PhysicsGlobals.SpaceTemperature;
double altitude = (position - body.position).magnitude - body.Radius;
if (altitude > body.atmosphereDepth)
return PhysicsGlobals.SpaceTemperature;
Vector3 up = (position - body.position).normalized;
float polarAngle = Mathf.Acos(Vector3.Dot(body.bodyTransform.up, up));
if (polarAngle > Mathf.PI / 2.0f)
{
polarAngle = Mathf.PI - polarAngle;
}
float time = (Mathf.PI / 2.0f - polarAngle) * 57.29578f;
Vector3 sunVector = (FlightGlobals.Bodies[0].position - position).normalized;
float sunAxialDot = Vector3.Dot(sunVector, body.bodyTransform.up);
float bodyPolarAngle = Mathf.Acos(Vector3.Dot(body.bodyTransform.up, up));
float sunPolarAngle = Mathf.Acos(sunAxialDot);
float sunBodyMaxDot = (1.0f + Mathf.Cos(sunPolarAngle - bodyPolarAngle)) * 0.5f;
float sunBodyMinDot = (1.0f + Mathf.Cos(sunPolarAngle + bodyPolarAngle)) * 0.5f;
float sunDotCorrected = (1.0f + Vector3.Dot(sunVector, Quaternion.AngleAxis(45f * Mathf.Sign((float)body.rotationPeriod), body.bodyTransform.up) * up)) * 0.5f;
float sunDotNormalized = (sunDotCorrected - sunBodyMinDot) / (sunBodyMaxDot - sunBodyMinDot);
double atmosphereTemperatureOffset = (double)body.latitudeTemperatureBiasCurve.Evaluate(time) + (double)body.latitudeTemperatureSunMultCurve.Evaluate(time) * sunDotNormalized + (double)body.axialTemperatureSunMultCurve.Evaluate(sunAxialDot);
double temperature = body.GetTemperature(altitude) + (double)body.atmosphereTemperatureSunMultCurve.Evaluate((float)altitude) * atmosphereTemperatureOffset;
return temperature;
}
public double GetAngleOfAttack(Vector3d position, Vector3d velocity)
{
if (!horizon)
return angle;
return Math.Acos(Vector3d.Dot(position, velocity) / (position.magnitude * velocity.magnitude)) - Math.PI * 0.5 + angle;
}
public void HoldOrientation(FlightCtrlState s, Vessel v, Quaternion orientation, bool roll)
{
mVesselState.Update(v);
// Used in the killRot activation calculation and drive_limit calculation
double precision = Math.Max(0.5, Math.Min(10.0, (Math.Min(mVesselState.torqueAvailable.x, mVesselState.torqueAvailable.z) + mVesselState.torqueThrustPYAvailable * s.mainThrottle) * 20.0 / mVesselState.MoI.magnitude));
// Reset the PID controller during roll to keep pitch and yaw errors
// from accumulating on the wrong axis.
double rollDelta = Mathf.Abs((float)(mVesselState.vesselRoll - lastResetRoll));
if (rollDelta > 180)
rollDelta = 360 - rollDelta;
if (rollDelta > 5) {
mPid.Reset();
lastResetRoll = mVesselState.vesselRoll;
}
// Direction we want to be facing
Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(v.ReferenceTransform.rotation) * orientation);
Vector3d deltaEuler = new Vector3d(
(delta.eulerAngles.x > 180) ? (delta.eulerAngles.x - 360.0F) : delta.eulerAngles.x,
-((delta.eulerAngles.y > 180) ? (delta.eulerAngles.y - 360.0F) : delta.eulerAngles.y),
(delta.eulerAngles.z > 180) ? (delta.eulerAngles.z - 360.0F) : delta.eulerAngles.z
);
Vector3d torque = new Vector3d(
mVesselState.torqueAvailable.x + mVesselState.torqueThrustPYAvailable * s.mainThrottle,
mVesselState.torqueAvailable.y,
mVesselState.torqueAvailable.z + mVesselState.torqueThrustPYAvailable * s.mainThrottle
);
Vector3d inertia = Vector3d.Scale(
mVesselState.angularMomentum.Sign(),
Vector3d.Scale(
Vector3d.Scale(mVesselState.angularMomentum, mVesselState.angularMomentum),
Vector3d.Scale(torque, mVesselState.MoI).Invert()
)
);
Vector3d err = deltaEuler * Math.PI / 180.0F;
err += inertia.Reorder(132);
err.Scale(Vector3d.Scale(mVesselState.MoI, torque.Invert()).Reorder(132));
Vector3d act = mPid.Compute(err);
float drive_limit = Mathf.Clamp01((float)(err.magnitude * drive_factor / precision));
act.x = Mathf.Clamp((float)act.x, drive_limit * -1, drive_limit);
act.y = Mathf.Clamp((float)act.y, drive_limit * -1, drive_limit);
act.z = Mathf.Clamp((float)act.z, drive_limit * -1, drive_limit);
act = lastAct + (act - lastAct) * (TimeWarp.fixedDeltaTime / Tf);
SetFlightCtrlState(act, deltaEuler, s, v, precision, drive_limit);
act = new Vector3d(s.pitch, s.yaw, s.roll);
lastAct = act;
stress = Math.Abs(act.x) + Math.Abs(act.y) + Math.Abs(act.z);
}
public override void Drive(FlightCtrlState s)
{
setPIDParameters();
// Removed the gravity since it also affect the target and we don't know the target pos here.
// Since the difference is negligable for docking it's removed
// TODO : add it back once we use the RCS Controler for other use than docking
Vector3d worldVelocityDelta = vessel.obt_velocity - targetVelocity;
//worldVelocityDelta += TimeWarp.fixedDeltaTime * vesselState.gravityForce; //account for one frame's worth of gravity
//worldVelocityDelta -= TimeWarp.fixedDeltaTime * gravityForce = FlightGlobals.getGeeForceAtPosition( Here be the target position ); ; //account for one frame's worth of gravity
// We work in local vessel coordinate
Vector3d velocityDelta = Quaternion.Inverse(vessel.GetTransform().rotation) * worldVelocityDelta;
if (!conserveFuel || (velocityDelta.magnitude > conserveThreshold))
{
if (!vessel.ActionGroups[KSPActionGroup.RCS])
{
vessel.ActionGroups.SetGroup(KSPActionGroup.RCS, true);
}
Vector3d rcs = new Vector3d();
foreach (Vector6.Direction dir in Enum.GetValues(typeof(Vector6.Direction)))
{
double dirDv = Vector3d.Dot(velocityDelta, Vector6.directions[dir]);
double dirAvail = vesselState.rcsThrustAvailable[dir];
if (dirAvail > 0 && Math.Abs(dirDv) > 0.001)
{
double dirAction = dirDv / (dirAvail * TimeWarp.fixedDeltaTime / vesselState.mass);
if (dirAction > 0)
{
rcs += Vector6.directions[dir] * dirAction;
}
}
}
Vector3d omega = Quaternion.Inverse(vessel.GetTransform().rotation) * (vessel.acceleration - vesselState.gravityForce);
rcs = pid.Compute(rcs, omega);
// Disabled the low pass filter for now. Was doing more harm than good
//rcs = lastAct + (rcs - lastAct) * (1 / ((Tf / TimeWarp.fixedDeltaTime) + 1));
lastAct = rcs;
s.X = Mathf.Clamp((float)rcs.x, -1, 1);
s.Y = Mathf.Clamp((float)rcs.z, -1, 1); //note that z and
s.Z = Mathf.Clamp((float)rcs.y, -1, 1); //y must be swapped
}
else if (conserveFuel)
{
if (vessel.ActionGroups[KSPActionGroup.RCS])
{
vessel.ActionGroups.SetGroup(KSPActionGroup.RCS, false);
}
}
base.Drive(s);
}
public static Vector3 GetWorldSurfacePostion(Vector3d geoPosition, CelestialBody body)
{
if(!body)
{
return Vector3.zero;
}
return body.GetWorldSurfacePosition(geoPosition.x, geoPosition.y, geoPosition.z);
}
protected override Vector3d ComputeForces_Model(Vector3d airVelocity, double altitude)
{
//Debug.Log("Trajectories: getting FAR forces");
Vector3 worldAirVel = new Vector3((float)airVelocity.x, (float)airVelocity.y, (float)airVelocity.z);
var parameters = new object[] { vessel_, new Vector3(), new Vector3(), worldAirVel, altitude };
FARAPI_CalculateVesselAeroForces.Invoke(null, parameters);
return (Vector3)parameters[1];
}
private void SendPositionAndRotationUpdates()
{
var unityPosition = transform.localPosition;
var position = new Vector3d(unityPosition.x * Scales.unityFactor, unityPosition.y * Scales.unityFactor, unityPosition.z * Scales.unityFactor);
var spatialPosition = (Vector3)(position / Scales.spatialFactor);
PositionWriter.Send(new Position.Update().SetCoords(spatialPosition.ToImprobableCoordinates()));
TransformInfoWriter.Send(new TransformInfo.Update().SetPosition(position.ToImprobable()).SetRotation(transform.rotation.ToImprobable()));
}
public static Entity CreatePlayerTemplate(string clientId, EntityId playerCreatorId, Vector3d position)
{
var rotation = Quaternion.identity;
var scale = new Vector3d(1.0, 1.0, 1.0);
var spatialPosition = position / Scales.spatialFactor;
var playerTemplate = EntityBuilder.Begin()
.AddPositionComponent((Vector3)spatialPosition, CommonRequirementSets.SpecificClientOnly(clientId))
.AddMetadataComponent(entityType: SimulationSettings.PlayerPrefabName)
.SetPersistence(false)
.SetReadAcl(CommonRequirementSets.PhysicsOrVisual)
.AddComponent(new TransformInfo.Data(position.ToImprobable(), rotation.ToImprobable(), scale.ToImprobable()), CommonRequirementSets.SpecificClientOnly(clientId))
.AddComponent(new ShipControls.Data(0, 0, 0), CommonRequirementSets.SpecificClientOnly(clientId))
.AddComponent(new ClientAuthorityCheck.Data(), CommonRequirementSets.SpecificClientOnly(clientId))
.AddComponent(new ClientConnection.Data(SimulationSettings.TotalHeartbeatsBeforeTimeout, clientId, playerCreatorId), CommonRequirementSets.PhysicsOnly)
.Build();
return(playerTemplate);
}
public static Improbable.Coordinates ToImprobableCoordinates(this UnityEngine.Vector3d vector)
{
return(new Improbable.Coordinates(vector.x, vector.y, vector.z));
}
public static Improbable.Vector3d ToImprobable(this UnityEngine.Vector3d vector)
{
return(new Improbable.Vector3d(vector.x, vector.y, vector.z));
}
