Exemplo n.º 1
0
        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;
        }