public bool getGravityVector(out Vector3D gravity) { gravity = Vector3D.Zero; if (_init()) { return(true); } rc.TryGetPlanetPosition(out gravity); gravity -= rc.GetPosition(); gravity.Normalize(); return(false); }
public Vector3 OffsetPositionFromPlanet(Vector3 position, float offset = 30) { Vector3D planetPosition; bool isNearPlanet = mainControl.TryGetPlanetPosition(out planetPosition); if (!isNearPlanet) { return(position); } Vector3 directionToPlanet = Vector3.Normalize(planetPosition - position); Vector3 offsetPosition = position + Vector3.Negate(directionToPlanet) * offset; return(offsetPosition); }
private void Tick() { var status = new MissileStatus() { Position = this.Position, State = state, DistToTarget = (this.Target - this.Position).Length(), }; status.ExtraData.AppendLine($"DeltaTime: {(DateTime.Now - launchTime).TotalSeconds:F2} seconds"); status.ExtraData.AppendLine($"Distance from launch position: {(this.Position - this.launchPos).Length():F2}"); if (state == LaunchState.Separation) { if ((this.Position - this.launchPos).LengthSquared() >= downSeparationRange * downSeparationRange) { LogLine($"Separation phase finished, transitioning to boost phase"); foreach (var thrust in downThrusters) { thrust.ThrustOverridePercentage = 0; } foreach (var thrust in fwdThrusters) { thrust.Enabled = true; thrust.ThrustOverridePercentage = 1; } this.state = LaunchState.Boost; } } else if (state == LaunchState.Boost) { if ((this.Position - this.launchPos).LengthSquared() >= boostManeuverRange * boostManeuverRange) { LogLine($"Boost phase finished, transitioning to parabolic flight mode"); this.state = LaunchState.Flight; foreach (var gyro in gyros) { gyro.Enabled = true; } this.enginesActive = false; Vector3D planet = Vector3D.Zero; if (!remote.TryGetPlanetPosition(out planet)) { LogLine("Couldn't find planet!"); } LogLine($"Generating parabolic trajectory"); this.trajectory = GenerateTrajectory(this.Position, this.finalTarget.Coords, 10, planet); this.trajectoryStage = 0; this.state = LaunchState.Flight; // temporarily stop forward thrust in order to allow missile to align to target foreach (var thruster in fwdThrusters) { thruster.ThrustOverride = 0; } } } else if (state == LaunchState.Flight) { double pitch, yaw, roll; var desiredFwdVec = Vector3D.Normalize(Target - Position); MathStuff.GetRotationAngles(desiredFwdVec, Vector3D.Zero, remote.WorldMatrix, out pitch, out yaw, out roll); status.DistToTarget = (Target - Position).Length(); status.Pitch = pitch; status.Yaw = yaw; status.Roll = roll; if (!this.enginesActive && (Math.Abs(pitch) < MIN_ANGLE_FOR_ENGINE_RESTART || Math.Abs(yaw) < MIN_ANGLE_FOR_ENGINE_RESTART || Math.Abs(roll) < MIN_ANGLE_FOR_ENGINE_RESTART)) { foreach (var thrust in fwdThrusters) { thrust.ThrustOverridePercentage = 1; } this.enginesActive = true; } var pitchCorrection = pitchPID.GetCorrection(pitch, PID_TIME_DELTA); var yawCorrection = yawPID.GetCorrection(yaw, PID_TIME_DELTA); var rollCorrection = rollPID.GetCorrection(roll, PID_TIME_DELTA); status.ExtraData.Append($"PID:\nPitchCorrection: {MathHelperD.ToDegrees(pitchCorrection):F2}\n" + $"YawCorrection: {MathHelperD.ToDegrees(yawCorrection):F2}\n" + $"RollCorrection: {MathHelperD.ToDegrees(rollCorrection):F2}\n"); if (applyGyro) { MathStuff.ApplyGyroOverride(pitchCorrection, yawCorrection, rollCorrection, this.gyros, this.remote.WorldMatrix); } if ((this.Position - Target).LengthSquared() <= SWITCH_WAY_POINT_DISTANCE * SWITCH_WAY_POINT_DISTANCE) { this.trajectoryStage++; } if (this.trajectoryStage >= this.trajectory.Count || (this.Position - finalTarget.Coords).LengthSquared() <= this.armDistance * this.armDistance) { ArmMissile(); this.trajectoryStage++; this.state = LaunchState.Terminal; } } else if (state == LaunchState.Terminal) { if ((this.Position - finalTarget.Coords).LengthSquared() <= blowDistance * blowDistance) { LogLine("Reached detonation threshold. Detonating and disabling script."); LogStatus("Detonated."); Detonate(); } } status.State = this.state; LogStatus(status.ToString()); }
void GuideMissile() { missilePos = shipReference.GetPosition(); //---Get orientation vectors of our missile Vector3D missileFrontVec = shipReference.WorldMatrix.Forward; Vector3D missileLeftVec = shipReference.WorldMatrix.Left; Vector3D missileUpVec = shipReference.WorldMatrix.Up; //---Check if we have gravity double rollAngle = 0; double rollSpeed = 0; var remote = remotes[0] as IMyRemoteControl; gravVec = shipReference.GetNaturalGravity(); double gravMagSquared = gravVec.LengthSquared(); if (gravMagSquared != 0) { if (gravVec.Dot(missileUpVec) < 0) { // rollAngle = Math.PI / 2 - Math.Acos(MathHelper.Clamp(gravVec.Dot(missileLeftVec) / gravVec.Length(), -1, 1)); } else { // rollAngle = Math.PI + Math.Acos(MathHelper.Clamp(gravVec.Dot(missileLeftVec) / gravVec.Length(), -1, 1)); } } else { rollSpeed = 0; } //---Get travel vector var missileVelocityVec = shipReference.GetShipVelocities().LinearVelocity; double speed = shipReference.GetShipSpeed(); //---Find vector from missile to destinationVec Vector3D missileToTargetVec; if (speed > 10 ||) { missileToTargetVec = Vector3D.Negate(missileVelocityVec); } else { shipReference.TryGetPlanetPosition(out targetPos); missileToTargetVec = Vector3D.Negate(targetPos - missilePos); // targetPos - missilePos; } //---Calc our new heading based upon our travel vector var headingVec = CalculateHeadingVector(missileToTargetVec, missileVelocityVec, driftCompensation); //---Get pitch and yaw angles double yawAngle; double pitchAngle; GetRotationAngles(headingVec, missileFrontVec, missileLeftVec, missileUpVec, out yawAngle, out pitchAngle); logger.Info("mttv: " + missileToTargetVec); logger.Info("mvv: " + missileVelocityVec); logger.Info("hv: " + headingVec); if (firstGuidance) { lastPitchAngle = pitchAngle; lastYawAngle = yawAngle; firstGuidance = false; } //---Angle controller logger.Debug(yawAngle + "|" + lastYawAngle); double yawSpeed = Math.Round(proportionalConstant * yawAngle + derivativeConstant * (yawAngle - lastYawAngle) * 60, 3); double pitchSpeed = Math.Round(proportionalConstant * pitchAngle + derivativeConstant * (pitchAngle - lastPitchAngle) * 60, 3); //---Set appropriate gyro override if (speed <= 1E-2) { logger.Info("Fin"); ApplyGyroOverride(0, 0, 0, gyros, shipReference); Runtime.UpdateFrequency = UpdateFrequency.None; } else { logger.Info("V=" + missileVelocityVec.Length()); ApplyGyroOverride(pitchSpeed, yawSpeed, rollSpeed, gyros, shipReference); } //---Store previous values lastYawAngle = yawAngle; lastPitchAngle = pitchAngle; lastRollAngle = rollAngle; }