public void ProcessThrustParallel(bool hasWaypoint)
{
if (!hasWaypoint)
{
//Logger.MsgDebug("No Waypoint", DebugTypeEnum.AutoPilot);
StopAllThrust();
return;
}
if (CurrentMode.HasFlag(NewAutoPilotMode.ThrustForward))
{
if (InGravity() && CurrentMode.HasFlag(NewAutoPilotMode.LevelWithGravity))
{
CalculateHoverThrust();
}
else
{
CalculateDirectForwardThrust();
}
}
if (CurrentMode.HasFlag(NewAutoPilotMode.Ram))
{
CalculateRamThrust();
}
//Logger.MsgDebug("Allow Strafe: " + this.Data.AllowStrafing, DebugTypeEnum.AutoPilot);
//Logger.MsgDebug("Is Strafe: " + CurrentMode.HasFlag(NewAutoPilotMode.Strafe), DebugTypeEnum.AutoPilot);
if (this.Data.AllowStrafing && CurrentMode.HasFlag(NewAutoPilotMode.Strafe))
{
//Logger.MsgDebug("Calc Strafe: ", DebugTypeEnum.AutoPilot);
CalculateStrafeThrust();
}
}
public void ProcessRotationParallel(bool hasWaypoint)
{
if (hasWaypoint && (CurrentMode.HasFlag(NewAutoPilotMode.RotateToWaypoint) || CurrentMode.HasFlag(NewAutoPilotMode.RotateToTarget) || CurrentMode.HasFlag(NewAutoPilotMode.Ram)))
{
CalculateGyroRotation();
}
else
{
this.RotationToApply = Vector3.Zero;
}
if (CurrentMode.HasFlag(NewAutoPilotMode.BarrelRoll))
{
this.RotationToApply.Z = 10;
}
PrepareGyroForRotation();
}
public void CalculateGyroRotation()
{
this.RotationToApply = Vector3.Zero;
var rotationTarget = _currentWaypoint;
if (Targeting.HasTarget())
{
if (CurrentMode.HasFlag(NewAutoPilotMode.RotateToTarget) || CurrentMode.HasFlag(NewAutoPilotMode.Ram))
{
rotationTarget = Targeting.TargetLastKnownCoords;
}
}
MatrixD referenceMatrix = this.RefBlockMatrixRotation; //This should be either the control block or at least represent what direction the ship should face
Vector3D directionToTarget = Vector3D.Normalize(rotationTarget - referenceMatrix.Translation);
Vector3 gyroRotation = new Vector3(0, 0, 0); // Pitch,Yaw,Roll
//Get Actual Angle To Target
double angleToTarget = VectorHelper.GetAngleBetweenDirections(referenceMatrix.Forward, directionToTarget);
this.AngleToCurrentWaypoint = angleToTarget;
this.AngleToUpDirection = VectorHelper.GetAngleBetweenDirections(referenceMatrix.Up, _upDirection);
//MyVisualScriptLogicProvider.ShowNotificationToAll("Total Angle: " + angleToTarget.ToString(), 166);
if (angleToTarget <= this.Data.DesiredAngleToTarget && _upDirection == Vector3D.Zero)
{
this.RotationToApply = Vector3.Zero;
return;
}
var gridSize = (_remoteControl.CubeGrid.GridSizeEnum == MyCubeSize.Small);
//Calculate Yaw
double angleLeftToTarget = VectorHelper.GetAngleBetweenDirections(referenceMatrix.Left, directionToTarget);
double angleRightToTarget = VectorHelper.GetAngleBetweenDirections(referenceMatrix.Right, directionToTarget);
YawTargetAngleResult = VectorHelper.GetAngleBetweenDirections(referenceMatrix.Forward, directionToTarget) - VectorHelper.GetAngleBetweenDirections(referenceMatrix.Backward, directionToTarget);
gyroRotation.Y = (float)CalculateGyroAxisRadians(angleLeftToTarget, angleRightToTarget, YawTargetAngleResult, gridSize, ref YawAngleDifference);
//Calculate Pitch
if (_upDirection != Vector3D.Zero && CurrentMode.HasFlag(NewAutoPilotMode.LevelWithGravity) && !CurrentMode.HasFlag(NewAutoPilotMode.Ram))
{
double angleForwardToUp = VectorHelper.GetAngleBetweenDirections(referenceMatrix.Forward, _upDirection);
double angleBackwardToUp = VectorHelper.GetAngleBetweenDirections(referenceMatrix.Backward, _upDirection);
PitchTargetAngleResult = VectorHelper.GetAngleBetweenDirections(referenceMatrix.Up, _upDirection) - VectorHelper.GetAngleBetweenDirections(referenceMatrix.Down, _upDirection);
gyroRotation.X = (float)CalculateGyroAxisRadians(angleForwardToUp, angleBackwardToUp, PitchTargetAngleResult, gridSize, ref PitchAngleDifference);
}
else
{
double angleUpToTarget = VectorHelper.GetAngleBetweenDirections(referenceMatrix.Up, directionToTarget);
double angleDownToTarget = VectorHelper.GetAngleBetweenDirections(referenceMatrix.Down, directionToTarget);
PitchTargetAngleResult = VectorHelper.GetAngleBetweenDirections(referenceMatrix.Forward, directionToTarget) - VectorHelper.GetAngleBetweenDirections(referenceMatrix.Backward, directionToTarget);
gyroRotation.X = (float)CalculateGyroAxisRadians(angleDownToTarget, angleUpToTarget, PitchTargetAngleResult, gridSize, ref PitchAngleDifference);
}
//Calculate Roll - If Specified
if (_upDirection != Vector3D.Zero)
{
double angleRollLeftToUp = VectorHelper.GetAngleBetweenDirections(referenceMatrix.Left, _upDirection);
double angleRollRightToUp = VectorHelper.GetAngleBetweenDirections(referenceMatrix.Right, _upDirection);
if (angleRollLeftToUp == angleRollRightToUp)
{
angleRollLeftToUp--;
angleRollRightToUp++;
}
RollTargetAngleResult = VectorHelper.GetAngleBetweenDirections(referenceMatrix.Up, _upDirection) - VectorHelper.GetAngleBetweenDirections(referenceMatrix.Down, _upDirection);
gyroRotation.Z = (float)CalculateGyroAxisRadians(angleRollLeftToUp, angleRollRightToUp, RollTargetAngleResult, gridSize, ref RollAngleDifference);
}
else
{
RollTargetAngleResult = 0;
RollAngleDifference = 0;
}
this.RotationToApply = gyroRotation;
/*
* var sb = new StringBuilder();
* sb.Append("Rotation Readout:").AppendLine().AppendLine();
*
* sb.Append("Pitch Angle Difference: ").Append(this.PitchAngleDifference.ToString()).AppendLine();
* sb.Append("Pitch Target Angle: ").Append((_upDirection != Vector3D.Zero && CurrentMode.HasFlag(NewAutoPilotMode.LevelWithGravity) ? this.AngleToUpDirection : angleToTarget).ToString()).AppendLine();
* sb.Append("Pitch Applied: ").Append(gyroRotation.X.ToString()).AppendLine().AppendLine();
*
* sb.Append("Yaw Angle Difference: ").Append(this.YawAngleDifference.ToString()).AppendLine();
* sb.Append("Yaw Target Angle: ").Append(this.AngleToCurrentWaypoint.ToString()).AppendLine();
* sb.Append("Yaw Applied: ").Append(gyroRotation.Y.ToString()).AppendLine().AppendLine();
*
* sb.Append("Roll Angle Difference: ").Append(this.RollAngleDifference.ToString()).AppendLine();
* sb.Append("Roll Target Angle: ").Append(this.AngleToUpDirection.ToString()).AppendLine();
* sb.Append("Roll Applied: ").Append(gyroRotation.Z.ToString()).AppendLine().AppendLine();
*
* Logger.MsgDebug(sb.ToString(), DebugTypeEnum.AutoPilotStats);
*/
return;
}
public void CalculateStrafeThrust()
{
if (this.Strafing == false && !CurrentMode.HasFlag(NewAutoPilotMode.Ram))
{
TimeSpan duration = MyAPIGateway.Session.GameDateTime - this.LastStrafeEndTime;
if (duration.TotalMilliseconds >= this.ThisStrafeCooldown)
{
if (MyVelocity.Length() > Data.StrafeSpeedCutOff || (_upDirection != Vector3D.Zero && VectorHelper.GetAngleBetweenDirections(_upDirection, RefBlockMatrixRotation.Up) > 90))
{
this.LastStrafeStartTime = MyAPIGateway.Session.GameDateTime;
_debugThrustForwardMode = "None";
_debugThrustUpMode = "None";
_debugThrustSideMode = "None";
return;
}
this.LastStrafeStartTime = MyAPIGateway.Session.GameDateTime;
this.ThisStrafeDuration = Rnd.Next(Data.StrafeMinDurationMs, Data.StrafeMaxDurationMs);
_strafeStartPosition = _myPosition;
this.Strafing = true;
Logger.MsgDebug("Begin Strafe. Dur: " + ThisStrafeDuration, DebugTypeEnum.AutoPilot);
Collision.RunSecondaryCollisionChecks();
var strafeRandomization = new Vector3I(Rnd.Next(-1, 2), Rnd.Next(-1, 2), Rnd.Next(-1, 2));
if (strafeRandomization.X != 0)
{
this.CurrentAllowedStrafeDirections.X = 1;
var rAngle = VectorHelper.GetAngleBetweenDirections(Collision.Matrix.Right, Vector3D.Normalize(GetCurrentWaypoint() - Collision.Matrix.Translation));
var lAngle = VectorHelper.GetAngleBetweenDirections(Collision.Matrix.Left, Vector3D.Normalize(GetCurrentWaypoint() - Collision.Matrix.Translation));
bool rTargetIntersect = (rAngle < this.Data.StrafeMinimumSafeAngleFromTarget && DistanceToCurrentWaypoint < this.Data.StrafeMinimumTargetDistance);
bool lTargetIntersect = (lAngle < this.Data.StrafeMinimumSafeAngleFromTarget && DistanceToCurrentWaypoint < this.Data.StrafeMinimumTargetDistance);
if (strafeRandomization.X == 1)
{
if (rTargetIntersect || (Collision.RightResult.HasTarget(Data.StrafeMinimumTargetDistance) && !Collision.LeftResult.HasTarget(Data.StrafeMinimumTargetDistance)))
{
Logger.MsgDebug("Strafe: X Reverse", DebugTypeEnum.Strafe);
strafeRandomization.X *= -1;
}
else if (lTargetIntersect || (Collision.RightResult.HasTarget(Data.StrafeMinimumTargetDistance) && Collision.LeftResult.HasTarget(Data.StrafeMinimumTargetDistance)))
{
Logger.MsgDebug("Strafe: X Negate", DebugTypeEnum.Strafe);
strafeRandomization.X = 0;
}
}
else
{
if (lTargetIntersect || (Collision.LeftResult.HasTarget(Data.StrafeMinimumTargetDistance) && !Collision.RightResult.HasTarget(Data.StrafeMinimumTargetDistance)))
{
Logger.MsgDebug("Strafe: X Reverse", DebugTypeEnum.Strafe);
strafeRandomization.X *= -1;
}
else if (rTargetIntersect || (Collision.LeftResult.HasTarget(Data.StrafeMinimumTargetDistance) && Collision.RightResult.HasTarget(Data.StrafeMinimumTargetDistance)))
{
Logger.MsgDebug("Strafe: X Negate", DebugTypeEnum.Strafe);
strafeRandomization.X = 0;
}
}
}
if (strafeRandomization.Y != 0)
{
strafeRandomization.Y = 1;
var uAngle = VectorHelper.GetAngleBetweenDirections(Collision.Matrix.Up, Vector3D.Normalize(GetCurrentWaypoint() - Collision.Matrix.Translation));
var dAngle = VectorHelper.GetAngleBetweenDirections(Collision.Matrix.Down, Vector3D.Normalize(GetCurrentWaypoint() - Collision.Matrix.Translation));
bool uTargetIntersect = (uAngle < this.Data.StrafeMinimumSafeAngleFromTarget && DistanceToCurrentWaypoint < this.Data.StrafeMinimumTargetDistance);
bool dTargetIntersect = (dAngle < this.Data.StrafeMinimumSafeAngleFromTarget && DistanceToCurrentWaypoint < this.Data.StrafeMinimumTargetDistance);
if (strafeRandomization.Y == 1)
{
if (uTargetIntersect || (Collision.UpResult.HasTarget(Data.StrafeMinimumTargetDistance) && !Collision.DownResult.HasTarget(Data.StrafeMinimumTargetDistance)))
{
Logger.MsgDebug("Strafe: Y Reverse", DebugTypeEnum.Strafe);
strafeRandomization.Y *= -1;
}
else if (dTargetIntersect || (Collision.UpResult.HasTarget(Data.StrafeMinimumTargetDistance) && Collision.DownResult.HasTarget(Data.StrafeMinimumTargetDistance)))
{
Logger.MsgDebug("Strafe: Y Negate", DebugTypeEnum.Strafe);
strafeRandomization.Y = 0;
}
}
else
{
if (dTargetIntersect || (Collision.DownResult.HasTarget(Data.StrafeMinimumTargetDistance) && !Collision.UpResult.HasTarget(Data.StrafeMinimumTargetDistance)))
{
Logger.MsgDebug("Strafe: Y Reverse", DebugTypeEnum.Strafe);
strafeRandomization.Y *= -1;
}
else if (uTargetIntersect || (Collision.DownResult.HasTarget(Data.StrafeMinimumTargetDistance) && Collision.UpResult.HasTarget(Data.StrafeMinimumTargetDistance)))
{
Logger.MsgDebug("Strafe: Y Negate", DebugTypeEnum.Strafe);
strafeRandomization.Y = 0;
}
}
}
if (strafeRandomization.Z != 0)
{
strafeRandomization.Z = 1;
var fAngle = VectorHelper.GetAngleBetweenDirections(Collision.Matrix.Forward, Vector3D.Normalize(GetCurrentWaypoint() - Collision.Matrix.Translation));
var bAngle = VectorHelper.GetAngleBetweenDirections(Collision.Matrix.Backward, Vector3D.Normalize(GetCurrentWaypoint() - Collision.Matrix.Translation));
bool fTargetIntersect = (fAngle < this.Data.StrafeMinimumSafeAngleFromTarget && DistanceToCurrentWaypoint < this.Data.StrafeMinimumTargetDistance);
bool bTargetIntersect = (bAngle < this.Data.StrafeMinimumSafeAngleFromTarget && DistanceToCurrentWaypoint < this.Data.StrafeMinimumTargetDistance);
if (strafeRandomization.Z == 1)
{
if (fTargetIntersect || (Collision.ForwardResult.HasTarget(Data.StrafeMinimumTargetDistance) && !Collision.BackwardResult.HasTarget(Data.StrafeMinimumTargetDistance)))
{
Logger.MsgDebug("Strafe: Z Reverse", DebugTypeEnum.AutoPilot);
strafeRandomization.Z *= -1;
}
else if (bTargetIntersect || (Collision.ForwardResult.HasTarget(Data.StrafeMinimumTargetDistance) && Collision.BackwardResult.HasTarget(Data.StrafeMinimumTargetDistance)))
{
Logger.MsgDebug("Strafe: Z Negate", DebugTypeEnum.AutoPilot);
strafeRandomization.Z = 0;
}
}
else
{
if (bTargetIntersect || (Collision.BackwardResult.HasTarget(Data.StrafeMinimumTargetDistance) && !Collision.ForwardResult.HasTarget(Data.StrafeMinimumTargetDistance)))
{
Logger.MsgDebug("Strafe: Z Reverse", DebugTypeEnum.AutoPilot);
strafeRandomization.Z *= -1;
}
else if (fTargetIntersect || (Collision.BackwardResult.HasTarget(Data.StrafeMinimumTargetDistance) && Collision.ForwardResult.HasTarget(Data.StrafeMinimumTargetDistance)))
{
Logger.MsgDebug("Strafe: Z Negate", DebugTypeEnum.AutoPilot);
strafeRandomization.Z = 0;
}
}
}
if (UpDirectionFromPlanet != Vector3D.Zero && MyAltitude < Data.MinimumPlanetAltitude)
{
var thrustDir = VectorHelper.GetThrustDirectionsAwayFromDirection(Collision.Matrix, -UpDirectionFromPlanet);
if (thrustDir.X != 0)
{
strafeRandomization.X = thrustDir.X;
}
if (thrustDir.Y != 0)
{
strafeRandomization.Y = thrustDir.Y;
}
if (thrustDir.Z != 0)
{
strafeRandomization.Z = thrustDir.Z;
}
}
//Set Thrust To Apply Here!
if (strafeRandomization.X != 0)
{
_thrustToApply.SetX(true, strafeRandomization.X == -1, 1, _orientation);
}
else
{
_thrustToApply.SetX(false, false, 0, _orientation);
}
if (strafeRandomization.Y != 0)
{
_thrustToApply.SetY(true, strafeRandomization.Y == -1, 1, _orientation);
}
else
{
_thrustToApply.SetY(false, false, 0, _orientation);
}
if (strafeRandomization.Z != 0)
{
_thrustToApply.SetZ(true, strafeRandomization.Z == -1, 1, _orientation);
}
else
{
_thrustToApply.SetZ(false, false, 0, _orientation);
}
}
else
{
_debugThrustForwardMode = "None";
_debugThrustUpMode = "None";
_debugThrustSideMode = "None";
}
}
else
{
if (!Strafing && _applyRamming)
{
return;
}
TimeSpan duration = MyAPIGateway.Session.GameDateTime - this.LastStrafeStartTime;
if (duration.TotalMilliseconds >= this.ThisStrafeDuration || MyVelocity.Length() > Data.StrafeSpeedCutOff || Vector3D.Distance(_strafeStartPosition, _myPosition) > Data.StrafeDistanceCutOff)
{
Logger.MsgDebug("End Strafe", DebugTypeEnum.AutoPilot);
this.InvertStrafingActivated = false;
this.LastStrafeEndTime = MyAPIGateway.Session.GameDateTime;
this.ThisStrafeCooldown = Rnd.Next(Data.StrafeMinCooldownMs, Data.StrafeMaxCooldownMs);
this.Strafing = false;
_collisionStrafeAdjusted = false;
_minAngleDistanceStrafeAdjusted = false;
_collisionStrafeDirection = Vector3D.Zero;
_thrustToApply.SetX(false, false, 0, _orientation);
_thrustToApply.SetY(false, false, 0, _orientation);
_thrustToApply.SetZ(false, false, 0, _orientation);
//Logger.AddMsg("Cooldown: " + this.ThisStrafeCooldown.ToString(), true);
}
else
{
//Logger.MsgDebug("Strafe Collision: " + Collision.VelocityResult.CollisionImminent.ToString() + " - " + Collision.VelocityResult.Time.ToString(), DebugTypeEnum.Collision);
if (!_collisionStrafeAdjusted && Collision.VelocityResult.CollisionImminent())
{
Logger.MsgDebug("Strafe Velocity Collision Detect: " + Collision.VelocityResult.Type.ToString() + ", " + Collision.VelocityResult.GetCollisionDistance(), DebugTypeEnum.Collision);
_collisionStrafeAdjusted = true;
StopStrafeDirectionNearestPosition(Collision.VelocityResult.GetCollisionCoords());
_collisionStrafeDirection = Vector3D.Normalize(Collision.VelocityResult.GetCollisionCoords() - _remoteControl.WorldMatrix.Translation);
}
else if (_collisionStrafeAdjusted && VectorHelper.GetAngleBetweenDirections(_collisionStrafeDirection, Vector3D.Normalize(Collision.Velocity - _remoteControl.WorldMatrix.Translation)) > 15)
{
Logger.MsgDebug("Strafe Collision Detect", DebugTypeEnum.General);
StopStrafeDirectionNearestPosition(Collision.VelocityResult.GetCollisionCoords());
_collisionStrafeDirection = Vector3D.Normalize(Collision.VelocityResult.GetCollisionCoords() - _remoteControl.WorldMatrix.Translation);
}
if (_minAngleDistanceStrafeAdjusted && AngleToCurrentWaypoint < this.Data.StrafeMinimumSafeAngleFromTarget && DistanceToCurrentWaypoint < this.Data.StrafeMinimumTargetDistance)
{
Logger.MsgDebug("Strafe Min Dist/Angle Detect", DebugTypeEnum.General);
_minAngleDistanceStrafeAdjusted = false;
StopStrafeDirectionNearestPosition(Collision.VelocityResult.GetCollisionCoords());
_collisionStrafeDirection = Vector3D.Normalize(Collision.VelocityResult.GetCollisionCoords() - _remoteControl.WorldMatrix.Translation);
}
var forwardData = _thrustToApply.GetThrustDataFromDirection(Direction.Forward, _behavior.Settings.BlockOrientation);
var upData = _thrustToApply.GetThrustDataFromDirection(Direction.Up, _behavior.Settings.BlockOrientation);
var sideData = _thrustToApply.GetThrustDataFromDirection(Direction.Right, _behavior.Settings.BlockOrientation);
_debugThrustForwardMode = (forwardData.X == 0 || forwardData.Z <= 0.01) ? "None" : forwardData.Y == 1 ? "Strafe Forward" : "Strafe Backward";
_debugThrustUpMode = (upData.X == 0 || upData.Z <= 0.01) ? "Strafe None" : upData.Y == 1 ? "Strafe Up" : "Strafe Down";
_debugThrustSideMode = (sideData.X == 0 || sideData.Z <= 0.01) ? "Strafe None" : sideData.Y == 1 ? "Strafe Right" : "Strafe Left";
}
}
}
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);
}
