update
This commit is contained in:
@@ -0,0 +1,208 @@
|
||||
using UnityEngine;
|
||||
|
||||
namespace Pathfinding.Util {
|
||||
public static class MovementUtilities {
|
||||
public static float FilterRotationDirection (ref Vector2 state, ref Vector2 state2, Vector2 deltaPosition, float threshold, float deltaTime, bool avoidingOtherAgents) {
|
||||
const float Decay = 0.5f;
|
||||
|
||||
var lastState = state;
|
||||
|
||||
if (!avoidingOtherAgents) {
|
||||
// When not avoiding other agents, we can be a bit more aggressive with rotating towards the target.
|
||||
// This is because in that case, the velocity is much less noisy.
|
||||
state += deltaPosition * 10;
|
||||
} else {
|
||||
state += deltaPosition;
|
||||
}
|
||||
|
||||
// Decay the state slowly (has the most effect if the agent is standing still)
|
||||
state *= Mathf.Clamp01(1.0f - deltaTime*Decay);
|
||||
float stateLength = state.magnitude;
|
||||
|
||||
if (stateLength > threshold*2f) {
|
||||
state = state * (threshold*2f/stateLength);
|
||||
stateLength = threshold*2f;
|
||||
}
|
||||
|
||||
// TODO: Figure out what to do with
|
||||
state2 += (state - lastState) * Decay;
|
||||
state2 *= Mathf.Clamp01(1.0f - deltaTime*Decay);
|
||||
|
||||
// Prevent rotation if the agent doesn't move much
|
||||
float speed = stateLength > threshold ? 1.0f : 0.0f;
|
||||
return speed;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Clamps the velocity to the max speed and optionally the forwards direction.
|
||||
///
|
||||
/// Note that all vectors are 2D vectors, not 3D vectors.
|
||||
///
|
||||
/// Returns: The clamped velocity in world units per second.
|
||||
/// </summary>
|
||||
/// <param name="velocity">Desired velocity of the character. In world units per second.</param>
|
||||
/// <param name="maxSpeed">Max speed of the character. In world units per second.</param>
|
||||
/// <param name="speedLimitFactor">Value between 0 and 1 which determines how much slower the character should move than normal.
|
||||
/// Normally 1 but should go to 0 when the character approaches the end of the path.</param>
|
||||
/// <param name="slowWhenNotFacingTarget">Slow the character down if the desired velocity is not in the same direction as the forward vector.</param>
|
||||
/// <param name="preventMovingBackwards">Prevent the velocity from being too far away from the forward direction of the character.</param>
|
||||
/// <param name="forward">Forward direction of the character. Used together with the slowWhenNotFacingTarget parameter.</param>
|
||||
public static Vector2 ClampVelocity (Vector2 velocity, float maxSpeed, float speedLimitFactor, bool slowWhenNotFacingTarget, bool preventMovingBackwards, Vector2 forward) {
|
||||
// Max speed to use for this frame
|
||||
var currentMaxSpeed = maxSpeed * speedLimitFactor;
|
||||
|
||||
// Check if the agent should slow down in case it is not facing the direction it wants to move in
|
||||
if (slowWhenNotFacingTarget && (forward.x != 0 || forward.y != 0)) {
|
||||
float currentSpeed;
|
||||
var normalizedVelocity = VectorMath.Normalize(velocity, out currentSpeed);
|
||||
float dot = Vector2.Dot(normalizedVelocity, forward);
|
||||
|
||||
// Lower the speed when the character's forward direction is not pointing towards the desired velocity
|
||||
// 1 when velocity is in the same direction as forward
|
||||
// 0.2 when they point in the opposite directions
|
||||
float directionSpeedFactor = Mathf.Clamp(dot+0.707f, 0.2f, 1.0f);
|
||||
currentMaxSpeed *= directionSpeedFactor;
|
||||
currentSpeed = Mathf.Min(currentSpeed, currentMaxSpeed);
|
||||
|
||||
if (preventMovingBackwards) {
|
||||
// Angle between the forwards direction of the character and our desired velocity
|
||||
float angle = Mathf.Acos(Mathf.Clamp(dot, -1, 1));
|
||||
|
||||
// Clamp the angle to 20 degrees
|
||||
// We cannot keep the velocity exactly in the forwards direction of the character
|
||||
// because we use the rotation to determine in which direction to rotate and if
|
||||
// the velocity would always be in the forwards direction of the character then
|
||||
// the character would never rotate.
|
||||
// Allow larger angles when near the end of the path to prevent oscillations.
|
||||
angle = Mathf.Min(angle, (20f + 180f*(1 - speedLimitFactor*speedLimitFactor))*Mathf.Deg2Rad);
|
||||
|
||||
float sin = Mathf.Sin(angle);
|
||||
float cos = Mathf.Cos(angle);
|
||||
|
||||
// Determine if we should rotate clockwise or counter-clockwise to move towards the current velocity
|
||||
sin *= Mathf.Sign(normalizedVelocity.x*forward.y - normalizedVelocity.y*forward.x);
|
||||
// Rotate the #forward vector by #angle radians
|
||||
// The rotation is done using an inlined rotation matrix.
|
||||
// See https://en.wikipedia.org/wiki/Rotation_matrix
|
||||
return new Vector2(forward.x*cos + forward.y*sin, forward.y*cos - forward.x*sin) * currentSpeed;
|
||||
} else {
|
||||
return normalizedVelocity * currentSpeed;
|
||||
}
|
||||
} else {
|
||||
return Vector2.ClampMagnitude(velocity, currentMaxSpeed);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>Calculate an acceleration to move deltaPosition units and get there with approximately a velocity of targetVelocity</summary>
|
||||
public static Vector2 CalculateAccelerationToReachPoint (Vector2 deltaPosition, Vector2 targetVelocity, Vector2 currentVelocity, float forwardsAcceleration, float rotationSpeed, float maxSpeed, Vector2 forwardsVector) {
|
||||
// Guard against div by zero
|
||||
if (forwardsAcceleration <= 0) return Vector2.zero;
|
||||
|
||||
float currentSpeed = currentVelocity.magnitude;
|
||||
|
||||
// Convert rotation speed to an acceleration
|
||||
// See https://en.wikipedia.org/wiki/Centripetal_force
|
||||
var sidewaysAcceleration = currentSpeed * rotationSpeed * Mathf.Deg2Rad;
|
||||
|
||||
// To avoid weird behaviour when the rotation speed is very low we allow the agent to accelerate sideways without rotating much
|
||||
// if the rotation speed is very small. Also guards against division by zero.
|
||||
sidewaysAcceleration = Mathf.Max(sidewaysAcceleration, forwardsAcceleration);
|
||||
|
||||
// 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 ellipseSqrFactorX = 1 / (forwardsAcceleration*forwardsAcceleration);
|
||||
float ellipseSqrFactorY = 1 / (sidewaysAcceleration*sidewaysAcceleration);
|
||||
|
||||
// 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*ellipseSqrFactorX + a.y*a.y*ellipseSqrFactorY > 1.0f || nextA.x*nextA.x*ellipseSqrFactorX + nextA.y*nextA.y*ellipseSqrFactorY > 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*ellipseSqrFactorX + finalAcceleration.y*finalAcceleration.y*ellipseSqrFactorY;
|
||||
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*ellipseSqrFactorX + finalAcceleration.y*finalAcceleration.y*ellipseSqrFactorY;
|
||||
if (ellipseMagnitude > 1.0f) finalAcceleration /= Mathf.Sqrt(ellipseMagnitude);
|
||||
|
||||
return VectorMath.ComplexMultiply(finalAcceleration, forwardsVector);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user