Merge branch 'sarbian'
[VesselSimulator.git] / KerbalEngineer / Flight / Readouts / Vessel / AttitudeProcessor.cs
blob:a/KerbalEngineer/Flight/Readouts/Vessel/AttitudeProcessor.cs -> blob:b/KerbalEngineer/Flight/Readouts/Vessel/AttitudeProcessor.cs
// //
// Kerbal Engineer Redux // Kerbal Engineer Redux
// //
// Copyright (C) 2014 CYBUTEK // Copyright (C) 2014 CYBUTEK
// //
// This program is free software: you can redistribute it and/or modify // This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by // it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or // the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version. // (at your option) any later version.
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details. // GNU General Public License for more details.
// //
// You should have received a copy of the GNU General Public License // You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
// //
   
namespace KerbalEngineer.Flight.Readouts.Vessel namespace KerbalEngineer.Flight.Readouts.Vessel
{ {
#region Using Directives #region Using Directives
   
using UnityEngine; using UnityEngine;
   
#endregion #endregion
   
public class AttitudeProcessor : IUpdatable, IUpdateRequest public class AttitudeProcessor : IUpdatable, IUpdateRequest
{ {
#region Fields #region Fields
   
private static readonly AttitudeProcessor instance = new AttitudeProcessor(); private static readonly AttitudeProcessor instance = new AttitudeProcessor();
   
private Vector3 centreOfMass = Vector3.zero; private Vector3 centreOfMass = Vector3.zero;
   
private double heading; private double heading;
private double headingRate; private double headingRate;
private Vector3 north = Vector3.zero; private Vector3 north = Vector3.zero;
private double pitch; private double pitch;
private double pitchRate; private double pitchRate;
private double previousHeading; private double previousHeading;
private double previousPitch; private double previousPitch;
private double previousRoll; private double previousRoll;
private double roll; private double roll;
private double rollRate; private double rollRate;
private Quaternion surfaceRotation; private Quaternion surfaceRotation;
private Vector3 up = Vector3.zero; private Vector3 up = Vector3.zero;
   
#endregion #endregion
   
#region Properties #region Properties
   
public static double Heading public static double Heading
{ {
get { return instance.heading; } get { return instance.heading; }
} }
   
public static double HeadingRate public static double HeadingRate
{ {
get { return instance.headingRate; } get { return instance.headingRate; }
} }
   
public static AttitudeProcessor Instance public static AttitudeProcessor Instance
{ {
get { return instance; } get { return instance; }
} }
   
public static double Pitch public static double Pitch
{ {
get { return instance.pitch; } get { return instance.pitch; }
} }
   
public static double PitchRate public static double PitchRate
{ {
get { return instance.pitchRate; } get { return instance.pitchRate; }
} }
   
public static double Roll public static double Roll
{ {
get { return instance.roll; } get { return instance.roll; }
} }
   
public static double RollRate public static double RollRate
{ {
get { return instance.rollRate; } get { return instance.rollRate; }
} }
   
public bool UpdateRequested { get; set; } public bool UpdateRequested { get; set; }
   
#endregion #endregion
   
#region Methods #region Methods
   
public static void RequestUpdate() public static void RequestUpdate()
{ {
instance.UpdateRequested = true; instance.UpdateRequested = true;
} }
   
public void Update() public void Update()
{ {
this.surfaceRotation = this.GetSurfaceRotation(); this.surfaceRotation = this.GetSurfaceRotation();
   
this.previousHeading = this.heading; this.previousHeading = this.heading;
this.previousPitch = this.pitch; this.previousPitch = this.pitch;
this.previousRoll = this.roll; this.previousRoll = this.roll;
   
// This code was derived from MechJeb2's implementation for getting the vessel's surface relative rotation. // This code was derived from MechJeb2's implementation for getting the vessel's surface relative rotation.
this.heading = this.surfaceRotation.eulerAngles.y; this.heading = this.surfaceRotation.eulerAngles.y;
this.pitch = this.surfaceRotation.eulerAngles.x > 180.0f this.pitch = this.surfaceRotation.eulerAngles.x > 180.0f
? 360.0f - this.surfaceRotation.eulerAngles.x ? 360.0f - this.surfaceRotation.eulerAngles.x
: -this.surfaceRotation.eulerAngles.x; : -this.surfaceRotation.eulerAngles.x;
this.roll = this.surfaceRotation.eulerAngles.z > 180.0f this.roll = this.surfaceRotation.eulerAngles.z > 180.0f
? this.surfaceRotation.eulerAngles.z - 360.0f ? this.surfaceRotation.eulerAngles.z - 360.0f
: this.surfaceRotation.eulerAngles.z; : this.surfaceRotation.eulerAngles.z;
   
this.headingRate = this.heading - this.previousHeading; this.headingRate = this.heading - this.previousHeading;
this.pitchRate = this.pitch - this.previousPitch; this.pitchRate = this.pitch - this.previousPitch;
this.rollRate = this.roll - this.previousRoll; this.rollRate = this.roll - this.previousRoll;
} }
   
private Quaternion GetSurfaceRotation() private Quaternion GetSurfaceRotation()
{ {
// This code was derived from MechJeb2's implementation for getting the vessel's surface relative rotation. // This code was derived from MechJeb2's implementation for getting the vessel's surface relative rotation.
this.centreOfMass = FlightGlobals.ActiveVessel.findWorldCenterOfMass(); this.centreOfMass = FlightGlobals.ActiveVessel.findWorldCenterOfMass();
this.up = (this.centreOfMass - FlightGlobals.ActiveVessel.mainBody.position).normalized; this.up = (this.centreOfMass - FlightGlobals.ActiveVessel.mainBody.position).normalized;
this.north = Vector3.Exclude(this.up, (FlightGlobals.ActiveVessel.mainBody.position + FlightGlobals.ActiveVessel.mainBody.transform.up * (float)FlightGlobals.ActiveVessel.mainBody.Radius) - this.centreOfMass).normalized; this.north = Vector3.ProjectOnPlane((FlightGlobals.ActiveVessel.mainBody.position + FlightGlobals.ActiveVessel.mainBody.transform.up * (float)FlightGlobals.ActiveVessel.mainBody.Radius) - this.centreOfMass, this.up).normalized;
   
return Quaternion.Inverse(Quaternion.Euler(90.0f, 0.0f, 0.0f) * Quaternion.Inverse(FlightGlobals.ActiveVessel.transform.rotation) * Quaternion.LookRotation(this.north, this.up)); return Quaternion.Inverse(Quaternion.Euler(90.0f, 0.0f, 0.0f) * Quaternion.Inverse(FlightGlobals.ActiveVessel.transform.rotation) * Quaternion.LookRotation(this.north, this.up));
} }
   
#endregion #endregion
} }
} }