public double getCableTension(double cableLength, double elasticExtension, winchDirection _winchDirection)
{
double cableTension = 0;
double dumpingLength = Math.Max(5.0, cableLength * elasticExtension / 100);
// CALCULATION TENSION MODIFIER
if (cableLength < _winchDirection.distance)
{
cableTension = (_winchDirection.distance - cableLength) / dumpingLength;
}
return(cableTension);
}
public void updateRadarThermals(Canvas RadarCanvas, string id, winchDirection thermalDirection, winchPosition thermalPosition, double finalModifier, double scale)
{
scale = Math.Max(0.1, scale) / 125 * 1000;
try
{
foreach (UIElement canv in RadarCanvas.Children)
{
if (canv.GetType() == typeof(Canvas) && ((Canvas)canv).Name == id.Split('_')[0] + "s")
{
foreach (UIElement el in ((Canvas)canv).Children)
{
if (el.GetType() == typeof(Ellipse) && ((Ellipse)el).Name == id)
{
Ellipse circle = (Ellipse)el;
circle.Width = thermalPosition.radius * 2 / scale;
circle.Height = thermalPosition.radius * 2 / scale;
SolidColorBrush strokeBrush = new SolidColorBrush(finalModifier >= 0 ? Colors.Red : Colors.Blue);
strokeBrush.Opacity = Math.Abs(finalModifier >= 0 ? Math.Max(0.1, finalModifier) : Math.Min(-0.1, finalModifier));
circle.Fill = strokeBrush;
Canvas.SetLeft(circle, 125 - circle.Width / 2 + thermalDirection.groundDistance / scale * Math.Sin(thermalDirection.heading));
Canvas.SetTop(circle, 125 - circle.Height / 2 - thermalDirection.groundDistance / scale * Math.Cos(thermalDirection.heading));
//Console.WriteLine(id + " scale: " + scale + " modif: " + finalModifier);
//break;
}
else if (el.GetType() == typeof(Label) && ((Label)el).Name == "Label_" + id)
{
Label label = (Label)el;
Canvas.SetLeft(label, 110 + thermalDirection.groundDistance / scale * Math.Sin(thermalDirection.heading));
Canvas.SetTop(label, 115 - thermalDirection.groundDistance / scale * Math.Cos(thermalDirection.heading));
//break;
}
}
break;
}
}
}
catch (Exception ex)
{
Console.WriteLine("Failed to update radar thermals: " + ex.Message);
}
}
public void updateRadarNearby(Canvas RadarCanvas, uint id, winchDirection planeDirection, winchPosition planePosition, bool currentTowPlane, double scale)
{
scale = Math.Max(0.1, scale) / 125 * 1000;
try
{
foreach (UIElement canv in RadarCanvas.Children)
{
if (canv.GetType() == typeof(Canvas) && ((Canvas)canv).Name == "Nearbies")
{
foreach (UIElement el in ((Canvas)canv).Children)
{
if (el.GetType() == typeof(Ellipse) && ((Ellipse)el).Name == "Nearby_" + id.ToString())
{
Ellipse circle = (Ellipse)el;
circle.Width = 10;
circle.Height = 10;
SolidColorBrush strokeBrush = new SolidColorBrush(currentTowPlane ? Colors.DarkRed : Colors.DarkGreen);
//strokeBrush.Opacity = Math.Abs(finalModifier);
circle.Fill = strokeBrush;
circle.Visibility = Visibility.Visible;
Canvas.SetLeft(circle, 125 - circle.Width / 2 + planeDirection.groundDistance / scale * Math.Sin(planeDirection.heading));
Canvas.SetTop(circle, 125 - circle.Height / 2 - planeDirection.groundDistance / scale * Math.Cos(planeDirection.heading));
}
else if (el.GetType() == typeof(Button) && ((Button)el).Name == "NearbyLabel_" + id.ToString())
{
Button label = (Button)el;
label.Visibility = Visibility.Visible;
label.Foreground = new SolidColorBrush(currentTowPlane ? Colors.DarkRed : Colors.DarkGreen);
label.Margin = new Thickness(125 + planeDirection.groundDistance / scale * Math.Sin(planeDirection.heading),
125 - planeDirection.groundDistance / scale * Math.Cos(planeDirection.heading), 0, 0);
}
}
break;
}
}
}
catch (Exception ex)
{
Console.WriteLine("Failed to update radar nearby: " + ex.Message);
}
}
public winchDirection getForceDirection(winchPosition _winchPosition, PlaneInfoResponse _planeInfoResponse)
{
winchDirection _winchDirection = new winchDirection();
double globalX = (_winchPosition.location.Longitude - _planeInfoResponse.Longitude) * Math.Cos(_winchPosition.location.Latitude) * 6378137;
double globalY = _winchPosition.alt - _planeInfoResponse.Altitude;
double globalZ = (_winchPosition.location.Latitude - _planeInfoResponse.Latitude) * 180 / Math.PI * 111694;
Vector3 globalToWinch = new Vector3((float)globalX, (float)globalY, (float)globalZ);
Vector3 globalToWinchNorm = globalToWinch;
globalToWinchNorm.Normalize();
_winchDirection.climbAngle = Math.Abs(Math.Asin(globalToWinchNorm.Y));
Matrix3x3 attitude = Matrix3x3.CreateFromYawPitchRoll((float)_planeInfoResponse.PlaneHeading, (float)_planeInfoResponse.PlanePitch, (float)_planeInfoResponse.PlaneBank);
_winchDirection.localForceDirection = Matrix3x3.Multiply(attitude.Inverse(), globalToWinchNorm);
_winchDirection.localForceDirection.Normalize();
_winchDirection.heading = Math.Atan2(_winchDirection.localForceDirection.X, _winchDirection.localForceDirection.Z);
_winchDirection.pitch = Math.Asin(_winchDirection.localForceDirection.Y);
_winchDirection.distance = (double)(globalToWinch.Norm);
globalToWinch.Y = 0;
_winchDirection.groundDistance = (double)(globalToWinch.Norm);
if (_winchDirection.heading > Math.PI)
{
_winchDirection.heading -= 2 * Math.PI;
}
if (_winchDirection.heading < -Math.PI)
{
_winchDirection.heading += 2 * Math.PI;
}
if (_winchDirection.pitch > Math.PI)
{
_winchDirection.pitch -= 2 * Math.PI;
}
if (_winchDirection.pitch < -Math.PI)
{
_winchDirection.pitch += 2 * Math.PI;
}
return(_winchDirection);
}
public void UpdateWinch(Canvas RadarCanvas, winchDirection winchPosition, double scale)
{
scale = Math.Max(0.1, scale) / 125 * 1000;
try
{
foreach (UIElement el in RadarCanvas.Children)
{
if (el.GetType() == typeof(Canvas) && (((Canvas)el).Name == "Winch"))
{
Canvas canvas = (Canvas)el;
Canvas.SetLeft(canvas, 125 - 10 + winchPosition.groundDistance / scale * Math.Sin(winchPosition.heading));
Canvas.SetTop(canvas, 125 - 10 - winchPosition.groundDistance / scale * Math.Cos(winchPosition.heading));
}
}
}
catch (Exception ex)
{
Console.WriteLine("FAILED to update winch position: " + ex.Message);
}
}
public double getBodyVelocity(winchDirection _winchDirection, PlaneInfoCommit _planeCommit, double cableTension, double accelerationLimit, double cableLength, double cableLengthPrev, double cableLengthPrePrev, double lastFrameTiming)
{
double baseAcceleration = 5 * 9.81;
if (Math.Abs(cableLength - cableLengthPrev) > 10)
{
cableLengthPrev = cableLength;
}
if (Math.Abs(cableLengthPrev - cableLengthPrePrev) > 10)
{
cableLengthPrePrev = cableLengthPrev;
}
double lengthDiff = (cableLength - cableLengthPrev) * 0.5 + (cableLengthPrev - cableLengthPrePrev) * 0.5;
double cableSpeed = lengthDiff / lastFrameTiming * cableTension;
if (cableTension > 1 && lengthDiff > 0)
{
cableSpeed += baseAcceleration * Math.Pow(cableTension - 1, 2);
}
if (cableSpeed < 2 * accelerationLimit && cableSpeed > accelerationLimit)
{
cableSpeed = Math.Min(cableSpeed, 0.99 * accelerationLimit);
}
Vector3 planeMotion = new Vector3((float)(_planeCommit.VelocityBodyX * Math.Cos(_winchDirection.heading) * Math.Cos(_winchDirection.pitch)),
(float)(_planeCommit.VelocityBodyY * Math.Sin(_winchDirection.heading)),
(float)(_planeCommit.VelocityBodyZ * Math.Cos(_winchDirection.heading) * Math.Sin(_winchDirection.pitch)));
double appliedVelocity = cableSpeed - planeMotion.Norm;
Console.WriteLine($"getBodyVelocity: {cableTension:F4} {appliedVelocity:F4}m/s lengthDiff{lengthDiff:F4}m cableSpeed{cableSpeed:F4}");
//Console.WriteLine($"cabC: {cableLength:F4}m cabP{cableLengthPrev:F4}m cabPP{cableLengthPrePrev:F4}m");
return(appliedVelocity > 0 ? appliedVelocity : 0);
}
