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