// Token: 0x06002966 RID: 10598 RVA: 0x001C0DF0 File Offset: 0x001BEFF0
public static Vector2 CalculateAccelerationToReachPoint(Vector2 deltaPosition, Vector2 targetVelocity, Vector2 currentVelocity, float forwardsAcceleration, float rotationSpeed, float maxSpeed, Vector2 forwardsVector)
{
if (forwardsAcceleration <= 0f)
{
return(Vector2.zero);
}
float magnitude = currentVelocity.magnitude;
float a = magnitude * rotationSpeed * 0.0174532924f;
a = Mathf.Max(a, forwardsAcceleration);
deltaPosition = VectorMath.ComplexMultiplyConjugate(deltaPosition, forwardsVector);
targetVelocity = VectorMath.ComplexMultiplyConjugate(targetVelocity, forwardsVector);
currentVelocity = VectorMath.ComplexMultiplyConjugate(currentVelocity, forwardsVector);
float num = 1f / (forwardsAcceleration * forwardsAcceleration);
float num2 = 1f / (forwardsAcceleration * forwardsAcceleration);
if (targetVelocity == Vector2.zero)
{
float num3 = 0.01f;
float num4 = 10f;
while (num4 - num3 > 0.01f)
{
float num5 = (num4 + num3) * 0.5f;
Vector2 vector = (6f * deltaPosition - 4f * num5 * currentVelocity) / (num5 * num5);
Vector2 a2 = 6f * (num5 * currentVelocity - 2f * deltaPosition) / (num5 * num5 * num5);
Vector2 vector2 = vector + a2 * num5;
if (vector.x * vector.x * num + vector.y * vector.y * num2 > 1f || vector2.x * vector2.x * num + vector2.y * vector2.y * num2 > 1f)
{
num3 = num5;
}
else
{
num4 = num5;
}
}
Vector2 vector3 = (6f * deltaPosition - 4f * num4 * currentVelocity) / (num4 * num4);
vector3.y *= 2f;
float num6 = vector3.x * vector3.x * num + vector3.y * vector3.y * num2;
if (num6 > 1f)
{
vector3 /= Mathf.Sqrt(num6);
}
return(VectorMath.ComplexMultiply(vector3, forwardsVector));
}
float num7;
Vector2 a3 = VectorMath.Normalize(targetVelocity, out num7);
float magnitude2 = deltaPosition.magnitude;
Vector2 vector4 = ((deltaPosition - a3 * Math.Min(0.5f * magnitude2 * num7 / (magnitude + num7), maxSpeed * 1.5f)).normalized * maxSpeed - currentVelocity) * 10f;
float num8 = vector4.x * vector4.x * num + vector4.y * vector4.y * num2;
if (num8 > 1f)
{
vector4 /= Mathf.Sqrt(num8);
}
return(VectorMath.ComplexMultiply(vector4, forwardsVector));
}
// Token: 0x06002DF4 RID: 11764 RVA: 0x001D5714 File Offset: 0x001D3914
public static Agent.VO SegmentObstacle(Vector2 segmentStart, Vector2 segmentEnd, Vector2 offset, float radius, float inverseDt, float inverseDeltaTime)
{
Agent.VO vo = default(Agent.VO);
vo.weightFactor = 1f;
vo.weightBonus = Mathf.Max(radius, 1f) * 40f;
Vector3 vector = VectorMath.ClosestPointOnSegment(segmentStart, segmentEnd, Vector2.zero);
if (vector.magnitude <= radius)
{
vo.colliding = true;
vo.line1 = vector.normalized * (vector.magnitude - radius) * 0.3f * inverseDeltaTime;
vo.dir1 = new Vector2(vo.line1.y, -vo.line1.x).normalized;
vo.line1 += offset;
vo.cutoffDir = Vector2.zero;
vo.cutoffLine = Vector2.zero;
vo.dir2 = Vector2.zero;
vo.line2 = Vector2.zero;
vo.radius = 0f;
vo.segmentStart = Vector2.zero;
vo.segmentEnd = Vector2.zero;
vo.segment = false;
}
else
{
vo.colliding = false;
segmentStart *= inverseDt;
segmentEnd *= inverseDt;
radius *= inverseDt;
Vector2 normalized = (segmentEnd - segmentStart).normalized;
vo.cutoffDir = normalized;
vo.cutoffLine = segmentStart + new Vector2(-normalized.y, normalized.x) * radius;
vo.cutoffLine += offset;
float sqrMagnitude = segmentStart.sqrMagnitude;
Vector2 vector2 = -VectorMath.ComplexMultiply(segmentStart, new Vector2(radius, Mathf.Sqrt(Mathf.Max(0f, sqrMagnitude - radius * radius)))) / sqrMagnitude;
float sqrMagnitude2 = segmentEnd.sqrMagnitude;
Vector2 vector3 = -VectorMath.ComplexMultiply(segmentEnd, new Vector2(radius, -Mathf.Sqrt(Mathf.Max(0f, sqrMagnitude2 - radius * radius)))) / sqrMagnitude2;
vo.line1 = segmentStart + vector2 * radius + offset;
vo.line2 = segmentEnd + vector3 * radius + offset;
vo.dir1 = new Vector2(vector2.y, -vector2.x);
vo.dir2 = new Vector2(vector3.y, -vector3.x);
vo.segmentStart = segmentStart;
vo.segmentEnd = segmentEnd;
vo.radius = radius;
vo.segment = true;
}
return(vo);
}
/// <summary>
/// Creates a VO for avoiding another agent.
/// Note that the segment is directed, the agent will want to be on the left side of the segment.
/// </summary>
public static VO SegmentObstacle(Vector2 segmentStart, Vector2 segmentEnd, Vector2 offset, float radius, float inverseDt, float inverseDeltaTime)
{
var vo = new VO();
// Adjusted so that a parameter weightFactor of 1 will be the default ("natural") weight factor
vo.weightFactor = 1;
// Just higher than anything else
vo.weightBonus = Mathf.Max(radius, 1) * 40;
var closestOnSegment = VectorMath.ClosestPointOnSegment(segmentStart, segmentEnd, Vector2.zero);
// Collision?
if (closestOnSegment.magnitude <= radius)
{
vo.colliding = true;
vo.line1 = closestOnSegment.normalized * (closestOnSegment.magnitude - radius) * 0.3f * inverseDeltaTime;
vo.dir1 = new Vector2(vo.line1.y, -vo.line1.x).normalized;
vo.line1 += offset;
vo.cutoffDir = Vector2.zero;
vo.cutoffLine = Vector2.zero;
vo.dir2 = Vector2.zero;
vo.line2 = Vector2.zero;
vo.radius = 0;
vo.segmentStart = Vector2.zero;
vo.segmentEnd = Vector2.zero;
vo.segment = false;
}
else
{
vo.colliding = false;
segmentStart *= inverseDt;
segmentEnd *= inverseDt;
radius *= inverseDt;
var cutoffTangent = (segmentEnd - segmentStart).normalized;
vo.cutoffDir = cutoffTangent;
vo.cutoffLine = segmentStart + new Vector2(-cutoffTangent.y, cutoffTangent.x) * radius;
vo.cutoffLine += offset;
// See documentation for details
// The call to Max is just to prevent floating point errors causing NaNs to appear
var startSqrMagnitude = segmentStart.sqrMagnitude;
var normal1 = -VectorMath.ComplexMultiply(segmentStart, new Vector2(radius, Mathf.Sqrt(Mathf.Max(0, startSqrMagnitude - radius * radius)))) / startSqrMagnitude;
var endSqrMagnitude = segmentEnd.sqrMagnitude;
var normal2 = -VectorMath.ComplexMultiply(segmentEnd, new Vector2(radius, -Mathf.Sqrt(Mathf.Max(0, endSqrMagnitude - radius * radius)))) / endSqrMagnitude;
vo.line1 = segmentStart + normal1 * radius + offset;
vo.line2 = segmentEnd + normal2 * radius + offset;
// Note that the normals are already normalized
vo.dir1 = new Vector2(normal1.y, -normal1.x);
vo.dir2 = new Vector2(normal2.y, -normal2.x);
vo.segmentStart = segmentStart;
vo.segmentEnd = segmentEnd;
vo.radius = radius;
vo.segment = true;
}
return(vo);
}
/** Calculate an acceleration to move deltaPosition units and get there with approximately a velocity of targetVelocity */
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);
sidewaysAcceleration = forwardsAcceleration;
// Transform coordinates to local space where +X is the forwards direction
// This is essentially equivalent to Transform.InverseTransformDirection.
deltaPosition = VectorMath.ComplexMultiplyConjugate(deltaPosition.ToPFV2(), forwardsVector.ToPFV2()).ToUnityV2();
targetVelocity = VectorMath.ComplexMultiplyConjugate(targetVelocity.ToPFV2(), forwardsVector.ToPFV2()).ToUnityV2();
currentVelocity = VectorMath.ComplexMultiplyConjugate(currentVelocity.ToPFV2(), forwardsVector.ToPFV2()).ToUnityV2();
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.ToPFV2(), forwardsVector.ToPFV2()).ToUnityV2());
}
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.ToPFV2(), out targetSpeed);
var distance = deltaPosition.magnitude;
var targetPoint = deltaPosition.ToPFV2() - 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.ToPFV2()) * (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.ToPFV2()).ToUnityV2());
}
}