//Basic Gets Predicted Position Of Enemy (Derived From Keen Code)
public static Vector3D GetPredictedTargetPosition2(IMyTerminalBlock shooter, Vector3 ShipVel, MyDetectedEntityInfo target, float shotSpeed)
{
Vector3D predictedPosition = target.Position;
Vector3D dirToTarget = Vector3D.Normalize(predictedPosition - shooter.GetPosition());
//Run Setup Calculations
Vector3 targetVelocity = target.Velocity;
targetVelocity -= ShipVel;
Vector3 targetVelOrth = Vector3.Dot(targetVelocity, dirToTarget) * dirToTarget;
Vector3 targetVelTang = targetVelocity - targetVelOrth;
Vector3 shotVelTang = targetVelTang;
float shotVelSpeed = shotVelTang.Length();
if (shotVelSpeed > shotSpeed)
{
// Shot is too slow
return(Vector3.Normalize(target.Velocity) * shotSpeed);
}
else
{
// Run Calculations
float shotSpeedOrth = (float)Math.Sqrt(shotSpeed * shotSpeed - shotVelSpeed * shotVelSpeed);
Vector3 shotVelOrth = dirToTarget * shotSpeedOrth;
float timeDiff = shotVelOrth.Length() - targetVelOrth.Length();
var timeToCollision = timeDiff != 0 ? ((shooter.GetPosition() - target.Position).Length()) / timeDiff : 0;
Vector3 shotVel = shotVelOrth + shotVelTang;
predictedPosition = timeToCollision > 0.01f ? shooter.GetPosition() + (Vector3D)shotVel * timeToCollision : predictedPosition;
return(predictedPosition);
}
}
public Vector3 ToLocalSpherical(Vector3D target)
{
Vector3D targetVector = target - core.GetPosition();
double front = targetVector.Dot(core.WorldMatrix.Forward) + offset.X;
double right = targetVector.Dot(core.WorldMatrix.Right) + offset.Y;
double up = targetVector.Dot(core.WorldMatrix.Up) + offset.Z;
double deltaAzimuth = Math.Atan(right / front);
double planelength = front / Math.Cos(deltaAzimuth);
double elevation = Math.Atan(up / planelength);
double length = planelength / Math.Cos(elevation);
if (front < 0)
{
deltaAzimuth += Math.PI;
elevation = -elevation;
}
if (deltaAzimuth > Math.PI)
{
deltaAzimuth -= 2 * Math.PI;
}
return(new Vector3(length, deltaAzimuth, elevation));
}
private double AngleTowards(IMyMotorStator stator, IMyTerminalBlock pointer, Vector3D direction)
{
Vector3D restDir = Vector3D.Normalize(pointer.GetPosition() - stator.GetPosition());
return(Math.Atan2(stator.WorldMatrix.Right.Dot(direction), stator.WorldMatrix.Forward.Dot(direction)) -
Math.Atan2(stator.Top.WorldMatrix.Right.Dot(restDir), stator.Top.WorldMatrix.Forward.Dot(restDir)));
}
/// <summary>
/// This code is taken from Whip's AI Rotor Turret Control Script. All credit to Whiplash.
/// https://steamcommunity.com/sharedfiles/filedetails/?id=672678005
/// </summary>
/// <param name="targetPosition">point to face</param>
private void TurnToFacePosition(Vector3D targetPosition)
{
IMyTerminalBlock turretReference = remoteControl;
Vector3D turretFrontVec = turretReference.WorldMatrix.Forward;
Vector3D absUpVec = rotor.WorldMatrix.Up;
Vector3D turretSideVec = turretReference.WorldMatrix.Right;
Vector3D turretFrontCrossSide = turretFrontVec.Cross(turretSideVec);
Vector3D turretLeftVec;
if (DotIsSameDirection(absUpVec, turretFrontCrossSide))
{
turretLeftVec = turretSideVec;
}
else
{
turretLeftVec = -1 * turretSideVec;
}
Vector3D referenceToTargetVec = targetPosition - turretReference.GetPosition();
//get projections onto axis made out of our plane orientation
Vector3D projOnFront = VectorProjection(referenceToTargetVec, turretFrontVec);
Vector3D projOnLeft = VectorProjection(referenceToTargetVec, turretLeftVec);
Vector3D projOnFrontLeftPlane = projOnFront + projOnLeft;
double azimuthAngle = Math.Asin(MathHelper.Clamp(projOnLeft.Length() * DotGetSign(projOnLeft, turretLeftVec) / projOnFrontLeftPlane.Length(), -1, 1));
double azimuthSpeed = 40 * azimuthAngle; //derivitave term is useless as rotors dampen by default
rotor.TargetVelocityRPM = -(float)azimuthSpeed; //negative because we want to cancel the positive angle via our movements
// rotor.SetValue("Velocity", -(float) azimuthSpeed); //negative because we want to cancel the positive angle via our movements
}
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);
}
void RepairDamaged(IMyCubeBlock block, IMyTerminalBlock nearestWelder)
{
if (activeRepairs.FirstOrDefault(x => x.Welder == nearestWelder && x.DamagedBlock == block) == null) //&& DistanceSquared(nearestWelder, block)<maximumWelderDamagedDistance
{
this.Echo(string.Format($"Welder: {nearestWelder.DisplayNameText}, {nearestWelder.EntityId}"));
if (DistanceSquared(nearestWelder.GetPosition(), block.GetPosition()) < maximumWelderDamagedDistance)
{
try
{
nearestWelder.ApplyAction("OnOff_On");
}
catch (Exception ex)
{
Echo(ex.Message);
}
activeRepairs.Add(new MyRepairInfo(nearestWelder, block, DistanceSquared(nearestWelder, block)));
}
else
{
Echo("Distance to big " + nearestWelder.DisplayNameText + ":" + DistanceSquared(nearestWelder, block).ToString());
}
}
}
public void Main(string argument, UpdateType updateSource)
{
var direction = (Vector3D)C**k.MoveIndicator;
//roboticArm.KeepMoving(new Vector3D(53539.59, -26784.67, 11963.55), 2);
roboticArm.KeepMoving(Tip.GetPosition() + direction, 6);
}
public bool BeamRider(Vector3D vStart, Vector3D vEnd, IMyTerminalBlock OrientationBlock)
{
// 'BeamRider' routine that takes start,end and tries to stay on that beam.
bool bAimed = false;
Vector3D vBoreEnd = (vEnd - vStart);
Vector3D vPosition;
if (OrientationBlock is IMyShipController)
{
vPosition = ((IMyShipController)OrientationBlock).CenterOfMass;
}
else
{
vPosition = OrientationBlock.GetPosition();
}
Vector3D vAimEnd = (vEnd - vPosition);
Vector3D vRejectEnd = VectorRejection(vBoreEnd, vAimEnd);
Vector3D vCorrectedAim = (vEnd - vRejectEnd * 2) - vPosition;
Matrix or1;
OrientationBlock.Orientation.GetMatrix(out or1);
bAimed = AlignGyros(or1.Forward, vCorrectedAim);
// bAimed = GyroMain("forward", vCorrectedAim, OrientationBlock);
bAimed = AlignGyros("forward", vCorrectedAim, OrientationBlock);
return(bAimed);
}
double CalculateRoll(Vector3D destination, IMyTerminalBlock Origin)
{
double rollAngle = 0;
bool facingTarget = false;
Vector3D vCenter;
Vector3D vBack;
Vector3D vUp;
Vector3D vRight;
MatrixD refOrientation = GetBlock2WorldTransform(Origin);
vCenter = Origin.GetPosition();
vBack = vCenter + 1.0 * Vector3D.Normalize(refOrientation.Backward);
vUp = vCenter + 1.0 * Vector3D.Normalize(refOrientation.Up);
vRight = vCenter + 1.0 * Vector3D.Normalize(refOrientation.Right);
double centerTargetDistance = calculateDistance(vCenter, destination);
double upTargetDistance = calculateDistance(vUp, destination);
double rightLocalDistance = calculateDistance(vRight, vCenter);
double rightTargetDistance = calculateDistance(vRight, destination);
facingTarget = centerTargetDistance > upTargetDistance;
rollAngle = (centerTargetDistance - rightTargetDistance) / rightLocalDistance;
//Echo("calc Angle=" + Math.Round(rollAngle,5));
if (!facingTarget)
{
Echo("ROLL:NOT FACING!");
rollAngle += (rollAngle < 0) ? -1 : 1;
}
return(rollAngle);
}
public void UpdateVectors()
{
GravVector = CockpitBlock.GetNaturalGravity();
GravVectorNorm = Vector3D.Normalize(GravVector);
ShipVector = ReferenceBlock.GetPosition();
ShipVectorNorm = Vector3D.Normalize(ShipVector);
}
/// <summary>
/// This code is taken from Whip's AI Rotor Turret Control Script. All credit to Whiplash.
/// https://steamcommunity.com/sharedfiles/filedetails/?id=672678005&
/// </summary>
/// <param name="targetPosition">point to face</param>
private void TurnToFacePosition(Vector3D targetPosition)
{
//get orientation of reference
IMyTerminalBlock turretReference = weapons[0];
Vector3D turretFrontVec = turretReference.WorldMatrix.Forward;
Vector3D absUpVec = azimuthRotor.WorldMatrix.Up;
Vector3D turretSideVec = elevationRotors[0].WorldMatrix.Up;
Vector3D turretFrontCrossSide = turretFrontVec.Cross(turretSideVec);
//check elevation rotor orientation w.r.t. reference
Vector3D turretUpVec;
Vector3D turretLeftVec;
if (DotIsSameDirection(absUpVec, turretFrontCrossSide))
{
turretUpVec = turretFrontCrossSide;
turretLeftVec = turretSideVec;
}
else
{
turretUpVec = -1 * turretFrontCrossSide;
turretLeftVec = -1 * turretSideVec;
}
//get vector to target point
Vector3D referenceToTargetVec = targetPosition - turretReference.GetPosition();
//get projections onto axis made out of our plane orientation
Vector3D projOnFront = VectorProjection(referenceToTargetVec, turretFrontVec);
Vector3D projOnLeft = VectorProjection(referenceToTargetVec, turretLeftVec);
Vector3D projOnUp = VectorProjection(referenceToTargetVec, turretUpVec);
Vector3D projOnFrontLeftPlane = projOnFront + projOnLeft;
double azimuthAngle = Math.Asin(MathHelper.Clamp(projOnLeft.Length() * DotGetSign(projOnLeft, turretLeftVec) / projOnFrontLeftPlane.Length(), -1, 1));
double elevationAngle = Math.Atan(projOnUp.Length() * DotGetSign(projOnUp, turretUpVec) / projOnFrontLeftPlane.Length()); //w.H*i/P=L-a!s,H-1,4.1 m.a/d.e t/h.i/s
if (DotIsSameDirection(absUpVec, turretFrontCrossSide))
{
elevationAngle *= -1;
}
double azimuthSpeed = 20 * azimuthAngle; //derivitave term is useless as rotors dampen by default
double elevationSpeed = 20 * elevationAngle;
azimuthRotor.SetValue("Velocity", -(float)azimuthSpeed); //negative because we want to cancel the positive angle via our movements
foreach (var elevationRotor in elevationRotors)
{
if (elevationRotor.WorldMatrix.Up == turretSideVec) // This is god-awful but maybe can be improved one day =/
{
elevationRotor.SetValue("Velocity", -(float)elevationSpeed);
}
else
{
elevationRotor.SetValue("Velocity", (float)elevationSpeed);
}
}
}
private void DegreesToVector(Vector3D TV)
{
IMyTerminalBlock guideblock = docking ? (IMyTerminalBlock)_dockngConnector : (IMyTerminalBlock)_remoteControl;
if (guideblock != null)
{
var Origin = guideblock.GetPosition();
var Up = guideblock.WorldMatrix.Up;
var Forward = guideblock.WorldMatrix.Forward;
var Right = guideblock.WorldMatrix.Right;
// ExplainVector(Origin, "Origin");
// ExplainVector(Up, "up");
Vector3D OV = Origin; //Get positions of reference blocks.
Vector3D FV = Origin + Forward;
Vector3D UV = Origin + Up;
Vector3D RV = Origin + Right;
//Get magnitudes of vectors.
double TVOV = (OV - TV).Length();
double TVFV = (FV - TV).Length();
double TVUV = (UV - TV).Length();
double TVRV = (RV - TV).Length();
double OVUV = (UV - OV).Length();
double OVRV = (RV - OV).Length();
double ThetaP = Math.Acos((TVUV * TVUV - OVUV * OVUV - TVOV * TVOV) / (-2 * OVUV * TVOV));
//Use law of cosines to determine angles.
double ThetaY = Math.Acos((TVRV * TVRV - OVRV * OVRV - TVOV * TVOV) / (-2 * OVRV * TVOV));
double RPitch = 90 - (ThetaP * 180 / Math.PI); //Convert from radians to degrees.
double RYaw = 90 - (ThetaY * 180 / Math.PI);
if (TVOV < TVFV)
{
RPitch = 180 - RPitch; //Normalize angles to -180 to 180 degrees.
}
if (RPitch > 180)
{
RPitch = -1 * (360 - RPitch);
}
if (TVOV < TVFV)
{
RYaw = 180 - RYaw;
}
if (RYaw > 180)
{
RYaw = -1 * (360 - RYaw);
}
_degreesToVectorYaw = RYaw;
_degreesToVectorPitch = RPitch;
}
}
void processTimerCommand()
{
string output = "";
currentPosition = anchorPosition.GetPosition();
output += velocityShip.ToString(velocityFormat) + " m/s";
output += " (" + (velocityShip * 3.6).ToString(velocityFormat) + "km/h)";
Log(output);
}
private bool AttemptReScan(HaE_Entity entity)
{
Vector3D Direction = entity.entityInfo.Position - reference.GetPosition();
double distance = Direction.Normalize();
for (int i = 0; i < MaxReScanAmount; i++)
{
Vector3D scanDir = TrackingUtils.GenerateShotgunVector(random, Direction, ReScanConeAngle);
HaE_Entity detected = RaycastDirection(Direction, distance * ReScanMultiplier);
if (detected != null)
{
OnEntityDetected?.Invoke(detected);
return(true);
}
}
return(false);
}
double CalculateYaw(Vector3D destination, IMyTerminalBlock Origin)
{
double yawAngle = 0;
bool facingTarget = false;
MatrixD refOrientation = GetBlock2WorldTransform(Origin);
Vector3D vCenter = Origin.GetPosition();
Vector3D vBack = vCenter + 1.0 * Vector3D.Normalize(refOrientation.Backward);
// Vector3D vUp = vCenter + 1.0 * Vector3D.Normalize(refOrientation.Up);
Vector3D vRight = vCenter + 1.0 * Vector3D.Normalize(refOrientation.Right);
Vector3D vLeft = vCenter - 1.0 * Vector3D.Normalize(refOrientation.Right);
// debugGPSOutput("vCenter", vCenter);
// debugGPSOutput("vBack", vBack);
// debugGPSOutput("vUp", vUp);
// debugGPSOutput("vRight", vRight);
// double centerTargetDistance = calculateDistance(vCenter, destination);
// double upTargetDistance = calculateDistance(vUp, destination);
// double backTargetDistance = calculateDistance(vBack, destination);
// double rightLocalDistance = calculateDistance(vRight, vCenter);
double rightTargetDistance = calculateDistance(vRight, destination);
double leftTargetDistance = calculateDistance(vLeft, destination);
double yawLocalDistance = calculateDistance(vRight, vLeft);
double centerTargetDistance = Vector3D.DistanceSquared(vCenter, destination);
double backTargetDistance = Vector3D.DistanceSquared(vBack, destination);
/*
* double upTargetDistance = Vector3D.DistanceSquared(vUp, destination);
* double rightLocalDistance = Vector3D.DistanceSquared(vRight, vCenter);
* double rightTargetDistance = Vector3D.DistanceSquared(vRight, destination);
*
* double leftTargetDistance = Vector3D.DistanceSquared(vLeft, destination);
*
* double yawLocalDistance = Vector3D.DistanceSquared(vRight, vLeft);
*/
facingTarget = centerTargetDistance < backTargetDistance;
yawAngle = (leftTargetDistance - rightTargetDistance) / yawLocalDistance;
Echo("calc Angle=" + Math.Round(yawAngle, 5));
if (!facingTarget)
{
//Echo("YAW:NOT FACING!");
yawAngle += (yawAngle < 0) ? -1 : 1;
}
// Echo("yawangle=" + Math.Round(yawAngle,5));
return(yawAngle);
}
bool ValidBlock(IMyTerminalBlock tb)
{
if (tb.GetPosition() == new Vector3D())
{
return(false);
}
else
{
return(true);
}
}
bool ValidBlock(IMyTerminalBlock tb)
{
if (tb.GetPosition() == _emptyV3D)
{
return(false);
}
else
{
return(true);
}
}
//grid map
public Program()
{
//load program state
if (alreadyRun & _ini.TryParse(Storage))
{
Vector3D.TryParse(_ini.Get("Basis", "A").ToString(), out _basisVecA);
Vector3D.TryParse(_ini.Get("Basis", "B").ToString(), out _basisVecB);
Vector3D.TryParse(_ini.Get("Basis", "Up").ToString(), out _basisVecUp);
Vector3D.TryParse(_ini.Get("Basis", "Origin").ToString(), out _origin);
}
//load config
else
{
MyIniParseResult result;
if (!_ini.TryParse(Me.CustomData, out result))
{
throw new Exception(result.ToString());
}
string nameA = _ini.Get("Basis", "A").ToString("A");
IMyTerminalBlock blockA = GridTerminalSystem.GetBlockWithName(nameA);
Vector3D pointA = blockA.GetPosition();
string nameB = _ini.Get("Basis", "B").ToString("B");
IMyTerminalBlock blockB = GridTerminalSystem.GetBlockWithName(nameB);
Vector3D pointB = blockB.GetPosition();
string nameUp = _ini.Get("Basis", "Up").ToString("Up");
IMyTerminalBlock blockUp = GridTerminalSystem.GetBlockWithName(nameUp);
Vector3D pointUp = blockUp.GetPosition();
string nameOrigin = _ini.Get("Basis", "Origin").ToString("Origin");
IMyTerminalBlock blockOrigin = GridTerminalSystem.GetBlockWithName(nameOrigin);
_origin = blockOrigin.GetPosition();
_basisVecA = Vector3D.Normalize(pointA - _origin);
_basisVecB = Vector3D.Normalize(pointB - _origin);
_basisVecUp = Vector3D.Normalize(pointUp - _origin);
_basisVecA = Me.CubeGrid.WorldMatrix.Forward;
_basisVecB = Me.CubeGrid.WorldMatrix.Right;
_basisVecUp = Me.CubeGrid.WorldMatrix.Up;
alreadyRun = true;
}
//IGC
_turtleListenerInit = IGC.RegisterBroadcastListener(_turtleInit);
_turtleListenerInit.SetMessageCallback(_turtleInit);
Runtime.UpdateFrequency |= UpdateFrequency.Update100;
}
/// <summary>
/// Determines whether or not the target block is within a 180 degree arc in front of the camera.
/// </summary>
public static bool IsLookingInBlockDir(IMyTerminalBlock block)
{
if (block != null)
{
Vector3D dir = (block.GetPosition() - MyAPIGateway.Session.Camera.Position),
forward = MyAPIGateway.Session.Camera.WorldMatrix.Forward;
return(Vector3D.Dot(dir, forward) > 0);
}
else
{
return(false);
}
}
int GetBlockOcclusion(IMyTerminalBlock Block)
{
int Occlusion = 0;
List <Vector3I> BlockPositions = new List <Vector3I>();
Block.CubeGrid.RayCastCells(Radar.Position, Block.GetPosition(), BlockPositions, havokWorld: true);
foreach (var Pos in BlockPositions)
{
if (Block.CubeGrid.CubeExists(Pos))
{
Occlusion += 1;
}
}
return(Occlusion);
}
//Use For Turning
#region GyroTurnMis #RFC#
/*=======================================================================================
* Function: GyroTurn6
* ---------------------------------------
* function will: A Variant of PD damped gyroturn used for missiles
* //----------==--------=------------=-----------=---------------=------------=-----=-----*/
void GyroTurn6(Vector3D TARGETVECTOR, double GAIN, double DAMPINGGAIN, IMyTerminalBlock REF, IMyGyro GYRO, double YawPrev, double PitchPrev, out double NewPitch, out double NewYaw)
{
//Pre Setting Factors
NewYaw = 0;
NewPitch = 0;
//Retrieving Forwards And Up
Vector3D ShipUp = REF.WorldMatrix.Up;
Vector3D ShipForward = REF.WorldMatrix.Backward; //Backward for thrusters
//Create And Use Inverse Quatinion
Quaternion Quat_Two = Quaternion.CreateFromForwardUp(ShipForward, ShipUp);
var InvQuat = Quaternion.Inverse(Quat_Two);
Vector3D DirectionVector = TARGETVECTOR; //RealWorld Target Vector
Vector3D RCReferenceFrameVector = Vector3D.Transform(DirectionVector, InvQuat); //Target Vector In Terms Of RC Block
//Convert To Local Azimuth And Elevation
double ShipForwardAzimuth = 0; double ShipForwardElevation = 0;
Vector3D.GetAzimuthAndElevation(RCReferenceFrameVector, out ShipForwardAzimuth, out ShipForwardElevation);
//Post Setting Factors
NewYaw = ShipForwardAzimuth;
NewPitch = ShipForwardElevation;
//Applies Some PID Damping
ShipForwardAzimuth = ShipForwardAzimuth + DAMPINGGAIN * ((ShipForwardAzimuth - YawPrev) / Global_Timestep);
ShipForwardElevation = ShipForwardElevation + DAMPINGGAIN * ((ShipForwardElevation - PitchPrev) / Global_Timestep);
//Does Some Rotations To Provide For any Gyro-Orientation
var REF_Matrix = MatrixD.CreateWorld(REF.GetPosition(), (Vector3)ShipForward, (Vector3)ShipUp).GetOrientation();
var Vector = Vector3.Transform((new Vector3D(ShipForwardElevation, ShipForwardAzimuth, 0)), REF_Matrix); //Converts To World
var TRANS_VECT = Vector3.Transform(Vector, Matrix.Transpose(GYRO.WorldMatrix.GetOrientation())); //Converts To Gyro Local
//Logic Checks for NaN's
if (double.IsNaN(TRANS_VECT.X) || double.IsNaN(TRANS_VECT.Y) || double.IsNaN(TRANS_VECT.Z))
{
return;
}
//Applies To Scenario
GYRO.Pitch = (float)MathHelper.Clamp((-TRANS_VECT.X) * GAIN, -1000, 1000);
GYRO.Yaw = (float)MathHelper.Clamp(((-TRANS_VECT.Y)) * GAIN, -1000, 1000);
GYRO.Roll = (float)MathHelper.Clamp(((-TRANS_VECT.Z)) * GAIN, -1000, 1000);
GYRO.GyroOverride = true;
}
void addDockableConnector(IMyTerminalBlock connector)
{
if (connector == null)
{
return;
}
Vector3D vPosition = connector.GetPosition();
// Vector3D vVec = calcBlockForwardVector(connector);
MatrixD worldtb = connector.WorldMatrix;
Vector3D vVec = worldtb.Forward;
vVec.Normalize();
addDockableConnector(connector.EntityId, connector.CustomName, vPosition, vVec);
}
public void step()
{
if (!refReady)
{
mover.stop();
return;
}
aligner.set_reference(reference, dir);
aligner.enabled = true;
if (!tposReady)
{
rc.getGravityVector(out aligner.tVector);
mover.stop();
return;
}
distValid = true;
Vector3D togo = tpos - reference.GetPosition();
dist = togo;
// if (old) {
double len = togo.Length();
// // if (len < breakingDistance*breakingDistance) {
// // togo.Normalize();
// // togo *=
// // }
if (len > 100)
{
togo /= (len / 100);
}
// togo = Vector3D.Normalize (togo) * 10;
// } else if (len < 7) {
// // togo *= 2;
// } else {
// // togo = Vector3D.Normalize (togo) * 20;
// // togo.Normalize();
// togo *= len / breakingDistance;
// }
// } else {
// Vector3D thrust = mover.getPotentialThrust;
// }
mover.setTargetVelocity(togo);
aligner.tVector = -mover.refVelocity;
}
public bool AlignBlockTo(Vector3D vPos, IMyTerminalBlock block = null)
{
if (block == null)
{
block = Remote;
}
Matrix m = block.WorldMatrix;
Vector3D forward = m.Forward;
Vector3D left = m.Left;
Vector3D up = m.Up;
Vector3D vTarget = (vPos - block.GetPosition());
vTarget.Normalize();
// Check angle.
var angleF = VectorHelper.AngleBetween(vTarget, m.Forward);
this.Program.Echo(string.Format("Angle: {0}", angleF));
if (angleF <= 0.01)
{
DisableGyroOverride();
return(true);
}
double yaw, pitch;
GetRotationAngles(vTarget, forward, left, up, out yaw, out pitch);
// Correct with PID.
var freq = this.Program.Runtime.TimeSinceLastRun.TotalMilliseconds / 1000;
pitch = PitchPID.CorrectError(pitch, freq) * 0.1; // * speedFactor; apply factor to reduce the speed if needed.
yaw = YawPID.CorrectError(yaw, freq) * 0.1; // * speedFactor; apply factor to reduce the speed if needed.
// Apply gyro overrides.
ApplyGyroOverride(pitch, yaw, 0, Gyros, m);
// Return not aligned for now. Will keep aligning :)
return(false);
}
Dictionary <string, string> ReadBlock(IMyTerminalBlock Block)
{
Dictionary <string, string> BlockReadout = new Dictionary <string, string>();
if (Block == null)
{
return(BlockReadout);
}
BlockReadout.Add("Type", Block.BlockDefinition.TypeId.ToString().Replace("MyObjectBuilder_", ""));
BlockReadout.Add("Subtype", Block.BlockDefinition.SubtypeName);
BlockReadout.Add("EntityID", Block.EntityId.ToString());
BlockReadout.Add("Grid", Block.CubeGrid.EntityId.ToString());
BlockReadout.Add("Enabled", (Block is IMyFunctionalBlock ? (Block as IMyFunctionalBlock).Enabled.ToString() : "null"));
BlockReadout.Add("Functional", Block.IsFunctional.ToString());
BlockReadout.Add("WorldPosition", Block.GetPosition().ToString());
BlockReadout.Add("OwnerID", Block.OwnerId.ToString());
BlockReadout.Add("Accessible", Block.HasPlayerAccess(RadarBlock.OwnerId).ToString());
BlockReadout.Add("Occlusion", GetBlockOcclusion(Block).ToString());
return(BlockReadout);
}
/*private static Vector3D GetInertia(MyCubeBlock block)
* {
* var invTensor = block.CubeGrid.Physics.RigidBody.InverseInertiaTensor;
* return new Vector3D(1 / invTensor.M11, 1 / invTensor.M22, 1 / invTensor.M33);
* }*/
private static double GetAltitude(IMyTerminalBlock block)
{
var blockPosition = block.GetPosition();
var planet = FindNearestPlanet(blockPosition);
if (planet == null)
{
return(double.NaN);
}
if (Vector3D.DistanceSquared(blockPosition, planet.WorldMatrix.Translation) > planet.GravityLimitSq)
{
return(double.NaN);
}
Vector3D closestSurfacePointLocal = planet.GetClosestSurfacePointGlobal(ref blockPosition);
return(Vector3D.Distance(blockPosition, closestSurfacePointLocal));
}
/// <summary>Main loop that handles movements and initialization</summary>
/// <returns>True if still moving</returns>
public bool Update()
{
if (this.IsInitialized())
{
if (this.waypoints.Count > 0)
{
IMyTerminalBlock block = this.getPositionBlock();
this.currentPosition = this.transformer.Pos(block.GetPosition());
this.currentOrientation = this.frontConnector?.WorldMatrix.Forward ?? FORWARD;
bool isReached = this.goToWaypoint(this.waypoints.Peek());
if (isReached)
{
Waypoint waypoint = this.waypoints.Dequeue();
this.log($"reached a waypoint, {this.waypoints.Count} to go");
this.Stop();
// We only try to connect on the last point
if (this.waypoints.Count == 0)
{
getConnector(waypoint.Connection)?.Connect();
}
this.lastConnectionType = waypoint.Connection;
}
}
bool res = this.waypoints.Count != 0;
foreach (IMyLightingBlock l in this.lights)
{
l.Enabled = res;
}
return(res);
}
else
{
this.initialize();
}
return(false);
}
Vector3D GetShipEdgeVector(IMyTerminalBlock reference, Vector3D direction)
{
//get grid relative max and min
var gridMinimum = reference.CubeGrid.Min;
var gridMaximum = reference.CubeGrid.Max;
//get dimension of grid cubes
var gridSize = reference.CubeGrid.GridSize;
//get worldmatrix for the grid
var gridMatrix = reference.CubeGrid.WorldMatrix;
//convert grid coordinates to world coords
var worldMinimum = Vector3D.Transform(gridMinimum * gridSize, gridMatrix);
var worldMaximum = Vector3D.Transform(gridMaximum * gridSize, gridMatrix);
//get reference position
var origin = reference.GetPosition();
//compute max and min relative vectors
var minRelative = worldMinimum - origin;
var maxRelative = worldMaximum - origin;
//project relative vectors on desired direction
var minProjected = Vector3D.Dot(minRelative, direction) / direction.LengthSquared() * direction;
var maxProjected = Vector3D.Dot(maxRelative, direction) / direction.LengthSquared() * direction;
//check direction of the projections to determine which is correct
if (Vector3D.Dot(minProjected, direction) > 0)
{
return(minProjected);
}
else
{
return(maxProjected);
}
}
protected bool TrackTarget(Ingame.MyDetectedEntityInfo Target, long SubtargetID)
{
if (TurretPosition.DistanceTo(Target.Position) > Turret.Range)
{
return(false);
}
IMyCubeGrid Grid = MyAPIGateway.Entities.GetEntityById(Target.EntityId) as IMyCubeGrid;
if (Grid == null)
{
return(false);
}
IMyTerminalBlock Block = null;
List <IMyTerminalBlock> Blocks = new List <IMyTerminalBlock>();
Grid.GetTerminalSystem().GetBlocks(Blocks);
Block = Blocks.FirstOrDefault(x => x.EntityId == SubtargetID);
if (Block == null || TurretPosition.DistanceTo(Block.GetPosition()) > Turret.Range)
{
return(false);
}
Turret.TrackTarget(Block);
return(true);
}
public static Vector3D GetXYZDistance(IMyTerminalBlock block, Vector3D targetPos)
{
Vector3D myPos = block.GetPosition();
Vector3D targetVector = Vector3D.Subtract(targetPos, myPos);
double targetDistance = targetVector.Length();
targetVector.Normalize();
PlaneD forwardReversePlane = new PlaneD(block.WorldMatrix.Forward, 0);
PlaneD leftRightPlane = new PlaneD(block.WorldMatrix.Left, 0);
PlaneD upDownPlane = new PlaneD(block.WorldMatrix.Up, 0);
float forwardReverseDistance = Convert.ToSingle(Math.Sin(forwardReversePlane.DotNormal(targetVector)) * targetDistance);
float leftRightDistance = Convert.ToSingle(Math.Sin(leftRightPlane.DotNormal(targetVector)) * targetDistance);
float upDownDistance = Convert.ToSingle(Math.Sin(upDownPlane.DotNormal(targetVector)) * targetDistance);
Vector3D xyzDistance = new Vector3D();
xyzDistance.X = leftRightDistance;
xyzDistance.Y = forwardReverseDistance;
xyzDistance.Z = upDownDistance;
return(xyzDistance);
}
public Ship(IMyTerminalBlock remote, IMyGridTerminalSystem gts, MyGridProgram pro, String Name = "LCDebug")
{
GridTerminalSystem = gts;
this.remote = remote;
PopulateGyros();
lastPos = remote.GetPosition();
Me = pro;
PopulateThrusters();
// GetStoredInScreen(Name);
}
public static double GetTargetRotorAngle(IMyTerminalBlock rotor, Vector3D targ)
{
Vector3D targDir = Vector3D.Normalize(targ - rotor.GetPosition());
targDir = Vector3D.Normalize(Vector3D.Reject(targDir, rotor.WorldMatrix.Up));
targDir = Vector3D.Transform(targDir, MatrixD.Invert(rotor.WorldMatrix.GetOrientation()));
double angleAway = Math.Atan2(targDir.GetDim(2), targDir.GetDim(0));
double finalAng = angleAway;
return finalAng;
}
public static double GetTargetRotorAngle(IMyTerminalBlock rotor, Vector3D targ, IMyTerminalBlock reference)
{
Vector3D targDir = Vector3D.Normalize(targ - reference.GetPosition());
targDir = Vector3D.Normalize(Vector3D.Reject(targDir, rotor.WorldMatrix.Up));
MatrixD state = MatrixD.Invert(rotor.WorldMatrix.GetOrientation());
targDir = Vector3D.Transform(targDir, state);
double angleAway = Math.Atan2(targDir.GetDim(2), targDir.GetDim(0));//Math.Atan2(Vector3D.Cross(targDir, rotor.WorldMatrix.Right).Length(), Vector3D.Dot(targDir, rotor.WorldMatrix.Right));
double finalAng = angleAway;
return finalAng;
}
void Main(string argument)
{
if (isfirst2)
{
//beginningvel = ship.Velocity;
isfirst2 = false;
}
if (isfirst)
{
Vector3D targ = new Vector3D(0, -60000, 0);
remote = GridTerminalSystem.GetBlockWithName("Remote");
timer = GridTerminalSystem.GetBlockWithName("Timer");
ship.Multiplier = Multiplier;
ship = new SingleAxisThrustShip(remote, GridTerminalSystem, this);
// ship.planet = planet;
ship.planet = Planet.ALIEN;
//ship.Mass = 959798 - ship.GetMassSummary();
ship.Target = (new Vector3D(-100, 0, 0)) + remote.GetPosition();
//arm = new ArmController(ship, "Shoulder Yaw", "Shoulder Pitch", "Elbow", "Wrist Pitch", "Wrist Yaw");
isfirst = false;
isfirst2 = true;
// panel = (IMyTextPanel)GridTerminalSystem.GetBlockWithName("LCDebug");
//arm.extensionOffsetAngle = Math.PI / 2;
targvel = remote.WorldMatrix.Forward * 10;
//InteropManager.Add(new InterpOperation(arm, Vector3D.Transform(new Vector3D(10, 5, 0), arm.shoulderYaw.WorldMatrix), 0, 0));
//InteropManager.Add(new InterpOperation(arm, remote.GetPosition(), 0, 0));
}
switch (argument)
{
case "Measure":
xs.Add(remote.GetPosition());
ds.Add(Vector3D.Normalize((remote as IMyRemoteControl).GetNaturalGravity()));
break;
default:
if (!comfound)
{
// comfound = ship.TestForInitialCoM();
}
if (xs.Count >= 2)
{
// Center = Planet.CalcCenterOfPlanet(xs, ds);
}
double poop = 0;
double pee = 0;
//test = arm.setArm(Vector3D.Transform(new Vector3D(10, 5, 0), arm.shoulderYaw.WorldMatrix), 0, 0);
// Echo((Vector3D.Transform(arm.FindPos(out poop,out pee), MatrixD.Invert(arm.shoulderYaw.WorldMatrix))).ToString());
//InteropManager.Tick();
// //Vector3D.Transform(new Vector3D(3, 0, 0), arm.shoulderYaw.WorldMatrix)
//new Vector3D(0,0, 9), arm.shoulderYaw.WorldMatrix)
//ship.WriteToScreen(ship.GetDisplayString(), "LCDebug", false);
Matrix orient;
remote.Orientation.GetMatrix(out orient);
MatrixD InvWorld = MatrixD.Invert(MatrixD.CreateFromDir(remote.WorldMatrix.
Forward, remote.WorldMatrix.Forward));
// if (!test)
// {
// ship.SetDampeners(false);
ship.MaintainVelocity(Vector3D.Normalize((remote as IMyRemoteControl).GetNaturalGravity()) * 1.2);
// }
// else
// {
// ship.SetDampeners(true);
// }
break;
}
//ship.set_oriented_gyros_planet(targ, new Vector3D(0, 0, 0), orient.Right);
if ((timer as IMyFunctionalBlock).Enabled)
{
timer.GetActionWithName("TriggerNow").Apply(timer);
}
}
