void InitializeOrbit()
{
if (!lastEnemy.HasValue)
{
return;
}
v1 = rc.GetPosition() - lastEnemy.Value.Position;
radius = MathHelper.Clamp(v1.Length(), minRadius, maxRadius);
v1.Normalize();
if (!useGravity)
{
Vector3D temp = RandomVector();
v2 = v1.Cross(temp);
}
else
{
Vector3D gravity = rc.GetNaturalGravity();
gravity.Normalize();
Vector3D temp = gravity.Dot(v1) * gravity;
v1 -= temp;
v2 = v1.Cross(gravity);
}
v2.Normalize();
theta = 0;
double angularVelocity = orbitVelocity / radius;
orbitStep = angularVelocity * (1f / 60);
}
public void Main()
{
//Получаем и нормализуем вектор гравитации. Это наше направление "вниз" на планете.
Vector3D GravityVector = RemCon.GetNaturalGravity();
Vector3D GravNorm = Vector3D.Normalize(GravityVector);
//Получаем проекции вектора прицеливания на все три оси блока ДУ.
double gF = GravNorm.Dot(RemCon.WorldMatrix.Forward);
double gL = GravNorm.Dot(RemCon.WorldMatrix.Left);
double gU = GravNorm.Dot(RemCon.WorldMatrix.Up);
//Получаем сигналы по тангажу и крены операцией atan2
float RollInput = (float)Math.Atan2(gL, -gU);
float PitchInput = -(float)Math.Atan2(gF, -gU);
//На рыскание просто отправляем сигнал рыскания с контроллера. Им мы будем управлять вручную.
float YawInput = RemCon.RotationIndicator.Y;
//для каждого гироскопа устанавливаем текущие значения по тангажу, крену, рысканию.
foreach (IMyGyro gyro in gyroList)
{
gyro.GyroOverride = true;
gyro.Yaw = YawInput;
gyro.Roll = RollInput;
gyro.Pitch = PitchInput;
}
}
///
/// обновляем вектор гравитации с учетом массы корабля
///
private void updateWeightVector()
{
Vector3D gravityVector = remoteControl.GetNaturalGravity();
float shipMass = remoteControl.CalculateShipMass().PhysicalMass;
Vector3D.Multiply(ref gravityVector, shipMass, out weightVector);
}
//конструктор
public Program()
{
RemCon = GridTerminalSystem.GetBlockWithName("RemCon") as IMyRemoteControl;
Gyros = new List <IMyGyro>();
GridTerminalSystem.GetBlocksOfType <IMyGyro>(Gyros);
Runtime.UpdateFrequency = UpdateFrequency.Update1;
Forward = RemCon.WorldMatrix.Forward;
Forward = Vector3D.Reject(Forward, Vector3D.Normalize(RemCon.GetNaturalGravity()));
}
public bool getGravity(out Vector3D gravity)
{
gravity = Vector3D.Zero;
if (_init())
{
return(true);
}
gravity = rc.GetNaturalGravity();
return(false);
}
void Main(string argument)
{
if (rc == null)
{
setup();
}
//Get orientation from rc
Matrix or;
rc.Orientation.GetMatrix(out or);
Vector3D down = or.Down;
Vector3D grav = rc.GetNaturalGravity();
grav.Normalize();
for (int i = 0; i < gyros.Count; ++i)
{
var g = gyros[i];
g.Orientation.GetMatrix(out or);
var localDown = Vector3D.Transform(down, MatrixD.Transpose(or));
var localGrav = Vector3D.Transform(grav, MatrixD.Transpose(g.WorldMatrix.GetOrientation()));
//Since the gyro ui lies, we are not trying to control yaw,pitch,roll but rather we
//need a rotation vector (axis around which to rotate)
var rot = Vector3D.Cross(localDown, localGrav);
double ang = rot.Length();
ang = Math.Atan2(ang, Math.Sqrt(Math.Max(0.0, 1.0 - ang * ang))); //More numerically stable than: ang=Math.Asin(ang)
if (ang < 0.01)
{ //Close enough
//Echo("Level");
g.SetValueBool("Override", false);
continue;
}
//Echo("Off level: "+(ang*180.0/3.14).ToString()+"deg");
//Control speed to be proportional to distance (angle) we have left
double ctrl_vel = g.GetMaximum <float>("Yaw") * (ang / Math.PI) * CTRL_COEFF;
ctrl_vel = Math.Min(g.GetMaximum <float>("Yaw"), ctrl_vel);
ctrl_vel = Math.Max(0.01, ctrl_vel); //Gyros don't work well at very low speeds
rot.Normalize();
rot *= ctrl_vel;
g.SetValueFloat("Pitch", (float)rot.GetDim(0));
g.SetValueFloat("Yaw", -(float)rot.GetDim(1));
g.SetValueFloat("Roll", -(float)rot.GetDim(2));
g.SetValueFloat("Power", 1.0f);
g.SetValueBool("Override", true);
}
}
Vector3D _gravity()
{
if (_initRc())
{
return(new Vector3D());
}
else
{
return(rc.GetNaturalGravity());
}
}
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);
}
}
}
// методы
public void Main()
{
// получим уклон по осям вперед/назад и влево-вправо
Vector3D GravVector = Vector3D.Normalize(RemCon.GetNaturalGravity());
GravVector = Vector3D.Reject(GravVector, RemCon.WorldMatrix.Down);
float Pitch = (float)GravVector.Dot(RemCon.WorldMatrix.Backward);
float Roll = (float)GravVector.Dot(RemCon.WorldMatrix.Left);
float Yaw = (float)Forward.Dot(Vector3D.Reject(RemCon.WorldMatrix.Forward, Vector3D.Normalize(RemCon.GetNaturalGravity())));
// For debug only
// Echo("Pitch: " + Pitch + "\n Roll: " + Roll + "\n Yaw: " + Yaw);
foreach (IMyGyro Gyro in Gyros)
{
Gyro.Pitch = Pitch * GyroMult;
Gyro.Roll = Roll * GyroMult;
Gyro.Yaw = Yaw * GyroMult;
}
}
void level()
{
Runtime.UpdateFrequency = UpdateFrequency.Update10;
//Get orientation from remoteControl
Matrix orientation;
remoteControl.Orientation.GetMatrix(out orientation);
Vector3D down = orientation.Down;
Vector3D grav = remoteControl.GetNaturalGravity();
grav.Normalize();
running = true;
for (int i = 0; i < gyroList.Count; ++i)
{
var g = gyroList[i];
g.Orientation.GetMatrix(out orientation);
var localDown = Vector3D.Transform(down, MatrixD.Transpose(orientation));
var localGrav = Vector3D.Transform(grav, MatrixD.Transpose(g.WorldMatrix.GetOrientation()));
var rot = Vector3D.Cross(localDown, localGrav);
ang = rot.Length();
ang = Math.Atan2(ang, Math.Sqrt(Math.Max(0.0, 1.0 - ang * ang)));
double ctrl_vel = g.GetMaximum <float>("Yaw") * (ang / Math.PI) * CTRL_COEFF;
ctrl_vel = Math.Min(g.GetMaximum <float>("Yaw"), ctrl_vel);
ctrl_vel = Math.Max(0.01, ctrl_vel); //Gyros don't work well at very low speeds
rot.Normalize();
rot *= ctrl_vel;
g.SetValueFloat("Pitch", (float)rot.GetDim(0));
g.SetValueFloat("Yaw", -(float)rot.GetDim(1));
g.SetValueFloat("Roll", -(float)rot.GetDim(2));
g.SetValueFloat("Power", 1.0f);
g.SetValueBool("Override", true);
}
}
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;
}
private bool EvaluateCondition()
{
switch (currentCondition)
{
case Conditions.NONE:
return(true);
case Conditions.PGRA:
double currentPGravity = control.GetNaturalGravity().Length();
switch (currentOperator)
{
case Operators.EQUAL:
return(currentPGravity == value);
case Operators.LESS:
return(currentPGravity < value);
case Operators.MORE:
return(currentPGravity > value);
}
break;
case Conditions.AGRA:
double currentAGravity = control.GetArtificialGravity().Length();
switch (currentOperator)
{
case Operators.EQUAL:
return(currentAGravity == value);
case Operators.LESS:
return(currentAGravity < value);
case Operators.MORE:
return(currentAGravity > value);
}
break;
case Conditions.SPEED:
double currentSpeed = control.GetShipSpeed();
switch (currentOperator)
{
case Operators.EQUAL:
return(currentSpeed == value);
case Operators.LESS:
return(currentSpeed < value);
case Operators.MORE:
return(currentSpeed > value);
}
break;
case Conditions.TIME:
if (time == -1)
{
time = DateTime.Now.Millisecond;
}
if (DateTime.Now.Millisecond - time > value * 1000)
{
time = -1;
return(true);
}
return(false);
case Conditions.LOCATION:
BoundingSphereD boundaries = control.CubeGrid.WorldVolume;
return(boundaries.Contains(conditionGPS) != ContainmentType.Disjoint);
}
return(false);
}
