private Vector3d GetDescendingNode(Orbit targetOrbit, Orbit originOrbit)
{
return(Vector3d.Cross(originOrbit.GetOrbitNormal(), targetOrbit.GetOrbitNormal()));
}
public Quaternion attitudeGetReferenceRotation(AttitudeReference reference)
{
Vector3 fwd, up;
Quaternion rotRef = Quaternion.identity;
if (core.target.Target == null && (reference == AttitudeReference.TARGET || reference == AttitudeReference.TARGET_ORIENTATION || reference == AttitudeReference.RELATIVE_VELOCITY))
{
attitudeDeactivate();
return(rotRef);
}
if ((reference == AttitudeReference.MANEUVER_NODE) && (vessel.patchedConicSolver.maneuverNodes.Count == 0))
{
attitudeDeactivate();
return(rotRef);
}
switch (reference)
{
case AttitudeReference.ORBIT:
rotRef = Quaternion.LookRotation(vesselState.orbitalVelocity.normalized, vesselState.up);
break;
case AttitudeReference.ORBIT_HORIZONTAL:
rotRef = Quaternion.LookRotation(Vector3d.Exclude(vesselState.up, vesselState.orbitalVelocity.normalized), vesselState.up);
break;
case AttitudeReference.SURFACE_NORTH:
rotRef = vesselState.rotationSurface;
break;
case AttitudeReference.SURFACE_VELOCITY:
rotRef = Quaternion.LookRotation(vesselState.surfaceVelocity.normalized, vesselState.up);
break;
case AttitudeReference.TARGET:
fwd = (core.target.Position - vessel.GetTransform().position).normalized;
up = Vector3d.Cross(fwd, vesselState.normalPlus);
Vector3.OrthoNormalize(ref fwd, ref up);
rotRef = Quaternion.LookRotation(fwd, up);
break;
case AttitudeReference.RELATIVE_VELOCITY:
fwd = core.target.RelativeVelocity.normalized;
up = Vector3d.Cross(fwd, vesselState.normalPlus);
Vector3.OrthoNormalize(ref fwd, ref up);
rotRef = Quaternion.LookRotation(fwd, up);
break;
case AttitudeReference.TARGET_ORIENTATION:
Transform targetTransform = core.target.Transform;
if (core.target.CanAlign)
{
rotRef = Quaternion.LookRotation(targetTransform.forward, targetTransform.up);
}
else
{
rotRef = Quaternion.LookRotation(targetTransform.up, targetTransform.right);
}
break;
case AttitudeReference.MANEUVER_NODE:
fwd = vessel.patchedConicSolver.maneuverNodes[0].GetBurnVector(orbit);
up = Vector3d.Cross(fwd, vesselState.normalPlus);
Vector3.OrthoNormalize(ref fwd, ref up);
rotRef = Quaternion.LookRotation(fwd, up);
break;
case AttitudeReference.SUN:
Orbit baseOrbit = vessel.mainBody == Planetarium.fetch.Sun ? vessel.orbit : orbit.TopParentOrbit();
up = vesselState.CoM - Planetarium.fetch.Sun.transform.position;
fwd = Vector3d.Cross(-baseOrbit.GetOrbitNormal().Reorder(132).normalized, up);
rotRef = Quaternion.LookRotation(fwd, up);
break;
case AttitudeReference.SURFACE_HORIZONTAL:
rotRef = Quaternion.LookRotation(Vector3d.Exclude(vesselState.up, vesselState.surfaceVelocity.normalized), vesselState.up);
break;
}
return(rotRef);
}
static ManeuverParameters ComputeEjectionManeuver(Vector3d exit_velocity, Orbit initial_orbit, double UT_0)
{
double GM = initial_orbit.referenceBody.gravParameter;
double C3 = exit_velocity.sqrMagnitude;
double sma = -GM / C3;
double Rpe = initial_orbit.semiMajorAxis;
double ecc = 1 - Rpe / sma;
double theta = Math.Acos(-1 / ecc);
Vector3d intersect_1;
Vector3d intersect_2;
// intersect_1 is the optimal position on the orbit assuming the orbit is circular and the sphere of influence is infinite
IntersectConePlane(exit_velocity, theta, initial_orbit.h, out intersect_1, out intersect_2);
Vector3d eccvec = initial_orbit.GetEccVector();
double true_anomaly = AngleAboutAxis(eccvec, intersect_1, initial_orbit.h);
// GetMeanAnomalyAtTrueAnomaly expects degrees and returns radians
double mean_anomaly = initial_orbit.GetMeanAnomalyAtTrueAnomaly(true_anomaly * 180 / Math.PI);
double delta_mean_anomaly = MuUtils.ClampRadiansPi(mean_anomaly - initial_orbit.MeanAnomalyAtUT(UT_0));
double UT = UT_0 + delta_mean_anomaly / initial_orbit.MeanMotion();
Vector3d V_0 = initial_orbit.getOrbitalVelocityAtUT(UT);
Vector3d pos = initial_orbit.getRelativePositionAtUT(UT);
double final_vel = Math.Sqrt(C3 + 2 * GM / pos.magnitude);
Vector3d x = pos.normalized;
Vector3d z = Vector3d.Cross(x, exit_velocity).normalized;
Vector3d y = Vector3d.Cross(z, x);
theta = Math.PI / 2;
double dtheta = 0.001;
Orbit sample = new Orbit();
double theta_err = Double.MaxValue;
for (int iteration = 0; iteration < 50; ++iteration)
{
Vector3d V_1 = final_vel * (Math.Cos(theta) * x + Math.Sin(theta) * y);
sample.UpdateFromStateVectors(pos, V_1, initial_orbit.referenceBody, UT);
theta_err = AngleAboutAxis(exit_velocity, sample.getOrbitalVelocityAtUT(OrbitExtensions.NextTimeOfRadius(sample, UT, sample.referenceBody.sphereOfInfluence)), z);
if (double.IsNaN(theta_err))
{
return(null);
}
if (Math.Abs(theta_err) <= Math.PI / 1800)
{
return(new ManeuverParameters((V_1 - V_0).xzy, UT));
}
V_1 = final_vel * (Math.Cos(theta + dtheta) * x + Math.Sin(theta + dtheta) * y);
sample.UpdateFromStateVectors(pos, V_1, initial_orbit.referenceBody, UT);
double theta_err_2 = AngleAboutAxis(exit_velocity, sample.getOrbitalVelocityAtUT(OrbitExtensions.NextTimeOfRadius(sample, UT, sample.referenceBody.sphereOfInfluence)), z);
V_1 = final_vel * (Math.Cos(theta - dtheta) * x + Math.Sin(theta - dtheta) * y);
sample.UpdateFromStateVectors(pos, V_1, initial_orbit.referenceBody, UT);
double theta_err_3 = AngleAboutAxis(exit_velocity, sample.getOrbitalVelocityAtUT(OrbitExtensions.NextTimeOfRadius(sample, UT, sample.referenceBody.sphereOfInfluence)), z);
double derr = MuUtils.ClampRadiansPi(theta_err_2 - theta_err_3) / (2 * dtheta);
theta = MuUtils.ClampRadiansTwoPi(theta - theta_err / derr);
// if theta > pi, replace with 2pi - theta, the function ejection_angle=f(theta) is symmetrc wrt theta=pi
if (theta > Math.PI)
{
theta = 2 * Math.PI - theta;
}
}
return(null);
}
public void Generate()
{
int s = GridSize + 1;
double scale = size / GridSize;
double invSize = 1.0 / size;
Vector3d p1, p2, p3, tmp, tan;
double n;
Vector3d offset = new Vector3d(GridSize * .5, 0, GridSize * .5);
vertices = new Vector3[s * s];
normals = new Vector3[s * s];
tangents = new Vector4[s * s];
texcoord0 = new Vector4[s * s];
weights = new Color[s * s];
Vector3d?[,] vertexCache = new Vector3d?[s, s];
double?[,] noiseCache = new double?[s, s];
bool outwardNormals = type != SurfaceType.Ground;
hasVisibleVertices = false;
System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
sw.Start();
int j = 0;
// surface mesh
for (int x = 0; x < s; x++)
{
for (int z = 0; z < s; z++)
{
// get normalized cube space vertex (vertices -.5 to .5)
if (vertexCache[x, z].HasValue)
{
p1 = vertexCache[x, z].Value;
n = noiseCache[x, z].Value;
}
else
{
p1 = Vector3d.Normalize(cubePos + rotation * (scale * (new Vector3d(x, 0, z) - offset)));
p1 = p1 * GetHeight(p1, out n, ref hasVisibleVertices) - meshPos;
vertexCache[x, z] = p1;
noiseCache[x, z] = n;
}
if (z + 1 < s && vertexCache[x, z + 1].HasValue)
{
p2 = vertexCache[x, z + 1].Value;
}
else
{
p2 = Vector3d.Normalize(cubePos + rotation * (scale * (new Vector3d(x, 0, z + 1) - offset)));
p2 = p2 * GetHeight(p2, out n, ref hasVisibleVertices) - meshPos;
if (z + 1 < s)
{
vertexCache[x, z + 1] = p2;
noiseCache[x, z + 1] = n;
}
}
if (x + 1 < s && vertexCache[x + 1, z].HasValue)
{
p3 = vertexCache[x + 1, z].Value;
}
else
{
p3 = Vector3d.Normalize(cubePos + rotation * (scale * (new Vector3d(x + 1, 0, z) - offset)));
p3 = p3 * GetHeight(p3, out n, ref hasVisibleVertices) - meshPos;
if (x + 1 < s)
{
vertexCache[x + 1, z] = p3;
noiseCache[x + 1, z] = n;
}
}
tmp = (p1 + meshPos).normalized;
tan = Vector3d.Cross(planet.rotation * Vector3d.up, tmp).normalized;
vertices[j] = (Vector3)(p1 * invSize);
normals[j] = outwardNormals ? (Vector3)tmp : Vector3.Cross((Vector3)Vector3d.Normalize(p2 - p1), (Vector3)Vector3d.Normalize(p3 - p1));
texcoord0[j] = new Vector4((float)tmp.x, (float)tmp.y, (float)tmp.z, (float)size);
tangents[j] = (Vector3)tan;
weights[j] = planet.heightWeightGradient.Evaluate((float)n);
j++;
}
}
sw.Stop();
lock (sfctimes) sfctimes.Add(sw.Elapsed.TotalMilliseconds);
sw.Reset();
// grass, trees, surface objects
if (type == SurfaceType.Ground)
{
// trees
if (planet.hasTrees &&
vertexResolution >= planet.treeVertexResolution && vertexResolution * .5 < planet.treeVertexResolution)
{
sw.Start();
trees = new TreeInstance[1];
System.Random rand = new System.Random(planet.seed ^ (meshPos.GetHashCode()));
GameObject tree = planet.treePrefabs[0];
bool q = false;
double h;
float x, z;
List <Matrix4x4> m = new List <Matrix4x4>();
for (int i = 0; i < .01 * (size * size); i++)
{
x = (float)rand.NextDouble() * GridSize;
z = (float)rand.NextDouble() * GridSize;
p1 = (cubePos + rotation * (scale * (new Vector3d(x, 0, z) - offset)));
p1 = p1.normalized;
h = GetHeight(p1, out n, ref q);
if (h > planet.radius + planet.waterHeight)
{
p2 = p1;
p1 = p1 * h - meshPos;
m.Add(Matrix4x4.TRS(
(Vector3)p1, Quaternion.FromToRotation(Vector3.up, (Vector3)p2), Vector3.one
));
}
}
TreeInstance t = new TreeInstance()
{
instances = m.ToArray(),
tmpinstances = new Matrix4x4[m.Count],
prefab = tree
};
trees[0] = t;
sw.Stop();
lock (treetimes) treetimes.Add(sw.Elapsed.TotalMilliseconds);
sw.Reset();
}
// grass
// TODO: grass generation is atrociously slow
if (planet.hasGrass &&
vertexResolution >= planet.grassVertexResolution && vertexResolution * .5 < planet.grassVertexResolution)
{
sw.Start();
System.Random rand = new System.Random(~(planet.seed ^ (meshPos.GetHashCode())));
int count = (int)(planet.grassDensity * size * size);
List <int> ginds = new List <int>(count);
gverts = new List <Vector3>(count);
gnorms = new List <Vector3>(count);
guvs = new List <Vector4>(count);
double h2 = (planet.radius + planet.waterHeight) * (planet.radius + planet.waterHeight);
float x, z, xr, zr;
Vector3d pos;
for (int i = 0; i < count; i++)
{
x = (float)(rand.NextDouble() * GridSize);
z = (float)(rand.NextDouble() * GridSize);
xr = x - (int)x;
zr = z - (int)z;
// interpolate height from mesh
pos = (Vector3d)bilinear(
vertices[(int)x * s + (int)z],
vertices[(int)x * s + (int)z + 1],
vertices[(int)(x + 1) * s + (int)z],
vertices[(int)(x + 1) * s + (int)z + 1],
xr, zr) * size;
double h = ((pos + meshPos).magnitude - planet.radius) / planet.terrainHeight;
if (planet.heightWeightGradient.Evaluate((float)h).g > .5f)
{
gverts.Add((Vector3)pos);
gnorms.Add(bilinear(
normals[(int)x * s + (int)z],
normals[(int)x * s + (int)z + 1],
normals[(int)(x + 1) * s + (int)z],
normals[(int)(x + 1) * s + (int)z + 1],
xr, zr).normalized);
guvs.Add(new Vector4((float)rand.NextDouble(), (float)rand.NextDouble(), 1f, (float)rand.NextDouble()));
ginds.Add(gverts.Count - 1);
}
}
grassindices = ginds.ToArray();
sw.Stop();
lock (grasstimes) grasstimes.Add(sw.Elapsed.TotalMilliseconds);
sw.Reset();
}
}
generated = true;
}
/// <summary>
/// Builds the piston joint.
/// </summary>
/// <returns>The piston joint.</returns>
/// <param name="simulationObjs">Simulation objects.</param>
public override List <JacobianConstraint> BuildJacobian()
{
var pistonConstraints = new List <JacobianConstraint> ();
IShape simulationObjectA = ShapeA;
IShape simulationObjectB = ShapeB;
#region Init Linear
Vector3d sliderAxis = GetSliderAxis();
Vector3d t1 = GeometryUtils.GetPerpendicularVector(sliderAxis).Normalize();
Vector3d t2 = sliderAxis.Cross(t1).Normalize();
Vector3d r1 = simulationObjectA.RotationMatrix *
StartErrorAxis1;
Vector3d r2 = simulationObjectB.RotationMatrix *
StartErrorAxis2;
Vector3d p1 = simulationObjectA.Position + r1;
Vector3d p2 = simulationObjectB.Position + r2;
Vector3d linearError = p2 - p1;
#endregion
Vector3d angularError = sliderAxis.Cross(
(simulationObjectB.RotationMatrix * PistonAxis));
#region Jacobian Constraint
double errorReduction = ErrorReductionParam;
double springCoefficient = SpringCoefficient;
#region Base Constraints
ConstraintType constraintType = ConstraintType.Joint;
if (SpringCoefficient > 0)
{
constraintType = ConstraintType.SoftJoint;
}
//DOF 1
double angularLimit = errorReduction *
t1.Dot(angularError);
pistonConstraints.Add(
JacobianCommon.GetDOF(
t1,
-1.0 * t1,
simulationObjectA,
simulationObjectB,
0.0,
angularLimit,
springCoefficient,
0.0,
constraintType));
//DOF 2
angularLimit = errorReduction *
t2.Dot(angularError);
pistonConstraints.Add(
JacobianCommon.GetDOF(
t2,
-1.0 * t2,
simulationObjectA,
simulationObjectB,
0.0,
angularLimit,
springCoefficient,
0.0,
constraintType));
//DOF 3
double constraintLimit = errorReduction * t1.Dot(linearError);
pistonConstraints.Add(JacobianCommon.GetDOF(
t1,
-1.0 * t1,
r1.Cross(t1),
-1.0 * r2.Cross(t1),
simulationObjectA,
simulationObjectB,
0.0,
constraintLimit,
springCoefficient,
0.0,
constraintType));
//DOF 4
constraintLimit = errorReduction * Vector3d.Dot(t2, linearError);
pistonConstraints.Add(JacobianCommon.GetDOF(
t2,
-1.0 * t2,
r1.Cross(t2),
-1.0 * r2.Cross(t2),
simulationObjectA,
simulationObjectB,
0.0,
constraintLimit,
springCoefficient,
0.0,
constraintType));
#endregion
#region Limit Constraints
pistonConstraints.AddRange(GetLinearLimit(
simulationObjectA,
simulationObjectB,
sliderAxis,
r1,
r2,
errorReduction));
pistonConstraints.AddRange(GetAnguarLimit(
simulationObjectA,
simulationObjectB,
sliderAxis,
errorReduction));
#endregion
#region Motor Constraint
pistonConstraints.AddRange(GetMotorConstraint(
simulationObjectA,
simulationObjectB,
sliderAxis));
#endregion
#endregion
return(pistonConstraints);
}
// Residual torque for given force application point.
public Vector3d TorqueAt(Vector3d origin)
{
return(totalZeroOriginTorque - Vector3d.Cross(origin, totalForce));
}
public void AddGlyphPoints(List <PositionTexture> pointList, Vector2d size, Rectangle position, Rectangle uv)
{
PositionTexture[] points = new PositionTexture[6];
for (int i = 0; i < 6; i++)
{
points[i] = new PositionTexture();
}
Vector3d left = Vector3d.Cross(center, up);
Vector3d right = Vector3d.Cross(up, center);
left.Normalize();
right.Normalize();
up.Normalize();
Vector3d upTan = Vector3d.Cross(center, right);
upTan.Normalize();
if (alignment == Alignment.Center)
{
left.Multiply(width - position.Left * 2);
right.Multiply(width - ((width * 2) - position.Right * 2));
}
else if (alignment == Alignment.Left)
{
left.Multiply(-position.Left * 2);
right.Multiply(position.Right * 2);
}
Vector3d top = upTan.Copy();
Vector3d bottom = Vector3d.SubtractVectors(Vector3d.Empty, upTan);
top.Multiply(height - position.Top * 2);
bottom.Multiply(height - ((height * 2) - position.Bottom * 2));
Vector3d ul = center.Copy();
ul.Add(top);
if (sky)
{
ul.Add(left);
}
else
{
ul.Subtract(left);
}
Vector3d ur = center.Copy();
ur.Add(top);
if (sky)
{
ur.Add(right);
}
else
{
ur.Subtract(right);
}
Vector3d ll = center.Copy();
if (sky)
{
ll.Add(left);
}
else
{
ll.Subtract(left);
}
ll.Add(bottom);
Vector3d lr = center.Copy();
if (sky)
{
lr.Add(right);
}
else
{
lr.Subtract(right);
}
lr.Add(bottom);
points[0].Position = ul.Copy();
points[0].Tu = uv.Left;
points[0].Tv = uv.Top;
// points[0].Color = Color;
points[2].Tu = uv.Left;
points[2].Tv = uv.Bottom;
points[2].Position = ll.Copy();
// points[2].Color = Color;
points[1].Tu = uv.Right;
points[1].Tv = uv.Top;
points[1].Position = ur.Copy();
// points[1].Color = Color;
points[3].Tu = uv.Right;
points[3].Tv = uv.Bottom;
points[3].Position = lr.Copy();
// points[3].Color = Color;
points[5].Tu = uv.Right;
points[5].Tv = uv.Top;
points[5].Position = ur.Copy();
// points[5].Color = Color;
points[4].Tu = uv.Left;
points[4].Tv = uv.Bottom;
points[4].Position = ll.Copy();
// points[4].Color = Color;
if (Rotation != 0 || Tilt != 0 || Bank != 0)
{
if (!matInit)
{
Matrix3d lookAt = Matrix3d.LookAtLH(center, new Vector3d(), up);
Matrix3d lookAtInv = lookAt.Clone();
lookAtInv.Invert();
rtbMat = Matrix3d.MultiplyMatrix(Matrix3d.MultiplyMatrix(Matrix3d.MultiplyMatrix(Matrix3d.MultiplyMatrix(lookAt, Matrix3d.RotationZ(-Rotation / 180 * Math.PI)), Matrix3d.RotationX(-Tilt / 180 * Math.PI)), Matrix3d.RotationY(-Bank / 180 * Math.PI)), lookAtInv);
//todo make this true after debug
matInit = true;
}
for (int i = 0; i < 6; i++)
{
points[i].Position = Vector3d.TransformCoordinate(points[i].Position, rtbMat);
}
}
foreach (PositionTexture pnt in points)
{
pointList.Add(pnt);
}
}
protected override void WindowGUI(int windowID)
{
if (btNormal == null)
{
btNormal = new GUIStyle(GUI.skin.button);
btNormal.normal.textColor = btNormal.focused.textColor = Color.white;
btNormal.hover.textColor = btNormal.active.textColor = Color.yellow;
btNormal.onNormal.textColor = btNormal.onFocused.textColor = btNormal.onHover.textColor = btNormal.onActive.textColor = Color.green;
btNormal.padding = new RectOffset(8, 8, 8, 8);
btActive = new GUIStyle(btNormal);
btActive.active = btActive.onActive;
btActive.normal = btActive.onNormal;
btActive.onFocused = btActive.focused;
btActive.hover = btActive.onHover;
}
GUILayout.BeginVertical();
if (autopilot != null)
{
if (autopilot.enabled)
{
if (GUILayout.Button("Disengage autopilot"))
{
autopilot.users.Remove(this);
}
}
else
{
if (GUILayout.Button("Engage autopilot"))
{
autopilot.users.Add(this);
}
}
if (ascentPathIdx == ascentType.PVG)
{
if (GUILayout.Button("Reset Guidance (DO NOT PRESS)"))
{
core.guidance.Reset();
}
GUILayout.BeginHorizontal(); // EditorStyles.toolbar);
if (GUILayout.Button("TARG", autopilot.showTargeting ? btActive : btNormal, GUILayout.ExpandWidth(true)))
{
autopilot.showTargeting = !autopilot.showTargeting;
}
if (GUILayout.Button("GUID", autopilot.showGuidanceSettings ? btActive : btNormal, GUILayout.ExpandWidth(true)))
{
autopilot.showGuidanceSettings = !autopilot.showGuidanceSettings;
}
if (GUILayout.Button("OPTS", autopilot.showSettings ? btActive : btNormal, GUILayout.ExpandWidth(true)))
{
autopilot.showSettings = !autopilot.showSettings;
}
if (GUILayout.Button("STATUS", autopilot.showStatus ? btActive : btNormal, GUILayout.ExpandWidth(true)))
{
autopilot.showStatus = !autopilot.showStatus;
}
GUILayout.EndHorizontal();
}
else if (ascentPathIdx == ascentType.GRAVITYTURN)
{
GUILayout.BeginHorizontal(); // EditorStyles.toolbar);
if (GUILayout.Button("TARG", autopilot.showTargeting ? btActive : btNormal, GUILayout.ExpandWidth(true)))
{
autopilot.showTargeting = !autopilot.showTargeting;
}
if (GUILayout.Button("GUID", autopilot.showGuidanceSettings ? btActive : btNormal, GUILayout.ExpandWidth(true)))
{
autopilot.showGuidanceSettings = !autopilot.showGuidanceSettings;
}
if (GUILayout.Button("OPTS", autopilot.showSettings ? btActive : btNormal, GUILayout.ExpandWidth(true)))
{
autopilot.showSettings = !autopilot.showSettings;
}
GUILayout.EndHorizontal();
}
else if (ascentPathIdx == ascentType.CLASSIC)
{
GUILayout.BeginHorizontal(); // EditorStyles.toolbar);
if (GUILayout.Button("TARG", autopilot.showTargeting ? btActive : btNormal, GUILayout.ExpandWidth(true)))
{
autopilot.showTargeting = !autopilot.showTargeting;
}
if (GUILayout.Button("OPTS", autopilot.showSettings ? btActive : btNormal, GUILayout.ExpandWidth(true)))
{
autopilot.showSettings = !autopilot.showSettings;
}
GUILayout.EndHorizontal();
}
if (autopilot.showTargeting)
{
if (ascentPathIdx == ascentType.PVG)
{
GuiUtils.SimpleTextBox("Target Periapsis", autopilot.desiredOrbitAltitude, "km");
GuiUtils.SimpleTextBox("Target Apoapsis:", pvgascent.desiredApoapsis, "km");
if (pvgascent.desiredApoapsis >= 0 && pvgascent.desiredApoapsis < autopilot.desiredOrbitAltitude)
{
GUIStyle s = new GUIStyle(GUI.skin.label);
s.normal.textColor = Color.yellow;
GUILayout.Label("Ap < Pe: circularizing orbit", s);
}
if (pvgascent.desiredApoapsis < 0)
{
GUIStyle s = new GUIStyle(GUI.skin.label);
s.normal.textColor = XKCDColors.Orange;
GUILayout.Label("Hyperbolic target orbit (neg Ap)", s);
}
}
else
{
GuiUtils.SimpleTextBox("Orbit altitude", autopilot.desiredOrbitAltitude, "km");
}
GUIStyle si = new GUIStyle(GUI.skin.label);
if (Math.Abs(desiredInclination) < Math.Abs(vesselState.latitude) - 2.001)
{
si.onHover.textColor = si.onNormal.textColor = si.normal.textColor = XKCDColors.Orange;
}
GUILayout.BeginHorizontal();
GUILayout.Label("Orbit inc.", si, GUILayout.ExpandWidth(true));
desiredInclination.text = GUILayout.TextField(desiredInclination.text, GUILayout.ExpandWidth(true), GUILayout.Width(100));
GUILayout.Label("º", GUILayout.ExpandWidth(false));
if (GUILayout.Button("Current"))
{
desiredInclination.val = vesselState.latitude;
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
if (Math.Abs(desiredInclination) < Math.Abs(vesselState.latitude) - 2.001)
{
GUILayout.Label(String.Format("inc {0:F1}º below current latitude", Math.Abs(vesselState.latitude) - Math.Abs(desiredInclination)), si);
}
GUILayout.EndHorizontal();
autopilot.desiredInclination = desiredInclination;
}
if (autopilot.showGuidanceSettings)
{
if (ascentPathIdx == ascentType.GRAVITYTURN)
{
GUILayout.BeginVertical();
GuiUtils.SimpleTextBox("Turn start altitude:", gtascent.turnStartAltitude, "km");
GuiUtils.SimpleTextBox("Turn start velocity:", gtascent.turnStartVelocity, "m/s");
GuiUtils.SimpleTextBox("Turn start pitch:", gtascent.turnStartPitch, "deg");
GuiUtils.SimpleTextBox("Intermediate altitude:", gtascent.intermediateAltitude, "km");
GuiUtils.SimpleTextBox("Hold AP Time:", gtascent.holdAPTime, "s");
GUILayout.EndVertical();
}
else if (ascentPathIdx == ascentType.PVG)
{
GUILayout.BeginVertical();
GuiUtils.SimpleTextBox("Booster Pitch start:", pvgascent.pitchStartVelocity, "m/s");
GuiUtils.SimpleTextBox("Booster Pitch rate:", pvgascent.pitchRate, "°/s");
GuiUtils.SimpleTextBox("Guidance Interval:", core.guidance.pvgInterval, "s");
if (core.guidance.pvgInterval < 1 || core.guidance.pvgInterval > 30)
{
GUIStyle s = new GUIStyle(GUI.skin.label);
s.normal.textColor = Color.yellow;
GUILayout.Label("Guidance intervals are limited to between 1s and 30s", s);
}
GuiUtils.SimpleTextBox("Qα limit", autopilot.limitQa, "Pa-rad");
if (autopilot.limitQa < 100 || autopilot.limitQa > 4000)
{
GUIStyle s = new GUIStyle(GUI.skin.label);
s.normal.textColor = Color.yellow;
if (autopilot.limitQa < 100)
{
GUILayout.Label("Qα limit cannot be set to lower than 100 Pa-rad", s);
}
else if (autopilot.limitQa > 10000)
{
GUILayout.Label("Qα limit cannot be set to higher than 10000 Pa-rad", s);
}
else
{
GUILayout.Label("Qα limit is recommended to be 1000 to 4000 Pa-rad", s);
}
}
pvgascent.omitCoast = GUILayout.Toggle(pvgascent.omitCoast, "Omit Coast");
GUILayout.EndVertical();
}
}
autopilot.limitQaEnabled = (ascentPathIdx == ascentType.PVG); // this is mandatory for PVG
if (autopilot.showSettings)
{
ToggleAscentNavballGuidanceInfoItem();
if (ascentPathIdx != ascentType.PVG)
{
core.thrust.LimitToPreventOverheatsInfoItem();
//core.thrust.LimitToTerminalVelocityInfoItem();
core.thrust.LimitToMaxDynamicPressureInfoItem();
core.thrust.LimitAccelerationInfoItem();
core.thrust.LimitThrottleInfoItem();
core.thrust.LimiterMinThrottleInfoItem();
core.thrust.LimitElectricInfoItem();
}
else
{
core.thrust.LimitToPreventOverheatsInfoItem();
//core.thrust.LimitToTerminalVelocityInfoItem();
core.thrust.LimitToMaxDynamicPressureInfoItem();
//core.thrust.LimitAccelerationInfoItem();
//core.thrust.LimitThrottleInfoItem();
core.thrust.LimiterMinThrottleInfoItem();
//core.thrust.LimitElectricInfoItem();
GUILayout.Label("FIXME: g-limiter is down for maintenance");
core.thrust.limitAcceleration = false;
core.thrust.limitThrottle = false;
core.thrust.limitToTerminalVelocity = false;
core.thrust.electricThrottle = false;
}
GUILayout.BeginHorizontal();
autopilot.forceRoll = GUILayout.Toggle(autopilot.forceRoll, "Force Roll");
if (autopilot.forceRoll)
{
GuiUtils.SimpleTextBox("climb", autopilot.verticalRoll, "º", 30f);
GuiUtils.SimpleTextBox("turn", autopilot.turnRoll, "º", 30f);
}
GUILayout.EndHorizontal();
if (ascentPathIdx != ascentType.PVG)
{
GUILayout.BeginHorizontal();
GUIStyle s = new GUIStyle(GUI.skin.toggle);
if (autopilot.limitingAoA)
{
s.onHover.textColor = s.onNormal.textColor = Color.green;
}
autopilot.limitAoA = GUILayout.Toggle(autopilot.limitAoA, "Limit AoA to", s, GUILayout.ExpandWidth(true));
autopilot.maxAoA.text = GUILayout.TextField(autopilot.maxAoA.text, GUILayout.Width(30));
GUILayout.Label("º (" + autopilot.currentMaxAoA.ToString("F1") + "°)", GUILayout.ExpandWidth(true));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Space(25);
if (autopilot.limitAoA)
{
GUIStyle sl = new GUIStyle(GUI.skin.label);
if (autopilot.limitingAoA && vesselState.dynamicPressure < autopilot.aoALimitFadeoutPressure)
{
sl.normal.textColor = sl.hover.textColor = Color.green;
}
GuiUtils.SimpleTextBox("Dynamic Pressure Fadeout", autopilot.aoALimitFadeoutPressure, "Pa", 50, sl);
}
GUILayout.EndHorizontal();
autopilot.limitQaEnabled = false; // this is only for PVG
}
if (ascentPathIdx == ascentType.CLASSIC)
{
// corrective steering only applies to Classic
GUILayout.BeginHorizontal();
autopilot.correctiveSteering = GUILayout.Toggle(autopilot.correctiveSteering, "Corrective steering", GUILayout.ExpandWidth(false));
if (autopilot.correctiveSteering)
{
GUILayout.Label("Gain", GUILayout.ExpandWidth(false));
autopilot.correctiveSteeringGain.text = GUILayout.TextField(autopilot.correctiveSteeringGain.text, GUILayout.Width(40));
}
GUILayout.EndHorizontal();
}
autopilot.autostage = GUILayout.Toggle(autopilot.autostage, "Autostage");
if (autopilot.autostage)
{
core.staging.AutostageSettingsInfoItem();
}
autopilot.autodeploySolarPanels = GUILayout.Toggle(autopilot.autodeploySolarPanels,
"Auto-deploy solar panels");
autopilot.autoDeployAntennas = GUILayout.Toggle(autopilot.autoDeployAntennas,
"Auto-deploy antennas");
GUILayout.BeginHorizontal();
core.node.autowarp = GUILayout.Toggle(core.node.autowarp, "Auto-warp");
if (ascentPathIdx != ascentType.PVG)
{
autopilot.skipCircularization = GUILayout.Toggle(autopilot.skipCircularization, "Skip Circularization");
}
else
{
// skipCircularization is always true for Optimizer
autopilot.skipCircularization = true;
}
GUILayout.EndHorizontal();
}
if (autopilot.showStatus)
{
if (ascentPathIdx == ascentType.PVG)
{
if (core.guidance.solution != null)
{
for (int i = core.guidance.solution.num_segments; i > 0; i--)
{
GUILayout.Label(String.Format("{0}: {1}", i, core.guidance.solution.ArcString(vesselState.time, i - 1)));
}
}
GUILayout.BeginHorizontal();
GUILayout.Label(String.Format("vgo: {0:F1}", core.guidance.vgo), GUILayout.Width(100));
GUILayout.Label(String.Format("heading: {0:F1}", core.guidance.heading), GUILayout.Width(100));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label(String.Format("tgo: {0:F3}", core.guidance.tgo), GUILayout.Width(100));
GUILayout.Label(String.Format("pitch: {0:F1}", core.guidance.pitch), GUILayout.Width(100));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUIStyle si = new GUIStyle(GUI.skin.label);
if (core.guidance.isStable())
{
si.onHover.textColor = si.onNormal.textColor = si.normal.textColor = XKCDColors.Green;
}
else if (core.guidance.isInitializing() || core.guidance.status == PVGStatus.FINISHED)
{
si.onHover.textColor = si.onNormal.textColor = si.normal.textColor = XKCDColors.Orange;
}
else
{
si.onHover.textColor = si.onNormal.textColor = si.normal.textColor = XKCDColors.Red;
}
GUILayout.Label("Guidance Status: " + core.guidance.status, si);
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("converges: " + core.guidance.successful_converges, GUILayout.Width(100));
GUILayout.Label("status: " + core.guidance.last_lm_status, GUILayout.Width(100));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("n: " + core.guidance.last_lm_iteration_count + "(" + core.guidance.max_lm_iteration_count + ")", GUILayout.Width(100));
GUILayout.Label("staleness: " + GuiUtils.TimeToDHMS(core.guidance.staleness));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label(String.Format("znorm: {0:G5}", core.guidance.last_znorm));
GUILayout.EndHorizontal();
if (core.guidance.last_failure_cause != null)
{
GUIStyle s = new GUIStyle(GUI.skin.label);
s.normal.textColor = Color.red;
GUILayout.BeginHorizontal();
GUILayout.Label("LAST FAILURE: " + core.guidance.last_failure_cause, s);
GUILayout.EndHorizontal();
}
if (vessel.situation != Vessel.Situations.LANDED && vessel.situation != Vessel.Situations.PRELAUNCH && vessel.situation != Vessel.Situations.SPLASHED)
{
double m0 = atmoStats[vessel.currentStage].startMass;
double thrust = atmoStats[vessel.currentStage].startThrust;
if (Math.Abs(vesselState.mass - m0) / m0 > 0.01)
{
GUIStyle s = new GUIStyle(GUI.skin.label);
s.normal.textColor = Color.yellow;
GUILayout.BeginHorizontal();
GUILayout.Label(String.Format("MASS IS OFF BY {0:F1}%", (vesselState.mass - m0) / m0 * 100.0), s);
GUILayout.EndHorizontal();
}
if (Math.Abs(vesselState.thrustCurrent - thrust) / thrust > 0.01)
{
GUIStyle s = new GUIStyle(GUI.skin.label);
s.normal.textColor = Color.yellow;
GUILayout.BeginHorizontal();
GUILayout.Label(String.Format("THRUST IS OFF BY {0:F1}%", (vesselState.thrustCurrent - thrust) / thrust * 100.0), s);
GUILayout.EndHorizontal();
}
}
}
}
if (vessel.LandedOrSplashed)
{
if (core.target.NormalTargetExists)
{
if (core.node.autowarp)
{
GUILayout.BeginHorizontal();
GUILayout.Label("Launch countdown:", GUILayout.ExpandWidth(true));
autopilot.warpCountDown.text = GUILayout.TextField(autopilot.warpCountDown.text,
GUILayout.Width(60));
GUILayout.Label("s", GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
}
if (!launchingToPlane && !launchingToRendezvous && !launchingToInterplanetary)
{
// disable plane/rendezvous/interplanetary for now
if (ascentPathIdx != ascentType.PVG)
{
GUILayout.BeginHorizontal();
if (GUILayout.Button("Launch to rendezvous:", GUILayout.ExpandWidth(false)))
{
launchingToRendezvous = true;
autopilot.StartCountdown(vesselState.time +
LaunchTiming.TimeToPhaseAngle(autopilot.launchPhaseAngle,
mainBody, vesselState.longitude, core.target.TargetOrbit));
}
autopilot.launchPhaseAngle.text = GUILayout.TextField(autopilot.launchPhaseAngle.text,
GUILayout.Width(60));
GUILayout.Label("º", GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
}
GUILayout.BeginHorizontal();
if (GUILayout.Button("Launch into plane of target", GUILayout.ExpandWidth(false)))
{
launchingToPlane = true;
autopilot.StartCountdown(vesselState.time +
LaunchTiming.TimeToPlane(autopilot.launchLANDifference,
mainBody, vesselState.latitude, vesselState.longitude,
core.target.TargetOrbit));
}
autopilot.launchLANDifference.text = GUILayout.TextField(
autopilot.launchLANDifference.text, GUILayout.Width(60));
GUILayout.Label("º", GUILayout.ExpandWidth(false));
GUILayout.EndHorizontal();
if (core.target.TargetOrbit.referenceBody == orbit.referenceBody.referenceBody)
{
if (GUILayout.Button("Launch at interplanetary window"))
{
launchingToInterplanetary = true;
//compute the desired launch date
OrbitalManeuverCalculator.DeltaVAndTimeForHohmannTransfer(mainBody.orbit,
core.target.TargetOrbit, vesselState.time, out interplanetaryWindowUT);
double desiredOrbitPeriod = 2 * Math.PI *
Math.Sqrt(
Math.Pow(mainBody.Radius + autopilot.desiredOrbitAltitude, 3)
/ mainBody.gravParameter);
//launch just before the window, but don't try to launch in the past
interplanetaryWindowUT -= 3 * desiredOrbitPeriod;
interplanetaryWindowUT = Math.Max(vesselState.time + autopilot.warpCountDown,
interplanetaryWindowUT);
autopilot.StartCountdown(interplanetaryWindowUT);
}
}
}
}
else
{
launchingToInterplanetary = launchingToPlane = launchingToRendezvous = false;
GUILayout.Label("Select a target for a timed launch.");
}
if (launchingToInterplanetary || launchingToPlane || launchingToRendezvous)
{
string message = "";
if (launchingToInterplanetary)
{
message = "Launching at interplanetary window";
}
else if (launchingToPlane)
{
desiredInclination = MuUtils.Clamp(core.target.TargetOrbit.inclination, Math.Abs(vesselState.latitude), 180 - Math.Abs(vesselState.latitude));
desiredInclination *=
Math.Sign(Vector3d.Dot(core.target.TargetOrbit.SwappedOrbitNormal(),
Vector3d.Cross(vesselState.CoM - mainBody.position, mainBody.transform.up)));
message = "Launching to target plane";
}
else if (launchingToRendezvous)
{
message = "Launching to rendezvous";
}
if (autopilot.tMinus > 3 * vesselState.deltaT)
{
message += ": T-" + GuiUtils.TimeToDHMS(autopilot.tMinus, 1);
}
GUILayout.Label(message);
if (GUILayout.Button("Abort"))
{
launchingToInterplanetary =
launchingToPlane = launchingToRendezvous = autopilot.timedLaunch = false;
}
}
}
if (autopilot.enabled)
{
GUILayout.Label("Autopilot status: " + autopilot.status);
}
}
if (!vessel.patchedConicsUnlocked() && ascentPathIdx != ascentType.PVG)
{
GUILayout.Label("Warning: MechJeb is unable to circularize without an upgraded Tracking Station.");
}
GUILayout.BeginHorizontal();
autopilot.ascentPathIdxPublic = (ascentType)GuiUtils.ComboBox.Box((int)autopilot.ascentPathIdxPublic, autopilot.ascentPathList, this);
GUILayout.EndHorizontal();
if (autopilot.ascentMenu != null)
{
autopilot.ascentMenu.enabled = GUILayout.Toggle(autopilot.ascentMenu.enabled, "Edit ascent path");
}
GUILayout.EndVertical();
base.WindowGUI(windowID);
}
public static void BoxAndBox(CollisionBox one, CollisionBox two, CollisionData data)
{
// Make sure we have contacts
if (data.NoMoreContacts())
{
return;
}
//if (!IntersectionTests.BoxAndBox(one, two)) return;
// Find the vector between the two centres
Vector3d toCentre = two.GetAxis(3) - one.GetAxis(3);
// We start assuming there is no contact
double pen = double.MaxValue;
int best = 0xffffff;
// Now we check each axes, returning if it gives us
// a separating axis, and keeping track of the axis with
// the smallest penetration otherwise.
if (!TryAxis(one, two, one.GetAxis(0), toCentre, 0, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, one.GetAxis(1), toCentre, 1, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, one.GetAxis(2), toCentre, 2, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, two.GetAxis(0), toCentre, 3, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, two.GetAxis(1), toCentre, 4, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, two.GetAxis(2), toCentre, 5, ref pen, ref best))
{
return;
}
// Store the best axis-major, in case we run into almost
// parallel edge collisions later
int bestSingleAxis = best;
if (!TryAxis(one, two, Vector3d.Cross(one.GetAxis(0), two.GetAxis(0)), toCentre, 6, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, Vector3d.Cross(one.GetAxis(0), two.GetAxis(1)), toCentre, 7, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, Vector3d.Cross(one.GetAxis(0), two.GetAxis(2)), toCentre, 8, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, Vector3d.Cross(one.GetAxis(1), two.GetAxis(0)), toCentre, 9, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, Vector3d.Cross(one.GetAxis(1), two.GetAxis(1)), toCentre, 10, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, Vector3d.Cross(one.GetAxis(1), two.GetAxis(2)), toCentre, 11, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, Vector3d.Cross(one.GetAxis(2), two.GetAxis(0)), toCentre, 12, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, Vector3d.Cross(one.GetAxis(2), two.GetAxis(1)), toCentre, 13, ref pen, ref best))
{
return;
}
if (!TryAxis(one, two, Vector3d.Cross(one.GetAxis(2), two.GetAxis(2)), toCentre, 14, ref pen, ref best))
{
return;
}
// Make sure we've got a result.
if (best == 0xffffff)
{
throw new Exception("best == 0xffffff");
}
// We now know there's a collision, and we know which
// of the axes gave the smallest penetration. We now
// can deal with it in different ways depending on
// the case.
if (best < 3)
{
// We've got a vertex of box two on a face of box one.
FillPointFaceBoxBox(one, two, toCentre, data, best, pen);
}
else if (best < 6)
{
// We've got a vertex of box one on a face of box two.
// We use the same algorithm as above, but swap around
// one and two (and therefore also the vector between their
// centres).
FillPointFaceBoxBox(two, one, toCentre * -1.0, data, best - 3, pen);
}
else
{
// We've got an edge-edge contact. Find out which axes
best -= 6;
int oneAxisIndex = best / 3;
int twoAxisIndex = best % 3;
Vector3d oneAxis = one.GetAxis(oneAxisIndex);
Vector3d twoAxis = two.GetAxis(twoAxisIndex);
Vector3d axis = Vector3d.Cross(oneAxis, twoAxis);
axis.Normalize();
// The axis should point from box one to box two.
if (Vector3d.Dot(axis, toCentre) > 0)
{
axis *= -1.0;
}
// We have the axes, but not the edges: each axis has 4 edges parallel
// to it, we need to find which of the 4 for each object. We do
// that by finding the point in the centre of the edge. We know
// its component in the direction of the box's collision axis is zero
// (its a mid-point) and we determine which of the extremes in each
// of the other axes is closest.
Vector3d ptOnOneEdge = one.HalfSize;
Vector3d ptOnTwoEdge = two.HalfSize;
for (int i = 0; i < 3; i++)
{
if (i == oneAxisIndex)
{
ptOnOneEdge[i] = 0;
}
else if (Vector3d.Dot(one.GetAxis(i), axis) > 0)
{
ptOnOneEdge[i] = -ptOnOneEdge[i];
}
if (i == twoAxisIndex)
{
ptOnTwoEdge[i] = 0;
}
else if (Vector3d.Dot(two.GetAxis(i), axis) < 0)
{
ptOnTwoEdge[i] = -ptOnTwoEdge[i];
}
}
// Move them into world coordinates (they are already oriented
// correctly, since they have been derived from the axes).
ptOnOneEdge = one.Transform * ptOnOneEdge;
ptOnTwoEdge = two.Transform * ptOnTwoEdge;
// So we have a point and a direction for the colliding edges.
// We need to find out point of closest approach of the two
// line-segments.
Vector3d vertex = ContactPoint(
ptOnOneEdge, oneAxis, one.HalfSize[oneAxisIndex],
ptOnTwoEdge, twoAxis, two.HalfSize[twoAxisIndex],
bestSingleAxis > 2
);
// We can fill the contact.
var contact = data.GetContact();
contact.Penetration = pen;
contact.ContactNormal = axis;
contact.ContactPoint = vertex;
contact.SetBodyData(one.Body, two.Body, data.Friction, data.Restitution);
}
}
public void Update(double elapsedTime, EntityManager entityManager)
{
var delta = (elapsedTime - _lastUpdate) * 5;
var forward = _camera.Target - _camera.Position;
forward.Normalize();
var up = _camera.Up;
up.Normalize();
var right = Vector3d.Cross(forward, up);
right.Normalize();
if (_keyboardInputProcessor.IsButtonDown(Key.W))
{
var result = Vector3d.Multiply(new Vector3d(forward.X, 0, forward.Z), delta);
_camera.Target += result;
_camera.Position += result;
}
if (_keyboardInputProcessor.IsButtonDown(Key.S))
{
var result = Vector3d.Multiply(new Vector3d(-forward.X, 0, -forward.Z), delta);
_camera.Target += result;
_camera.Position += result;
}
if (_keyboardInputProcessor.IsButtonDown(Key.D))
{
var result = Vector3d.Multiply(new Vector3d(right.X, 0, right.Z), delta);
_camera.Target += result;
_camera.Position += result;
}
if (_keyboardInputProcessor.IsButtonDown(Key.A))
{
var result = Vector3d.Multiply(new Vector3d(-right.X, 0, -right.Z), delta);
_camera.Target += result;
_camera.Position += result;
}
if (_keyboardInputProcessor.IsButtonDown(Key.E))
{
var rotation = Matrix4d.Rotate(new Vector3d(0, 1, 0), -delta * 0.2);
_camera.Position = _camera.Target + Vector3d.Transform(_camera.Position - _camera.Target, rotation);
}
if (_keyboardInputProcessor.IsButtonDown(Key.Q))
{
var rotation = Matrix4d.Rotate(new Vector3d(0, 1, 0), delta * 0.2);
_camera.Position = _camera.Target + Vector3d.Transform(_camera.Position - _camera.Target, rotation);
}
if (_keyboardInputProcessor.IsButtonDown(Key.LShift))
{
_camera.Position += Vector3d.Multiply(new Vector3d(0, 1, 0), delta);
}
if (_keyboardInputProcessor.IsButtonDown(Key.LControl))
{
_camera.Position += Vector3d.Multiply(new Vector3d(0, 1, 0), -delta);
}
_lastUpdate = elapsedTime;
}
public Vector3d PointVelocity(Vector3d point)
{
return(Vector3d.Cross(Physic.AngularVelocity, point) + Physic.Velocity);
}
bool AvoideVessel(VesselWrapper vsl, Vector3d dir, float dist, Vector3d dV,
float r2, Bounds exhaust2, Transform refT2,
out Vector3d maneuver, float threshold = -1)
{
maneuver = Vector3d.zero;
//filter vessels on non-collision courses
var dVn = dV.normalized;
var cosA = Mathf.Clamp(Vector3.Dot(dir, dVn), -1, 1);
var collision_dist = threshold.Equals(0)?
2 * dist - vsl.Geometry.DistToBounds(vsl.Physics.wCoM + dir * dist) -
Mathf.Sqrt(exhaust2.SqrDistance(refT2.InverseTransformPoint(vsl.Physics.wCoM))) :
Mathf.Max(vsl.Geometry.R, dist - vsl.Geometry.DistToBounds(vsl.Physics.wCoM + dir * dist)) +
Mathf.Max(r2, dist - Mathf.Sqrt(exhaust2.SqrDistance(refT2.InverseTransformPoint(vsl.Physics.wCoM))));
// vsl.Log("R {}, R2 {}; r {}, r2 {}", vsl.Geometry.R, r2,
// dist-vsl.DistToBounds(vsl.wCoM+dir*dist),
// dist-Mathf.Sqrt(exhaust2.SqrDistance(refT2.InverseTransformPoint(vsl.wCoM))));//debug
// Utils.Log("{}: cosA: {}, threshold {}", vsl.vessel.vesselName, cosA, threshold);//debug
if (cosA <= 0)
{
goto check_distance;
}
if (threshold < 0)
{
threshold = C.MinDistance;
}
var sinA = Mathf.Sqrt(1 - cosA * cosA);
var min_separation = dist * sinA;
var sep_threshold = collision_dist + threshold;
// Utils.Log("{}: min_sep > sep_thresh: {} > {}, threshold {}",
// vsl.vessel.vesselName, min_separation, sep_threshold, threshold);//debug
if (min_separation > sep_threshold)
{
goto check_distance;
}
//calculate time to collision
var vDist = 0f;
var alpha = Mathf.Acos(cosA);
var dVm = dV.magnitude;
if (sinA <= 0)
{
vDist = dist - sep_threshold;
}
else if (dist > sep_threshold)
{
vDist = sep_threshold * Mathf.Sin(Mathf.Asin(min_separation / sep_threshold) - alpha) / sinA;
}
var vTime = Utils.ClampL(vDist, 0.1f) / dVm;
// Utils.Log("{}: vTime > SafeTime: {} > {}",
// vsl.vessel.vesselName, vTime, CPS.SafeTime/Utils.Clamp(Mathf.Abs(Vector3.Dot(vsl.wMaxAngularA, dVn)), 0.01f, 1f));//debug
if (vTime > SafeTime(vsl, dVn))
{
goto check_distance;
}
//calculate maneuver
Vector3d side;
if (cosA < 0.9)
{
side = (dVn * cosA - dir).normalized;
}
else if (Math.Abs(Vector3d.Dot(dVn, vsl.Physics.Up)) < 0.9)
{
side = Vector3d.Cross(dVn, vsl.Physics.Up).normalized;
}
else
{
side = Vector3d.Cross(dVn, vsl.OnPlanetParams.Fwd).normalized;
}
// if(dist > sep_threshold)
// {
// var beta = Mathf.Asin(sep_threshold/dist)-alpha;
// maneuver = (Mathf.Sin(beta)*side + dVn*(Mathf.Cos(beta)-1)).normalized;
// }
// else
maneuver = side;
maneuver += vsl.Physics.Up * Mathf.Sign(Vector3.Dot(dir, vsl.Physics.Up)) * (min_separation / sep_threshold - 1);
maneuver *= (sep_threshold - min_separation) / Math.Sqrt(vTime);
// maneuver = (-vsl.Up*Mathf.Sign(Vector3.Dot(dir, vsl.Up))*(1-min_separation/sep_threshold) +
// (dVn*cosA-dir).normalized).normalized * (sep_threshold-min_separation) / vTime;
// vsl.Log("\ndist {}\ndV {}\nmaneuver: {}\n" +
// "vTime {}, vDist {}, min sep {}, sep_thresh {}",
// dir*dist, dV, maneuver, vTime, vDist, min_separation, sep_threshold);//debug
#if DEBUG
Collided |= dist < collision_dist;
#endif
//if distance is not safe, correct course anyway
check_distance:
var collision = !maneuver.IsZero();
dist -= collision_dist;
if (dist < threshold)
{
var dist_to_safe = Utils.ClampH(dist - threshold, -0.01f);
var dc = dir * dist_to_safe;
if (vsl.HorizontalSpeed.NeededVector.sqrMagnitude > C.LatAvoidMinVelSqr)
{
var lat_avoid = Vector3d.Cross(vsl.Physics.Up, vsl.HorizontalSpeed.NeededVector.normalized);
dc = Vector3d.Dot(dc, lat_avoid) >= 0?
lat_avoid * dist_to_safe :
lat_avoid * -dist_to_safe;
}
if (dc.sqrMagnitude > 0.25)
{
maneuver += dc / C.SafeTime * 2;
}
// vsl.Log("adding safe distance correction: {}", dc/CPS.SafeTime*2);//debug
}
// #if DEBUG
// Dir = dir*dist;
// DeltaV = dV;
// Maneuver = maneuver;
// #endif
return(collision);
}
public InputManager(KeyboardDevice keyboard)
{
_config = new CameraConfig(new Vector3d(10, 0, 0), new Vector3d(-1, 0, 0), new Vector3d(0, 1, 0), 1, 0, 1);
_keyboard = keyboard;
_bindings = new Dictionary <Key, Action <float> >
{
{ Key.W, dt => _config.Position += _config.Lookat * dt * _config.MoveSpeed },
{ Key.S, dt => _config.Position -= _config.Lookat * dt * _config.MoveSpeed },
{ Key.A, dt => _config.Position += Vector3d.Cross(_config.Up, _config.Lookat) * dt * _config.MoveSpeed },
{ Key.D, dt => _config.Position -= Vector3d.Cross(_config.Up, _config.Lookat) * dt * _config.MoveSpeed },
{ Key.ShiftLeft, dt => _config.Position += _config.Up * dt * _config.MoveSpeed },
{ Key.Space, dt => _config.Position -= _config.Up * dt * _config.MoveSpeed },
{ Key.Q, dt => _config.Up = Vector3d.Transform(_config.Up, Matrix4d.CreateFromAxisAngle(_config.Lookat, TurnSpeed * dt)) },
{ Key.E, dt => _config.Up = Vector3d.Transform(_config.Up, Matrix4d.CreateFromAxisAngle(_config.Lookat, -TurnSpeed * dt)) },
{ Key.Left, dt => _config.Lookat = Vector3d.Transform(_config.Lookat, Matrix4d.CreateFromAxisAngle(_config.Up, TurnSpeed * dt * _config.Fov)) },
{ Key.Right, dt => _config.Lookat = Vector3d.Transform(_config.Lookat, Matrix4d.CreateFromAxisAngle(_config.Up, -TurnSpeed * dt * _config.Fov)) },
{ Key.Up, dt => _config.Lookat = Vector3d.Transform(_config.Lookat, Matrix4d.CreateFromAxisAngle(Vector3d.Cross(_config.Up, _config.Lookat), TurnSpeed * dt * _config.Fov)) },
{ Key.Down, dt => _config.Lookat = Vector3d.Transform(_config.Lookat, Matrix4d.CreateFromAxisAngle(Vector3d.Cross(_config.Up, _config.Lookat), -TurnSpeed * dt * _config.Fov)) },
{ Key.R, dt => _config.MoveSpeed *= 1 + dt },
{ Key.F, dt => _config.MoveSpeed *= 1 - dt },
{ Key.N, dt => _config.Fov *= 1 + dt },
{ Key.M, dt => _config.Fov *= 1 - dt }
};
_keyboard.KeyDown += KeyboardOnKeyDown;
}
/// <summary>
/// Main control function
/// </summary>
/// <param name="desired_vel">Desired velocity direction in surface reference frame.</param>
/// <param name="desired_acceleration">Desired acceleration.</param>
public void ApplyControl(FlightCtrlState state, Vector3d desired_vel, Vector3d desired_acceleration)
{
Vector3d planet2ves = vessel.ReferenceTransform.position - vessel.mainBody.position;
Vector3d planet2vesNorm = planet2ves.normalized;
Vector3d shift_acc = Vector3d.zero;
// centrifugal acceleration to stay on desired altitude
//Vector3d level_acc = -planet2vesNorm * (imodel.surface_v - Vector3d.Project(imodel.surface_v, planet2vesNorm)).sqrMagnitude / planet2ves.magnitude;
// Rotation vector
Vector3d desired_turn_acc_dir = Vector3d.Cross(
Vector3d.Cross(imodel.surface_v, desired_vel).normalized,
imodel.surface_v).normalized;
angular_error = Vector3d.Angle(imodel.surface_v.normalized, desired_vel) * dgr2rad;
max_lift_acc = Math.Max(0.01, max_lift_acceleration(PITCH));
max_sideslip_acc = Math.Max(0.001, max_lift_acceleration(YAW));
// let's find this craft's maximum acceleration toward desired_turn_acc_dir without stalling
// it can be solved from simple quadratic equation
Vector3d max_turn_acc = non_stall_turn_acceleration(desired_turn_acc_dir, max_lift_acc);
max_turn_acc = strength * max_turn_acc * ((vessel == FlightGlobals.ActiveVessel && FlightInputHandler.fetch.precisionMode) ? 0.4 : 1.0);
// now let's take roll speed and relaxation into account
double max_angular_v = max_turn_acc.magnitude / imodel.surface_v_magnitude;
double t1 = max_roll_v / roll_acc_factor;
double t2 = (90.0 * dgr2rad - t1 * max_roll_v) / max_roll_v;
double stop_time_roll = roll_stop_k * (2.0 * t1 + t2);
if (double.IsNaN(stop_time_roll) || double.IsInfinity(stop_time_roll))
{
stop_time_roll = 2.0;
}
if (stop_time_roll <= 0.0)
{
stop_time_roll = 0.5;
}
// now let's generate desired acceleration
if (angular_error / max_angular_v > stop_time_roll)
{
// we're far away from relaxation, let's simply produce maximum acceleration
shift_acc = max_turn_acc;
}
else
{
// we're relaxing now, quadratic descend is good approximation
{
double tk = (angular_error / max_angular_v) / stop_time_roll;
if (Math.Abs(angular_error) < relaxation_margin)
{
tk *= Mathf.Lerp(1.0f, angle_relaxation_k, (float)(1.0f - Math.Abs(angular_error) / relaxation_margin));
}
shift_acc = max_turn_acc * tk;
}
}
//if (angular_error > 0.2 || Vector3d.Dot(desired_acceleration, shift_acc) < -0.1)
// target_acc = shift_acc;
//else
target_acc = desired_acceleration + shift_acc;
//current_acc = imodel.sum_acc;
// we need aoa moderation for AoA controllers to work
pitch_c.moderate_aoa = true;
yaw_c.moderate_aoa = true;
Vector3d neutral_acc = -imodel.gravity_acc - imodel.noninert_acc;
Vector3d target_lift_acc = target_acc + neutral_acc;
Vector3d target_normal_lift_acc = target_lift_acc - Vector3d.Project(target_lift_acc, imodel.surface_v);
// prevent rolling on small errors
if (angular_error < 2e-2 && target_normal_lift_acc.magnitude < neutral_acc.magnitude * 0.3)
{
target_lift_acc = Vector3d.Project(target_lift_acc, neutral_acc);
target_normal_lift_acc = target_lift_acc - Vector3d.Project(target_lift_acc, imodel.surface_v);
}
Vector3d desired_right_direction = Vector3d.Cross(target_normal_lift_acc, vessel.ReferenceTransform.up).normalized;
// let's apply roll to maintain desired_right_direction
Vector3 right_vector = imodel.virtualRotation * Vector3.right;
double new_roll_error = Math.Sign(Vector3d.Dot(right_vector, target_normal_lift_acc)) *
Math.Acos(Math.Min(Math.Max(Vector3d.Dot(desired_right_direction, right_vector), -1.0), 1.0));
// rolling to pitch up is not always as efficient as pitching down
double spine_to_zenith = Vector3d.Dot(desired_right_direction, Vector3d.Cross(imodel.surface_v, imodel.gravity_acc));
if (target_normal_lift_acc.magnitude < max_neg_g * 9.81 || !allow_spine_down || vessel.heightFromTerrain < min_rollover_alt)
{
if (Math.Abs(new_roll_error) > 90.0 * dgr2rad && spine_to_zenith < 0.0)
{
new_roll_error = new_roll_error - 180.0 * dgr2rad * Math.Sign(new_roll_error);
}
}
// filter it
if ((Math.Abs(new_roll_error) < roll_error_filter_margin * dgr2rad) && (Math.Abs(roll_error) < roll_error_filter_margin * dgr2rad))
{
roll_error = Common.simple_filter(new_roll_error, roll_error, roll_error_filter_k);
}
else
{
roll_error = new_roll_error;
}
// generate desired roll angular_v
roll_c.user_controlled = false;
roll_c.ApplyControl(state, get_desired_roll_v(roll_error));
// now let's apply pitch and yaw AoA controls
// pitch AoA
desired_pitch_lift = 0.0;
if (Math.Abs(roll_error) < 30.0 * dgr2rad || Math.Abs(roll_error - 180.0) < 30.0 * dgr2rad)
{
desired_pitch_lift = Vector3.Dot(imodel.pitch_tangent, target_normal_lift_acc);
}
else
{
desired_pitch_lift = Vector3.Dot(imodel.pitch_tangent, neutral_acc);
}
desired_pitch_acc = desired_pitch_lift + imodel.pitch_gravity_acc + imodel.pitch_noninert_acc;
desired_pitch_v = desired_pitch_acc / imodel.surface_v_magnitude;
// let's find equilibrium AoA for desired lift
desired_aoa = get_desired_aoa(imodel.pitch_rot_model_gen, desired_pitch_v, 0.0);
if (float.IsNaN(desired_aoa) || float.IsInfinity(desired_aoa))
{
desired_aoa = 0.0f;
}
aoa_c.user_controlled = false;
aoa_c.ApplyControl(state, desired_aoa, 0.0f);
// yaw sideslip
//if (Math.Abs(roll_angle) > 3.0 * dgr2rad)
//{
// desired_yaw_lift = 0.0;
// desired_sideslip = 0.0f;
//}
//else
//{
desired_yaw_lift = 0.0; // Vector3.Dot(imodel.yaw_tangent, normal_lift_acc);
desired_yaw_acc = desired_yaw_lift + imodel.yaw_gravity_acc + imodel.yaw_noninert_acc;
desired_yaw_v = desired_yaw_acc / imodel.surface_v_magnitude;
// let's find equilibrium sideslip for desired lift
//if (Math.Abs(desired_yaw_lift) < 0.01f)
desired_sideslip = (float)Common.simple_filter(get_desired_aoa(imodel.yaw_rot_model_gen, desired_yaw_v, 0.0), desired_sideslip, sideslip_filter_k);
if (float.IsNaN(desired_sideslip) || float.IsInfinity(desired_sideslip) || Math.Abs(desired_sideslip) < 0.001f)
{
desired_sideslip = 0.0f;
}
desired_sideslip = 0.0f;
//}
//desired_sideslip = 0.0f;
side_c.user_controlled = false;
side_c.ApplyControl(state, desired_sideslip, 0.0f);
}
private void RenderUpcomingEvents()
{
string vessel_guid = vessel_.id.ToString();
double current_time = plugin_.CurrentTime();
bool should_clear_guidance = true;
for (int i = 0; i < burn_editors_.Count; ++i)
{
NavigationManoeuvre manoeuvre =
plugin_.FlightPlanGetManoeuvre(vessel_guid, i);
if (manoeuvre.final_time > current_time)
{
if (manoeuvre.burn.initial_time > current_time)
{
UnityEngine.GUILayout.TextArea("Upcoming manœuvre: #" + (i + 1));
UnityEngine.GUILayout.Label(
"Ignition " + FormatTimeSpan(TimeSpan.FromSeconds(
current_time - manoeuvre.burn.initial_time)));
}
else
{
UnityEngine.GUILayout.TextArea("Ongoing manœuvre: #" + (i + 1));
UnityEngine.GUILayout.Label(
"Cutoff " + FormatTimeSpan(TimeSpan.FromSeconds(
current_time - manoeuvre.final_time)));
}
show_guidance_ =
UnityEngine.GUILayout.Toggle(show_guidance_, "Show on navball");
if (show_guidance_ &&
!double.IsNaN(manoeuvre.inertial_direction.x +
manoeuvre.inertial_direction.y +
manoeuvre.inertial_direction.z))
{
if (guidance_node_ == null)
{
guidance_node_ = vessel_.patchedConicSolver.AddManeuverNode(
manoeuvre.burn.initial_time);
}
Vector3d stock_velocity_at_node_time =
vessel_.orbit.getOrbitalVelocityAtUT(
manoeuvre.burn.initial_time).xzy;
Vector3d stock_displacement_from_parent_at_node_time =
vessel_.orbit.getRelativePositionAtUT(
manoeuvre.burn.initial_time).xzy;
UnityEngine.Quaternion stock_frenet_frame_to_world =
UnityEngine.Quaternion.LookRotation(
stock_velocity_at_node_time,
Vector3d.Cross(stock_velocity_at_node_time,
stock_displacement_from_parent_at_node_time));
guidance_node_.OnGizmoUpdated(
((Vector3d)manoeuvre.burn.delta_v).magnitude *
(Vector3d)(stock_frenet_frame_to_world.Inverse() *
(Vector3d)manoeuvre.inertial_direction),
manoeuvre.burn.initial_time);
should_clear_guidance = false;
}
break;
}
}
if (should_clear_guidance && guidance_node_ != null)
{
vessel_.patchedConicSolver.RemoveManeuverNode(guidance_node_);
guidance_node_ = null;
}
}
public void GetClCdCmSteady(
InstantConditionSimInput input,
out InstantConditionSimOutput output,
bool clear,
bool reset_stall = false
)
{
output = new InstantConditionSimOutput();
double area = 0;
double MAC = 0;
double b_2 = 0;
Vector3d forward = Vector3.forward;
Vector3d up = Vector3.up;
Vector3d right = Vector3.right;
Vector3d CoM = Vector3d.zero;
if (EditorDriver.editorFacility == EditorFacility.VAB)
{
forward = Vector3.up;
up = -Vector3.forward;
}
double mass = 0;
List <Part> partsList = EditorLogic.SortedShipList;
foreach (Part p in partsList)
{
if (FARAeroUtil.IsNonphysical(p))
{
continue;
}
double partMass = p.mass;
if (p.Resources.Count > 0)
{
partMass += p.GetResourceMass();
}
// If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass
CoM += partMass * (Vector3d)p.transform.TransformPoint(p.CoMOffset);
mass += partMass;
}
CoM /= mass;
// Rodhern: The original reference directions (velocity, liftVector, sideways) did not form an orthonormal
// basis. That in turn produced some counterintuitive calculation results, such as coupled yaw and pitch
// derivatives. A more thorough discussion of the topic can be found on the KSP forums:
// https://forum.kerbalspaceprogram.com/index.php?/topic/19321-131-ferram-aerospace-research-v01591-liepmann-4218/&do=findComment&comment=2781270
// The reference directions have been replaced by new ones that are orthonormal by construction.
// In dkavolis branch Vector3.Cross() and Vector3d.Normalize() are used explicitly. There is no apparent
// benefit to this other than possibly improved readability.
double sinAlpha = Math.Sin(input.alpha * Math.PI / 180);
double cosAlpha = Math.Sqrt(Math.Max(1 - sinAlpha * sinAlpha, 0));
double sinBeta = Math.Sin(input.beta * Math.PI / 180);
double cosBeta = Math.Sqrt(Math.Max(1 - sinBeta * sinBeta, 0));
double sinPhi = Math.Sin(input.phi * Math.PI / 180);
double cosPhi = Math.Sqrt(Math.Max(1 - sinPhi * sinPhi, 0));
double alphaDot = input.alphaDot * Math.PI / 180;
double betaDot = input.betaDot * Math.PI / 180;
double phiDot = input.phiDot * Math.PI / 180;
Vector3d velocity = forward * cosAlpha * cosBeta;
velocity += right * (sinPhi * sinAlpha * cosBeta + cosPhi * sinBeta);
velocity += -up * (cosPhi * sinAlpha * cosBeta - sinPhi * sinBeta);
velocity.Normalize();
Vector3d liftDown = -forward * sinAlpha;
liftDown += right * sinPhi * cosAlpha;
liftDown += -up * cosPhi * cosAlpha;
liftDown.Normalize();
Vector3d sideways = Vector3.Cross(velocity, liftDown);
sideways.Normalize();
Vector3d angVel = forward * (phiDot - sinAlpha * betaDot);
angVel += right * (cosPhi * alphaDot + cosAlpha * sinPhi * betaDot);
angVel += up * (sinPhi * alphaDot - cosAlpha * cosPhi * betaDot);
foreach (FARWingAerodynamicModel w in _wingAerodynamicModel)
{
if (!(w && w.part))
{
continue;
}
w.ComputeForceEditor(velocity, input.machNumber, 2);
if (clear)
{
w.EditorClClear(reset_stall);
}
Vector3d relPos = w.GetAerodynamicCenter() - CoM;
Vector3d vel = velocity + Vector3d.Cross(angVel, relPos);
if (w is FARControllableSurface controllableSurface)
{
controllableSurface.SetControlStateEditor(CoM,
vel,
(float)input.pitchValue,
0,
0,
input.flaps,
input.spoilers);
}
else if (w.isShielded)
{
continue;
}
Vector3d force = w.ComputeForceEditor(vel.normalized, input.machNumber, 2) * 1000;
output.Cl += -Vector3d.Dot(force, liftDown);
output.Cy += Vector3d.Dot(force, sideways);
output.Cd += -Vector3d.Dot(force, velocity);
Vector3d moment = -Vector3d.Cross(relPos, force);
output.Cm += Vector3d.Dot(moment, sideways);
output.Cn += Vector3d.Dot(moment, liftDown);
output.C_roll += Vector3d.Dot(moment, velocity);
area += w.S;
MAC += w.GetMAC() * w.S;
b_2 += w.Getb_2() * w.S;
}
var center = new FARCenterQuery();
foreach (FARAeroSection aeroSection in _currentAeroSections)
{
aeroSection.PredictionCalculateAeroForces(2,
(float)input.machNumber,
10000,
0,
0.005f,
velocity.normalized,
center);
}
Vector3d centerForce = center.force * 1000;
output.Cl += -Vector3d.Dot(centerForce, liftDown);
output.Cy += Vector3d.Dot(centerForce, sideways);
output.Cd += -Vector3d.Dot(centerForce, velocity);
Vector3d centerMoment = -center.TorqueAt(CoM) * 1000;
output.Cm += Vector3d.Dot(centerMoment, sideways);
output.Cn += Vector3d.Dot(centerMoment, liftDown);
output.C_roll += Vector3d.Dot(centerMoment, velocity);
if (area.NearlyEqual(0))
{
area = _maxCrossSectionFromBody;
b_2 = 1;
MAC = _bodyLength;
}
double recipArea = 1 / area;
MAC *= recipArea;
b_2 *= recipArea;
output.Cl *= recipArea;
output.Cd *= recipArea;
output.Cm *= recipArea / MAC;
output.Cy *= recipArea;
output.Cn *= recipArea / b_2;
output.C_roll *= recipArea / b_2;
}
// Feed an force to the accumulator.
public void AddForce(Vector3d applicationPoint, Vector3d force)
{
totalForce += force;
totalZeroOriginTorque += Vector3d.Cross(applicationPoint, force);
avgApplicationPoint.Add(applicationPoint, force.magnitude);
}
// Show exit orbital predictions
private void ShowExitOrbit(Vessel nearObject, Vessel farObject)
{
// Recenter map, save previous state.
wasInMapView = MapView.MapIsEnabled;
if (!MapView.MapIsEnabled)
{
MapView.EnterMapView();
}
//log.Debug("Finding target.");
MapObject farTarget = FindVesselBody(farObject);
if (farTarget != null)
{
MapView.MapCamera.SetTarget(farTarget);
}
Vector3 mapCamPos = ScaledSpace.ScaledToLocalSpace(MapView.MapCamera.transform.position);
Vector3 farTarPos = ScaledSpace.ScaledToLocalSpace(farTarget.transform.position);
float dirScalar = Vector3.Distance(mapCamPos, farTarPos);
//log.Debug("Initializing, camera distance is " + dirScalar);
// Initialize projection stuff.
if (!IsPatchedConicsAvailable)
{
HideExitOrbit();
return;
}
predictionsDrawn = true;
//log.Debug("Beginning orbital projection.");
Vector3d exitTraj = ESLDBeacon.GetJumpVelOffset(nearObject, farObject);
if (predictionGameObject != null)
{
Destroy(predictionGameObject);
}
predictionGameObject = new GameObject("OrbitRendererGameObject");
predictionOrbitDriver = predictionGameObject.AddComponent <OrbitDriver>();
predictionOrbitDriver.orbit.referenceBody = farObject.mainBody;
predictionOrbitDriver.orbit = new Orbit();
predictionOrbitDriver.referenceBody = farObject.mainBody;
predictionOrbitDriver.upperCamVsSmaRatio = 999999; // Took forever to figure this out - this sets at what zoom level the orbit appears. Was causing it not to appear at small bodies.
predictionOrbitDriver.lowerCamVsSmaRatio = 0.0001f;
predictionOrbitDriver.orbit.UpdateFromStateVectors(farObject.orbit.pos, exitTraj, farObject.mainBody, Planetarium.GetUniversalTime());
predictionOrbitDriver.orbit.Init();
Vector3d p = predictionOrbitDriver.orbit.getRelativePositionAtUT(Planetarium.GetUniversalTime());
Vector3d v = predictionOrbitDriver.orbit.getOrbitalVelocityAtUT(Planetarium.GetUniversalTime());
predictionOrbitDriver.orbit.h = Vector3d.Cross(p, v);
predictionOrbitDriver.updateMode = OrbitDriver.UpdateMode.TRACK_Phys;
predictionOrbitDriver.orbitColor = Color.red;
//log.Debug("Displaying orbital projection.");
predictionOrbitRenderer = predictionGameObject.AddComponent <OrbitRenderer>();
predictionOrbitRenderer.SetColor(Color.red);
predictionOrbitRenderer.vessel = this.vessel;
predictionOrbitDriver.vessel = this.vessel;
predictionOrbitRenderer.upperCamVsSmaRatio = 999999;
predictionOrbitRenderer.lowerCamVsSmaRatio = 0.0001f;
predictionOrbitRenderer.celestialBody = farObject.mainBody;
predictionOrbitRenderer.driver = predictionOrbitDriver;
predictionOrbitDriver.Renderer = predictionOrbitRenderer;
if (true)
{
// This draws the full Patched Conics prediction.
predictionPatchedConicSolver = predictionGameObject.AddComponent <PatchedConicSolver>();
predictionPatchedConicRenderer = predictionGameObject.AddComponent <PatchedConicRenderer>();
predictionOrbitRenderer.drawIcons = OrbitRendererBase.DrawIcons.NONE;
predictionOrbitRenderer.drawMode = OrbitRendererBase.DrawMode.OFF;
}
else
{
// This draws just the first patch, similar to a Level 1 tracking station.
predictionOrbitRenderer.driver.drawOrbit = true;
predictionOrbitRenderer.drawIcons = OrbitRenderer.DrawIcons.OBJ_PE_AP;
predictionOrbitRenderer.drawMode = OrbitRenderer.DrawMode.REDRAW_AND_RECALCULATE;
predictionOrbitRenderer.enabled = true;
}
this.StartCoroutine(NullOrbitDriverVessels());
// Splash some color on it.
// Directional indicator.
/*
* float baseWidth = 20.0f;
* double baseStart = 10;
* double baseEnd = 50;
* oDirObj = new GameObject("Indicator");
* oDirObj.layer = 10; // Map layer!
* oDirection = oDirObj.AddComponent<LineRenderer>();
* oDirection.useWorldSpace = false;
* oOrigin = null;
* foreach (Transform sstr in ScaledSpace.Instance.scaledSpaceTransforms)
* {
* if (sstr.name == far.mainBody.name)
* {
* oOrigin = sstr;
* log.debug("Found origin: " + sstr.name);
* break;
* }
* }
* oDirection.transform.parent = oOrigin;
* oDirection.transform.position = ScaledSpace.LocalToScaledSpace(far.transform.position);
* oDirection.material = new Material(Shader.Find("Particles/Additive"));
* oDirection.SetColors(Color.clear, Color.red);
* if (dirScalar / 325000 > baseWidth) baseWidth = dirScalar / 325000f;
* oDirection.SetWidth(baseWidth, 0.01f);
* log.debug("Base Width set to " + baseWidth);
* oDirection.SetVertexCount(2);
* if (dirScalar / 650000 > baseStart) baseStart = dirScalar / 650000;
* if (dirScalar / 130000 > baseEnd) baseEnd = dirScalar / 130000;
* log.debug("Base Start set to " + baseStart);
* log.debug("Base End set to " + baseEnd);
* oDirection.SetPosition(0, Vector3d.zero + exitTraj.xzy.normalized * baseStart);
* oDirection.SetPosition(1, exitTraj.xzy.normalized * baseEnd);
* oDirection.enabled = true;
*/
}
// fixed-time maximal energy from https://doi.org/10.2514/2.5045
private void flightangle3constraintMAXE(double[] yT, double[] z, bool terminal)
{
Vector3d rf = new Vector3d(yT[0], yT[1], yT[2]);
Vector3d vf = new Vector3d(yT[3], yT[4], yT[5]);
Vector3d pvf = new Vector3d(yT[6], yT[7], yT[8]);
Vector3d prf = new Vector3d(yT[9], yT[10], yT[11]);
Vector3d n = new Vector3d(0, -1, 0); /* angular momentum vectors point south in KSP and we're in xzy coords */
Vector3d hf = Vector3d.Cross(rf, vf);
if (!terminal)
{
z[0] = (rf.sqrMagnitude - rTm * rTm) / 2.0;
z[1] = Vector3d.Dot(n, hf) - hf.magnitude * Math.Cos(incT);
z[2] = Vector3d.Dot(rf, vf) - rf.magnitude * vf.magnitude * Math.Sin(gammaT);
z[3] = Vector3d.Dot(vf, prf) * rf.sqrMagnitude - Vector3d.Dot(rf, pvf) * vf.sqrMagnitude + Vector3d.Dot(rf, vf) * (vf.sqrMagnitude - Vector3d.Dot(rf, prf));
z[4] = Vector3d.Dot(vf, pvf) - vf.sqrMagnitude;
z[5] = Vector3d.Dot(hf, prf) * Vector3d.Dot(hf, Vector3d.Cross(rf, n)) + Vector3d.Dot(hf, pvf) * Vector3d.Dot(hf, Vector3d.Cross(vf, n));
}
else
{
z[0] = z[1] = z[2] = z[3] = z[4] = z[5] = 0.0;
}
}
public void FixedUpdate()
{
var vessel = GetComponent <Vessel>();
if (vessel != FlightGlobals.ActiveVessel)
{
Destroy(this);
return;
}
if (TimeWarp.CurrentRateIndex != 0)
{
TimeWarp.SetRate(0, true);
Telemagic.logTM("cancel time warp");
}
if (AlreadyTeleported)
{
if (vessel.LandedOrSplashed)
{
Destroy(this);
}
else
{
var accel = (vessel.srf_velocity + vessel.upAxis) * -0.5;
vessel.ChangeWorldVelocity(accel);
}
}
else
{
//NOT AlreadyTeleported
//Still calculating
var pqs = Body.pqsController;
if (pqs == null) // The sun has no terrain. Everthing else has a PQScontroller.
{
Destroy(this);
return;
}
var alt = pqs.GetSurfaceHeight(Body.GetRelSurfaceNVector(Latitude, Longitude)) - Body.Radius;
var tmpAlt = Body.TerrainAltitude(Latitude, Longitude);
double landHeight = FlightGlobals.ActiveVessel.altitude - FlightGlobals.ActiveVessel.pqsAltitude;
double finalAltitude = 0.0;
var checkAlt = FlightGlobals.ActiveVessel.altitude;
var checkPQSAlt = FlightGlobals.ActiveVessel.pqsAltitude;
double terrainAlt = GetTerrainAltitude();
Telemagic.logTM("-------------------");
Telemagic.logTM($"m1. Body.Radius = {Body.Radius}");
Telemagic.logTM("m2. PQS SurfaceHeight = " + pqs.GetSurfaceHeight(Body.GetRelSurfaceNVector(Latitude, Longitude)));
Telemagic.logTM("alt ( m2 - m1 ) = " + alt);
Telemagic.logTM("Body.TerrainAltitude = " + tmpAlt);
Telemagic.logTM("checkAlt = " + checkAlt);
Telemagic.logTM("checkPQSAlt = " + checkPQSAlt);
Telemagic.logTM("landheight = " + landHeight);
Telemagic.logTM("terrainAlt = " + terrainAlt);
Telemagic.logTM("-------------------");
Telemagic.logTM($"Latitude: {Latitude} Longitude: {Longitude}");
Telemagic.logTM("-------------------");
alt = Math.Max(alt, 0d);
// HoldVesselUnpack is in display frames, not physics frames
Vector3d teleportPosition;
if (!teleportedToLandingAlt)
{
Telemagic.logTM("teleportedToLandingAlt == false");
Telemagic.logTM("interimAltitude: " + InterimAltitude);
Telemagic.logTM("Altitude: " + Altitude);
if (InterimAltitude > Altitude)
{
if (Planetarium.GetUniversalTime() - lastUpdate >= 0.5)
{
InterimAltitude = InterimAltitude / 10;
terrainAlt = GetTerrainAltitude();
if (InterimAltitude < terrainAlt)
{
InterimAltitude = terrainAlt + Altitude;
}
//InterimAltitude = terrainAlt + Altitude;
teleportPosition = Body.GetWorldSurfacePosition(Latitude, Longitude, InterimAltitude) - Body.position;
Telemagic.logTM("1. teleportPosition = " + teleportPosition);
Telemagic.logTM("1. interimAltitude: " + InterimAltitude);
if (lastUpdate != 0)
{
InterimAltitude = Altitude;
}
lastUpdate = Planetarium.GetUniversalTime();
}
else
{
Telemagic.logTM("teleportPositionAltitude (no time change):");
teleportPosition = Body.GetWorldSurfacePosition(Latitude, Longitude, alt + InterimAltitude) - Body.position;
Telemagic.logTM("2. teleportPosition = " + teleportPosition);
Telemagic.logTM("2. alt: " + alt);
Telemagic.logTM("2. interimAltitude: " + InterimAltitude);
}
}
else
{
//InterimAltitude <= Altitude
Telemagic.logTM("3. teleportedToLandingAlt sets to true");
landHeight = FlightGlobals.ActiveVessel.altitude - FlightGlobals.ActiveVessel.pqsAltitude;
terrainAlt = GetTerrainAltitude();
//trying to find the correct altitude here.
if (checkPQSAlt > terrainAlt)
{
alt = checkPQSAlt;
}
else
{
alt = terrainAlt;
}
if (alt == 0.0)
{
//now what?
}
teleportedToLandingAlt = true;
//finalAltitude = alt + Altitude;
if (alt < 0)
{
finalAltitude = Altitude;
}
else if (alt > 0)
{
finalAltitude = alt + Altitude;
}
else
{
finalAltitude = alt + Altitude;
}
teleportPosition = Body.GetWorldSurfacePosition(Latitude, Longitude, finalAltitude) - Body.position;
Telemagic.logTM("3. teleportPosition = " + teleportPosition);
Telemagic.logTM($"3. alt = {alt} Altitude = {Altitude} InterimAltitude = {InterimAltitude}");
Telemagic.logTM($"3. TerrainAlt = {terrainAlt} landHeight = {landHeight}");
}
}
else
{
Telemagic.logTM("teleportedToLandingAlt == true");
landHeight = FlightGlobals.ActiveVessel.altitude - FlightGlobals.ActiveVessel.pqsAltitude;
terrainAlt = GetTerrainAltitude();
Telemagic.logTM($"4. finalAltitude = {finalAltitude}");
//finalAltitude = alt + Altitude;
if (alt < 0)
{
finalAltitude = Altitude;
}
else if (alt > 0)
{
finalAltitude = alt + Altitude;
}
else
{
finalAltitude = alt + Altitude;
}
//teleportPosition = Body.GetRelSurfacePosition(Latitude, Longitude, finalAltitude);
teleportPosition = Body.GetWorldSurfacePosition(Latitude, Longitude, finalAltitude) - Body.position;
Telemagic.logTM("4. teleportPosition = " + teleportPosition);
Telemagic.logTM($"4. alt = {alt} Altitude = { Altitude} InterimAltitude = { InterimAltitude}");
Telemagic.logTM($"4. TerrainAlt = {terrainAlt} landHeight = {landHeight}");
Telemagic.logTM("4. finalAltitude = " + finalAltitude);
}
var teleportVelocity = Vector3d.Cross(Body.angularVelocity, teleportPosition);
// convert from world space to orbit space
teleportPosition = teleportPosition.xzy;
teleportVelocity = teleportVelocity.xzy;
Telemagic.logTM("0. teleportPosition(xzy): " + teleportPosition);
Telemagic.logTM("0. teleportVelocity(xzy): " + teleportVelocity);
Telemagic.logTM("0. Body : " + Body);
// counter for the momentary fall when on rails (about one second)
teleportVelocity += teleportPosition.normalized * (Body.gravParameter / teleportPosition.sqrMagnitude);
Quaternion rotation;
if (SetRotation)
{
// Need to check vessel and find up for the root command pod
vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, false); //hopefully this disables SAS as it causes unknown results!
var from = Vector3d.up; //Sensible default for all vessels
if (vessel.displaylandedAt == "Runway" || vessel.vesselType.ToString() == "Plane")
{
from = vessel.vesselTransform.up;
}
var to = teleportPosition.xzy.normalized;
rotation = Quaternion.FromToRotation(from, to);
}
else
{
var oldUp = vessel.orbit.pos.xzy.normalized;
var newUp = teleportPosition.xzy.normalized;
rotation = Quaternion.FromToRotation(oldUp, newUp) * vessel.vesselTransform.rotation;
}
var orbit = Telemagic.Clone(vessel.orbitDriver.orbit);
orbit.UpdateFromStateVectors(teleportPosition, teleportVelocity, Body, Planetarium.GetUniversalTime());
Telemagic.SetOrbit(vessel, orbit);
vessel.SetRotation(rotation);
if (teleportedToLandingAlt)
{
AlreadyTeleported = true;
Telemagic.logTM(" :FINISHED TELEPORTING:");
}
}
}
// Compensating torque at different origin.
public Vector3d TorqueAt(Vector3d origin)
{
return(torque - Vector3d.Cross(origin, force));
}
/// <summary>
/// Runs a timer. If the refresh time is exceed updates all vessel data and
/// triggers a VesselDataInvalidated event.
/// </summary>
public void UpdateVesselData()
{
if (FlightGlobals.ActiveVessel != null)
{
bool vesselChanged = false;
if (_activeVessel == null || _activeVessel.id != FlightGlobals.ActiveVessel.id)
{
vesselChanged = true;
}
if (vesselChanged)
{
_activeVessel = FlightGlobals.ActiveVessel;
_vData.vesselSync++;
if (_vData.vesselSync == 0)
{
_vData.vesselSync++;
}
_vesselSync = _vData.vesselSync;
}
_theTime = Time.unscaledTime;
if ((_theTime - _lastUpdate) * 1000 > _refreshrate)
{
IOResource TempR = new IOResource();
Vessel ActiveVessel = FlightGlobals.ActiveVessel;
_lastUpdate = _theTime;
List <Part> ActiveEngines = new List <Part>();
ActiveEngines = GetListOfActivatedEngines(ActiveVessel);
_vData.AP = (float)ActiveVessel.orbit.ApA;
_vData.PE = (float)ActiveVessel.orbit.PeA;
_vData.SemiMajorAxis = (float)ActiveVessel.orbit.semiMajorAxis;
_vData.SemiMinorAxis = (float)ActiveVessel.orbit.semiMinorAxis;
_vData.e = (float)ActiveVessel.orbit.eccentricity;
_vData.inc = (float)ActiveVessel.orbit.inclination;
_vData.VVI = (float)ActiveVessel.verticalSpeed;
_vData.G = (float)ActiveVessel.geeForce;
_vData.TAp = (int)Math.Round(ActiveVessel.orbit.timeToAp);
_vData.TPe = (int)Math.Round(ActiveVessel.orbit.timeToPe);
_vData.Density = (float)ActiveVessel.atmDensity;
_vData.TrueAnomaly = (float)ActiveVessel.orbit.trueAnomaly;
_vData.period = (int)Math.Round(ActiveVessel.orbit.period);
double ASL = ActiveVessel.mainBody.GetAltitude(ActiveVessel.CoM);
double AGL = (ASL - ActiveVessel.terrainAltitude);
if (AGL < ASL)
{
_vData.RAlt = (float)AGL;
}
else
{
_vData.RAlt = (float)ASL;
}
_vData.Alt = (float)ASL;
_vData.Vsurf = (float)ActiveVessel.srfSpeed;
_vData.Lat = (float)ActiveVessel.latitude;
_vData.Lon = (float)ActiveVessel.longitude;
TempR = GetResourceTotal(ActiveVessel, "LiquidFuel");
_vData.LiquidFuelTot = TempR.Max;
_vData.LiquidFuel = TempR.Current;
_vData.LiquidFuelTotS = (float)ProspectForResourceMax("LiquidFuel", ActiveEngines);
_vData.LiquidFuelS = (float)ProspectForResource("LiquidFuel", ActiveEngines);
TempR = GetResourceTotal(ActiveVessel, "Oxidizer");
_vData.OxidizerTot = TempR.Max;
_vData.Oxidizer = TempR.Current;
_vData.OxidizerTotS = (float)ProspectForResourceMax("Oxidizer", ActiveEngines);
_vData.OxidizerS = (float)ProspectForResource("Oxidizer", ActiveEngines);
TempR = GetResourceTotal(ActiveVessel, "ElectricCharge");
_vData.EChargeTot = TempR.Max;
_vData.ECharge = TempR.Current;
TempR = GetResourceTotal(ActiveVessel, "MonoPropellant");
_vData.MonoPropTot = TempR.Max;
_vData.MonoProp = TempR.Current;
TempR = GetResourceTotal(ActiveVessel, "IntakeAir");
_vData.IntakeAirTot = TempR.Max;
_vData.IntakeAir = TempR.Current;
TempR = GetResourceTotal(ActiveVessel, "SolidFuel");
_vData.SolidFuelTot = TempR.Max;
_vData.SolidFuel = TempR.Current;
TempR = GetResourceTotal(ActiveVessel, "XenonGas");
_vData.XenonGasTot = TempR.Max;
_vData.XenonGas = TempR.Current;
_missionTime = ActiveVessel.missionTime;
_deltaT = _missionTime - _missionTimeOld;
_missionTimeOld = _missionTime;
_vData.MissionTime = (UInt32)Math.Round(_missionTime);
_vData.deltaTime = (float)_deltaT;
_vData.VOrbit = (float)ActiveVessel.orbit.GetVel().magnitude;
Vector3d CoM, north, up, east;
Quaternion rotationSurface;
CoM = ActiveVessel.CoM;
up = (CoM - ActiveVessel.mainBody.position).normalized;
north = Vector3d.Exclude(up, (ActiveVessel.mainBody.position + ActiveVessel.mainBody.transform.up * (float)ActiveVessel.mainBody.Radius) - CoM).normalized;
east = Vector3d.Cross(up, north);
rotationSurface = Quaternion.LookRotation(north, up);
Vector3d attitude = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(ActiveVessel.GetTransform().rotation) * rotationSurface).eulerAngles;
_vData.Roll = ToScaledUInt((attitude.z > 180) ? (attitude.z - 360.0) : attitude.z);
_vData.Pitch = ToScaledUInt((attitude.x > 180) ? (attitude.x - 360.0) : attitude.x);
_vData.Heading = ToScaledUInt((attitude.y > 180) ? (attitude.y - 360.0) : attitude.y);
Vector3d prograde = new Vector3d(0, 0, 0);
switch (FlightGlobals.speedDisplayMode)
{
case FlightGlobals.SpeedDisplayModes.Surface:
prograde = ActiveVessel.srf_velocity.normalized;
break;
case FlightGlobals.SpeedDisplayModes.Orbit:
prograde = ActiveVessel.obt_velocity.normalized;
break;
case FlightGlobals.SpeedDisplayModes.Target:
prograde = FlightGlobals.ship_tgtVelocity;
break;
}
NavHeading zeroHeading; zeroHeading.Pitch = zeroHeading.Heading = 0;
NavHeading Prograde = WorldVecToNavHeading(up, north, east, prograde), Target = zeroHeading, Maneuver = zeroHeading;
_vData.ProgradeHeading = ToScaledUInt(Prograde.Heading);
_vData.ProgradePitch = ToScaledUInt(Prograde.Pitch);
if (TargetExists())
{
_vData.TargetDist = (float)Vector3.Distance(FlightGlobals.fetch.VesselTarget.GetVessel().transform.position, ActiveVessel.transform.position);
_vData.TargetV = (float)FlightGlobals.ship_tgtVelocity.magnitude;
Target = WorldVecToNavHeading(up, north, east, ActiveVessel.targetObject.GetTransform().position - ActiveVessel.transform.position);
}
_vData.TargetHeading = ToScaledUInt(Target.Heading);
_vData.TargetPitch = ToScaledUInt(Target.Pitch);
_vData.NormalHeading = ToScaledUInt(WorldVecToNavHeading(up, north, east, Vector3d.Cross(ActiveVessel.obt_velocity.normalized, up)).Heading);
_vData.MNTime = 0;
_vData.MNDeltaV = 0;
if (ActiveVessel.patchedConicSolver != null)
{
if (ActiveVessel.patchedConicSolver.maneuverNodes != null)
{
if (ActiveVessel.patchedConicSolver.maneuverNodes.Count > 0)
{
_vData.MNTime = (UInt32)Math.Round(ActiveVessel.patchedConicSolver.maneuverNodes[0].UT - Planetarium.GetUniversalTime());
_vData.MNDeltaV = (float)ActiveVessel.patchedConicSolver.maneuverNodes[0].GetBurnVector(ActiveVessel.patchedConicSolver.maneuverNodes[0].patch).magnitude; //Added JS
Maneuver = WorldVecToNavHeading(up, north, east, ActiveVessel.patchedConicSolver.maneuverNodes[0].GetBurnVector(ActiveVessel.patchedConicSolver.maneuverNodes[0].patch));
}
}
}
_vData.ManeuverHeading = ToScaledUInt(Maneuver.Heading);
_vData.ManeuverPitch = ToScaledUInt(Maneuver.Pitch);
ControlStatus((int)EnumAG.SAS, ActiveVessel.ActionGroups[KSPActionGroup.SAS]);
ControlStatus((int)EnumAG.RCS, ActiveVessel.ActionGroups[KSPActionGroup.RCS]);
ControlStatus((int)EnumAG.Light, ActiveVessel.ActionGroups[KSPActionGroup.Light]);
ControlStatus((int)EnumAG.Gear, ActiveVessel.ActionGroups[KSPActionGroup.Gear]);
ControlStatus((int)EnumAG.Brakes, ActiveVessel.ActionGroups[KSPActionGroup.Brakes]);
ControlStatus((int)EnumAG.Abort, ActiveVessel.ActionGroups[KSPActionGroup.Abort]);
ControlStatus((int)EnumAG.Custom01, ActiveVessel.ActionGroups[KSPActionGroup.Custom01]);
ControlStatus((int)EnumAG.Custom02, ActiveVessel.ActionGroups[KSPActionGroup.Custom02]);
ControlStatus((int)EnumAG.Custom03, ActiveVessel.ActionGroups[KSPActionGroup.Custom03]);
ControlStatus((int)EnumAG.Custom04, ActiveVessel.ActionGroups[KSPActionGroup.Custom04]);
ControlStatus((int)EnumAG.Custom05, ActiveVessel.ActionGroups[KSPActionGroup.Custom05]);
ControlStatus((int)EnumAG.Custom06, ActiveVessel.ActionGroups[KSPActionGroup.Custom06]);
ControlStatus((int)EnumAG.Custom07, ActiveVessel.ActionGroups[KSPActionGroup.Custom07]);
ControlStatus((int)EnumAG.Custom08, ActiveVessel.ActionGroups[KSPActionGroup.Custom08]);
ControlStatus((int)EnumAG.Custom09, ActiveVessel.ActionGroups[KSPActionGroup.Custom09]);
ControlStatus((int)EnumAG.Custom10, ActiveVessel.ActionGroups[KSPActionGroup.Custom10]);
if (ActiveVessel.orbit.referenceBody != null)
{
_vData.SOINumber = GetSOINumber(ActiveVessel.orbit.referenceBody.name);
}
_vData.MaxOverHeat = GetMaxOverHeat(ActiveVessel);
_vData.MachNumber = (float)ActiveVessel.mach;
_vData.IAS = (float)ActiveVessel.indicatedAirSpeed;
_vData.CurrentStage = (byte)StageManager.CurrentStage;
_vData.TotalStage = (byte)StageManager.StageCount;
_vData.NavballSASMode = (byte)(((int)FlightGlobals.speedDisplayMode + 1) << 4); //get navball speed display mode
if (ActiveVessel.ActionGroups[KSPActionGroup.SAS])
{
_vData.NavballSASMode = (byte)(((int)FlightGlobals.ActiveVessel.Autopilot.Mode + 1) | _vData.NavballSASMode);
}
//target distance and velocity stuff
_vData.TargetDist = 0;
_vData.TargetV = 0;
if (TargetExists())
{
_vData.TargetDist = (float)Vector3.Distance(FlightGlobals.fetch.VesselTarget.GetVessel().transform.position, ActiveVessel.transform.position);
_vData.TargetV = (float)FlightGlobals.ship_tgtVelocity.magnitude;
}
_vData.NavballSASMode = (byte)(((int)FlightGlobals.speedDisplayMode + 1) << 4); //get navball speed display mode
if (ActiveVessel.ActionGroups[KSPActionGroup.SAS])
{
_vData.NavballSASMode = (byte)(((int)FlightGlobals.ActiveVessel.Autopilot.Mode + 1) | _vData.NavballSASMode);
}
//Notify listeners
NotifyInvalidate(_vData);
}
}
}
void update_formation_info()
{
tVSL = CFG.Target.GetVessel();
if (tVSL == null)
{
reset_formation();
CanManeuver = false;
return;
}
if (tPN == null || !tPN.Valid)
{
tTCA = ModuleTCA.EnabledTCA(tVSL);
tPN = tTCA != null?tTCA.GetModule <PointNavigator>() : null;
}
var only_count = false;
if (tVSL.srf_velocity.sqrMagnitude < C.FormationSpeedSqr)
{
reset_formation(); CanManeuver = false; only_count = true;
}
//update followers
var offset = 0f;
var can_maneuver = true;
all_followers.Clear();
for (int i = 0, num_vessels = FlightGlobals.Vessels.Count; i < num_vessels; i++)
{
var v = FlightGlobals.Vessels[i];
if (v == null || v.packed || !v.loaded)
{
continue;
}
var tca = ModuleTCA.EnabledTCA(v);
if (tca != null &&
(tca.vessel == VSL.vessel ||
tca.CFG.Nav[Navigation.FollowTarget] &&
tca.CFG.Target.GetTarget() != null &&
tca.CFG.Target.GetTarget() == CFG.Target.GetTarget()))
{
var vPN = tca.GetModule <PointNavigator>();
if (vPN == null)
{
continue;
}
all_followers.Add(v.id, vPN);
if (offset < vPN.VSL.Geometry.R)
{
offset = vPN.VSL.Geometry.R;
}
if (v.id != VSL.vessel.id)
{
can_maneuver &= !vPN.Maneuvering ||
(Maneuvering && VSL.vessel.id.CompareTo(v.id) > 0);
}
}
}
if (only_count)
{
return;
}
CanManeuver = can_maneuver;
var follower_index = all_followers.IndexOfKey(VSL.vessel.id);
if (follower_index == 0)
{
var forward = tVSL == null? Vector3d.zero : -tVSL.srf_velocity.normalized;
var side = Vector3d.Cross(VSL.Physics.Up, forward).normalized;
var num_offsets = all_followers.Count + (all_followers.Count % 2);
offset *= 2;
var target_size = tTCA != null? tTCA.VSL.Geometry.D : Utils.ClampL(Math.Pow(tVSL.totalMass, 1 / 3f), 1);
if (offset < target_size)
{
offset = (float)target_size;
}
offset *= C.MinDistance;
if (Formation == null || Formation.Count != num_offsets || FormationUpdateTimer.TimePassed)
{
FormationUpdateTimer.Reset();
Formation = new List <FormationNode>(num_offsets);
for (int i = 0; i < num_offsets; i++)
{
Formation.Add(new FormationNode(tVSL, i, forward, side, offset));
}
all_followers.ForEach(p => p.Value.UpdateFormation(Formation));
}
else
{
for (int i = 0; i < num_offsets; i++)
{
Formation[i].Update(forward, side, offset);
}
}
}
keep_formation = Formation != null;
if (Formation == null || fnode != null)
{
return;
}
//compute follower offset
var min_d = -1f;
var min_off = 0;
for (int i = 0; i < Formation.Count; i++)
{
var node = Formation[i];
if (node.Follower != null)
{
continue;
}
var d = node.Distance(VSL.vessel);
if (min_d < 0 || min_d > d)
{
min_d = d;
min_off = i;
}
}
Formation[min_off].Follower = VSL.vessel;
fnode = Formation[min_off];
}
// Analysis disable once UnusedParameter
public bool RenderOrbit(RenderTexture screen, float cameraAspect)
{
if (!startupComplete)
{
JUtil.AnnoyUser(this);
}
// Make sure the parameters fit on the screen.
Vector4 displayPosition = orbitDisplayPosition;
displayPosition.z = Mathf.Min(screen.width - displayPosition.x, displayPosition.z);
displayPosition.w = Mathf.Min(screen.height - displayPosition.y, displayPosition.w);
// Here is our pixel budget in each direction:
double horizPixelSize = displayPosition.z - iconPixelSize;
double vertPixelSize = displayPosition.w - iconPixelSize;
// Find a basis for transforming values into the framework of
// vessel.orbit. The rendering framework assumes the periapsis
// is drawn directly to the right of the mainBody center of mass.
// It assumes the orbit's prograde direction is "up" (screen
// relative) at the periapsis, providing a counter-clockwise
// motion for vessel.
// Once we have the basic transform, we will add in scalars
// that will ultimately transform an arbitrary point (relative to
// the planet's center) into screen space.
Matrix4x4 screenTransform = Matrix4x4.identity;
double now = Planetarium.GetUniversalTime();
double timeAtPe = vessel.orbit.NextPeriapsisTime(now);
// Get the 3 direction vectors, based on Pe being on the right of the screen
// OrbitExtensions provides handy utilities to get these.
Vector3d right = vessel.orbit.Up(timeAtPe);
Vector3d forward = vessel.orbit.SwappedOrbitNormal();
// MOARdV: OrbitExtensions.Horizontal is unstable. I've seen it
// become (0, 0, 0) intermittently in flight. Instead, use the
// cross product of the other two.
// We flip the sign of this vector because we are using an inverted
// y coordinate system to keep the icons right-side up.
Vector3d up = -Vector3d.Cross(forward, right);
//Vector3d up = -vessel.orbit.Horizontal(timeAtPe);
screenTransform.SetRow(0, new Vector4d(right.x, right.y, right.z, 0.0));
screenTransform.SetRow(1, new Vector4d(up.x, up.y, up.z, 0.0));
screenTransform.SetRow(2, new Vector4d(forward.x, forward.y, forward.z, 0.0));
// Figure out our bounds. First, make sure the entire planet
// fits on the screen. We define the center of the vessel.mainBody
// as the origin of our coodinate system.
double maxX = vessel.mainBody.Radius;
double minX = -maxX;
double maxY = maxX;
double minY = -maxX;
if (vessel.mainBody.atmosphere)
{
maxX += vessel.mainBody.maxAtmosphereAltitude;
minX = -maxX;
maxY = maxX;
minY = -maxX;
}
// Now make sure the entire orbit fits on the screen.
Vector3 vesselPos;
// The PeR, ApR, and semiMinorAxis are all one dimensional, so we
// can just apply them directly to these values.
maxX = Math.Max(maxX, vessel.orbit.PeR);
if (vessel.orbit.eccentricity < 1.0)
{
minX = Math.Min(minX, -vessel.orbit.ApR);
maxY = Math.Max(maxY, vessel.orbit.semiMinorAxis);
minY = Math.Min(minY, -vessel.orbit.semiMinorAxis);
}
else if (vessel.orbit.EndUT > 0.0)
{
// If we're hyperbolic, let's get the SoI transition
vesselPos = screenTransform.MultiplyPoint3x4(vessel.orbit.SwappedRelativePositionAtUT(vessel.orbit.EndUT));
maxX = Math.Max(maxX, vesselPos.x);
minX = Math.Min(minX, vesselPos.x);
maxY = Math.Max(maxY, vesselPos.y);
minY = Math.Min(minY, vesselPos.y);
}
// Make sure the vessel shows up on-screen. Since a hyperbolic
// orbit doesn't have a meaningful ApR, we use this as a proxy for
// how far we need to extend the bounds to show the vessel.
vesselPos = screenTransform.MultiplyPoint3x4(vessel.orbit.SwappedRelativePositionAtUT(now));
maxX = Math.Max(maxX, vesselPos.x);
minX = Math.Min(minX, vesselPos.x);
maxY = Math.Max(maxY, vesselPos.y);
minY = Math.Min(minY, vesselPos.y);
// Account for a target vessel
var targetBody = FlightGlobals.fetch.VesselTarget as CelestialBody;
var targetVessel = FlightGlobals.fetch.VesselTarget as Vessel;
if (targetVessel != null)
{
if (targetVessel.mainBody == vessel.mainBody)
{
double tgtPe = targetVessel.orbit.NextPeriapsisTime(now);
vesselPos = screenTransform.MultiplyPoint3x4(targetVessel.orbit.SwappedRelativePositionAtUT(tgtPe));
maxX = Math.Max(maxX, vesselPos.x);
minX = Math.Min(minX, vesselPos.x);
maxY = Math.Max(maxY, vesselPos.y);
minY = Math.Min(minY, vesselPos.y);
if (targetVessel.orbit.eccentricity < 1.0)
{
vesselPos = screenTransform.MultiplyPoint3x4(targetVessel.orbit.SwappedRelativePositionAtUT(targetVessel.orbit.NextApoapsisTime(now)));
maxX = Math.Max(maxX, vesselPos.x);
minX = Math.Min(minX, vesselPos.x);
maxY = Math.Max(maxY, vesselPos.y);
minY = Math.Min(minY, vesselPos.y);
}
vesselPos = screenTransform.MultiplyPoint3x4(targetVessel.orbit.SwappedRelativePositionAtUT(now));
maxX = Math.Max(maxX, vesselPos.x);
minX = Math.Min(minX, vesselPos.x);
maxY = Math.Max(maxY, vesselPos.y);
minY = Math.Min(minY, vesselPos.y);
}
else
{
// We only care about tgtVessel if it is in the same SoI.
targetVessel = null;
}
}
if (targetBody != null)
{
// Validate some values up front, so we don't need to test them later.
if (targetBody.GetOrbit() == null)
{
targetBody = null;
}
else if (targetBody.orbit.referenceBody == vessel.orbit.referenceBody)
{
// If the target body orbits our current world, let's at
// least make sure the body's location is visible.
vesselPos = screenTransform.MultiplyPoint3x4(targetBody.GetOrbit().SwappedRelativePositionAtUT(now));
maxX = Math.Max(maxX, vesselPos.x);
minX = Math.Min(minX, vesselPos.x);
maxY = Math.Max(maxY, vesselPos.y);
minY = Math.Min(minY, vesselPos.y);
}
}
ManeuverNode node = (vessel.patchedConicSolver.maneuverNodes.Count > 0) ? vessel.patchedConicSolver.maneuverNodes[0] : null;
if (node != null)
{
double nodePe = node.nextPatch.NextPeriapsisTime(now);
vesselPos = screenTransform.MultiplyPoint3x4(node.nextPatch.SwappedRelativePositionAtUT(nodePe));
maxX = Math.Max(maxX, vesselPos.x);
minX = Math.Min(minX, vesselPos.x);
maxY = Math.Max(maxY, vesselPos.y);
minY = Math.Min(minY, vesselPos.y);
if (node.nextPatch.eccentricity < 1.0)
{
double nodeAp = node.nextPatch.NextApoapsisTime(now);
vesselPos = screenTransform.MultiplyPoint3x4(node.nextPatch.SwappedRelativePositionAtUT(nodeAp));
maxX = Math.Max(maxX, vesselPos.x);
minX = Math.Min(minX, vesselPos.x);
maxY = Math.Max(maxY, vesselPos.y);
minY = Math.Min(minY, vesselPos.y);
}
else if (node.nextPatch.EndUT > 0.0)
{
// If the next patch is hyperbolic, include the endpoint.
vesselPos = screenTransform.MultiplyPoint3x4(vessel.orbit.SwappedRelativePositionAtUT(node.nextPatch.EndUT));
maxX = Math.Max(maxX, vesselPos.x);
minX = Math.Min(minX, vesselPos.x);
maxY = Math.Max(maxY, vesselPos.y);
minY = Math.Min(minY, vesselPos.y);
}
}
// Add translation. This will ensure that all of the features
// under consideration above will be displayed.
screenTransform[0, 3] = -0.5f * (float)(maxX + minX);
screenTransform[1, 3] = -0.5f * (float)(maxY + minY);
double neededWidth = maxX - minX;
double neededHeight = maxY - minY;
// Pick a scalar that will fit the bounding box we just created.
float pixelScalar = (float)Math.Min(horizPixelSize / neededWidth, vertPixelSize / neededHeight);
screenTransform = Matrix4x4.Scale(new Vector3(pixelScalar, pixelScalar, pixelScalar)) * screenTransform;
GL.Clear(true, true, backgroundColorValue);
GL.PushMatrix();
GL.LoadPixelMatrix(-displayPosition.z * 0.5f, displayPosition.z * 0.5f, displayPosition.w * 0.5f, -displayPosition.w * 0.5f);
GL.Viewport(new Rect(displayPosition.x, screen.height - displayPosition.y - displayPosition.w, displayPosition.z, displayPosition.w));
lineMaterial.SetPass(0);
GL.Begin(GL.LINES);
// Draw the planet:
Vector3 focusCenter = screenTransform.MultiplyPoint3x4(new Vector3(0.0f, 0.0f, 0.0f));
// orbitDriver is null on the sun, so we'll just use white instead.
GL.Color((vessel.mainBody.orbitDriver == null) ? new Color(1.0f, 1.0f, 1.0f) : vessel.mainBody.orbitDriver.orbitColor);
DrawCircle(focusCenter.x, focusCenter.y, (float)(vessel.mainBody.Radius * pixelScalar), orbitPoints);
if (vessel.mainBody.atmosphere)
{
// Use the atmospheric ambient to color the atmosphere circle.
GL.Color(vessel.mainBody.atmosphericAmbientColor);
DrawCircle(focusCenter.x, focusCenter.y, (float)((vessel.mainBody.Radius + vessel.mainBody.maxAtmosphereAltitude) * pixelScalar), orbitPoints);
}
if (targetVessel != null)
{
GL.Color(iconColorTargetValue);
if (!targetVessel.orbit.activePatch && targetVessel.orbit.eccentricity < 1.0 && targetVessel.orbit.referenceBody == vessel.orbit.referenceBody)
{
// For some reason, activePatch is false for targetVessel.
// If we have a stable orbit for the target, use a fallback
// rendering method:
ReallyDrawOrbit(targetVessel.orbit, vessel.orbit.referenceBody, screenTransform, orbitPoints);
}
else
{
DrawOrbit(targetVessel.orbit, vessel.orbit.referenceBody, screenTransform, orbitPoints);
}
}
foreach (CelestialBody moon in vessel.orbit.referenceBody.orbitingBodies)
{
if (moon != targetBody)
{
GL.Color(moon.orbitDriver.orbitColor);
ReallyDrawOrbit(moon.GetOrbit(), vessel.orbit.referenceBody, screenTransform, orbitPoints);
}
}
if (targetBody != null)
{
GL.Color(iconColorTargetValue);
ReallyDrawOrbit(targetBody.GetOrbit(), vessel.orbit.referenceBody, screenTransform, orbitPoints);
}
if (node != null)
{
GL.Color(orbitColorNextNodeValue);
DrawOrbit(node.nextPatch, vessel.orbit.referenceBody, screenTransform, orbitPoints);
}
if (vessel.orbit.nextPatch != null && vessel.orbit.nextPatch.activePatch)
{
GL.Color(orbitColorNextNodeValue);
DrawOrbit(vessel.orbit.nextPatch, vessel.orbit.referenceBody, screenTransform, orbitPoints);
}
// Draw the vessel orbit
GL.Color(orbitColorSelfValue);
DrawOrbit(vessel.orbit, vessel.orbit.referenceBody, screenTransform, orbitPoints);
// Done drawing lines.
GL.End();
// Draw target vessel icons.
Vector3 transformedPosition;
foreach (CelestialBody moon in vessel.orbit.referenceBody.orbitingBodies)
{
if (moon != targetBody)
{
transformedPosition = screenTransform.MultiplyPoint3x4(moon.getTruePositionAtUT(now) - vessel.orbit.referenceBody.getTruePositionAtUT(now));
DrawIcon(transformedPosition.x, transformedPosition.y, VesselType.Unknown, moon.orbitDriver.orbitColor, MapIcons.OtherIcon.PLANET);
}
}
if (targetVessel != null || targetBody != null)
{
var orbit = (targetVessel != null) ? targetVessel.GetOrbit() : targetBody.GetOrbit();
DrawNextPe(orbit, vessel.orbit.referenceBody, now, iconColorTargetValue, screenTransform);
DrawNextAp(orbit, vessel.orbit.referenceBody, now, iconColorTargetValue, screenTransform);
if (targetBody != null)
{
transformedPosition = screenTransform.MultiplyPoint3x4(targetBody.getTruePositionAtUT(now) - vessel.orbit.referenceBody.getTruePositionAtUT(now));
DrawIcon(transformedPosition.x, transformedPosition.y, VesselType.Unknown, iconColorTargetValue, MapIcons.OtherIcon.PLANET);
}
else
{
transformedPosition = screenTransform.MultiplyPoint3x4(orbit.SwappedRelativePositionAtUT(now));
DrawIcon(transformedPosition.x, transformedPosition.y, targetVessel.vesselType, iconColorTargetValue);
}
if (vessel.orbit.AscendingNodeExists(orbit))
{
double anTime = vessel.orbit.TimeOfAscendingNode(orbit, now);
if (anTime < vessel.orbit.EndUT || (vessel.orbit.patchEndTransition != Orbit.PatchTransitionType.ESCAPE && vessel.orbit.patchEndTransition != Orbit.PatchTransitionType.ENCOUNTER))
{
transformedPosition = screenTransform.MultiplyPoint3x4(vessel.orbit.SwappedRelativePositionAtUT(anTime));
DrawIcon(transformedPosition.x, transformedPosition.y, VesselType.Unknown, orbitColorSelfValue, MapIcons.OtherIcon.AN);
}
}
if (vessel.orbit.DescendingNodeExists(orbit))
{
double dnTime = vessel.orbit.TimeOfDescendingNode(orbit, now);
if (dnTime < vessel.orbit.EndUT || (vessel.orbit.patchEndTransition != Orbit.PatchTransitionType.ESCAPE && vessel.orbit.patchEndTransition != Orbit.PatchTransitionType.ENCOUNTER))
{
transformedPosition = screenTransform.MultiplyPoint3x4(vessel.orbit.SwappedRelativePositionAtUT(dnTime));
DrawIcon(transformedPosition.x, transformedPosition.y, VesselType.Unknown, orbitColorSelfValue, MapIcons.OtherIcon.DN);
}
}
double tClosestApproach;
double dClosestApproach = JUtil.GetClosestApproach(vessel.orbit, orbit, out tClosestApproach);
Orbit o = GetPatchAtUT(vessel.orbit, tClosestApproach);
if (o != null)
{
Vector3d encounterPosition = o.SwappedRelativePositionAtUT(tClosestApproach) + o.referenceBody.getTruePositionAtUT(tClosestApproach) - vessel.orbit.referenceBody.getTruePositionAtUT(tClosestApproach);
transformedPosition = screenTransform.MultiplyPoint3x4(encounterPosition);
DrawIcon(transformedPosition.x, transformedPosition.y, VesselType.Unknown, iconColorClosestApproachValue, MapIcons.OtherIcon.SHIPATINTERCEPT);
}
// Unconditionally try to draw the closest approach point on
// the target orbit.
transformedPosition = screenTransform.MultiplyPoint3x4(orbit.SwappedRelativePositionAtUT(tClosestApproach));
DrawIcon(transformedPosition.x, transformedPosition.y, VesselType.Unknown, iconColorClosestApproachValue, MapIcons.OtherIcon.TGTATINTERCEPT);
}
else
{
if (vessel.orbit.AscendingNodeEquatorialExists())
{
double anTime = vessel.orbit.TimeOfAscendingNodeEquatorial(now);
if (anTime < vessel.orbit.EndUT || (vessel.orbit.patchEndTransition != Orbit.PatchTransitionType.ESCAPE && vessel.orbit.patchEndTransition != Orbit.PatchTransitionType.ENCOUNTER))
{
transformedPosition = screenTransform.MultiplyPoint3x4(vessel.orbit.SwappedRelativePositionAtUT(anTime));
DrawIcon(transformedPosition.x, transformedPosition.y, VesselType.Unknown, orbitColorSelfValue, MapIcons.OtherIcon.AN);
}
}
if (vessel.orbit.DescendingNodeEquatorialExists())
{
double dnTime = vessel.orbit.TimeOfDescendingNodeEquatorial(now);
if (dnTime < vessel.orbit.EndUT || (vessel.orbit.patchEndTransition != Orbit.PatchTransitionType.ESCAPE && vessel.orbit.patchEndTransition != Orbit.PatchTransitionType.ENCOUNTER))
{
transformedPosition = screenTransform.MultiplyPoint3x4(vessel.orbit.SwappedRelativePositionAtUT(dnTime));
DrawIcon(transformedPosition.x, transformedPosition.y, VesselType.Unknown, orbitColorSelfValue, MapIcons.OtherIcon.DN);
}
}
}
// Draw orbital features
DrawNextPe(vessel.orbit, vessel.orbit.referenceBody, now, iconColorPEValue, screenTransform);
DrawNextAp(vessel.orbit, vessel.orbit.referenceBody, now, iconColorAPValue, screenTransform);
if (vessel.orbit.nextPatch != null && vessel.orbit.nextPatch.activePatch)
{
transformedPosition = screenTransform.MultiplyPoint3x4(vessel.orbit.SwappedRelativePositionAtUT(vessel.orbit.EndUT));
DrawIcon(transformedPosition.x, transformedPosition.y, VesselType.Unknown, orbitColorSelfValue, MapIcons.OtherIcon.EXITSOI);
Orbit nextPatch = vessel.orbit.nextPatch.nextPatch;
if (nextPatch != null && nextPatch.activePatch)
{
transformedPosition = screenTransform.MultiplyPoint3x4(nextPatch.SwappedRelativePositionAtUT(nextPatch.EndUT) + nextPatch.referenceBody.getTruePositionAtUT(nextPatch.EndUT) - vessel.orbit.referenceBody.getTruePositionAtUT(nextPatch.EndUT));
DrawIcon(transformedPosition.x, transformedPosition.y, VesselType.Unknown, orbitColorNextNodeValue, MapIcons.OtherIcon.EXITSOI);
}
}
if (node != null && node.nextPatch.activePatch)
{
DrawNextPe(node.nextPatch, vessel.orbit.referenceBody, now, orbitColorNextNodeValue, screenTransform);
DrawNextAp(node.nextPatch, vessel.orbit.referenceBody, now, orbitColorNextNodeValue, screenTransform);
transformedPosition = screenTransform.MultiplyPoint3x4(vessel.orbit.SwappedRelativePositionAtUT(node.UT));
DrawIcon(transformedPosition.x, transformedPosition.y, VesselType.Unknown, orbitColorNextNodeValue, MapIcons.OtherIcon.NODE);
}
// Draw ownship icon
transformedPosition = screenTransform.MultiplyPoint3x4(vessel.orbit.SwappedRelativePositionAtUT(now));
DrawIcon(transformedPosition.x, transformedPosition.y, vessel.vesselType, iconColorSelfValue);
GL.PopMatrix();
GL.Viewport(new Rect(0, 0, screen.width, screen.height));
return(true);
}
public static ManeuverParameters ComputeEjectionManeuver(Vector3d exit_velocity, Orbit initial_orbit, double UT_0)
{
double GM = initial_orbit.referenceBody.gravParameter;
double C3 = exit_velocity.sqrMagnitude;
double Mh = initial_orbit.referenceBody.sphereOfInfluence;
double Rpe = initial_orbit.semiMajorAxis;
double isma = 2 / Mh - C3 / GM; //inverted Semi-major Axis, will work for parabolic orbit
double ecc = 1.0 - Rpe * isma;
//double vstart = Math.Sqrt(GM * (2 / Rpe - isma)); //{ total start boost}
//double slat = Rpe * Rpe * vstart * vstart / GM;
//the problem here should be R for circular orbit instead of Rpe
double slat = 2 * Rpe - isma * Rpe * Rpe; //but we don't know start point (yet) in elliptic orbit
double theta = Math.Acos((slat / Mh - 1) / ecc);
/*
* //old way infinity hyperbolic:
* //problems: it's not necessary hyperbolic (in case of low speed exit_velocity),
* //and exit_velocity appears not infinite far from celestial body, but only sphereOfInfluence far
* //i.e. Mh in previous statements(theta, isma) is not infinity!
*
* double sma = -GM / C3;
*
* double ecc = 1 - Rpe / sma;
* double theta = Math.Acos(-1 / ecc);
*
*/
Vector3d intersect_1;
Vector3d intersect_2;
// intersect_1 is the optimal position on the orbit assuming the orbit is circular and the sphere of influence is infinite
IntersectConePlane(exit_velocity, theta, initial_orbit.h, out intersect_1, out intersect_2);
Vector3d eccvec = initial_orbit.GetEccVector();
double true_anomaly = AngleAboutAxis(eccvec, intersect_1, initial_orbit.h);
// GetMeanAnomalyAtTrueAnomaly expects degrees and returns radians
double mean_anomaly = initial_orbit.GetMeanAnomalyAtTrueAnomaly(true_anomaly * UtilMath.Rad2Deg);
double delta_mean_anomaly = MuUtils.ClampRadiansPi(mean_anomaly - initial_orbit.MeanAnomalyAtUT(UT_0));
double UT = UT_0 + delta_mean_anomaly / initial_orbit.MeanMotion();
Vector3d V_0 = initial_orbit.getOrbitalVelocityAtUT(UT);
Vector3d pos = initial_orbit.getRelativePositionAtUT(UT);
double final_vel = Math.Sqrt(C3 + 2 * GM / pos.magnitude);
Vector3d x = pos.normalized;
Vector3d z = Vector3d.Cross(x, exit_velocity).normalized;
Vector3d y = Vector3d.Cross(z, x);
theta = Math.PI / 2;
double dtheta = 0.001;
Orbit sample = new Orbit();
double theta_err = Double.MaxValue;
for (int iteration = 0; iteration < 50; ++iteration)
{
Vector3d V_1 = final_vel * (Math.Cos(theta) * x + Math.Sin(theta) * y);
sample.UpdateFromStateVectors(pos, V_1, initial_orbit.referenceBody, UT);
theta_err = AngleAboutAxis(exit_velocity, sample.getOrbitalVelocityAtUT(OrbitExtensions.NextTimeOfRadius(sample, UT, sample.referenceBody.sphereOfInfluence)), z);
if (double.IsNaN(theta_err))
{
return(null);
}
if (Math.Abs(theta_err) <= 0.1 * UtilMath.Deg2Rad)
{
return(new ManeuverParameters((V_1 - V_0).xzy, UT));
}
V_1 = final_vel * (Math.Cos(theta + dtheta) * x + Math.Sin(theta + dtheta) * y);
sample.UpdateFromStateVectors(pos, V_1, initial_orbit.referenceBody, UT);
double theta_err_2 = AngleAboutAxis(exit_velocity, sample.getOrbitalVelocityAtUT(OrbitExtensions.NextTimeOfRadius(sample, UT, sample.referenceBody.sphereOfInfluence)), z);
V_1 = final_vel * (Math.Cos(theta - dtheta) * x + Math.Sin(theta - dtheta) * y);
sample.UpdateFromStateVectors(pos, V_1, initial_orbit.referenceBody, UT);
double theta_err_3 = AngleAboutAxis(exit_velocity, sample.getOrbitalVelocityAtUT(OrbitExtensions.NextTimeOfRadius(sample, UT, sample.referenceBody.sphereOfInfluence)), z);
double derr = MuUtils.ClampRadiansPi(theta_err_2 - theta_err_3) / (2 * dtheta);
theta = MuUtils.ClampRadiansTwoPi(theta - theta_err / derr);
// if theta > pi, replace with 2pi - theta, the function ejection_angle=f(theta) is symmetrc wrt theta=pi
if (theta > Math.PI)
{
theta = 2 * Math.PI - theta;
}
}
return(null);
}
public void FixedUpdate()
{
//Debug.Log ("LanderAttachment.FixedUpdate");
var vessel = GetComponent <Vessel>();
if (vessel != FlightGlobals.ActiveVessel)
{
Destroy(this);
return;
}
if (AlreadyTeleported)
{
//Debug.Log ("FixedUpdate: AlreadyTeleported");
if (vessel.LandedOrSplashed)
{
Destroy(this);
}
else
{
var accel = (vessel.srf_velocity + vessel.upAxis) * -0.5;
vessel.ChangeWorldVelocity(accel);
RateLimitedLogger.Log(_accelLogObject,
$"(Happening every frame) Soft-lander changed ship velocity this frame by vector {accel.x},{accel.y},{accel.z} (mag {accel.magnitude})");
}
}
else
{
//Debug.Log ("FixedUpdate: not AlreadyTeleported");
var pqs = Body.pqsController;
if (pqs == null)
{
Destroy(this);
return;
}
var alt = pqs.GetSurfaceHeight(Body.GetRelSurfaceNVector(Latitude, Longitude)) - Body.Radius;
alt = Math.Max(alt, 0); // Underwater!
if (TimeWarp.CurrentRateIndex != 0)
{
TimeWarp.SetRate(0, true);
Extensions.Log("Set time warp to index 0");
}
// HoldVesselUnpack is in display frames, not physics frames
//Debug.Log("alt: " + alt.ToString() + " Altitude: " + Altitude.ToString());
//Debug.Log ("Latitude: " + Latitude.ToString () + " Longitude: " + Longitude.ToString ());
//var teleportPosition = Body.GetRelSurfacePosition(Latitude, Longitude, alt +Altitude);
Vector3d teleportPosition;
if (!teleportedToLandingAlt)
{
//Debug.Log("teleportedToLandingAlt == false, intermAltitude: " + intermAltitude.ToString() + " Altitude: " + Altitude.ToString());
if (intermAltitude > Altitude)
{
if (Planetarium.GetUniversalTime() - lastUpdate >= 0.5)
{
intermAltitude = intermAltitude / 10;
//Debug.Log("Planetarium.GetUniversalTime (): " + Planetarium.GetUniversalTime ().ToString() + " lastUpdate: " + lastUpdate.ToString());
//Debug.Log("intermAltitude: " + intermAltitude.ToString());
teleportPosition = Body.GetRelSurfacePosition(Latitude, Longitude, alt + intermAltitude);
if (lastUpdate != 0)
{
intermAltitude = Altitude;
}
lastUpdate = Planetarium.GetUniversalTime();
}
else
{
//Debug.Log("teleportPositionAltitude (no time change): alt: " + alt.ToString() + " intermAltitude: " + intermAltitude.ToString());
teleportPosition = Body.GetRelSurfacePosition(Latitude, Longitude, alt + intermAltitude);
}
}
else
{
//Debug.Log("teleportedToLandingAlt set to true");
teleportedToLandingAlt = true;
teleportPosition = Body.GetRelSurfacePosition(Latitude, Longitude, alt + Altitude);
}
}
else
{
teleportPosition = Body.GetRelSurfacePosition(Latitude, Longitude, alt + Altitude);
}
var teleportVelocity = Vector3d.Cross(Body.angularVelocity, teleportPosition);
//var teleportVelocity = Vector3d.Cross(Vector3d.down, teleportPosition.normalized)*
// (Math.Cos(L atitude*(Math.PI/180))*teleportPosition.magnitude*
// (Math.PI*2)/(Body.rotationPeriod));
// convert from world space to orbit space
teleportPosition = teleportPosition.xzy;
teleportVelocity = teleportVelocity.xzy;
// counter for the momentary fall when on rails (about one second)
teleportVelocity += teleportPosition.normalized * (Body.gravParameter / teleportPosition.sqrMagnitude);
Quaternion rotation;
if (SetRotation)
{
var from = Vector3d.up;
var to = teleportPosition.xzy.normalized;
rotation = Quaternion.FromToRotation(from, to);
}
else
{
var oldUp = vessel.orbit.pos.xzy.normalized;
var newUp = teleportPosition.xzy.normalized;
rotation = Quaternion.FromToRotation(oldUp, newUp) * vessel.vesselTransform.rotation;
}
var orbit = vessel.orbitDriver.orbit.Clone();
orbit.UpdateFromStateVectors(teleportPosition, teleportVelocity, Body, Planetarium.GetUniversalTime());
vessel.SetOrbit(orbit);
vessel.SetRotation(rotation);
if (teleportedToLandingAlt)
{
AlreadyTeleported = true;
}
}
}
public override List <JacobianConstraint> BuildJacobian()
{
var angularConstraints = new List <JacobianConstraint>();
IShape simulationObjectA = ShapeA;
IShape simulationObjectB = ShapeB;
double errorReduction = ErrorReductionParam;
double springCoefficientHingeAxis = SpringCoefficientHingeAxis;
double springCoefficientRotationAxis = SpringCoefficientRotationAxis;
#region Init Angular
Vector3d hingeAxis = GetHingeAxis();
Vector3d rotationAxis = GetRotationAxis();
double k = hingeAxis.Dot(rotationAxis);
Vector3d tempPerpendicular = rotationAxis - k * hingeAxis;
Vector3d t1 = hingeAxis.Cross(tempPerpendicular).Normalize();
double hingeAngle = GetAngle1(
hingeAxis,
rotationAxis,
HingeAxis,
simulationObjectA.RotationStatus,
RelativeOrientation1);
double twistAngle = GetAngle2(
hingeAxis,
rotationAxis,
RotationAxis,
simulationObjectB.RotationStatus,
RelativeOrientation2);
#endregion
#region Jacobian Constraint
double angularLimit = errorReduction * hingeAngle;
angularConstraints.Add(JacobianCommon.GetDOF(
hingeAxis,
-1.0 * hingeAxis,
simulationObjectA,
simulationObjectB,
0.0,
angularLimit,
springCoefficientHingeAxis,
0.0,
ConstraintType.Joint));
angularLimit = errorReduction * twistAngle;
angularConstraints.Add(JacobianCommon.GetDOF(
rotationAxis,
-1.0 * rotationAxis,
simulationObjectA,
simulationObjectB,
0.0,
angularLimit,
springCoefficientRotationAxis,
0.0,
ConstraintType.Joint));
#endregion
return(angularConstraints);
}
public void Init(CatOrbiter parent, float soi)
{
TimingManager.FixedUpdateAdd(TimingManager.TimingStage.Earlyish, DoForces);
orbiters.Add(this);
rb = gameObject.AddComponent <Rigidbody2D>();
rb.isKinematic = true;
if (orbiters.Count == 1)
{
sun = this;
Vector3 spos = new Vector3(Screen.width * 0.5f, Screen.height * 0.5f, -1);
transform.position = KSP.UI.UIMainCamera.Camera.ScreenToWorldPoint(spos);
Mass = 2E17;
pos.x = transform.position.x;
pos.y = transform.position.y;
}
else
{
Vector2 relativePos = Random.insideUnitCircle;
if (relativePos.magnitude < 0.2)
{
relativePos = relativePos.normalized * 0.3f;
}
Vector3 spos = UIMainCamera.Camera.WorldToScreenPoint(parent.transform.position) + (Vector3)(relativePos * soi);
spos.z = -1;
transform.position = UIMainCamera.Camera.ScreenToWorldPoint(spos);
pos.x = transform.position.x;
pos.y = transform.position.y;
//int scaleRange = 10;
//
//float factor = (1 + (scaleRange - 1) * Random.value);
//
//scale = parent.scale * factor / scaleRange;
scale = parent.scale * 0.6f;
transform.localScale *= scale;
TrailRenderer trail = gameObject.GetComponent <TrailRenderer>();
trail.colorGradient = new Gradient()
{
alphaKeys = new GradientAlphaKey[3] {
new GradientAlphaKey(1, 0), new GradientAlphaKey(1, 0.7f), new GradientAlphaKey(0, 1)
}
};
trail.startWidth *= scale;
//trail.endWidth *= scale;
trail.widthCurve = new AnimationCurve(new Keyframe(0, trail.startWidth), new Keyframe(0.7f, trail.startWidth), new Keyframe(1, trail.startWidth * 0.9f));
//Mass = factor * 2E16;
Mass = parent.Mass * 0.025;
Vector2d dist = parent.pos - pos;
double circularVel = Math.Sqrt(G * (Mass + parent.Mass) / dist.magnitude);
if (parent == sun)
{
circularVel *= Random.Range(0.9f, 1.1f);
}
CatManager.LOG.info("CatOrbiter {0} {1} {2} {3}", circularVel.ToString("F3"), Mass.ToString("F2"), orbiters[0].Mass.ToString("F2"), dist.magnitude.ToString("F2"));
Vector3d normal = (Random.value >= 0.3) ? Vector3d.back : Vector3d.forward;
Vector3d vel3d = Vector3d.Cross(dist, normal).normalized *circularVel;
vel.x = parent.vel.x + vel3d.x;
vel.y = parent.vel.y + vel3d.y;
}
rb.MovePosition(new Vector2((float)pos.x, (float)pos.y));
}
public void GetClCdCmSteady(InstantConditionSimInput input, out InstantConditionSimOutput output, bool clear, bool reset_stall = false)
{
output = new InstantConditionSimOutput();
double area = 0;
double MAC = 0;
double b_2 = 0;
Vector3d forward = Vector3.forward;
Vector3d up = Vector3.up;
Vector3d right = Vector3.right;
Vector3d CoM = Vector3d.zero;
double mass = 0;
List <Part> partsList = EditorLogic.SortedShipList;
for (int i = 0; i < partsList.Count; i++)
{
Part p = partsList[i];
if (FARAeroUtil.IsNonphysical(p))
{
continue;
}
double partMass = p.mass;
if (p.Resources.Count > 0)
{
partMass += p.GetResourceMass();
}
//partMass += p.GetModuleMass(p.mass);
// If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass
CoM += partMass * (Vector3d)p.transform.TransformPoint(p.CoMOffset);
mass += partMass;
}
CoM /= mass;
if (EditorDriver.editorFacility == EditorFacility.VAB)
{
forward = Vector3.up;
up = -Vector3.forward;
}
double sinAlpha = Math.Sin(input.alpha * Math.PI / 180);
double cosAlpha = Math.Sqrt(Math.Max(1 - sinAlpha * sinAlpha, 0));
double sinBeta = Math.Sin(input.beta * Math.PI / 180);
double cosBeta = Math.Sqrt(Math.Max(1 - sinBeta * sinBeta, 0));
double sinPhi = Math.Sin(input.phi * Math.PI / 180);
double cosPhi = Math.Sqrt(Math.Max(1 - sinPhi * sinPhi, 0));
double alphaDot = input.alphaDot * Math.PI / 180;
double betaDot = input.betaDot * Math.PI / 180;
double phiDot = input.phiDot * Math.PI / 180;
Vector3d AngVel = (phiDot - sinAlpha * betaDot) * forward;
AngVel += (cosPhi * alphaDot + cosAlpha * sinPhi * betaDot) * right;
AngVel += (sinPhi * alphaDot - cosAlpha * cosPhi * betaDot) * up;
Vector3d velocity = forward * cosAlpha * cosBeta;
velocity += right * (sinPhi * cosAlpha * cosBeta + cosPhi * sinBeta);
velocity += -up * cosPhi * (sinAlpha * cosBeta + sinBeta);
velocity.Normalize();
//this is negative wrt the ground
Vector3d liftVector = -forward * sinAlpha + right * sinPhi * cosAlpha - up * cosPhi * cosAlpha;
Vector3d sideways = Vector3.Cross(velocity, liftVector).normalized;
for (int i = 0; i < _wingAerodynamicModel.Count; i++)
{
FARWingAerodynamicModel w = _wingAerodynamicModel[i];
if (!(w && w.part))
{
continue;
}
w.ComputeForceEditor(velocity.normalized, input.machNumber, 2);
if (clear)
{
w.EditorClClear(reset_stall);
}
Vector3d relPos = w.GetAerodynamicCenter() - CoM;
Vector3d vel = velocity + Vector3d.Cross(AngVel, relPos);
if (w is FARControllableSurface)
{
(w as FARControllableSurface).SetControlStateEditor(CoM, vel, (float)input.pitchValue, 0, 0, input.flaps, input.spoilers);
}
else if (w.isShielded)
{
continue;
}
//w.ComputeForceEditor(velocity, input.machNumber); //do this just to get the AC right
Vector3d force = w.ComputeForceEditor(vel.normalized, input.machNumber, 2) * 1000;
output.Cl += -Vector3d.Dot(force, liftVector);
output.Cy += Vector3d.Dot(force, sideways);
output.Cd += -Vector3d.Dot(force, velocity);
Vector3d moment = -Vector3d.Cross(relPos, force);
output.Cm += Vector3d.Dot(moment, sideways);
output.Cn += Vector3d.Dot(moment, liftVector);
output.C_roll += Vector3d.Dot(moment, velocity);
//w.ComputeClCdEditor(vel.normalized, input.machNumber);
/*double tmpCl = w.GetCl() * w.S;
* output.Cl += tmpCl * -Vector3d.Dot(w.GetLiftDirection(), liftVector);
* output.Cy += tmpCl * -Vector3d.Dot(w.GetLiftDirection(), sideways);
* double tmpCd = w.GetCd() * w.S;
* output.Cd += tmpCd;
* output.Cm += tmpCl * Vector3d.Dot((relPos), velocity) * -Vector3d.Dot(w.GetLiftDirection(), liftVector) + tmpCd * -Vector3d.Dot((relPos), liftVector);
* output.Cn += tmpCd * Vector3d.Dot((relPos), sideways) + tmpCl * Vector3d.Dot((relPos), velocity) * -Vector3d.Dot(w.GetLiftDirection(), sideways);
* output.C_roll += tmpCl * Vector3d.Dot((relPos), sideways) * -Vector3d.Dot(w.GetLiftDirection(), liftVector);*/
area += w.S;
MAC += w.GetMAC() * w.S;
b_2 += w.Getb_2() * w.S;
}
FARCenterQuery center = new FARCenterQuery();
for (int i = 0; i < _currentAeroSections.Count; i++)
{
_currentAeroSections[i].PredictionCalculateAeroForces(2, (float)input.machNumber, 10000, 0.005f, velocity.normalized, center);
}
Vector3d centerForce = center.force * 1000;
output.Cl += -Vector3d.Dot(centerForce, liftVector);
output.Cy += Vector3d.Dot(centerForce, sideways);
output.Cd += -Vector3d.Dot(centerForce, velocity);
Vector3d centerMoment = -center.TorqueAt(CoM) * 1000;
output.Cm += Vector3d.Dot(centerMoment, sideways);
output.Cn += Vector3d.Dot(centerMoment, liftVector);
output.C_roll += Vector3d.Dot(centerMoment, velocity);
/*for (int i = 0; i < FARAeroUtil.CurEditorParts.Count; i++)
* {
* Part p = FARAeroUtil.CurEditorParts[i];
* if (FARAeroUtil.IsNonphysical(p))
* continue;
*
* Vector3 part_pos = p.transform.TransformPoint(p.CoMOffset) - CoM;
* double partMass = p.mass;
* if (p.Resources.Count > 0)
* partMass += p.GetResourceMass();
*
* double stock_drag = partMass * p.maximum_drag * FlightGlobals.DragMultiplier * 1000;
* output.Cd += stock_drag;
* output.Cm += stock_drag * -Vector3d.Dot(part_pos, liftVector);
* output.Cn += stock_drag * Vector3d.Dot(part_pos, sideways);
* }*/
if (area == 0)
{
area = _maxCrossSectionFromBody;
b_2 = 1;
MAC = _bodyLength;
}
double recipArea = 1 / area;
MAC *= recipArea;
b_2 *= recipArea;
output.Cl *= recipArea;
output.Cd *= recipArea;
output.Cm *= recipArea / MAC;
output.Cy *= recipArea;
output.Cn *= recipArea / b_2;
output.C_roll *= recipArea / b_2;
}
/// <summary>
/// Builds the slider joint.
/// </summary>
/// <returns>The slider joint.</returns>
/// <param name="simulationObjs">Simulation objects.</param>
public override List <JacobianConstraint> BuildJacobian()
{
var sliderConstraints = new List <JacobianConstraint> ();
IShape simulationObjectA = ShapeA;
IShape simulationObjectB = ShapeB;
#region Init Linear
Vector3d sliderAxis = GetSliderAxis();
Vector3d t1 = GeometryUtils.GetPerpendicularVector(sliderAxis).Normalize();
Vector3d t2 = sliderAxis.Cross(t1).Normalize();
Vector3d r1 = simulationObjectA.RotationMatrix *
StartErrorAxis1;
Vector3d r2 = simulationObjectB.RotationMatrix *
StartErrorAxis2;
Vector3d p1 = simulationObjectA.Position + r1;
Vector3d p2 = simulationObjectB.Position + r2;
Vector3d linearError = p2 - p1;
#endregion
#region Init Angular
Vector3d angularError = JacobianCommon.GetFixedAngularError(
simulationObjectA,
simulationObjectB,
RelativeOrientation);
#endregion
#region Jacobian Constraint
double errorReduction = ErrorReductionParam;
double springCoefficient = SpringCoefficient;
#region Base Constraints
ConstraintType constraintType = ConstraintType.Joint;
if (SpringCoefficient > 0)
{
constraintType = ConstraintType.SoftJoint;
}
double constraintLimit = errorReduction * 2.0 * angularError.x;
//DOF 1
sliderConstraints.Add(JacobianCommon.GetDOF(
xVecNeg,
xVec,
simulationObjectA,
simulationObjectB,
0.0,
constraintLimit,
springCoefficient,
0.0,
constraintType));
//DOF 2
constraintLimit = errorReduction * 2.0 * angularError.y;
sliderConstraints.Add(JacobianCommon.GetDOF(
yVecNeg,
yVec,
simulationObjectA,
simulationObjectB,
0.0,
constraintLimit,
springCoefficient,
0.0,
constraintType));
//DOF 3
constraintLimit = errorReduction * 2.0 * angularError.z;
sliderConstraints.Add(JacobianCommon.GetDOF(
zVecNeg,
zVec,
simulationObjectA,
simulationObjectB,
0.0,
constraintLimit,
springCoefficient,
0.0,
constraintType));
//DOF 4
constraintLimit = errorReduction * t1.Dot(linearError);
sliderConstraints.Add(JacobianCommon.GetDOF(
t1,
-1.0 * t1,
r1.Cross(t1),
-1.0 * r2.Cross(t1),
simulationObjectA,
simulationObjectB,
0.0,
constraintLimit,
springCoefficient,
0.0,
constraintType));
//DOF 5
constraintLimit = errorReduction * t2.Dot(linearError);
sliderConstraints.Add(JacobianCommon.GetDOF(
t2,
-1.0 * t2,
r1.Cross(t2),
-1.0 * r2.Cross(t2),
simulationObjectA,
simulationObjectB,
0.0,
constraintLimit,
springCoefficient,
0.0,
constraintType));
#endregion
#region Limit Constraints
// Limit extraction
if (LinearLimitMin.HasValue &&
LinearLimitMax.HasValue)
{
sliderConstraints.Add(
JacobianCommon.GetLinearLimit(
simulationObjectA,
simulationObjectB,
sliderAxis,
r1,
r2,
errorReduction,
0.0,
LinearLimitMin.Value,
LinearLimitMax.Value));
}
#endregion
#region Motor Constraint
if (ForceLimit.HasValue &&
SpeedValue.HasValue)
{
sliderConstraints.Add(JacobianCommon.GetDOF(
sliderAxis,
-1.0 * sliderAxis,
new Vector3d(),
new Vector3d(),
simulationObjectA,
simulationObjectB,
SpeedValue.Value,
0.0,
0.0,
ForceLimit.Value,
ConstraintType.JointMotor));
}
#endregion
#endregion
return(sliderConstraints);
}
