internal void computeNewVelocity()
{
float minPenalty = float.MaxValue;
Vector2 velCand;
for (int i = 0; i < m_angleSampleCount; i++)
{
float angle = m_orient + i / (float)m_angleSampleCount * 2 * Mathf.PI;
for (int j = 0; j < m_velSampleCount; j++)
{
if (i == 0 && j == 0)
{
velCand = m_velPref;
}
else
{
float radius = (j + 1) / (float)m_velSampleCount;
velCand = new Vector2(radius * m_maxSpeed * Mathf.Cos(angle), radius * m_maxSpeed * Mathf.Sin(angle));
}
float dv = 0;
dv = (velCand - m_velPref).magnitude;
if (m_isCollide)
{
dv = 0;
}
float collisionTime = float.MaxValue;
foreach (var pair in m_agentNeighbors)
{
var distSq = pair.Key;
var other = pair.Value;
//Vector2 vab = velCand - other.m_vel; //VO
float effort = 1 - this.m_priority / (float)(this.m_priority + other.m_priority);
float effortInv = 1 / effort;
Vector2 vab = effortInv * velCand + (1 - effortInv) * m_vel - other.m_vel;//RVO
float time = time2Collision(m_position, vab, other.m_position, m_radius + other.m_radius, m_isCollide);
if (m_isCollide)
{
time = -Mathf.Ceil(time / m_sim.m_timeStep);
time -= RVOMath.Sqr(velCand.magnitude / m_maxSpeed);
}
if (time < collisionTime)
{
collisionTime = time;
if (m_safetyFactor / collisionTime + dv > minPenalty)
{
break;
}
}
}
float penalty = m_safetyFactor / collisionTime + dv;
if (penalty <= minPenalty - 0.001f)
{
minPenalty = penalty;
m_velNew = velCand;
}
}
}
}
/**
* <summary>Recursive method for computing the agent neighbors of the
* specified agent.</summary>
*
* <param name="agent">The agent for which agent neighbors are to be
* computed.</param>
* <param name="rangeSq">The squared range around the agent.</param>
* <param name="node">The current agent k-D tree node index.</param>
*/
private void queryAgentTreeRecursive(Agent agent, ref float rangeSq, int node)
{
if (agentTree_[node].end_ - agentTree_[node].begin_ <= MAX_LEAF_SIZE)
{
for (int i = agentTree_[node].begin_; i < agentTree_[node].end_; ++i)
{
agent.insertAgentNeighbor(agents_[i], ref rangeSq);
}
}
else
{
//在范围内部返回零，在外边返回距离矩形的最短长度平方
float distSqLeft = RVOMath.Sqr(Math.Max(0.0f, agentTree_[agentTree_[node].left_].minX_ - agent.m_position.x))
+ RVOMath.Sqr(Math.Max(0.0f, agent.m_position.x - agentTree_[agentTree_[node].left_].maxX_))
+ RVOMath.Sqr(Math.Max(0.0f, agentTree_[agentTree_[node].left_].minY_ - agent.m_position.y))
+ RVOMath.Sqr(Math.Max(0.0f, agent.m_position.y - agentTree_[agentTree_[node].left_].maxY_));
float distSqRight = RVOMath.Sqr(Math.Max(0.0f, agentTree_[agentTree_[node].right_].minX_ - agent.m_position.x))
+ RVOMath.Sqr(Math.Max(0.0f, agent.m_position.x - agentTree_[agentTree_[node].right_].maxX_))
+ RVOMath.Sqr(Math.Max(0.0f, agentTree_[agentTree_[node].right_].minY_ - agent.m_position.y))
+ RVOMath.Sqr(Math.Max(0.0f, agent.m_position.y - agentTree_[agentTree_[node].right_].maxY_));
if (distSqLeft < distSqRight)
{
if (distSqLeft < rangeSq)
{
queryAgentTreeRecursive(agent, ref rangeSq, agentTree_[node].left_);
if (distSqRight < rangeSq)
{
queryAgentTreeRecursive(agent, ref rangeSq, agentTree_[node].right_);
}
}
}
else
{
if (distSqRight < rangeSq)
{
queryAgentTreeRecursive(agent, ref rangeSq, agentTree_[node].right_);
if (distSqLeft < rangeSq)
{
queryAgentTreeRecursive(agent, ref rangeSq, agentTree_[node].left_);
}
}
}
}
}
/// <summary>
/// Time to collision of a ray to a disc.
/// </summary>
/// <param name="p">The start position of the ray</param>
/// <param name="v">The velocity vector of the ray</param>
/// <param name="p2">The center position of the disc</param>
/// <param name="radius">The radius of the disc</param>
/// <param name="collision">Specifies whether the time to collision is computed (false), or the time from collision (true).计算将要发生碰撞的时间，或者发生碰撞后经过的时间</param>
/// <returns>Returns the time to collision of ray p + tv to disc D(p2, radius), and #RVO_INFTY when the disc is not hit. If collision is true, the value is negative.</returns>
//点到直线距离公式：d = |AX0+BY0+C|/sqrt(A*A+B*B)
//直线AX+BY+C=0,A=-V.y B=V.x C =P.x*V.y-V.x*P.y
//可能碰撞：d<r=>d*d-r*r<0
//p2带入AX0+BY0+C
//AX0+BY0+C = -v.y*p2.x+v.x*p2.y+p.x*v.y-v.x*p.y=v.x(p2.y-p.y)-v.y(p2.x-p.x)=det(v, ba)
//d*d = det(v, ba)*det(v, ba)/absSq(v)
//r*r-d*d>0 => r*r-det(v, ba)*det(v, ba)/absSq(v)>0 => discr = r*r*absSq(v) - det(v, ba)*det(v, ba)>0
//
private float time2Collision(Vector2 p, Vector2 v, Vector2 p2, float radius, bool collision)
{
Vector2 ba = p2 - p;
float sq_diam = radius * radius;
float time;
float discr = -RVOMath.Sqr(RVOMath.Det(v, ba)) + sq_diam * RVOMath.AbsSq(v);
if (discr > 0)
{
if (collision)
{
time = (Vector2.Dot(v, ba) + Mathf.Sqrt(discr)) / RVOMath.AbsSq(v);//脱落碰撞需要的时间
if (time < 0)
{
time = -float.MaxValue; //
Debug.LogError("isCollide time < 0 "); //?
}
}
else
{
time = (Vector2.Dot(v, ba) - Mathf.Sqrt(discr)) / RVOMath.AbsSq(v);//将要发生碰撞的时间
if (time < 0)
{
time = float.MaxValue;//永远不会发生碰撞
}
}
}
else
{//不碰撞
if (collision)
{
time = -float.MaxValue;
}
else
{
time = float.MaxValue;
}
}
return(time);
}