// 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);
}
/// <summary>
/// Calculates the force necessary to move the grid.
/// </summary>
/// <param name="block">To get world position from.</param>
/// <param name="destPoint">The world position of the destination</param>
/// <param name="destVelocity">The speed of the destination</param>
/// <param name="landing">Puts an emphasis on not overshooting the target.</param>
public void CalcMove(PseudoBlock block, ref Vector3 destDisp, ref Vector3 destVelocity)
{
Log.DebugLog("Not on autopilot thread: " + ThreadTracker.ThreadName, Logger.severity.ERROR, condition: !ThreadTracker.ThreadName.StartsWith("Autopilot"));
CheckGrid();
m_lastMoveAttempt = Globals.UpdateCount;
Thrust.Update();
// using local vectors
Vector3 velocity = LinearVelocity;
MatrixD positionToLocal = Block.CubeBlock.WorldMatrixNormalizedInv;
MatrixD directionToLocal = positionToLocal.GetOrientation();
destVelocity = Vector3.Transform(destVelocity, directionToLocal);
velocity = Vector3.Transform(velocity, directionToLocal);
Vector3 targetVelocity;
//float distance;
if (destDisp.LengthSquared() > 0.01f)
{
destDisp = Vector3.Transform(destDisp, directionToLocal);
float distance = destDisp.Length();
targetVelocity = MaximumVelocity(destDisp);
// project targetVelocity onto destination direction (take shortest path)
Vector3 destDir = destDisp / distance;
targetVelocity = Vector3.Dot(targetVelocity, destDir) * destDir;
// apply relative speed limit
float relSpeedLimit = NavSet.Settings_Current.SpeedMaxRelative;
float landingSpeed = Math.Max(distance * DistanceSpeedFactor, DistanceSpeedFactor);
if (relSpeedLimit > landingSpeed)
{
relSpeedLimit = landingSpeed;
}
if (relSpeedLimit < float.MaxValue)
{
float tarSpeedSq_1 = targetVelocity.LengthSquared();
if (tarSpeedSq_1 > relSpeedLimit * relSpeedLimit)
{
targetVelocity *= relSpeedLimit / (float)Math.Sqrt(tarSpeedSq_1);
//Log.DebugLog("imposing relative speed limit: " + relSpeedLimit + ", targetVelocity: " + targetVelocity, "CalcMove()");
}
}
}
else
{
targetVelocity = Vector3.Zero;
}
targetVelocity += destVelocity;
// apply speed limit
float tarSpeedSq = targetVelocity.LengthSquared();
float speedRequest = NavSet.Settings_Current.SpeedTarget;
if (tarSpeedSq > speedRequest * speedRequest)
{
targetVelocity *= speedRequest / (float)Math.Sqrt(tarSpeedSq);
//Log.DebugLog("imposing speed limit: " + speedRequest + ", targetVelocity: " + targetVelocity, "CalcMove()");
}
m_moveAccel = targetVelocity - velocity;
if (m_moveAccel.LengthSquared() < 0.01f)
{
Log.DebugLog("Wriggle unstuck autopilot. Near target velocity, move accel: " + m_moveAccel + ". m_lastMoveAccel set to now", condition: (Globals.UpdateCount - m_lastAccel) > WriggleAfter);
m_lastMoveAccel = m_moveAccel;
m_lastAccel = Globals.UpdateCount;
}
else
{
float diffSq; Vector3.DistanceSquared(ref m_moveAccel, ref m_lastMoveAccel, out diffSq);
if (diffSq > 1f)
{
Log.DebugLog("Wriggle unstuck autopilot. Change in move accel from " + m_lastMoveAccel + " to " + m_moveAccel + ". m_lastMoveAccel set to now", condition: (Globals.UpdateCount - m_lastAccel) > WriggleAfter);
m_lastMoveAccel = m_moveAccel;
m_lastAccel = Globals.UpdateCount;
}
}
CalcMove(ref velocity);
Log.TraceLog(string.Empty
//+ "block: " + block.Block.getBestName()
//+ ", dest point: " + destPoint
//+ ", position: " + block.WorldPosition
+ "destDisp: " + destDisp
+ ", destVelocity: " + destVelocity
+ ", targetVelocity: " + targetVelocity
+ ", velocity: " + velocity
+ ", m_moveAccel: " + m_moveAccel
+ ", moveForceRatio: " + m_moveForceRatio);
}