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