public void Apply() { if (HighLogic.LoadedScene == GameScenes.LOADING) { return; } //Get updating player string updatePlayer = LockSystem.fetch.LockExists(vesselID) ? LockSystem.fetch.LockOwner(vesselID) : "Unknown"; //Ignore updates to our own vessel if we are in flight and we aren't spectating if (!VesselWorker.fetch.isSpectating && (FlightGlobals.fetch.activeVessel != null ? FlightGlobals.fetch.activeVessel.id.ToString() == vesselID : false) && HighLogic.LoadedScene == GameScenes.FLIGHT) { DarkLog.Debug("ApplyVesselUpdate - Ignoring update for active vessel from " + updatePlayer); return; } Vessel updateVessel = FlightGlobals.fetch.vessels.FindLast(v => v.id.ToString() == 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; } //Rotation Quaternion updateRotation = new Quaternion(rotation[0], rotation[1], rotation[2], rotation[3]); updateVessel.SetRotation(updateVessel.mainBody.bodyTransform.rotation * updateRotation); if (updateVessel.packed) { updateVessel.srfRelRotation = updateRotation; updateVessel.protoVessel.rotation = updateVessel.srfRelRotation; } //Position/Velocity if (isSurfaceUpdate) { //Get the new position/velocity Vector3d updatePostion = updateBody.GetWorldSurfacePosition(position[0], position[1], position[2]); Vector3d updateVelocity = updateBody.bodyTransform.rotation * new Vector3d(velocity[0], velocity[1], velocity[2]); Vector3d updateAcceleration = updateBody.bodyTransform.rotation * new Vector3d(acceleration[0], acceleration[1], acceleration[2]); if (updateVessel.packed) { updateVessel.latitude = position[0]; updateVessel.longitude = position[1]; updateVessel.altitude = position[2]; updateVessel.protoVessel.latitude = updateVessel.latitude; updateVessel.protoVessel.longitude = updateVessel.longitude; updateVessel.protoVessel.altitude = updateVessel.altitude; if (!updateVessel.LandedOrSplashed) { //Not landed but under 10km. Vector3d orbitalPos = updatePostion - updateBody.position; Vector3d surfaceOrbitVelDiff = updateBody.getRFrmVel(updatePostion); Vector3d orbitalVel = updateVelocity + surfaceOrbitVelDiff; 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 { double planetariumDifference = Planetarium.GetUniversalTime() - planetTime; Vector3d positionFudge = Vector3d.zero; Vector3d velocityFudge = Vector3d.zero; if (Math.Abs(planetariumDifference) < 3f) { velocityFudge = updateAcceleration * planetariumDifference; //Use the average velocity to determine the new position positionFudge = (updateVelocity + (velocityFudge / 2)) * planetariumDifference; } Vector3d velocityOffset = (updateVelocity + velocityFudge) - updateVessel.srf_velocity; updateVessel.SetPosition(updatePostion + positionFudge, true); updateVessel.ChangeWorldVelocity(velocityOffset); } } 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()); if (updateVessel.packed) { //The OrbitDriver update call will set the vessel position on the next fixed update VesselUtil.CopyOrbit(updateOrbit, updateVessel.orbitDriver.orbit); updateVessel.orbitDriver.pos = updateVessel.orbitDriver.orbit.pos.xzy; updateVessel.orbitDriver.vel = updateVessel.orbitDriver.orbit.vel; } else { //Vessel.SetPosition is full of fun and games. Avoid at all costs. //Also, It's quite difficult to figure out the world velocity due to Krakensbane, and the reference frame. Vector3d posDelta = updateOrbit.getPositionAtUT(Planetarium.GetUniversalTime()) - updateVessel.orbitDriver.orbit.getPositionAtUT(Planetarium.GetUniversalTime()); Vector3d velDelta = updateOrbit.getOrbitalVelocityAtUT(Planetarium.GetUniversalTime()).xzy - updateVessel.orbitDriver.orbit.getOrbitalVelocityAtUT(Planetarium.GetUniversalTime()).xzy; //Vector3d velDelta = updateOrbit.vel.xzy - updateVessel.orbitDriver.orbit.vel.xzy; updateVessel.Translate(posDelta); updateVessel.ChangeWorldVelocity(velDelta); } } //Angular velocity //Vector3 angularVelocity = new Vector3(this.angularVelocity[0], this.angularVelocity[1], this.angularVelocity[2]); if (updateVessel.parts != null) { //Vector3 newAng = updateVessel.ReferenceTransform.rotation * angularVelocity; foreach (Part vesselPart in updateVessel.parts) { if (vesselPart.rb != null && !vesselPart.rb.isKinematic && vesselPart.State == PartStates.ACTIVE) { //The parts can have different rotations - This transforms them into the root part direction which is where the angular velocity is transferred. //vesselPart.rb.angularVelocity = (Quaternion.Inverse(updateVessel.rootPart.rb.rotation) * vesselPart.rb.rotation) * newAng; vesselPart.rb.angularVelocity = Vector3.zero; } } } //Flight state controls (Throttle etc) if (!VesselWorker.fetch.isSpectating) { updateVessel.ctrlState.CopyFrom(flightState); } else { FlightInputHandler.state.CopyFrom(flightState); } //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]); }
public void Apply() { 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; //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; 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; 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; 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 { Vector3d velocityOffset = updateVelocity - updateVessel.srf_velocity; updateVessel.SetPosition(updatePostion, true); updateVessel.ChangeWorldVelocity(velocityOffset); } } 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()); 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; if (updateVessel.packed) { //The OrbitDriver update call will set the vessel position on the next fixed update VesselUtil.CopyOrbit(updateOrbit, updateVessel.orbitDriver.orbit); updateVessel.orbitDriver.pos = updateVessel.orbitDriver.orbit.pos.xzy; updateVessel.orbitDriver.vel = updateVessel.orbitDriver.orbit.vel; } else { //Vessel.SetPosition is full of fun and games. Avoid at all costs. //Also, It's quite difficult to figure out the world velocity due to Krakensbane, and the reference frame. Vector3d posDelta = updateOrbit.getPositionAtUT(Planetarium.GetUniversalTime()) - updateVessel.orbitDriver.orbit.getPositionAtUT(Planetarium.GetUniversalTime()); Vector3d velDelta = updateOrbit.getOrbitalVelocityAtUT(Planetarium.GetUniversalTime()).xzy - updateVessel.orbitDriver.orbit.getOrbitalVelocityAtUT(Planetarium.GetUniversalTime()).xzy; //Vector3d velDelta = updateOrbit.vel.xzy - updateVessel.orbitDriver.orbit.vel.xzy; updateVessel.Translate(posDelta); updateVessel.ChangeWorldVelocity(velDelta); } } //Rotation Quaternion unfudgedRotation = new Quaternion(rotation[0], rotation[1], rotation[2], rotation[3]); Quaternion updateRotation = normalRotate * unfudgedRotation; updateVessel.SetRotation(updateVessel.mainBody.bodyTransform.rotation * updateRotation); if (updateVessel.packed) { updateVessel.srfRelRotation = updateRotation; updateVessel.protoVessel.rotation = updateVessel.srfRelRotation; } //Angular velocity Vector3 angularVelocity = updateVessel.mainBody.bodyTransform.rotation * updateRotation * new Vector3(this.angularVelocity[0], this.angularVelocity[1], this.angularVelocity[2]); if (updateVessel.parts != null) { foreach (Part vesselPart in updateVessel.parts) { if (vesselPart.rb != null && !vesselPart.rb.isKinematic && vesselPart.State == PartStates.ACTIVE) { vesselPart.rb.angularVelocity = angularVelocity; if (vesselPart != updateVessel.rootPart) { Vector3 rootPos = FlightGlobals.ActiveVessel.rootPart.rb.position; Vector3 rootVel = FlightGlobals.ActiveVessel.rootPart.rb.velocity; Vector3 diffPos = vesselPart.rb.position - rootPos; Vector3 partVelDifference = Vector3.Cross(angularVelocity, diffPos); vesselPart.rb.velocity = rootVel + partVelDifference; } } } } //Flight state controls (Throttle etc) if (!vesselWorker.isSpectating) { updateVessel.ctrlState.CopyFrom(flightState); } else { FlightInputHandler.state.CopyFrom(flightState); } //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]); }
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); }