public Matrix3x3 inverse()
{
double det = m[0, 0] * m[1, 1] * m[2, 2] + m[0, 1] * m[1, 2] * m[2, 0] + m[0, 2] * m[1, 0] * m[2, 1]
- m[0, 0] * m[1, 2] * m[2, 1] - m[0, 1] * m[1, 0] * m[2, 2] - m[0, 2] * m[1, 1] * m[2, 0];
Matrix3x3 ret = new Matrix3x3();
for(int r = 0; r < 3; r++) {
for(int c = 0; c < 3; c++) {
int mr1 = (r == 0 ? 1 : 0);
int mr2 = (r == 2 ? 1 : 2);
int mc1 = (c == 0 ? 1 : 0);
int mc2 = (c == 2 ? 1 : 2);
ret.m[r, c] = 1/det * (m[mr1, mc1] * m[mr2, mc2] - m[mr2, mc1] * m[mr1, mc2]);
}
}
return ret;
}
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 = MuUtils.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 = MuUtils.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 = massDrag = torqueThrustPYAvailable = 0;
thrustVectorLastFrame = new Vector3d();
thrustVectorMaxThrottle = new Vector3d();
thrustVectorMinThrottle = new Vector3d();
torqueAvailable = new Vector3d();
rcsThrustAvailable = new Vector6();
rcsTorqueAvailable = new Vector6();
ctrlTorqueAvailable = new Vector6();
EngineInfo einfo = new EngineInfo(CoM);
IntakeInfo iinfo = new IntakeInfo();
parachutes = new List<ModuleParachute>();
var rcsbal = vessel.GetMasterMechJeb().rcsbal;
if (vessel.ActionGroups[KSPActionGroup.RCS] && rcsbal.enabled)
{
Vector3d rot = Vector3d.zero;
foreach (Vector6.Direction dir6 in Enum.GetValues(typeof(Vector6.Direction)))
{
Vector3d dir = Vector6.directions[dir6];
double[] throttles;
List<RCSSolver.Thruster> thrusters;
rcsbal.GetThrottles(dir, out throttles, out thrusters);
if (throttles != null)
{
for (int i = 0; i < throttles.Length; i++)
{
if (throttles[i] > 0)
{
Vector3d force = thrusters[i].GetThrust(dir, rot);
rcsThrustAvailable.Add(dir * Vector3d.Dot(force * throttles[i], dir));
}
}
}
}
}
foreach (Part p in vessel.parts)
{
if (p.physicalSignificance != Part.PhysicalSignificance.NONE)
{
double partMass = p.TotalMass();
mass += partMass;
massDrag += partMass * p.maximum_drag;
}
if (vessel.ActionGroups[KSPActionGroup.RCS] && !rcsbal.enabled)
{
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 ControlSurface)
{
Vector3d partPosition = p.Rigidbody.worldCenterOfMass - CoM;
ControlSurface cs = (p as ControlSurface);
// Air Speed is velocityVesselSurface
// AddForceAtPosition seems to need the airspeed vector rotated with the flap rotation x its surface
Quaternion airSpeedRot = Quaternion.AngleAxis(cs.ctrlSurfaceRange * cs.ctrlSurfaceArea, cs.transform.rotation * cs.pivotAxis);
Vector3 ctrlTroquePos = vessel.GetTransform().InverseTransformDirection(Vector3.Cross(partPosition, cs.getLiftVector( airSpeedRot * velocityVesselSurface )));
Vector3 ctrlTroqueNeg = vessel.GetTransform().InverseTransformDirection(Vector3.Cross(partPosition, cs.getLiftVector( Quaternion.Inverse(airSpeedRot) * velocityVesselSurface )));
ctrlTorqueAvailable.Add(ctrlTroquePos);
ctrlTorqueAvailable.Add(ctrlTroqueNeg);
}
if (p is CommandPod)
{
torqueAvailable += Vector3d.one * Math.Abs(((CommandPod)p).rotPower);
}
foreach (VesselStatePartExtension vspe in vesselStatePartExtensions)
{
vspe(p);
}
foreach (PartModule pm in p.Modules)
{
if (!pm.isEnabled) continue;
if (pm is ModuleReactionWheel)
{
ModuleReactionWheel rw = (ModuleReactionWheel)pm;
// I had to remove the test for active in .23 since the new ressource system reply to the RW that
// there is no energy available when the RW do tiny adjustement.
// I replaceed it with a test that check if there is electricity anywhere on the ship.
// Let's hope we don't get reaction wheel that use something else
//if (rw.wheelState == ModuleReactionWheel.WheelState.Active && !rw.stateString.Contains("Not enough"))
if (rw.wheelState == ModuleReactionWheel.WheelState.Active && vessel.HasElectricCharge())
torqueAvailable += new Vector3d(rw.PitchTorque, rw.RollTorque, rw.YawTorque);
}
else if (pm is ModuleEngines)
{
einfo.AddNewEngine(pm as ModuleEngines);
}
else if (pm is ModuleEnginesFX)
{
einfo.AddNewEngine(pm as ModuleEnginesFX);
}
else if (pm is ModuleResourceIntake)
{
iinfo.addIntake(pm as ModuleResourceIntake);
}
else if (pm is ModuleParachute)
{
parachutes.Add(pm as ModuleParachute);
}
foreach (VesselStatePartModuleExtension vspme in vesselStatePartModuleExtensions)
{
vspme(pm);
}
}
}
torqueAvailable += Vector3d.Max(rcsTorqueAvailable.positive, rcsTorqueAvailable.negative); // Should we use Max or Min ?
torqueAvailable += Vector3d.Max(ctrlTorqueAvailable.positive, ctrlTorqueAvailable.negative); // Should we use Max or Min ?
thrustVectorMaxThrottle += einfo.thrustMax;
thrustVectorMinThrottle += einfo.thrustMin;
thrustVectorLastFrame += einfo.thrustCurrent;
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;
intakeAirAllIntakes = 0;
if (resources.ContainsKey(intakeAirId))
{
intakeAir = resources[intakeAirId].intakeProvided;
intakeAirAllIntakes = resources[intakeAirId].intakeAvailable;
intakeAirNeeded = resources[intakeAirId].required;
intakeAirAtMax = resources[intakeAirId].requiredAtMaxThrottle;
}
angularMomentum = new Vector3d(angularVelocity.x * MoI.x, angularVelocity.y * MoI.y, angularVelocity.z * MoI.z);
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.TotalMass();
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;
}
