public override void LoadFromXmlElement(XmlElement el)
{
base.LoadFromXmlElement(el);
int id = KInt.FromXmlString(el.GetAttribute("PersonID"));
person = id == int.MinValue?null:new Person(id);
}
/**
* <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_);
}
}
}
public override void LoadFromXmlElement(XmlElement el)
{
base.LoadFromXmlElement(el);
int id = KInt.FromXmlString(el.GetAttribute("DocumentID"));
document = id == int.MinValue ? null : new Document(id);
}
// Use this for initialization
void Start()
{
KInt timestep = 0.25f;
Simulator.Instance.setTimeStep(timestep);
Simulator.Instance.SetSingleTonMode(true);
Simulator.Instance.setAgentDefaults(15, 10, 5, 5, 2, 2, KInt2.zero);
for (int i = 0; i < agentCount; i++)
{
float angle = ((float)i / agentCount) * (float)System.Math.PI * 2;
Vector3 pos = new Vector3((float)System.Math.Cos(angle), 0, (float)System.Math.Sin(angle)) * ringSize;
Vector3 antipodal = -pos + goalOffset;
int sid = Simulator.Instance.addAgent((KInt2)pos, neighborDist, maxNeighbors, timeHorizon, timeHorizonObst, radius, maxSpeed, velocity);
if (sid >= 0)
{
GameObject go = LeanPool.Spawn(agentPrefab, new Vector3(pos.x, 0, pos.y), Quaternion.Euler(0, angle + 180, 0));
go.transform.parent = transform;
go.transform.position = pos;
GameAgent ga = go.GetComponent <GameAgent>();
Assert.IsNotNull(ga);
ga.sid = sid;
m_agentMap.Add(sid, ga);
}
}
Simulator.Instance.SetNumWorkers(0);
}
/**
* <summary>Inserts an agent neighbor into the set of neighbors of this
* agent.</summary>
*
* <param name="agent">A pointer to the agent to be inserted.</param>
* <param name="rangeSq">The squared range around this agent.</param>
*/
internal void insertAgentNeighbor(Agent agent, ref KInt rangeSq)
{
if (this != agent)
{
KInt distSq = RVOMath.absSq(position_ - agent.position_);
if (distSq < rangeSq)
{
if (agentNeighbors_.Count < maxNeighbors_)
{
agentNeighbors_.Add(new KeyValuePair <KInt, Agent>(distSq, agent));
}
int i = agentNeighbors_.Count - 1;
while (i != 0 && distSq < agentNeighbors_[i - 1].Key)
{
agentNeighbors_[i] = agentNeighbors_[i - 1];
--i;
}
agentNeighbors_[i] = new KeyValuePair <KInt, Agent>(distSq, agent);
if (agentNeighbors_.Count == maxNeighbors_)
{
rangeSq = agentNeighbors_[agentNeighbors_.Count - 1].Key;
}
}
}
}
public override void LoadFromXmlElement(XmlElement el)
{
base.LoadFromXmlElement(el);
int id = KInt.FromXmlString(el.GetAttribute("EmployeeID"));
employee = id == int.MinValue ? null : new Employee(id);
}
public int queryNearAgent(KInt2 point, KInt radius)
{
if (getNumAgents() == 0)
{
return(-1);
}
return(kdTree_.queryNearAgent(point, radius));
}
/**
* <summary>Solves a two-dimensional linear program subject to linear
* constraints defined by lines and a circular constraint.</summary>
*
* <param name="lines">Lines defining the linear constraints.</param>
* <param name="numObstLines">Count of obstacle lines.</param>
* <param name="beginLine">The line on which the 2-d linear program
* failed.</param>
* <param name="radius">The radius of the circular constraint.</param>
* <param name="result">A reference to the result of the linear program.
* </param>
*/
private void linearProgram3(IList <Line> lines, int numObstLines, int beginLine, KInt radius, ref KInt2 result)
{
KInt distance = 0;
for (int i = beginLine; i < lines.Count; ++i)
{
if (RVOMath.det(lines[i].direction, lines[i].point - result) > distance)
{
/* Result does not satisfy constraint of line i. */
IList <Line> projLines = new List <Line>();
for (int ii = 0; ii < numObstLines; ++ii)
{
projLines.Add(lines[ii]);
}
for (int j = numObstLines; j < i; ++j)
{
Line line = new Line();
KInt determinant = RVOMath.det(lines[i].direction, lines[j].direction);
if (RVOMath.fabs(determinant) <= 0)
{
/* Line i and line j are parallel. */
if (RVOMath.Dot(lines[i].direction, lines[j].direction) > 0)
{
/* Line i and line j point in the same direction. */
continue;
}
else
{
/* Line i and line j point in opposite direction. */
line.point = (lines[i].point + lines[j].point) / 2;
}
}
else
{
line.point = lines[i].point + (RVOMath.det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction;
}
line.direction = RVOMath.normalize((lines[j].direction - lines[i].direction));
projLines.Add(line);
}
KInt2 tempResult = result;
if (linearProgram2(projLines, radius, KInt2.ToInt2(-lines[i].direction.IntY, lines[i].direction.IntX), true, ref result) < projLines.Count)
{
/*
* This should in principle not happen. The result is by
* definition already in the feasible region of this
* linear program. If it fails, it is due to small
* floating point error, and the current result is kept.
*/
result = tempResult;
}
distance = RVOMath.det(lines[i].direction, lines[i].point - result);
}
}
}
/**
* <summary>Computes the absolute value of a float.</summary>
*
* <returns>The absolute value of the float.</returns>
*
* <param name="scalar">The float of which to compute the absolute
* value.</param>
*/
internal static KInt fabs(KInt scalar)
{
if (scalar < 0)
{
return(-scalar);
}
return(scalar);
}
private static KInt Min(KInt left, long right)
{
right = right * KInt.divscale / KInt2.divscale;
if (left.IntValue < right)
{
return(left);
}
return(KInt.ToInt(right));
}
private static KInt ReduceMax(KInt value, long right)
{
right = right * KInt.divscale / KInt2.divscale;
KInt data = KInt.ToInt(value.IntValue - right);
if (data <= 0)
{
return(KInt.Zero);
}
return(data);
}
internal int queryNearAgent(KInt2 point, KInt radius)
{
KInt rangeSq = KInt.MaxValue;
int agentNo = -1;
queryAgentTreeRecursive(point, ref rangeSq, ref agentNo, 0);
if (rangeSq < radius * radius)
{
return(agentNo);
}
return(-1);
}
private static KInt ReduceMax(long left, KInt value)
{
left = left * KInt.divscale / KInt2.divscale;
KInt data = KInt.ToInt(left - value.IntValue);
if (data <= 0)
{
return(KInt.Zero);
}
return(data);
}
/**
* <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>Clears the simulation.</summary>
*/
public void Clear()
{
agents_ = new List <Agent>();
agentNo2indexDict_ = new Dictionary <int, int>();
index2agentNoDict_ = new Dictionary <int, int>();
defaultAgent_ = null;
kdTree_ = new KdTree();
obstacles_ = new List <Obstacle>();
globalTime_ = 0;
isError = false;
timeStep_ = KInt.ToInt(KInt.divscale / 10);
SetNumWorkers(0);
}
public override void LoadFromXmlElement(XmlElement el)
{
base.LoadFromXmlElement(el);
int id = KInt.FromXmlString(el.GetAttribute("TypeID"));
try
{
Filter = byte.Parse(el.GetAttribute("Filter"));
}
catch
{
Filter = 0;
}
type = id == int.MinValue ? null : new DocumentType(id);
}
/**
* <summary>Sets the default properties for any new agent that is added.
* </summary>
*
* <param name="neighborDist">The default maximum distance (center point
* to center point) to other agents a new agent takes into account in
* the navigation. The larger this number, the longer he running time of
* the simulation. If the number is too low, the simulation will not be
* safe. Must be non-negative.</param>
* <param name="maxNeighbors">The default maximum number of other agents
* a new agent takes into account in the navigation. The larger this
* number, the longer the running time of the simulation. If the number
* is too low, the simulation will not be safe.</param>
* <param name="timeHorizon">The default minimal amount of time for
* which a new agent's velocities that are computed by the simulation
* are safe with respect to other agents. The larger this number, the
* sooner an agent will respond to the presence of other agents, but the
* less freedom the agent has in choosing its velocities. Must be
* positive.</param>
* <param name="timeHorizonObst">The default minimal amount of time for
* which a new agent's velocities that are computed by the simulation
* are safe with respect to obstacles. The larger this number, the
* sooner an agent will respond to the presence of obstacles, but the
* less freedom the agent has in choosing its velocities. Must be
* positive.</param>
* <param name="radius">The default radius of a new agent. Must be
* non-negative.</param>
* <param name="maxSpeed">The default maximum speed of a new agent. Must
* be non-negative.</param>
* <param name="velocity">The default initial two-dimensional linear
* velocity of a new agent.</param>
*/
public void setAgentDefaults(KInt neighborDist, int maxNeighbors, KInt timeHorizon, KInt timeHorizonObst, KInt radius, KInt maxSpeed, KInt2 velocity)
{
if (defaultAgent_ == null)
{
defaultAgent_ = new Agent();
}
defaultAgent_.maxNeighbors_ = maxNeighbors;
defaultAgent_.maxSpeed_ = maxSpeed;
defaultAgent_.neighborDist_ = neighborDist;
defaultAgent_.radius_ = radius;
defaultAgent_.timeHorizon_ = timeHorizon;
defaultAgent_.timeHorizonObst_ = timeHorizonObst;
defaultAgent_.velocity_ = velocity;
}
/**
* <summary>Computes the squared distance from a line segment with the
* specified endpoints to a specified point.</summary>
*
* <returns>The squared distance from the line segment to the point.
* </returns>
*
* <param name="vector1">The first endpoint of the line segment.</param>
* <param name="vector2">The second endpoint of the line segment.
* </param>
* <param name="vector3">The point to which the squared distance is to
* be calculated.</param>
*/
internal static KInt distSqPointLineSegment(KInt2 vector1, KInt2 vector2, KInt2 vector3)
{
KInt r = Dot(vector3 - vector1, vector2 - vector1) / absSq(vector2 - vector1);// (v31.IntX * v21.IntX + v31.IntY * v21.IntY) * KInt.divscale / KInt2.div2scale;
if (r < 0)
{
return(absSq(vector3 - vector1));
}
if (r > 1)
{
return(absSq(vector3 - vector2));
}
return(absSq(vector3 - (vector1 + r * (vector2 - vector1))));
}
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_);
}
}
}
}
}
/**
* <summary>Adds a new agent to the simulation.</summary>
*
* <returns>The number of the agent.</returns>
*
* <param name="position">The two-dimensional starting position of this
* agent.</param>
* <param name="neighborDist">The maximum distance (center point to
* center point) to other agents this agent takes into account in the
* navigation. The larger this number, the longer the running time of
* the simulation. If the number is too low, the simulation will not be
* safe. Must be non-negative.</param>
* <param name="maxNeighbors">The maximum number of other agents this
* agent takes into account in the navigation. The larger this number,
* the longer the running time of the simulation. If the number is too
* low, the simulation will not be safe.</param>
* <param name="timeHorizon">The minimal amount of time for which this
* agent's velocities that are computed by the simulation are safe with
* respect to other agents. The larger this number, the sooner this
* agent will respond to the presence of other agents, but the less
* freedom this agent has in choosing its velocities. Must be positive.
* </param>
* <param name="timeHorizonObst">The minimal amount of time for which
* this agent's velocities that are computed by the simulation are safe
* with respect to obstacles. The larger this number, the sooner this
* agent will respond to the presence of obstacles, but the less freedom
* this agent has in choosing its velocities. Must be positive.</param>
* <param name="radius">The radius of this agent. Must be non-negative.
* </param>
* <param name="maxSpeed">The maximum speed of this agent. Must be
* non-negative.</param>
* <param name="velocity">The initial two-dimensional linear velocity of
* this agent.</param>
*/
public int addAgent(KInt2 position, KInt neighborDist, int maxNeighbors, KInt timeHorizon, KInt timeHorizonObst, KInt radius, KInt maxSpeed, KInt2 velocity)
{
Agent agent = new Agent();
agent.id_ = s_totalID;
s_totalID++;
agent.maxNeighbors_ = maxNeighbors;
agent.maxSpeed_ = maxSpeed;
agent.neighborDist_ = neighborDist;
agent.position_ = position;
agent.radius_ = radius;
agent.timeHorizon_ = timeHorizon;
agent.timeHorizonObst_ = timeHorizonObst;
agent.velocity_ = velocity;
agents_.Add(agent);
onAddAgent();
return(agent.id_);
}
// Use this for initialization
void Start()
{
KInt timestep = 0.25f;
Simulator.Instance.setTimeStep(timestep);
//设置单线程，true:Unity主线程工作，false:开启多线程
Simulator.Instance.SetSingleTonMode(true);
Simulator.Instance.setAgentDefaults(15, 10, 5, 5, 2, 2, KInt2.zero);
for (int i = 0; i < agentCount; i++)
{
float angle = ((float)i / agentCount) * (float)System.Math.PI * 2;
Vector3 pos = new Vector3((float)System.Math.Cos(angle), 0, (float)System.Math.Sin(angle)) * ringSize;
Vector3 antipodal = -pos + goalOffset;
int sid = Simulator.Instance.addAgent((KInt2)pos, neighborDist, maxNeighbors, timeHorizon, timeHorizonObst, radius, maxSpeed, velocity);
if (sid >= 0)
{
GameObject go = LeanPool.Spawn(agentPrefab, new Vector3(pos.x, 0, pos.y), Quaternion.Euler(0, angle + 180, 0));
go.transform.parent = transform;
go.transform.position = pos;
RVOPathAgent ga = go.GetComponent <RVOPathAgent>();
Assert.IsNotNull(ga);
ga.sid = sid;
m_agentMap.Add(sid, ga);
}
}
Simulator.Instance.SetNumWorkers(0);
SetObstacles();
Simulator.Instance.processObstacles();
style = new GUIStyle()
{
fontSize = 26,
alignment = TextAnchor.MiddleCenter,
normal = new GUIStyleState()
{
textColor = Color.white
}
};
}
protected СвязанСДокументом(XmlElement el) : base(el)
{
shortTextPrefix = Resources.GetString("shortTextPrefix");
shortTextPostfix = "";
htmlPrefix = Resources.GetString("htmlPrefix");
htmlPostfix = "";
textItemPrefix = "[";
textItemPostfix = "]";
int id = KInt.FromXmlString(el.GetAttribute("ID"));
docID = id;
osnovaniya = KBoolean.FromXmlString(el.GetAttribute("Osnovaniya"), true);
osnovaniyaAll = KBoolean.FromXmlString(el.GetAttribute("OsnovaniyaAll"), false);
vytekayuschie = KBoolean.FromXmlString(el.GetAttribute("Vytekayuschie"), true);
vytekayuschieAll = KBoolean.FromXmlString(el.GetAttribute("VytekayuschieAll"), false);
}
/**
* <summary>Inserts a static obstacle neighbor into the set of neighbors
* of this agent.</summary>
*
* <param name="obstacle">The number of the static obstacle to be
* inserted.</param>
* <param name="rangeSq">The squared range around this agent.</param>
*/
internal void insertObstacleNeighbor(Obstacle obstacle, KInt rangeSq)
{
Obstacle nextObstacle = obstacle.next_;
KInt distSq = RVOMath.distSqPointLineSegment(obstacle.point_, nextObstacle.point_, position_);
if (distSq < rangeSq)
{
obstacleNeighbors_.Add(new KeyValuePair <KInt, Obstacle>(distSq, obstacle));
int i = obstacleNeighbors_.Count - 1;
while (i != 0 && distSq < obstacleNeighbors_[i - 1].Key)
{
obstacleNeighbors_[i] = obstacleNeighbors_[i - 1];
--i;
}
obstacleNeighbors_[i] = new KeyValuePair <KInt, Obstacle>(distSq, obstacle);
}
}
/**
* <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_);
}
}
}
}
}
/**
* <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(KInt2 q1, KInt2 q2, KInt radius, ObstacleTreeNode node)
{
if (node == null)
{
return(true);
}
Obstacle obstacle1 = node.obstacle_;
Obstacle obstacle2 = obstacle1.next_;
KInt q1LeftOfI = RVOMath.leftOf(obstacle1.point_, obstacle2.point_, q1);
KInt q2LeftOfI = RVOMath.leftOf(obstacle1.point_, obstacle2.point_, q2);
KInt LengthI = RVOMath.absSq(obstacle2.point_ - obstacle1.point_);
// KInt invLengthI = 1.0f / RVOMath.absSq(obstacle2.point_ - obstacle1.point_);
if (q1LeftOfI >= 0 && q2LeftOfI >= 0)
{
return(queryVisibilityRecursive(q1, q2, radius, node.left_) && ((RVOMath.sqr(q1LeftOfI) / LengthI >= RVOMath.sqr(radius) && RVOMath.sqr(q2LeftOfI) / LengthI >= RVOMath.sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node.right_)));
}
if (q1LeftOfI <= 0 && q2LeftOfI <= 0)
{
return(queryVisibilityRecursive(q1, q2, radius, node.right_) && ((RVOMath.sqr(q1LeftOfI) / LengthI >= RVOMath.sqr(radius) && RVOMath.sqr(q2LeftOfI) / LengthI >= RVOMath.sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node.left_)));
}
if (q1LeftOfI >= 0 && q2LeftOfI <= 0)
{
/* One can see through obstacle from left to right. */
return(queryVisibilityRecursive(q1, q2, radius, node.left_) && queryVisibilityRecursive(q1, q2, radius, node.right_));
}
KInt point1LeftOfQ = RVOMath.leftOf(q1, q2, obstacle1.point_);
KInt point2LeftOfQ = RVOMath.leftOf(q1, q2, obstacle2.point_);
KInt LengthQ = RVOMath.absSq(q2 - q1);
// KInt invLengthQ = 1.0f / RVOMath.absSq(q2 - q1);
return(point1LeftOfQ * point2LeftOfQ >= 0 && RVOMath.sqr(point1LeftOfQ) / LengthQ > RVOMath.sqr(radius) && RVOMath.sqr(point2LeftOfQ) / LengthQ > RVOMath.sqr(radius) && queryVisibilityRecursive(q1, q2, radius, node.left_) && queryVisibilityRecursive(q1, q2, radius, node.right_));
}
/**
* <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, KInt radius, KInt2 optVelocity, bool directionOpt, ref KInt2 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)
{
/* Result does not satisfy constraint i. Compute new optimal result. */
KInt2 tempResult = result;
if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, ref result))
{
result = tempResult;
return(i);
}
}
}
return(lines.Count);
}
/**
* <summary>Sets the maximum neighbor distance of a specified agent.
* </summary>
*
* <param name="agentNo">The number of the agent whose maximum neighbor
* distance is to be modified.</param>
* <param name="neighborDist">The replacement maximum neighbor distance.
* Must be non-negative.</param>
*/
public void setAgentNeighborDist(int agentNo, KInt neighborDist)
{
agents_[agentNo2indexDict_[agentNo]].neighborDist_ = neighborDist;
}
/**
* <summary>Sets the maximum speed of a specified agent.</summary>
*
* <param name="agentNo">The number of the agent whose maximum speed is
* to be modified.</param>
* <param name="maxSpeed">The replacement maximum speed. Must be
* non-negative.</param>
*/
public void setAgentMaxSpeed(int agentNo, KInt maxSpeed)
{
agents_[agentNo2indexDict_[agentNo]].maxSpeed_ = maxSpeed;
}
/**
* <summary>Performs a visibility query between the two specified points
* with respect to the obstacles.</summary>
*
* <returns>A boolean specifying whether the two points are mutually
* visible. Returns true when the obstacles have not been processed.
* </returns>
*
* <param name="point1">The first point of the query.</param>
* <param name="point2">The second point of the query.</param>
* <param name="radius">The minimal distance between the line connecting
* the two points and the obstacles in order for the points to be
* mutually visible (optional). Must be non-negative.</param>
*/
public bool queryVisibility(KInt2 point1, KInt2 point2, KInt radius)
{
return(kdTree_.queryVisibility(point1, point2, radius));
}
/**
* <summary>Performs a simulation step and updates the two-dimensional
* position and two-dimensional velocity of each agent.</summary>
*
* <returns>The global time after the simulation step.</returns>
*/
public KInt doStep_Update()
{
if (isError)
{
return(globalTime_);
}
try
{
updateDeleteAgent();
if (!singletonMode)
{
if (workers_ == null)
{
workers_ = new Worker[numWorkers_];
doneEvents_ = new ManualResetEvent[workers_.Length];
workerAgentCount_ = getNumAgents();
for (int index_block = 0; index_block < workers_.Length; ++index_block)
{
doneEvents_[index_block] = new ManualResetEvent(false);
workers_[index_block] = new Worker(index_block * getNumAgents() / workers_.Length, (index_block + 1) * getNumAgents() / workers_.Length, doneEvents_[index_block]);
}
}
if (workerAgentCount_ != getNumAgents())
{
workerAgentCount_ = getNumAgents();
for (int block = 0; block < workers_.Length; ++block)
{
workers_[block].config(block * getNumAgents() / workers_.Length, (block + 1) * getNumAgents() / workers_.Length);
}
}
}
kdTree_.buildAgentTree();
if (!singletonMode)
{
for (int block = 0; block < workers_.Length; ++block)
{
doneEvents_[block].Reset();
ThreadPool.QueueUserWorkItem(workers_[block].step_Update);
}
WaitHandle.WaitAll(doneEvents_);
for (int block = 0; block < workers_.Length; ++block)
{
doneEvents_[block].Reset();
ThreadPool.QueueUserWorkItem(workers_[block].update);
}
WaitHandle.WaitAll(doneEvents_);
}
else
{
for (int index = 0; index < getNumAgents(); ++index)
{
agents_[index].computeNeighbors();
agents_[index].computeNewVelocity();
}
for (int index = 0; index < getNumAgents(); ++index)
{
agents_[index].update();
}
}
globalTime_ += timeStep_;
return(globalTime_);
}
catch (Exception ex)
{
LogMgr.LogError(ex);
this.isError = true;
return(globalTime_);
}
}
