示例#1
0
        public void Apply(PosistionStatistics posistionStatistics, Dictionary <Guid, VesselCtrlUpdate> ctrlUpdate, VesselUpdate previousUpdate, VesselUpdate nextUpdate, Settings dmpSettings)
        {
            if (HighLogic.LoadedScene == GameScenes.LOADING)
            {
                return;
            }

            //Ignore updates to our own vessel if we are in flight and we aren't spectating
            if (!vesselWorker.isSpectating && (FlightGlobals.fetch.activeVessel != null ? FlightGlobals.fetch.activeVessel.id == vesselID : false) && HighLogic.LoadedScene == GameScenes.FLIGHT)
            {
                return;
            }
            Vessel updateVessel = FlightGlobals.fetch.vessels.FindLast(v => v.id == vesselID);

            if (updateVessel == null)
            {
                //DarkLog.Debug("ApplyVesselUpdate - Got vessel update for " + vesselID + " but vessel does not exist");
                return;
            }
            CelestialBody updateBody = FlightGlobals.Bodies.Find(b => b.bodyName == bodyName);

            if (updateBody == null)
            {
                //DarkLog.Debug("ApplyVesselUpdate - updateBody not found");
                return;
            }

            double interpolatorDelay = 0f;

            if (dmpSettings.interpolatorType == InterpolatorType.INTERPOLATE1S)
            {
                interpolatorDelay = 1f;
            }
            if (dmpSettings.interpolatorType == InterpolatorType.INTERPOLATE3S)
            {
                interpolatorDelay = 3f;
            }

            bool interpolatorEnabled = dmpSettings.interpolatorType == InterpolatorType.INTERPOLATE1S || dmpSettings.interpolatorType == InterpolatorType.INTERPOLATE3S;
            bool extrapolatorEnabled = dmpSettings.interpolatorType == InterpolatorType.EXTRAPOLATE_NO_ROT || dmpSettings.interpolatorType == InterpolatorType.EXTRAPOLATE_FULL;

            Quaternion normalRotate = Quaternion.identity;
            Vector3    oldPos       = updateVessel.GetWorldPos3D();
            Vector3    oldVelocity  = updateVessel.orbitDriver.orbit.GetVel();

            //Position/Velocity
            if (isSurfaceUpdate)
            {
                //Get the new position/velocity
                double altitudeFudge = 0;
                VesselUtil.DMPRaycastPair dmpRaycast = VesselUtil.RaycastGround(position[0], position[1], updateBody);
                if (dmpRaycast.altitude != -1d && position[3] != -1d)
                {
                    Vector3 theirNormal = new Vector3(terrainNormal[0], terrainNormal[1], terrainNormal[2]);
                    altitudeFudge = dmpRaycast.altitude - position[3];
                    if (Math.Abs(position[2] - position[3]) < 50f)
                    {
                        normalRotate = Quaternion.FromToRotation(theirNormal, dmpRaycast.terrainNormal);
                    }
                }

                Vector3d updateAcceleration = updateBody.bodyTransform.rotation * new Vector3d(acceleration[0], acceleration[1], acceleration[2]);
                Vector3d updateVelocity     = updateBody.bodyTransform.rotation * new Vector3d(velocity[0], velocity[1], velocity[2]);
                Vector3d updatePostion      = updateBody.GetWorldSurfacePosition(position[0], position[1], position[2] + altitudeFudge);
                Vector3d newUpdatePostion   = updatePostion;
                Vector3d newUpdateVelocity  = updateVelocity;

                double planetariumDifference = Planetarium.GetUniversalTime() - (planetTime + interpolatorDelay);

                if (extrapolatorEnabled)
                {
                    if (Math.Abs(planetariumDifference) < 3f)
                    {
                        if (dmpSettings.interpolatorType == InterpolatorType.EXTRAPOLATE_NO_ROT || previousUpdate == null)
                        {
                            StepExtrapolate(updatePostion, updateVelocity, updateAcceleration, planetariumDifference, out newUpdatePostion, out newUpdateVelocity);
                        }
                        if (dmpSettings.interpolatorType == InterpolatorType.EXTRAPOLATE_FULL && previousUpdate != null)
                        {
                            StepExtrapolateWithRotation(previousUpdate, updatePostion, updateVelocity, updateAcceleration, planetariumDifference, out newUpdatePostion, out newUpdateVelocity);
                        }
                    }
                }

                if (interpolatorEnabled && nextUpdate != null && (Math.Abs(nextUpdate.planetTime - Planetarium.GetUniversalTime())) < 5f)
                {
                    double   scaling      = (Planetarium.GetUniversalTime() - interpolatorDelay - planetTime) / (nextUpdate.planetTime - planetTime);
                    Vector3d nextPosition = updateBody.GetWorldSurfacePosition(nextUpdate.position[0], nextUpdate.position[1], nextUpdate.position[2] + altitudeFudge);
                    Vector3d nextVelocity = updateBody.bodyTransform.rotation * new Vector3d(nextUpdate.velocity[0], nextUpdate.velocity[1], nextUpdate.velocity[2]);
                    newUpdatePostion  = Vector3d.Lerp(updatePostion, nextPosition, scaling);
                    newUpdateVelocity = Vector3d.Lerp(updateVelocity, nextVelocity, scaling);
                }

                Vector3d orbitalPos          = newUpdatePostion - updateBody.position;
                Vector3d surfaceOrbitVelDiff = updateBody.getRFrmVel(newUpdatePostion);
                Vector3d orbitalVel          = newUpdateVelocity + surfaceOrbitVelDiff;
                updateVessel.orbitDriver.orbit.UpdateFromStateVectors(orbitalPos.xzy, orbitalVel.xzy, updateBody, Planetarium.GetUniversalTime());
            }
            else
            {
                updateVessel.orbit.SetOrbit(orbit[0], orbit[1], orbit[2], orbit[3], orbit[4], orbit[5], orbit[6], updateBody);
            }
            //Updates orbit.pos/vel
            updateVessel.orbitDriver.orbit.UpdateFromOrbitAtUT(updateVessel.orbitDriver.orbit, Planetarium.GetUniversalTime(), updateBody);

            //Updates vessel pos from the orbit, as if on rails
            updateVessel.orbitDriver.updateFromParameters();

            //Rotation
            Quaternion unfudgedRotation = new Quaternion(rotation[0], rotation[1], rotation[2], rotation[3]);

            //Rotation extrapolation? :O
            if (previousUpdate != null && (extrapolatorEnabled || (interpolatorEnabled && !isSurfaceUpdate)))
            {
                double deltaUpdateT = planetTime - previousUpdate.planetTime;
                double deltaRealT   = Planetarium.GetUniversalTime() - previousUpdate.planetTime;
                float  scaling      = (float)(deltaRealT / deltaUpdateT);
                if (Math.Abs(deltaRealT) < 3f)
                {
                    Quaternion previousRotation = new Quaternion(previousUpdate.rotation[0], previousUpdate.rotation[1], previousUpdate.rotation[2], previousUpdate.rotation[3]);
                    unfudgedRotation = RotationLerp(previousRotation, unfudgedRotation, scaling);
                }
            }

            if (nextUpdate != null && interpolatorEnabled && isSurfaceUpdate)
            {
                double deltaUpdateT = nextUpdate.planetTime - planetTime;
                double deltaRealT   = Planetarium.GetUniversalTime() - interpolatorDelay - planetTime;
                float  scaling      = (float)(deltaRealT / deltaUpdateT);
                if (Math.Abs(deltaRealT) < 3f)
                {
                    Quaternion nextRotation = new Quaternion(nextUpdate.rotation[0], nextUpdate.rotation[1], nextUpdate.rotation[2], nextUpdate.rotation[3]);
                    unfudgedRotation = RotationLerp(unfudgedRotation, nextRotation, scaling);
                }
            }

            Quaternion updateRotation = normalRotate * unfudgedRotation;
            //Rotational error tracking
            double rotationalError = Quaternion.Angle(updateVessel.srfRelRotation, updateRotation);

            updateVessel.SetRotation(updateVessel.mainBody.bodyTransform.rotation * updateRotation);
            updateVessel.srfRelRotation = updateRotation;

            Vector3 angularVel = updateVessel.ReferenceTransform.rotation * new Vector3(angularVelocity[0], angularVelocity[1], angularVelocity[2]);

            if (updateVessel.parts != null && !updateVessel.packed)
            {
                for (int i = 0; i < updateVessel.parts.Count; i++)
                {
                    Part thisPart = updateVessel.parts[i];
                    thisPart.vel = updateVessel.orbit.GetVel() - Krakensbane.GetFrameVelocity();
                    if (thisPart.orbit.referenceBody.inverseRotation)
                    {
                        thisPart.vel -= updateBody.getRFrmVel(thisPart.partTransform.position);
                    }
                    if (thisPart.rb != null && thisPart.State != PartStates.DEAD)
                    {
                        thisPart.rb.velocity = thisPart.vel;
                        //Angular Vel
                        thisPart.rb.angularVelocity = angularVel;
                        if (thisPart != updateVessel.rootPart)
                        {
                            Vector3 diffPos           = thisPart.rb.position - updateVessel.CoM;
                            Vector3 partVelDifference = Vector3.Cross(angularVel, diffPos);
                            thisPart.rb.velocity = thisPart.rb.velocity + partVelDifference;
                        }
                    }
                }
            }

            //Updates Vessel.CoMD, which is used for GetWorldPos3D
            updateVessel.precalc.CalculatePhysicsStats();
            updateVessel.latitude  = updateBody.GetLatitude(updateVessel.GetWorldPos3D());
            updateVessel.longitude = updateBody.GetLongitude(updateVessel.GetWorldPos3D());
            updateVessel.altitude  = updateBody.GetAltitude(updateVessel.GetWorldPos3D());

            double distanceError = Vector3d.Distance(oldPos, updateVessel.GetWorldPos3D());
            double velocityError = Vector3d.Distance(oldVelocity, updateVessel.orbitDriver.orbit.GetVel());

            if (ctrlUpdate != null)
            {
                if (ctrlUpdate.ContainsKey(updateVessel.id))
                {
                    updateVessel.OnFlyByWire -= ctrlUpdate[updateVessel.id].UpdateControls;
                    ctrlUpdate.Remove(updateVessel.id);
                }
                VesselCtrlUpdate vcu = new VesselCtrlUpdate(updateVessel, ctrlUpdate, planetTime + 5, flightState);
                ctrlUpdate.Add(updateVessel.id, vcu);
                updateVessel.OnFlyByWire += vcu.UpdateControls;
            }

            //Action group controls
            updateVessel.ActionGroups.SetGroup(KSPActionGroup.Gear, actiongroupControls[0]);
            updateVessel.ActionGroups.SetGroup(KSPActionGroup.Light, actiongroupControls[1]);
            updateVessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, actiongroupControls[2]);
            updateVessel.ActionGroups.SetGroup(KSPActionGroup.SAS, actiongroupControls[3]);
            updateVessel.ActionGroups.SetGroup(KSPActionGroup.RCS, actiongroupControls[4]);

            if (sasEnabled)
            {
                updateVessel.Autopilot.SetMode((VesselAutopilot.AutopilotMode)autopilotMode);
                updateVessel.Autopilot.SAS.LockRotation(new Quaternion(lockedRotation[0], lockedRotation[1], lockedRotation[2], lockedRotation[3]));
            }
            UpdateProtovessel(updateVessel);
            posistionStatistics.LogError(updateVessel.id, distanceError, velocityError, rotationalError, planetTime);
        }
