private void UpdateMovement()
{
if (AntMath.Distance(_currentPoint, (Vector2)_t.position) < 0.5f)
{
// Arrived to the current way point.
_pointIndex++;
if (_pointIndex < _route.Count)
{
// Move to next one.
_currentPoint = _route[_pointIndex];
}
else
{
// This is end of the way.
float dist = AntMath.Distance(_currentPoint, (Vector2)_t.position);
if (dist < 0.5f)
{
// Enable break.
_speed = AntMath.Lerp(_speed, 0.0f, 1.0f - breakCurve.Evaluate(dist / 0.5f));
_isBrake = true;
}
if (AntMath.Equal(_speed, 0.0f, 0.1f))
{
// Absolutely arrived.
StopMove();
if (EventArrived != null)
{
EventArrived(this);
}
}
}
_steeringTime = 0.0f;
_isSteering = false;
}
float targetAngle = AntMath.Angle(AntMath.AngleDeg(_t.position, _currentPoint));
_debugTargetAngle = targetAngle * AntMath.RADIANS;
float angleDiff = AntMath.AngleDifference(Angle, targetAngle) * AntMath.DEGREES;
// If our direction incorrect.
if (!AntMath.Equal(angleDiff, 0.0f, 0.01f) && !_isSteering)
{
// Correct our angle to the current way point.
_isSteering = true;
_steeringTime = 0.0f;
_totalSteeringTime = totalSteeringTime * (1.0f - Mathf.Abs(angleDiff / 360.0f));
}
// Acceleration!
if (!_isBrake && _accelerationTime < totalAccelTime)
{
_accelerationTime += Time.deltaTime;
_speed = AntMath.Lerp(_speed, moveSpeed, accelCurve.Evaluate(_accelerationTime / totalAccelTime));
}
// Correction of the angle.
if (_isSteering)
{
_steeringTime += Time.deltaTime;
Angle = AntMath.LerpAngleDeg(Angle, targetAngle, steeringCurve.Evaluate(_steeringTime / _totalSteeringTime));
if (AntMath.Equal(angleDiff, 0.0f, 0.01f))
{
_isSteering = false;
_steeringTime = 0.0f;
}
}
// Movement.
float ang = Angle * AntMath.RADIANS;
Velocity = new Vector2(_speed * Mathf.Cos(ang), _speed * Mathf.Sin(ang));
}