public void CalculateHoverThrust()
{
var upAngle = VectorHelper.GetAngleBetweenDirections(_upDirection, RefBlockMatrixRotation.Up);
var planetPos = CurrentPlanet.PositionComp.WorldAABB.Center;
var velocity = _remoteControl.SlimBlock.CubeGrid.Physics.LinearVelocity;
var velocityAmount = velocity.Length();
var velocityToTargetAngle = VectorHelper.GetAngleBetweenDirections(Vector3D.Normalize(GetCurrentWaypoint() - _remoteControl.WorldMatrix.Translation), Vector3D.Normalize(velocity));
//UP Axis
//TODO: change magic 10 to configurable field
if (upAngle <= 10)
{
var myDistToCore = Vector3D.Distance(_myPosition, planetPos);
var targetDistCore = Vector3D.Distance(_currentWaypoint, planetPos);
var altitudeDist = myDistToCore - targetDistCore;
if (Math.Abs(altitudeDist) > this.Data.WaypointTolerance)
{
bool skipCheck = false;
bool invertedDir = false;
double stoppingDist = 0;
double upDistance = 0;
double upVelocityAmt = 0;
if (altitudeDist < 0)
{
//UP
stoppingDist = CalculateStoppingDistance(velocity, RefBlockMatrixRotation.Up, Base6Directions.Direction.Up);
upDistance = Vector3D.Dot(_currentWaypoint - _myPosition, _upDirection);
upVelocityAmt = Vector3D.Dot(velocity, RefBlockMatrixRotation.Up);
invertedDir = false;
}
else
{
//DOWN
stoppingDist = CalculateStoppingDistance(velocity, RefBlockMatrixRotation.Down, Base6Directions.Direction.Down);
upDistance = -Vector3D.Dot(_currentWaypoint - _myPosition, _upDirection);
upVelocityAmt = Vector3D.Dot(velocity, RefBlockMatrixRotation.Down);
invertedDir = true;
}
/*
* Logger.MsgDebug("-------------------", DebugTypeEnum.Thrust);
* Logger.MsgDebug("Altitude Distance: " + altitudeDist.ToString(), DebugTypeEnum.Thrust);
* Logger.MsgDebug("Up Distance: " + upDistance.ToString(), DebugTypeEnum.Thrust);
* Logger.MsgDebug("Stop Distance: " + stoppingDist.ToString(), DebugTypeEnum.Thrust);
* Logger.MsgDebug("-------------------", DebugTypeEnum.Thrust);
*/
if (!IndirectWaypointType.HasFlag(WaypointModificationEnum.PlanetPathingAscend) && !IndirectWaypointType.HasFlag(WaypointModificationEnum.Collision))
{
if ((stoppingDist >= upDistance && upVelocityAmt >= Data.IdealMinSpeed))
{
_thrustToApply.SetY(false, false, 0, _orientation);
skipCheck = true;
_debugThrustUpMode = string.Format("Current Dist / Stop Dist : {0} / {1}", Math.Round(upDistance), Math.Round(stoppingDist));
}
if (upDistance <= this.Data.WaypointTolerance)
{
_thrustToApply.SetY(false, false, 0, _orientation);
skipCheck = true;
_debugThrustUpMode = "Within Waypoint Tolerance Distance";
}
}
if (!skipCheck && upVelocityAmt < Data.IdealMaxSpeed - Data.MaxSpeedTolerance)
{
_thrustToApply.SetY(true, invertedDir, 1, _orientation);
skipCheck = true;
_debugThrustUpMode = "Thrust To Desired Speed";
}
if (!skipCheck && upVelocityAmt > Data.IdealMaxSpeed + Data.MaxSpeedTolerance)
{
_thrustToApply.SetY(true, invertedDir ? false : true, 1, _orientation);
skipCheck = true;
_debugThrustUpMode = "Brake To Desired Speed";
}
if (!skipCheck)
{
_thrustToApply.SetY(true, invertedDir, 0.0001f, _orientation);
_debugThrustUpMode = "Drifting At Desired Speed";
}
}
else
{
_debugThrustUpMode = "At Desired Altitude Range";
_thrustToApply.SetY(false, false, 0, _orientation);
}
}
else
{
_debugThrustForwardMode = "Not Level With Gravity Direction";
_debugThrustUpMode = "Not Level With Gravity Direction";
_thrustToApply.SetY(false, false, 0, _orientation);
_thrustToApply.SetZ(false, false, 0, _orientation);
return;
}
//Forward Axis
var forwardToDestAngle = VectorHelper.GetAngleBetweenDirections(this.RefBlockMatrixRotation.Forward, Vector3D.Normalize(_currentWaypoint - _myPosition));
if (forwardToDestAngle <= 90)
{
if (this.YawAngleDifference <= this.Data.AngleAllowedForForwardThrust)
{
var stoppingDist = CalculateStoppingDistance(velocity, _forwardDir, Base6Directions.Direction.Backward);
var forwardDistance = Vector3D.Dot(_currentWaypoint - _myPosition, _forwardDir);
bool skipCheck = false;
var forwardVelocityAmt = Vector3D.Dot(velocity, _forwardDir);
if (!IndirectWaypointType.HasFlag(WaypointModificationEnum.PlanetPathingAscend) && !IndirectWaypointType.HasFlag(WaypointModificationEnum.Collision))
{
if ((stoppingDist >= forwardDistance && forwardVelocityAmt >= Data.IdealMinSpeed))
{
_debugThrustForwardMode = string.Format("Current Dist / Stop Dist : {0} / {1}", Math.Round(forwardDistance), Math.Round(stoppingDist));
_thrustToApply.SetZ(false, false, 0, _orientation);
skipCheck = true;
}
if (forwardDistance <= this.Data.WaypointTolerance)
{
_debugThrustForwardMode = "Within Waypoint Tolerance Distance";
_thrustToApply.SetZ(false, false, 0, _orientation);
skipCheck = true;
}
}
if (!skipCheck && forwardVelocityAmt < Data.IdealMaxSpeed - Data.MaxSpeedTolerance)
{
_debugThrustForwardMode = "Thrust To Desired Speed";
_thrustToApply.SetZ(true, false, 1, _orientation);
skipCheck = true;
}
if (!skipCheck && forwardVelocityAmt > Data.IdealMaxSpeed + Data.MaxSpeedTolerance)
{
_debugThrustForwardMode = "Brake To Desired Speed";
_thrustToApply.SetZ(true, true, 1, _orientation);
skipCheck = true;
}
if (!skipCheck)
{
_debugThrustForwardMode = "Drifting At Desired Speed";
_thrustToApply.SetZ(true, false, 0.0001f, _orientation);
}
}
else
{
_thrustToApply.SetZ(false, false, 0, _orientation);
}
}
else
{
_debugThrustForwardMode = "Forward Angle To Waypoint Greater Than 90";
_thrustToApply.SetZ(false, false, 0, _orientation);
}
}
public void CalculateDirectForwardThrust()
{
if (CurrentMode.HasFlag(NewAutoPilotMode.Strafe) || _remoteControl?.SlimBlock?.CubeGrid?.Physics == null)
{
_debugThrustForwardMode = "Strafing Still Active";
return;
}
if (this.AngleToCurrentWaypoint > this.Data.AngleAllowedForForwardThrust)
{
_debugThrustForwardMode = "Thrust Angle Not Matched";
_thrustToApply.SetZ(false, false, 0, _orientation);
return;
}
var velocityToTargetAngle = VectorHelper.GetAngleBetweenDirections(Vector3D.Normalize(GetCurrentWaypoint() - _remoteControl.WorldMatrix.Translation), Vector3D.Normalize(_remoteControl.SlimBlock.CubeGrid.Physics.LinearVelocity));
var velocity = _remoteControl.SlimBlock.CubeGrid.Physics.LinearVelocity;
var velocityAmount = velocity.Length();
var stoppingDist = CalculateStoppingDistance(velocity, _forwardDir, Base6Directions.Direction.Forward);
if (!IndirectWaypointType.HasFlag(WaypointModificationEnum.PlanetPathingAscend) && !IndirectWaypointType.HasFlag(WaypointModificationEnum.Collision))
{
if ((stoppingDist >= this.DistanceToCurrentWaypoint && velocityAmount >= Data.IdealMinSpeed) || this.DistanceToCurrentWaypoint <= this.Data.WaypointTolerance)
{
_debugThrustForwardMode = string.Format("Current Dist / Stop Dist : {0} / {1}", Math.Round(this.DistanceToCurrentWaypoint), Math.Round(stoppingDist));
_thrustToApply.SetZ(false, false, 0, _orientation);
return;
}
if (this.DistanceToCurrentWaypoint <= this.Data.WaypointTolerance)
{
_debugThrustForwardMode = "Within Waypoint Tolerance Distance";
_thrustToApply.SetZ(false, false, 0, _orientation);
return;
}
}
if (velocityToTargetAngle > this.Data.MaxVelocityAngleForSpeedControl || velocity.Length() <= 0.1)
{
_debugThrustForwardMode = "Velocity Angle Not Within Range";
_thrustToApply.SetZ(true, false, 1, _orientation);
return;
}
//Logger.MsgDebug(string.Format("Forward Thrust Check: {0} / {1}", velocityAmount, IdealMaxSpeed - MaxSpeedTolerance), DebugTypeEnum.Thrust);
if (velocityAmount < Data.IdealMaxSpeed - Data.MaxSpeedTolerance)
{
_debugThrustForwardMode = "Thrust To Desired Speed";
_thrustToApply.SetZ(true, false, 1, _orientation);
return;
}
//Logger.MsgDebug(string.Format("Reverse Thrust Check: {0} / {1}", velocityAmount, IdealMaxSpeed + MaxSpeedTolerance), DebugTypeEnum.Thrust);
if (velocityAmount > Data.IdealMaxSpeed + Data.MaxSpeedTolerance)
{
_debugThrustForwardMode = "Brake To Desired Speed";
_thrustToApply.SetZ(true, true, 1, _orientation);
return;
}
_debugThrustForwardMode = "Drifting At Desired Speed " + stoppingDist.ToString();
_thrustToApply.SetZ(true, false, 0.0001f, _orientation);
}