123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174 |
- using UnityEngine;
- using System.Collections;
- namespace Pathfinding.Util {
- public static class MovementUtilities {
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- public static Vector2 ClampVelocity (Vector2 velocity, float maxSpeed, float slowdownFactor, bool slowWhenNotFacingTarget, Vector2 forward) {
-
- var currentMaxSpeed = maxSpeed * slowdownFactor;
-
- if (slowWhenNotFacingTarget && (forward.x != 0 || forward.y != 0)) {
- float currentSpeed;
- var normalizedVelocity = VectorMath.Normalize(velocity, out currentSpeed);
- float dot = Vector2.Dot(normalizedVelocity, forward);
-
-
-
- float directionSpeedFactor = Mathf.Clamp(dot+0.707f, 0.2f, 1.0f);
- currentMaxSpeed *= directionSpeedFactor;
- currentSpeed = Mathf.Min(currentSpeed, currentMaxSpeed);
-
- float angle = Mathf.Acos(Mathf.Clamp(dot, -1, 1));
-
-
-
-
-
-
- angle = Mathf.Min(angle, (20f + 180f*(1 - slowdownFactor*slowdownFactor))*Mathf.Deg2Rad);
- float sin = Mathf.Sin(angle);
- float cos = Mathf.Cos(angle);
-
- sin *= Mathf.Sign(normalizedVelocity.x*forward.y - normalizedVelocity.y*forward.x);
-
-
-
- return new Vector2(forward.x*cos + forward.y*sin, forward.y*cos - forward.x*sin) * currentSpeed;
- } else {
- return Vector2.ClampMagnitude(velocity, currentMaxSpeed);
- }
- }
-
- public static Vector2 CalculateAccelerationToReachPoint (Vector2 deltaPosition, Vector2 targetVelocity, Vector2 currentVelocity, float forwardsAcceleration, float rotationSpeed, float maxSpeed, Vector2 forwardsVector) {
-
- if (forwardsAcceleration <= 0) return Vector2.zero;
- float currentSpeed = currentVelocity.magnitude;
-
-
- var sidewaysAcceleration = currentSpeed * rotationSpeed * Mathf.Deg2Rad;
-
-
- sidewaysAcceleration = Mathf.Max(sidewaysAcceleration, forwardsAcceleration);
-
-
- 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 (targetVelocity == Vector2.zero) {
-
- float mn = 0.01f;
- float mx = 10;
- while (mx - mn > 0.01f) {
- var time = (mx + mn) * 0.5f;
-
-
-
-
-
-
-
-
-
- var a = (6*deltaPosition - 4*time*currentVelocity)/(time*time);
- var q = 6*(time*currentVelocity - 2*deltaPosition)/(time*time*time);
-
-
-
-
-
-
- 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);
-
- {
-
-
-
-
-
-
- const float Boost = 1;
- finalAcceleration.y *= 1 + Boost;
-
-
- 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 {
-
-
-
- const float TargetVelocityWeight = 0.5f;
-
-
- 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);
-
-
-
- const float TimeToReachDesiredVelocity = 0.1f;
-
- var finalAcceleration = (targetPoint.normalized*maxSpeed - currentVelocity) * (1f/TimeToReachDesiredVelocity);
-
-
- 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);
- }
- }
- }
- }
|