public void Update(Vessel vessel)
{
if (vessel.rigidbody == null)
{
return; //if we try to update before rigidbodies exist we spam the console with NullPointerExceptions.
}
//if (vessel.packed) return;
time = Planetarium.GetUniversalTime();
deltaT = TimeWarp.fixedDeltaTime;
CoM = vessel.findWorldCenterOfMass();
up = (CoM - vessel.mainBody.position).normalized;
Rigidbody rigidBody = vessel.rootPart.rigidbody;
if (rigidBody != null)
{
rootPartPos = rigidBody.position;
}
north = Vector3d.Exclude(up, (vessel.mainBody.position + vessel.mainBody.transform.up * (float)vessel.mainBody.Radius) - CoM).normalized;
east = vessel.mainBody.getRFrmVel(CoM).normalized;
forward = vessel.GetTransform().up;
rotationSurface = Quaternion.LookRotation(north, up);
rotationVesselSurface = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vessel.GetTransform().rotation) * rotationSurface);
velocityVesselOrbit = vessel.orbit.GetVel();
velocityVesselOrbitUnit = velocityVesselOrbit.normalized;
velocityVesselSurface = velocityVesselOrbit - vessel.mainBody.getRFrmVel(CoM);
velocityVesselSurfaceUnit = velocityVesselSurface.normalized;
velocityMainBodySurface = rotationSurface * velocityVesselSurface;
horizontalOrbit = Vector3d.Exclude(up, velocityVesselOrbit).normalized;
horizontalSurface = Vector3d.Exclude(up, velocityVesselSurface).normalized;
angularVelocity = Quaternion.Inverse(vessel.GetTransform().rotation) * vessel.rigidbody.angularVelocity;
radialPlusSurface = Vector3d.Exclude(velocityVesselSurface, up).normalized;
radialPlus = Vector3d.Exclude(velocityVesselOrbit, up).normalized;
normalPlusSurface = -Vector3d.Cross(radialPlusSurface, velocityVesselSurfaceUnit);
normalPlus = -Vector3d.Cross(radialPlus, velocityVesselOrbitUnit);
gravityForce = FlightGlobals.getGeeForceAtPosition(CoM);
localg = gravityForce.magnitude;
speedOrbital.value = velocityVesselOrbit.magnitude;
speedSurface.value = velocityVesselSurface.magnitude;
speedVertical.value = Vector3d.Dot(velocityVesselSurface, up);
speedSurfaceHorizontal.value = (velocityVesselSurface - (speedVertical * up)).magnitude;
speedOrbitHorizontal = (velocityVesselOrbit - (speedVertical * up)).magnitude;
vesselHeading.value = rotationVesselSurface.eulerAngles.y;
vesselPitch.value = (rotationVesselSurface.eulerAngles.x > 180) ? (360.0 - rotationVesselSurface.eulerAngles.x) : -rotationVesselSurface.eulerAngles.x;
vesselRoll.value = (rotationVesselSurface.eulerAngles.z > 180) ? (rotationVesselSurface.eulerAngles.z - 360.0) : rotationVesselSurface.eulerAngles.z;
altitudeASL.value = vessel.mainBody.GetAltitude(CoM);
RaycastHit sfc;
if (Physics.Raycast(CoM, -up, out sfc, (float)altitudeASL + 10000.0F, 1 << 15))
{
altitudeTrue.value = sfc.distance;
}
else if (vessel.mainBody.pqsController != null)
{
// from here: http://kerbalspaceprogram.com/forum/index.php?topic=10324.msg161923#msg161923
altitudeTrue.value = vessel.mainBody.GetAltitude(CoM) - (vessel.mainBody.pqsController.GetSurfaceHeight(QuaternionD.AngleAxis(vessel.mainBody.GetLongitude(CoM), Vector3d.down) * QuaternionD.AngleAxis(vessel.mainBody.GetLatitude(CoM), Vector3d.forward) * Vector3d.right) - vessel.mainBody.pqsController.radius);
}
else
{
altitudeTrue.value = vessel.mainBody.GetAltitude(CoM);
}
double surfaceAltitudeASL = altitudeASL - altitudeTrue;
altitudeBottom = altitudeTrue;
foreach (Part p in vessel.parts)
{
if (p.collider != null)
{
Vector3d bottomPoint = p.collider.ClosestPointOnBounds(vessel.mainBody.position);
double partBottomAlt = vessel.mainBody.GetAltitude(bottomPoint) - surfaceAltitudeASL;
altitudeBottom = Math.Max(0, Math.Min(altitudeBottom, partBottomAlt));
}
}
double atmosphericPressure = FlightGlobals.getStaticPressure(altitudeASL, vessel.mainBody);
if (atmosphericPressure < vessel.mainBody.atmosphereMultiplier * 1e-6)
{
atmosphericPressure = 0;
}
atmosphericDensity = FlightGlobals.getAtmDensity(atmosphericPressure);
atmosphericDensityGrams = atmosphericDensity * 1000;
orbitApA.value = vessel.orbit.ApA;
orbitPeA.value = vessel.orbit.PeA;
orbitPeriod.value = vessel.orbit.period;
orbitTimeToAp.value = vessel.orbit.timeToAp;
if (vessel.orbit.eccentricity < 1)
{
orbitTimeToPe.value = vessel.orbit.timeToPe;
}
else
{
orbitTimeToPe.value = -vessel.orbit.meanAnomaly / (2 * Math.PI / vessel.orbit.period);
}
orbitLAN.value = vessel.orbit.LAN;
orbitArgumentOfPeriapsis.value = vessel.orbit.argumentOfPeriapsis;
orbitInclination.value = vessel.orbit.inclination;
orbitEccentricity.value = vessel.orbit.eccentricity;
orbitSemiMajorAxis.value = vessel.orbit.semiMajorAxis;
latitude.value = vessel.mainBody.GetLatitude(CoM);
longitude.value = RTUtil.ClampDegrees180(vessel.mainBody.GetLongitude(CoM));
if (vessel.mainBody != Planetarium.fetch.Sun)
{
Vector3d delta = vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime() + 1) - vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime() - 1);
Vector3d plUp = Vector3d.Cross(vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime()) - vessel.mainBody.referenceBody.getPositionAtUT(Planetarium.GetUniversalTime()), vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime() + vessel.mainBody.orbit.period / 4) - vessel.mainBody.referenceBody.getPositionAtUT(Planetarium.GetUniversalTime() + vessel.mainBody.orbit.period / 4)).normalized;
angleToPrograde = RTUtil.ClampDegrees360((((vessel.orbit.inclination > 90) || (vessel.orbit.inclination < -90)) ? 1 : -1) * ((Vector3)up).AngleInPlane(plUp, delta));
}
else
{
angleToPrograde = 0;
}
mainBody = vessel.mainBody;
radius = (CoM - vessel.mainBody.position).magnitude;
mass = thrustAvailable = thrustMinimum = massDrag = torqueThrustPYAvailable = 0;
torqueAvailable = new Vector3d();
rcsThrustAvailable = new Vector6();
rcsTorqueAvailable = new Vector6();
EngineInfo einfo = new EngineInfo(forward, CoM);
IntakeInfo iinfo = new IntakeInfo();
foreach (Part p in vessel.parts)
{
if (p.physicalSignificance != Part.PhysicalSignificance.NONE)
{
double partMass = p.mass + p.GetResourceMass();
mass += partMass;
massDrag += partMass * p.maximum_drag;
}
if (vessel.ActionGroups[KSPActionGroup.RCS])
{
foreach (ModuleRCS pm in p.Modules.OfType <ModuleRCS>())
{
double maxT = pm.thrusterPower;
Vector3d partPosition = p.Rigidbody.worldCenterOfMass - CoM;
if ((pm.isEnabled) && (!pm.isJustForShow))
{
foreach (Transform t in pm.thrusterTransforms)
{
Vector3d thrusterThrust = -t.up * pm.thrusterPower;
rcsThrustAvailable.Add(thrusterThrust);
Vector3d thrusterTorque = vessel.GetTransform().InverseTransformDirection(Vector3.Cross(partPosition, thrusterThrust));
rcsTorqueAvailable.Add(thrusterTorque);
}
}
}
}
if (p is CommandPod)
{
torqueAvailable += Vector3d.one * Math.Abs(((CommandPod)p).rotPower);
}
foreach (PartModule pm in p.Modules)
{
if (!pm.isEnabled)
{
continue;
}
if (pm is ModuleReactionWheel)
{
ModuleReactionWheel rw = (ModuleReactionWheel)pm;
if (rw.wheelState == ModuleReactionWheel.WheelState.Active)
{
torqueAvailable += new Vector3d(rw.PitchTorque, rw.RollTorque, rw.YawTorque);
}
}
if (pm is ModuleEngines)
{
einfo.AddNewEngine(pm as ModuleEngines);
}
else if (pm is ModuleResourceIntake)
{
iinfo.addIntake(pm as ModuleResourceIntake);
}
}
}
torqueAvailable += Vector3d.Max(rcsTorqueAvailable.positive, rcsTorqueAvailable.negative); // Should we use Max or Min ?
thrustAvailable += einfo.thrustAvailable;
thrustMinimum += einfo.thrustMinimum;
torqueThrustPYAvailable += einfo.torqueThrustPYAvailable;
// Convert the resource information from the einfo and iinfo format
// to the more useful ResourceInfo format.
resources = new Dictionary <int, ResourceInfo>();
foreach (var info in einfo.resourceRequired)
{
int id = info.Key;
var req = info.Value;
resources[id] = new ResourceInfo(
PartResourceLibrary.Instance.GetDefinition(id),
req.requiredLastFrame,
req.requiredAtMaxThrottle,
iinfo.getIntakes(id));
}
int intakeAirId = PartResourceLibrary.Instance.GetDefinition("IntakeAir").id;
intakeAir = 0;
intakeAirNeeded = 0;
intakeAirAtMax = 0;
if (resources.ContainsKey(intakeAirId))
{
intakeAir = resources[intakeAirId].intakeProvided;
intakeAirNeeded = resources[intakeAirId].required;
intakeAirAtMax = resources[intakeAirId].requiredAtMaxThrottle;
}
angularMomentum = new Vector3d(angularVelocity.x * MoI.x, angularVelocity.y * MoI.y, angularVelocity.z * MoI.z);
maxThrustAccel = thrustAvailable / mass;
minThrustAccel = thrustMinimum / mass;
inertiaTensor = new Matrix3x3();
foreach (Part p in vessel.parts)
{
if (p.Rigidbody == null)
{
continue;
}
//Compute the contributions to the vessel inertia tensor due to the part inertia tensor
Vector3d principalMoments = p.Rigidbody.inertiaTensor;
Quaternion princAxesRot = Quaternion.Inverse(vessel.GetTransform().rotation) * p.transform.rotation * p.Rigidbody.inertiaTensorRotation;
Quaternion invPrincAxesRot = Quaternion.Inverse(princAxesRot);
for (int i = 0; i < 3; i++)
{
Vector3d iHat = Vector3d.zero;
iHat[i] = 1;
for (int j = 0; j < 3; j++)
{
Vector3d jHat = Vector3d.zero;
jHat[j] = 1;
inertiaTensor[i, j] += Vector3d.Dot(iHat, princAxesRot * Vector3d.Scale(principalMoments, invPrincAxesRot * jHat));
}
}
//Compute the contributions to the vessel inertia tensor due to the part mass and position
double partMass = p.mass + p.GetResourceMass();
Vector3 partPosition = vessel.GetTransform().InverseTransformDirection(p.Rigidbody.worldCenterOfMass - CoM);
for (int i = 0; i < 3; i++)
{
inertiaTensor[i, i] += partMass * partPosition.sqrMagnitude;
for (int j = 0; j < 3; j++)
{
inertiaTensor[i, j] += -partMass * partPosition[i] * partPosition[j];
}
}
}
MoI = new Vector3d(inertiaTensor[0, 0], inertiaTensor[1, 1], inertiaTensor[2, 2]);
angularMomentum = inertiaTensor * angularVelocity;
}
public void Update(Vessel vessel)
{
if (vessel.rigidbody == null)
return; //if we try to update before rigidbodies exist we spam the console with NullPointerExceptions.
//if (vessel.packed) return;
time = Planetarium.GetUniversalTime();
deltaT = TimeWarp.fixedDeltaTime;
CoM = vessel.findWorldCenterOfMass();
up = (CoM - vessel.mainBody.position).normalized;
Rigidbody rigidBody = vessel.rootPart.rigidbody;
if (rigidBody != null)
rootPartPos = rigidBody.position;
north = Vector3d.Exclude(up, (vessel.mainBody.position + vessel.mainBody.transform.up * (float)vessel.mainBody.Radius) - CoM).normalized;
east = vessel.mainBody.getRFrmVel(CoM).normalized;
forward = vessel.GetTransform().up;
rotationSurface = Quaternion.LookRotation(north, up);
rotationVesselSurface = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vessel.GetTransform().rotation) * rotationSurface);
velocityVesselOrbit = vessel.orbit.GetVel();
velocityVesselOrbitUnit = velocityVesselOrbit.normalized;
velocityVesselSurface = velocityVesselOrbit - vessel.mainBody.getRFrmVel(CoM);
velocityVesselSurfaceUnit = velocityVesselSurface.normalized;
velocityMainBodySurface = rotationSurface * velocityVesselSurface;
horizontalOrbit = Vector3d.Exclude(up, velocityVesselOrbit).normalized;
horizontalSurface = Vector3d.Exclude(up, velocityVesselSurface).normalized;
angularVelocity = Quaternion.Inverse(vessel.GetTransform().rotation) * vessel.rigidbody.angularVelocity;
radialPlusSurface = Vector3d.Exclude(velocityVesselSurface, up).normalized;
radialPlus = Vector3d.Exclude(velocityVesselOrbit, up).normalized;
normalPlusSurface = -Vector3d.Cross(radialPlusSurface, velocityVesselSurfaceUnit);
normalPlus = -Vector3d.Cross(radialPlus, velocityVesselOrbitUnit);
gravityForce = FlightGlobals.getGeeForceAtPosition(CoM);
localg = gravityForce.magnitude;
speedOrbital.value = velocityVesselOrbit.magnitude;
speedSurface.value = velocityVesselSurface.magnitude;
speedVertical.value = Vector3d.Dot(velocityVesselSurface, up);
speedSurfaceHorizontal.value = (velocityVesselSurface - (speedVertical * up)).magnitude;
speedOrbitHorizontal = (velocityVesselOrbit - (speedVertical * up)).magnitude;
vesselHeading.value = rotationVesselSurface.eulerAngles.y;
vesselPitch.value = (rotationVesselSurface.eulerAngles.x > 180) ? (360.0 - rotationVesselSurface.eulerAngles.x) : -rotationVesselSurface.eulerAngles.x;
vesselRoll.value = (rotationVesselSurface.eulerAngles.z > 180) ? (rotationVesselSurface.eulerAngles.z - 360.0) : rotationVesselSurface.eulerAngles.z;
altitudeASL.value = vessel.mainBody.GetAltitude(CoM);
RaycastHit sfc;
if (Physics.Raycast(CoM, -up, out sfc, (float)altitudeASL + 10000.0F, 1 << 15)) {
altitudeTrue.value = sfc.distance;
} else if (vessel.mainBody.pqsController != null) {
// from here: http://kerbalspaceprogram.com/forum/index.php?topic=10324.msg161923#msg161923
altitudeTrue.value = vessel.mainBody.GetAltitude(CoM) - (vessel.mainBody.pqsController.GetSurfaceHeight(QuaternionD.AngleAxis(vessel.mainBody.GetLongitude(CoM), Vector3d.down) * QuaternionD.AngleAxis(vessel.mainBody.GetLatitude(CoM), Vector3d.forward) * Vector3d.right) - vessel.mainBody.pqsController.radius);
} else {
altitudeTrue.value = vessel.mainBody.GetAltitude(CoM);
}
double surfaceAltitudeASL = altitudeASL - altitudeTrue;
altitudeBottom = altitudeTrue;
foreach (Part p in vessel.parts) {
if (p.collider != null) {
Vector3d bottomPoint = p.collider.ClosestPointOnBounds(vessel.mainBody.position);
double partBottomAlt = vessel.mainBody.GetAltitude(bottomPoint) - surfaceAltitudeASL;
altitudeBottom = Math.Max(0, Math.Min(altitudeBottom, partBottomAlt));
}
}
double atmosphericPressure = FlightGlobals.getStaticPressure(altitudeASL, vessel.mainBody);
if (atmosphericPressure < vessel.mainBody.atmosphereMultiplier * 1e-6)
atmosphericPressure = 0;
atmosphericDensity = FlightGlobals.getAtmDensity(atmosphericPressure);
atmosphericDensityGrams = atmosphericDensity * 1000;
orbitApA.value = vessel.orbit.ApA;
orbitPeA.value = vessel.orbit.PeA;
orbitPeriod.value = vessel.orbit.period;
orbitTimeToAp.value = vessel.orbit.timeToAp;
if (vessel.orbit.eccentricity < 1)
orbitTimeToPe.value = vessel.orbit.timeToPe;
else
orbitTimeToPe.value = -vessel.orbit.meanAnomaly / (2 * Math.PI / vessel.orbit.period);
orbitLAN.value = vessel.orbit.LAN;
orbitArgumentOfPeriapsis.value = vessel.orbit.argumentOfPeriapsis;
orbitInclination.value = vessel.orbit.inclination;
orbitEccentricity.value = vessel.orbit.eccentricity;
orbitSemiMajorAxis.value = vessel.orbit.semiMajorAxis;
latitude.value = vessel.mainBody.GetLatitude(CoM);
longitude.value = RTUtil.ClampDegrees180(vessel.mainBody.GetLongitude(CoM));
if (vessel.mainBody != Planetarium.fetch.Sun) {
Vector3d delta = vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime() + 1) - vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime() - 1);
Vector3d plUp = Vector3d.Cross(vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime()) - vessel.mainBody.referenceBody.getPositionAtUT(Planetarium.GetUniversalTime()), vessel.mainBody.getPositionAtUT(Planetarium.GetUniversalTime() + vessel.mainBody.orbit.period / 4) - vessel.mainBody.referenceBody.getPositionAtUT(Planetarium.GetUniversalTime() + vessel.mainBody.orbit.period / 4)).normalized;
angleToPrograde = RTUtil.ClampDegrees360((((vessel.orbit.inclination > 90) || (vessel.orbit.inclination < -90)) ? 1 : -1) * ((Vector3)up).AngleInPlane(plUp, delta));
} else {
angleToPrograde = 0;
}
mainBody = vessel.mainBody;
radius = (CoM - vessel.mainBody.position).magnitude;
mass = thrustAvailable = thrustMinimum = massDrag = torqueThrustPYAvailable = 0;
torqueAvailable = new Vector3d();
rcsThrustAvailable = new Vector6();
rcsTorqueAvailable = new Vector6();
EngineInfo einfo = new EngineInfo(forward, CoM);
IntakeInfo iinfo = new IntakeInfo();
foreach (Part p in vessel.parts) {
if (p.physicalSignificance != Part.PhysicalSignificance.NONE) {
double partMass = p.mass + p.GetResourceMass();
mass += partMass;
massDrag += partMass * p.maximum_drag;
}
if (vessel.ActionGroups[KSPActionGroup.RCS]) {
foreach (ModuleRCS pm in p.Modules.OfType<ModuleRCS>()) {
double maxT = pm.thrusterPower;
Vector3d partPosition = p.Rigidbody.worldCenterOfMass - CoM;
if ((pm.isEnabled) && (!pm.isJustForShow)) {
foreach (Transform t in pm.thrusterTransforms) {
Vector3d thrusterThrust = -t.up * pm.thrusterPower;
rcsThrustAvailable.Add(thrusterThrust);
Vector3d thrusterTorque = vessel.GetTransform().InverseTransformDirection(Vector3.Cross(partPosition, thrusterThrust));
rcsTorqueAvailable.Add(thrusterTorque);
}
}
}
}
if (p is CommandPod) {
torqueAvailable += Vector3d.one * Math.Abs(((CommandPod)p).rotPower);
}
foreach (PartModule pm in p.Modules) {
if (!pm.isEnabled)
continue;
if (pm is ModuleReactionWheel) {
ModuleReactionWheel rw = (ModuleReactionWheel)pm;
if (rw.wheelState == ModuleReactionWheel.WheelState.Active) {
torqueAvailable += new Vector3d(rw.PitchTorque, rw.RollTorque, rw.YawTorque);
}
}
if (pm is ModuleEngines) {
einfo.AddNewEngine(pm as ModuleEngines);
} else if (pm is ModuleResourceIntake) {
iinfo.addIntake(pm as ModuleResourceIntake);
}
}
}
torqueAvailable += Vector3d.Max(rcsTorqueAvailable.positive, rcsTorqueAvailable.negative); // Should we use Max or Min ?
thrustAvailable += einfo.thrustAvailable;
thrustMinimum += einfo.thrustMinimum;
torqueThrustPYAvailable += einfo.torqueThrustPYAvailable;
// Convert the resource information from the einfo and iinfo format
// to the more useful ResourceInfo format.
resources = new Dictionary<int, ResourceInfo>();
foreach (var info in einfo.resourceRequired) {
int id = info.Key;
var req = info.Value;
resources[id] = new ResourceInfo(
PartResourceLibrary.Instance.GetDefinition(id),
req.requiredLastFrame,
req.requiredAtMaxThrottle,
iinfo.getIntakes(id));
}
int intakeAirId = PartResourceLibrary.Instance.GetDefinition("IntakeAir").id;
intakeAir = 0;
intakeAirNeeded = 0;
intakeAirAtMax = 0;
if (resources.ContainsKey(intakeAirId)) {
intakeAir = resources[intakeAirId].intakeProvided;
intakeAirNeeded = resources[intakeAirId].required;
intakeAirAtMax = resources[intakeAirId].requiredAtMaxThrottle;
}
angularMomentum = new Vector3d(angularVelocity.x * MoI.x, angularVelocity.y * MoI.y, angularVelocity.z * MoI.z);
maxThrustAccel = thrustAvailable / mass;
minThrustAccel = thrustMinimum / mass;
inertiaTensor = new Matrix3x3();
foreach (Part p in vessel.parts) {
if (p.Rigidbody == null)
continue;
//Compute the contributions to the vessel inertia tensor due to the part inertia tensor
Vector3d principalMoments = p.Rigidbody.inertiaTensor;
Quaternion princAxesRot = Quaternion.Inverse(vessel.GetTransform().rotation) * p.transform.rotation * p.Rigidbody.inertiaTensorRotation;
Quaternion invPrincAxesRot = Quaternion.Inverse(princAxesRot);
for (int i = 0; i < 3; i++) {
Vector3d iHat = Vector3d.zero;
iHat[i] = 1;
for (int j = 0; j < 3; j++) {
Vector3d jHat = Vector3d.zero;
jHat[j] = 1;
inertiaTensor[i, j] += Vector3d.Dot(iHat, princAxesRot * Vector3d.Scale(principalMoments, invPrincAxesRot * jHat));
}
}
//Compute the contributions to the vessel inertia tensor due to the part mass and position
double partMass = p.mass + p.GetResourceMass();
Vector3 partPosition = vessel.GetTransform().InverseTransformDirection(p.Rigidbody.worldCenterOfMass - CoM);
for (int i = 0; i < 3; i++) {
inertiaTensor[i, i] += partMass * partPosition.sqrMagnitude;
for (int j = 0; j < 3; j++) {
inertiaTensor[i, j] += -partMass * partPosition[i] * partPosition[j];
}
}
}
MoI = new Vector3d(inertiaTensor[0, 0], inertiaTensor[1, 1], inertiaTensor[2, 2]);
angularMomentum = inertiaTensor * angularVelocity;
}
