public static Vector2 CalcPrefVelocity(Vector2 agentPosition, Vector2 goalPosisiton, RVOAgent agent)
{
Vector2 prefDistance = goalPosisiton - agentPosition;
if (agent.GetIsStop() == true)
{
return(new Vector2(0f, 0f));
}
if (agent.isPlayer == true)
{
/* Is inertial model used */
Vector2 nowVelocity = Simulator.Instance.getAgentVelocity(agent.GetIndex());
float nowSpeed = RVOMath.abs(nowVelocity);
Vector2 prefVelocity = RVOMath.normalize(prefDistance) * agent.prefSpeed;//nowSpeed -> prefSpeed
Vector2 finalVelocity = new Vector2(0, 0);
if (nowSpeed < agent.minSpeedToTurn)
{
/* Not fast enough to turn */
Vector2 advanceDirection = GetDirection(agent);
if (agent.prefSpeed > nowSpeed) //Speed up fitting
{
finalVelocity = nowVelocity + advanceDirection * agent.acceleration;
}
else //Speed down fitting
{
if (nowSpeed - agent.prefSpeed < agent.acceleration)
{
finalVelocity = prefVelocity;
}
else
{
finalVelocity = nowVelocity - advanceDirection * agent.acceleration;
}
}
}
else
{
/* turn */
float direction = RVOMath.det(nowVelocity, prefVelocity);
if (direction > eps) //left
{
Vector2 rightVerticalVelocity = new Vector2(-nowVelocity.y_, nowVelocity.x_);
rightVerticalVelocity = RVOMath.normalize(rightVerticalVelocity) * agent.speed * agent.angularSpeed;
if (RVOMath.det(nowVelocity + rightVerticalVelocity, prefVelocity) < eps) //success
{
finalVelocity = RVOMath.normalize(prefVelocity) * nowSpeed;
}
else
{
finalVelocity = RVOMath.normalize(nowVelocity + rightVerticalVelocity) * nowSpeed;
}
}
if (direction >= -eps && direction <= eps) //same direction
{
finalVelocity = RVOMath.normalize(prefVelocity) * nowSpeed;
}
if (direction < -eps) //right
{
Vector2 leftVerticalVelocity = new Vector2(nowVelocity.y_, -nowVelocity.x_);
leftVerticalVelocity = RVOMath.normalize(leftVerticalVelocity) * agent.speed * agent.angularSpeed;
if (RVOMath.det(nowVelocity + leftVerticalVelocity, prefVelocity) > -eps) //success
{
finalVelocity = RVOMath.normalize(prefVelocity) * nowSpeed;
}
else
{
finalVelocity = RVOMath.normalize(nowVelocity + leftVerticalVelocity) * nowSpeed;
}
}
//Debug.Log("!!!" + RVOMath.abs(finalVelocity).ToString());
/* acc and slow */
if (agent.prefSpeed - nowSpeed > eps) //acc
{
if (IsVectorNear(nowVelocity, prefVelocity, 0.05f))
{
finalVelocity = finalVelocity + RVOMath.normalize(finalVelocity) * Mathf.Min(agent.acceleration, agent.prefSpeed - nowSpeed);
}
}
else //slow
{
finalVelocity = finalVelocity - RVOMath.normalize(finalVelocity) * Mathf.Min(agent.acceleration, nowSpeed - agent.prefSpeed);
}
// Debug.Log("!!!" + RVOMath.abs(finalVelocity).ToString());
}
Vector2 ditherVelocity = GetDitherVelocity(ditherSize);
if (RVOMath.abs(finalVelocity) < eps)
{
return(finalVelocity);
}
else
{
return(finalVelocity + ditherVelocity);
}
}
else
{
Vector2 ditherVelocity = GetDitherVelocity(ditherSize);
Vector2 prefVelocity = RVOMath.normalize(prefDistance);
return(prefVelocity * agent.prefSpeed + ditherVelocity);
}
}