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