public void Update(Vector3D target)
{
MatrixD transpose = MatrixD.Transpose(rc.WorldMatrix);
Vector3D meToTarget = rc.WorldMatrix.Translation - target;
Vector3D localError = Vector3D.TransformNormal(meToTarget, transpose);
localError.Y = 0;
if (localError.X > -0.5 && localError.X < 0.5)
{
localError.X = 0;
}
if (localError.Z > -0.5 && localError.Z < 0.5)
{
localError.Z = 0;
}
float correction = (float)forwardPID.Control(localError.Z);
float force = correction * rc.CalculateShipMass().TotalMass;
float rightLeft = (float)anglePID.Control(-localError.X);
Vector3D localVelocity = Vector3D.TransformNormal(rc.GetShipVelocities().LinearVelocity, transpose);
float angle = -rightLeft;
if (localVelocity.Z < 0)
{
angle *= -1;
}
foreach (Wheel w in wheels)
{
IMyMotorSuspension wheel = w.block;
if (first)
{
Vector3D center = Vector3D.TransformNormal(rc.CenterOfMass - rc.GetPosition(), transpose);
Vector3D local = Vector3D.TransformNormal(wheel.GetPosition() - rc.GetPosition(), transpose);
wheel.InvertSteer = (local.Z > center.Z);
wheel.InvertPropulsion = (wheel.Orientation.Left != rc.Orientation.Forward);
wheel.Brake = false;
}
if (wheel.Steering)
{
wheel.SetValueFloat("Steer override", angle);
}
if (wheel.Propulsion)
{
float maxForce = w.maxForce;
if (maxForce <= 0)
{
continue;
}
float percent = MathHelper.Clamp(force / maxForce, -1, 1);
force -= percent * maxForce;
wheel.SetValueFloat("Propulsion override", percent);
}
}
first = false;
}
public void FaceVectors(Vector3D forward, Vector3D up)
{
// In (pitch, yaw, roll)
Vector3D error = GetAngles(rc.WorldMatrix, forward, up);
Vector3D angles = new Vector3D(pitchPID.Control(-error.X), yawPID.Control(-error.Y), rollPID.Control(-error.Z));
ApplyGyroOverride(rc.WorldMatrix, angles);
}
public void Turn(MatrixD Reference, Vector3D target)
{
if (Reference == MatrixD.Zero)
{
return;
}
MatrixD orientationMatrix = MatrixD.Identity;
orientationMatrix.Translation = Reference.Translation;
Vector3D orientationForward = Controller.WorldMatrix.Forward + Controller.WorldMatrix.Down + Controller.WorldMatrix.Left;
orientationForward.Normalize();
orientationMatrix.Forward = orientationForward;
Vector3D orientationUp = Reference.Forward;
orientationUp -= VectorHelpers.VectorProjection(orientationUp, orientationForward);
orientationUp.Normalize();
orientationMatrix.Up = orientationUp;
Vector3D OrientationRight = orientationForward.Cross(orientationUp);
orientationMatrix.Right = OrientationRight;
var gravDir = Controller.GetNaturalGravity();
gravDir.Normalize();
double yawAngle, pitchAngle, spinAngle;
GravAngle = VectorHelpers.VectorAngleBetween(gravDir, orientationMatrix.Forward);
if (target != Vector3D.Zero && GravAngle < Math.PI * 0.1)
{
Vector3D TargetDir = target - orientationMatrix.Translation;
TargetDir.Normalize();
var projectedTargetUp = TargetDir - VectorHelpers.VectorProjection(TargetDir, orientationForward);
spinAngle = -1 * VectorHelpers.VectorAngleBetween(orientationMatrix.Up, projectedTargetUp) * Math.Sign(orientationMatrix.Left.Dot(TargetDir));
}
else
{
spinAngle = 0;
SpinPID.Reset();
}
TrigHelpers.GetRotationAngles(gravDir, orientationMatrix.Forward, orientationMatrix.Left, orientationMatrix.Up, out yawAngle, out pitchAngle);
TrigHelpers.ApplyGyroOverride(PitchPID.Control(pitchAngle), YawPID.Control(yawAngle), SpinPID.Control(spinAngle), Gyros, orientationMatrix);
}
public void Update()
{
LifeTimeTicks++;
// Startup Subroutine
if (LifeTimeTicks == 1)
{
foreach (var engine in Drive.HoverEngines)
{
engine.AltitudeMin = 1;
engine.PushOnly = true;
}
}
if (LifeTimeTicks == 60)
{
foreach (var engine in Drive.HoverEngines)
{
engine.AltitudeMin = 2;
}
}
if (LifeTimeTicks == 120)
{
foreach (var engine in Drive.HoverEngines)
{
engine.AltitudeMin = 7;
}
}
if (LifeTimeTicks % kRunEveryXUpdates == 0)
{
Vector3D PlanetDir = PlanetPos - Controller.WorldMatrix.Translation;
PlanetDir.Normalize();
// Orient Self
Drive.Turn(Gats.Count > 0 ? Gats[0].WorldMatrix : Controller.WorldMatrix, target);
// Aim Turret
if (TurretRotor != null)
{
if (target != Vector3D.Zero && Drive.GravAngle < Math.PI * 0.1)
{
var TurretAngle = TrigHelpers.FastAsin(Gats[0].WorldMatrix.Forward.Dot(PlanetDir));
Vector3D TargetDir = target - Gats[0].WorldMatrix.Translation;
var targetDist = TargetDir.Length();
TargetDir.Normalize();
var TargetAngle = TrigHelpers.FastAsin(TargetDir.Dot(PlanetDir));
var angleDiff = TargetAngle - TurretAngle;
if (VectorHelpers.VectorAngleBetween(Gats[0].WorldMatrix.Forward, TargetDir) < 0.05 && targetDist < 800)
{
Fire();
}
TurretRotor.TargetVelocityRPM = (float)TurretPID.Control(angleDiff);
}
else
{
TurretPID.Reset();
TurretRotor.TargetVelocityRPM = 0;
}
}
// Check your fire
fireTicks--;
if (fireTicks == -1)
{
foreach (var gat in Gats)
{
TerminalPropertiesHelper.SetValue(gat, "Shoot", false);
}
}
// Check Movement
if (Destination != Vector3D.Zero)
{
Drive.Drive(Destination);
if (Drive.Arrived)
{
Destination = Vector3D.Zero;
Drive.Flush();
}
}
}
}
public Vector3D Control(Vector3D error)
{
return(new Vector3D(X.Control(error.X), Y.Control(error.Y), Z.Control(error.Z)));
}
public void Update(TimeSpan timestamp, UpdateFrequency updateFlags)
{
runs++;
if (runs % 30 == 0)
{
MaxLiftThrust = 0;
MaxLateralThrust = 0;
MaxDownThrust = 0;
foreach (var kvp in Thrusters)
{
if (kvp.Key == Base6Directions.Direction.Down)
{
foreach (var thruster in kvp.Value)
{
MaxLiftThrust += thruster.MaxEffectiveThrust;
}
}
else if (kvp.Key == Base6Directions.Direction.Forward)
{
foreach (var thruster in kvp.Value)
{
MaxLateralThrust += thruster.MaxEffectiveThrust;
}
}
else if (kvp.Key == Base6Directions.Direction.Up)
{
foreach (var thruster in kvp.Value)
{
MaxDownThrust += thruster.MaxEffectiveThrust;
}
}
ThrusterManagers[kvp.Key].RecalculateThrust();
}
}
if (runs % 5 == 0)
{
StatusBuilder.Clear();
shipMass = Controller.CalculateShipMass().PhysicalMass;
gravDir = Controller.GetNaturalGravity();
double yawAngle, pitchAngle, spinAngle;
yawAngle = pitchAngle = spinAngle = 0;
MatrixD orientationMatrix = new MatrixD();
var flatCurrentDir = Controller.WorldMatrix.Forward - VectorHelpers.VectorProjection(Controller.WorldMatrix.Forward, gravDir);
flatCurrentDir.Normalize();
var flatLeftDir = Vector3D.Cross(flatCurrentDir, gravDir);
if (gravDir != Vector3D.Zero)
{
gravStr = gravDir.Length();
gravDir.Normalize();
// Rotational Control
var targetDir = Vector3D.Zero;
if (ForwardDir != Vector3D.Zero)
{
targetDir = ForwardDir;
}
else if (FullAuto)
{
targetDir = Controller.WorldMatrix.Forward - VectorHelpers.VectorProjection(Controller.WorldMatrix.Forward, gravDir);
}
if (targetDir != Vector3D.Zero)
{
if (UpDir == Vector3D.Zero)
{
var angleFromVertical = VectorHelpers.VectorAngleBetween(targetDir, gravDir) - Math.PI * 0.5;
var maxAngleFromVertical = GetMaxAngleConstraint();
angleFromVertical = Math.Max(Math.Min(angleFromVertical, maxAngleFromVertical), -maxAngleFromVertical);
var flatAimDir = targetDir - VectorHelpers.VectorProjection(targetDir, gravDir);
flatAimDir.Normalize();
var downDir = TrigHelpers.FastCos(angleFromVertical) * gravDir + TrigHelpers.FastSin(angleFromVertical) * flatAimDir;
orientationMatrix.Forward = Controller.WorldMatrix.Down;
orientationMatrix.Left = Controller.WorldMatrix.Left;
orientationMatrix.Up = Controller.WorldMatrix.Forward;
spinAngle = -VectorHelpers.VectorAngleBetween(flatAimDir, flatCurrentDir) * Math.Sign(Controller.WorldMatrix.Left.Dot(flatAimDir));
TrigHelpers.GetRotationAngles(downDir, orientationMatrix.Forward, orientationMatrix.Left, orientationMatrix.Up, out yawAngle, out pitchAngle);
}
else
{
orientationMatrix = reference.WorldMatrix;
TrigHelpers.GetRotationAngles(ForwardDir, reference.WorldMatrix.Forward, reference.WorldMatrix.Left, reference.WorldMatrix.Up, out yawAngle, out pitchAngle);
var projectedTargetUp = UpDir - reference.WorldMatrix.Forward.Dot(UpDir) * reference.WorldMatrix.Forward;
spinAngle = -1 * VectorHelpers.VectorAngleBetween(reference.WorldMatrix.Up, projectedTargetUp) * Math.Sign(reference.WorldMatrix.Left.Dot(UpDir));
}
}
}
else if (ForwardDir != Vector3D.Zero)
{
orientationMatrix = reference.WorldMatrix;
TrigHelpers.GetRotationAngles(ForwardDir, reference.WorldMatrix.Forward, reference.WorldMatrix.Left, reference.WorldMatrix.Up, out yawAngle, out pitchAngle);
if (UpDir != Vector3D.Zero)
{
var projectedTargetUp = UpDir - reference.WorldMatrix.Forward.Dot(UpDir) * reference.WorldMatrix.Forward;
spinAngle = -1 * VectorHelpers.VectorAngleBetween(reference.WorldMatrix.Up, projectedTargetUp) * Math.Sign(reference.WorldMatrix.Left.Dot(UpDir));
}
}
if (yawAngle != 0 || pitchAngle != 0 || spinAngle != 0)
{
TrigHelpers.ApplyGyroOverride(PitchPID.Control(pitchAngle), YawPID.Control(yawAngle), gravDir == Vector3D.Zero ? spinAngle : SpinPID.Control(spinAngle), Gyros, orientationMatrix);
}
else
{
foreach (var gyro in Gyros)
{
if (gyro.GyroOverride)
{
gyro.GyroOverride = false;
}
}
}
// Translational Control
if (Destination == Vector3D.Zero && TargetDrift == Vector3D.Zero)
{
foreach (var kvp in ThrusterManagers)
{
kvp.Value.SetThrust(0);
}
if (FullAuto)
{
Controller.DampenersOverride = true;
}
}
else
{
Controller.DampenersOverride = false;
// Compute direction of motion
var destinationDir = Destination - Reference.GetPosition();
var destinationDist = destinationDir.Length();
destinationDir.Normalize();
// Compute current motion to find desired acceleration
var currentVel = Controller.GetShipVelocities().LinearVelocity;
if (destinationDist < 0.25 && currentVel.Length() < 0.25 && TargetDrift == Vector3D.Zero)
{
foreach (var kvp in ThrusterManagers)
{
kvp.Value.SetThrust(0);
}
Controller.DampenersOverride = true;
Destination = Vector3D.Zero;
}
else
{
Vector3D desiredVel = Vector3D.Zero;
if (Destination != Vector3D.Zero)
{
var maxSpeed = Math.Min(MaxSpeed, GetMaxSpeedFromBrakingDistance(destinationDist, GetMaxAccelFromAngleDeviation((float)GetMaxAngleConstraint() * MaxAngleTolerance)) * 0.9);
maxSpeed = Math.Min(maxSpeed, destinationDist * destinationDist + 0.5);
desiredVel = destinationDir * maxSpeed;
}
desiredVel += TargetDrift;
var adjustVector = currentVel - VectorHelpers.VectorProjection(currentVel, desiredVel);
var desiredAccel = desiredVel - currentVel - adjustVector * 2;
// Transform desired acceleration into remote control frame
var gridDesiredAccel = Vector3D.TransformNormal(desiredAccel, MatrixD.Transpose(Controller.WorldMatrix));
double accelMagnitude = gridDesiredAccel.Length();
if (accelMagnitude < PrecisionThreshold)
{
gridDesiredAccel *= Math.Max(accelMagnitude / PrecisionThreshold, .1);
}
gridDesiredAccel.X = XPID.Control(gridDesiredAccel.X);
gridDesiredAccel.Y = YPID.Control(gridDesiredAccel.Y);
gridDesiredAccel.Z = ZPID.Control(gridDesiredAccel.Z);
double MinScale = 10;
var gridGravDir = Vector3D.TransformNormal(gravDir, MatrixD.Transpose(Controller.WorldMatrix));
foreach (var kvp in ThrusterManagers)
{
var desiredDirectionalThrust = -1 * Base6Directions.GetVector(kvp.Key).Dot(gridDesiredAccel) * shipMass;
var gravAssist = Base6Directions.GetVector(kvp.Key).Dot(gridGravDir) * shipMass;
if (desiredDirectionalThrust > 0)
{
MinScale = Math.Min((kvp.Value.MaxThrust - gravAssist) / desiredDirectionalThrust, MinScale);
}
}
gridDesiredAccel *= MinScale;
gridDesiredAccel -= gridGravDir * gravStr;
foreach (var kvp in ThrusterManagers)
{
kvp.Value.SetThrust(-1 * Base6Directions.GetVector(kvp.Key).Dot(gridDesiredAccel + gridGravDir) * shipMass);
}
}
}
}
}
