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); }