/// <summary>
/// Calculates position with variable linear speed and zero angular speed
/// </summary>
/// <param name="velocity">Linear velocity</param>
/// <param name="acceleration">Linear acceleration</param>
/// <param name="position">Position</param>
/// <param name="facing">Angle</param>
/// <param name="time">Time</param>
public static void GetVarLinearPosition(ref Vector2 velocity, float maxVelocity, Vector2 accelerationDirection, float acceleration, ref Vector2 position, float facing, float time)
{
float exp = (float)Math.Exp(-acceleration * time / maxVelocity);
Vector2 endVelocity = accelerationDirection * maxVelocity * (1 - exp) + velocity * exp;
float k = maxVelocity * (1 - exp) / acceleration;
position += accelerationDirection * maxVelocity * (time - k) + velocity * k;
velocity = endVelocity;
}
/// <summary>
/// Calculates position with variable linear speed and constant angular speed
/// </summary>
/// <returns>The variable linear constant angular shift.</returns>
/// <param name="velocity">Linear velocity</param>
/// <param name="acceleration">Linear acceleration</param>
/// <param name="position">Position</param>
/// <param name="angularVelocity">Constant angular velocity</param>
/// <param name="facing">Angle</param>
/// <param name="time">Time</param>
public static void GetVarLinearConstantAngularPosition(ref Vector2 velocity, float maxVelocity, Vector2 accelerationDirection, float acceleration, ref Vector2 position, float angularVelocity, ref float facing, float time)
{
if (Math.Abs(angularVelocity) < Epsilon)
{
GetVarLinearPosition(ref velocity, maxVelocity, accelerationDirection, acceleration, ref position, facing, time);
return;
}
float sinb;
float cosb;
SinCos(facing, out sinb, out cosb);
float endFacing = facing + angularVelocity * time;
float sinend;
float cosend;
SinCos(endFacing, out sinend, out cosend);
float exp = (float)Math.Exp(-acceleration * time / maxVelocity);
float vw = maxVelocity * angularVelocity;
float divider = maxVelocity * maxVelocity / (acceleration * acceleration + vw * vw);
float vx = maxVelocity * cosend - (maxVelocity * cosend - velocity.X) * exp;
float vy = maxVelocity * sinend - (maxVelocity * sinend - velocity.Y) * exp;
Vector2 endVelocity = new Vector2(vx, vy);
Vector2 shift = velocity * maxVelocity * (1 - exp) / acceleration;
float x =
(-sinb + sinend) * maxVelocity / angularVelocity +
divider * (exp * (acceleration * cosend - vw * sinend) - (acceleration * cosb - vw * sinb));
float y =
(cosb - cosend) * maxVelocity / angularVelocity -
divider * (-exp * (acceleration * sinend + vw * cosend) + (acceleration * sinb + vw * cosb));
position += shift + new Vector2(x, y);
velocity = endVelocity;
facing = endFacing;
}
/// <summary>
/// Calculates average movement time to the point
/// </summary>
/// <returns>The average time.</returns>
/// <param name="velocity">Velocity.</param>
/// <param name="maxVelocity">Max velocity.</param>
/// <param name="position">Position.</param>
/// <param name="target">Target.</param>
public static float GetAverageMoveTime(Vector velocity, float maxVelocity, float damping, float acceleration, Vector position, Vector target, float epsilon)
{
Vector direction = (target - position);
float distance = direction.Normalize();
float v = Vector.Dot(velocity, direction);
float dampingDistance = maxVelocity * maxVelocity * s_oneMinusLog2 / damping;
float t;
if (distance > dampingDistance)
{
t = (distance - dampingDistance) / maxVelocity + (maxVelocity - v) / acceleration; // rought acceleration time, will be smaller than actual time
}
else
{
t = 0.1f;
}
float ndistance;
float u;
int iterations = 10; // limit the cycle
do
{
float exp = (float)Math.Exp(-acceleration * t / maxVelocity);
u = maxVelocity - (maxVelocity - v) * exp;
ndistance = maxVelocity * (t - (u - v) / acceleration) + GetDampingDistance(u, maxVelocity, damping);
if (ndistance >= distance + epsilon) // average goal
{
break;
}
t += 0.1f;
} while(ndistance < distance && --iterations >= 0);
return(t + maxVelocity * (float)Math.Log(1.0f + u / maxVelocity) / damping);
}