public override SteeringVelocity DoCalculate(Entity <Game> owner, ref SteeringVelocity accumulatedSteering)
{
var steering = new SteeringVelocity();
if (!owner.Has <FlowField>())
{
return(steering);
}
var flowField = owner.Get <FlowField>().Field;
var flowVector = flowField.LookupFlowVector(owner.Get <Position>().Value);
if (!owner.Has <TargetOrientation>())
{
return(SteeringVelocity.Zero);
}
if (flowVector.MagnitudeSqr <= ZeroVelocity * ZeroVelocity)
{
return(SteeringVelocity.Zero);
}
var targetOrientation = FixMath.Atan2(-flowVector.X, flowVector.Y);
return(DoReachOrientation(owner, targetOrientation));
}
/// <summary>
/// Converts a quaternion to a euler angle.
/// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
/// </summary>
/// <param name="quaternion">The quaternion to convert from.</param>
/// <returns>An euler angle.</returns>
public static FixVec3 ToEuler(FixQuaternion quaternion)
{
FixVec3 result;
Fix t0 = 2 * (quaternion.w * quaternion.z + quaternion.x * quaternion.y);
Fix t1 = Fix.one - (2 * (quaternion.y * quaternion.y + quaternion.z * quaternion.z));
result.z = FixMath.Atan2(t0, t1);
Fix t2 = 2 * (quaternion.w * quaternion.y - quaternion.z * quaternion.x);
if (t2 >= Fix.one)
{
result.y = FixMath.PI / 2;
}
else if (t2 <= -Fix.one)
{
result.y = -(FixMath.PI / 2);
}
else
{
result.y = FixMath.Asin(t2);
}
Fix t3 = 2 * (quaternion.w * quaternion.x + quaternion.y * quaternion.z);
Fix t4 = Fix.one - (2 * (quaternion.x * quaternion.x + quaternion.y * quaternion.y));
result.x = FixMath.Atan2(t3, t4);
return(result);
}
public static Fix MatrixToDegrees(Mat22 m)
{
if (m.m10 == Fix.zero && m.m00 == Fix.zero)
{
return(Fix.zero);
}
return(FixMath.Atan2(m.m10, m.m00));
}
public static DynValue atan2(ScriptExecutionContext executionContext, CallbackArguments args)
{
return(exec2(args, "atan2", (dx, dy) =>
{
if (dx == 0 && dy == 0)
{
return (Fix)0.75; //special case
}
var a = FixMath.Atan2(dy, dx) / 360;
return a > 0 ? 1 - a : -a;
}));
}
public override SteeringVelocity DoCalculate(Entity <Game> owner, ref SteeringVelocity steering)
{
var linearVelocity = steering.Linear;
if (linearVelocity.MagnitudeSqr <= ZeroVelocity * ZeroVelocity)
{
return(SteeringVelocity.Zero);
}
var targetOrientation = FixMath.Atan2(-linearVelocity.X, linearVelocity.Y);
return(DoReachOrientation(owner, targetOrientation));
}
public void AtanAverageDeviationTest()
{
var totalDiff = 0.0;
var maxDiff = double.MinValue;
for (var i = 0; i < 100; i++)
{
for (var j = 0; j < 100; j++)
{
var diff = (double)FixMath.Atan2(i, j) - Math.Atan2(i, j);
totalDiff += diff;
maxDiff = Math.Max(maxDiff, Math.Abs(diff));
}
}
// Max & Average deviation
FixAssert.AssertEquals(maxDiff, 0, 0.012f);
FixAssert.AssertEquals(totalDiff / 360, 0, 0.008f);
}
public override SteeringVelocity DoCalculate(Entity <Game> owner, ref SteeringVelocity steering)
{
var targetLocation = FixVec2.Zero;
if (!owner.TryGetTargetPosition(ref targetLocation))
{
return(SteeringVelocity.Zero);
}
var direction = targetLocation - owner.Get <Position>().Value;
if (direction.MagnitudeSqr <= ZeroVelocity * ZeroVelocity)
{
return(SteeringVelocity.Zero);
}
var targetOrientation = FixMath.Atan2(direction.Y, -direction.X);
return(DoReachOrientation(owner, targetOrientation));
}