diff --git a/Source/Camera/DockingCamera.cs b/Source/Camera/DockingCamera.cs index bd8a165..165aa47 100644 --- a/Source/Camera/DockingCamera.cs +++ b/Source/Camera/DockingCamera.cs @@ -15,12 +15,18 @@ class DockingCamera : BaseCamera private int _id; private int _idTextureNoise; - private Texture2D _textureVLineOLDD; - private Texture2D _textureHLineOLDD; - private Texture2D _textureVLine; - private Texture2D _textureHLine; - private Texture2D _textureVLineBack; - private Texture2D _textureHLineBack; + private Texture2D _VLineAttitude; + private Texture2D _HLineAttitude; + + private Texture2D _VLineDesiredVelocity; + private Texture2D _HLineDesiredVelocity; + private Texture2D _VLineDesiredVelocityBack; + private Texture2D _HLineDesiredVelocityBack; + + private Texture2D _VLineCurrentVelocity; + private Texture2D _HLineCurrentVelocity; + private Texture2D _VLineCurrentVelocityBack; + private Texture2D _HLineCurrentVelocityBack; internal readonly GameObject _moduleDockingNodeGameObject; private TargetHelper _target; @@ -34,47 +40,16 @@ class DockingCamera : BaseCamera private bool _rotatorState = true; private readonly float _maxSpeed = 2; - private Color _targetCrossColorOLDD = new Color(0, 0, 0.9f, 1); - private Color _targetCrossColorDPAI = new Color(0.5f, .0f, 0, 1); - private Color _targetCrossColorBack = new Color(.9f, 0, 0, 1); + private Color _colorAttitude = new Color(0, 0, 0.9f, 1); + private Color _colorDesiredVelocity = new Color(0.0f, .9f, 0, 1); + private Color _colorDesiredVelocityBack = new Color(.9f, 0, 0, 1); + private Color _colorCurrentVelocity = new Color(.9f, .9f, 0, 1); + private Color _colorCurrentVelocityBack = new Color(.9f, 0, .9f, 1); private string _lastVesselName; private string _windowLabelSuffix; Modules.DockingCameraModule _dcm; - - public Color TargetCrossColorOLDD - { - - get { return _targetCrossColorOLDD; } - set - { - _targetCrossColorOLDD = value; - _textureVLineOLDD = Util.MonoColorVerticalLineTexture(_targetCrossColorOLDD, (int)WindowSize * WindowSizeCoef); - _textureHLineOLDD = Util.MonoColorHorizontalLineTexture(_targetCrossColorOLDD, (int)WindowSize * WindowSizeCoef); - } - } - - public Color TargetCrossColorDPAI - { - get { return _targetCrossColorDPAI; } - set - { - _targetCrossColorDPAI = value; - _textureVLine = Util.MonoColorVerticalLineTexture(TargetCrossColorDPAI, (int)WindowSize * WindowSizeCoef); - _textureHLine = Util.MonoColorHorizontalLineTexture(TargetCrossColorDPAI, (int)WindowSize * WindowSizeCoef); - } - } - public Color TargetCrossColorBack - { - get { return _targetCrossColorBack; } - set - { - _targetCrossColorBack = value; - _textureVLineBack = Util.MonoColorVerticalLineTexture(TargetCrossColorBack, (int)WindowSize * WindowSizeCoef); - _textureHLineBack = Util.MonoColorHorizontalLineTexture(TargetCrossColorBack, (int)WindowSize * WindowSizeCoef); - } - } public void UpdateLocalPosition(Modules.DockingCameraModule dcm) @@ -142,12 +117,18 @@ private void LevelWasLoaded(GameScenes data) protected override void InitTextures() { base.InitTextures(); - _textureVLineOLDD = Util.MonoColorVerticalLineTexture(TargetCrossColorOLDD, (int)WindowSize * WindowSizeCoef); - _textureHLineOLDD = Util.MonoColorHorizontalLineTexture(TargetCrossColorOLDD, (int)WindowSize * WindowSizeCoef); - _textureVLine = Util.MonoColorVerticalLineTexture(TargetCrossColorDPAI, (int)WindowSize * WindowSizeCoef); - _textureHLine = Util.MonoColorHorizontalLineTexture(TargetCrossColorDPAI, (int)WindowSize * WindowSizeCoef); - _textureVLineBack = Util.MonoColorVerticalLineTexture(_targetCrossColorBack, (int)WindowSize * WindowSizeCoef); - _textureHLineBack = Util.MonoColorHorizontalLineTexture(_targetCrossColorBack, (int)WindowSize * WindowSizeCoef); + _VLineAttitude = Util.MonoColorVerticalLineTexture(_colorAttitude, (int)WindowSize * WindowSizeCoef); + _HLineAttitude = Util.MonoColorHorizontalLineTexture(_colorAttitude, (int)WindowSize * WindowSizeCoef); + + _VLineDesiredVelocity = Util.MonoColorVerticalLineTexture(_colorDesiredVelocity, (int)WindowSize * WindowSizeCoef); + _HLineDesiredVelocity = Util.MonoColorHorizontalLineTexture(_colorDesiredVelocity, (int)WindowSize * WindowSizeCoef); + _VLineDesiredVelocityBack = Util.MonoColorVerticalLineTexture(_colorDesiredVelocityBack, (int)WindowSize * WindowSizeCoef); + _HLineDesiredVelocityBack = Util.MonoColorHorizontalLineTexture(_colorDesiredVelocityBack, (int)WindowSize * WindowSizeCoef); + + _VLineCurrentVelocity = Util.MonoColorVerticalLineTexture(_colorCurrentVelocity, (int)WindowSize * WindowSizeCoef); + _HLineCurrentVelocity = Util.MonoColorHorizontalLineTexture(_colorCurrentVelocity, (int)WindowSize * WindowSizeCoef); + _VLineCurrentVelocityBack = Util.MonoColorVerticalLineTexture(_colorCurrentVelocityBack, (int)WindowSize * WindowSizeCoef); + _HLineCurrentVelocityBack = Util.MonoColorHorizontalLineTexture(_colorCurrentVelocityBack, (int)WindowSize * WindowSizeCoef); } protected override void ExtendedDrawWindowL1() @@ -274,17 +255,30 @@ private void GetWindowLabel() private void GetCross() { - if (TargetCrossDPAI && _target.IsDockPort) - { - var textV = _target.LookForward ? _textureVLine : _textureVLineBack; - var textH = _target.LookForward ? _textureHLine : _textureHLineBack; - var tx = _target.TargetMoveHelpX; - var ty = _target.TargetMoveHelpY; - if (!_target.LookForward) - { - tx = 1 - tx; - ty = 1 - ty; - } + if (TargetCrossDPAI) + { + var textV = _target.CurrentVelocityMarker.Forward ? _VLineCurrentVelocity : _VLineCurrentVelocityBack; + var textH = _target.CurrentVelocityMarker.Forward ? _HLineCurrentVelocity : _HLineCurrentVelocityBack; + float tx; + float ty; + tx = _target.CurrentVelocityMarker.X; + ty = 1.0f - _target.CurrentVelocityMarker.Y; + + GUI.DrawTexture(new Rect(TexturePosition.xMin + Math.Abs(tx * TexturePosition.width) % TexturePosition.width, + TexturePosition.yMin, 1, TexturePosition.height), textV); + GUI.DrawTexture(new Rect(TexturePosition.xMin, TexturePosition.yMin + + Math.Abs(ty * TexturePosition.height) % TexturePosition.height, TexturePosition.width, 1), textH); + } + + if (TargetCrossDPAI) + { + var textV = _target.DesiredVelocityMarker.Forward ? _VLineDesiredVelocity : _VLineDesiredVelocityBack; + var textH = _target.DesiredVelocityMarker.Forward ? _HLineDesiredVelocity : _HLineDesiredVelocityBack; + float tx; + float ty; + tx = _target.DesiredVelocityMarker.X; + ty = 1.0f - _target.DesiredVelocityMarker.Y; + GUI.DrawTexture(new Rect(TexturePosition.xMin + Math.Abs(tx * TexturePosition.width) % TexturePosition.width, TexturePosition.yMin, 1, TexturePosition.height), textV); GUI.DrawTexture(new Rect(TexturePosition.xMin, TexturePosition.yMin @@ -293,18 +287,15 @@ private void GetCross() if (TargetCrossOLDD && _target.IsDockPort) { - var tx = TexturePosition.width / 2; - var ty = TexturePosition.height / 2; - if (Mathf.Abs(_target.AngleX) > 20) - tx += (_target.AngleX > 0 ? -1 : 1) * (TexturePosition.width / 2 - 1); - else - tx += TexturePosition.width / 40 * -_target.AngleX; - if (Mathf.Abs(_target.AngleY) > 20) - ty += (_target.AngleY > 0 ? -1 : 1) * (TexturePosition.height / 2 - 1); - else - ty += TexturePosition.height / 40 * -_target.AngleY; - GUI.DrawTexture(new Rect(TexturePosition.xMin + tx, TexturePosition.yMin, 1, TexturePosition.height), _textureVLineOLDD); - GUI.DrawTexture(new Rect(TexturePosition.xMin, TexturePosition.yMin + ty, TexturePosition.width, 1), _textureHLineOLDD); + float tx; + float ty; + tx = 0.5f - _target.AngleY / 360.0f; + ty = 0.5f + _target.AngleX / 360.0f; + + GUI.DrawTexture(new Rect(TexturePosition.xMin + Math.Abs(tx * TexturePosition.width) % TexturePosition.width, + TexturePosition.yMin, 1, TexturePosition.height), _VLineAttitude); + GUI.DrawTexture(new Rect(TexturePosition.xMin, TexturePosition.yMin + + Math.Abs(ty * TexturePosition.height) % TexturePosition.height, TexturePosition.width, 1), _HLineAttitude); } } @@ -332,29 +323,29 @@ private void GetFlightData() GUI.Label(new Rect(TexturePosition.xMax - 70, 34 + i++ * stringOffset, 70, 20), string.Format("Dist:" + dataFormat, _target.Destination), Styles.Label13B); i += .2f; - GUI.Label(new Rect(TexturePosition.xMax - 70, 34 + i++ * stringOffset, 70, 20), string.Format("dX:" + dataFormat, _target.DX)); - GUI.Label(new Rect(TexturePosition.xMax - 70, 34 + i++ * stringOffset, 70, 20), string.Format("dY:" + dataFormat, _target.DY)); - GUI.Label(new Rect(TexturePosition.xMax - 70, 34 + i++ * stringOffset, 70, 20), string.Format("dZ:" + dataFormat, _target.DZ)); + GUI.Label(new Rect(TexturePosition.xMax - 70, 34 + i++ * stringOffset, 70, 20), string.Format("dX:" + dataFormat, -_target.DPos.x)); + GUI.Label(new Rect(TexturePosition.xMax - 70, 34 + i++ * stringOffset, 70, 20), string.Format("dY:" + dataFormat, -_target.DPos.y)); + GUI.Label(new Rect(TexturePosition.xMax - 70, 34 + i++ * stringOffset, 70, 20), string.Format("dZ:" + dataFormat, -_target.DPos.z)); i += .2f; - if (Math.Abs(_target.SpeedX) > _maxSpeed && Math.Abs(_target.Destination) < 200) - GUI.Label(new Rect(TexturePosition.xMax - 70, 38 + i++ * stringOffset, 70, 20), $"vX:{_target.SpeedX:f2}", Styles.RedLabel13); + if (Math.Abs(_target.Velocity.x) > _maxSpeed && Math.Abs(_target.Destination) < 200) + GUI.Label(new Rect(TexturePosition.xMax - 70, 38 + i++ * stringOffset, 70, 20), $"vX:{_target.Velocity.x:f2}", Styles.RedLabel13); else - GUI.Label(new Rect(TexturePosition.xMax - 70, 38 + i++ * stringOffset, 70, 20), $"vX:{_target.SpeedX:f2}", Styles.Label13); + GUI.Label(new Rect(TexturePosition.xMax - 70, 38 + i++ * stringOffset, 70, 20), $"vX:{_target.Velocity.x:f2}", Styles.Label13); - if (Math.Abs(_target.SpeedY) > _maxSpeed && Math.Abs(_target.Destination) < 200) - GUI.Label(new Rect(TexturePosition.xMax - 70, 38 + i++ * stringOffset, 70, 20), $"vY:{_target.SpeedY:f2}", Styles.RedLabel13); + if (Math.Abs(_target.Velocity.y) > _maxSpeed && Math.Abs(_target.Destination) < 200) + GUI.Label(new Rect(TexturePosition.xMax - 70, 38 + i++ * stringOffset, 70, 20), $"vY:{_target.Velocity.y:f2}", Styles.RedLabel13); else - GUI.Label(new Rect(TexturePosition.xMax - 70, 38 + i++ * stringOffset, 70, 20), $"vY:{_target.SpeedY:f2}", Styles.Label13); + GUI.Label(new Rect(TexturePosition.xMax - 70, 38 + i++ * stringOffset, 70, 20), $"vY:{_target.Velocity.y:f2}", Styles.Label13); - if (Math.Abs(_target.SpeedZ) > _maxSpeed && Math.Abs(_target.Destination) < 200) - GUI.Label(new Rect(TexturePosition.xMax - 70, 38 + i++ * stringOffset, 70, 20), $"vZ:{_target.SpeedZ:f2}", Styles.RedLabel13); + if (Math.Abs(_target.Velocity.z) > _maxSpeed && Math.Abs(_target.Destination) < 200) + GUI.Label(new Rect(TexturePosition.xMax - 70, 38 + i++ * stringOffset, 70, 20), $"vZ:{_target.Velocity.z:f2}", Styles.RedLabel13); else - GUI.Label(new Rect(TexturePosition.xMax - 70, 38 + i++ * stringOffset, 70, 20), $"vZ:{_target.SpeedZ:f2}", Styles.Label13); + GUI.Label(new Rect(TexturePosition.xMax - 70, 38 + i++ * stringOffset, 70, 20), $"vZ:{_target.Velocity.z:f2}", Styles.Label13); i += .2f; - GUI.Label(new Rect(TexturePosition.xMax - 70, 40 + i++ * stringOffset, 70, 20), $"Yaw:{_target.AngleX:f0}°"); - GUI.Label(new Rect(TexturePosition.xMax - 70, 40 + i++ * stringOffset, 70, 20), $"Pitch:{_target.AngleY:f0}°"); + GUI.Label(new Rect(TexturePosition.xMax - 70, 40 + i++ * stringOffset, 70, 20), $"Yaw:{_target.AngleY:f0}°"); + GUI.Label(new Rect(TexturePosition.xMax - 70, 40 + i++ * stringOffset, 70, 20), $"Pitch:{_target.AngleX:f0}°"); GUI.Label(new Rect(TexturePosition.xMax - 70, 40 + i * stringOffset, 70, 20), $"Roll:{_target.AngleZ:f0}°"); } } diff --git a/Source/Utils/TargetHelper.cs b/Source/Utils/TargetHelper.cs index bc86e97..73d77b7 100644 --- a/Source/Utils/TargetHelper.cs +++ b/Source/Utils/TargetHelper.cs @@ -11,21 +11,24 @@ public class TargetHelper { private readonly GameObject _self; private readonly Part _selfPart; - public float DX; - public float DY; - public float DZ; - public float SpeedX; - public float SpeedY; - public float SpeedZ; + public Matrix4x4 CoordinateTransform; + public Vector3 DPos; + public Vector3 Velocity; public float AngleX; public float AngleY; public float AngleZ; public float Destination; public bool IsMoveToTarget; public float SecondsToDock; - public bool LookForward; - public float TargetMoveHelpX; - public float TargetMoveHelpY; + + public struct VelocityMarker + { + public bool Forward; + public float X; + public float Y; + } + public VelocityMarker DesiredVelocityMarker; + public VelocityMarker CurrentVelocityMarker; /// Object of comparison public TargetHelper(Part from) @@ -43,28 +46,65 @@ public TargetHelper(Part from) public void Update() { + UpdateTransformationMatrix(); UpdatePosition(); - var velocity = UpdateSpeed(); - Destination = (float)Math.Sqrt(Math.Pow(DX, 2) + Math.Pow(DY, 2) + Math.Pow(DZ, 2)); + UpdateVelocity(); + Destination = DPos.magnitude; UpdateAngles(); - UpdateIsMoveToTarget(velocity); - UpdateTargetMoveHelp(); + UpdateIsMoveToTarget(); + DesiredVelocityMarker = UpdateVelocityMarker(DPos); + CurrentVelocityMarker = UpdateVelocityMarker(Velocity); + } + + private void UpdateTransformationMatrix() + { + CoordinateTransform.SetColumn(0, _self.transform.right); + CoordinateTransform.SetColumn(1, -_self.transform.forward); + CoordinateTransform.SetColumn(2, _self.transform.up); + CoordinateTransform.SetColumn(3, new Vector4(0, 0, 0, 1)); + CoordinateTransform = CoordinateTransform.inverse; } private void UpdatePosition() { - DX = targetTransform.position.x - _self.transform.position.x; - DY = targetTransform.position.y - _self.transform.position.y; - DZ = targetTransform.position.z - _self.transform.position.z; + DPos = targetTransform.position - _self.transform.position; + DPos = CoordinateTransform.MultiplyPoint3x4(DPos); + } + + private void UpdateVelocity() + { + Velocity = _selfPart.vessel.GetObtVelocity() - Target.GetObtVelocity(); + Velocity = CoordinateTransform.MultiplyPoint3x4(Velocity); + } + + private void UpdateAngles() + { + AngleX = -SignedAngleAroundVector(-targetTransform.forward, _self.transform.up, _self.transform.right); + AngleY = SignedAngleAroundVector(-targetTransform.forward, _self.transform.up, -_self.transform.forward); + AngleZ = SignedAngleAroundVector(targetTransform.up, _self.transform.forward, -_self.transform.up); } - private void UpdateIsMoveToTarget(Vector3 velocity) // dockingLamp- + private VelocityMarker UpdateVelocityMarker(Vector3 velocity) + { + const float epsilon = 0.000001f; + VelocityMarker velocityMarker = new VelocityMarker(); + float zOverX = Mathf.Sign(velocity.x) * velocity.z / (Mathf.Abs(velocity.x) + epsilon); + float zOverY = Mathf.Sign(velocity.y) * velocity.z / (Mathf.Abs(velocity.y) + epsilon); + velocityMarker.X = Mathf.Sign(velocity.z)*Mathf.Sign(zOverX) / (1 + Mathf.Abs(zOverX)); + velocityMarker.Y = Mathf.Sign(velocity.z)*Mathf.Sign(zOverY) / (1 + Mathf.Abs(zOverY)); + velocityMarker.X = (velocityMarker.X + 1) / 2; + velocityMarker.Y = (velocityMarker.Y + 1) / 2; + velocityMarker.Forward = velocity.z > 0; + return velocityMarker; + } + + private void UpdateIsMoveToTarget() // dockingLamp- { var checkedDevByZero = false; try { - SecondsToDock = Destination / velocity.magnitude; + SecondsToDock = Destination / Velocity.magnitude; checkedDevByZero = true; } catch (DivideByZeroException) @@ -76,63 +116,47 @@ private void UpdateIsMoveToTarget(Vector3 velocity) // dockingLamp- float timeX; float timeY; - if (SpeedX == 0 && Mathf.Abs(DX) < .5f) + if (Velocity.x == 0 && Mathf.Abs(DPos.x) < .5f) timeX = SecondsToDock; else - timeX = (Mathf.Abs(DX) < .5f) ? SecondsToDock : -DX / SpeedX; + timeX = (Mathf.Abs(DPos.x) < .5f) ? SecondsToDock : -DPos.x / Velocity.x; - if (SpeedY == 0 && Mathf.Abs(DY) < .5f) + if (Velocity.y == 0 && Mathf.Abs(DPos.y) < .5f) timeY = SecondsToDock; else - timeY = (Mathf.Abs(DY) < .5f) ? SecondsToDock : -DY / SpeedY; + timeY = (Mathf.Abs(DPos.y) < .5f) ? SecondsToDock : -DPos.y / Velocity.y; IsMoveToTarget = Mathf.Abs(SecondsToDock - timeX) < 1 && Mathf.Abs(SecondsToDock - timeY) < 1 && - DZ * SpeedZ < 0; + DPos.z * Velocity.z < 0; } - private void UpdateAngles() - { - AngleX = SignedAngleAroundVector(-targetTransform.forward, _self.transform.up, -_self.transform.forward); - AngleY = SignedAngleAroundVector(-targetTransform.forward, _self.transform.up, _self.transform.right); - AngleZ = SignedAngleAroundVector(targetTransform.up, -_self.transform.forward, -_self.transform.up); - } - - private Vector3 UpdateSpeed() - { - var velocity = Target.GetObtVelocity() - _selfPart.vessel.GetObtVelocity(); - SpeedX = velocity.x; - SpeedY = velocity.y; - SpeedZ = velocity.z; - return velocity; - } - - private void UpdateTargetMoveHelp() - { - Vector3 targetToOwnship = _self.transform.position - targetTransform.position; - var translationDeviation = new Vector2( - SignedAngleAroundVector(targetToOwnship, targetTransform.forward.normalized, _self.transform.forward), - SignedAngleAroundVector(targetToOwnship, targetTransform.forward.normalized, -_self.transform.right)); - LookForward = Math.Abs(translationDeviation.x) < 90; - float gaugeX = (LookForward ? 1 : -1) * ((translationDeviation.x / 90f)%2); - float gaugeY = (LookForward ? 1 : -1) * ((translationDeviation.y / 90f)%2); - float exponent = .75f; - - if (Destination <= 5f) - exponent = 1f; - else - if (Destination < 15f) - { - float toGo = Destination - 5f; - float range = 15f - 5f; - float lerp = toGo / range; - float exponentReduction = 1f - .75f; - exponent = 1 - (exponentReduction) * lerp; - } - - TargetMoveHelpX = (ScaleExponentially(gaugeX, exponent) + 1) / 2f; - TargetMoveHelpY = (ScaleExponentially(gaugeY, exponent) + 1) / 2f; - } + //private void UpdateTargetMoveHelp() + //{ + // Vector3 targetToOwnship = _self.transform.position - targetTransform.position; + // var translationDeviation = new Vector2( + // SignedAngleAroundVector(targetToOwnship, targetTransform.forward.normalized, _self.transform.forward), + // SignedAngleAroundVector(targetToOwnship, targetTransform.forward.normalized, -_self.transform.right)); + // LookForward = Math.Abs(translationDeviation.x) < 90; + // float gaugeX = (LookForward ? 1 : -1) * ((translationDeviation.x / 90f)%2); + // float gaugeY = (LookForward ? 1 : -1) * ((translationDeviation.y / 90f)%2); + // float exponent = .75f; + + // if (Destination <= 5f) + // exponent = 1f; + // else + // if (Destination < 15f) + // { + // float toGo = Destination - 5f; + // float range = 15f - 5f; + // float lerp = toGo / range; + // float exponentReduction = 1f - .75f; + // exponent = 1 - (exponentReduction) * lerp; + // } + + // TargetMoveHelpX = (ScaleExponentially(gaugeX, exponent) + 1) / 2f; + // TargetMoveHelpY = (ScaleExponentially(gaugeY, exponent) + 1) / 2f; + //} private static float SignedAngleAroundVector(Vector3 a, Vector3 b, Vector3 c) {