I am currently creating my own movement script based on your AIPath. My game is 2D, and I have copied some of your code, which seems to be working for now. The movement plane is defined by a new Quaternion(-0.67677f, -0.20491f, 0.20491f, 0.67677f). Since my game is in 2D, I would like to know if it is possible to completely remove the movement plane from the calculations.
private static readonly SimpleMovementPlane movementPlane =
new SimpleMovementPlane(new Quaternion(-0.67677f, -0.20491f, 0.20491f, 0.67677f));
public Vector3 MovementUpdate(float deltaTime, float speed, Vector3 simulatedPosition)
{
ZAssert.IsFalse(ReachedDestination);
float currentAcceleration = 2.5f * speed;
var steeringTarget = SkipWayPoints(simulatedPosition);
var dir = movementPlane.ToPlane(steeringTarget - simulatedPosition);
var forwards = movementPlane.ToPlane(Quaternion.identity * Vector3.up);
var distanceToEnd = (simulatedPosition - Path[0]).sqrMagnitude;
if (distanceToEnd < CommonConstant.PathEndReachedDistance)
ReachedDestination = true;
velocity2D +=
CalculateAccelerationToReachPoint(dir, dir.normalized * speed, velocity2D, currentAcceleration,
speed, forwards) * deltaTime;
velocity2D = Vector2.ClampMagnitude(velocity2D, speed);
var delta2D = Vector2.ClampMagnitude(velocity2D * deltaTime, distanceToEnd);
return simulatedPosition + movementPlane.ToWorld(delta2D);
}
private Vector3 SkipPoints(Vector3 currentPosition)
{
while (curPathIndex > 1 &&
VectorMath.ClosestPointOnLineFactor(Path[curPathIndex], Path[curPathIndex - 1], currentPosition) > 1)
{
curPathIndex--;
}
var circleCenter = movementPlane.ToPlane(currentPosition);
while (curPathIndex > 1 &&
(movementPlane.ToPlane(Path[curPathIndex - 1]) - circleCenter).sqrMagnitude <
CommonConstant.PickNextWaypointDistSqr)
{
curPathIndex--;
}
var factor = VectorMath.LineCircleIntersectionFactor(circleCenter,
movementPlane.ToPlane(Path[curPathIndex]), movementPlane.ToPlane(Path[curPathIndex - 1]),
CommonConstant.PickNextWaypointDist);
var t = Math.Clamp(factor, 0, 1);
return Vector3.Lerp(Path[curPathIndex], Path[curPathIndex - 1], t);
}
private Vector2 CalculateAccelerationToReachPoint(Vector2 deltaPosition, Vector2 targetVelocity,
Vector2 currentVelocity, float forwardsAcceleration, float maxSpeed, Vector2 forwardsVector)
{
// Guard against div by zero
if (forwardsAcceleration <= 0) return Vector2.zero;
float currentSpeed = currentVelocity.magnitude;
// Transform coordinates to local space where +X is the forwards direction
// This is essentially equivalent to Transform.InverseTransformDirection.
deltaPosition = VectorMath.ComplexMultiplyConjugate(deltaPosition, forwardsVector);
targetVelocity = VectorMath.ComplexMultiplyConjugate(targetVelocity, forwardsVector);
currentVelocity = VectorMath.ComplexMultiplyConjugate(currentVelocity, forwardsVector);
float ellipseSqrFactor = 1 / (forwardsAcceleration * forwardsAcceleration);
// If the target velocity is zero we can use a more fancy approach
// and calculate a nicer path.
// In particular, this is the case at the end of the path.
if (targetVelocity == Vector2.zero)
{
// Run a binary search over the time to get to the target point.
float mn = 0.01f;
float mx = 10;
while (mx - mn > 0.01f)
{
var time = (mx + mn) * 0.5f;
// Given that we want to move deltaPosition units from out current position, that our current velocity is given
// and that when we reach the target we want our velocity to be zero. Also assume that our acceleration will
// vary linearly during the slowdown. Then we can calculate what our acceleration should be during this frame.
//{ t = time
//{ deltaPosition = vt + at^2/2 + qt^3/6
//{ 0 = v + at + qt^2/2
//{ solve for a
// a = acceleration vector
// q = derivative of the acceleration vector
var a = (6 * deltaPosition - 4 * time * currentVelocity) / (time * time);
var q = 6 * (time * currentVelocity - 2 * deltaPosition) / (time * time * time);
// Make sure the acceleration is not greater than our maximum allowed acceleration.
// If it is we increase the time we want to use to get to the target
// and if it is not, we decrease the time to get there faster.
// Since the acceleration is described by acceleration = a + q*t
// we only need to check at t=0 and t=time.
// Note that the acceleration limit is described by an ellipse, not a circle.
var nextA = a + q * time;
if (a.x * a.x * ellipseSqrFactor + a.y * a.y * ellipseSqrFactor > 1.0f ||
nextA.x * nextA.x * ellipseSqrFactor + nextA.y * nextA.y * ellipseSqrFactor > 1.0f)
{
mn = time;
}
else
{
mx = time;
}
}
var finalAcceleration = (6 * deltaPosition - 4 * mx * currentVelocity) / (mx * mx);
// Boosting
{
// The trajectory calculated above has a tendency to use very wide arcs
// and that does unfortunately not look particularly good in some cases.
// Here we amplify the component of the acceleration that is perpendicular
// to our current velocity. This will make the agent turn towards the
// target quicker.
// How much amplification to use. Value is unitless.
const float Boost = 1;
finalAcceleration.y *= 1 + Boost;
// Clamp the velocity to the maximum acceleration.
// Note that the maximum acceleration constraint is shaped like an ellipse, not like a circle.
float ellipseMagnitude = finalAcceleration.x * finalAcceleration.x * ellipseSqrFactor +
finalAcceleration.y * finalAcceleration.y * ellipseSqrFactor;
if (ellipseMagnitude > 1.0f) finalAcceleration /= Mathf.Sqrt(ellipseMagnitude);
}
return VectorMath.ComplexMultiply(finalAcceleration, forwardsVector);
}
else
{
// Here we try to move towards the next waypoint which has been modified slightly using our
// desired velocity at that point so that the agent will more smoothly round the corner.
// How much to strive for making sure we reach the target point with the target velocity. Unitless.
const float TargetVelocityWeight = 0.5f;
// Limit to how much to care about the target velocity. Value is in seconds.
// This prevents the character from moving away from the path too much when the target point is far away
const float TargetVelocityWeightLimit = 1.5f;
float targetSpeed;
var normalizedTargetVelocity = VectorMath.Normalize(targetVelocity, out targetSpeed);
var distance = deltaPosition.magnitude;
var targetPoint = deltaPosition - normalizedTargetVelocity *
System.Math.Min(TargetVelocityWeight * distance * targetSpeed / (currentSpeed + targetSpeed),
maxSpeed * TargetVelocityWeightLimit);
// How quickly the agent will try to reach the velocity that we want it to have.
// We need this to prevent oscillations and jitter which is what happens if
// we let the constant go towards zero. Value is in seconds.
const float TimeToReachDesiredVelocity = 0.1f;
// TODO: Clamp to ellipse using more accurate acceleration (use rotation speed as well)
var finalAcceleration = (targetPoint.normalized * maxSpeed - currentVelocity) *
(1f / TimeToReachDesiredVelocity);
// Clamp the velocity to the maximum acceleration.
// Note that the maximum acceleration constraint is shaped like an ellipse, not like a circle.
float ellipseMagnitude = finalAcceleration.x * finalAcceleration.x * ellipseSqrFactor +
finalAcceleration.y * finalAcceleration.y * ellipseSqrFactor;
if (ellipseMagnitude > 1.0f) finalAcceleration /= Mathf.Sqrt(ellipseMagnitude);
return VectorMath.ComplexMultiply(finalAcceleration, forwardsVector);
}
}