public void Rotate()
{
if (OrbitEntity == null)
{
m_flyTo = true;
m_mover.StopRotate();
}
else if (OrbitEntity is MyPlanet)
{
m_flyTo = false;
m_mover.CalcRotate(m_navBlock, RelativeDirection3F.FromWorld(m_navBlock.Grid, m_faceDirection));
}
else if (m_navSet.DistanceLessThan(OrbitSpeed))
{
if (m_flyTo)
{
CalcFakeOrbitSpeedForce();
m_flyTo = false;
}
m_mover.CalcRotate(m_navBlock, RelativeDirection3F.FromWorld(m_navBlock.Grid, m_faceDirection));
}
else
{
m_flyTo = true;
m_mover.CalcRotate();
}
}
public void Rotate()
{
//Log.DebugLog("entered");
if (m_enemy == null)
{
m_mover.StopRotate();
return;
}
if (m_approaching)
{
//Log.DebugLog("approaching");
m_mover.CalcRotate();
}
else
{
//Log.DebugLog("ramming");
if (m_navSet.DistanceLessThan(100f))
{
// just before impact, face the target
Vector3 targetDirection = m_enemy.GetPosition() - m_controlBlock.CubeBlock.GetPosition();
targetDirection.Normalize();
m_mover.CalcRotate(m_mover.Thrust.Standard, RelativeDirection3F.FromWorld(m_mover.Block.CubeGrid, targetDirection));
}
else
{
m_mover.CalcRotate_Accel();
}
}
}
public override void Rotate()
{
if (m_gridFinder.Grid == null)
{
m_mover.StopRotate();
return;
}
if (m_landingState == LandingState.None)
{
if (m_targetBlock != null && m_targetBlock.Forward.HasValue)
{
m_navSet.GetSettingsLevel(m_settingLevel).NavigatorRotator = this;
//Log.DebugLog("matching target direction", "Rotate()");
m_mover.CalcRotate(m_navBlock, m_gridFinder.Block, m_targetBlock.Forward, m_targetBlock.Upward);
return;
}
else
{
m_mover.CalcRotate();
}
return;
}
if (m_landingState == LandingState.Approach)
{
//Log.DebugLog("facing controller towards target : " + m_targetPosition, "Rotate()");
m_mover.CalcRotate();
return;
}
if (m_gridFinder.Block == null)
{
if (m_landGearWithoutTargetBlock)
{
m_mover.CalcRotate(m_navBlock, RelativeDirection3F.FromWorld(m_navBlock.Grid, m_destination.WorldPosition() - m_navBlock.WorldPosition));
return;
}
m_mover.CalcRotate();
return;
}
//Log.DebugLog("rotating for landing", "Rotate()");
if (IsLocked())
{
Log.DebugLog("already landed");
return;
}
m_mover.CalcRotate(m_navBlock, m_gridFinder.Block, m_landingDirection, m_targetBlock.Upward);
return;
}
public override void Rotate()
{
switch (m_stage)
{
case Stage.Mine:
case Stage.Tunnel:
{
Vector3 direction = Vector3.Normalize(m_target.WorldPosition() - m_navBlock.WorldPosition);
m_mover.CalcRotate(m_navBlock, RelativeDirection3F.FromWorld(m_grid, direction));
break;
}
}
}
/// <summary>
/// Match orientation with the target block.
/// </summary>
/// <param name="block">The navigation block</param>
/// <param name="destBlock">The destination block</param>
/// <param name="forward">The direction of destBlock that will be matched to navigation block's forward</param>
/// <param name="upward">The direction of destBlock that will be matched to navigation block's upward</param>
/// <returns>True iff localMatrix is facing the same direction as destBlock's forward</returns>
public void CalcRotate(PseudoBlock block, IMyCubeBlock destBlock, Base6Directions.Direction?forward, Base6Directions.Direction?upward)
{
//Log.DebugLog("entered CalcRotate(PseudoBlock block, IMyCubeBlock destBlock, Base6Directions.Direction? forward, Base6Directions.Direction? upward)", "CalcRotate()");
if (forward == null)
{
forward = Base6Directions.Direction.Forward;
}
RelativeDirection3F faceForward = RelativeDirection3F.FromWorld(block.Grid, destBlock.WorldMatrix.GetDirectionVector(forward.Value));
RelativeDirection3F faceUpward = upward.HasValue ? RelativeDirection3F.FromWorld(block.Grid, destBlock.WorldMatrix.GetDirectionVector(upward.Value)) : null;
CalcRotate(block.LocalMatrix, faceForward, faceUpward);
}
public void Rotate()
{
switch (m_stage)
{
case Stage.Approach:
m_mover.CalcRotate();
return;
case Stage.Rotate:
case Stage.Land:
m_mover.CalcRotate(m_landBlock, RelativeDirection3F.FromWorld(m_landBlock.Grid, m_targetPostion.WorldPosition() - m_landBlock.WorldPosition));
return;
}
}
// necessary wrapper for main CalcRotate, should always be called.
private void CalcRotate(Matrix localMatrix, RelativeDirection3F Direction, RelativeDirection3F UpDirect = null, bool gravityAdjusted = false, IMyEntity targetEntity = null)
{
//Log.DebugLog("entered CalcRotate(Matrix localMatrix, RelativeDirection3F Direction, RelativeDirection3F UpDirect = null, bool levelingOff = false, IMyEntity targetEntity = null)", "CalcRotate()");
CheckGrid();
Thrust.Update();
if (!gravityAdjusted && ThrustersOverWorked())
{
CalcRotate_InGravity(Direction);
return;
}
in_CalcRotate(localMatrix, Direction, UpDirect, targetEntity);
}
public override void Rotate()
{
Vector3 linearVelocity = m_grid.Physics.LinearVelocity;
float speedSquared = linearVelocity.LengthSquared();
if (speedSquared < 1f)
{
m_mover.CalcRotate();
m_navSet.Settings_Current.DistanceAngle = 0f;
}
else
{
m_mover.CalcRotate(m_navBlock, RelativeDirection3F.FromWorld(m_grid, linearVelocity));
}
}
public void Rotate()
{
if (m_enemy == null || (m_navSet.DistanceLessThan(1f) && m_navSet.Settings_Current.DistanceAngle <= MaxAngleRotate))
{
m_mover.CalcRotate_Stop();
return;
}
if (m_stage == Stage.Intercept)
{
m_mover.CalcRotate();
return;
}
//Log.DebugLog("rotating to " + m_targetPosition);
m_mover.CalcRotate(m_navGrind, RelativeDirection3F.FromWorld(m_navGrind.Grid, m_targetPosition - m_navGrind.WorldPosition));
}
/// <summary>
/// Rotate to face the acceleration direction. If acceleration is zero, invokes CalcRotate_Stop.
/// </summary>
public void CalcRotate_Accel()
{
if (m_moveAccel == Vector3.Zero)
{
CalcRotate_Stop();
return;
}
if (SignificantGravity())
{
CalcRotate_InGravity(RelativeDirection3F.FromBlock(Block.CubeBlock, m_moveAccel));
}
else
{
CalcRotate(Thrust.Standard.LocalMatrix, RelativeDirection3F.FromBlock(Block.CubeBlock, m_moveAccel));
}
}
public override void Rotate()
{
switch (m_stage)
{
case Stage.Backout:
{
Vector3 direction = Vector3.Normalize(m_navBlock.WorldPosition - m_target.WorldPosition());
m_mover.CalcRotate(m_navBlock, RelativeDirection3F.FromWorld(m_grid, direction));
break;
}
case Stage.FromCentre:
{
m_mover.StopRotate();
break;
}
}
}
public void Rotate()
{
if (!m_weaponArmed)
{
m_mover.StopRotate();
return;
}
if (m_weapon_primary == null || m_weapon_primary.CubeBlock.Closed)
{
Log.DebugLog("no primary weapon");
Disarm();
m_mover.StopRotate();
return;
}
Vector3?FiringDirection = m_weapon_primary.CurrentTarget.FiringDirection;
if (!FiringDirection.HasValue)
{
if (m_orbiter != null)
{
m_orbiter.Rotate();
}
else
{
m_mover.CalcRotate();
}
return;
}
//Log.DebugLog("facing target at " + firingDirection.Value, "Rotate()");
RelativeDirection3F upDirect = null;
if (m_mover.SignificantGravity())
{
upDirect = RelativeDirection3F.FromWorld(m_controlBlock.CubeGrid, -m_mover.Thrust.WorldGravity.vector);
}
m_mover.CalcRotate(m_weapon_primary_pseudo, RelativeDirection3F.FromWorld(m_weapon_primary_pseudo.Grid, FiringDirection.Value), UpDirect: upDirect, targetEntity: m_weapon_primary.CurrentTarget.Entity);
}
/// <summary>
/// When in space, stops rotation. When in gravity, rotate to hover.
/// </summary>
private void CalcRotate_Hover()
{
if (SignificantGravity())
{
float accel = Thrust.SecondaryForce / Block.Physics.Mass;
if (accel > Thrust.GravityStrength)
{
Log.DebugLog("facing secondary away from gravity, secondary: " + Thrust.Gravity.LocalMatrix.Forward + ", away from gravity: " + (-Thrust.LocalGravity.vector));
CalcRotate(Thrust.Gravity.LocalMatrix, RelativeDirection3F.FromLocal(Block.CubeGrid, -Thrust.LocalGravity.vector), gravityAdjusted: true);
}
else
{
Log.DebugLog("facing primary away from gravity, primary: " + Thrust.Standard.LocalMatrix.Forward + ", away from gravity: " + (-Thrust.LocalGravity.vector));
CalcRotate(Thrust.Standard.LocalMatrix, RelativeDirection3F.FromLocal(Block.CubeGrid, -Thrust.LocalGravity.vector), gravityAdjusted: true);
}
return;
}
//Log.DebugLog("stopping rotation");
StopRotate();
}
/// <summary>
/// Calculate the best rotation to stop the ship.
/// </summary>
/// TODO: if ship cannot rotate quickly, find a reasonable alternative to facing primary
public void CalcRotate_Stop()
{
//Log.DebugLog("entered CalcRotate_Stop()", "CalcRotate_Stop()");
Vector3 linearVelocity = LinearVelocity;
if (!Thrust.CanMoveAnyDirection() || linearVelocity.LengthSquared() > 1f)
{
//Log.DebugLog("rotate to stop");
if (SignificantGravity())
{
CalcRotate_InGravity(RelativeDirection3F.FromWorld(Block.CubeGrid, -linearVelocity));
}
else
{
CalcRotate(Thrust.Standard.LocalMatrix, RelativeDirection3F.FromWorld(Block.CubeGrid, -linearVelocity));
}
return;
}
CalcRotate_Hover();
}
/// <summary>
/// Calculates the force necessary to rotate the grid.
/// </summary>
/// <param name="Direction">The direction to face the localMatrix in.</param>
/// <param name="block"></param>
/// <returns>True iff localMatrix is facing Direction</returns>
public void CalcRotate(PseudoBlock block, RelativeDirection3F Direction, RelativeDirection3F UpDirect = null, IMyEntity targetEntity = null)
{
//m_logger.debugLog("entered CalcRotate(PseudoBlock block, RelativeDirection3F Direction, RelativeDirection3F UpDirect = null, IMyEntity targetEntity = null)", "CalcRotate()");
CalcRotate(block.LocalMatrix, Direction, UpDirect, targetEntity: targetEntity);
}
/// <summary>
/// Calculates the force necessary to rotate the grid.
/// </summary>
/// <param name="localMatrix">The matrix to rotate to face the direction, use a block's local matrix or result of GetMatrix()</param>
/// <param name="Direction">The direction to face the localMatrix in.</param>
/// <param name="angularVelocity">The local angular velocity of the controlling block.</param>
private void CalcRotate(Matrix localMatrix, RelativeDirection3F Direction, RelativeDirection3F UpDirect, out Vector3 angularVelocity)
{
CheckGrid();
myLogger.debugLog(Direction == null, "Direction == null", "CalcRotate()", Logger.severity.ERROR);
angularVelocity = -Vector3.Transform(Block.Physics.AngularVelocity, Block.CubeBlock.WorldMatrixNormalizedInv.GetOrientation());
//myLogger.debugLog("angular: " + angularVelocity, "CalcRotate()");
float gyroForce = myGyro.TotalGyroForce();
const ulong UpdateFrequency = ShipController_Autopilot.UpdateFrequency;
if (rotateForceRatio != Vector3.Zero)
if (Globals.UpdateCount - updated_prevAngleVel == UpdateFrequency) // needs to be == because displacment is not divided by frequency
{
Vector3 ratio = (angularVelocity - prevAngleVel) / (rotateForceRatio * gyroForce);
//myLogger.debugLog("rotateForceRatio: " + rotateForceRatio + ", ratio: " + ratio + ", accel: " + (angularVelocity - prevAngleVel) + ", torque: " + (rotateForceRatio * gyroForce), "CalcRotate()");
myGyro.Update_torqueAccelRatio(rotateForceRatio, ratio);
}
else
myLogger.debugLog("prevAngleVel is old: " + (Globals.UpdateCount - updated_prevAngleVel), "CalcRotate()", Logger.severity.DEBUG);
localMatrix.M41 = 0; localMatrix.M42 = 0; localMatrix.M43 = 0; localMatrix.M44 = 1;
Matrix inverted; Matrix.Invert(ref localMatrix, out inverted);
localMatrix = localMatrix.GetOrientation();
inverted = inverted.GetOrientation();
//myLogger.debugLog("local matrix: right: " + localMatrix.Right + ", up: " + localMatrix.Up + ", back: " + localMatrix.Backward + ", trans: " + localMatrix.Translation, "CalcRotate()");
//myLogger.debugLog("inverted matrix: right: " + inverted.Right + ", up: " + inverted.Up + ", back: " + inverted.Backward + ", trans: " + inverted.Translation, "CalcRotate()");
//myLogger.debugLog("local matrix: " + localMatrix, "CalcRotate()");
//myLogger.debugLog("inverted matrix: " + inverted, "CalcRotate()");
Vector3 localDirect = Direction.ToLocal();
Vector3 rotBlockDirect; Vector3.Transform(ref localDirect, ref inverted, out rotBlockDirect);
rotBlockDirect.Normalize();
float azimuth, elevation; Vector3.GetAzimuthAndElevation(rotBlockDirect, out azimuth, out elevation);
Vector3 rotaRight = localMatrix.Right;
Vector3 rotaUp = localMatrix.Up;
Vector3 NFR_right = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaRight));
Vector3 NFR_up = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaUp));
Vector3 displacement = -elevation * NFR_right - azimuth * NFR_up;
if (UpDirect != null)
{
Vector3 upLocal = UpDirect.ToLocal();
Vector3 upRotBlock; Vector3.Transform(ref upLocal, ref inverted, out upRotBlock);
float roll; Vector3.Dot(ref upRotBlock, ref Vector3.Right, out roll);
Vector3 rotaBackward = localMatrix.Backward;
Vector3 NFR_backward = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaBackward));
myLogger.debugLog("roll: " + roll + ", displacement: " + displacement + ", NFR_backward: " + NFR_backward + ", change: " + (-roll * NFR_backward), "CalcRotate()");
displacement += roll * NFR_backward;
}
NavSet.Settings_Task_NavWay.DistanceAngle = displacement.Length();
if (NavSet.Settings_Current.CollisionAvoidance)
{
myPathfinder.TestRotate(displacement);
if (!myPathfinder.CanRotate)
{
Logger.debugNotify("Cannot Rotate", 50);
myLogger.debugLog("Pathfinder not allowing rotation", "CalcRotate()");
return;
}
}
//myLogger.debugLog("localDirect: " + localDirect + ", rotBlockDirect: " + rotBlockDirect + ", elevation: " + elevation + ", NFR_right: " + NFR_right + ", azimuth: " + azimuth + ", NFR_up: " + NFR_up + ", disp: " + displacement, "CalcRotate()");
if (myGyro.torqueAccelRatio == 0)
{
// do a test
myLogger.debugLog("torqueAccelRatio == 0", "CalcRotate()");
rotateForceRatio = new Vector3(0, 1f, 0);
return;
}
Vector3 targetVelocity = MaxAngleVelocity(displacement);
// Adjust for moving target by measuring changes in displacement. Part of the change in displacement is attributable to ship rotation.
const float dispToVel = (float)Globals.UpdatesPerSecond / (float)ShipController_Autopilot.UpdateFrequency;
Vector3 addVelocity = angularVelocity - (prevAngleDisp - displacement) * dispToVel;
if (addVelocity.LengthSquared() < 0.1f)
{
myLogger.debugLog("Adjust for moving, adding to target velocity: " + addVelocity, "CalcRotate()");
targetVelocity += addVelocity;
}
else
myLogger.debugLog("Not adjusting for moving, assuming target changed: " + addVelocity, "CalcRotate()");
prevAngleDisp = displacement;
Vector3 diffVel = targetVelocity - angularVelocity;
rotateForceRatio = diffVel / (myGyro.torqueAccelRatio * gyroForce);
myLogger.debugLog("targetVelocity: " + targetVelocity + ", angularVelocity: " + angularVelocity + ", diffVel: " + diffVel, "CalcRotate()");
//myLogger.debugLog("diffVel: " + diffVel + ", torque: " + (myGyro.torqueAccelRatio * gyroForce) + ", rotateForceRatio: " + rotateForceRatio, "CalcRotate()");
// dampeners
for (int i = 0; i < 3; i++)
{
// if targetVelocity is close to 0, use dampeners
float target = targetVelocity.GetDim(i);
if (target > -0.01f && target < 0.01f)
{
myLogger.debugLog("target near 0 for " + i + ", " + target, "CalcRotate()");
rotateForceRatio.SetDim(i, 0f);
continue;
}
float velDim = angularVelocity.GetDim(i);
if (velDim < 0.01f && velDim > -0.01f)
continue;
// where rotateForceRatio opposes angularVelocity, use dampeners
float dim = rotateForceRatio.GetDim(i);
if (Math.Sign(dim) * Math.Sign(angularVelocity.GetDim(i)) < 0)
//{
// myLogger.debugLog("force ratio(" + dim + ") opposes velocity(" + angularVelocity.GetDim(i) + "), index: " + i + ", " + Math.Sign(dim) + ", " + Math.Sign(angularVelocity.GetDim(i)), "CalcRotate()");
rotateForceRatio.SetDim(i, 0f);
//}
//else
// myLogger.debugLog("force ratio is aligned with velocity: " + i + ", " + Math.Sign(dim) + ", " + Math.Sign(angularVelocity.GetDim(i)), "CalcRotate()");
}
}
/// <summary>
/// Calculates the force necessary to rotate the grid.
/// </summary>
/// <param name="Direction">The direction to face the localMatrix in.</param>
/// <param name="block"></param>
/// <returns>True iff localMatrix is facing Direction</returns>
public void CalcRotate(PseudoBlock block, RelativeDirection3F Direction, RelativeDirection3F UpDirect = null)
{
Vector3 angleVelocity;
CalcRotate(block.LocalMatrix, Direction, UpDirect, out angleVelocity);
updated_prevAngleVel = Globals.UpdateCount;
prevAngleVel = angleVelocity;
//myLogger.debugLog("displacement.LengthSquared(): " + displacement.LengthSquared(), "CalcRotate()");
//return displacement.LengthSquared() < 0.01f;
//if (NavSet.Settings_Current.CollisionAvoidance)
// myPathfinder.TestRotate(displacement);
}
/// <summary>
/// Calculates the force necessary to rotate the grid.
/// </summary>
/// <param name="Direction">The direction to face the localMatrix in.</param>
/// <param name="block"></param>
/// <returns>True iff localMatrix is facing Direction</returns>
public void CalcRotate(PseudoBlock block, RelativeDirection3F Direction, RelativeDirection3F UpDirect = null, IMyEntity targetEntity = null)
{
CalcRotate(block.LocalMatrix, Direction, UpDirect, targetEntity: targetEntity);
}
// necessary wrapper for main CalcRotate, should always be called.
private void CalcRotate(Matrix localMatrix, RelativeDirection3F Direction, RelativeDirection3F UpDirect, bool levelingOff = false, IMyEntity targetEntity = null)
{
CheckGrid();
myThrust.Update();
if (!levelingOff)
{
if (ThrustersOverWorked())
{
myLogger.debugLog("Thrusters overworked; overruling navigator, need to level off", "CalcRotate()");
if (InGravity_LevelOff())
return;
else
myLogger.alwaysLog("Failed gravity check!", "CalcRotate()", Logger.severity.FATAL);
}
if (NavSet.Settings_Current.CollisionAvoidance && !myPathfinder.CanMove && InGravity_LevelOff())
{
myLogger.debugLog("Pathfinder preventing movement; level off", "CalcRotate()");
return;
}
}
Vector3 angleVelocity;
CalcRotate(localMatrix, Direction, UpDirect, out angleVelocity, targetEntity);
prevAngleVel = angleVelocity;
}
/// <summary>
/// Calculates the force necessary to rotate the grid.
/// </summary>
/// <param name="Direction">The direction to face the localMatrix in.</param>
/// <param name="block"></param>
/// <returns>True iff localMatrix is facing Direction</returns>
public void CalcRotate(PseudoBlock block, RelativeDirection3F Direction, RelativeDirection3F UpDirect = null, IMyEntity targetEntity = null)
{
//Log.DebugLog("entered CalcRotate(PseudoBlock block, RelativeDirection3F Direction, RelativeDirection3F UpDirect = null, IMyEntity targetEntity = null)", "CalcRotate()");
CalcRotate(block.LocalMatrix, Direction, UpDirect, targetEntity: targetEntity);
}
/// <summary>
/// Calculates the force necessary to rotate the grid. Two degrees of freedom are used to rotate forward toward Direction; the remaining degree is used to face upward towards UpDirect.
/// </summary>
/// <param name="localMatrix">The matrix to rotate to face the direction, use a block's local matrix or result of GetMatrix()</param>
/// <param name="Direction">The direction to face the localMatrix in.</param>
private void in_CalcRotate(Matrix localMatrix, RelativeDirection3F Direction, RelativeDirection3F UpDirect, IMyEntity targetEntity)
{
m_logger.debugLog(Direction == null, "Direction == null", Logger.severity.ERROR);
m_gyro.Update();
float minimumMoment = Math.Min(m_gyro.InvertedInertiaMoment.Min(), MaxInverseTensor);
if (minimumMoment <= 0f)
{
// == 0f, not calculated yet. < 0f, we have math failure
StopRotate();
m_logger.debugLog(minimumMoment < 0f, "minimumMoment < 0f", Logger.severity.FATAL);
return;
}
localMatrix.M41 = 0; localMatrix.M42 = 0; localMatrix.M43 = 0; localMatrix.M44 = 1;
Matrix inverted; Matrix.Invert(ref localMatrix, out inverted);
localMatrix = localMatrix.GetOrientation();
inverted = inverted.GetOrientation();
Vector3 localDirect = Direction.ToLocalNormalized();
Vector3 rotBlockDirect; Vector3.Transform(ref localDirect, ref inverted, out rotBlockDirect);
float azimuth, elevation; Vector3.GetAzimuthAndElevation(rotBlockDirect, out azimuth, out elevation);
Vector3 rotaRight = localMatrix.Right;
Vector3 rotaUp = localMatrix.Up;
Vector3 NFR_right = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaRight));
Vector3 NFR_up = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaUp));
Vector3 displacement = -elevation * NFR_right - azimuth * NFR_up;
if (UpDirect != null)
{
Vector3 upLocal = UpDirect.ToLocalNormalized();
Vector3 upRotBlock; Vector3.Transform(ref upLocal, ref inverted, out upRotBlock);
upRotBlock.Z = 0f;
upRotBlock.Normalize();
float roll = Math.Sign(upRotBlock.X) * (float)Math.Acos(MathHelper.Clamp(upRotBlock.Y, -1f, 1f));
Vector3 rotaBackward = localMatrix.Backward;
Vector3 NFR_backward = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaBackward));
//m_logger.debugLog("upLocal: " + upLocal + ", upRotBlock: " + upRotBlock + ", roll: " + roll + ", displacement: " + displacement + ", NFR_backward: " + NFR_backward + ", change: " + roll * NFR_backward, "in_CalcRotate()");
displacement += roll * NFR_backward;
}
m_lastMoveAttempt = Globals.UpdateCount;
Pathfinder.TestRotate(displacement);
switch (Pathfinder.m_rotateState)
{
case Autopilot.Pathfinder.Pathfinder.PathState.Not_Running:
m_logger.debugLog("Pathfinder not run yet: " + Pathfinder.m_rotateState);
m_lastMove = Globals.UpdateCount;
return;
case Autopilot.Pathfinder.Pathfinder.PathState.No_Obstruction:
break;
default:
m_logger.debugLog("Pathfinder not allowing rotation: " + Pathfinder.m_rotateState);
return;
}
float distanceAngle = displacement.Length();
if (distanceAngle < m_bestAngle || float.IsNaN(m_navSet.Settings_Current.DistanceAngle))
{
m_bestAngle = distanceAngle;
m_lastMove = Globals.UpdateCount;
}
m_navSet.Settings_Task_NavWay.DistanceAngle = distanceAngle;
//myLogger.debugLog("localDirect: " + localDirect + ", rotBlockDirect: " + rotBlockDirect + ", elevation: " + elevation + ", NFR_right: " + NFR_right + ", azimuth: " + azimuth + ", NFR_up: " + NFR_up + ", disp: " + displacement, "in_CalcRotate()");
m_rotateTargetVelocity = MaxAngleVelocity(displacement, minimumMoment, targetEntity != null);
// adjustment to face a moving entity
if (targetEntity != null)
{
Vector3 relativeLinearVelocity = targetEntity.GetLinearVelocity() - LinearVelocity;
float distance = Vector3.Distance(targetEntity.GetCentre(), Block.CubeBlock.GetPosition());
//myLogger.debugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", tangentialVelocity: " + tangentialVelocity + ", localTangVel: " + localTangVel, "in_CalcRotate()");
float RLV_pitch = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Down);
float RLV_yaw = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Right);
float angl_pitch = (float)Math.Atan2(RLV_pitch, distance);
float angl_yaw = (float)Math.Atan2(RLV_yaw, distance);
m_logger.debugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", RLV_yaw: " + RLV_yaw + ", RLV_pitch: " + RLV_pitch + ", angl_yaw: " + angl_yaw + ", angl_pitch: " + angl_pitch + ", total adjustment: " + (NFR_right * angl_pitch + NFR_up * angl_yaw));
m_rotateTargetVelocity += NFR_right * angl_pitch + NFR_up * angl_yaw;
}
//m_logger.debugLog("targetVelocity: " + m_rotateTargetVelocity, "in_CalcRotate()");
// angular velocity is reversed
Vector3 angularVelocity = AngularVelocity.ToBlock(Block.CubeBlock);// ((DirectionWorld)(-Block.Physics.AngularVelocity)).ToBlock(Block.CubeBlock);
m_rotateForceRatio = (m_rotateTargetVelocity + angularVelocity) / (minimumMoment * m_gyro.GyroForce);
m_logger.debugLog("targetVelocity: " + m_rotateTargetVelocity + ", angularVelocity: " + angularVelocity + ", accel: " + (m_rotateTargetVelocity + angularVelocity));
m_logger.debugLog("minimumMoment: " + minimumMoment + ", force: " + m_gyro.GyroForce + ", rotateForceRatio: " + m_rotateForceRatio);
// dampeners
for (int index = 0; index < 3; index++)
{
// if targetVelocity is close to 0, use dampeners
float target = m_rotateTargetVelocity.GetDim(index);
if (target > -0.01f && target < 0.01f)
{
//m_logger.debugLog("target near 0 for " + i + ", " + target, "in_CalcRotate()");
m_rotateTargetVelocity.SetDim(index, 0f);
m_rotateForceRatio.SetDim(index, 0f);
continue;
}
}
}
private void CalcRotate_InGravity(RelativeDirection3F direction)
{
//m_logger.debugLog("entered CalcRotate_InGravity(RelativeDirection3F direction)", "CalcRotate_InGravity()");
float secondaryAccel = Thrust.SecondaryForce / Block.Physics.Mass;
RelativeDirection3F fightGrav = RelativeDirection3F.FromLocal(Block.CubeGrid, -Thrust.LocalGravity.vector);
if (secondaryAccel > Thrust.GravityStrength)
{
// secondary thrusters are strong enough to fight gravity
if (Vector3.Dot(direction.ToLocalNormalized(), fightGrav.ToLocalNormalized()) > 0f)
{
// direction is away from gravity
m_logger.debugLog("Facing primary towards direction(" + direction.ToLocalNormalized() + "), rolling secondary away from gravity(" + fightGrav.ToLocalNormalized() + ")");
CalcRotate(Thrust.Standard.LocalMatrix, direction, fightGrav, gravityAdjusted: true);
return;
}
else
{
// direction is towards gravity
m_logger.debugLog("Facing secondary away from gravity(" + fightGrav.ToLocalNormalized() + "), rolling primary towards direction(" + direction.ToLocalNormalized() + ")");
CalcRotate(Thrust.Gravity.LocalMatrix, fightGrav, direction, gravityAdjusted: true);
return;
}
}
// secondary thrusters are not strong enough to fight gravity
if (secondaryAccel > 1f)
{
m_logger.debugLog("Facing primary towards gravity(" + fightGrav.ToLocalNormalized() + "), rolling secondary towards direction(" + direction.ToLocalNormalized() + ")");
CalcRotate(Thrust.Standard.LocalMatrix, fightGrav, direction, gravityAdjusted: true);
return;
}
// helicopter
float primaryAccel = Math.Max(Thrust.PrimaryForce / Block.Physics.Mass - Thrust.GravityStrength, 1f); // obviously less than actual value but we do not need maximum theoretical acceleration
DirectionGrid moveAccel = m_moveAccel != Vector3.Zero ? ((DirectionBlock)m_moveAccel).ToGrid(Block.CubeBlock) : LinearVelocity.ToGrid(Block.CubeGrid) * -0.5f;
if (moveAccel.vector.LengthSquared() > primaryAccel * primaryAccel)
{
//myLogger.debugLog("move accel is over available acceleration: " + moveAccel.vector.Length() + " > " + primaryAccel, "CalcRotate_InGravity()");
moveAccel = Vector3.Normalize(moveAccel.vector) * primaryAccel;
}
m_logger.debugLog("Facing primary away from gravity and towards direction, moveAccel: " + moveAccel + ", fight gravity: " + fightGrav.ToLocal() + ", direction: " + direction.ToLocalNormalized() + ", new direction: " + (moveAccel + fightGrav.ToLocal()));
Vector3 dirV = moveAccel + fightGrav.ToLocal();
direction = RelativeDirection3F.FromLocal(Block.CubeGrid, dirV);
CalcRotate(Thrust.Standard.LocalMatrix, direction, gravityAdjusted: true);
Vector3 fightGravDirection = fightGrav.ToLocal() / Thrust.GravityStrength;
// determine acceleration needed in forward direction to move to desired altitude or remain at current altitude
float projectionMagnitude = Vector3.Dot(dirV, fightGravDirection) / Vector3.Dot(Thrust.Standard.LocalMatrix.Forward, fightGravDirection);
Vector3 projectionDirection = ((DirectionGrid)Thrust.Standard.LocalMatrix.Forward).ToBlock(Block.CubeBlock);
m_moveForceRatio = projectionDirection * projectionMagnitude * Block.CubeGrid.Physics.Mass / Thrust.PrimaryForce;
m_logger.debugLog("changed moveForceRatio, projectionMagnitude: " + projectionMagnitude + ", projectionDirection: " + projectionDirection + ", moveForceRatio: " + m_moveForceRatio);
}
/// <summary>
/// Calculates the force necessary to rotate the grid.
/// </summary>
/// <param name="localMatrix">The matrix to rotate to face the direction, use a block's local matrix or result of GetMatrix()</param>
/// <param name="Direction">The direction to face the localMatrix in.</param>
/// <param name="angularVelocity">The local angular velocity of the controlling block.</param>
private void CalcRotate(Matrix localMatrix, RelativeDirection3F Direction, RelativeDirection3F UpDirect, out Vector3 angularVelocity, IMyEntity targetEntity)
{
myLogger.debugLog(Direction == null, "Direction == null", "CalcRotate()", Logger.severity.ERROR);
angularVelocity = -Vector3.Transform(Block.Physics.AngularVelocity, Block.CubeBlock.WorldMatrixNormalizedInv.GetOrientation());
//myLogger.debugLog("angular: " + angularVelocity, "CalcRotate()");
float gyroForce = myGyro.TotalGyroForce();
float secondsSinceLast = (float)(DateTime.UtcNow - updated_prevAngleVel).TotalSeconds;
updated_prevAngleVel = DateTime.UtcNow;
if (rotateForceRatio != Vector3.Zero)
{
if (secondsSinceLast <= MaxUpdateSeconds)
{
Vector3 ratio = (angularVelocity - prevAngleVel) / (rotateForceRatio * gyroForce * secondsSinceLast);
//myLogger.debugLog("rotateForceRatio: " + rotateForceRatio + ", ratio: " + ratio + ", accel: " + (angularVelocity - prevAngleVel) + ", torque: " + (rotateForceRatio * gyroForce), "CalcRotate()");
myGyro.Update_torqueAccelRatio(rotateForceRatio, ratio);
}
else
myLogger.debugLog("prevAngleVel is old: " + secondsSinceLast, "CalcRotate()", Logger.severity.DEBUG);
}
localMatrix.M41 = 0; localMatrix.M42 = 0; localMatrix.M43 = 0; localMatrix.M44 = 1;
Matrix inverted; Matrix.Invert(ref localMatrix, out inverted);
localMatrix = localMatrix.GetOrientation();
inverted = inverted.GetOrientation();
//myLogger.debugLog("local matrix: right: " + localMatrix.Right + ", up: " + localMatrix.Up + ", back: " + localMatrix.Backward + ", trans: " + localMatrix.Translation, "CalcRotate()");
//myLogger.debugLog("inverted matrix: right: " + inverted.Right + ", up: " + inverted.Up + ", back: " + inverted.Backward + ", trans: " + inverted.Translation, "CalcRotate()");
//myLogger.debugLog("local matrix: " + localMatrix, "CalcRotate()");
//myLogger.debugLog("inverted matrix: " + inverted, "CalcRotate()");
Vector3 localDirect = Direction.ToLocal();
Vector3 rotBlockDirect; Vector3.Transform(ref localDirect, ref inverted, out rotBlockDirect);
rotBlockDirect.Normalize();
float azimuth, elevation; Vector3.GetAzimuthAndElevation(rotBlockDirect, out azimuth, out elevation);
Vector3 rotaRight = localMatrix.Right;
Vector3 rotaUp = localMatrix.Up;
Vector3 NFR_right = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaRight));
Vector3 NFR_up = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaUp));
Vector3 displacement = -elevation * NFR_right - azimuth * NFR_up;
if (UpDirect != null)
{
Vector3 upLocal = UpDirect.ToLocal();
Vector3 upRotBlock; Vector3.Transform(ref upLocal, ref inverted, out upRotBlock);
float roll; Vector3.Dot(ref upRotBlock, ref Vector3.Right, out roll);
Vector3 rotaBackward = localMatrix.Backward;
Vector3 NFR_backward = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaBackward));
myLogger.debugLog("roll: " + roll + ", displacement: " + displacement + ", NFR_backward: " + NFR_backward + ", change: " + (roll * NFR_backward), "CalcRotate()");
displacement += roll * NFR_backward;
}
NavSet.Settings_Task_NavWay.DistanceAngle = displacement.Length();
if (NavSet.Settings_Current.CollisionAvoidance)
{
myPathfinder.TestRotate(displacement);
if (!myPathfinder.CanRotate)
{
// if cannot rotate and not calculating move, move away from obstruction
if (myPathfinder.RotateObstruction != null && Globals.UpdateCount >= m_notCalcMove)
{
Vector3 position = Block.CubeBlock.GetPosition();
Vector3 away = position - myPathfinder.RotateObstruction.GetCentre();
away.Normalize();
myLogger.debugLog("Cannot rotate and not calculating move, creating GOLIS to move away from obstruction", "CalcRotate()", Logger.severity.INFO);
new GOLIS(this, NavSet, position + away * (10f + NavSet.Settings_Current.DestinationRadius), true);
}
Logger.debugNotify("Cannot Rotate", 50);
myLogger.debugLog("Pathfinder not allowing rotation", "CalcRotate()");
return;
}
}
//myLogger.debugLog("localDirect: " + localDirect + ", rotBlockDirect: " + rotBlockDirect + ", elevation: " + elevation + ", NFR_right: " + NFR_right + ", azimuth: " + azimuth + ", NFR_up: " + NFR_up + ", disp: " + displacement, "CalcRotate()");
if (myGyro.torqueAccelRatio == 0)
{
// do a test
myLogger.debugLog("torqueAccelRatio == 0", "CalcRotate()");
rotateForceRatio = new Vector3(0, 1f, 0);
return;
}
Vector3 targetVelocity = MaxAngleVelocity(displacement, secondsSinceLast);
if (targetEntity != null)
{
Vector3 relativeLinearVelocity = targetEntity.GetLinearVelocity() - Block.Physics.LinearVelocity;
float distance = Vector3.Distance(targetEntity.GetCentre(), Block.CubeBlock.GetPosition());
//Vector3 tangentialVelocity = Vector3.Reject(relativeLinearVelocity, targetEntity.GetCentre() - Block.CubeBlock.GetPosition());
//Vector3 localTangVel = Vector3.Transform(tangentialVelocity, Block.CubeBlock.WorldMatrixNormalizedInv.GetOrientation());
//myLogger.debugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", tangentialVelocity: " + tangentialVelocity + ", localTangVel: " + localTangVel, "CalcRotate()");
float RLV_pitch = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Up);
float RLV_yaw = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Left);
float angl_pitch = (float)Math.Atan2(RLV_pitch, distance);
float angl_yaw = (float)Math.Atan2(RLV_yaw, distance);
myLogger.debugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", RLV_yaw: " + RLV_yaw + ", RLV_pitch: " + RLV_pitch + ", angl_yaw: " + angl_yaw + ", angl_pitch: " + angl_pitch, "CalcRotate()");
targetVelocity += new Vector3(angl_pitch, angl_yaw, 0f);
}
Vector3 accel = (targetVelocity - angularVelocity) / secondsSinceLast;
rotateForceRatio = accel / (myGyro.torqueAccelRatio * secondsSinceLast * gyroForce);
myLogger.debugLog("targetVelocity: " + targetVelocity + ", angularVelocity: " + angularVelocity + ", accel: " + accel, "CalcRotate()");
myLogger.debugLog("accel: " + accel + ", torque: " + (myGyro.torqueAccelRatio * secondsSinceLast * gyroForce) + ", rotateForceRatio: " + rotateForceRatio, "CalcRotate()");
// dampeners
for (int i = 0; i < 3; i++)
{
// if targetVelocity is close to 0, use dampeners
float target = targetVelocity.GetDim(i);
if (target > -0.01f && target < 0.01f)
{
//myLogger.debugLog("target near 0 for " + i + ", " + target, "CalcRotate()");
rotateForceRatio.SetDim(i, 0f);
continue;
}
}
}
public void Rotate()
{
m_mover.CalcRotate(m_welder, RelativeDirection3F.FromWorld(m_welder.Grid, m_targetWorld - m_welder.WorldPosition));
}
/// <remarks>
/// Must execute regularly on game thread.
/// </remarks>
protected override void Update1_GameThread()
{
if (!MyAPIGateway.Multiplayer.IsServer)
{
return;
}
if (CurrentControl == Control.Off)
{
#if USE_AI
if (!myTurret.AIEnabled)
#endif
{
setElevation = myTurret.Elevation;
setAzimuth = myTurret.Azimuth;
}
return;
}
// CurrentTarget may be changed by WeaponTargeting
Target GotTarget = CurrentTarget;
if (GotTarget.Entity == null)
{
//FireWeapon = false;
return;
}
if (!GotTarget.FiringDirection.HasValue || !GotTarget.ContactPoint.HasValue) // happens alot
{
return;
}
#if USE_AI
// broken in UNSTABLE
if (myTurret.AIEnabled)
{
myTurret.SetTarget(ProjectilePosition() + GotTarget.FiringDirection.Value * 1000f);
return;
}
#endif
//Vector3 RotateTo = RelativeVector3F.createFromWorld(GotTarget.FiringDirection.Value, weapon.CubeGrid).getBlock(weapon);
Vector3 RotateToDirection = RelativeDirection3F.FromWorld(CubeBlock.CubeGrid, GotTarget.FiringDirection.Value).ToBlockNormalized(CubeBlock);
//Log.DebugLog("FiringDirection = " + GotTarget.FiringDirection.Value + ", RotateToDirection = " + RotateToDirection, "Update()");
float targetElevation, targetAzimuth; // the position of the target
Vector3.GetAzimuthAndElevation(RotateToDirection, out targetAzimuth, out targetElevation);
if (!targetElevation.IsValid() || !targetAzimuth.IsValid())
{
//Log.DebugLog("cannot rotate, invalid el(" + targetElevation + ") or az(" + targetAzimuth + ")", "RotateAndFire()");
return;
}
// should azimuth rotate past 180°?
if (Can360 && Math.Abs(setAzimuth - targetAzimuth) > Math.PI)
{
if (targetAzimuth < 0)
{
targetAzimuth += MathHelper.TwoPi;
}
else
{
targetAzimuth -= MathHelper.TwoPi;
}
}
// add amount based on speed
float nextElevation, nextAzimuth;
if (targetElevation > setElevation)
{
nextElevation = Math.Min(targetElevation, setElevation + speedElevation);
}
else if (targetElevation < setElevation)
{
nextElevation = Math.Max(targetElevation, setElevation - speedElevation);
}
else
{
nextElevation = setElevation;
}
if (targetAzimuth > setAzimuth)
{
nextAzimuth = Math.Min(targetAzimuth, setAzimuth + speedAzimuth);
}
else if (targetAzimuth < setAzimuth)
{
nextAzimuth = Math.Max(targetAzimuth, setAzimuth - speedAzimuth);
}
else
{
nextAzimuth = setAzimuth;
}
// impose limits
if (nextElevation < minElevation)
{
nextElevation = minElevation;
}
else if (nextElevation > maxElevation)
{
nextElevation = maxElevation;
}
if (!Can360)
{
if (nextAzimuth < minAzimuth)
{
nextAzimuth = minAzimuth;
}
else if (nextAzimuth > maxAzimuth)
{
nextAzimuth = maxAzimuth;
}
}
//Log.DebugLog("next elevation = " + nextElevation + ", next azimuth = " + nextAzimuth, "RotateAndFire()");
// apply changes
if (nextElevation != myTurret.Elevation)
{
myTurret.Elevation = nextElevation;
myTurret.SyncElevation();
}
//else
// Log.DebugLog("not setting elevation", "RotateAndFire()");
if (nextAzimuth != myTurret.Azimuth)
{
myTurret.Azimuth = nextAzimuth;
myTurret.SyncAzimuth();
}
//else
// Log.DebugLog("not setting azimuth", "RotateAndFire()");
setElevation = nextElevation;
setAzimuth = nextAzimuth;
}
private void CalcRotate_InGravity(RelativeDirection3F direction)
{
//Log.DebugLog("entered CalcRotate_InGravity(RelativeDirection3F direction)", "CalcRotate_InGravity()");
float secondaryAccel = Thrust.SecondaryForce / Block.Physics.Mass;
RelativeDirection3F fightGrav = RelativeDirection3F.FromLocal(Block.CubeGrid, -Thrust.LocalGravity.vector);
if (secondaryAccel > Thrust.GravityStrength)
{
// secondary thrusters are strong enough to fight gravity
if (Vector3.Dot(direction.ToLocalNormalized(), fightGrav.ToLocalNormalized()) > 0f)
{
// direction is away from gravity
Log.DebugLog("Facing primary towards direction(" + direction.ToLocalNormalized() + "), rolling secondary away from gravity(" + fightGrav.ToLocalNormalized() + ")");
CalcRotate(Thrust.Standard.LocalMatrix, direction, fightGrav, gravityAdjusted: true);
return;
}
else
{
// direction is towards gravity
Log.DebugLog("Facing secondary away from gravity(" + fightGrav.ToLocalNormalized() + "), rolling primary towards direction(" + direction.ToLocalNormalized() + ")");
CalcRotate(Thrust.Gravity.LocalMatrix, fightGrav, direction, gravityAdjusted: true);
return;
}
}
// secondary thrusters are not strong enough to fight gravity
if (secondaryAccel > 1f)
{
Log.DebugLog("Facing primary towards gravity(" + fightGrav.ToLocalNormalized() + "), rolling secondary towards direction(" + direction.ToLocalNormalized() + ")");
CalcRotate(Thrust.Standard.LocalMatrix, fightGrav, direction, gravityAdjusted: true);
return;
}
// helicopter
float primaryAccel = Math.Max(Thrust.PrimaryForce / Block.Physics.Mass - Thrust.GravityStrength, 1f); // obviously less than actual value but we do not need maximum theoretical acceleration
DirectionGrid moveAccel = m_moveAccel != Vector3.Zero ? ((DirectionBlock)m_moveAccel).ToGrid(Block.CubeBlock) : LinearVelocity.ToGrid(Block.CubeGrid) * -0.5f;
if (moveAccel.vector.LengthSquared() > primaryAccel * primaryAccel)
{
//Log.DebugLog("move accel is over available acceleration: " + moveAccel.vector.Length() + " > " + primaryAccel, "CalcRotate_InGravity()");
moveAccel = Vector3.Normalize(moveAccel.vector) * primaryAccel;
}
//Log.DebugLog("Facing primary away from gravity and towards direction, moveAccel: " + moveAccel + ", fight gravity: " + fightGrav.ToLocal() + ", direction: " + direction.ToLocalNormalized() + ", new direction: " + (moveAccel + fightGrav.ToLocal()));
Vector3 dirV = moveAccel + fightGrav.ToLocal();
direction = RelativeDirection3F.FromLocal(Block.CubeGrid, dirV);
CalcRotate(Thrust.Standard.LocalMatrix, direction, gravityAdjusted: true);
Vector3 fightGravDirection = fightGrav.ToLocal() / Thrust.GravityStrength;
// determine acceleration needed in forward direction to move to desired altitude or remain at current altitude
float projectionMagnitude = Vector3.Dot(dirV, fightGravDirection) / Vector3.Dot(Thrust.Standard.LocalMatrix.Forward, fightGravDirection);
Vector3 projectionDirection = ((DirectionGrid)Thrust.Standard.LocalMatrix.Forward).ToBlock(Block.CubeBlock);
m_moveForceRatio = projectionDirection * projectionMagnitude * Block.CubeGrid.Physics.Mass / Thrust.PrimaryForce;
Log.DebugLog("changed moveForceRatio, projectionMagnitude: " + projectionMagnitude + ", projectionDirection: " + projectionDirection + ", moveForceRatio: " + m_moveForceRatio);
}
/// <summary>
/// Calculates the force necessary to rotate the grid. Two degrees of freedom are used to rotate forward toward Direction; the remaining degree is used to face upward towards UpDirect.
/// </summary>
/// <param name="localMatrix">The matrix to rotate to face the direction, use a block's local matrix or result of GetMatrix()</param>
/// <param name="Direction">The direction to face the localMatrix in.</param>
private void in_CalcRotate(Matrix localMatrix, RelativeDirection3F Direction, RelativeDirection3F UpDirect, IMyEntity targetEntity)
{
Log.DebugLog("Direction == null", Logger.severity.ERROR, condition: Direction == null);
m_gyro.Update();
float minimumMoment = Math.Min(m_gyro.InvertedInertiaMoment.Min(), MaxInverseTensor);
if (minimumMoment <= 0f)
{
// == 0f, not calculated yet. < 0f, we have math failure
StopRotate();
Log.DebugLog("minimumMoment < 0f", Logger.severity.FATAL, condition: minimumMoment < 0f);
return;
}
localMatrix.M41 = 0; localMatrix.M42 = 0; localMatrix.M43 = 0; localMatrix.M44 = 1;
Matrix inverted; Matrix.Invert(ref localMatrix, out inverted);
localMatrix = localMatrix.GetOrientation();
inverted = inverted.GetOrientation();
Vector3 localDirect = Direction.ToLocalNormalized();
Vector3 rotBlockDirect; Vector3.Transform(ref localDirect, ref inverted, out rotBlockDirect);
float azimuth, elevation; Vector3.GetAzimuthAndElevation(rotBlockDirect, out azimuth, out elevation);
Vector3 rotaRight = localMatrix.Right;
Vector3 rotaUp = localMatrix.Up;
Vector3 NFR_right = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaRight));
Vector3 NFR_up = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaUp));
Vector3 displacement = -elevation * NFR_right - azimuth * NFR_up;
if (UpDirect != null)
{
Vector3 upLocal = UpDirect.ToLocalNormalized();
Vector3 upRotBlock; Vector3.Transform(ref upLocal, ref inverted, out upRotBlock);
upRotBlock.Z = 0f;
upRotBlock.Normalize();
float roll = Math.Sign(upRotBlock.X) * (float)Math.Acos(MathHelper.Clamp(upRotBlock.Y, -1f, 1f));
Vector3 rotaBackward = localMatrix.Backward;
Vector3 NFR_backward = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaBackward));
//Log.DebugLog("upLocal: " + upLocal + ", upRotBlock: " + upRotBlock + ", roll: " + roll + ", displacement: " + displacement + ", NFR_backward: " + NFR_backward + ", change: " + roll * NFR_backward, "in_CalcRotate()");
displacement += roll * NFR_backward;
}
m_lastMoveAttempt = Globals.UpdateCount;
RotateCheck.TestRotate(displacement);
float distanceAngle = displacement.Length();
//if (distanceAngle < m_bestAngle || float.IsNaN(NavSet.Settings_Current.DistanceAngle))
//{
// m_bestAngle = distanceAngle;
// if (RotateCheck.ObstructingEntity == null)
// m_lastAccel = Globals.UpdateCount;
//}
NavSet.Settings_Task_NavWay.DistanceAngle = distanceAngle;
//Log.DebugLog("localDirect: " + localDirect + ", rotBlockDirect: " + rotBlockDirect + ", elevation: " + elevation + ", NFR_right: " + NFR_right + ", azimuth: " + azimuth + ", NFR_up: " + NFR_up + ", disp: " + displacement, "in_CalcRotate()");
m_rotateTargetVelocity = MaxAngleVelocity(displacement, minimumMoment, targetEntity != null);
// adjustment to face a moving entity
if (targetEntity != null)
{
Vector3 relativeLinearVelocity = (targetEntity.GetLinearVelocity() - LinearVelocity) * 1.1f;
float distance = Vector3.Distance(targetEntity.GetCentre(), Block.CubeBlock.GetPosition());
//Log.DebugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", tangentialVelocity: " + tangentialVelocity + ", localTangVel: " + localTangVel, "in_CalcRotate()");
float RLV_pitch = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Down);
float RLV_yaw = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Right);
float angl_pitch = (float)Math.Atan2(RLV_pitch, distance);
float angl_yaw = (float)Math.Atan2(RLV_yaw, distance);
Log.DebugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", RLV_yaw: " + RLV_yaw + ", RLV_pitch: " + RLV_pitch + ", angl_yaw: " + angl_yaw + ", angl_pitch: " + angl_pitch + ", total adjustment: " + (NFR_right * angl_pitch + NFR_up * angl_yaw));
m_rotateTargetVelocity += NFR_right * angl_pitch + NFR_up * angl_yaw;
}
//Log.DebugLog("targetVelocity: " + m_rotateTargetVelocity, "in_CalcRotate()");
if (RotateCheck.ObstructingEntity != null)
{
float maxVel = (float)Math.Atan2(1d, Block.CubeGrid.LocalVolume.Radius);
float lenSq = m_rotateTargetVelocity.LengthSquared();
if (lenSq > maxVel)
{
Log.DebugLog("Reducing target velocity from " + Math.Sqrt(lenSq) + " to " + maxVel);
Vector3 normVel; Vector3.Divide(ref m_rotateTargetVelocity, (float)Math.Sqrt(lenSq), out normVel);
Vector3.Multiply(ref normVel, maxVel, out m_rotateTargetVelocity);
}
}
// angular velocity is reversed
Vector3 angularVelocity = AngularVelocity.ToBlock(Block.CubeBlock); // ((DirectionWorld)(-Block.Physics.AngularVelocity)).ToBlock(Block.CubeBlock);
m_rotateForceRatio = (m_rotateTargetVelocity + angularVelocity) / (minimumMoment * m_gyro.GyroForce);
//Log.DebugLog("targetVelocity: " + m_rotateTargetVelocity + ", angularVelocity: " + angularVelocity + ", accel: " + (m_rotateTargetVelocity + angularVelocity));
//Log.DebugLog("minimumMoment: " + minimumMoment + ", force: " + m_gyro.GyroForce + ", rotateForceRatio: " + m_rotateForceRatio);
// dampeners
for (int index = 0; index < 3; index++)
{
// if targetVelocity is close to 0, use dampeners
float target = m_rotateTargetVelocity.GetDim(index);
if (target > -0.01f && target < 0.01f)
{
//Log.DebugLog("target near 0 for " + i + ", " + target, "in_CalcRotate()");
m_rotateTargetVelocity.SetDim(index, 0f);
m_rotateForceRatio.SetDim(index, 0f);
continue;
}
}
}
// necessary wrapper for main CalcRotate, should always be called.
private void CalcRotate(Matrix localMatrix, RelativeDirection3F Direction, RelativeDirection3F UpDirect = null, bool gravityAdjusted = false, IMyEntity targetEntity = null)
{
//m_logger.debugLog("entered CalcRotate(Matrix localMatrix, RelativeDirection3F Direction, RelativeDirection3F UpDirect = null, bool levelingOff = false, IMyEntity targetEntity = null)", "CalcRotate()");
CheckGrid();
Thrust.Update();
if (!gravityAdjusted && ThrustersOverWorked())
{
CalcRotate_InGravity(Direction);
return;
}
in_CalcRotate(localMatrix, Direction, UpDirect, targetEntity);
}
public void Rotate()
{
if (isMiningPlanet)
{
switch (m_state)
{
case State.Approaching:
m_mover.CalcRotate();
return;
case State.Rotating:
if (m_navSet.DirectionMatched())
{
m_logger.debugLog("Finished rotating", Logger.severity.INFO);
m_state = State.MoveTo;
m_mover.StopRotate();
return;
}
break;
}
m_mover.Thrust.Update();
m_mover.CalcRotate(m_navDrill, RelativeDirection3F.FromWorld(m_controlBlock.CubeGrid, m_mover.Thrust.WorldGravity.vector));
return;
}
switch (m_state)
{
case State.Approaching:
if (m_navSet.DistanceLessThan(m_longestDimension))
{
m_logger.debugLog("closer to destination than longest dim");
m_mover.StopRotate();
}
else
{
m_mover.CalcRotate();
}
return;
case State.GetTarget:
case State.Mining_Escape:
case State.Move_Away:
m_logger.debugLog("no rotation");
m_mover.StopRotate();
return;
case State.MoveTo:
case State.Mining:
case State.Mining_Tunnel:
if (m_navSet.DistanceLessThan(3f))
{
m_mover.StopRotate();
return;
}
break;
case State.Rotating:
if (m_navSet.DirectionMatched())
{
m_logger.debugLog("Finished rotating", Logger.severity.INFO);
m_state = State.MoveTo;
m_mover.StopRotate();
return;
}
break;
default:
throw new Exception("case not implemented: " + m_state);
}
if (m_navDrill.FunctionalBlocks == 0)
{
m_logger.debugLog("no functional blocks, cannot rotate");
m_mover.StopRotate();
}
else
{
m_logger.debugLog("rotate to face " + m_currentTarget);
m_mover.CalcRotate(m_navDrill, RelativeDirection3F.FromWorld(m_controlBlock.CubeGrid, m_currentTarget - m_navDrill.WorldPosition));
}
}
/// <summary>
/// Calculates the rotation to face the rotation block towards the target.
/// </summary>
public override void Rotate()
{
m_mover.CalcRotate(m_pseudoBlock, RelativeDirection3F.FromWorld(m_pseudoBlock.Block.CubeGrid, TargetDirection()));
}
