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(TimeSpan timestamp, UpdateFrequency updateFlags)
{
if (runs % 240 == 0)
{
statusBuilder.Clear();
foreach (var tube in LandpedoTubes)
{
tube.CheckLandpedo();
// statusBuilder.AppendLine(tube.GetStatus());
}
}
if (runs % 10 == 0)
{
statusBuilder.Clear();
var targets = IntelProvider.GetFleetIntelligences(timestamp);
var canonicalTime = IntelProvider.CanonicalTimeDiff + timestamp;
DeadLandpedos.Clear();
foreach (var landpedo in Landpedos)
{
statusBuilder.AppendLine("LANDPEDO===");
if (landpedo.Fired)
{
landpedo.DeadCount--;
}
if (!landpedo.IsOK())
{
DeadLandpedos.Add(landpedo);
continue;
}
landpedo.runs++;
var landpedoPosition = landpedo.Controller.WorldMatrix.Translation;
var landpedoVelocity = landpedo.Controller.GetShipVelocities().LinearVelocity;
var gravDir = landpedo.Controller.GetNaturalGravity();
var gravStr = gravDir.Normalize();
var planarVelocity = landpedoVelocity - VectorHelpers.VectorProjection(landpedoVelocity, gravDir);
if (landpedo.TargetID != -1)
{
var key = MyTuple.Create(IntelItemType.Enemy, landpedo.TargetID);
if (targets.ContainsKey(key))
{
var target = targets[key];
landpedo.TargetPosition = target.GetPositionFromCanonicalTime(canonicalTime);
landpedo.TargetVelocity = target.GetVelocity();
}
}
statusBuilder.AppendLine(landpedo.TargetPosition.ToString());
if (landpedo.TargetPosition != Vector3D.Zero)
{
var relativeVector = landpedo.TargetPosition - landpedoPosition;
var planarVector = relativeVector - VectorHelpers.VectorProjection(relativeVector, gravDir);
var targetPoint = AttackHelpers.GetAttackPoint(landpedo.TargetVelocity, landpedo.TargetPosition - landpedoPosition, landpedo.lastSpeed);
if (targetPoint == Vector3D.Zero)
{
targetPoint = landpedo.TargetPosition - landpedoPosition;
}
var targetPointDist = targetPoint.Length();
var planarDist = planarVector.Length();
var velocity = landpedo.Controller.GetShipVelocities().LinearVelocity;
var verticalVelocity = VectorHelpers.VectorProjection(velocity, gravDir);
var planarLeft = landpedo.Controller.WorldMatrix.Left - VectorHelpers.VectorProjection(landpedo.Controller.WorldMatrix.Left, gravDir);
planarLeft.Normalize();
var planarForward = landpedo.Controller.WorldMatrix.Forward - VectorHelpers.VectorProjection(landpedo.Controller.WorldMatrix.Forward, gravDir);
planarForward.Normalize();
double altitude;
landpedo.Controller.TryGetPlanetElevation(MyPlanetElevation.Surface, out altitude);
if (targetPointDist > 350 || landpedo.Launchers.Count == 0)
{
landpedo.desiredAltitude = planarVector.Length() > 200 ? 10 : 50;
planarVector.Normalize();
MatrixD orientationMatrix = MatrixD.Identity;
orientationMatrix.Translation = landpedo.Controller.WorldMatrix.Translation;
orientationMatrix.Up = -gravDir;
orientationMatrix.Left = planarLeft;
orientationMatrix.Forward = planarForward;
var spinAngle = VectorHelpers.VectorAngleBetween(planarForward, planarVector) * Math.Sign(-planarLeft.Dot(planarVector));
// var planarVelocity = velocity - verticalVelocity;
// var velocityAdjust = planarVelocity - VectorHelpers.VectorProjection(velocity, planarVector);
// velocityAdjust.Normalize();
// planarVector -= velocityAdjust;
// var MoveIndicator = Vector3D.TransformNormal(planarVector, MatrixD.Transpose(orientationMatrix));
var rangeVector = planarVector;
var waypointVector = rangeVector;
var distTargetSq = rangeVector.LengthSquared();
var targetPlanarVelocity = landpedo.TargetVelocity - VectorHelpers.VectorProjection(landpedo.TargetVelocity, gravDir);
Vector3D velocityVector = targetPlanarVelocity - planarVelocity;
var speed = planarVelocity.Length();
Vector3D AccelerationVector;
double alignment = planarVelocity.Dot(ref waypointVector);
if (alignment > 0)
{
Vector3D rangeDivSqVector = waypointVector / waypointVector.LengthSquared();
Vector3D compensateVector = velocityVector - (velocityVector.Dot(ref waypointVector) * rangeDivSqVector);
Vector3D targetANVector;
var targetAccel = (landpedo.lastTargetVelocity - targetPlanarVelocity) * 0.16667;
targetANVector = targetAccel - (targetAccel.Dot(ref waypointVector) * rangeDivSqVector);
bool accelerating = speed > landpedo.lastSpeed + 1;
if (accelerating)
{
AccelerationVector = planarVelocity + (10 * 1.5 * (compensateVector + (0.5 * targetANVector)));
}
else
{
AccelerationVector = planarVelocity + (10 * (compensateVector + (0.5 * targetANVector)));
}
}
// going backwards or perpendicular
else
{
AccelerationVector = (waypointVector * 0.1) + velocityVector;
}
landpedo.lastTargetVelocity = landpedo.TargetVelocity;
landpedo.lastSpeed = speed;
var MoveIndicator = Vector3D.TransformNormal(AccelerationVector, MatrixD.Transpose(orientationMatrix));
MoveIndicator.Y = 0;
MoveIndicator.Normalize();
statusBuilder.AppendLine(MoveIndicator.ToString());
landpedo.Drive.MoveIndicators = MoveIndicator;
landpedo.Drive.RotationIndicators = new Vector3(0, spinAngle, 0);
landpedo.Drive.Drive();
if (verticalVelocity.Length() > 10 && verticalVelocity.Dot(gravDir) > 0)
{
landpedo.Drive.maxFlightPitch = 20;
landpedo.Drive.maxFlightRoll = 20;
}
else
{
landpedo.Drive.maxFlightPitch = 60;
landpedo.Drive.maxFlightRoll = 60;
}
if (targetPointDist < 1000)
{
landpedo.Drive.maxFlightPitch = 20;
}
}
else if (landpedo.TargetID != -1)
{
var key = MyTuple.Create(IntelItemType.Enemy, landpedo.TargetID);
if (!targets.ContainsKey(key))
{
return;
}
var target = targets[key];
var posDiff = target.GetPositionFromCanonicalTime(canonicalTime) - landpedoPosition;
var avgVel = 400 * Math.Sqrt(posDiff.Length() / 400);
var relativeAttackPoint = AttackHelpers.GetAttackPoint(landpedo.TargetVelocity - velocity, posDiff, avgVel);
targetPointDist = relativeAttackPoint.Length();
double yawAngle, pitchAngle;
TrigHelpers.GetRotationAngles(relativeAttackPoint, landpedo.Controller.WorldMatrix.Forward, landpedo.Controller.WorldMatrix.Left, landpedo.Controller.WorldMatrix.Up, out yawAngle, out pitchAngle);
TrigHelpers.ApplyGyroOverride(landpedo.PitchPID.Control(pitchAngle), landpedo.YawPID.Control(yawAngle), 0, landpedo.Gyros, landpedo.Controller.WorldMatrix);
if ((targetPointDist < 125 || landpedo.minDist < planarDist))
{
foreach (var weapon in landpedo.Launchers)
{
weapon.Enabled = true;
}
landpedo.Fired = true;
}
landpedo.minDist = Math.Min(landpedo.minDist, planarDist);
}
var verticalThrustRatio = TrigHelpers.FastCos(VectorHelpers.VectorAngleBetween(landpedo.Controller.WorldMatrix.Down, gravDir));
var desiredThrust = ((landpedo.desiredAltitude - altitude) + verticalVelocity.LengthSquared() * 0.1 + gravStr) * landpedo.Controller.CalculateShipMass().PhysicalMass / verticalThrustRatio;
var individualThrust = desiredThrust / landpedo.Thrusters.Count();
if (individualThrust <= 0)
{
individualThrust = 0.001;
}
foreach (var thruster in landpedo.Thrusters)
{
thruster.Enabled = ((verticalVelocity.Length() > 20 && verticalVelocity.Dot(gravDir) < 0) || ((verticalVelocity.Length() > 2 && verticalVelocity.Dot(gravDir) < 0) && targetPointDist < 1000)) ? false : true;
thruster.ThrustOverride = (float)individualThrust;
}
statusBuilder.AppendLine($"{landpedo.Drive.thrustController.CalculateThrustToHover()}");
statusBuilder.AppendLine($"{landpedo.Drive.thrustController.upThrusters.Count} : {landpedo.Drive.thrustController.downThrusters.Count}");
}
}
foreach (var landpedo in DeadLandpedos)
{
Landpedos.Remove(landpedo);
}
}
runs++;
}
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);
}
}
}
}
}
