private void acceleratedProgram(Vector2 velocity, ref Vector2 result)
{
//Debug.Log(velocity.ToString() + " " + result.ToString());
float speed = RVOMath.abs(velocity);
float maxSpeed = RVOMath.Min(speed + accelerated_, maxSpeed_);
float minSpeed = RVOMath.Max(speed - accelerated_, 0.0f);
Vector2 tempResult = result;
//Debug.Log(speed);
if (speed >= minSpeedToTurn_)
{
Vector2 leftVerticalVelocity = new Vector2(-velocity.y_, velocity.x_);
Vector2 rightVerticalVelocity = new Vector2(velocity.y_, -velocity.x_);
leftVerticalVelocity = RVOMath.normalize(leftVerticalVelocity) * speed * angularSpeed_;
rightVerticalVelocity = RVOMath.normalize(rightVerticalVelocity) * speed * angularSpeed_;
Vector2 leftMin = RVOMath.normalize(velocity + leftVerticalVelocity) * minSpeed;
Vector2 leftMax = RVOMath.normalize(velocity + leftVerticalVelocity) * maxSpeed;
Vector2 rightMin = RVOMath.normalize(velocity + rightVerticalVelocity) * minSpeed;
Vector2 rightMax = RVOMath.normalize(velocity + rightVerticalVelocity) * maxSpeed;
Vector2 leftVector2 = leftMax - leftMin;
Vector2 rightVector2 = rightMax - rightMin;
if (RVOMath.det(leftVector2, result) < 0.0f && RVOMath.det(rightVector2, result) > 0.0f) //middle
{
float resultSpeed = RVOMath.abs(result);
if (resultSpeed > maxSpeed)
{
tempResult = RVOMath.normalize(result) * maxSpeed;
}
if (resultSpeed < minSpeed)
{
tempResult = RVOMath.normalize(result) * minSpeed;
}
}
else
{
if (RVOMath.det(leftVector2, result) > 0.0f) //left
{
float r = ((result - leftMin) * (leftMax - leftMin)) / RVOMath.absSq(leftMax - leftMin);
if (id_ == 2)
{
Debug.Log(r);
}
if (r < 0.0f)
{
tempResult = RVOMath.normalize(leftVector2) * minSpeed;
}
if (r > 1.0f)
{
tempResult = RVOMath.normalize(leftVector2) * maxSpeed;
}
if (r >= 0.0f && r <= 1.0f)
{
float resultSpeed = RVOMath.absSq(result) - RVOMath.distSqPointLineSegment(leftMin, leftMax, result);
resultSpeed = RVOMath.sqrt(resultSpeed);
if (resultSpeed > maxSpeed)
{
tempResult = RVOMath.normalize(leftVector2) * maxSpeed;
}
if (resultSpeed < minSpeed)
{
tempResult = RVOMath.normalize(leftVector2) * minSpeed;
}
if (resultSpeed >= minSpeed && resultSpeed <= maxSpeed)
{
tempResult = RVOMath.normalize(leftVector2) * resultSpeed;
}
}
}
if (RVOMath.det(rightVector2, result) < 0.0f) //right
{
float r = ((result - rightMin) * (rightMax - rightMin)) / RVOMath.absSq(rightMax - rightMin);
if (r < 0.0f)
{
tempResult = RVOMath.normalize(rightVector2) * minSpeed;
}
if (r > 1.0f)
{
tempResult = RVOMath.normalize(rightVector2) * maxSpeed;
}
if (r >= 0.0f && r <= 1.0f)
{
float resultSpeed = RVOMath.absSq(result) - RVOMath.distSqPointLineSegment(rightMin, rightMax, result);
resultSpeed = RVOMath.sqrt(resultSpeed);
if (resultSpeed > maxSpeed)
{
tempResult = RVOMath.normalize(rightVector2) * maxSpeed;
}
if (resultSpeed < minSpeed)
{
tempResult = RVOMath.normalize(rightVector2) * minSpeed;
}
if (resultSpeed >= minSpeed && resultSpeed <= maxSpeed)
{
tempResult = RVOMath.normalize(rightVector2) * resultSpeed;
}
}
}
}
}
else
{
float resultSpeed = RVOMath.abs(result);
if (resultSpeed > maxSpeed)
{
if (RVOMath.abs(velocity) < 1e-6f)
{
tempResult = RVOMath.normalize(prefVelocity_) * maxSpeed;
}
else
{
tempResult = RVOMath.normalize(velocity) * maxSpeed;
}
}
if (resultSpeed < minSpeed)
{
tempResult = RVOMath.normalize(velocity) * minSpeed;
}
}
result = tempResult;
}