1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
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);
}
}
}
}
|
