public Vector3D GetBurnVector(Vector3D targetPosition, Vector3D targetVelocity, IMyRemoteControl core, Vector3 offset) { Vector3D targetVector = targetPosition - core.GetPosition(); double timeToTarget = targetVector.Length() / 100; return(targetVector + timeToTarget * (targetVelocity - 0.9 * core.GetShipVelocities().LinearVelocity)); }
Vector3D _velocity() { if (_checkOrInitRc()) { return(new Vector3D()); } return(rc.GetShipVelocities().LinearVelocity); }
Vector3D GetLeadPosition(Vector3D targetPos, Vector3D targetVel) { Vector3D ownPos = rc.GetPosition(); Vector3D ownVelocity = rc.GetShipVelocities().LinearVelocity; double oldTime = 0.01; Vector3D predictedPos = new Vector3D(); for (int i = 0; i < 20; i++) { predictedPos = targetPos + targetVel * oldTime; Vector3D path = predictedPos - ownPos; double newTime = path.Length() / muzzleVelocity; if (Math.Abs(newTime - oldTime) < 0.000001) { break; } oldTime = newTime; } return(predictedPos - ownVelocity * oldTime); }
Vector3D GetBurnVector(IMyRemoteControl core, bool oldSystem) { if (oldSystem) { Vector3D targetVector = predictedHitPoint - core.GetPosition(); targetVector.Normalize(); targetVector = targetVector * 120; Vector3D velocity = core.GetShipVelocities().LinearVelocity; Vector3D correctVector = targetVector - velocity; return(correctVector + core.GetPosition()); } else { Vector3D targetVector = predictedPosition - core.GetPosition(); return(targetVector + timeToTarget * (TargetInfo.Velocity - 0.9 * core.GetShipVelocities().LinearVelocity)); } }
public void OnFlightUpdateFrame() { if (is_enabled) { desired = Vector3D.Zero; velocity = control.GetShipVelocities().LinearVelocity; gravity = control.GetNaturalGravity(); ship_mass = control.CalculateShipMass(); effective_breaking_distance = Settings.ARRIVAL_DIST * (ship_mass.TotalMass / ship_mass.BaseMass); tasks.OnUpdateFrame(); if ((flags & SpaceshipFlags.CAS) == SpaceshipFlags.CAS) { cas.OnUpdateFrame(); } if ((flags & SpaceshipFlags.FD) == SpaceshipFlags.FD) { fd.OnUpdateFrame(); } Vector3DLimit(ref desired, Settings.MAX_SPEED); if ((flags & SpaceshipFlags.AP) == SpaceshipFlags.AP) { autopilot.OnUpdateFrame(); } if ((flags & SpaceshipFlags.TM) == SpaceshipFlags.TM) { thrusters.OnUpdateFrame(); } else { thrusters.Disable(); } CheckConnector(); if (fd.prev == fd.next) { fd.SetFlightPlan(null); } } }
double CAF_DIST_TO_TARGET; //Outputs distance to target void CollectAndFire2(Vector3D INPUT_POINT, double INPUT_VELOCITY, double INPUT_MAX_VELOCITY, Vector3D REFPOS, IMyRemoteControl RC) { //Function Initialisation //-------------------------------------------------------------------- if (C_A_F_HASRUN == false) { //Initialise Classes And Basic System CAF2_FORWARD = new Thrust_info(RC.WorldMatrix.Forward, myGridTerminal, RC.CubeGrid); CAF2_UP = new Thrust_info(RC.WorldMatrix.Up, myGridTerminal, RC.CubeGrid); CAF2_RIGHT = new Thrust_info(RC.WorldMatrix.Right, myGridTerminal, RC.CubeGrid); CAFTHI = new List <Thrust_info>() { CAF2_FORWARD, CAF2_UP, CAF2_RIGHT }; myGridTerminal.GetBlocksOfType <IMyThrust>(CAF2_THRUST, block => block.CubeGrid == RC.CubeGrid); C_A_F_HASRUN = true; //Initialises Braking Component foreach (var item in CAFTHI) { CAF2_BRAKING_COUNT = (item.PositiveMaxForce < CAF2_BRAKING_COUNT) ? item.PositiveMaxForce : CAF2_BRAKING_COUNT; CAF2_BRAKING_COUNT = (item.NegativeMaxForce < CAF2_BRAKING_COUNT) ? item.PositiveMaxForce : CAF2_BRAKING_COUNT; } } //Generating Maths To Point and decelleration information etc. //-------------------------------------------------------------------- double SHIPMASS = Convert.ToDouble(RC.CalculateShipMass().PhysicalMass); Vector3D INPUT_VECTOR = Vector3D.Normalize(INPUT_POINT - REFPOS); double VELOCITY = RC.GetShipSpeed(); CAF_DIST_TO_TARGET = (REFPOS - INPUT_POINT).Length(); CAF_SHIP_DECELLERATION = 0.75 * (CAF2_BRAKING_COUNT / SHIPMASS); CAF_STOPPING_DIST = (((VELOCITY * VELOCITY) - (INPUT_VELOCITY * INPUT_VELOCITY))) / (2 * CAF_SHIP_DECELLERATION); //If Within Stopping Distance Halts Programme //-------------------------------------------- if (!(CAF_DIST_TO_TARGET > (CAF_STOPPING_DIST + 0.25)) || CAF_DIST_TO_TARGET < 0.25 || VELOCITY > INPUT_MAX_VELOCITY) { foreach (var thruster in CAF2_THRUST) { (thruster as IMyThrust).ThrustOverride = 0; } return; } //dev notes, this is the most major source of discontinuity between theorised system response //Reflects Vector To Cancel Orbiting //------------------------------------ Vector3D DRIFT_VECTOR = Vector3D.Normalize(RC.GetShipVelocities().LinearVelocity + RC.WorldMatrix.Forward * 0.00001); Vector3D R_DRIFT_VECTOR = -1 * Vector3D.Normalize(Vector3D.Reflect(DRIFT_VECTOR, INPUT_VECTOR)); R_DRIFT_VECTOR = ((Vector3D.Dot(R_DRIFT_VECTOR, INPUT_VECTOR) < -0.3)) ? 0 * R_DRIFT_VECTOR : R_DRIFT_VECTOR; INPUT_VECTOR = Vector3D.Normalize((4 * R_DRIFT_VECTOR) + INPUT_VECTOR); //Components Of Input Vector In FUR Axis //---------------------------------------- double F_COMP_IN = Vector_Projection(INPUT_VECTOR, RC.WorldMatrix.Forward); double U_COMP_IN = Vector_Projection(INPUT_VECTOR, RC.WorldMatrix.Up); double R_COMP_IN = Vector_Projection(INPUT_VECTOR, RC.WorldMatrix.Right); //Calculate MAX Allowable in Each Axis & Length //----------------------------------------------- double F_COMP_MAX = (F_COMP_IN > 0) ? CAF2_FORWARD.PositiveMaxForce : -1 * CAF2_FORWARD.NegativeMaxForce; double U_COMP_MAX = (U_COMP_IN > 0) ? CAF2_UP.PositiveMaxForce : -1 * CAF2_UP.NegativeMaxForce; double R_COMP_MAX = (R_COMP_IN > 0) ? CAF2_RIGHT.PositiveMaxForce : -1 * CAF2_RIGHT.NegativeMaxForce; double MAX_FORCE = Math.Sqrt(F_COMP_MAX * F_COMP_MAX + U_COMP_MAX * U_COMP_MAX + R_COMP_MAX * R_COMP_MAX); //Apply Length to Input Components and Calculates Smallest Multiplier //-------------------------------------------------------------------- double F_COMP_PROJ = F_COMP_IN * MAX_FORCE; double U_COMP_PROJ = U_COMP_IN * MAX_FORCE; double R_COMP_PROJ = R_COMP_IN * MAX_FORCE; double MULTIPLIER = 1; MULTIPLIER = (F_COMP_MAX / F_COMP_PROJ < MULTIPLIER) ? F_COMP_MAX / F_COMP_PROJ : MULTIPLIER; MULTIPLIER = (U_COMP_MAX / U_COMP_PROJ < MULTIPLIER) ? U_COMP_MAX / U_COMP_PROJ : MULTIPLIER; MULTIPLIER = (R_COMP_MAX / R_COMP_PROJ < MULTIPLIER) ? R_COMP_MAX / R_COMP_PROJ : MULTIPLIER; //Calculate Multiplied Components //--------------------------------- CAF2_FORWARD.VCF = ((F_COMP_PROJ * MULTIPLIER) / F_COMP_MAX) * Math.Sign(F_COMP_MAX); CAF2_UP.VCF = ((U_COMP_PROJ * MULTIPLIER) / U_COMP_MAX) * Math.Sign(U_COMP_MAX); CAF2_RIGHT.VCF = ((R_COMP_PROJ * MULTIPLIER) / R_COMP_MAX) * Math.Sign(R_COMP_MAX); //Runs System Thrust Application //---------------------------------- Dictionary <IMyThrust, float> THRUSTVALUES = new Dictionary <IMyThrust, float>(); foreach (var thruster in CAF2_THRUST) { THRUSTVALUES.Add((thruster as IMyThrust), 0f); } foreach (var THRUSTSYSTM in CAFTHI) { List <IMyThrust> POSTHRUST = THRUSTSYSTM.PositiveThrusters; List <IMyThrust> NEGTHRUST = THRUSTSYSTM.NegativeThrusters; if (THRUSTSYSTM.VCF < 0) { POSTHRUST = THRUSTSYSTM.NegativeThrusters; NEGTHRUST = THRUSTSYSTM.PositiveThrusters; } foreach (var thruster in POSTHRUST) { THRUSTVALUES[thruster as IMyThrust] = (float)(Math.Abs(THRUSTSYSTM.VCF)) * (thruster as IMyThrust).MaxThrust; } foreach (var thruster in NEGTHRUST) { THRUSTVALUES[thruster as IMyThrust] = 1; } //(float)0.01001;} foreach (var thruster in THRUSTVALUES) { thruster.Key.ThrustOverride = thruster.Value; } //thruster.Key.ThrustOverride = thruster.Value; } }
//Primary Generic Functions //========================== //Use For General Drone Flying: void RC_Manager(Vector3D TARGET, IMyRemoteControl RC, bool TURN_ONLY) { //Uses Rotation Control To Handle Max Rotational Velocity //--------------------------------------------------------- if (RC.GetShipVelocities().AngularVelocity.AbsMax() > PrecisionMaxAngularVel) { print("转动速度放缓"); RC.SetAutoPilotEnabled(false); return; } //Setup Of Common Variables //-------------------------------------------- Vector3D DronePosition = RC.GetPosition(); Vector3D Drone_To_Target = Vector3D.Normalize(TARGET - DronePosition); //Override Direction Detection //------------------------------- double To_Target_Angle = Vector3D.Dot(Vector3D.Normalize(RC.GetShipVelocities().LinearVelocity), Drone_To_Target); double Ship_Velocity = RC.GetShipVelocities().LinearVelocity.Length(); //Turn Only: (Will drift ship automatically) //-------------------------------------------- /*List<MyWaypointInfo> way = new List<MyWaypointInfo>(); * RC.GetWaypointInfo(way); * if (way.Count>0) * { * if (way[0].Coords!= TARGET) * { * //RC.ApplyAction("AutoPilot_Off"); * //RC.ClearWaypoints(); * } * }*/ if (TURN_ONLY) { //if (way.Count <1) { RC.ClearWaypoints(); GYRO.GyroOverride = false; RC.AddWaypoint(TARGET, "母船起点"); RC.FlightMode = FlightMode.OneWay; RC.Direction = Base6Directions.Direction.Forward; RC.ApplyAction("AutoPilot_On"); RC.ApplyAction("CollisionAvoidance_Off"); RC.ControlThrusters = false; } return; } //Drift Cancellation Enabled: //----------------------------- if (To_Target_Angle < 0.4 && Ship_Velocity > 3) { //Aim Gyro To Reflected Vector Vector3D DRIFT_VECTOR = Vector3D.Normalize(RC.GetShipVelocities().LinearVelocity); Vector3D REFLECTED_DRIFT_VECTOR = -1 * (Vector3D.Normalize(Vector3D.Reflect(DRIFT_VECTOR, Drone_To_Target))); Vector3D AIMPINGPOS = (-1 * DRIFT_VECTOR * 500) + DronePosition; //if (way.Count < 1 ) { RC.ClearWaypoints(); GYRO.GyroOverride = false; RC.AddWaypoint(AIMPINGPOS, "AIMPINGPOS"); RC.SpeedLimit = 100; RC.FlightMode = FlightMode.OneWay; RC.Direction = Base6Directions.Direction.Forward; RC.ApplyAction("AutoPilot_On"); RC.ApplyAction("CollisionAvoidance_Off"); } } //System Standard Operation: //--------------------------- else { //Assign To RC, Clear And Refresh Command List <ITerminalAction> action = new List <ITerminalAction>(); RC.GetActions(action); RC.ControlThrusters = true; RC.ClearWaypoints(); GYRO.GyroOverride = false; RC.AddWaypoint(TARGET, "目标"); RC.SpeedLimit = 100; RC.FlightMode = FlightMode.OneWay; RC.Direction = Base6Directions.Direction.Forward; RC.ApplyAction("AutoPilot_On"); RC.ApplyAction("DockingMode_Off"); RC.ApplyAction("CollisionAvoidance_On"); } }
public void Main(string argument, UpdateType updateSource)//main is called once per pysics update, ideally 60Hz { if (argument.ToLower() == "reset") { stage = 0; } bool valid = controller.TryGetPlanetElevation(MyPlanetElevation.Surface, out currentAltitude); var parts = Storage.Split(';'); stage = int.Parse(parts[0]); maxTotalEffectiveThrust = 0.0f;//have to recalculate every time because it increases with alt foreach (var thruster in thrusters) { maxTotalEffectiveThrust += thruster.MaxEffectiveThrust; } physicalMass = controller.CalculateShipMass().PhysicalMass; currentNaturalGravity = controller.GetNaturalGravity(); // double gravityMagnitude = currentNaturalGravity.Length(); //what a weird thing to call magnitude of a vector Echo($"STAGE: {stage}\nSPEED: {controller.GetShipVelocities().LinearVelocity.Length()}\nALT: {currentAltitude}"); antenna.HudText = stage.ToString(); if (stage == 0) { if (Freefall()) //if we are in freefall. Freefall is defined as accelerating at a rate ~= gravity, this implies thrusters are not acive. Also should check if it's heading towards or away from the gravity well. Linear alg time yay { foreach (var thruster in thrusters) { thruster.ThrustOverride = 0; } stage++; Storage = stage + ";"; } } else if (stage == 1) { if (!Freefall()) { stage = 0; } double a = maxTotalEffectiveThrust / physicalMass - currentNaturalGravity.Length(); //acceleration with full retro burn double b = -controller.GetShipVelocities().LinearVelocity.Length(); //velocity double c = currentAltitude; double thrustStopTime = -b / (2 * a); //y=ax^2+bx -> dy = 2ax+b | 0 = 2*a*x+b -> -b/2a=x Func <double, double> altitudeFunc = delegate(double x) { //x=time, y=altitude assuming full retro thrust return(a * x * x + b * x + c); }; double minHeight = altitudeFunc(thrustStopTime); //the lowest point of the trajectory IF thrust is engaged instantly double calculatedStoppingDistance = currentAltitude - minHeight; double safetyBuffer = Math.Abs(b) * 1.25 + craftHeight; //a coefficient in front of the safety buffer feels wrong double safetyStoppingDistance = calculatedStoppingDistance + safetyBuffer; if (!controller.DampenersOverride && currentAltitude <= safetyStoppingDistance) { stage++; Storage = stage + ";"; controller.DampenersOverride = true; } } else if (stage == 2)//descent burn has been initiated, goto stg 3 when a safe speed has been reached { antenna.HudText = stage.ToString(); if (controller.GetShipVelocities().LinearVelocity.Length() < safeVelocity) { stage++; Storage = stage + ";"; } } else if (stage == 3)//target safe descent speed has been reached, maintain low speed until touchdown to planet { float totalThrustNeededToHover = physicalMass * (float)currentNaturalGravity.Length(); float idealThrustPerThruster = (float)totalThrustNeededToHover / (float)numThrusters; float thrustRatio = thrusters[0].MaxThrust / thrusters[0].MaxEffectiveThrust;//actual output of thrust is similar to but different from the input set by code/user. float adjustedThrustPerThruster = idealThrustPerThruster * thrustRatio; foreach (var thruster in thrusters) { thruster.ThrustOverride = adjustedThrustPerThruster; } if (currentAltitude < touchdownHeight) { stage++; Storage = stage + ";"; foreach (var thruster in thrusters) { controller.DampenersOverride = false; thruster.ThrustOverride = 0.0f; } } } else if (stage == 4)//touchdown, turn stuff off pls { antenna.HudText = stage.ToString(); Echo($"TOUCHDOWN"); stage = 0; Storage = stage + ";"; Runtime.UpdateFrequency = UpdateFrequency.None;//stop running the script } lastVelocities = controller.GetShipVelocities(); }
public void Update(MyDetectedEntityInfo target, Vector3D T, Vector3D PointerPos, Vector3D PointerDir) { if (!Launched) { foreach (IMyThrust thr in thrusters) { thr.Enabled = true; thr.ThrustOverridePercentage = 1; } Launched = true; } else { counter++; if (remcon.IsFunctional && (counter > VerticalDelay)) { double currentVelocity = remcon.GetShipVelocities().LinearVelocity.Length(); Vector3D targetvector = new Vector3D(); if (LASER_GUIDED) { targetvector = ((remcon.GetPosition() - PointerPos).Dot(PointerDir) + 700) * PointerDir + PointerPos - remcon.GetPosition(); } else { targetvector = FindInterceptVector(remcon.GetPosition(), currentVelocity * INTERCEPT_COURSE, T, target.Velocity); } Vector3D trgNorm = Vector3D.Normalize(targetvector); if ((target.Position - remcon.GetPosition()).Length() < WH_ARM_DIST) { if (currentVelocity - MyVelocity < -ACCEL_DET) { foreach (IMyWarhead wh in warheads) { wh.Detonate(); } } MyVelocity = currentVelocity; } Vector3D velNorm = Vector3D.Normalize(remcon.GetShipVelocities().LinearVelocity); Vector3D CorrectionVector = Math.Max(ReflectK * trgNorm.Dot(velNorm), 1) * trgNorm - velNorm; Vector3D G = remcon.GetNaturalGravity(); if (G.LengthSquared() == 0) { CorrectionVector = Math.Max(ReflectK * trgNorm.Dot(velNorm), 1) * trgNorm - velNorm; } else { if (JAVELIN) { //trgNorm = Vector3D.Normalize(Vector3D.Reflect(-G, trgNorm)); trgNorm = Vector3D.Normalize(G.Dot(trgNorm) * trgNorm * JAVELIN_CURVE - G); } CorrectionVector = Math.Max(ReflectK * trgNorm.Dot(velNorm), 1) * trgNorm - velNorm; double A = 0; foreach (IMyThrust thr in thrusters) { A += thr.MaxEffectiveThrust; } A /= remcon.CalculateShipMass().PhysicalMass; Vector3D CorrectionNorm = Vector3D.Normalize(CorrectionVector); //CorrectionVector = CorrectionNorm * A - G; Vector3D gr = Vector3D.Reject(remcon.GetNaturalGravity(), CorrectionNorm); CorrectionVector = CorrectionNorm * Math.Sqrt(A * A + gr.LengthSquared()) - gr; } Vector3D Axis = Vector3D.Normalize(CorrectionVector).Cross(remcon.WorldMatrix.Forward); if (Axis.LengthSquared() < 0.1) { Axis += remcon.WorldMatrix.Backward * ROLL; } Axis *= GyroMult; foreach (IMyGyro gyro in gyros) { gyro.Pitch = (float)Axis.Dot(gyro.WorldMatrix.Right); gyro.Yaw = (float)Axis.Dot(gyro.WorldMatrix.Up); gyro.Roll = (float)Axis.Dot(gyro.WorldMatrix.Backward); } } else { foreach (IMyGyro gyro in gyros) { gyro.Pitch = 0; gyro.Yaw = 0; gyro.Roll = 0; } } } }
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; }
public bool FlyTo(Vector3D position, IMyTerminalBlock reference, bool align, double speedLimit = -1) { Vector3D translationVector = position - reference.GetPosition(); if (translationVector.Length() < 0.25) { AllStop(); return(true); } double targetSpeed; if (speedLimit == -1) { //TODO: modulate speed my thrust to weight ratio targetSpeed = Math.Pow(translationVector.Length(), 1 / 1.6); } else { targetSpeed = speedLimit; } Vector3D velocityDelta = Vector3D.Normalize(translationVector) * targetSpeed - Remote.GetShipVelocities().LinearVelocity; Vector3D transformedVelocityDelta = Vector3D.TransformNormal(velocityDelta, MatrixD.Transpose(Remote.WorldMatrix)); this.ManeuverService.SetThrust(new Vector3D(0, 0, transformedVelocityDelta.Z), align); this.ManeuverService.SetThrust(new Vector3D(transformedVelocityDelta.X, 0, 0), align); this.ManeuverService.SetThrust(new Vector3D(0, transformedVelocityDelta.Y, 0), align); return(false); }