| static private sqr ( float scalar ) : float | ||
| scalar | float | The float to be squared. |
| Résultat | float | |
/**
* <summary>Recursive method for computing the obstacle neighbors of the
* specified agent.</summary>
*
* <param name="agent">The agent for which obstacle neighbors are to be
* computed.</param>
* <param name="rangeSq">The squared range around the agent.</param>
* <param name="node">The current obstacle k-D node.</param>
*/
private void queryObstacleTreeRecursive(Agent agent, KInt rangeSq, ObstacleTreeNode node)
{
if (node != null)
{
Obstacle obstacle1 = node.obstacle_;
Obstacle obstacle2 = obstacle1.next_;
KInt agentLeftOfLine = RVOMath.leftOf(obstacle1.point_, obstacle2.point_, agent.position_);
queryObstacleTreeRecursive(agent, rangeSq, agentLeftOfLine >= 0 ? node.left_ : node.right_);
if (RVOMath.absSq(obstacle2.point_ - obstacle1.point_) == 0)
{
return;
}
KInt distSqLine = RVOMath.sqr(agentLeftOfLine) / RVOMath.absSq(obstacle2.point_ - obstacle1.point_);
if (distSqLine < rangeSq)
{
if (agentLeftOfLine < 0)
{
/*
* Try obstacle at this node only if agent is on right side of
* obstacle (and can see obstacle).
*/
agent.insertObstacleNeighbor(node.obstacle_, rangeSq);
}
/* Try other side of line. */
queryObstacleTreeRecursive(agent, rangeSq, agentLeftOfLine >= 0 ? node.right_ : node.left_);
}
}
}
void queryObstacleTreeRecursive(Agent agent, float rangeSq, ObstacleTreeNode node)
{
if (node == null)
{
return;
}
else
{
Obstacle obstacle1 = node.obstacle;
Obstacle obstacle2 = obstacle1.nextObstacle;
float agentLeftOfLine = RVOMath.leftOf(obstacle1.point_, obstacle2.point_, agent.position_);
queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node.left : node.right));
float distSqLine = RVOMath.sqr(agentLeftOfLine) / RVOMath.absSq(obstacle2.point_ - obstacle1.point_);
if (distSqLine < rangeSq)
{
if (agentLeftOfLine < 0.0f)
{
/*
* Try obstacle at this node only if agent is on right side of
* obstacle (and can see obstacle).
*/
agent.insertObstacleNeighbor(node.obstacle, rangeSq);
}
/* Try other side of line. */
queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node.right : node.left));
}
}
}
private void queryGetNearestAgent(Agent agent, ref int _resultUid, double _minDistance, ref double _distance, int node, Func <int, bool> _callBack)
{
if (agentTree_[node].end_ - agentTree_[node].begin_ <= MAX_LEAF_SIZE)
{
for (int i = agentTree_[node].begin_; i < agentTree_[node].end_; ++i)
{
Agent tmpAgent = agents_[i];
if (tmpAgent == agent || tmpAgent.type == AgentType.SkillPush || tmpAgent.type == AgentType.SkillObstacle || tmpAgent.type == AgentType.SkillUnit)
{
continue;
}
double dist = Vector2.Distance(tmpAgent.position_, agent.position_);
if (dist - tmpAgent.radius_ < _distance && dist + tmpAgent.radius_ > _minDistance)
{
if (_callBack(tmpAgent.uid))
{
_resultUid = tmpAgent.uid;
_distance = dist - tmpAgent.radius_;
}
}
}
}
else
{
double distSqLeft = RVOMath.sqr(Math.Max(0.0, agentTree_[agentTree_[node].left_].minX_ - agent.position_.x)) + RVOMath.sqr(Math.Max(0.0, agent.position_.x - agentTree_[agentTree_[node].left_].maxX_)) + RVOMath.sqr(Math.Max(0.0, agentTree_[agentTree_[node].left_].minY_ - agent.position_.y)) + RVOMath.sqr(Math.Max(0.0, agent.position_.y - agentTree_[agentTree_[node].left_].maxY_));
double distSqRight = RVOMath.sqr(Math.Max(0.0, agentTree_[agentTree_[node].right_].minX_ - agent.position_.x)) + RVOMath.sqr(Math.Max(0.0, agent.position_.x - agentTree_[agentTree_[node].right_].maxX_)) + RVOMath.sqr(Math.Max(0.0, agentTree_[agentTree_[node].right_].minY_ - agent.position_.y)) + RVOMath.sqr(Math.Max(0.0, agent.position_.y - agentTree_[agentTree_[node].right_].maxY_));
double distSq = (_distance + simulator.getMaxRadius()) * (_distance + simulator.getMaxRadius());
if (distSqLeft < distSqRight)
{
if (distSqLeft < distSq)
{
queryGetNearestAgent(agent, ref _resultUid, _minDistance, ref _distance, agentTree_[node].left_, _callBack);
if (distSqRight < distSq)
{
queryGetNearestAgent(agent, ref _resultUid, _minDistance, ref _distance, agentTree_[node].right_, _callBack);
}
}
}
else
{
if (distSqRight < distSq)
{
queryGetNearestAgent(agent, ref _resultUid, _minDistance, ref _distance, agentTree_[node].right_, _callBack);
if (distSqLeft < distSq)
{
queryGetNearestAgent(agent, ref _resultUid, _minDistance, ref _distance, agentTree_[node].left_, _callBack);
}
}
}
}
}
private void queryAgentTreeRecursive(Vector3 position, ref float rangeSq, ref int agentNo, int node)
{
if (agentTree[node].end - agentTree[node].begin <= MAXLEAFSIZE)
{
for (int i = agentTree[node].begin; i < agentTree[node].end; ++i)
{
float distSq = RVOMath.absSq(position - agents[i].position);
if (distSq < rangeSq)
{
rangeSq = distSq;
agentNo = agents[i].id;
}
}
}
else
{
float distSqLeft = RVOMath.sqr(Math.Max(0.0f, agentTree[agentTree[node].left].minCoord[0] - position.x)) +
RVOMath.sqr(Math.Max(0.0f, position.x - agentTree[agentTree[node].left].maxCoord[0])) +
RVOMath.sqr(Math.Max(0.0f, agentTree[agentTree[node].left].minCoord[1] - position.y)) +
RVOMath.sqr(Math.Max(0.0f, position.y - agentTree[agentTree[node].left].maxCoord[1])) +
RVOMath.sqr(Math.Max(0.0f, agentTree[agentTree[node].left].minCoord[2] - position.z)) +
RVOMath.sqr(Math.Max(0.0f, position.z - agentTree[agentTree[node].left].maxCoord[2]));
float distSqRight = RVOMath.sqr(Math.Max(0.0f, agentTree[agentTree[node].right].minCoord[0] - position.x)) +
RVOMath.sqr(Math.Max(0.0f, position.x - agentTree[agentTree[node].right].maxCoord[0])) +
RVOMath.sqr(Math.Max(0.0f, agentTree[agentTree[node].right].minCoord[1] - position.y)) +
RVOMath.sqr(Math.Max(0.0f, position.y - agentTree[agentTree[node].right].maxCoord[1])) +
RVOMath.sqr(Math.Max(0.0f, agentTree[agentTree[node].right].minCoord[2] - position.z)) +
RVOMath.sqr(Math.Max(0.0f, position.z - agentTree[agentTree[node].right].maxCoord[2]));
if (distSqLeft < distSqRight)
{
if (distSqLeft < rangeSq)
{
queryAgentTreeRecursive(position, ref rangeSq, ref agentNo, agentTree[node].left);
if (distSqRight < rangeSq)
{
queryAgentTreeRecursive(position, ref rangeSq, ref agentNo, agentTree[node].right);
}
}
}
else
{
if (distSqRight < rangeSq)
{
queryAgentTreeRecursive(position, ref rangeSq, ref agentNo, agentTree[node].right);
if (distSqLeft < rangeSq)
{
queryAgentTreeRecursive(position, ref rangeSq, ref agentNo, agentTree[node].left);
}
}
}
}
}
private void queryPointTreeRecursive(Vector2 _pos, double _range, int node, List <int> list)
{
if (agentTree_[node].end_ - agentTree_[node].begin_ <= MAX_LEAF_SIZE)
{
for (int i = agentTree_[node].begin_; i < agentTree_[node].end_; ++i)
{
Agent agent = agents_[i];
if (agent.type == AgentType.SkillPush || agent.type == AgentType.SkillObstacle || agent.type == AgentType.SkillUnit)
{
continue;
}
double dist = Vector2.Distance(agent.position_, _pos);
if (dist < _range + agent.radius_)
{
list.Add(agent.uid);
}
}
}
else
{
double distSqLeft = RVOMath.sqr(Math.Max(0.0, agentTree_[agentTree_[node].left_].minX_ - _pos.x)) + RVOMath.sqr(Math.Max(0.0, _pos.x - agentTree_[agentTree_[node].left_].maxX_)) + RVOMath.sqr(Math.Max(0.0, agentTree_[agentTree_[node].left_].minY_ - _pos.y)) + RVOMath.sqr(Math.Max(0.0, _pos.y - agentTree_[agentTree_[node].left_].maxY_));
double distSqRight = RVOMath.sqr(Math.Max(0.0, agentTree_[agentTree_[node].right_].minX_ - _pos.x)) + RVOMath.sqr(Math.Max(0.0, _pos.x - agentTree_[agentTree_[node].right_].maxX_)) + RVOMath.sqr(Math.Max(0.0, agentTree_[agentTree_[node].right_].minY_ - _pos.y)) + RVOMath.sqr(Math.Max(0.0, _pos.y - agentTree_[agentTree_[node].right_].maxY_));
double rangeSq = (_range + simulator.getMaxRadius()) * (_range + simulator.getMaxRadius());
if (distSqLeft < distSqRight)
{
if (distSqLeft < rangeSq)
{
queryPointTreeRecursive(_pos, _range, agentTree_[node].left_, list);
if (distSqRight < rangeSq)
{
queryPointTreeRecursive(_pos, _range, agentTree_[node].right_, list);
}
}
}
else
{
if (distSqRight < rangeSq)
{
queryPointTreeRecursive(_pos, _range, agentTree_[node].right_, list);
if (distSqLeft < rangeSq)
{
queryPointTreeRecursive(_pos, _range, agentTree_[node].left_, list);
}
}
}
}
}
/**
* <summary>Recursive method for querying the visibility between two
* points within a specified radius.</summary>
*
* <returns>True if q1 and q2 are mutually visible within the radius;
* false otherwise.</returns>
*
* <param name="q1">The first point between which visibility is to be
* tested.</param>
* <param name="q2">The second point between which visibility is to be
* tested.</param>
* <param name="radius">The radius within which visibility is to be
* tested.</param>
* <param name="node">The current obstacle k-D node.</param>
*/
private bool queryVisibilityRecursive(Vector2 q1, Vector2 q2, float radius, ObstacleTreeNode node)
{
if (node == null)
{
return(true);
}
Obstacle obstacle1 = node.obstacle_;
Obstacle obstacle2 = obstacle1.next_;
float q1LeftOfI = RVOMath.leftOf(obstacle1.point_, obstacle2.point_, q1);
float q2LeftOfI = RVOMath.leftOf(obstacle1.point_, obstacle2.point_, q2);
float invLengthI = 1.0f / RVOMath.absSq(obstacle2.point_ - obstacle1.point_);
if (q1LeftOfI >= 0.0f && q2LeftOfI >= 0.0f)
{
return(queryVisibilityRecursive(q1, q2, radius, node.left_) &&
(
(RVOMath.sqr(q1LeftOfI) * invLengthI >= RVOMath.sqr(radius) &&
RVOMath.sqr(q2LeftOfI) * invLengthI >= RVOMath.sqr(radius)) ||
queryVisibilityRecursive(q1, q2, radius, node.right_)
));
}
if (q1LeftOfI <= 0.0f && q2LeftOfI <= 0.0f)
{
return(queryVisibilityRecursive(q1, q2, radius, node.right_) &&
(
(RVOMath.sqr(q1LeftOfI) * invLengthI >= RVOMath.sqr(radius) &&
RVOMath.sqr(q2LeftOfI) * invLengthI >= RVOMath.sqr(radius)) ||
queryVisibilityRecursive(q1, q2, radius, node.left_)
));
}
if (q1LeftOfI >= 0.0f && q2LeftOfI <= 0.0f)
{
/* One can see through obstacle from left to right. */
return(queryVisibilityRecursive(q1, q2, radius, node.left_) &&
queryVisibilityRecursive(q1, q2, radius, node.right_));
}
float point1LeftOfQ = RVOMath.leftOf(q1, q2, obstacle1.point_);
float point2LeftOfQ = RVOMath.leftOf(q1, q2, obstacle2.point_);
float invLengthQ = 1.0f / RVOMath.absSq(q2 - q1);
return(point1LeftOfQ * point2LeftOfQ >= 0.0f &&
RVOMath.sqr(point1LeftOfQ) * invLengthQ > RVOMath.sqr(radius) &&
RVOMath.sqr(point2LeftOfQ) * invLengthQ > RVOMath.sqr(radius) &&
queryVisibilityRecursive(q1, q2, radius, node.left_) &&
queryVisibilityRecursive(q1, q2, radius, node.right_));
}
/**
* <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 <= MAXLEAFSIZE)
{
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].minCoord[0] - agent.position.x)) +
RVOMath.sqr(Math.Max(0.0f, agent.position.x - agentTree[agentTree[node].left].maxCoord[0])) +
RVOMath.sqr(Math.Max(0.0f, agentTree[agentTree[node].left].minCoord[1] - agent.position.y)) +
RVOMath.sqr(Math.Max(0.0f, agent.position.y - agentTree[agentTree[node].left].maxCoord[1])) +
RVOMath.sqr(Math.Max(0.0f, agentTree[agentTree[node].left].minCoord[2] - agent.position.z)) +
RVOMath.sqr(Math.Max(0.0f, agent.position.z - agentTree[agentTree[node].left].maxCoord[2]));
float distSqRight = RVOMath.sqr(Math.Max(0.0f, agentTree[agentTree[node].right].minCoord[0] - agent.position.x)) +
RVOMath.sqr(Math.Max(0.0f, agent.position.x - agentTree[agentTree[node].right].maxCoord[0])) +
RVOMath.sqr(Math.Max(0.0f, agentTree[agentTree[node].right].minCoord[1] - agent.position.y)) +
RVOMath.sqr(Math.Max(0.0f, agent.position.y - agentTree[agentTree[node].right].maxCoord[1])) +
RVOMath.sqr(Math.Max(0.0f, agentTree[agentTree[node].right].minCoord[2] - agent.position.z)) +
RVOMath.sqr(Math.Max(0.0f, agent.position.z - agentTree[agentTree[node].right].maxCoord[2]));
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);
}
}
}
}
}
public void computeNeighbors()
{
// obstacleNeighbors_.Clear();
float rangeSq = RVOMath.sqr(timeHorizonObst_ * MaxSpeed + Radius);
// Simulator.Instance.kdTree_.computeObstacleNeighbors(this, rangeSq);
agentNeighbors_.Clear();
if (maxNeighbors_ > 0)
{
rangeSq = RVOMath.sqr(neighborDist_);
Simulator.Instance.kdTree_.computeAgentNeighbors(this, ref rangeSq);
}
}
/**
* <summary>Computes the neighbors of this agent.</summary>
*/
internal void computeNeighbors()
{
obstacleNeighbors_.Clear();
KInt rangeSq = RVOMath.sqr(timeHorizonObst_ * maxSpeed_ + radius_);
Simulator.Instance.kdTree_.computeObstacleNeighbors(this, rangeSq);
agentNeighbors_.Clear();
if (maxNeighbors_ > 0)
{
rangeSq = RVOMath.sqr(neighborDist_);
Simulator.Instance.kdTree_.computeAgentNeighbors(this, ref rangeSq);
}
}
/**
* <summary>Recursive method for querying the visibility between two
* points within a specified radius.</summary>
*
* <returns>True if q1 and q2 are mutually visible within the radius;
* false otherwise.</returns>
*
* <param name="q1">The first point between which visibility is to be
* tested.</param>
* <param name="q2">The second point between which visibility is to be
* tested.</param>
* <param name="radius">The radius within which visibility is to be
* tested.</param>
* <param name="node">The current obstacle k-D node.</param>
*/
private bool queryVisibilityRecursive(Vec2 q1, Vec2 q2, Fix64 radius, ObstacleTreeNode node)
{
if (node == null)
{
return(true);
}
Obstacle obstacle1 = node.obstacle_;
Obstacle obstacle2 = obstacle1.next_;
Fix64 q1LeftOfI = RVOMath.leftOf(obstacle1.point_, obstacle2.point_, q1);
Fix64 q2LeftOfI = RVOMath.leftOf(obstacle1.point_, obstacle2.point_, q2);
Fix64 q12 = RVOMath.absSq(obstacle2.point_ - obstacle1.point_);
if (q12 == Fix64.Zero)
{
return(true);
}
Fix64 invLengthI = Fix64.One / RVOMath.absSq(obstacle2.point_ - obstacle1.point_);
if (q1LeftOfI >= Fix64.Zero && q2LeftOfI >= Fix64.Zero)
{
return(queryVisibilityRecursive(q1, q2, radius, node.left_) && ((RVOMath.sqr(q1LeftOfI) * invLengthI >= RVOMath.sqr(radius) && RVOMath.sqr(q2LeftOfI) * invLengthI >= RVOMath.sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node.right_)));
}
if (q1LeftOfI <= Fix64.Zero && q2LeftOfI <= Fix64.Zero)
{
return(queryVisibilityRecursive(q1, q2, radius, node.right_) && ((RVOMath.sqr(q1LeftOfI) * invLengthI >= RVOMath.sqr(radius) && RVOMath.sqr(q2LeftOfI) * invLengthI >= RVOMath.sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node.left_)));
}
if (q1LeftOfI >= Fix64.Zero && q2LeftOfI <= Fix64.Zero)
{
/* One can see through obstacle from left to right. */
return(queryVisibilityRecursive(q1, q2, radius, node.left_) && queryVisibilityRecursive(q1, q2, radius, node.right_));
}
Fix64 point1LeftOfQ = RVOMath.leftOf(q1, q2, obstacle1.point_);
Fix64 point2LeftOfQ = RVOMath.leftOf(q1, q2, obstacle2.point_);
Fix64 sqp12 = RVOMath.absSq(q2 - q1);
if (sqp12 == Fix64.Zero)
{
return(true);
}
Fix64 invLengthQ = Fix64.One / sqp12;
return(point1LeftOfQ * point2LeftOfQ >= Fix64.Zero && RVOMath.sqr(point1LeftOfQ) * invLengthQ > RVOMath.sqr(radius) && RVOMath.sqr(point2LeftOfQ) * invLengthQ > RVOMath.sqr(radius) && queryVisibilityRecursive(q1, q2, radius, node.left_) && queryVisibilityRecursive(q1, q2, radius, node.right_));
}
/**
* <summary>Computes the neighbors of this agent.</summary>
*/
internal void computeNeighbors()
{
obstacleNeighbors.Clear();
float rangeSq = RVOMath.sqr(timeHorizonObst * maxSpeed + radius);
Simulator.Instance.kdTree.computeObstacleNeighbors(this, rangeSq);
agentNeighbors.Clear();
if (maxNeighbors > 0)
{
rangeSq = RVOMath.sqr(neighborDist);
Simulator.Instance.kdTree.computeAgentNeighbors(this, ref rangeSq);
}
}
/**
* <summary>Computes the neighbors of this agent.</summary>
*/
internal void computeNeighbors()
{
obstacleNeighbors_.Clear();
float rangeSq = RVOMath.sqr(timeHorizonObst_ * maxSpeed_ + radius_);
sim_.kdTree_.computeObstacleNeighbors(this, rangeSq);
agentNeighbors_.Clear();
if (maxNeighbors_ > 0)
{
rangeSq = RVOMath.sqr(neighborDist_) + RVOMath.absSq(velocity_);// RVOMath.sqr(neighborDist_);
sim_.kdTree_.computeAgentNeighbors(this, ref rangeSq);
}
}
/**
* <summary>Solves a two-dimensional linear program subject to linear
* constraints defined by lines and a circular constraint.</summary>
* 解决一个二维线性规划问题，由直线和一个圆形约束定义的线性约束
*
* <returns>The number of the line it fails on, and the number of lines
* if successful.</returns>
*
* <param name="lines">Lines defining the linear constraints.</param>
* <param name="radius">The radius of the circular constraint.</param>
* <param name="optVelocity">The optimization velocity.</param>
* <param name="directionOpt">True if the direction should be optimized.
* </param>
* <param name="result">A reference to the result of the linear program.
* </param>
*/
private int linearProgram2(IList <Line> lines, float radius, Vector2 optVelocity, bool directionOpt, ref Vector2 result)
{
if (directionOpt)
{
/*
* Optimize direction. Note that the optimization velocity is of
* unit length in this case.
* 优化方向。注意，在这种情况下，优化速度是单位长度的
*/
result = optVelocity * radius;
}
else if (RVOMath.absSq(optVelocity) > RVOMath.sqr(radius))
{
/* Optimize closest point and outside circle.
* 优化圆外最接近的点 当单位速度大小大于最大速度时，即速度在圆外
*/
result = RVOMath.normalize(optVelocity) * radius;
}
else
{
/* Optimize closest point and inside circle.
* 优化圆内最接近的点 当单位速度小于最大速度时，即速度在圆内
*/
result = optVelocity;
}
for (int i = 0; i < lines.Count; ++i)
{
if (RVOMath.det(lines[i].direction, lines[i].point - result) > 0.0f)
{
/* Result does not satisfy constraint i. Compute new optimal result.
* 结果不满足约束i，计算新的最优结果
*/
Vector2 tempResult = result;
if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, ref result))
{
result = tempResult;
return(i);
}
}
}
return(lines.Count);
}
/**
* <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.position_.x_))
+ RVOMath.sqr(Math.Max(0.0f, agent.position_.x_ - agentTree_[agentTree_[node].left_].maxX_))
+ RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].left_].minY_ - agent.position_.y_))
+ RVOMath.sqr(Math.Max(0.0f, agent.position_.y_ - agentTree_[agentTree_[node].left_].maxY_));
float distSqRight = RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].right_].minX_ - agent.position_.x_))
+ RVOMath.sqr(Math.Max(0.0f, agent.position_.x_ - agentTree_[agentTree_[node].right_].maxX_))
+ RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].right_].minY_ - agent.position_.y_))
+ RVOMath.sqr(Math.Max(0.0f, agent.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_);
}
}
}
}
}
private void queryAgentTreeRecursive(KInt2 position, ref KInt rangeSq, ref int agentNo, int node)
{
if (agentTree_[node].end_ - agentTree_[node].begin_ <= MAX_LEAF_SIZE)
{
for (int i = agentTree_[node].begin_; i < agentTree_[node].end_; ++i)
{
KInt distSq = RVOMath.absSq(position - agents_[i].position_);
if (distSq < rangeSq)
{
rangeSq = distSq;
agentNo = agents_[i].id_;
}
}
}
else
{
KInt distSqLeft = RVOMath.sqr(ReduceMax(agentTree_[agentTree_[node].left_].minx, position.IntX)) + RVOMath.sqr(ReduceMax(position.IntX, agentTree_[agentTree_[node].left_].maxx)) + RVOMath.sqr(ReduceMax(agentTree_[agentTree_[node].left_].miny, position.IntY)) + RVOMath.sqr(ReduceMax(position.IntY, agentTree_[agentTree_[node].left_].maxy));
KInt distSqRight = RVOMath.sqr(ReduceMax(agentTree_[agentTree_[node].right_].minx, position.IntX)) + RVOMath.sqr(ReduceMax(position.IntX, agentTree_[agentTree_[node].right_].maxx)) + RVOMath.sqr(ReduceMax(agentTree_[agentTree_[node].right_].miny, position.IntY)) + RVOMath.sqr(ReduceMax(position.IntY, agentTree_[agentTree_[node].right_].maxy));
if (distSqLeft < distSqRight)
{
if (distSqLeft < rangeSq)
{
queryAgentTreeRecursive(position, ref rangeSq, ref agentNo, agentTree_[node].left_);
if (distSqRight < rangeSq)
{
queryAgentTreeRecursive(position, ref rangeSq, ref agentNo, agentTree_[node].right_);
}
}
}
else
{
if (distSqRight < rangeSq)
{
queryAgentTreeRecursive(position, ref rangeSq, ref agentNo, agentTree_[node].right_);
if (distSqLeft < rangeSq)
{
queryAgentTreeRecursive(position, ref rangeSq, ref agentNo, agentTree_[node].left_);
}
}
}
}
}
private void queryAgentTreeRecursive(Vector2 position, ref float rangeSq, ref int agentNo, int node)
{
if (agentTree_[node].end_ - agentTree_[node].begin_ <= MAX_LEAF_SIZE)
{
for (int i = agentTree_[node].begin_; i < agentTree_[node].end_; ++i)
{
float distSq = RVOMath.absSq(position - agents_[i].position_);
if (distSq < rangeSq)
{
rangeSq = distSq;
agentNo = agents_[i].id_;
}
}
}
else
{
float distSqLeft = RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].left_].minX_ - position.x_)) + RVOMath.sqr(Math.Max(0.0f, position.x_ - agentTree_[agentTree_[node].left_].maxX_)) + RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].left_].minY_ - position.y_)) + RVOMath.sqr(Math.Max(0.0f, position.y_ - agentTree_[agentTree_[node].left_].maxY_));
float distSqRight = RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].right_].minX_ - position.x_)) + RVOMath.sqr(Math.Max(0.0f, position.x_ - agentTree_[agentTree_[node].right_].maxX_)) + RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].right_].minY_ - position.y_)) + RVOMath.sqr(Math.Max(0.0f, position.y_ - agentTree_[agentTree_[node].right_].maxY_));
if (distSqLeft < distSqRight)
{
if (distSqLeft < rangeSq)
{
queryAgentTreeRecursive(position, ref rangeSq, ref agentNo, agentTree_[node].left_);
if (distSqRight < rangeSq)
{
queryAgentTreeRecursive(position, ref rangeSq, ref agentNo, agentTree_[node].right_);
}
}
}
else
{
if (distSqRight < rangeSq)
{
queryAgentTreeRecursive(position, ref rangeSq, ref agentNo, agentTree_[node].right_);
if (distSqLeft < rangeSq)
{
queryAgentTreeRecursive(position, ref rangeSq, ref agentNo, agentTree_[node].left_);
}
}
}
}
}
/**
* <summary>Solves a two-dimensional linear program subject to linear
* constraints defined by lines and a circular constraint.</summary>
*
* <returns>The number of the line it fails on, and the number of lines
* if successful.</returns>
*
* <param name="lines">Lines defining the linear constraints.</param>
* <param name="radius">The radius of the circular constraint.</param>
* <param name="optVelocity">The optimization velocity.</param>
* <param name="directionOpt">True if the direction should be optimized.
* </param>
* <param name="result">A reference to the result of the linear program.
* </param>
*/
private int linearProgram2(IList <Line> lines, float radius, Vector2 optVelocity, bool directionOpt, ref Vector2 result)
{
// directionOpt 第一次为false，第二次为true，directionOpt主要用在 linearProgram1 里面
if (directionOpt)
{
/*
* Optimize direction. Note that the optimization velocity is of
* unit length in this case.
*/
// 1.这个其实没什么用，只是因为velocity是归一化的所以直接乘 radius
result = optVelocity * radius;
}
else if (RVOMath.absSq(optVelocity) > RVOMath.sqr(radius))
{
/* Optimize closest point and outside circle. */
// 2.当 optVelocity 太大时，先归一化optVelocity，再乘 radius
result = RVOMath.normalize(optVelocity) * radius;
}
else
{
/* Optimize closest point and inside circle. */
// 3.当 optVelocity 小于maxSpeed时
result = optVelocity;
}
for (int i = 0; i < lines.Count; ++i)
{
if (RVOMath.det(lines[i].direction, lines[i].point - result) > 0.0f)
{
/* Result does not satisfy constraint i. Compute new optimal result. */
Vector2 tempResult = result;
if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, ref result))
{
result = tempResult;
return(i);
}
}
}
return(lines.Count);
}
/**
* <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 KInt 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
{
KInt distSqLeft = RVOMath.sqr(ReduceMax(agentTree_[agentTree_[node].left_].minx, agent.position_.IntX)) + RVOMath.sqr(ReduceMax(agent.position_.IntX, agentTree_[agentTree_[node].left_].maxx)) + RVOMath.sqr(ReduceMax(agentTree_[agentTree_[node].left_].miny, agent.position_.IntY)) + RVOMath.sqr(ReduceMax(agent.position_.IntY, agentTree_[agentTree_[node].left_].maxy));
KInt distSqRight = RVOMath.sqr(ReduceMax(agentTree_[agentTree_[node].right_].minx, agent.position_.IntX)) + RVOMath.sqr(ReduceMax(agent.position_.IntX, agentTree_[agentTree_[node].right_].maxx)) + RVOMath.sqr(ReduceMax(agentTree_[agentTree_[node].right_].miny, agent.position_.IntY)) + RVOMath.sqr(ReduceMax(agent.position_.IntY, 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_);
}
}
}
}
}
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.position_.x_)) + RVOMath.sqr(Math.Max(0.0f, agent.position_.x_ - agentTree_[agentTree_[node].left].maxX)) + RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].left].minY - agent.position_.y_)) + RVOMath.sqr(Math.Max(0.0f, agent.position_.y_ - agentTree_[agentTree_[node].left].maxY));
float distSqRight = RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].right].minX - agent.position_.x_)) + RVOMath.sqr(Math.Max(0.0f, agent.position_.x_ - agentTree_[agentTree_[node].right].maxX)) + RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].right].minY - agent.position_.y_)) + RVOMath.sqr(Math.Max(0.0f, agent.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>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 Fix64 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
{
Fix64 distSqLeft = RVOMath.sqr(MathEx.Max(0.0f, agentTree_[agentTree_[node].left_].minX_ - agent.position_.x)) + RVOMath.sqr(MathEx.Max(Fix64.Zero, agent.position_.x - agentTree_[agentTree_[node].left_].maxX_)) + RVOMath.sqr(MathEx.Max(Fix64.Zero, agentTree_[agentTree_[node].left_].minY_ - agent.position_.y)) + RVOMath.sqr(MathEx.Max(Fix64.Zero, agent.position_.y - agentTree_[agentTree_[node].left_].maxY_));
Fix64 distSqRight = RVOMath.sqr(MathEx.Max(0.0f, agentTree_[agentTree_[node].right_].minX_ - agent.position_.x)) + RVOMath.sqr(MathEx.Max(Fix64.Zero, agent.position_.x - agentTree_[agentTree_[node].right_].maxX_)) + RVOMath.sqr(MathEx.Max(Fix64.Zero, agentTree_[agentTree_[node].right_].minY_ - agent.position_.y)) + RVOMath.sqr(MathEx.Max(Fix64.Zero, agent.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>Solves a two-dimensional linear program subject to linear
* constraints defined by lines and a circular constraint.</summary>
*
* <returns>The number of the line it fails on, and the number of lines
* if successful.</returns>
*
* <param name="lines">Lines defining the linear constraints.</param>
* <param name="radius">The radius of the circular constraint.</param>
* <param name="optVelocity">The optimization velocity.</param>
* <param name="directionOpt">True if the direction should be optimized.
* </param>
* <param name="result">A reference to the result of the linear program.
* </param>
*/
private int linearProgram2(ref IList <Line> lines, float radius, float2 optVelocity, bool directionOpt, ref float2 result)
{
if (directionOpt)
{
/*
* Optimize direction. Note that the optimization velocity is of
* unit length in this case.
*/
result = optVelocity * radius;
}
else if (RVOMath.absSq(optVelocity) > RVOMath.sqr(radius))
{
/* Optimize closest point and outside circle. */
result = math.normalize(optVelocity) * radius;
}
else
{
/* Optimize closest point and inside circle. */
result = optVelocity;
}
for (int i = 0; i < lines.Count; ++i)
{
if (RVOMath.det(lines[i].direction, lines[i].point - result) > 0.0f)
{
/* Result does not satisfy constraint i. Compute new optimal result. */
float2 tempResult = result;
if (!linearProgram1(ref lines, i, radius, optVelocity, directionOpt, ref result))
{
result = tempResult;
return(i);
}
}
}
return(lines.Count);
}
/**
* <summary>Solves a one-dimensional linear program on a specified line
* subject to linear constraints defined by lines and a circular
* constraint.</summary>
*
* <returns>True if successful.</returns>
*
* <param name="lines">Lines defining the linear constraints.</param>
* <param name="lineNo">The specified line constraint.</param>
* <param name="radius">The radius of the circular constraint.</param>
* <param name="optVelocity">The optimization velocity.</param>
* <param name="directionOpt">True if the direction should be optimized.
* </param>
* <param name="result">A reference to the result of the linear program.
* </param>
*/
private bool linearProgram1(IList <Line> lines, int lineNo, float radius, Vector2 optVelocity, bool directionOpt, ref Vector2 result)
{
float dotProduct = Vector2.Dot(lines[lineNo].point, lines[lineNo].direction);
float discriminant = RVOMath.sqr(dotProduct) + RVOMath.sqr(radius) - RVOMath.absSq(lines[lineNo].point);
if (discriminant < 0.0f)
{
/* Max speed circle fully invalidates line lineNo. */
return(false);
}
float sqrtDiscriminant = RVOMath.sqrt(discriminant);
float tLeft = -dotProduct - sqrtDiscriminant;
float tRight = -dotProduct + sqrtDiscriminant;
for (int i = 0; i < lineNo; ++i)
{
float denominator = RVOMath.det(lines[lineNo].direction, lines[i].direction);
float numerator = RVOMath.det(lines[i].direction, lines[lineNo].point - lines[i].point);
if (RVOMath.fabs(denominator) <= RVOMath.RVO_EPSILON)
{
/* Lines lineNo and i are (almost) parallel. */
if (numerator < 0.0f)
{
return(false);
}
continue;
}
float t = numerator / denominator;
if (denominator >= 0.0f)
{
/* Line i bounds line lineNo on the right. */
tRight = Mathf.Min(tRight, t);
}
else
{
/* Line i bounds line lineNo on the left. */
tLeft = Mathf.Max(tLeft, t);
}
if (tLeft > tRight)
{
return(false);
}
}
if (directionOpt)
{
/* Optimize direction. */
if (Vector2.Dot(optVelocity, lines[lineNo].direction) > 0.0f)
{
/* Take right extreme. */
result = lines[lineNo].point + tRight * lines[lineNo].direction;
}
else
{
/* Take left extreme. */
result = lines[lineNo].point + tLeft * lines[lineNo].direction;
}
}
else
{
/* Optimize closest point. */
float t = Vector2.Dot(lines[lineNo].direction, (optVelocity - lines[lineNo].point));
if (t < tLeft)
{
result = lines[lineNo].point + tLeft * lines[lineNo].direction;
}
else if (t > tRight)
{
result = lines[lineNo].point + tRight * lines[lineNo].direction;
}
else
{
result = lines[lineNo].point + t * lines[lineNo].direction;
}
}
return(true);
}
/**
* <summary>Computes the new velocity of this agent.</summary>
*/
internal void computeNewVelocity()
{
if (isKinematic)
{
newVelocity_ = prefVelocity_ / prefVelocity_.magnitude * maxSpeed_;
return;
}
orcaLines_.Clear();
float invTimeHorizonObst = 1.0f / timeHorizonObst_;
/* Create obstacle ORCA lines. */
for (int i = 0; i < obstacleNeighbors_.Count; ++i)
{
Obstacle obstacle1 = obstacleNeighbors_[i].Value;
Obstacle obstacle2 = obstacle1.next_;
Vector2 relativePosition1 = obstacle1.point_ - position_;
Vector2 relativePosition2 = obstacle2.point_ - position_;
/*
* Check if velocity obstacle of obstacle is already taken care
* of by previously constructed obstacle ORCA lines.
*/
bool alreadyCovered = false;
for (int j = 0; j < orcaLines_.Count; ++j)
{
if (RVOMath.det(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVOMath.RVO_EPSILON && RVOMath.det(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVOMath.RVO_EPSILON)
{
alreadyCovered = true;
break;
}
}
if (alreadyCovered)
{
continue;
}
/* Not yet covered. Check for collisions. */
float distSq1 = RVOMath.absSq(relativePosition1);
float distSq2 = RVOMath.absSq(relativePosition2);
float radiusSq = RVOMath.sqr(radius_);
Vector2 obstacleVector = obstacle2.point_ - obstacle1.point_;
float s = Vector2.Dot(-relativePosition1, obstacleVector) / RVOMath.absSq(obstacleVector);
float distSqLine = RVOMath.absSq(-relativePosition1 - s * obstacleVector);
Line line;
if (s < 0.0f && distSq1 <= radiusSq)
{
/* Collision with left vertex. Ignore if non-convex. */
if (obstacle1.convex_)
{
line.point = new Vector2(0.0f, 0.0f);
line.direction = RVOMath.normalize(new Vector2(-relativePosition1.y, relativePosition1.x));
orcaLines_.Add(line);
}
continue;
}
else if (s > 1.0f && distSq2 <= radiusSq)
{
/*
* Collision with right vertex. Ignore if non-convex or if
* it will be taken care of by neighboring obstacle.
*/
if (obstacle2.convex_ && RVOMath.det(relativePosition2, obstacle2.direction_) >= 0.0f)
{
line.point = new Vector2(0.0f, 0.0f);
line.direction = RVOMath.normalize(new Vector2(-relativePosition2.y, relativePosition2.x));
orcaLines_.Add(line);
}
continue;
}
else if (s >= 0.0f && s < 1.0f && distSqLine <= radiusSq)
{
/* Collision with obstacle segment. */
line.point = new Vector2(0.0f, 0.0f);
line.direction = -obstacle1.direction_;
orcaLines_.Add(line);
continue;
}
/*
* No collision. Compute legs. When obliquely viewed, both legs
* can come from a single vertex. Legs extend cut-off line when
* non-convex vertex.
*/
Vector2 leftLegDirection, rightLegDirection;
if (s < 0.0f && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that left vertex
* defines velocity obstacle.
*/
if (!obstacle1.convex_)
{
/* Ignore obstacle. */
continue;
}
obstacle2 = obstacle1;
float leg1 = RVOMath.sqrt(distSq1 - radiusSq);
leftLegDirection = new Vector2(relativePosition1.x * leg1 - relativePosition1.y * radius_, relativePosition1.x * radius_ + relativePosition1.y * leg1) / distSq1;
rightLegDirection = new Vector2(relativePosition1.x * leg1 + relativePosition1.y * radius_, -relativePosition1.x * radius_ + relativePosition1.y * leg1) / distSq1;
}
else if (s > 1.0f && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that
* right vertex defines velocity obstacle.
*/
if (!obstacle2.convex_)
{
/* Ignore obstacle. */
continue;
}
obstacle1 = obstacle2;
float leg2 = RVOMath.sqrt(distSq2 - radiusSq);
leftLegDirection = new Vector2(relativePosition2.x * leg2 - relativePosition2.y * radius_, relativePosition2.x * radius_ + relativePosition2.y * leg2) / distSq2;
rightLegDirection = new Vector2(relativePosition2.x * leg2 + relativePosition2.y * radius_, -relativePosition2.x * radius_ + relativePosition2.y * leg2) / distSq2;
}
else
{
/* Usual situation. */
if (obstacle1.convex_)
{
float leg1 = RVOMath.sqrt(distSq1 - radiusSq);
leftLegDirection = new Vector2(relativePosition1.x * leg1 - relativePosition1.y * radius_, relativePosition1.x * radius_ + relativePosition1.y * leg1) / distSq1;
}
else
{
/* Left vertex non-convex; left leg extends cut-off line. */
leftLegDirection = -obstacle1.direction_;
}
if (obstacle2.convex_)
{
float leg2 = RVOMath.sqrt(distSq2 - radiusSq);
rightLegDirection = new Vector2(relativePosition2.x * leg2 + relativePosition2.y * radius_, -relativePosition2.x * radius_ + relativePosition2.y * leg2) / distSq2;
}
else
{
/* Right vertex non-convex; right leg extends cut-off line. */
rightLegDirection = obstacle1.direction_;
}
}
/*
* Legs can never point into neighboring edge when convex
* vertex, take cutoff-line of neighboring edge instead. If
* velocity projected on "foreign" leg, no constraint is added.
*/
Obstacle leftNeighbor = obstacle1.previous_;
bool isLeftLegForeign = false;
bool isRightLegForeign = false;
if (obstacle1.convex_ && RVOMath.det(leftLegDirection, -leftNeighbor.direction_) >= 0.0f)
{
/* Left leg points into obstacle. */
leftLegDirection = -leftNeighbor.direction_;
isLeftLegForeign = true;
}
if (obstacle2.convex_ && RVOMath.det(rightLegDirection, obstacle2.direction_) <= 0.0f)
{
/* Right leg points into obstacle. */
rightLegDirection = obstacle2.direction_;
isRightLegForeign = true;
}
/* Compute cut-off centers. */
Vector2 leftCutOff = invTimeHorizonObst * (obstacle1.point_ - position_);
Vector2 rightCutOff = invTimeHorizonObst * (obstacle2.point_ - position_);
Vector2 cutOffVector = rightCutOff - leftCutOff;
/* Project current velocity on velocity obstacle. */
/* Check if current velocity is projected on cutoff circles. */
float t = obstacle1 == obstacle2 ? 0.5f : Vector2.Dot((velocity_ - leftCutOff), cutOffVector) / RVOMath.absSq(cutOffVector);
float tLeft = Vector2.Dot(velocity_ - leftCutOff, leftLegDirection);
float tRight = Vector2.Dot(velocity_ - rightCutOff, rightLegDirection);
if ((t < 0.0f && tLeft < 0.0f) || (obstacle1 == obstacle2 && tLeft < 0.0f && tRight < 0.0f))
{
/* Project on left cut-off circle. */
Vector2 unitW = RVOMath.normalize(velocity_ - leftCutOff);
line.direction = new Vector2(unitW.y, -unitW.x);
line.point = leftCutOff + radius_ * invTimeHorizonObst * unitW;
orcaLines_.Add(line);
continue;
}
else if (t > 1.0f && tRight < 0.0f)
{
/* Project on right cut-off circle. */
Vector2 unitW = RVOMath.normalize(velocity_ - rightCutOff);
line.direction = new Vector2(unitW.y, -unitW.x);
line.point = rightCutOff + radius_ * invTimeHorizonObst * unitW;
orcaLines_.Add(line);
continue;
}
/*
* Project on left leg, right leg, or cut-off line, whichever is
* closest to velocity.
*/
float distSqCutoff = (t <0.0f || t> 1.0f || obstacle1 == obstacle2) ? float.PositiveInfinity : RVOMath.absSq(velocity_ - (leftCutOff + t * cutOffVector));
float distSqLeft = tLeft < 0.0f ? float.PositiveInfinity : RVOMath.absSq(velocity_ - (leftCutOff + tLeft * leftLegDirection));
float distSqRight = tRight < 0.0f ? float.PositiveInfinity : RVOMath.absSq(velocity_ - (rightCutOff + tRight * rightLegDirection));
if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight)
{
/* Project on cut-off line. */
line.direction = -obstacle1.direction_;
line.point = leftCutOff + radius_ * invTimeHorizonObst * new Vector2(-line.direction.y, line.direction.x);
orcaLines_.Add(line);
continue;
}
if (distSqLeft <= distSqRight)
{
/* Project on left leg. */
if (isLeftLegForeign)
{
continue;
}
line.direction = leftLegDirection;
line.point = leftCutOff + radius_ * invTimeHorizonObst * new Vector2(-line.direction.y, line.direction.x);
orcaLines_.Add(line);
continue;
}
/* Project on right leg. */
if (isRightLegForeign)
{
continue;
}
line.direction = -rightLegDirection;
line.point = rightCutOff + radius_ * invTimeHorizonObst * new Vector2(-line.direction.y, line.direction.x);
orcaLines_.Add(line);
}
int numObstLines = orcaLines_.Count;
float invTimeHorizon = 1.0f / timeHorizon_;
/* Create agent ORCA lines. */
for (int i = 0; i < agentNeighbors_.Count; ++i)
{
Agent other = agentNeighbors_[i].Value;
Vector2 relativePosition = other.position_ - position_;
Vector2 relativeVelocity = velocity_ - other.velocity_;
float distSq = RVOMath.absSq(relativePosition);
float combinedRadius = radius_ + other.radius_;
float combinedRadiusSq = RVOMath.sqr(combinedRadius);
Line line;
Vector2 u;
if (distSq > combinedRadiusSq)
{
/* No collision. */
Vector2 w = relativeVelocity - invTimeHorizon * relativePosition;
/* Vector from cutoff center to relative velocity. */
float wLengthSq = RVOMath.absSq(w);
float dotProduct1 = Vector2.Dot(w, relativePosition);
if (dotProduct1 < 0.0f && RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq)
{
/* Project on cut-off circle. */
float wLength = RVOMath.sqrt(wLengthSq);
Vector2 unitW = w / wLength;
line.direction = new Vector2(unitW.y, -unitW.x);
u = (combinedRadius * invTimeHorizon - wLength) * unitW;
}
else
{
/* Project on legs. */
float leg = RVOMath.sqrt(distSq - combinedRadiusSq);
if (RVOMath.det(relativePosition, w) > 0.0f)
{
/* Project on left leg. */
line.direction = new Vector2(relativePosition.x * leg - relativePosition.y * combinedRadius, relativePosition.x * combinedRadius + relativePosition.y * leg) / distSq;
}
else
{
/* Project on right leg. */
line.direction = -new Vector2(relativePosition.x * leg + relativePosition.y * combinedRadius, -relativePosition.x * combinedRadius + relativePosition.y * leg) / distSq;
}
float dotProduct2 = Vector2.Dot(relativeVelocity, line.direction);
u = dotProduct2 * line.direction - relativeVelocity;
}
}
else
{
/* Collision. Project on cut-off circle of time timeStep. */
float invTimeStep = 1.0f / Simulator.Instance.timeStep_;
/* Vector from cutoff center to relative velocity. */
Vector2 w = relativeVelocity - invTimeStep * relativePosition;
float wLength = RVOMath.abs(w);
Vector2 unitW = w / wLength;
line.direction = new Vector2(unitW.y, -unitW.x);
u = (combinedRadius * invTimeStep - wLength) * unitW;
}
if (!other.isKinematic)
{
line.point = velocity_ + 0.5f * u;
}
else
{
line.point = relativeVelocity;
}
orcaLines_.Add(line);
}
int lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, ref newVelocity_);
if (lineFail < orcaLines_.Count)
{
linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, ref newVelocity_);
}
}
/**
* <summary>Computes the new velocity of this agent.</summary>
*/
internal void computeNewVelocity()
{
orcaLines_.Clear();
//KInt invTimeHorizonObst = 1 / timeHorizonObst_;
KInt tempradius = radius_ / timeHorizonObst_;
/* Create obstacle ORCA lines. */
for (int i = 0; i < obstacleNeighbors_.Count; ++i)
{
Obstacle obstacle1 = obstacleNeighbors_[i].Value;
Obstacle obstacle2 = obstacle1.next_;
KInt2 relativePosition1 = obstacle1.point_ - position_;
KInt2 relativePosition2 = obstacle2.point_ - position_;
/*
* Check if velocity obstacle of obstacle is already taken care
* of by previously constructed obstacle ORCA lines.
*/
bool alreadyCovered = false;
for (int j = 0; j < orcaLines_.Count; ++j)
{
if (RVOMath.det(relativePosition1 / timeHorizonObst_ - orcaLines_[j].point, orcaLines_[j].direction) - tempradius >= 0 && RVOMath.det(relativePosition2 / timeHorizonObst_ - orcaLines_[j].point, orcaLines_[j].direction) - tempradius >= 0)
{
alreadyCovered = true;
break;
}
}
if (alreadyCovered)
{
continue;
}
/* Not yet covered. Check for collisions. */
KInt distSq1 = RVOMath.absSq(relativePosition1);
KInt distSq2 = RVOMath.absSq(relativePosition2);
KInt radiusSq = RVOMath.sqr(radius_);
KInt2 obstacleVector = obstacle2.point_ - obstacle1.point_;
KInt s = (-RVOMath.Dot(relativePosition1, obstacleVector)) / RVOMath.absSq(obstacleVector);
KInt distSqLine = RVOMath.absSq(-relativePosition1 - s * obstacleVector);
Line line = new Line();
if (s < 0 && distSq1 <= radiusSq)
{
/* Collision with left vertex. Ignore if non-convex. */
if (obstacle1.convex_)
{
line.point = KInt2.zero;
line.direction = RVOMath.normalize(KInt2.ToInt2(-relativePosition1.IntY, relativePosition1.IntX));
orcaLines_.Add(line);
}
continue;
}
else if (s > 1 && distSq2 <= radiusSq)
{
/*
* Collision with right vertex. Ignore if non-convex or if
* it will be taken care of by neighboring obstacle.
*/
if (obstacle2.convex_ && RVOMath.det(relativePosition2, obstacle2.direction_) >= 0)
{
line.point = KInt2.zero;
line.direction = RVOMath.normalize(KInt2.ToInt2(-relativePosition2.IntY, relativePosition2.IntX));
orcaLines_.Add(line);
}
continue;
}
else if (s >= 0 && s < 1 && distSqLine <= radiusSq)
{
/* Collision with obstacle segment. */
line.point = KInt2.zero;
line.direction = -obstacle1.direction_;
orcaLines_.Add(line);
continue;
}
/*
* No collision. Compute legs. When obliquely viewed, both legs
* can come from a single vertex. Legs extend cut-off line when
* non-convex vertex.
*/
KInt2 leftLegDirection, rightLegDirection;
if (s < 0 && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that left vertex
* defines velocity obstacle.
*/
if (!obstacle1.convex_)
{
/* Ignore obstacle. */
continue;
}
obstacle2 = obstacle1;
KInt leg1 = RVOMath.sqrt(distSq1 - radiusSq);
leftLegDirection = KInt2.ToInt2(relativePosition1.IntX * leg1 - relativePosition1.IntY * radius_, relativePosition1.IntX * radius_ + relativePosition1.IntY * leg1) / distSq1;
rightLegDirection = KInt2.ToInt2(relativePosition1.IntX * leg1 + relativePosition1.IntY * radius_, -relativePosition1.IntX * radius_ + relativePosition1.IntY * leg1) / distSq1;
if (isover(leftLegDirection) || isover(rightLegDirection))
{
LogMgr.LogError("!!!");
}
}
else if (s > 1 && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that
* right vertex defines velocity obstacle.
*/
if (!obstacle2.convex_)
{
/* Ignore obstacle. */
continue;
}
obstacle1 = obstacle2;
KInt leg2 = RVOMath.sqrt(distSq2 - radiusSq);
leftLegDirection = KInt2.ToInt2(relativePosition2.IntX * leg2 - relativePosition2.IntY * radius_, relativePosition2.IntX * radius_ + relativePosition2.IntY * leg2) / distSq2;
rightLegDirection = KInt2.ToInt2(relativePosition2.IntX * leg2 + relativePosition2.IntY * radius_, -relativePosition2.IntX * radius_ + relativePosition2.IntY * leg2) / distSq2;
if (isover(leftLegDirection) || isover(rightLegDirection))
{
LogMgr.LogError("!!!");
}
}
else
{
/* Usual situation. */
if (obstacle1.convex_)
{
KInt leg1 = RVOMath.sqrt(distSq1 - radiusSq);
leftLegDirection = KInt2.ToInt2(relativePosition1.IntX * leg1 - relativePosition1.IntY * radius_, relativePosition1.IntX * radius_ + relativePosition1.IntY * leg1) / distSq1;
if (isover(leftLegDirection))
{
LogMgr.LogError("!!!");
}
}
else
{
/* Left vertex non-convex; left leg extends cut-off line. */
leftLegDirection = -obstacle1.direction_;
if (isover(leftLegDirection))
{
LogMgr.LogError("!!!");
}
}
if (obstacle2.convex_)
{
KInt leg2 = RVOMath.sqrt(distSq2 - radiusSq);
rightLegDirection = KInt2.ToInt2(relativePosition2.IntX * leg2 + relativePosition2.IntY * radius_, -relativePosition2.IntX * radius_ + relativePosition2.IntY * leg2) / distSq2;
if (isover(rightLegDirection))
{
LogMgr.LogError("!!!");
}
}
else
{
/* Right vertex non-convex; right leg extends cut-off line. */
rightLegDirection = obstacle1.direction_;
if (isover(rightLegDirection))
{
LogMgr.LogError("!!!");
}
}
}
/*
* Legs can never point into neighboring edge when convex
* vertex, take cutoff-line of neighboring edge instead. If
* velocity projected on "foreign" leg, no constraint is added.
*/
Obstacle leftNeighbor = obstacle1.previous_;
bool isLeftLegForeign = false;
bool isRightLegForeign = false;
if (obstacle1.convex_ && RVOMath.det(leftLegDirection, -leftNeighbor.direction_) >= 0)
{
/* Left leg points into obstacle. */
leftLegDirection = -leftNeighbor.direction_;
if (isover(leftLegDirection))
{
LogMgr.LogError("!!!");
}
isLeftLegForeign = true;
}
if (obstacle2.convex_ && RVOMath.det(rightLegDirection, obstacle2.direction_) <= 0)
{
/* Right leg points into obstacle. */
rightLegDirection = obstacle2.direction_;
isRightLegForeign = true;
if (isover(rightLegDirection))
{
LogMgr.LogError("!!!");
}
}
/* Compute cut-off centers. */
KInt2 leftCutOff = (obstacle1.point_ - position_) / timeHorizonObst_;
KInt2 rightCutOff = (obstacle2.point_ - position_) / timeHorizonObst_;
KInt2 cutOffVector = rightCutOff - leftCutOff;
/* Project current velocity on velocity obstacle. */
/* Check if current velocity is projected on cutoff circles. */
KInt sqvalue = RVOMath.absSq(cutOffVector);
KInt t = KInt.ToInt(KInt.divscale / 2);
if (obstacle1 != obstacle2)
{
if (sqvalue == 0)
{
t = KInt.MaxValue;
}
else
{
t = RVOMath.Dot((velocity_ - leftCutOff), cutOffVector) / sqvalue;
}
}
KInt tLeft = RVOMath.Dot((velocity_ - leftCutOff), leftLegDirection);
KInt tRight = RVOMath.Dot((velocity_ - rightCutOff), rightLegDirection);
if ((t < 0 && tLeft < 0) || (obstacle1 == obstacle2 && tLeft < 0 && tRight < 0))
{
/* Project on left cut-off circle. */
KInt2 unitW = RVOMath.normalize((velocity_ - leftCutOff));
line.direction = KInt2.ToInt2(unitW.IntY, -unitW.IntX);
line.point = leftCutOff + radius_ * unitW / timeHorizonObst_;
orcaLines_.Add(line);
continue;
}
else if (t > 1 && tRight < 0)
{
/* Project on right cut-off circle. */
KInt2 unitW = RVOMath.normalize((velocity_ - rightCutOff));
line.direction = KInt2.ToInt2(unitW.IntY, -unitW.IntX);
line.point = rightCutOff + radius_ * unitW / timeHorizonObst_;
orcaLines_.Add(line);
continue;
}
/*
* Project on left leg, right leg, or cut-off line, whichever is
* closest to velocity.
*/
KInt distSqCutoff = (t < 0 || t > 1 || obstacle1 == obstacle2) ? KInt.MaxValue : RVOMath.absSq(velocity_ - (leftCutOff + t * cutOffVector));
KInt distSqLeft = tLeft < 0 ? KInt.MaxValue : RVOMath.absSq(velocity_ - (leftCutOff + tLeft * leftLegDirection));
KInt distSqRight = tRight < 0 ? KInt.MaxValue : RVOMath.absSq(velocity_ - (rightCutOff + tRight * rightLegDirection));
if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight)
{
/* Project on cut-off line. */
line.direction = -obstacle1.direction_;
line.point = leftCutOff + radius_ * KInt2.ToInt2(-line.direction.IntY, line.direction.IntX) / timeHorizonObst_;
orcaLines_.Add(line);
continue;
}
if (distSqLeft <= distSqRight)
{
/* Project on left leg. */
if (isLeftLegForeign)
{
continue;
}
line.direction = leftLegDirection;
line.point = leftCutOff + radius_ * KInt2.ToInt2(-line.direction.IntY, line.direction.IntX) / timeHorizonObst_;
orcaLines_.Add(line);
continue;
}
/* Project on right leg. */
if (isRightLegForeign)
{
continue;
}
line.direction = -rightLegDirection;
line.point = rightCutOff + radius_ * KInt2.ToInt2(-line.direction.IntY, line.direction.IntX) / timeHorizonObst_;
orcaLines_.Add(line);
}
int numObstLines = orcaLines_.Count;
//KInt invTimeHorizon = 1 / timeHorizon_;
/* Create agent ORCA lines. */
for (int i = 0; i < agentNeighbors_.Count; ++i)
{
Agent other = agentNeighbors_[i].Value;
KInt2 relativePosition = other.position_ - position_;
KInt2 relativeVelocity = velocity_ - other.velocity_;
KInt distSq = RVOMath.absSq(relativePosition);
KInt combinedRadius = radius_ + other.radius_;
KInt combinedRadiusSq = RVOMath.sqr(combinedRadius);
Line line = new Line();
KInt2 u;
if (distSq > combinedRadiusSq)
{
/* No collision. */
KInt2 w = relativeVelocity - relativePosition / timeHorizon_;
/* Vector from cutoff center to relative velocity. */
KInt wLengthSq = RVOMath.absSq(w);
KInt dotProduct1 = RVOMath.Dot(w, relativePosition);
if (dotProduct1 < 0 && RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq)
{
/* Project on cut-off circle. */
KInt wLength = RVOMath.sqrt(wLengthSq);
if (wLength == 0)
{
continue;
}
KInt2 unitW = w / wLength;
line.direction = KInt2.ToInt2(unitW.IntY, -unitW.IntX);
u = (combinedRadius / timeHorizon_ - wLength) * unitW;
}
else
{
/* Project on legs. */
KInt leg = RVOMath.sqrt(distSq - combinedRadiusSq);
if (RVOMath.det(relativePosition, w) > 0)
{
/* Project on left leg. */
line.direction = KInt2.ToInt2(relativePosition.IntX * leg - relativePosition.IntY * combinedRadius, relativePosition.IntX * combinedRadius + relativePosition.IntY * leg) / distSq;
}
else
{
/* Project on right leg. */
line.direction = -KInt2.ToInt2(relativePosition.IntX * leg + relativePosition.IntY * combinedRadius, -relativePosition.IntX * combinedRadius + relativePosition.IntY * leg) / distSq;
}
KInt dotProduct2 = RVOMath.Dot(relativeVelocity, line.direction);
u = dotProduct2 * line.direction - relativeVelocity;
}
}
else
{
/* Collision. Project on cut-off circle of time timeStep. */
//KInt invTimeStep = 1 / Simulator.Instance.timeStep_;
/* Vector from cutoff center to relative velocity. */
KInt2 w = relativeVelocity - relativePosition / Simulator.Instance.timeStep_;
KInt wLength = RVOMath.abs(w);
if (wLength == 0)
{
continue;
}
KInt2 unitW = w / wLength;
line.direction = KInt2.ToInt2(unitW.IntY, -unitW.IntX);
u = (combinedRadius / Simulator.Instance.timeStep_ - wLength) * unitW;
}
line.point = velocity_ + u / 2;
orcaLines_.Add(line);
}
int lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, ref newVelocity_);
if (lineFail < orcaLines_.Count)
{
linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, ref newVelocity_);
}
}
/**
* <summary>Computes the new velocity of this agent.</summary>
*/
internal void computeNewVelocity()
{
orcaLines_.Clear();
Fix64 invTimeHorizonObst = Fix64.One / timeHorizonObst_;
/* Create obstacle ORCA lines. */
for (int i = 0; i < obstacleNeighbors_.Count; ++i)
{
Obstacle obstacle1 = obstacleNeighbors_[i].Value;
Obstacle obstacle2 = obstacle1.next_;
Vec2 relativePosition1 = obstacle1.point_ - position_;
Vec2 relativePosition2 = obstacle2.point_ - position_;
/*
* Check if velocity obstacle of obstacle is already taken care
* of by previously constructed obstacle ORCA lines.
*/
bool alreadyCovered = false;
for (int j = 0; j < orcaLines_.Count; ++j)
{
if (RVOMath.det(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVOMath.RVO_EPSILON && RVOMath.det(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVOMath.RVO_EPSILON)
{
alreadyCovered = true;
break;
}
}
if (alreadyCovered)
{
continue;
}
/* Not yet covered. Check for collisions. */
Fix64 distSq1 = RVOMath.absSq(relativePosition1);
Fix64 distSq2 = RVOMath.absSq(relativePosition2);
Fix64 radiusSq = RVOMath.sqr(radius_);
Vec2 obstacleVector = obstacle2.point_ - obstacle1.point_;
Fix64 obstacleVectorSq = RVOMath.absSq(obstacleVector);
Fix64 s = obstacleVectorSq == Fix64.Zero ? Fix64.One : (-relativePosition1 * obstacleVector) / obstacleVectorSq;
Fix64 distSqLine = RVOMath.absSq(-relativePosition1 - s * obstacleVector);
Line line;
if (s < Fix64.Zero && distSq1 <= radiusSq)
{
/* Collision with left vertex. Ignore if non-convex. */
if (obstacle1.convex_)
{
line.point = Vec2.Zero;
line.direction = RVOMath.normalize(new Vec2(-relativePosition1.y, relativePosition1.x));
orcaLines_.Add(line);
}
continue;
}
else if (s > Fix64.One && distSq2 <= radiusSq)
{
/*
* Collision with right vertex. Ignore if non-convex or if
* it will be taken care of by neighboring obstacle.
*/
if (obstacle2.convex_ && RVOMath.det(relativePosition2, obstacle2.direction_) >= Fix64.Zero)
{
line.point = Vec2.Zero;
line.direction = RVOMath.normalize(new Vec2(-relativePosition2.y, relativePosition2.x));
orcaLines_.Add(line);
}
continue;
}
else if (s >= Fix64.Zero && s < Fix64.One && distSqLine <= radiusSq)
{
/* Collision with obstacle segment. */
line.point = Vec2.Zero;
line.direction = -obstacle1.direction_;
orcaLines_.Add(line);
continue;
}
/*
* No collision. Compute legs. When obliquely viewed, both legs
* can come from a single vertex. Legs extend cut-off line when
* non-convex vertex.
*/
Vec2 leftLegDirection, rightLegDirection;
if (s < Fix64.Zero && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that left vertex
* defines velocity obstacle.
*/
if (!obstacle1.convex_)
{
/* Ignore obstacle. */
continue;
}
obstacle2 = obstacle1;
Fix64 leg1 = RVOMath.sqrt((distSq1 - radiusSq).Clamp(Fix64.Zero, Fix64.MaxValue));
leftLegDirection = new Vec2(relativePosition1.x * leg1 - relativePosition1.y * radius_, relativePosition1.x * radius_ + relativePosition1.y * leg1) / distSq1;
rightLegDirection = new Vec2(relativePosition1.x * leg1 + relativePosition1.y * radius_, -relativePosition1.x * radius_ + relativePosition1.y * leg1) / distSq1;
}
else if (s > Fix64.One && distSqLine <= radiusSq)
{
/*
* Obstacle viewed obliquely so that
* right vertex defines velocity obstacle.
*/
if (!obstacle2.convex_)
{
/* Ignore obstacle. */
continue;
}
obstacle1 = obstacle2;
Fix64 leg2 = RVOMath.sqrt((distSq2 - radiusSq).Clamp(Fix64.Zero, Fix64.MaxValue));
leftLegDirection = new Vec2(relativePosition2.x * leg2 - relativePosition2.y * radius_, relativePosition2.x * radius_ + relativePosition2.y * leg2) / distSq2;
rightLegDirection = new Vec2(relativePosition2.x * leg2 + relativePosition2.y * radius_, -relativePosition2.x * radius_ + relativePosition2.y * leg2) / distSq2;
}
else
{
/* Usual situation. */
if (obstacle1.convex_)
{
Fix64 leg1 = RVOMath.sqrt((distSq1 - radiusSq).Clamp(Fix64.Zero, Fix64.MaxValue));
leftLegDirection = new Vec2(relativePosition1.x * leg1 - relativePosition1.y * radius_, relativePosition1.x * radius_ + relativePosition1.y * leg1) / distSq1;
}
else
{
/* Left vertex non-convex; left leg extends cut-off line. */
leftLegDirection = -obstacle1.direction_;
}
if (obstacle2.convex_)
{
Fix64 leg2 = RVOMath.sqrt((distSq2 - radiusSq).Clamp(Fix64.Zero, Fix64.MaxValue));
rightLegDirection = new Vec2(relativePosition2.x * leg2 + relativePosition2.y * radius_, -relativePosition2.x * radius_ + relativePosition2.y * leg2) / distSq2;
}
else
{
/* Right vertex non-convex; right leg extends cut-off line. */
rightLegDirection = obstacle1.direction_;
}
}
/*
* Legs can never point into neighboring edge when convex
* vertex, take cutoff-line of neighboring edge instead. If
* velocity projected on "foreign" leg, no constraint is added.
*/
Obstacle leftNeighbor = obstacle1.previous_;
bool isLeftLegForeign = false;
bool isRightLegForeign = false;
if (obstacle1.convex_ && RVOMath.det(leftLegDirection, -leftNeighbor.direction_) >= Fix64.Zero)
{
/* Left leg points into obstacle. */
leftLegDirection = -leftNeighbor.direction_;
isLeftLegForeign = true;
}
if (obstacle2.convex_ && RVOMath.det(rightLegDirection, obstacle2.direction_) <= Fix64.Zero)
{
/* Right leg points into obstacle. */
rightLegDirection = obstacle2.direction_;
isRightLegForeign = true;
}
/* Compute cut-off centers. */
Vec2 leftCutOff = invTimeHorizonObst * (obstacle1.point_ - position_);
Vec2 rightCutOff = invTimeHorizonObst * (obstacle2.point_ - position_);
Vec2 cutOffVector = rightCutOff - leftCutOff;
/* Project current velocity on velocity obstacle. */
/* Check if current velocity is projected on cutoff circles. */
Fix64 cutOffVectorSq = RVOMath.absSq(cutOffVector);
Fix64 t = obstacle1 == obstacle2 || cutOffVectorSq == Fix64.Zero ? 0.5f : ((velocity_ - leftCutOff) * cutOffVector) / cutOffVectorSq;
Fix64 tLeft = (velocity_ - leftCutOff) * leftLegDirection;
Fix64 tRight = (velocity_ - rightCutOff) * rightLegDirection;
if ((t < Fix64.Zero && tLeft < Fix64.Zero) || (obstacle1 == obstacle2 && tLeft < Fix64.Zero && tRight < Fix64.Zero))
{
/* Project on left cut-off circle. */
Vec2 unitW = RVOMath.normalize(velocity_ - leftCutOff);
line.direction = new Vec2(unitW.y, -unitW.x);
line.point = leftCutOff + radius_ * invTimeHorizonObst * unitW;
orcaLines_.Add(line);
continue;
}
else if (t > Fix64.One && tRight < Fix64.Zero)
{
/* Project on right cut-off circle. */
Vec2 unitW = RVOMath.normalize(velocity_ - rightCutOff);
line.direction = new Vec2(unitW.y, -unitW.x);
line.point = rightCutOff + radius_ * invTimeHorizonObst * unitW;
orcaLines_.Add(line);
continue;
}
/*
* Project on left leg, right leg, or cut-off line, whichever is
* closest to velocity.
*/
Fix64 distSqCutoff = (t <Fix64.Zero || t> Fix64.One || obstacle1 == obstacle2) ? Fix64.MaxValue : RVOMath.absSq(velocity_ - (leftCutOff + t * cutOffVector));
Fix64 distSqLeft = tLeft < Fix64.Zero ? Fix64.MaxValue : RVOMath.absSq(velocity_ - (leftCutOff + tLeft * leftLegDirection));
Fix64 distSqRight = tRight < Fix64.Zero ? Fix64.MaxValue : RVOMath.absSq(velocity_ - (rightCutOff + tRight * rightLegDirection));
if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight)
{
/* Project on cut-off line. */
line.direction = -obstacle1.direction_;
line.point = leftCutOff + radius_ * invTimeHorizonObst * new Vec2(-line.direction.y, line.direction.x);
orcaLines_.Add(line);
continue;
}
if (distSqLeft <= distSqRight)
{
/* Project on left leg. */
if (isLeftLegForeign)
{
continue;
}
line.direction = leftLegDirection;
line.point = leftCutOff + radius_ * invTimeHorizonObst * new Vec2(-line.direction.y, line.direction.x);
orcaLines_.Add(line);
continue;
}
/* Project on right leg. */
if (isRightLegForeign)
{
continue;
}
line.direction = -rightLegDirection;
line.point = rightCutOff + radius_ * invTimeHorizonObst * new Vec2(-line.direction.y, line.direction.x);
orcaLines_.Add(line);
}
int numObstLines = orcaLines_.Count;
Fix64 invTimeHorizon = Fix64.One / timeHorizon_;
/* Create agent ORCA lines. */
for (int i = 0; i < agentNeighbors_.Count; ++i)
{
Agent other = agentNeighbors_[i].Value;
Vec2 relativePosition = other.position_ - position_;
Vec2 relativeVelocity = velocity_ - other.velocity_;
Fix64 distSq = RVOMath.absSq(relativePosition);
Fix64 combinedRadius = radius_ + other.radius_;
Fix64 combinedRadiusSq = RVOMath.sqr(combinedRadius);
Line line;
Vec2 u;
if (distSq > combinedRadiusSq)
{
/* No collision. */
Vec2 w = relativeVelocity - invTimeHorizon * relativePosition;
/* the w has a change to be zero exactly.
* for example: the two objects are at the exactly same position and with the exactly same velocity
* so we have to make some perturbation to separate them */
if (w == Vec2.Zero)
{
w = id_ > other.id_ ? Vec2.Left : Vec2.Right;
}
/* Vector from cutoff center to relative velocity. */
Fix64 wLengthSq = RVOMath.absSq(w);
Fix64 dotProduct1 = w * relativePosition;
if (dotProduct1 < Fix64.Zero && RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq)
{
/* Project on cut-off circle. */
Fix64 wLength = RVOMath.sqrt(wLengthSq);
Vec2 unitW = w / wLength;
line.direction = new Vec2(unitW.y, -unitW.x);
u = (combinedRadius * invTimeHorizon - wLength) * unitW;
}
else
{
/* Project on legs. */
Fix64 leg = RVOMath.sqrt(distSq - combinedRadiusSq);
if (RVOMath.det(relativePosition, w) > Fix64.Zero)
{
/* Project on left leg. */
line.direction = new Vec2(relativePosition.x * leg - relativePosition.y * combinedRadius, relativePosition.x * combinedRadius + relativePosition.y * leg) / distSq;
}
else
{
/* Project on right leg. */
line.direction = -new Vec2(relativePosition.x * leg + relativePosition.y * combinedRadius, -relativePosition.x * combinedRadius + relativePosition.y * leg) / distSq;
}
Fix64 dotProduct2 = relativeVelocity * line.direction;
u = dotProduct2 * line.direction - relativeVelocity;
}
}
else
{
/* Collision. Project on cut-off circle of time timeStep. */
Fix64 invTimeStep = Fix64.One / simulator_.timeStep_;
/* Vector from cutoff center to relative velocity. */
Vec2 w = relativeVelocity - invTimeStep * relativePosition;
/* the w has a change to be zero exactly.
* for example: the two objects are at the exactly same position and with the exactly same velocity
* so we have to make some perturbation to separate them */
if (w == Vec2.Zero)
{
w = id_ > other.id_ ? Vec2.Left : Vec2.Right;
}
Fix64 wLength = RVOMath.abs(w);
Vec2 unitW = wLength == Fix64.Zero ? Vec2.Zero : w / wLength;
line.direction = new Vec2(unitW.y, -unitW.x);
u = (combinedRadius * invTimeStep - wLength) * unitW;
}
line.point = velocity_ + 0.5f * u;
orcaLines_.Add(line);
}
int lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, ref newVelocity_);
if (lineFail < orcaLines_.Count)
{
linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, ref newVelocity_);
}
}
/* Search for the best target velocity. */
public void computeTargetVelocity()
{
orcaLines_.Clear();
float invTimeHorizonObst = 1.0f / timeHorizonObst_;
int numObstLines = orcaLines_.Count;
float invTimeHorizon = 1.0f / timeHorizon_;
/* Create agent ORCA lines. */
for (int i = 0; i < agentNeighbors_.Count; ++i)
{
RVOAgent other = agentNeighbors_[i].Value;
Vector2 relativePosition = other.Position - Position;
Vector2 relativeVelocity = TargetVelocity - other.TargetVelocity;
float distSq = RVOMath.absSq(relativePosition);
float combinedRadius = Radius + other.Radius;
float combinedRadiusSq = RVOMath.sqr(combinedRadius);
Line line;
Vector2 u;
if (distSq > combinedRadiusSq)
{
/* No collision. */
Vector2 w = relativeVelocity - invTimeHorizon * relativePosition;
/* Vector from cutoff center to relative velocity. */
float wLengthSq = RVOMath.absSq(w);
float dotProduct1 = w * relativePosition;
if (dotProduct1 < 0.0f && RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq)
{
/* Project on cut-off circle. */
float wLength = RVOMath.sqrt(wLengthSq);
Vector2 unitW = w / wLength;
line.direction = new Vector2(unitW.y(), -unitW.x());
u = (combinedRadius * invTimeHorizon - wLength) * unitW;
}
else
{
/* Project on legs. */
float leg = RVOMath.sqrt(distSq - combinedRadiusSq);
if (RVOMath.det(relativePosition, w) > 0.0f)
{
/* Project on left leg. */
line.direction = new Vector2(relativePosition.x() * leg - relativePosition.y() * combinedRadius, relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
}
else
{
/* Project on right leg. */
line.direction = -new Vector2(relativePosition.x() * leg + relativePosition.y() * combinedRadius, -relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
}
float dotProduct2 = relativeVelocity * line.direction;
u = dotProduct2 * line.direction - relativeVelocity;
}
}
else
{
/* Collision. Project on cut-off circle of time timeStep. */
float invTimeStep = 1.0f / Simulator.Instance.timeStep_;
/* Vector from cutoff center to relative velocity. */
Vector2 w = relativeVelocity - invTimeStep * relativePosition;
float wLength = RVOMath.abs(w);
Vector2 unitW = w / wLength;
line.direction = new Vector2(unitW.y(), -unitW.x());
u = (combinedRadius * invTimeStep - wLength) * unitW;
}
line.point = TargetVelocity + 0.5f * u;
orcaLines_.Add(line);
}
int lineFail = linearProgram2(orcaLines_, MaxSpeed, PreferredVelocity, false, ref TargetVelocity);
if (lineFail < orcaLines_.Count)
{
linearProgram3(orcaLines_, numObstLines, lineFail, MaxSpeed, ref TargetVelocity);
}
}
internal SuperAgent RepresentGroup(Agent observer)
{
// Compute the left & right tangent points obtained for each agent of the group
// First element of the pair = right tangent
// Second element of the pair = left tangent
IList <Pair <Vector2, Vector2> > tangents = new List <Pair <Vector2, Vector2> >();
IList <Pair <Vector2, Vector2> > radii = new List <Pair <Vector2, Vector2> >();
for (int i = 0; i < agents_.Count; i++)
{
tangents.Add(computeTangentsPoints(observer, agents_[i]));
Pair <Vector2, Vector2> rads = new Pair <Vector2, Vector2>();
rads.First = tangents[i].First - observer.position_;
rads.Second = tangents[i].Second - observer.position_;
radii.Add(rads);
}
// Compute the group tangent points (extrema)
int rightExtremumId = 0;
int leftExtremumId = 0;
for (int i = 1; i < tangents.Count; i++)
{
// Comparison
if (Vector2.isOnTheLeftSide(radii[rightExtremumId].First, radii[i].First))
{
rightExtremumId = i;
}
if (Vector2.isOnTheLeftSide(radii[i].Second, radii[leftExtremumId].Second))
{
leftExtremumId = i;
}
}
// If the tangent are taking more than 180°, cannot be considered as a group
for (int i = 0; i < agents_.Count; i++)
{
if (Vector2.isOnTheLeftSide(radii[rightExtremumId].First, radii[i].First))
{
//std::cout << "Problem representing groups : tangent angle > 180°\n";
return(new SuperAgent(null));
}
if (Vector2.isOnTheLeftSide(radii[i].Second, radii[leftExtremumId].Second))
{
//std::cout << "Problem representing groups : tangent angle > 180°\n";
return(new SuperAgent(null));
}
}
// Compute bisector
Vector2 rightTangent = Vector2.rotation(radii[rightExtremumId].First, (float)Math.PI / 2);
Vector2 leftTangent = Vector2.rotation(radii[leftExtremumId].Second, (float)-Math.PI / 2);
Vector2 intersectionPoint = Vector2.intersectOf2Lines(tangents[rightExtremumId].First, tangents[rightExtremumId].First + rightTangent,
tangents[leftExtremumId].Second, tangents[leftExtremumId].Second + leftTangent);
// alpha/2 is more usefull than alpha
float alpha2 = Vector2.angle(Vector2.rotation(tangents[leftExtremumId].Second - intersectionPoint, -Vector2.angle(tangents[rightExtremumId].First - intersectionPoint))) / 2;
if (alpha2 < 0)
{
Debug.Log("Problem representing groups : angle computation\n SHALL NOT HAPPEN !!! \n");
// But if radii are different or if
return(new SuperAgent(null));
}
Vector2 bisector_normalize_vector = Vector2.normalize(observer.position_ - intersectionPoint);
// Compute circle
// The distance between the observer and the circle (along the bisector axis)
float d = Single.PositiveInfinity;
float a, b, c, delta, x;
int constrainingAgent = 0;
for (int i = 0; i < agents_.Count; i++)
{
Vector2 ic1 = agents_[i].position_ - intersectionPoint;
a = 1 - RVOMath.sqr((float)Math.Sin(alpha2));
b = 2 * (agents_[i].radius_ * (float)Math.Sin(alpha2) - ic1 * bisector_normalize_vector);
c = Vector2.absSq(ic1) - RVOMath.sqr(agents_[i].radius_);
delta = RVOMath.sqr(b) - 4 * a * c;
if (delta <= 0)
{
if (delta < -4 * RVO_EPSILON * c)
{
return(new SuperAgent(null));
}
else
{
delta = 0;
}
}
x = (-b + (float)Math.Sqrt(delta)) / (2 * a);
if (x < d)
{
d = x;
constrainingAgent = i;
}
}
if (d < Vector2.abs(observer.position_ - intersectionPoint) + observer.radius_ + d * Math.Sin(alpha2))
{
return(new SuperAgent(null));
}
SuperAgent toReturn = new SuperAgent(sim_);
toReturn.position_ = intersectionPoint + bisector_normalize_vector * d;
toReturn.radius_ = d * (float)Math.Sin(alpha2);
toReturn.velocity_ = agents_[constrainingAgent].velocity_;
return(toReturn);
}