public int AddAgent(GameObject agentGameObject)
{
rvoGameObject.Add(agentGameObject);
agentPosition.Add(RVOWithUnity.Vec3ToVec2(agentGameObject.transform.position));
RVOAgent rvoAgent = agentGameObject.GetComponent <RVOAgent>();
if (rvoAgent.isPlayer == false)
{
return(Simulator.Instance.addAgent(RVOWithUnity.Vec3ToVec2(agentGameObject.transform.position)));
}
else
{
return(Simulator.Instance.addAgent(RVOWithUnity.Vec3ToVec2(agentGameObject.transform.position),
neighborDist,
maxNeighbor,
rvoAgent.timeHorizon,
timeHorizonObst,
rvoAgent.radius,
rvoAgent.maxSpeed,
new RVO.Vector2(0.0f, 0.0f),
rvoAgent.acceleration,
rvoAgent.angularSpeed,
rvoAgent.minSpeedToTurn
));
}
}
// Start is called before the first frame update
void Start()
{
if (playerMakerSFM == null)
{
gameObject.GetComponent <PlayMakerFSM>();
}
agent = GetComponent <RVOAgent>();
}
// Use this for initialization
protected virtual void Start()
{
rvoAgent = this.GetComponent <RVOAgent>();
characterController = this.GetComponent <CharacterController>();
animancer = this.GetComponent <AnimancerComponent>();
lastState = currentState;
}
public static Vector2 GetDirection(RVOAgent agent)
{
Vector2 nowVelocity = Simulator.Instance.getAgentVelocity(agent.GetIndex());
if (RVOMath.abs(nowVelocity) < eps)
{
nowVelocity = agent.GetDirection();
}
return(RVOMath.normalize(nowVelocity));
}
void FixedUpdate()
{
int agentAmount = Simulator.Instance.getNumAgents();
for (int i = 0; i < agentAmount; i++)
{
Vector2 agentVec2 = Simulator.Instance.getAgentPosition(i);
Vector2 station = rvoGameObject[i].GetComponent <RVOAgent>().GetStation();
RVOAgent agent = rvoGameObject[i].GetComponent <RVOAgent>();
Vector2 prefVelocity = RVOWithUnity.CalcPrefVelocity(agentVec2, station, agent);
Simulator.Instance.setAgentPrefVelocity(i, prefVelocity);
}
Simulator.Instance.doStep();
}
public void computeAgentNeighbors(RVOAgent agent, ref float rangeSquared)
{
queryAgentTreeRecursive(agent, ref rangeSquared, 0);
}
void queryAgentTreeRecursive(RVOAgent 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 = Squared(Mathf.Max(0.0f, agentTree[agentTree[node].left].minCoord[0] - agent.position.x))
+ Squared(Mathf.Max(0.0f, agent.position.x - agentTree[agentTree[node].left].maxCoord[0]))
+ Squared(Mathf.Max(0.0f, agentTree[agentTree[node].left].minCoord[1] - agent.position.y))
+ Squared(Mathf.Max(0.0f, agent.position.y - agentTree[agentTree[node].left].maxCoord[1]))
+ Squared(Mathf.Max(0.0f, agentTree[agentTree[node].left].minCoord[2] - agent.position.z))
+ Squared(Mathf.Max(0.0f, agent.position.z - agentTree[agentTree[node].left].maxCoord[2]));
float distSqRight = Squared(Mathf.Max(0.0f, agentTree[agentTree[node].right].minCoord[0] - agent.position.x))
+ Squared(Mathf.Max(0.0f, agent.position.x - agentTree[agentTree[node].right].maxCoord[0]))
+ Squared(Mathf.Max(0.0f, agentTree[agentTree[node].right].minCoord[1] - agent.position.y))
+ Squared(Mathf.Max(0.0f, agent.position.y - agentTree[agentTree[node].right].maxCoord[1]))
+ Squared(Mathf.Max(0.0f, agentTree[agentTree[node].right].minCoord[2] - agent.position.z))
+ Squared(Mathf.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);
}
}
}
}
}
/// <summary>
/// Inserts the agent neighbor.
/// </summary>
/// <param name="agent">The agent.</param>
/// <param name="rangeSquared">The range squared.</param>
public void insertAgentNeighbor(RVOAgent agent, ref float rangeSquared)
{
if(this != agent)
{
float distSq = (position - agent.position).sqrMagnitude;
if (distSq < rangeSquared)
{
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)
{
rangeSquared = agentNeighbors[agentNeighbors.Count - 1].Key;
}
}
}
}
public static Vector2 CalcPrefVelocity(Vector2 agentPosition, Vector2 goalPosisiton, RVOAgent agent)
{
Vector2 prefDistance = goalPosisiton - agentPosition;
if (agent.GetIsStop() == true)
{
return(new Vector2(0f, 0f));
}
if (agent.isPlayer == true)
{
/* Is inertial model used */
Vector2 nowVelocity = Simulator.Instance.getAgentVelocity(agent.GetIndex());
float nowSpeed = RVOMath.abs(nowVelocity);
Vector2 prefVelocity = RVOMath.normalize(prefDistance) * agent.prefSpeed;//nowSpeed -> prefSpeed
Vector2 finalVelocity = new Vector2(0, 0);
if (nowSpeed < agent.minSpeedToTurn)
{
/* Not fast enough to turn */
Vector2 advanceDirection = GetDirection(agent);
if (agent.prefSpeed > nowSpeed) //Speed up fitting
{
finalVelocity = nowVelocity + advanceDirection * agent.acceleration;
}
else //Speed down fitting
{
if (nowSpeed - agent.prefSpeed < agent.acceleration)
{
finalVelocity = prefVelocity;
}
else
{
finalVelocity = nowVelocity - advanceDirection * agent.acceleration;
}
}
}
else
{
/* turn */
float direction = RVOMath.det(nowVelocity, prefVelocity);
if (direction > eps) //left
{
Vector2 rightVerticalVelocity = new Vector2(-nowVelocity.y_, nowVelocity.x_);
rightVerticalVelocity = RVOMath.normalize(rightVerticalVelocity) * agent.speed * agent.angularSpeed;
if (RVOMath.det(nowVelocity + rightVerticalVelocity, prefVelocity) < eps) //success
{
finalVelocity = RVOMath.normalize(prefVelocity) * nowSpeed;
}
else
{
finalVelocity = RVOMath.normalize(nowVelocity + rightVerticalVelocity) * nowSpeed;
}
}
if (direction >= -eps && direction <= eps) //same direction
{
finalVelocity = RVOMath.normalize(prefVelocity) * nowSpeed;
}
if (direction < -eps) //right
{
Vector2 leftVerticalVelocity = new Vector2(nowVelocity.y_, -nowVelocity.x_);
leftVerticalVelocity = RVOMath.normalize(leftVerticalVelocity) * agent.speed * agent.angularSpeed;
if (RVOMath.det(nowVelocity + leftVerticalVelocity, prefVelocity) > -eps) //success
{
finalVelocity = RVOMath.normalize(prefVelocity) * nowSpeed;
}
else
{
finalVelocity = RVOMath.normalize(nowVelocity + leftVerticalVelocity) * nowSpeed;
}
}
//Debug.Log("!!!" + RVOMath.abs(finalVelocity).ToString());
/* acc and slow */
if (agent.prefSpeed - nowSpeed > eps) //acc
{
if (IsVectorNear(nowVelocity, prefVelocity, 0.05f))
{
finalVelocity = finalVelocity + RVOMath.normalize(finalVelocity) * Mathf.Min(agent.acceleration, agent.prefSpeed - nowSpeed);
}
}
else //slow
{
finalVelocity = finalVelocity - RVOMath.normalize(finalVelocity) * Mathf.Min(agent.acceleration, nowSpeed - agent.prefSpeed);
}
// Debug.Log("!!!" + RVOMath.abs(finalVelocity).ToString());
}
Vector2 ditherVelocity = GetDitherVelocity(ditherSize);
if (RVOMath.abs(finalVelocity) < eps)
{
return(finalVelocity);
}
else
{
return(finalVelocity + ditherVelocity);
}
}
else
{
Vector2 ditherVelocity = GetDitherVelocity(ditherSize);
Vector2 prefVelocity = RVOMath.normalize(prefDistance);
return(prefVelocity * agent.prefSpeed + ditherVelocity);
}
}