private void showInfo()
{
Vector3D GravityVector = cockpit.GetNaturalGravity();
Vector3D GravNorm = Vector3D.Normalize(GravityVector);
//Получаем проекции вектора прицеливания на все три оси блока ДУ.
double gF = GravNorm.Dot(cockpit.WorldMatrix.Forward);
double gU = GravNorm.Dot(cockpit.WorldMatrix.Up);
float PitchInput = -(float)Math.Atan2(gF, -gU);
lcd.WriteText(gF.ToString());
lcd.WriteText("\n", true);
lcd.WriteText(gU.ToString(), true);
lcd.WriteText("\n", true);
lcd.WriteText(PitchInput.ToString(), true);
}
public void Main(string argument, UpdateType updateSource)
{
IMyCockpit myCockpit = (IMyCockpit)GridTerminalSystem.GetBlockWithName("Cockpit");
List <IMyThrust> upthrusters = new List <IMyThrust>();
upthrusters = GetMyUpThrusts("ThrustUP");
double gravity = myCockpit.GetNaturalGravity().Length() / 9.81;
double speed = myCockpit.GetShipSpeed();
Echo($"ship speed = {speed.ToString()}");
Echo(gravity.ToString());
if (gravity > 0.30)
{
foreach (IMyThrust ThrustUP in upthrusters)
{
ThrustUP.Enabled = true;
ThrustUP.ThrustOverridePercentage = 70;
}
Echo("im here 1");
}
else
{
foreach (IMyThrust ThrustUP in upthrusters)
{
ThrustUP.Enabled = false;
ThrustUP.ThrustOverridePercentage = 0;
}
Echo("im here 2");
}
}
public Program()
{
cockpit = GridTerminalSystem.GetBlockWithName(NAME_COCKPIT) as IMyCockpit;
if (cockpit != null)
{
Echo("init OK");
Echo("SurfaceCount: " + cockpit.SurfaceCount);
lcd = cockpit.GetSurface(0); // or 1
lcd.ContentType = VRage.Game.GUI.TextPanel.ContentType.TEXT_AND_IMAGE;
Echo("OxygenCapacity: " + cockpit.OxygenCapacity);
Echo("OxygenFilledRatio: " + cockpit.OxygenFilledRatio);
// гравитация, вектор указывает на центр планеты
Vector3D gravityVector = cockpit.GetNaturalGravity();
stringBuilder.Append("\ngravityVector: ").Append(gravityVector.Length())
.Append("\n X: ").Append(gravityVector.X)
.Append("\n Y:").Append(gravityVector.Y)
.Append("\n Z:").Append(gravityVector.Z);
Echo(stringBuilder.ToString());
// масса корабля
float PhysicalMass = cockpit.CalculateShipMass().PhysicalMass;
Echo("PhysicalMass: " + PhysicalMass);
Runtime.UpdateFrequency = UpdateFrequency.Update1;
}
else
{
Echo("init ERROR");
}
}
public void GyroControl()
{
var vector = cockpit.GetNaturalGravity();
var cockpitmatrix = cockpit.WorldMatrix;
Vector3D direction = Vector3D.TransformNormal(vector, Matrix.Transpose(cockpitmatrix));
float pitch = (float)direction.Z / 2;
float roll = -((float)direction.X / 2);
foreach (var item in gyros)
{
var gyro = item as IMyGyro;
gyro.Pitch = (float)pitch / 5;
gyro.Roll = (float)roll / 5;
//gyro.Pitch = pitch > 0 ? (float)Math.Pow(pitch, 2) : -(float)Math.Pow(pitch, 2);
//gyro.Roll = roll > 0 ? (float)Math.Pow(roll, 2) : -(float)Math.Pow(roll, 2);
}
//var rot = cockpit.WorldMatrix;
//var rotinv = MatrixD.Transpose(rot);
//var shipPos = cockpit.GetPosition();
//var deltaInWorld = shipPos - (shipPos + vector);
////var deltaInLocal = Vector3.Transform(deltaInWorld, rotinv);
//var deltaInLocal = Vector3D.Transform(vector, rotinv);
//var pitch = Math.Atan2(deltaInLocal.GetDim(0), deltaInLocal.GetDim(2));
//var roll = Math.Atan2(deltaInLocal.GetDim(0), deltaInLocal.GetDim(1));
//var pitchBackup = pitch;
//var rollBackup = roll;
//pitch = ToRPM(pitch, lastpitch);
//roll = ToRPM(roll, lastRoll);
//lastpitch = pitchBackup;
//lastRoll = rollBackup;
////foreach (var item in gyros)
//{
// var gyro = item as IMyGyro;
// gyro.Pitch = (float)-pitch/5;
// gyro.Roll = (float)roll/5;
//}
}
public void GyroControl()
{
var vector = cockpit.GetNaturalGravity();
var cockpitmatrix = cockpit.WorldMatrix;
Vector3D direction = Vector3D.TransformNormal(vector, Matrix.Transpose(cockpitmatrix));
float pitch = (float)direction.Z / 5;
float roll = -((float)direction.X / 5);
foreach (var item in gyros)
{
var gyro = item as IMyGyro;
gyro.Pitch = pitch;
gyro.Roll = roll;
}
}
public static void KeepHorizon(string arg, ref bool keepHorizon, IMyCockpit cockpit, List <IMyGyro> gyros)
{
switch (arg)
{
case KeepHorizonOffArg:
keepHorizon = false;
break;
case KeepHorizonOnArg:
keepHorizon = true;
break;
}
foreach (var gyro in gyros)
{
gyro.GyroOverride = keepHorizon;
}
if (keepHorizon)
{
Vector3D grav = Vector3D.Normalize(cockpit.GetNaturalGravity());
Vector3D axis = grav.Cross(cockpit.WorldMatrix.Down);
var signal = cockpit.WorldMatrix.Up * cockpit.RollIndicator;
if (grav.Dot(cockpit.WorldMatrix.Down) < 0)
{
axis = Vector3D.Normalize(axis);
}
axis += signal;
foreach (var gyro in gyros)
{
gyro.Yaw = (float)axis.Dot(gyro.WorldMatrix.Up);
gyro.Pitch = (float)axis.Dot(gyro.WorldMatrix.Right);
gyro.Roll = (float)axis.Dot(gyro.WorldMatrix.Backward);
}
}
}
void UpdateReletiveTargetData(object Sender, UpdateEventArgs e) // Only works in atmosphere
{
Vector3D PlanetPos;
Cockpit.TryGetPlanetPosition(out PlanetPos);
Vector3D MePos = Me.CubeGrid.GetPosition();
Vector3D TargetPos = LastTargetData.HitPosition ?? MePos;
Vector3D MeVelocity = Cockpit.GetShipVelocities().LinearVelocity;
Vector3D MeGravity = Cockpit.GetNaturalGravity();
double Distance = Vector3D.Distance(TargetPos, MePos);
double MeHeight = Vector3D.Distance(MePos, PlanetPos);
double TargetHeight = Vector3D.Distance(TargetPos, PlanetPos);
double RelativeTargetHeight = TargetHeight - MeHeight; // Should be negative when aircraft is above
Vector3D RelativePlanetPos = PlanetPos - MePos;
Vector3D MeRelativeGroundPos = RelativePlanetPos * (-RelativeTargetHeight / MeHeight);
Vector3D MeGroundPos = MePos + MeRelativeGroundPos;
Vector3D GroundPosToTarget = TargetPos - MeGroundPos;
double S, U, V, A, T; // Equations of motion in SUVAT notation.
T = ProjectileGroundInterceptTime(RelativeTargetHeight, Cockpit.GetShipVelocities().LinearVelocity, Cockpit.GetNaturalGravity());
S = ProjectileLateralTravelDistance(T, GroundPosToTarget, Cockpit.GetShipVelocities().LinearVelocity);
double GroundDistanceToDrop = Vector3D.Distance(MeGroundPos, TargetPos) - S;
double TimeUntilDrop = GroundDistanceToDrop / Cockpit.GetShipVelocities().LinearVelocity.Dot(Vector3D.Normalize(GroundPosToTarget));
Vector3D RelativePlanetPosFromTarget = PlanetPos - MePos;
Vector3D ToRight = Vector3D.Normalize(GroundPosToTarget.Cross(MeGravity));
double ToRightCorrection = Vector3D.Normalize(MeVelocity).Dot(ToRight);
double ToRightCorrectionAngle = Math.Asin(ToRightCorrection) / Math.PI * 180;
string ToRightText = ToRightCorrectionAngle >= 0 ? "Right" : "Left";
ConsoleBuffers["ToRightCorrectionAngle"] = $"To {ToRightText}: {Math.Abs(ToRightCorrectionAngle):F1}°";
ConsoleBuffers["TimeUntilDrop"] = $"Time until Drop: {TimeUntilDrop:F1}s";
ConsoleBuffers["GroundDistanceUntillDrop"] = $"Distance until Drop: {GroundDistanceToDrop:F1}m";
}
public void GyroControl()
{
//var vector = cockpit.GetNaturalGravity();
//var cockpitmatrix = cockpit.WorldMatrix;
//Vector3D direction = Vector3D.TransformNormal(vector, Matrix.Transpose(cockpitmatrix));
//float pitch = (float)direction.Z / 2;
//float roll = -((float)direction.X / 2);
//foreach (var item in gyros)
//{
// var gyro = item as IMyGyro;
// gyro.Pitch = pitch > 0 ? (float)Math.Pow(pitch, 2) : -(float)Math.Pow(pitch, 2);
// gyro.Roll = roll > 0 ? (float)Math.Pow(roll, 2) : -(float)Math.Pow(roll, 2);
//}
var vector = cockpit.GetNaturalGravity();
var cockpitmatrix = cockpit.WorldMatrix;
Vector3D direction = Vector3D.TransformNormal(vector, Matrix.Transpose(cockpitmatrix));
float pitch = (float)direction.Z / 2;
float roll = -((float)direction.X / 2);
foreach (var item in gyros)
{
var gyro = item as IMyGyro;
gyro.Pitch = (float)pitch;
//gyro.Roll = (float)roll;
//gyro.Pitch = pitch > 0 ? (float)Math.Pow(pitch, 2) : -(float)Math.Pow(pitch, 2);
//gyro.Roll = roll > 0 ? (float)Math.Pow(roll, 2) : -(float)Math.Pow(roll, 2);
}
}
private bool MoveRespawnShip(long playerId)
{
// make sure we actually are in control of something
IMyPlayer player = GetPlayer(playerId);
if (player == null)
{
return(false);
}
if (player.Controller == null)
{
return(false);
}
if (player.Controller.ControlledEntity == null)
{
return(false);
}
if (player.Controller.ControlledEntity.Entity == null)
{
return(false);
}
if (player.Controller.ControlledEntity.Entity.Parent == null)
{
return(false);
}
// make sure we're in control of a spawn ship
IMyEntity ship = player.Controller.ControlledEntity.Entity.Parent;
if (!player.RespawnShip.Contains(ship.EntityId))
{
return(true);
}
// make sure the spawn ship has a cockpit
if (!(ship is IMyCubeGrid))
{
return(false);
}
List <IMySlimBlock> blocks = new List <IMySlimBlock>();
((IMyCubeGrid)ship).GetBlocks(blocks, b => b.FatBlock != null && b.FatBlock is IMyCockpit);
if (blocks.Count == 0)
{
// no cockpit found
return(true);
}
// make sure the cockpit isn't in gravity
IMyCockpit cockpit = (IMyCockpit)(blocks.FirstOrDefault().FatBlock);
if (cockpit.GetNaturalGravity().Length() > 0)
{
// cockpit is in gravity
return(true);
}
ship.SetPosition(GetRespawnPoint());
return(true);
}
public void Main(string argument, UpdateType updateSource)
{
subsystemManager.UpdateTime();
if (argument == "descend")
{
Mode = 1;
lockMatrix = Drive.Controller.WorldMatrix;
}
else if (argument == "ascend")
{
Mode = 2;
lockMatrix = Drive.Controller.WorldMatrix;
}
else if (argument == "stop")
{
Mode = 0;
lockMatrix = MatrixD.Zero;
Drive.Clear();
}
else if (commandLine.TryParse(argument))
{
subsystemManager.Command(commandLine.Argument(0), commandLine.Argument(1), commandLine.ArgumentCount > 2 ? commandLine.Argument(2) : null);
}
else
{
try
{
subsystemManager.Update(updateSource);
runs++;
if (runs % 5 == 0)
{
OutputBuilder.Clear();
OutputBuilder.Append($"STATUS: ").AppendLine(Mode == 0 ? "STOP" : (Mode == 1 ? "DESC" : "ASCE"));
foreach (var screen in Displays)
{
screen.ContentType = ContentType.TEXT_AND_IMAGE;
screen.WriteText(OutputBuilder.ToString());
}
var destination = new Waypoint();
if (Mode == 0)
{
Drive.Clear();
}
else if (Mode == 1)
{
destination.MaxSpeed = DescendSpeed;
var gravdir = Cockpit.GetNaturalGravity();
if (gravdir == Vector3D.Zero)
{
gravdir.Normalize();
destination.Position = gravdir * 10 + Cockpit.GetPosition();
var flatForward = Cockpit.WorldMatrix.Forward - VectorHelpers.VectorProjection(Cockpit.WorldMatrix.Forward, gravdir);
flatForward.Normalize();
var flatLeft = Cockpit.WorldMatrix.Left - VectorHelpers.VectorProjection(Cockpit.WorldMatrix.Forward, gravdir);
flatLeft.Normalize();
destination.Direction = flatForward * TrigHelpers.FastCos(0.02 * RotateSpeed) + flatLeft * TrigHelpers.FastSin(0.02 * RotateSpeed);
}
else
{
destination.Position = lockMatrix.Down * 10 + Cockpit.GetPosition();
destination.DirectionUp = lockMatrix.Up;
var flatForward = Cockpit.WorldMatrix.Forward - VectorHelpers.VectorProjection(Cockpit.WorldMatrix.Forward, lockMatrix.Down);
flatForward.Normalize();
var flatLeft = Cockpit.WorldMatrix.Left - VectorHelpers.VectorProjection(Cockpit.WorldMatrix.Forward, lockMatrix.Down);
flatLeft.Normalize();
destination.Direction = flatForward * TrigHelpers.FastCos(0.02 * RotateSpeed) + flatLeft * TrigHelpers.FastSin(0.02 * RotateSpeed);
}
}
else if (Mode == 2)
{
destination.MaxSpeed = AscendSpeed;
if (Drive.Controller == Cockpit)
{
var gravdir = Cockpit.GetNaturalGravity();
gravdir.Normalize();
destination.Position = -gravdir * 10 + Cockpit.GetPosition();
}
else
{
destination.Position = lockMatrix.Down * 10 + Cockpit.GetPosition();
destination.Direction = lockMatrix.Forward;
}
}
Drive.Move(destination.Position);
Drive.Turn(destination.Direction);
Drive.Spin(destination.DirectionUp);
Drive.SetMaxSpeed(destination.MaxSpeed);
}
}
catch (Exception e)
{
Me.GetSurface(0).WriteText(e.Message);
Me.GetSurface(0).WriteText("\n", true);
Me.GetSurface(0).WriteText(e.StackTrace);
Me.GetSurface(0).WriteText("\n", true);
Me.GetSurface(0).WriteText(e.ToString());
}
var s = subsystemManager.GetStatus();
if (!string.IsNullOrEmpty(s))
{
Echo(s);
}
else
{
Echo(((int)subsystemManager.OutputMode).ToString());
}
}
}
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;
}
}
}
}
public void GyroControl()
{
var vector = cockpit.GetNaturalGravity();
//var cockpitmatrix = Matrix.CreateFromDir(cockpit.WorldMatrix.Down);
var cockpitmatrix = MatrixD.CreateWorld(cockpit.GetPosition(), cockpit.WorldMatrix.Forward, cockpit.WorldMatrix.Down);
//cockpitmatrix.Forward = cockpitmatrix.Down;
Vector3D direction = Vector3D.TransformNormal(vector, Matrix.Transpose(cockpitmatrix));
var diffangle = MathHelperD.ToDegrees(Math.Atan2(direction.Z, direction.Y));
if (diffangle < Math.Abs(oldAngle - diffangle))
{
}
float pitch = 0f;
WriteText("Gyro diffangle: " + diffangle.ToString() + " -\n");
WriteText("Gyro pitch1: " + ((pitch1)).ToString() + " -\n");
WriteText("Gyro pitch60: " + ((pitch60)).ToString() + " -\n");
WriteText("Gyro pitch: " + ((float)(pitch1 * diffangle)).ToString() + " -\n");
if (diffangle > 5)
{
if (diffangle > 20)
{
pitch = (float)(0.8 * Math.PI);
}
else
{
pitch = (float)(pitch1 * 10);
}
foreach (var item in gyros)
{
var gyro = item as IMyGyro;
gyro.CustomData = gyro.Pitch.ToString() + " ::::" + ((float)2 * Math.PI).ToString() + " cheack " + gyro.Pitch.Equals((float)(2 * Math.PI));
if (!gyro.Pitch.Equals(pitch))
{
gyro.Pitch = pitch;
}
}
}
else if (diffangle < -5)
{
if (diffangle < -20)
{
pitch = (float)(-0.8 * Math.PI);
}
else
{
pitch = (float)(pitch1 * -10);
}
foreach (var item in gyros)
{
var gyro = item as IMyGyro;
if (!gyro.Pitch.Equals(pitch))
{
gyro.Pitch = pitch;
}
}
}
else
{
foreach (var item in gyros)
{
var gyro = item as IMyGyro;
gyro.Pitch = (float)(pitch1 * diffangle);
}
}
oldAngle = diffangle;
//diffangle = MathHelperD.ToDegrees(Math.Atan2(direction.X, direction.Y));
//float roll = 0f;
//if (diffangle > 5)
//{
// if (diffangle > 5)
// {
// roll = (float)(1 * Math.PI);
// }
// foreach (var item in gyrorolls)
// {
// var gyro = item as IMyGyro;
// gyro.CustomData = gyro.Roll.ToString() + " ::::" + ((float)2 * Math.PI).ToString() + " cheack " + gyro.Roll.Equals((float)(2 * Math.PI));
// if (!gyro.Roll.Equals(roll))
// {
// gyro.Roll = roll;
// }
// }
//}
//else if (diffangle < -5)
//{
// if (diffangle < -5)
// {
// roll = (float)(-1 * Math.PI);
// }
// foreach (var item in gyrorolls)
// {
// var gyro = item as IMyGyro;
// if (!gyro.Roll.Equals(roll))
// {
// gyro.Roll = roll;
// }
// }
//}
//else
//{
// foreach (var item in gyrorolls)
// {
// var gyro = item as IMyGyro;
// if (!gyro.Roll.Equals(0))
// {
// gyro.Roll = 0;
// }
// }
//}
}