/// <summary>
/// Try to face in the target direction. Must execute on game thread.
/// </summary>
public void FaceTowards(DirectionWorld target)
{
Log.DebugLog("Not server!", Logger.severity.FATAL, condition: !MyAPIGateway.Multiplayer.IsServer);
Debug.Assert(Threading.ThreadTracker.IsGameThread, "not game thread");
if (!m_claimedElevation && !m_claimedAzimuth)
{
// nothing to do
return;
}
FaceResult bestResult;
if (!CalcFaceTowards(target, out bestResult))
{
Stop();
return;
}
Log.TraceLog("Face: " + target + ", elevation: " + bestResult.DeltaElevation + ", azimuth: " + bestResult.DeltaAzimuth);
if (m_claimedElevation)
{
SetVelocity(StatorEl, bestResult.DeltaElevation);
}
if (m_claimedAzimuth)
{
SetVelocity(StatorAz, bestResult.DeltaAzimuth);
}
}
private bool CalcFaceTowards_NotClaimed(ref FaceResult bestResult, DirectionWorld target, DirectionWorld faceDirection, float azimuthDelta)
{
if (Math.Abs(azimuthDelta) > m_requiredAccuracyRadians)
{
return(false);
}
float elevationDelta, elevationAccuracy;
if (CalcElevation(target, faceDirection, azimuthDelta, out elevationDelta, out elevationAccuracy))
{
float accSq = azimuthDelta * azimuthDelta + elevationAccuracy * elevationAccuracy;
if (accSq > m_requiredAccuracyRadians * m_requiredAccuracyRadians)
{
return(false);
}
float sumDeltaMag = Math.Abs(elevationDelta);
Log.TraceLog("Best: " + bestResult + ", current: " + new FaceResult()
{
AccuracySquared = accSq, SumDeltaMag = sumDeltaMag, DeltaElevation = elevationDelta, DeltaAzimuth = 0f
});
if (bestResult.ReplaceBy(accSq, sumDeltaMag))
{
bestResult.AccuracySquared = accSq;
bestResult.SumDeltaMag = sumDeltaMag;
bestResult.DeltaAzimuth = 0f;
bestResult.DeltaElevation = elevationDelta;
}
return(true);
}
return(false);
}
private bool CalcElevation(DirectionWorld targetWorldSpace, DirectionWorld faceDirection, float azimuthDelta, out float elevationDelta, out float elevationAccuracy)
{
if (StatorOk(StatorAz) && m_claimedAzimuth)
{
ApplyAzimuthChange(ref faceDirection, azimuthDelta);
}
Vector3 current = faceDirection.ToBlock(StatorEl);
Vector3 target = targetWorldSpace.ToBlock(StatorEl);
Log.TraceLog(nameof(targetWorldSpace) + ": " + targetWorldSpace + ", " + nameof(faceDirection) + ": " + faceDirection + ", " + nameof(target) + ": " + target + ", " + nameof(current) + ": " + current);
float firstDelta, secondDelta;
CalcDelta(current, target, out firstDelta, out secondDelta);
if (m_claimedElevation)
{
// elevation has been claimed, check limits
if (WithinLimits(StatorEl, firstDelta))
{
Log.TraceLog("First elevation delta is reachable: " + firstDelta);
elevationDelta = firstDelta;
elevationAccuracy = 0f;
return(true);
}
if (WithinLimits(StatorEl, secondDelta))
{
Log.TraceLog("Second elevation delta is reachable: " + secondDelta);
elevationDelta = secondDelta;
elevationAccuracy = 0f;
return(true);
}
BestEffort(StatorEl, firstDelta, secondDelta, out elevationDelta, out elevationAccuracy);
if (elevationAccuracy < m_requiredAccuracyRadians)
{
Log.TraceLog("Best effort: " + elevationDelta);
return(true);
}
}
else
{
// elevation not claimed, check that the current elevation is close enough
elevationAccuracy = Math.Abs(firstDelta);
if (elevationAccuracy < m_requiredAccuracyRadians)
{
Log.TraceLog("Elevation within tolerance: " + firstDelta);
elevationDelta = 0f; // not claimed, no rotation
return(true);
}
}
Log.TraceLog("Neither elevation delta acceptable");
elevationDelta = float.NaN;
elevationAccuracy = float.PositiveInfinity;
return(false);
}
private bool CalcFaceTowards(DirectionWorld targetWorldSpace, out FaceResult bestResult)
{
if (StatorOk(StatorAz))
{
return(CalcFaceTowards_AzimuthOk(targetWorldSpace, out bestResult));
}
else
{
return(CalcFaceTowards_NoAzimuth(targetWorldSpace, out bestResult));
}
}
/// <summary>
/// Determine if the motor turret can face the target direction. Thread-safe.
/// </summary>
/// <param name="target">The direction to face.</param>
/// <returns>True iff the turret can face the target direction.</returns>
public bool CanFaceTowards(DirectionWorld target)
{
if (!StatorOk(StatorEl))
{
return(false);
}
FaceResult bestResult;
return(CalcFaceTowards(target, out bestResult));
}
/// <summary>
/// Rotate current direction by azimuth stator's delta.
/// </summary>
/// <param name="facing">The current direction.</param>
/// <param name="azDelta">The change in angle of azimuth stator.</param>
private void ApplyAzimuthChange(ref DirectionWorld facing, float azDelta)
{
Log.TraceLog(nameof(facing) + " is " + facing);
Vector3 axis = StatorAz.WorldMatrix.Down;
Log.TraceLog(nameof(axis) + " is " + axis);
Log.TraceLog(nameof(azDelta) + " is " + azDelta);
Quaternion rotation; Quaternion.CreateFromAxisAngle(ref axis, azDelta, out rotation);
Vector3.Transform(ref facing.vector, ref rotation, out facing.vector);
Log.TraceLog(nameof(facing) + " is " + facing);
}
private bool CalcFaceTowards_Claimed(ref FaceResult bestResult, DirectionWorld target, DirectionWorld faceDirection, float azimuthDelta)
{
float azimuthAccuracy;
if (WithinLimits(StatorAz, azimuthDelta))
{
azimuthAccuracy = 0f;
}
else
{
float clamped = ClampToLimits(StatorAz, azimuthDelta);
azimuthAccuracy = Math.Abs(azimuthDelta - clamped);
if (azimuthAccuracy > m_requiredAccuracyRadians)
{
return(false);
}
azimuthDelta = clamped;
}
float elevationDelta, elevationAccuracy;
if (CalcElevation(target, faceDirection, azimuthDelta, out elevationDelta, out elevationAccuracy))
{
float accSq = azimuthAccuracy * azimuthAccuracy + elevationAccuracy * elevationAccuracy;
float sumDeltaMag = Math.Abs(azimuthDelta) + Math.Abs(elevationDelta);
Log.TraceLog("Best: " + bestResult + ", current: " + new FaceResult()
{
AccuracySquared = accSq, SumDeltaMag = sumDeltaMag, DeltaElevation = elevationDelta, DeltaAzimuth = azimuthDelta
});
if (bestResult.ReplaceBy(accSq, sumDeltaMag))
{
bestResult.AccuracySquared = accSq;
bestResult.SumDeltaMag = sumDeltaMag;
bestResult.DeltaAzimuth = azimuthDelta;
bestResult.DeltaElevation = elevationDelta;
}
return(true);
}
return(false);
}
private bool CalcFaceTowards_NoAzimuth(DirectionWorld targetWorldSpace, out FaceResult bestResult)
{
bestResult = FaceResult.Default;
foreach (var direction in FaceBlock.FaceDirections())
{
DirectionWorld faceDirection = new DirectionWorld()
{
vector = FaceBlock.WorldMatrix.GetDirectionVector(direction)
};
CalcFaceTowards_NotClaimed(ref bestResult, targetWorldSpace, faceDirection, MathHelper.TwoPi);
}
if (bestResult.AccuracySquared != float.PositiveInfinity)
{
Log.TraceLog("Best: " + bestResult);
return(true);
}
Log.TraceLog("Cannot rotate to target");
return(false);
}
private bool CalcFaceTowards_AzimuthOk(DirectionWorld targetWorldSpace, out FaceResult bestResult)
{
bestResult = FaceResult.Default;
Vector3 target = targetWorldSpace.ToBlock(StatorAz);
foreach (var direction in FaceBlock.FaceDirections())
{
DirectionWorld faceDirection = new DirectionWorld()
{
vector = FaceBlock.WorldMatrix.GetDirectionVector(direction)
};
Vector3 current = faceDirection.ToBlock(StatorAz);
float firstDelta, secondDelta;
CalcDelta(current, target, out firstDelta, out secondDelta);
if (m_claimedAzimuth)
{
// azimuth has been claimed, check limits
if (CalcFaceTowards_Claimed(ref bestResult, targetWorldSpace, faceDirection, firstDelta))
{
Log.TraceLog("First azimuth delta is reachable: " + firstDelta);
}
else if (CalcFaceTowards_Claimed(ref bestResult, targetWorldSpace, faceDirection, secondDelta))
{
Log.TraceLog("Second azimuth delta is reachable: " + secondDelta);
}
if (bestResult.AccuracySquared > 0.1f)
{
// try flipped
float clamped = ClampToLimits(StatorAz, firstDelta);
if (clamped > 0f)
{
firstDelta = clamped - MathHelper.Pi;
secondDelta = clamped + MathHelper.Pi;
}
else
{
firstDelta = clamped + MathHelper.Pi;
secondDelta = clamped - MathHelper.Pi;
}
if (CalcFaceTowards_Claimed(ref bestResult, targetWorldSpace, faceDirection, firstDelta))
{
Log.TraceLog("First flipped azimuth delta is reachable: " + firstDelta);
}
else if (CalcFaceTowards_Claimed(ref bestResult, targetWorldSpace, faceDirection, secondDelta))
{
Log.TraceLog("Second flipped azimuth delta is reachable: " + secondDelta);
}
}
}
else if (CalcFaceTowards_NotClaimed(ref bestResult, targetWorldSpace, faceDirection, firstDelta)) // azimuth has not been claimed, check that current azimuth is close enough
{
Log.TraceLog("Azimuth is within tolerance: " + firstDelta);
}
else
{
Log.TraceLog("Azimuth is outside tolerance: " + firstDelta);
}
}
if (bestResult.AccuracySquared != float.PositiveInfinity)
{
Log.TraceLog("Best: " + bestResult);
return(true);
}
Log.TraceLog("Cannot rotate to target");
return(false);
}
