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 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 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 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); }
/// <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); }