public static VesselUpdate CopyFromVessel(VesselWorker vesselWorker, Vessel updateVessel)
{
VesselUpdate returnUpdate = new VesselUpdate(vesselWorker);
try
{
returnUpdate.vesselID = updateVessel.id;
returnUpdate.planetTime = Planetarium.GetUniversalTime();
returnUpdate.bodyName = updateVessel.mainBody.bodyName;
returnUpdate.rotation = new float[4];
returnUpdate.rotation[0] = updateVessel.srfRelRotation.x;
returnUpdate.rotation[1] = updateVessel.srfRelRotation.y;
returnUpdate.rotation[2] = updateVessel.srfRelRotation.z;
returnUpdate.rotation[3] = updateVessel.srfRelRotation.w;
returnUpdate.angularVelocity = new float[3];
returnUpdate.angularVelocity[0] = updateVessel.angularVelocity.x;
returnUpdate.angularVelocity[1] = updateVessel.angularVelocity.y;
returnUpdate.angularVelocity[2] = updateVessel.angularVelocity.z;
//Flight state
returnUpdate.flightState = new FlightCtrlState();
returnUpdate.flightState.CopyFrom(updateVessel.ctrlState);
returnUpdate.actiongroupControls = new bool[5];
returnUpdate.actiongroupControls[0] = updateVessel.ActionGroups[KSPActionGroup.Gear];
returnUpdate.actiongroupControls[1] = updateVessel.ActionGroups[KSPActionGroup.Light];
returnUpdate.actiongroupControls[2] = updateVessel.ActionGroups[KSPActionGroup.Brakes];
returnUpdate.actiongroupControls[3] = updateVessel.ActionGroups[KSPActionGroup.SAS];
returnUpdate.actiongroupControls[4] = updateVessel.ActionGroups[KSPActionGroup.RCS];
if (updateVessel.altitude < 10000)
{
//Use surface position under 10k
returnUpdate.isSurfaceUpdate = true;
returnUpdate.position = new double[4];
returnUpdate.position[0] = updateVessel.latitude;
returnUpdate.position[1] = updateVessel.longitude;
returnUpdate.position[2] = updateVessel.altitude;
VesselUtil.DMPRaycastPair groundRaycast = VesselUtil.RaycastGround(updateVessel.latitude, updateVessel.longitude, updateVessel.mainBody);
returnUpdate.position[3] = groundRaycast.altitude;
returnUpdate.terrainNormal = new float[3];
returnUpdate.terrainNormal[0] = groundRaycast.terrainNormal.x;
returnUpdate.terrainNormal[1] = groundRaycast.terrainNormal.y;
returnUpdate.terrainNormal[2] = groundRaycast.terrainNormal.z;
returnUpdate.velocity = new double[3];
Vector3d srfVel = Quaternion.Inverse(updateVessel.mainBody.bodyTransform.rotation) * updateVessel.srf_velocity;
returnUpdate.velocity[0] = srfVel.x;
returnUpdate.velocity[1] = srfVel.y;
returnUpdate.velocity[2] = srfVel.z;
returnUpdate.acceleration = new double[3];
Vector3d srfAcceleration = Quaternion.Inverse(updateVessel.mainBody.bodyTransform.rotation) * updateVessel.acceleration;
returnUpdate.acceleration[0] = srfAcceleration.x;
returnUpdate.acceleration[1] = srfAcceleration.y;
returnUpdate.acceleration[2] = srfAcceleration.z;
}
else
{
//Use orbital positioning over 10k
returnUpdate.isSurfaceUpdate = false;
returnUpdate.orbit = new double[7];
returnUpdate.orbit[0] = updateVessel.orbit.inclination;
returnUpdate.orbit[1] = updateVessel.orbit.eccentricity;
returnUpdate.orbit[2] = updateVessel.orbit.semiMajorAxis;
returnUpdate.orbit[3] = updateVessel.orbit.LAN;
returnUpdate.orbit[4] = updateVessel.orbit.argumentOfPeriapsis;
returnUpdate.orbit[5] = updateVessel.orbit.meanAnomalyAtEpoch;
returnUpdate.orbit[6] = updateVessel.orbit.epoch;
}
returnUpdate.sasEnabled = updateVessel.Autopilot.Enabled;
if (returnUpdate.sasEnabled)
{
returnUpdate.autopilotMode = (int)updateVessel.Autopilot.Mode;
returnUpdate.lockedRotation = new float[4];
returnUpdate.lockedRotation[0] = updateVessel.Autopilot.SAS.lockedRotation.x;
returnUpdate.lockedRotation[1] = updateVessel.Autopilot.SAS.lockedRotation.y;
returnUpdate.lockedRotation[2] = updateVessel.Autopilot.SAS.lockedRotation.z;
returnUpdate.lockedRotation[3] = updateVessel.Autopilot.SAS.lockedRotation.w;
}
}
catch (Exception e)
{
DarkLog.Debug("Failed to get vessel update, exception: " + e);
returnUpdate = null;
}
return(returnUpdate);
}
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);
}
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 CopyFromVessel(Vessel updateVessel)
{
updateOK = true;
SetVesselWorker(vesselWorker);
try
{
vesselID = updateVessel.id;
planetTime = Planetarium.GetUniversalTime();
bodyName = updateVessel.mainBody.bodyName;
rotation[0] = updateVessel.srfRelRotation.x;
rotation[1] = updateVessel.srfRelRotation.y;
rotation[2] = updateVessel.srfRelRotation.z;
rotation[3] = updateVessel.srfRelRotation.w;
angularVelocity[0] = updateVessel.angularVelocity.x;
angularVelocity[1] = updateVessel.angularVelocity.y;
angularVelocity[2] = updateVessel.angularVelocity.z;
//Flight state
flightState.CopyFrom(updateVessel.ctrlState);
actiongroupControls[0] = updateVessel.ActionGroups[KSPActionGroup.Gear];
actiongroupControls[1] = updateVessel.ActionGroups[KSPActionGroup.Light];
actiongroupControls[2] = updateVessel.ActionGroups[KSPActionGroup.Brakes];
actiongroupControls[3] = updateVessel.ActionGroups[KSPActionGroup.SAS];
actiongroupControls[4] = updateVessel.ActionGroups[KSPActionGroup.RCS];
if (updateVessel.altitude < 10000)
{
//Use surface position under 10k
isSurfaceUpdate = true;
position[0] = updateVessel.latitude;
position[1] = updateVessel.longitude;
position[2] = updateVessel.altitude;
VesselUtil.DMPRaycastPair groundRaycast = VesselUtil.RaycastGround(updateVessel.latitude, updateVessel.longitude, updateVessel.mainBody);
position[3] = groundRaycast.altitude;
terrainNormal[0] = groundRaycast.terrainNormal.x;
terrainNormal[1] = groundRaycast.terrainNormal.y;
terrainNormal[2] = groundRaycast.terrainNormal.z;
Vector3d srfVel = Quaternion.Inverse(updateVessel.mainBody.bodyTransform.rotation) * updateVessel.srf_velocity;
velocity[0] = srfVel.x;
velocity[1] = srfVel.y;
velocity[2] = srfVel.z;
Vector3d srfAcceleration = Quaternion.Inverse(updateVessel.mainBody.bodyTransform.rotation) * updateVessel.acceleration;
acceleration[0] = srfAcceleration.x;
acceleration[1] = srfAcceleration.y;
acceleration[2] = srfAcceleration.z;
}
else
{
//Use orbital positioning over 10k
isSurfaceUpdate = false;
orbit[0] = updateVessel.orbit.inclination;
orbit[1] = updateVessel.orbit.eccentricity;
orbit[2] = updateVessel.orbit.semiMajorAxis;
orbit[3] = updateVessel.orbit.LAN;
orbit[4] = updateVessel.orbit.argumentOfPeriapsis;
orbit[5] = updateVessel.orbit.meanAnomalyAtEpoch;
orbit[6] = updateVessel.orbit.epoch;
}
sasEnabled = updateVessel.Autopilot.Enabled;
if (sasEnabled)
{
autopilotMode = (int)updateVessel.Autopilot.Mode;
lockedRotation[0] = updateVessel.Autopilot.SAS.lockedRotation.x;
lockedRotation[1] = updateVessel.Autopilot.SAS.lockedRotation.y;
lockedRotation[2] = updateVessel.Autopilot.SAS.lockedRotation.z;
lockedRotation[3] = updateVessel.Autopilot.SAS.lockedRotation.w;
}
}
catch (Exception e)
{
DarkLog.Debug("Failed to get vessel update, exception: " + e);
updateOK = false;
}
}
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);
}