public void AdditionOperator()
{
QuaternionD a = new QuaternionD(1.0, 2.0, 3.0, 4.0);
QuaternionD b = new QuaternionD(2.0, 3.0, 4.0, 5.0);
QuaternionD c = a + b;
Assert.AreEqual(new QuaternionD(3.0, 5.0, 7.0, 9.0), c);
}
public void Addition()
{
QuaternionD a = new QuaternionD(1.0, 2.0, 3.0, 4.0);
QuaternionD b = new QuaternionD(2.0, 3.0, 4.0, 5.0);
QuaternionD c = QuaternionD.Add(a, b);
Assert.AreEqual(new QuaternionD(3.0, 5.0, 7.0, 9.0), c);
}
public void AreEqualWithEpsilon()
{
double epsilon = 0.001;
QuaternionD q1 = new QuaternionD(1.0, 2.0, 3.0, 4.0);
QuaternionD q2 = new QuaternionD(1.002, 2.002, 3.002, 4.002);
QuaternionD q3 = new QuaternionD(1.0001, 2.0001, 3.0001, 4.0001);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q1, q1, epsilon));
Assert.IsFalse(QuaternionD.AreNumericallyEqual(q1, q2, epsilon));
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q1, q3, epsilon));
}
public void AreEqual()
{
double originalEpsilon = Numeric.EpsilonD;
Numeric.EpsilonD = 1e-8;
QuaternionD q1 = new QuaternionD(1.0, 2.0, 3.0, 4.0);
QuaternionD q2 = new QuaternionD(1.000001, 2.000001, 3.000001, 4.000001);
QuaternionD q3 = new QuaternionD(1.00000001, 2.00000001, 3.00000001, 4.00000001);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q1, q1));
Assert.IsFalse(QuaternionD.AreNumericallyEqual(q1, q2));
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q1, q3));
Numeric.EpsilonD = originalEpsilon;
}
public void NegationOperator()
{
QuaternionD a = new QuaternionD(1.0, 2.0, 3.0, 4.0);
Assert.AreEqual(new QuaternionD(-1.0, -2.0, -3.0, -4.0), -a);
}
public void Update()
{
this.impactHappening = false;
if (FlightGlobals.ActiveVessel.mainBody.pqsController != null)
{
//do impact site calculations
this.impactHappening = true;
this.impactTime = 0;
this.impactLongitude = 0;
this.impactLatitude = 0;
this.impactAltitude = 0;
var e = FlightGlobals.ActiveVessel.orbit.eccentricity;
//get current position direction vector
var currentpos = this.RadiusDirection(FlightGlobals.ActiveVessel.orbit.trueAnomaly * Units.RAD_TO_DEG);
//calculate longitude in inertial reference frame from that
var currentirflong = 180 * Math.Atan2(currentpos.x, currentpos.y) / Math.PI;
//experimentally determined; even for very flat trajectories, the errors go into the sub-millimeter area after 5 iterations or so
const int impactiterations = 6;
//do a few iterations of impact site calculations
for (var i = 0; i < impactiterations; i++)
{
if (FlightGlobals.ActiveVessel.orbit.PeA >= this.impactAltitude)
{
//periapsis must be lower than impact alt
this.impactHappening = false;
}
if ((FlightGlobals.ActiveVessel.orbit.eccentricity < 1) && (FlightGlobals.ActiveVessel.orbit.ApA <= this.impactAltitude))
{
//apoapsis must be higher than impact alt
this.impactHappening = false;
}
if ((FlightGlobals.ActiveVessel.orbit.eccentricity >= 1) && (FlightGlobals.ActiveVessel.orbit.timeToPe <= 0))
{
//if currently escaping, we still need to be before periapsis
this.impactHappening = false;
}
if (!this.impactHappening)
{
this.impactTime = 0;
this.impactLongitude = 0;
this.impactLatitude = 0;
this.impactAltitude = 0;
break;
}
double impacttheta = 0;
if (e > 0)
{
//in this step, we are using the calculated impact altitude of the last step, to refine the impact site position
impacttheta = -180 * Math.Acos((FlightGlobals.ActiveVessel.orbit.PeR * (1 + e) / (FlightGlobals.ActiveVessel.mainBody.Radius + this.impactAltitude) - 1) / e) / Math.PI;
}
//calculate time to impact
this.impactTime = FlightGlobals.ActiveVessel.orbit.timeToPe - this.TimeToPeriapsis(impacttheta);
//calculate position vector of impact site
var impactpos = this.RadiusDirection(impacttheta);
//calculate longitude of impact site in inertial reference frame
var impactirflong = 180 * Math.Atan2(impactpos.x, impactpos.y) / Math.PI;
var deltairflong = impactirflong - currentirflong;
//get body rotation until impact
var bodyrot = 360 * this.impactTime / FlightGlobals.ActiveVessel.mainBody.rotationPeriod;
//get current longitude in body coordinates
var currentlong = FlightGlobals.ActiveVessel.longitude;
//finally, calculate the impact longitude in body coordinates
this.impactLongitude = this.NormAngle(currentlong - deltairflong - bodyrot);
//calculate impact latitude from impact position
this.impactLatitude = 180 * Math.Asin(impactpos.z / impactpos.magnitude) / Math.PI;
//calculate the actual altitude of the impact site
//altitude for long/lat code stolen from some ISA MapSat forum post; who knows why this works, but it seems to.
var rad = QuaternionD.AngleAxis(this.impactLongitude, Vector3d.down) * QuaternionD.AngleAxis(this.impactLatitude, Vector3d.forward) * Vector3d.right;
this.impactAltitude = FlightGlobals.ActiveVessel.mainBody.pqsController.GetSurfaceHeight(rad) - FlightGlobals.ActiveVessel.mainBody.pqsController.radius;
if ((this.impactAltitude < 0) && FlightGlobals.ActiveVessel.mainBody.ocean)
{
this.impactAltitude = 0;
}
}
}
// Set accessable properties.
if (this.impactHappening)
{
ShowDetails = true;
Time = this.impactTime;
Longitude = this.impactLongitude;
Latitude = this.impactLatitude;
Altitude = this.impactAltitude;
Biome = ScienceUtil.GetExperimentBiome(FlightGlobals.ActiveVessel.mainBody, this.impactLatitude, this.impactLongitude);
}
else
{
ShowDetails = false;
}
}
protected void Init(Part p, ReentrySimulation.SimCurves _simCurves)
{
Rigidbody rigidbody = p.rb;
totalMass = rigidbody == null ? 0 : rigidbody.mass; // TODO : check if we need to use this or the one without the childMass
shieldedFromAirstream = p.ShieldedFromAirstream;
noDrag = rigidbody == null && !PhysicsGlobals.ApplyDragToNonPhysicsParts;
hasLiftModule = p.hasLiftModule;
bodyLiftMultiplier = p.bodyLiftMultiplier * PhysicsGlobals.BodyLiftMultiplier;
simCurves = _simCurves;
//cubes = new DragCubeList();
CopyDragCubesList(p.DragCubes, cubes);
// Rotation to convert the vessel space vesselVelocity to the part space vesselVelocity
// QuaternionD.LookRotation is not working...
partToVessel = Quaternion.LookRotation(p.vessel.GetTransform().InverseTransformDirection(p.transform.forward), p.vessel.GetTransform().InverseTransformDirection(p.transform.up));
vesselToPart = Quaternion.Inverse(partToVessel);
//DragCubeMultiplier = PhysicsGlobals.DragCubeMultiplier;
//DragMultiplier = PhysicsGlobals.DragMultiplier;
//if (p.dragModel != Part.DragModel.CUBE)
// MechJebCore.print(p.name + " " + p.dragModel);
//oPart = p;
}
public void SetAltitudeToCurrent()
{
var pqs = Body.pqsController;
if (pqs == null)
{
Destroy(this);
return;
}
var alt = pqs.GetSurfaceHeight(QuaternionD.AngleAxis(Longitude, Vector3d.down) * QuaternionD.AngleAxis(Latitude, Vector3d.forward) * Vector3d.right) - pqs.radius;
//alt = Math.Max(alt, 0); // No need for underwater check, allow park subs
Altitude = GetComponent <Vessel>().altitude - alt;
}
// Impact Code by: mic_e
private void DrawSurface()
{
bool impacthappening = false;
double impacttime = 0;
double impactlong = 0;
double impactlat = 0;
double impactalt = 0;
if (FlightGlobals.ActiveVessel.mainBody.pqsController != null)
{
//do impact site calculations
impacthappening = true;
impacttime = 0;
impactlong = 0;
impactlat = 0;
impactalt = 0;
double e = this.vessel.orbit.eccentricity;
//get current position direction vector
Vector3d currentpos = radiusdirection(this.vessel.orbit.trueAnomaly);
//calculate longitude in inertial reference frame from that
double currentirflong = 180 * Math.Atan2(currentpos.x, currentpos.y) / Math.PI;
//experimentally determined; even for very flat trajectories, the errors go into the sub-millimeter area after 5 iterations or so
const int impactiterations = 6;
//do a few iterations of impact site calculations
for (int i = 0; i < impactiterations; i++)
{
if (this.vessel.orbit.PeA >= impactalt)
{
//periapsis must be lower than impact alt
impacthappening = false;
}
if ((this.vessel.orbit.eccentricity < 1) && (this.vessel.orbit.ApA <= impactalt))
{
//apoapsis must be higher than impact alt
impacthappening = false;
}
if ((this.vessel.orbit.eccentricity >= 1) && (this.vessel.orbit.timeToPe <= 0))
{
//if currently escaping, we still need to be before periapsis
impacthappening = false;
}
if (!impacthappening)
{
impacttime = 0;
impactlong = 0;
impactlat = 0;
impactalt = 0;
break;
}
double impacttheta = 0;
if (e > 0)
{
//in this step, we are using the calculated impact altitude of the last step, to refine the impact site position
impacttheta = -180 * Math.Acos((this.vessel.orbit.PeR * (1 + e) / (this.vessel.mainBody.Radius + impactalt) - 1) / e) / Math.PI;
}
//calculate time to impact
impacttime = this.vessel.orbit.timeToPe - timetoperiapsis(impacttheta);
//calculate position vector of impact site
Vector3d impactpos = radiusdirection(impacttheta);
//calculate longitude of impact site in inertial reference frame
double impactirflong = 180 * Math.Atan2(impactpos.x, impactpos.y) / Math.PI;
double deltairflong = impactirflong - currentirflong;
//get body rotation until impact
double bodyrot = 360 * impacttime / this.vessel.mainBody.rotationPeriod;
//get current longitude in body coordinates
double currentlong = this.vessel.longitude;
//finally, calculate the impact longitude in body coordinates
impactlong = normangle(currentlong - deltairflong - bodyrot);
//calculate impact latitude from impact position
impactlat = 180 * Math.Asin(impactpos.z / impactpos.magnitude) / Math.PI;
//calculate the actual altitude of the impact site
//altitude for long/lat code stolen from some ISA MapSat forum post; who knows why this works, but it seems to.
Vector3d rad = QuaternionD.AngleAxis(impactlong, Vector3d.down) * QuaternionD.AngleAxis(impactlat, Vector3d.forward) * Vector3d.right;
impactalt = this.vessel.mainBody.pqsController.GetSurfaceHeight(rad) - this.vessel.mainBody.pqsController.radius;
if ((impactalt < 0) && (this.vessel.mainBody.ocean == true))
{
impactalt = 0;
}
}
}
if (this.vessel.geeForce > maxGForce)
{
maxGForce = this.vessel.geeForce;
}
if (!hasCheckedForFAR)
{
hasCheckedForFAR = true;
foreach (AssemblyLoader.LoadedAssembly assembly in AssemblyLoader.loadedAssemblies)
{
if (assembly.assembly.ToString().Split(',')[0] == "FerramAerospaceResearch")
{
hasInstalledFAR = true;
print("[KerbalEngineer]: FAR detected! Turning off atmospheric details!");
}
}
}
GUILayout.Label("SURFACE DISPLAY", headingStyle);
GUILayout.BeginHorizontal(areaStyle);
GUILayout.BeginVertical();
settings.Set("*SPACER_SURFACE", "");
settings.Set("*headingStyle_SURFACE", "SURFACE DISPLAY");
if (settings.Get <bool>("Surface: Altitude (Sea Level)", true))
{
GUILayout.Label("Altitude (Sea Level)", headingStyle);
}
if (settings.Get <bool>("Surface: Altitude (Terrain)", true))
{
GUILayout.Label("Altitude (Terrain)", headingStyle);
}
if (settings.Get <bool>("Surface: Vertical Speed", true))
{
GUILayout.Label("Vertical Speed", headingStyle);
}
if (settings.Get <bool>("Surface: Horizontal Speed", true))
{
GUILayout.Label("Horizontal Speed", headingStyle);
}
if (settings.Get <bool>("Surface: Longitude", true))
{
GUILayout.Label("Longitude", headingStyle);
}
if (settings.Get <bool>("Surface: Latitude", true))
{
GUILayout.Label("Latitude", headingStyle);
}
if (impacthappening)
{
if (settings.Get <bool>("Surface: Impact Time", true))
{
GUILayout.Label("Impact Time", headingStyle);
}
if (settings.Get <bool>("Surface: Impact Longitude", true))
{
GUILayout.Label("Impact Longitude", headingStyle);
}
if (settings.Get <bool>("Surface: Impact Latitude", true))
{
GUILayout.Label("Impact Latitude", headingStyle);
}
if (settings.Get <bool>("Surface: Impact Altitude", true))
{
GUILayout.Label("Impact Altitude", headingStyle);
}
}
if (settings.Get <bool>("Surface: G-Force", true))
{
GUILayout.Label("G-Force", headingStyle);
}
if (!hasInstalledFAR)
{
if (settings.Get <bool>("Surface: Terminal Velocity", true))
{
GUILayout.Label("Terminal Velocity", headingStyle);
}
if (settings.Get <bool>("Surface: Atmospheric Efficiency", true))
{
GUILayout.Label("Atmospheric Efficiency", headingStyle);
}
if (settings.Get <bool>("Surface: Atmospheric Drag", true))
{
GUILayout.Label("Atmospheric Drag", headingStyle);
}
if (settings.Get <bool>("Surface: Atmospheric Pressure", true))
{
GUILayout.Label("Atmospheric Pressure", headingStyle);
}
if (settings.Get <bool>("Surface: Atmospheric Density", true))
{
GUILayout.Label("Atmospheric Density", headingStyle);
}
}
GUILayout.EndVertical();
GUILayout.BeginVertical();
if (settings.Get <bool>("Surface: Altitude (Sea Level)"))
{
GUILayout.Label(Tools.FormatSI(this.vessel.mainBody.GetAltitude(this.vessel.CoM), Tools.SIUnitType.Distance), dataStyle);
}
if (settings.Get <bool>("Surface: Altitude (Terrain)"))
{
GUILayout.Label(Tools.FormatSI(this.vessel.mainBody.GetAltitude(this.vessel.CoM) - this.vessel.terrainAltitude, Tools.SIUnitType.Distance), dataStyle);
}
if (settings.Get <bool>("Surface: Vertical Speed"))
{
GUILayout.Label(Tools.FormatSI(this.vessel.verticalSpeed, Tools.SIUnitType.Speed), dataStyle);
}
if (settings.Get <bool>("Surface: Horizontal Speed"))
{
GUILayout.Label(Tools.FormatSI(this.vessel.horizontalSrfSpeed, Tools.SIUnitType.Speed), dataStyle);
}
if (settings.Get <bool>("Surface: Longitude"))
{
GUILayout.Label(Tools.FormatNumber(this.vessel.longitude, "°", 6), dataStyle);
}
if (settings.Get <bool>("Surface: Latitude"))
{
GUILayout.Label(Tools.FormatNumber(this.vessel.latitude, "°", 6), dataStyle);
}
if (impacthappening)
{
if (settings.Get <bool>("Surface: Impact Time", true))
{
GUILayout.Label(Tools.FormatTime(impacttime), dataStyle);
}
if (settings.Get <bool>("Surface: Impact Longitude", true))
{
GUILayout.Label(Tools.FormatNumber(impactlong, "°", 6), dataStyle);
}
if (settings.Get <bool>("Surface: Impact Latitude", true))
{
GUILayout.Label(Tools.FormatNumber(impactlat, "°", 6), dataStyle);
}
if (settings.Get <bool>("Surface: Impact Altitude", true))
{
GUILayout.Label(Tools.FormatSI(impactalt, Tools.SIUnitType.Distance), dataStyle);
}
}
if (settings.Get <bool>("Surface: G-Force"))
{
GUILayout.Label(Tools.FormatNumber(this.vessel.geeForce, 3) + " / " + Tools.FormatNumber(maxGForce, "g", 3), dataStyle);
}
if (!hasInstalledFAR)
{
double totalMass = 0d;
double massDrag = 0d;
foreach (Part part in this.vessel.parts)
{
if (part.physicalSignificance != Part.PhysicalSignificance.NONE)
{
double partMass = part.mass + part.GetResourceMass();
totalMass += partMass;
massDrag += partMass * part.maximum_drag;
}
}
double gravity = FlightGlobals.getGeeForceAtPosition(this.vessel.CoM).magnitude;
double atmosphere = this.vessel.atmDensity;
double terminalVelocity = 0d;
if (atmosphere > 0)
{
terminalVelocity = Math.Sqrt((2 * totalMass * gravity) / (atmosphere * massDrag * FlightGlobals.DragMultiplier));
}
double atmosphericEfficiency = 0d;
if (terminalVelocity > 0)
{
atmosphericEfficiency = FlightGlobals.ship_srfSpeed / terminalVelocity;
}
double dragForce = 0.5 * atmosphere * Math.Pow(FlightGlobals.ship_srfSpeed, 2) * massDrag * FlightGlobals.DragMultiplier;
if (settings.Get <bool>("Surface: Terminal Velocity"))
{
GUILayout.Label(Tools.FormatSI(terminalVelocity, Tools.SIUnitType.Speed), dataStyle);
}
if (settings.Get <bool>("Surface: Atmospheric Efficiency"))
{
GUILayout.Label(Tools.FormatNumber(atmosphericEfficiency * 100, "%", 2), dataStyle);
}
if (settings.Get <bool>("Surface: Atmospheric Drag"))
{
GUILayout.Label(Tools.FormatSI(dragForce, Tools.SIUnitType.Force), dataStyle);
}
if (settings.Get <bool>("Surface: Atmospheric Pressure"))
{
GUILayout.Label(Tools.FormatSI(this.part.dynamicPressureAtm * 100, Tools.SIUnitType.Pressure), dataStyle);
}
if (settings.Get <bool>("Surface: Atmospheric Density"))
{
GUILayout.Label(Tools.FormatSI(this.vessel.atmDensity, Tools.SIUnitType.Density), dataStyle);
}
}
GUILayout.EndVertical();
GUILayout.EndHorizontal();
}
public void IndexerReadException2()
{
QuaternionD q = new QuaternionD(1.0, 2.0, 3.0, 4.0);
double d = q[4];
}
public void IndexerReadException2()
{
QuaternionD q = new QuaternionD(1.0, 2.0, 3.0, 4.0);
double d = q[4];
}
public void Length()
{
QuaternionD q = new QuaternionD(1.0, 2.0, 3.0, 4.0);
double length = (double)Math.Sqrt(1 + 4 + 9 + 16);
Assert.AreEqual(length, q.Modulus);
}
public void LengthSquared()
{
QuaternionD q = new QuaternionD(1.0, 2.0, 3.0, 4.0);
double lengthSquared = 1 + 4 + 9 + 16;
Assert.AreEqual(lengthSquared, q.Norm);
}
public void IsNaN()
{
const int numberOfRows = 4;
Assert.IsFalse(new QuaternionD().IsNaN);
for (int i = 0; i < numberOfRows; i++)
{
QuaternionD v = new QuaternionD();
v[i] = double.NaN;
Assert.IsTrue(v.IsNaN);
}
}
public void Invert()
{
QuaternionD inverseIdentity = QuaternionD.Identity;
inverseIdentity.Invert();
Assert.AreEqual(QuaternionD.Identity, inverseIdentity);
double angle = 0.4;
Vector3D axis = new Vector3D(1.0, 1.0, 1.0);
axis.Normalize();
QuaternionD q = QuaternionD.CreateRotation(axis, angle);
q.Invert();
Assert.IsTrue(Vector3D.AreNumericallyEqual(-axis, q.Axis));
q = new QuaternionD(1, 2, 3, 4);
QuaternionD inverse = q;
inverse.Invert();
Assert.IsTrue(QuaternionD.AreNumericallyEqual(QuaternionD.Identity, inverse * q));
}
public void IndexerWriteException2()
{
QuaternionD q = new QuaternionD(1.0, 2.0, 3.0, 4.0);
q[4] = 0.0;
}
public void ParseException()
{
QuaternionD vector = QuaternionD.Parse("(0.0123; 9.876; 4.1; -9.0)");
}
public void CreateRotation()
{
QuaternionD q;
// From matrix vs. from angle/axis
Matrix33D m = Matrix33D.CreateRotation(Vector3D.UnitX, (double)Math.PI / 4);
q = QuaternionD.CreateRotation(m);
QuaternionD q2 = QuaternionD.CreateRotation(Vector3D.UnitX, (double)Math.PI / 4);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q2, q));
m = Matrix33D.CreateRotation(Vector3D.UnitY, (double)Math.PI / 4);
q = QuaternionD.CreateRotation(m);
q2 = QuaternionD.CreateRotation(Vector3D.UnitY, (double)Math.PI / 4);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q2, q));
m = Matrix33D.CreateRotation(Vector3D.UnitZ, (double)Math.PI / 4);
q = QuaternionD.CreateRotation(m);
q2 = QuaternionD.CreateRotation(Vector3D.UnitZ, (double)Math.PI / 4);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q2, q));
// From vector-vector
Vector3D start, end;
start = Vector3D.UnitX;
end = Vector3D.UnitY;
q = QuaternionD.CreateRotation(start, end);
Assert.IsTrue(Vector3D.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
start = Vector3D.UnitY;
end = Vector3D.UnitZ;
q = QuaternionD.CreateRotation(start, end);
Assert.IsTrue(Vector3D.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
start = Vector3D.UnitZ;
end = Vector3D.UnitX;
q = QuaternionD.CreateRotation(start, end);
Assert.IsTrue(Vector3D.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
start = new Vector3D(1, 1, 1);
end = new Vector3D(1, 1, 1);
q = QuaternionD.CreateRotation(start, end);
Assert.IsTrue(Vector3D.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
start = new Vector3D(1.0, 1.0, 1.0);
end = new Vector3D(-1.0, -1.0, -1.0);
q = QuaternionD.CreateRotation(start, end);
Assert.IsTrue(Vector3D.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
start = new Vector3D(-1.0, 2.0, 1.0);
end = new Vector3D(-2.0, -1.0, -1.0);
q = QuaternionD.CreateRotation(start, end);
Assert.IsTrue(Vector3D.AreNumericallyEqual(end, q.ToRotationMatrix33() * start));
double degree45 = MathHelper.ToRadians(45);
q = QuaternionD.CreateRotation(degree45, Vector3D.UnitZ, degree45, Vector3D.UnitY, degree45, Vector3D.UnitX, false);
QuaternionD expected = QuaternionD.CreateRotation(Vector3D.UnitZ, degree45) * QuaternionD.CreateRotation(Vector3D.UnitY, degree45)
* QuaternionD.CreateRotation(Vector3D.UnitX, degree45);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(expected, q));
q = QuaternionD.CreateRotation(degree45, Vector3D.UnitZ, degree45, Vector3D.UnitY, degree45, Vector3D.UnitX, true);
expected = QuaternionD.CreateRotation(Vector3D.UnitX, degree45) * QuaternionD.CreateRotation(Vector3D.UnitY, degree45)
* QuaternionD.CreateRotation(Vector3D.UnitZ, degree45);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(expected, q));
}
public void HashCode()
{
QuaternionD q = new QuaternionD(1.0, 2.0, 3.0, 4.0);
Assert.AreNotEqual(QuaternionD.Zero.GetHashCode(), q.GetHashCode());
}
public void IndexerRead()
{
QuaternionD q = new QuaternionD(1.0, 2.0, 3.0, 4.0);
Assert.AreEqual(1.0, q[0]);
Assert.AreEqual(2.0, q[1]);
Assert.AreEqual(3.0, q[2]);
Assert.AreEqual(4.0, q[3]);
}
public void NormalizeException()
{
QuaternionD q = QuaternionD.Zero;
q.Normalize();
}
public void Ln4()
{
double θ = 0.0;
Vector3D v = new Vector3D(1.0, 2.0, 3.0);
v.Normalize();
QuaternionD q = new QuaternionD((double)Math.Cos(θ), (double)Math.Sin(θ) * v);
q.Ln();
Assert.IsTrue(Numeric.AreEqual(0.0, q.W));
Assert.IsTrue(Vector3D.AreNumericallyEqual(θ * v, q.V));
}
public void CreateRotationException2()
{
QuaternionD.CreateRotation(Vector3D.UnitY, Vector3D.Zero);
}
public void LnException()
{
QuaternionD q = new QuaternionD(1.5, 0.0, 0.0, 0.0);
QuaternionD.Ln(q);
}
public static Coordinates GetMouseCoordinates(CelestialBody body)
{
Ray mouseRay = PlanetariumCamera.Camera.ScreenPointToRay(Input.mousePosition);
mouseRay.origin = ScaledSpace.ScaledToLocalSpace(mouseRay.origin);
Vector3d relOrigin = mouseRay.origin - body.position;
Vector3d relSurfacePosition;
double curRadius = body.pqsController.radiusMax;
double lastRadius = 0;
double error = 0;
int loops = 0;
float st = Time.time;
while (loops < 50)
{
if (PQS.LineSphereIntersection(relOrigin, mouseRay.direction, curRadius, out relSurfacePosition))
{
Vector3d surfacePoint = body.position + relSurfacePosition;
double alt = body.pqsController.GetSurfaceHeight(QuaternionD.AngleAxis(body.GetLongitude(surfacePoint), Vector3d.down) * QuaternionD.AngleAxis(body.GetLatitude(surfacePoint), Vector3d.forward) * Vector3d.right);
error = Math.Abs(curRadius - alt);
if (error < (body.pqsController.radiusMax - body.pqsController.radiusMin) / 100)
{
return(new Coordinates(body.GetLatitude(surfacePoint), MuUtils.ClampDegrees180(body.GetLongitude(surfacePoint))));
}
else
{
lastRadius = curRadius;
curRadius = alt;
loops++;
}
}
else
{
if (loops == 0)
{
break;
}
else
{ // Went too low, needs to try higher
curRadius = (lastRadius * 9 + curRadius) / 10;
loops++;
}
}
}
return(null);
}
public void MultiplyScalar()
{
double s = 123.456;
QuaternionD q = new QuaternionD(1.0, 2.0, 3.0, 4.0);
QuaternionD expectedResult = new QuaternionD(s * 1.0, s * 2.0, s * 3.0, s * 4.0);
QuaternionD result = QuaternionD.Multiply(s, q);
Assert.AreEqual(expectedResult, result);
}
internal static double getElevation(this CelestialBody body, Vector3d worldPosition)
{
if (body.pqsController == null)
{
return(0);
}
Vector3d pqsRadialVector = QuaternionD.AngleAxis(body.GetLongitude(worldPosition), Vector3d.down) * QuaternionD.AngleAxis(body.GetLatitude(worldPosition), Vector3d.forward) * Vector3d.right;
double ret = body.pqsController.GetSurfaceHeight(pqsRadialVector) - body.pqsController.radius;
if (ret < 0)
{
ret = 0;
}
return(ret);
}
public void MultiplyScalarOperator()
{
double s = 123.456;
QuaternionD q = new QuaternionD(1.0, 2.0, 3.0, 4.0);
QuaternionD expectedResult = new QuaternionD(s * 1.0, s * 2.0, s * 3.0, s * 4.0);
QuaternionD result1 = s * q;
QuaternionD result2 = q * s;
Assert.AreEqual(expectedResult, result1);
Assert.AreEqual(expectedResult, result2);
}
public ReentrySimulation(Orbit _initialOrbit, double _UT, SimulatedVessel _vessel, SimCurves _simcurves, IDescentSpeedPolicy _descentSpeedPolicy, double _decelEndAltitudeASL, double _maxThrustAccel, double _parachuteSemiDeployMultiplier, double _probableLandingSiteASL, bool _multiplierHasError, double _dt, double _min_dt)
{
// Store all the input values as they were given
input_initialOrbit = _initialOrbit;
input_UT = _UT;
vessel = _vessel;
input_descentSpeedPolicy = _descentSpeedPolicy;
input_decelEndAltitudeASL = _decelEndAltitudeASL;
input_maxThrustAccel = _maxThrustAccel;
input_parachuteSemiDeployMultiplier = _parachuteSemiDeployMultiplier;
input_probableLandingSiteASL = _probableLandingSiteASL;
input_multiplierHasError = _multiplierHasError;
input_dt = _dt;
// the vessel attitude relative to the surface vel. Fixed for now
attitude = Quaternion.Euler(180,0,0);
min_dt = _min_dt;
max_dt = _dt;
dt = max_dt;
// Get a copy of the original orbit, to be more thread safe
initialOrbit = new Orbit();
initialOrbit.UpdateFromOrbitAtUT(_initialOrbit, _UT, _initialOrbit.referenceBody);
CelestialBody body = _initialOrbit.referenceBody;
bodyHasAtmosphere = body.atmosphere;
bodyRadius = body.Radius;
gravParameter = body.gravParameter;
this.parachuteSemiDeployMultiplier = _parachuteSemiDeployMultiplier;
this.multiplierHasError = _multiplierHasError;
bodyAngularVelocity = body.angularVelocity;
this.descentSpeedPolicy = _descentSpeedPolicy;
decelRadius = bodyRadius + _decelEndAltitudeASL;
aerobrakedRadius = bodyRadius + body.RealMaxAtmosphereAltitude();
mainBody = body;
this.maxThrustAccel = _maxThrustAccel;
this.probableLandingSiteASL = _probableLandingSiteASL;
this.probableLandingSiteRadius = _probableLandingSiteASL + bodyRadius;
referenceFrame = ReferenceFrame.CreateAtCurrentTime(_initialOrbit.referenceBody);
orbitReenters = OrbitReenters(_initialOrbit);
if (orbitReenters)
startUT = _UT;
maxDragGees = 0;
deltaVExpended = 0;
trajectory = new List<AbsoluteVector>();
simCurves = _simcurves;
once = true;
}
public void Negation()
{
QuaternionD a = new QuaternionD(1.0, 2.0, 3.0, 4.0);
Assert.AreEqual(new QuaternionD(-1.0, -2.0, -3.0, -4.0), QuaternionD.Negate(a));
}
public static double TerrainAltitude(this CelestialBody body, double latitude, double longitude)
{
if (body.pqsController == null)
{
return(0);
}
Vector3d pqsRadialVector = QuaternionD.AngleAxis(longitude, Vector3d.down) * QuaternionD.AngleAxis(latitude, Vector3d.forward) * Vector3d.right;
double ret = body.pqsController.GetSurfaceHeight(pqsRadialVector) - body.pqsController.radius;
if (ret < 0)
{
ret = 0;
}
return(ret);
}
public void NegationOperator()
{
QuaternionD a = new QuaternionD(1.0, 2.0, 3.0, 4.0);
Assert.AreEqual(new QuaternionD(-1.0, -2.0, -3.0, -4.0), -a);
}
public void LnException()
{
QuaternionD q = new QuaternionD(1.5, 0.0, 0.0, 0.0);
QuaternionD.Ln(q);
}
public void Normalized()
{
QuaternionD q = new QuaternionD(1.0, 2.0, 3.0, 4.0);
Assert.AreNotEqual(1.0, q.Modulus);
Assert.IsFalse(q.IsNumericallyNormalized);
QuaternionD normalized = q.Normalized;
Assert.AreEqual(new QuaternionD(1.0, 2.0, 3.0, 4.0), q);
Assert.IsTrue(Numeric.AreEqual(1.0, normalized.Modulus));
Assert.IsTrue(normalized.IsNumericallyNormalized);
}
public void Negation()
{
QuaternionD a = new QuaternionD(1.0, 2.0, 3.0, 4.0);
Assert.AreEqual(new QuaternionD(-1.0, -2.0, -3.0, -4.0), QuaternionD.Negate(a));
}
public void Power2()
{
const double θ = 0.4;
const double t = -1.2;
Vector3D v = new Vector3D(2.3, 1.0, -2.0);
v.Normalize();
QuaternionD q = new QuaternionD(Math.Cos(θ), Math.Sin(θ) * v);
q.Power(t);
QuaternionD expected = new QuaternionD(Math.Cos(t * θ), Math.Sin(t * θ) * v);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(expected, q));
}
public void TestEquals2()
{
QuaternionD q = QuaternionD.Identity;
Assert.IsFalse(q.Equals(q.ToString()));
}
public void Power3()
{
const double θ = 0.4;
Vector3D v = new Vector3D(2.3, 1.0, -2.0);
v.Normalize();
QuaternionD q = new QuaternionD(Math.Cos(θ), Math.Sin(θ) * v);
QuaternionD q2 = q;
q2.Power(2);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q * q, q2));
QuaternionD q3 = q;
q3.Power(3);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q * q * q, q3));
q2 = q;
q2.Power(-2);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q.Inverse * q.Inverse, q2));
q3 = q;
q3.Power(-3);
Assert.IsTrue(QuaternionD.AreNumericallyEqual(q.Inverse * q.Inverse * q.Inverse, q3));
}
public void InvertException()
{
QuaternionD inverseOfZero = QuaternionD.Zero;
inverseOfZero.Invert();
}
public void Properties()
{
QuaternionD q = new QuaternionD(0.123, 1.0, 2.0, 3.0);
Assert.AreEqual(0.123, q.W);
Assert.AreEqual(1.0, q.X);
Assert.AreEqual(2.0, q.Y);
Assert.AreEqual(3.0, q.Z);
q.W = 1.0;
q.X = 2.0;
q.Y = 3.0;
q.Z = 4.0;
Assert.AreEqual(1.0, q.W);
Assert.AreEqual(2.0, q.X);
Assert.AreEqual(3.0, q.Y);
Assert.AreEqual(4.0, q.Z);
q.V = new Vector3D(-1.0, -2.0, -3.0);
Assert.AreEqual(-1.0, q.X);
Assert.AreEqual(-2.0, q.Y);
Assert.AreEqual(-3.0, q.Z);
Assert.AreEqual(new Vector3D(-1.0, -2.0, -3.0), q.V);
}
public void IndexerWriteException2()
{
QuaternionD q = new QuaternionD(1.0, 2.0, 3.0, 4.0);
q[4] = 0.0;
}
/// <summary>
/// Transforms a 3D vector by the given <see cref="SharpDX.Quaternion"/> rotation.
/// </summary>
/// <param name="vector">The vector to rotate.</param>
/// <param name="rotation">The <see cref="SharpDX.Quaternion"/> rotation to apply.</param>
/// <param name="result">When the method completes, contains the transformed <see cref="SharpDX.Vector4"/>.</param>
public static void Transform(ref Vector3D vector, ref QuaternionD rotation, out Vector3D result)
{
double x = rotation.X + rotation.X;
double y = rotation.Y + rotation.Y;
double z = rotation.Z + rotation.Z;
double wx = rotation.W * x;
double wy = rotation.W * y;
double wz = rotation.W * z;
double xx = rotation.X * x;
double xy = rotation.X * y;
double xz = rotation.X * z;
double yy = rotation.Y * y;
double yz = rotation.Y * z;
double zz = rotation.Z * z;
result = new Vector3D(
((vector.X * ((1.0 - yy) - zz)) + (vector.Y * (xy - wz))) + (vector.Z * (xz + wy)),
((vector.X * (xy + wz)) + (vector.Y * ((1.0 - xx) - zz))) + (vector.Z * (yz - wx)),
((vector.X * (xz - wy)) + (vector.Y * (yz + wx))) + (vector.Z * ((1.0 - xx) - yy)));
}
public void NormalizedException()
{
QuaternionD q = QuaternionD.Zero;
q = q.Normalized;
}
/// <summary>
/// Transforms a 3D vector by the given <see cref="SharpDX.Quaternion"/> rotation.
/// </summary>
/// <param name="vector">The vector to rotate.</param>
/// <param name="rotation">The <see cref="SharpDX.Quaternion"/> rotation to apply.</param>
/// <returns>The transformed <see cref="SharpDX.Vector4"/>.</returns>
public static Vector3D Transform(Vector3D vector, QuaternionD rotation)
{
Vector3D result;
Transform(ref vector, ref rotation, out result);
return result;
}
public void CreateRotationException1()
{
QuaternionD.CreateRotation(Vector3D.Zero, Vector3D.UnitX);
}
public static QuaternionD GetValueDefault(this ConfigNode config, string name, QuaternionD default_value)
{
string val = config.GetValue(name);
return (val != null ? ConfigNode.ParseQuaternionD(val) : default_value);
}
/// <summary>
/// This is the recursive function that implements the numeric terrain hit solver.
/// The exact algorithm is too wordy to explain here in a text comment.
/// See this markdown file on github for the full explanation:
/// doc/Recursive_Numeric_Terrain_Hit.md
/// </summary>
/// <param name="newDist">The "return value" (the actual return is bool, this is the distance if return is true).</param>
/// <param name="done">returns true if the algorithm came to a final answer, false it if needs more time.</param></param>
/// <param name="hitBody">body of the terrain</param>
/// <param name="origin">start of this line segment of the ray</param>
/// <param name="pointingUnitVec">direction of the ray - must be a unit vector starting at origin</param>
/// <param name="dist">length of this line segment of the ray</param>
/// <param name="slices">Number of slices to try to cut the line segment into</param>
/// <returns>True if there was a hit</returns>
private bool numericPQSSolver(
out double newDist,
out bool done,
CelestialBody hitBody,
Vector3d origin,
Vector3d pointingUnitVec,
double dist,
int slices)
{
// Some bodies have no PQS collider - like the sun. For them they have no surface and this doesn't work:
if (hitBody.pqsController == null)
{
newDist = -1;
done = true;
return(false);
}
DebugMsg("numericPQSSolver( (out),(out)," + hitBody.name + ", " + origin + ", " + pointingUnitVec + ", " + dist + ", " + slices + ");");
bool success = false;
bool continueNextTime = false;
bool hasOcean = hitBody.ocean;
int i;
double lat;
double lng;
double segmentLength = 0.0;
newDist = dist;
int slicesThisTime = slices;
Vector3d samplePoint = origin;
Vector3d prevSamplePoint;
if (dist <= epsilon)
{
continueNextTime = false;
success = this.honingSuccess;
newDist = dist;
DebugMsg("dist is now small enough to quit. continue=" + continueNextTime + ", success=" + success);
}
else
{
// We already know i=0 is above ground, so start at i=1:
for (i = 1; i <= slicesThisTime; ++i)
{
prevSamplePoint = samplePoint;
samplePoint = origin + (i * (dist / slicesThisTime)) * pointingUnitVec;
segmentLength = (samplePoint - prevSamplePoint).magnitude;
lat = hitBody.GetLatitude(samplePoint);
lng = hitBody.GetLongitude(samplePoint);
var bodyUpVector = new Vector3d(1, 0, 0);
bodyUpVector = QuaternionD.AngleAxis(lat, Vector3d.forward /*around Z axis*/) * bodyUpVector;
bodyUpVector = QuaternionD.AngleAxis(lng, Vector3d.down /*around -Y axis*/) * bodyUpVector;
double groundAlt = hitBody.pqsController.GetSurfaceHeight(bodyUpVector) - hitBody.Radius;
double samplePointAlt = hitBody.GetAltitude(samplePoint);
if (samplePointAlt <= groundAlt || (hasOcean && samplePointAlt < 0))
{
DebugMsg("Found a below ground: samplePointAlt=" + samplePointAlt + ", groundAlt=" + groundAlt);
success = true;
this.honingSuccess = true;
double subSectionDist;
bool subDone;
numericPQSSolver(out subSectionDist, out subDone, hitBody, prevSamplePoint, pointingUnitVec,
segmentLength, slices);
continueNextTime = !subDone;
newDist = ((i - 1) * (dist / slicesThisTime)) + subSectionDist;
break;
}
if (rayCastTimer.Elapsed.TotalMilliseconds > millisecondCap)
{
DebugMsg("Ran out of milliseconds: " + rayCastTimer.Elapsed.TotalMilliseconds + " > " + millisecondCap);
this.hitBody = hitBody;
this.origin = prevSamplePoint - pointingUnitVec * 20; // back up 20 meters because the planet will move some
this.pointingUnitVec = pointingUnitVec;
this.dist = segmentLength * (slicesThisTime - i + 2);
this.slices = slices;
continueNextTime = true;
break;
}
}
DebugMsg("numericPQSSolver after " + Math.Round(rayCastTimer.Elapsed.TotalMilliseconds, 3) + " millis i = " + i + " and continueNextTime=" + continueNextTime);
if (i > slicesThisTime && segmentLength > epsilon && !continueNextTime)
{
// The above loop got to the end without finding a hit, and the length of
// the line segments is still not so small as to be time to give up yet.
// Before giving up, it might be the case that there's a hit under the ground
// in-between the sample points that were tried, like this:
//
// __
// / \
// *----------*----------*--/----\--*----------*----------*----------*----
// _ ___/ ^ \ ____
// _____ ____/ \_/ | \___/ \ _________
// \____/ hit in \______/ \__________
// between the
// sample points
bool subDone = false;
DebugMsg("numericPQSSolver recursing.");
success = numericPQSSolver(out newDist, out subDone, hitBody, origin, pointingUnitVec,
dist, 2 * slices);
continueNextTime = !subDone;
}
}
done = !continueNextTime;
DebugMsg("numericPQSSolver returning " + success + " dist=" + newDist + " done=" + done);
return(success);
}
private void OnXyzwChanged()
{
if (_isUpdating)
return;
_isUpdating = true;
try
{
if (_vectorType == typeof(Vector4))
Value = new Vector4((float)X, (float)Y, (float)Z, (float)W);
else if (_vectorType == typeof(Quaternion))
Value = new Quaternion((float)X, (float)Y, (float)Z, (float)W);
else if (_vectorType == typeof(Vector4F))
Value = new Vector4F((float)X, (float)Y, (float)Z, (float)W);
else if (_vectorType == typeof(Vector4D))
Value = new Vector4D(X, Y, Z, W);
else if (_vectorType == typeof(QuaternionF))
Value = new QuaternionF((float)W, (float)X, (float)Y, (float)Z);
else if (_vectorType == typeof(QuaternionD))
Value = new QuaternionD(W, X, Y, Z);
}
finally
{
_isUpdating = false;
}
}
/// <summary>
/// Get true altitude above terrain (from MuMech lib)
/// Also from: http://kerbalspaceprogram.com/forum/index.php?topic=10324.msg161923#msg161923
/// </summary>
public static double GetTrueAltitude(Vessel vessel)
{
Vector3 CoM = vessel.findWorldCenterOfMass();
Vector3 up = (CoM - vessel.mainBody.position).normalized;
double altitudeASL = vessel.mainBody.GetAltitude(CoM);
double altitudeTrue = 0.0;
RaycastHit sfc;
if (Physics.Raycast(CoM, -up, out sfc, (float)altitudeASL + 10000.0F, 1 << 15))
{
altitudeTrue = sfc.distance;
}
else if (vessel.mainBody.pqsController != null)
{
altitudeTrue = vessel.mainBody.GetAltitude(CoM) - (vessel.mainBody.pqsController.GetSurfaceHeight(QuaternionD.AngleAxis(vessel.mainBody.GetLongitude(CoM), Vector3d.down) * QuaternionD.AngleAxis(vessel.mainBody.GetLatitude(CoM), Vector3d.forward) * Vector3d.right) - vessel.mainBody.pqsController.radius);
}
else
{
altitudeTrue = vessel.mainBody.GetAltitude(CoM);
}
return(altitudeTrue);
}
internal void UpdateRotation(QuaternionD rotation, Matrix4x4 World2Planet, Matrix4x4 mainRotationMatrix, Matrix4x4 detailRotationMatrix)
{
if (rotation != null)
{
CloudMesh.transform.localRotation = rotation;
if (ShadowProjector != null && Sunlight != null)
{
Vector3 worldSunDir;
Vector3 sunDirection;
//AtmosphereManager.Log("light: " + Sunlight.intensity);
//AtmosphereManager.Log("light: " + Sunlight.color);
worldSunDir = Vector3.Normalize(Sunlight.transform.forward);
sunDirection = Vector3.Normalize(ShadowProjector.transform.parent.InverseTransformDirection(worldSunDir));
ShadowProjector.transform.localPosition = radiusScaleLocal * -sunDirection;
ShadowProjector.transform.forward = worldSunDir;
if (Scaled)
{
ShadowProjector.material.SetVector(ShaderProperties.SUNDIR_PROPERTY, sunDirection);
}
else
{
ShadowProjector.material.SetVector(ShaderProperties.SUNDIR_PROPERTY, worldSunDir);
}
}
}
CloudMaterial.SetVector(ShaderProperties.PLANET_ORIGIN_PROPERTY, CloudMesh.transform.position);
SetRotations(World2Planet, mainRotationMatrix, detailRotationMatrix);
}
public static QuaternionD BodyRotationAtdT(CelestialBody Body, double dT)
{
var angle = -(dT / Body.rotationPeriod * 360 % 360.0);
return(QuaternionD.AngleAxis(angle, Body.zUpAngularVelocity.normalized));
}
public static void DrawMapViewGroundMarker(CelestialBody body, double latitude, double longitude, Color c, double rotation = 0, double radius = 0)
{
Vector3d up = body.GetSurfaceNVector(latitude, longitude);
var height = body.pqsController.GetSurfaceHeight(QuaternionD.AngleAxis(longitude, Vector3d.down) * QuaternionD.AngleAxis(latitude, Vector3d.forward) * Vector3d.right);
if (height < body.Radius)
{
height = body.Radius;
}
Vector3d center = body.position + height * up;
if (IsOccluded(center, body))
{
return;
}
Vector3d north = Vector3d.Exclude(up, body.transform.up).normalized;
if (radius <= 0)
{
radius = body.Radius / 15;
}
GLTriangleMap(new Vector3d[] {
center,
center + radius * (QuaternionD.AngleAxis(rotation - 10, up) * north),
center + radius * (QuaternionD.AngleAxis(rotation + 10, up) * north)
}, c);
GLTriangleMap(new Vector3d[] {
center,
center + radius * (QuaternionD.AngleAxis(rotation + 110, up) * north),
center + radius * (QuaternionD.AngleAxis(rotation + 130, up) * north)
}, c);
GLTriangleMap(new Vector3d[] {
center,
center + radius * (QuaternionD.AngleAxis(rotation - 110, up) * north),
center + radius * (QuaternionD.AngleAxis(rotation - 130, up) * north)
}, c);
}
private void UpdateMove()
{
if (!MovingVessel)
{
EndMove();
return;
}
MovingVessel.IgnoreGForces(240);
// Lerp is animating move
if (!_hoverChanged)
{
MoveHeight = Mathf.Lerp(MoveHeight, _vBounds.BottomLength + HoverHeight, 10 * Time.fixedDeltaTime);
}
else
{
double alt = MovingVessel.radarAltitude;
// sINCE Lerp is animating move from 0 to hoverheight, we do not want this going below current altitude
if (MoveHeight < alt)
{
MoveHeight = Convert.ToSingle(alt);
}
MoveHeight = MovingVessel.Splashed
? Mathf.Lerp(MoveHeight, _vBounds.BottomLength + _hoverAdjust, 10 * Time.fixedDeltaTime)
: Mathf.Lerp(MoveHeight, _vBounds.BottomLength + (MoveHeight + _hoverAdjust < 0 ? -MoveHeight : _hoverAdjust), 10 * Time.fixedDeltaTime);
}
MovingVessel.ActionGroups.SetGroup(KSPActionGroup.RCS, false);
_up = (MovingVessel.transform.position - FlightGlobals.currentMainBody.transform.position).normalized;
Vector3 forward;
if (MapView.MapIsEnabled)
{
forward = North();
}
else
{
forward = Vector3.ProjectOnPlane(MovingVessel.CoM - FlightCamera.fetch.mainCamera.transform.position, _up).normalized;
if (Vector3.Dot(-_up, FlightCamera.fetch.mainCamera.transform.up) > 0)
{
forward = Vector3.ProjectOnPlane(FlightCamera.fetch.mainCamera.transform.up, _up).normalized;
}
}
Vector3 right = Vector3.Cross(_up, forward);
Vector3 offsetDirection = Vector3.zero;
bool inputting = false;
//Altitude Adjustment
if (GameSettings.THROTTLE_CUTOFF.GetKey())
{
_hoverAdjust = 0f;
_hoverChanged = false;
}
if (GameSettings.THROTTLE_UP.GetKey())
{
_hoverAdjust += MoveSpeed * Time.fixedDeltaTime;
inputting = true;
_hoverChanged = true;
}
if (GameSettings.THROTTLE_DOWN.GetKey())
{
_hoverAdjust += -(MoveSpeed * Time.fixedDeltaTime);
inputting = true;
_hoverChanged = true;
}
if (GameSettings.PITCH_DOWN.GetKey())
{
offsetDirection += (forward * MoveSpeed * Time.fixedDeltaTime);
inputting = true;
}
if (GameSettings.PITCH_UP.GetKey())
{
offsetDirection += (-forward * MoveSpeed * Time.fixedDeltaTime);
inputting = true;
}
if (GameSettings.YAW_RIGHT.GetKey())
{
offsetDirection += (right * MoveSpeed * Time.fixedDeltaTime);
inputting = true;
}
if (GameSettings.YAW_LEFT.GetKey())
{
offsetDirection += (-right * MoveSpeed * Time.fixedDeltaTime);
inputting = true;
}
if (GameSettings.TRANSLATE_RIGHT.GetKey())
{
_startRotation = Quaternion.AngleAxis(-RotationSpeed, MovingVessel.ReferenceTransform.forward) * _startRotation;
_hasRotated = true;
}
else if (GameSettings.TRANSLATE_LEFT.GetKey())
{
_startRotation = Quaternion.AngleAxis(RotationSpeed, MovingVessel.ReferenceTransform.forward) * _startRotation;
_hasRotated = true;
}
if (GameSettings.TRANSLATE_DOWN.GetKey())
{
_startRotation = Quaternion.AngleAxis(RotationSpeed, MovingVessel.ReferenceTransform.right) * _startRotation;
_hasRotated = true;
}
else if (GameSettings.TRANSLATE_UP.GetKey())
{
_startRotation = Quaternion.AngleAxis(-RotationSpeed, MovingVessel.ReferenceTransform.right) * _startRotation;
_hasRotated = true;
}
if (GameSettings.ROLL_LEFT.GetKey())
{
_startRotation = Quaternion.AngleAxis(RotationSpeed, MovingVessel.ReferenceTransform.up) * _startRotation;
_hasRotated = true;
}
else if (GameSettings.ROLL_RIGHT.GetKey())
{
_startRotation = Quaternion.AngleAxis(-RotationSpeed, MovingVessel.ReferenceTransform.up) * _startRotation;
_hasRotated = true;
}
//auto level plane
if (GameSettings.TRANSLATE_FWD.GetKey())
{
Quaternion targetRot = Quaternion.LookRotation(-_up, forward);
_startRotation = Quaternion.RotateTowards(_startRotation, targetRot, RotationSpeed * 2);
_hasRotated = true;
}
else if (GameSettings.TRANSLATE_BACK.GetKey())//auto level rocket
{
Quaternion targetRot = Quaternion.LookRotation(forward, _up);
_startRotation = Quaternion.RotateTowards(_startRotation, targetRot, RotationSpeed * 2);
_hasRotated = true;
}
if (inputting)
{
_currMoveSpeed = Mathf.Clamp(Mathf.MoveTowards(_currMoveSpeed, MoveSpeed, MoveAccel * Time.fixedDeltaTime), 0, MoveSpeed);
}
else
{
_currMoveSpeed = 0;
}
Vector3 offset = offsetDirection.normalized * _currMoveSpeed;
_currMoveVelocity = offset / Time.fixedDeltaTime;
Vector3 vSrfPt = MovingVessel.CoM - (MoveHeight * _up);
bool srfBelowWater = false;
RaycastHit ringHit;
bool surfaceDetected = CapsuleCast(out ringHit);
Vector3 finalOffset = Vector3.zero;
if (surfaceDetected)
{
if (FlightGlobals.getAltitudeAtPos(ringHit.point) < 0)
{
srfBelowWater = true;
}
Vector3 rOffset = Vector3.Project(ringHit.point - vSrfPt, _up);
Vector3 mOffset = (vSrfPt + offset) - MovingVessel.CoM;
finalOffset = rOffset + mOffset + (MoveHeight * _up);
MovingVessel.Translate(finalOffset);
}
PQS bodyPQS = MovingVessel.mainBody.pqsController;
Vector3d geoCoords = WorldPositionToGeoCoords(MovingVessel.GetWorldPos3D() + (_currMoveVelocity * Time.fixedDeltaTime), MovingVessel.mainBody);
double lat = geoCoords.x;
double lng = geoCoords.y;
Vector3d bodyUpVector = new Vector3d(1, 0, 0);
bodyUpVector = QuaternionD.AngleAxis(lat, Vector3d.forward /*around Z axis*/) * bodyUpVector;
bodyUpVector = QuaternionD.AngleAxis(lng, Vector3d.down /*around -Y axis*/) * bodyUpVector;
double srfHeight = bodyPQS.GetSurfaceHeight(bodyUpVector);
//double alt = srfHeight - bodyPQS.radius;
//double rAlt = movingVessel.radarAltitude;
//double tAlt = TrueAlt(movingVessel);
//double pAlt = movingVessel.pqsAltitude;
//double teralt = movingVessel.mainBody.TerrainAltitude(movingVessel.mainBody.GetLatitude(geoCoords), movingVessel.mainBody.GetLongitude(geoCoords));
//Debug.Log ("Surface height: "+movingVessel.mainBody.pqsController.GetSurfaceHeight(up));
if (!surfaceDetected || srfBelowWater)
{
Vector3 terrainPos = MovingVessel.mainBody.position + (float)srfHeight * _up;
Vector3 waterSrfPoint = FlightGlobals.currentMainBody.position + ((float)FlightGlobals.currentMainBody.Radius * _up);
if (!surfaceDetected)
{
MovingVessel.SetPosition(terrainPos + (MoveHeight * _up) + offset);
}
else
{
MovingVessel.SetPosition(waterSrfPoint + (MoveHeight * _up) + offset);
}
//update vessel situation to splashed down:
MovingVessel.UpdateLandedSplashed();
}
//fix surface rotation
Quaternion srfRotFix = Quaternion.FromToRotation(_startingUp, _up);
_currRotation = srfRotFix * _startRotation;
MovingVessel.SetRotation(_currRotation);
if (Vector3.Angle(_startingUp, _up) > 5)
{
_startRotation = _currRotation;
_startingUp = _up;
}
MovingVessel.SetWorldVelocity(Vector3d.zero);
MovingVessel.angularVelocity = Vector3.zero;
MovingVessel.angularMomentum = Vector3.zero;
}
public static bool IsEqual(QuaternionD value1, QuaternionD value2)
{
return(MyUtils.IsZero(value1.X - value2.X) && MyUtils.IsZero(value1.Y - value2.Y) && MyUtils.IsZero(value1.Z - value2.Z) && MyUtils.IsZero(value1.W - value2.W));
}
protected override void Update()
{
switch (stage)
{
case Stage.Start:
if (VSL.LandedOrSplashed || VSL.VerticalSpeed.Absolute < 5)
{
stage = Stage.Liftoff;
}
else
{
ToOrbit.StartGravityTurn();
stage = Stage.GravityTurn;
}
break;
case Stage.Liftoff:
if (ToOrbit.Liftoff())
{
break;
}
stage = Stage.GravityTurn;
break;
case Stage.GravityTurn:
update_inclination_limits();
var norm = VesselOrbit.GetOrbitNormal();
var needed_norm = Vector3d.Cross(VesselOrbit.pos, ToOrbit.Target.normalized);
var norm2norm = Math.Abs(Utils.Angle2(norm, needed_norm) - 90);
if (norm2norm > 60)
{
var ApV = VesselOrbit.getRelativePositionAtUT(VSL.Physics.UT + VesselOrbit.timeToAp);
var arcApA = AngleDelta(VesselOrbit, ApV);
var arcT = AngleDelta(VesselOrbit, ToOrbit.Target);
if (arcT > 0 && arcT < arcApA)
{
ApV.Normalize();
var chord = ApV * VesselOrbit.radius - VesselOrbit.pos;
var alpha = inclination_correction(VesselOrbit.inclination, chord.magnitude);
var axis = Vector3d.Cross(norm, ApV);
ToOrbit.Target = QuaternionD.AngleAxis(alpha, axis) * ApV * ToOrbit.TargetR;
}
else
{
var inclination = Math.Acos(needed_norm.z / needed_norm.magnitude) * Mathf.Rad2Deg;
var chord = Vector3d.Exclude(norm, ToOrbit.Target).normalized *VesselOrbit.radius - VesselOrbit.pos;
var alpha = inclination_correction(inclination, chord.magnitude);
var axis = Vector3d.Cross(norm, VesselOrbit.pos.normalized);
if (arcT < 0)
{
alpha = -alpha;
}
ToOrbit.Target = QuaternionD.AngleAxis(alpha, axis) * ToOrbit.Target;
}
}
if (ToOrbit.GravityTurn(C.Dtol))
{
break;
}
CFG.BR.OffIfOn(BearingMode.Auto);
var ApAUT = VSL.Physics.UT + VesselOrbit.timeToAp;
if (ApR > ToOrbit.MaxApR)
{
change_ApR(ApAUT);
}
else
{
circularize(ApAUT);
}
break;
case Stage.ChangeApA:
TmpStatus("Achieving target apoapsis...");
if (CFG.AP1[Autopilot1.Maneuver])
{
break;
}
circularize(VSL.Physics.UT + VesselOrbit.timeToAp);
stage = Stage.Circularize;
break;
case Stage.Circularize:
TmpStatus("Circularization...");
if (CFG.AP1[Autopilot1.Maneuver])
{
break;
}
Disable();
ClearStatus();
break;
}
}
public void RandomizeNear(double centerLatitude, double centerLongitude, string celestialName, double searchRadius, bool waterAllowed = true)
{
CelestialBody myPlanet = null;
foreach (CelestialBody body in FlightGlobals.Bodies)
{
if (body.GetName() == celestialName)
{
myPlanet = body;
}
}
if (myPlanet != null)
{
if (myPlanet.ocean && !waterAllowed)
{
if (myPlanet.pqsController != null)
{
while (true)
{
double distancePerDegree = (myPlanet.Radius * 2 * UnityEngine.Mathf.PI) / 360.0;
double lng_min = centerLongitude - searchRadius / UnityEngine.Mathf.Abs(UnityEngine.Mathf.Cos(UnityEngine.Mathf.Deg2Rad * (float)centerLatitude) * (float)distancePerDegree);
double lng_max = centerLongitude + searchRadius / UnityEngine.Mathf.Abs(UnityEngine.Mathf.Cos(UnityEngine.Mathf.Deg2Rad * (float)centerLatitude) * (float)distancePerDegree);
double lat_min = centerLatitude - (searchRadius / distancePerDegree);
double lat_max = centerLatitude + (searchRadius / distancePerDegree);
latitude = UnityEngine.Random.Range((float)lat_min, (float)lat_max);
longitude = UnityEngine.Random.Range((float)lng_min, (float)lng_max);
Vector3d pqsRadialVector = QuaternionD.AngleAxis(longitude, Vector3d.down) * QuaternionD.AngleAxis(latitude, Vector3d.forward) * Vector3d.right;
double chosenHeight = myPlanet.pqsController.GetSurfaceHeight(pqsRadialVector) - myPlanet.pqsController.radius;
if (chosenHeight < 0)
{
continue;
}
else
{
break;
}
}
}
}
else
{
double distancePerDegree = (myPlanet.Radius * 2 * UnityEngine.Mathf.PI) / 360.0;
double lng_min = centerLongitude - searchRadius / UnityEngine.Mathf.Abs(UnityEngine.Mathf.Cos(UnityEngine.Mathf.Deg2Rad * (float)centerLatitude) * (float)distancePerDegree);
double lng_max = centerLongitude + searchRadius / UnityEngine.Mathf.Abs(UnityEngine.Mathf.Cos(UnityEngine.Mathf.Deg2Rad * (float)centerLatitude) * (float)distancePerDegree);
double lat_min = centerLatitude - (searchRadius / distancePerDegree);
double lat_max = centerLatitude + (searchRadius / distancePerDegree);
latitude = UnityEngine.Random.Range((float)lat_min, (float)lat_max);
longitude = UnityEngine.Random.Range((float)lng_min, (float)lng_max);
}
}
}
internal void UpdatePos(Vector3 WorldPos, Matrix4x4 World2Planet, QuaternionD rotation, QuaternionD detailRotation, Matrix4x4 mainRotationMatrix, Matrix4x4 detailRotationMatrix)
{
if (HighLogic.LoadedScene == GameScenes.FLIGHT || HighLogic.LoadedScene == GameScenes.SPACECENTER)
{
Matrix4x4 rotationMatrix = mainRotationMatrix * World2Planet;
ParticleMaterial.SetMatrix(ShaderProperties.MAIN_ROTATION_PROPERTY, rotationMatrix);
ParticleMaterial.SetMatrix(ShaderProperties.DETAIL_ROTATION_PROPERTY, detailRotationMatrix);
if (followDetail)
{
rotationMatrix = detailRotationMatrix * mainRotationMatrix * World2Planet;
volumeHolder.transform.localRotation = rotation * detailRotation;
ParticleMaterial.SetMatrix(ShaderProperties._PosRotation_Property, rotationMatrix);
}
else
{
volumeHolder.transform.localRotation = rotation;
ParticleMaterial.SetMatrix(ShaderProperties._PosRotation_Property, rotationMatrix);
}
Vector3 intendedPoint = volumeHolder.transform.InverseTransformPoint(WorldPos);
intendedPoint.Normalize();
volumeManager.Update(intendedPoint);
double ut = Planetarium.GetUniversalTime();
double particleRotation = (ut * rotationSpeed);
particleRotation -= (int)particleRotation;
ParticleMaterial.SetFloat(ShaderProperties.ROTATION_PROPERTY, (float)particleRotation);
}
}
public void RandomizePosition(bool waterAllowed = true)
{
CelestialBody myPlanet = null;
foreach (CelestialBody body in FlightGlobals.Bodies)
{
if (body.GetName() == celestialName)
{
myPlanet = body;
}
}
if (myPlanet != null)
{
if (myPlanet.ocean && !waterAllowed)
{
if (myPlanet.pqsController != null)
{
while (true)
{
double rand = UnityEngine.Random.value;
rand = 1.0 - (rand * 2);
latitude = Math.Asin(rand) * UnityEngine.Mathf.Rad2Deg;
longitude = UnityEngine.Random.value * 360 - 180;
Vector3d pqsRadialVector = QuaternionD.AngleAxis(longitude, Vector3d.down) * QuaternionD.AngleAxis(latitude, Vector3d.forward) * Vector3d.right;
double chosenHeight = myPlanet.pqsController.GetSurfaceHeight(pqsRadialVector) - myPlanet.pqsController.radius;
if (chosenHeight < 0)
{
continue;
}
else
{
break;
}
}
}
}
else
{
double rand = UnityEngine.Random.value;
rand = 1.0 - (rand * 2);
latitude = Math.Asin(rand) * UnityEngine.Mathf.Rad2Deg;
longitude = UnityEngine.Random.value * 360 - 180;
}
}
}
public void Init(Orbit _initialOrbit, double _UT, SimulatedVessel _vessel, SimCurves _simcurves, IDescentSpeedPolicy _descentSpeedPolicy, double _decelEndAltitudeASL, double _maxThrustAccel, double _parachuteSemiDeployMultiplier, double _probableLandingSiteASL, bool _multiplierHasError, double _dt, double _min_dt, double _maxOrbits, bool _noSKiptoFreefall)
{
// Store all the input values as they were given
input_initialOrbit = _initialOrbit;
input_UT = _UT;
vessel = _vessel;
input_descentSpeedPolicy = _descentSpeedPolicy;
input_decelEndAltitudeASL = _decelEndAltitudeASL;
input_maxThrustAccel = _maxThrustAccel;
input_parachuteSemiDeployMultiplier = _parachuteSemiDeployMultiplier;
input_probableLandingSiteASL = _probableLandingSiteASL;
input_multiplierHasError = _multiplierHasError;
input_dt = _dt;
// the vessel attitude relative to the surface vel. Fixed for now
attitude = Quaternion.Euler(180,0,0);
min_dt = _min_dt;
max_dt = _dt;
dt = max_dt;
steps = 0;
maxOrbits = _maxOrbits;
noSKiptoFreefall = _noSKiptoFreefall;
// Get a copy of the original orbit, to be more thread safe
//initialOrbit = new Orbit();
initialOrbit.UpdateFromOrbitAtUT(_initialOrbit, _UT, _initialOrbit.referenceBody);
CelestialBody body = _initialOrbit.referenceBody;
bodyHasAtmosphere = body.atmosphere;
bodyRadius = body.Radius;
gravParameter = body.gravParameter;
this.parachuteSemiDeployMultiplier = _parachuteSemiDeployMultiplier;
this.multiplierHasError = _multiplierHasError;
bodyAngularVelocity = body.angularVelocity;
this.descentSpeedPolicy = _descentSpeedPolicy;
decelRadius = bodyRadius + _decelEndAltitudeASL;
aerobrakedRadius = bodyRadius + body.RealMaxAtmosphereAltitude();
mainBody = body;
this.maxThrustAccel = _maxThrustAccel;
this.probableLandingSiteASL = _probableLandingSiteASL;
this.probableLandingSiteRadius = _probableLandingSiteASL + bodyRadius;
referenceFrame.UpdateAtCurrentTime(_initialOrbit.referenceBody);
orbitReenters = OrbitReenters(_initialOrbit);
startX = initialOrbit.SwappedRelativePositionAtUT(startUT);
// This calls some Unity function so it should be done outside the thread
if (orbitReenters)
{
startUT = _UT;
t = startUT;
AdvanceToFreefallEnd(initialOrbit);
}
maxDragGees = 0;
deltaVExpended = 0;
trajectory = ListPool<AbsoluteVector>.Instance.Borrow();
simCurves = _simcurves;
once = true;
}
private float GetTimeToImpact()
{
Vessel vessel = FlightGlobals.ActiveVessel;
if (vessel == null)
{
return(NO_IMPACT_TIME);
}
if (FlightGlobals.ActiveVessel.mainBody.pqsController == null)
{
return(NO_IMPACT_TIME);
}
if (!IsOn())
{
return(NO_IMPACT_TIME);
}
double impactTime = 0;
double impactAltitude = 0;
double e = FlightGlobals.ActiveVessel.orbit.eccentricity;
//get current position direction vector
Vector3d currentpos = this.RadiusDirection(FlightGlobals.ActiveVessel.orbit.trueAnomaly);
//calculate longitude in inertial reference frame from that
double currentirflong = 180 * Math.Atan2(currentpos.x, currentpos.y) / Math.PI;
//experimentally determined; even for very flat trajectories, the errors go into the sub-millimeter area after 5 iterations or so
const int impactiterations = 6;
//do a few iterations of impact site calculations
for (var i = 0; i < impactiterations; i++)
{
if (FlightGlobals.ActiveVessel.orbit.PeA >= impactAltitude)
{
//periapsis must be lower than impact alt
return(NO_IMPACT_TIME);
}
if ((FlightGlobals.ActiveVessel.orbit.eccentricity < 1) && (FlightGlobals.ActiveVessel.orbit.ApA <= impactAltitude))
{
//apoapsis must be higher than impact alt
return(NO_IMPACT_TIME);
}
if ((FlightGlobals.ActiveVessel.orbit.eccentricity >= 1) && (FlightGlobals.ActiveVessel.orbit.timeToPe <= 0))
{
//if currently escaping, we still need to be before periapsis
return(NO_IMPACT_TIME);
}
double impacttheta = 0;
if (e > 0)
{
//in this step, we are using the calculated impact altitude of the last step, to refine the impact site position
impacttheta = -180 * Math.Acos((FlightGlobals.ActiveVessel.orbit.PeR * (1 + e) / (FlightGlobals.ActiveVessel.mainBody.Radius + impactAltitude) - 1) / e) / Math.PI;
}
//calculate time to impact
impactTime = FlightGlobals.ActiveVessel.orbit.timeToPe - this.TimeToPeriapsis(impacttheta);
//calculate position vector of impact site
Vector3d impactpos = this.RadiusDirection(impacttheta);
//calculate longitude of impact site in inertial reference frame
double impactirflong = 180 * Math.Atan2(impactpos.x, impactpos.y) / Math.PI;
double deltairflong = impactirflong - currentirflong;
//get body rotation until impact
double bodyrot = 360 * impactTime / FlightGlobals.ActiveVessel.mainBody.rotationPeriod;
//get current longitude in body coordinates
double currentlong = FlightGlobals.ActiveVessel.longitude;
//finally, calculate the impact longitude in body coordinates
double impactLongitude = this.NormAngle(currentlong - deltairflong - bodyrot);
//calculate impact latitude from impact position
double impactLatitude = 180 * Math.Asin(impactpos.z / impactpos.magnitude) / Math.PI;
//calculate the actual altitude of the impact site
//altitude for long/lat code stolen from some ISA MapSat forum post; who knows why this works, but it seems to.
Vector3d rad = QuaternionD.AngleAxis(impactLongitude, Vector3d.down) * QuaternionD.AngleAxis(impactLatitude, Vector3d.forward) * Vector3d.right;
impactAltitude = FlightGlobals.ActiveVessel.mainBody.pqsController.GetSurfaceHeight(rad) - FlightGlobals.ActiveVessel.mainBody.pqsController.radius;
if ((impactAltitude < 0) && FlightGlobals.ActiveVessel.mainBody.ocean)
{
//When hitting sea level.
impactAltitude = 0;
}
}
return((float)impactTime);
}