示例#2
0
        public void Apply(PosistionStatistics posistionStatistics, Dictionary <Guid, VesselCtrlUpdate> ctrlUpdate)
        {
            if (HighLogic.LoadedScene == GameScenes.LOADING)
            {
                return;
            }

            //Ignore updates to our own vessel if we are in flight and we aren't spectating
            if (!vesselWorker.isSpectating && (FlightGlobals.fetch.activeVessel != null ? FlightGlobals.fetch.activeVessel.id == vesselID : false) && HighLogic.LoadedScene == GameScenes.FLIGHT)
            {
                return;
            }
            Vessel updateVessel = FlightGlobals.fetch.vessels.FindLast(v => v.id == vesselID);

            if (updateVessel == null)
            {
                //DarkLog.Debug("ApplyVesselUpdate - Got vessel update for " + vesselID + " but vessel does not exist");
                return;
            }
            CelestialBody updateBody = FlightGlobals.Bodies.Find(b => b.bodyName == bodyName);

            if (updateBody == null)
            {
                //DarkLog.Debug("ApplyVesselUpdate - updateBody not found");
                return;
            }
            Quaternion normalRotate    = Quaternion.identity;
            double     distanceError   = 0;
            double     velocityError   = 0;
            double     rotationalError = 0;

            //Position/Velocity
            if (isSurfaceUpdate)
            {
                //Get the new position/velocity
                double altitudeFudge = 0;
                VesselUtil.DMPRaycastPair dmpRaycast = VesselUtil.RaycastGround(position[0], position[1], updateBody);
                if (dmpRaycast.altitude != -1d && position[3] != -1d)
                {
                    Vector3 theirNormal = new Vector3(terrainNormal[0], terrainNormal[1], terrainNormal[2]);
                    altitudeFudge = dmpRaycast.altitude - position[3];
                    if (Math.Abs(position[2] - position[3]) < 50f)
                    {
                        normalRotate = Quaternion.FromToRotation(theirNormal, dmpRaycast.terrainNormal);
                    }
                }

                double planetariumDifference = Planetarium.GetUniversalTime() - planetTime;

                //Velocity fudge
                Vector3d updateAcceleration = updateBody.bodyTransform.rotation * new Vector3d(acceleration[0], acceleration[1], acceleration[2]);
                Vector3d velocityFudge      = Vector3d.zero;
                if (Math.Abs(planetariumDifference) < 3f)
                {
                    //Velocity = a*t
                    velocityFudge = updateAcceleration * planetariumDifference;
                }

                //Position fudge
                Vector3d updateVelocity = updateBody.bodyTransform.rotation * new Vector3d(velocity[0], velocity[1], velocity[2]) + velocityFudge - Krakensbane.GetFrameVelocity();
                Vector3d positionFudge  = Vector3d.zero;

                if (Math.Abs(planetariumDifference) < 3f)
                {
                    //Use the average velocity to determine the new position
                    //Displacement = v0*t + 1/2at^2.
                    positionFudge = (updateVelocity * planetariumDifference) + (0.5d * updateAcceleration * planetariumDifference * planetariumDifference);
                }

                Vector3d updatePostion = updateBody.GetWorldSurfacePosition(position[0], position[1], position[2] + altitudeFudge) + positionFudge;

                //Posisional Error Tracking
                distanceError = Vector3d.Distance(updatePostion, updateVessel.GetWorldPos3D());

                double latitude  = updateBody.GetLatitude(updatePostion);
                double longitude = updateBody.GetLongitude(updatePostion);
                double altitude  = updateBody.GetAltitude(updatePostion);
                updateVessel.latitude              = latitude;
                updateVessel.longitude             = longitude;
                updateVessel.altitude              = altitude;
                updateVessel.protoVessel.latitude  = latitude;
                updateVessel.protoVessel.longitude = longitude;
                updateVessel.protoVessel.altitude  = altitude;

                if (updateVessel.packed)
                {
                    if (!updateVessel.LandedOrSplashed)
                    {
                        //Not landed but under 10km.
                        Vector3d orbitalPos          = updatePostion - updateBody.position;
                        Vector3d surfaceOrbitVelDiff = updateBody.getRFrmVel(updatePostion);
                        Vector3d orbitalVel          = updateVelocity + surfaceOrbitVelDiff;
                        velocityError = Vector3d.Distance(orbitalVel, updateVessel.obt_velocity);
                        updateVessel.orbitDriver.orbit.UpdateFromStateVectors(orbitalPos.xzy, orbitalVel.xzy, updateBody, Planetarium.GetUniversalTime());
                        updateVessel.orbitDriver.pos = updateVessel.orbitDriver.orbit.pos.xzy;
                        updateVessel.orbitDriver.vel = updateVessel.orbitDriver.orbit.vel;
                    }
                }
                else
                {
                    updateVessel.SetPosition(updatePostion, true);
                    updateVessel.SetWorldVelocity(updateVelocity);
                }
            }
            else
            {
                Orbit updateOrbit = new Orbit(orbit[0], orbit[1], orbit[2], orbit[3], orbit[4], orbit[5], orbit[6], updateBody);
                updateOrbit.Init();
                updateOrbit.UpdateFromUT(Planetarium.GetUniversalTime());

                //Positional Error Tracking
                distanceError = Vector3d.Distance(updateVessel.GetWorldPos3D(), updateOrbit.pos);
                double latitude  = updateBody.GetLatitude(updateOrbit.pos);
                double longitude = updateBody.GetLongitude(updateOrbit.pos);
                double altitude  = updateBody.GetAltitude(updateOrbit.pos);
                updateVessel.latitude              = latitude;
                updateVessel.longitude             = longitude;
                updateVessel.altitude              = altitude;
                updateVessel.protoVessel.latitude  = latitude;
                updateVessel.protoVessel.longitude = longitude;
                updateVessel.protoVessel.altitude  = altitude;
                VesselUtil.CopyOrbit(updateOrbit, updateVessel.orbitDriver.orbit);
                updateVessel.orbitDriver.updateFromParameters();

                if (!updateVessel.packed)
                {
                    updateVessel.SetWorldVelocity(updateVessel.orbitDriver.orbit.GetVel() - updateBody.getRFrmVelOrbit(updateVessel.orbitDriver.orbit) - Krakensbane.GetFrameVelocity());
                }
            }

            //Rotation
            Quaternion unfudgedRotation = new Quaternion(rotation[0], rotation[1], rotation[2], rotation[3]);
            Quaternion updateRotation   = normalRotate * unfudgedRotation;

            //Rotational error tracking
            rotationalError = Quaternion.Angle(updateVessel.srfRelRotation, updateRotation);
            updateVessel.SetRotation(updateVessel.mainBody.bodyTransform.rotation * updateRotation);
            if (updateVessel.packed)
            {
                updateVessel.srfRelRotation       = updateRotation;
                updateVessel.protoVessel.rotation = updateVessel.srfRelRotation;
            }

            //Angular velocity
            Vector3 angularVel = updateVessel.ReferenceTransform.rotation * new Vector3(angularVelocity[0], angularVelocity[1], angularVelocity[2]);

            updateVessel.precalc.CalculatePhysicsStats();

            if (updateVessel.parts != null)
            {
                for (int i = 0; i < updateVessel.parts.Count; i++)
                {
                    Part vesselPart = updateVessel.parts[i];
                    if (vesselPart.rb != null && vesselPart.State != PartStates.DEAD)
                    {
                        vesselPart.rb.angularVelocity = angularVel;
                        if (vesselPart != updateVessel.rootPart)
                        {
                            Vector3 rootVel           = FlightGlobals.ActiveVessel.rootPart.rb.velocity;
                            Vector3 diffPos           = vesselPart.rb.position - updateVessel.CoM;
                            Vector3 partVelDifference = Vector3.Cross(angularVel, diffPos);
                            vesselPart.rb.velocity = rootVel + partVelDifference;
                        }
                    }
                }
            }

            if (ctrlUpdate != null)
            {
                if (ctrlUpdate.ContainsKey(updateVessel.id))
                {
                    updateVessel.OnFlyByWire -= ctrlUpdate[updateVessel.id].UpdateControls;
                    ctrlUpdate.Remove(updateVessel.id);
                }
                VesselCtrlUpdate vcu = new VesselCtrlUpdate(updateVessel, ctrlUpdate, planetTime + 5, flightState);
                ctrlUpdate.Add(updateVessel.id, vcu);
                updateVessel.OnFlyByWire += vcu.UpdateControls;
            }

            //Action group controls
            updateVessel.ActionGroups.SetGroup(KSPActionGroup.Gear, actiongroupControls[0]);
            updateVessel.ActionGroups.SetGroup(KSPActionGroup.Light, actiongroupControls[1]);
            updateVessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, actiongroupControls[2]);
            updateVessel.ActionGroups.SetGroup(KSPActionGroup.SAS, actiongroupControls[3]);
            updateVessel.ActionGroups.SetGroup(KSPActionGroup.RCS, actiongroupControls[4]);

            if (sasEnabled)
            {
                updateVessel.Autopilot.SetMode((VesselAutopilot.AutopilotMode)autopilotMode);
                updateVessel.Autopilot.SAS.LockRotation(new Quaternion(lockedRotation[0], lockedRotation[1], lockedRotation[2], lockedRotation[3]));
            }
            posistionStatistics.LogError(updateVessel.id, distanceError, velocityError, rotationalError, planetTime);
        }