public void insertAgentNeighbor(RVOAgent agent, ref float rangeSq)
{
if (this != agent)
{
float distSq = RVOMath.absSq(Position - agent.Position);
if (distSq < rangeSq)
{
if (agentNeighbors_.Count < maxNeighbors_)
{
agentNeighbors_.Add(new KeyValuePair <float, RVOAgent>(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 <float, RVOAgent>(distSq, agent);
if (agentNeighbors_.Count == maxNeighbors_)
{
rangeSq = agentNeighbors_[agentNeighbors_.Count - 1].Key;
}
}
}
}
public int addAgent(Vector2 position, int type, bool follow, bool group,
float neighborDist, int maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, Vector2 velocity)
{
if (defaultAgent_ == null)
{
return(RVO_ERROR);
}
Agent agent;
switch (type)
{
case 0:
{
agent = new RVOAgent(this);
}
break;
case 1:
{
agent = new HelbingAgent(this, 0);
}
break;
case 2:
{
agent = new HelbingAgent(this, 1);
}
break;
default:
return(RVO_ERROR);
}
agent.position_ = position;
agent.maxNeighbors_ = maxNeighbors;
agent.maxSpeed_ = maxSpeed;
agent.neighborDist_ = neighborDist;
agent.radius_ = radius;
agent.timeHorizon_ = timeHorizon;
agent.timeHorizonObst_ = timeHorizonObst;
agent.velocity_ = velocity;
agent.follows_ = follow;
agent.considerGroups_ = group;
agent.id_ = agents_.Count;
agents_.Add(agent);
return(agents_.Count - 1);
}
// This put transform and rigidbody in cache
public override void Awake()
{
base.Awake(); //does the caching.
maxSpeed = speed * 2.1f;
// Set the radius
CapsuleCollider capsuleCollider = GetComponent <Collider>() as CapsuleCollider;
radius = capsuleCollider.radius;
// Handling RVO
RVOAgent = new RVO.RVOAgent();
RVOAgent.MaxSpeed = speed;
RVOAgent.Radius = radius;
RVOAgent.Position = new RVO.Vector2(transform.position.x, transform.position.z);
RVO.Simulator.Instance.addAgent(RVOAgent);
}
public int addVirtualAgent(int model_no, Vector2 position)
{
Agent model = agents_[model_no];
Agent agent;
agent = new RVOAgent(this);
agent.position_ = position;
agent.radius_ = model.radius_;
agent.velocity_ = model.velocity_;
agent.velocityBuffer_ = model.velocityBuffer_;
agent.follows_ = model.follows_;
agent.considerGroups_ = model.considerGroups_;
agent.id_ = virtual_and_agents_.Count;
virtual_and_agents_.Add(agent);
return(virtual_and_agents_.Count - 1);
}
/* 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);
}
}
public void removeAgent(RVOAgent agent)
{
// ******** BUG: This will shift your indices and break agentId!! ********
agents_.Remove(agent);
}
public int addAgent(RVOAgent agent)
{
agents_.Add(agent);
return(agents_.Count - 1);
}