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