void SetPreferredVelocities()
{
/*
* Set the preferred velocity to be a vector of unit magnitude
* (speed) in the direction of the goal.
*/
Vector3 g = Vector3.zero;
for (int i = 0; i < Simulator.Instance.getNumAgents(); ++i)
{
RVO.Vector2 goalVector = goals[i] - Simulator.Instance.getAgentPosition(i);
if (RVOMath.absSq(goalVector) > 1.0f)
{
goalVector = RVOMath.normalize(goalVector);
}
Simulator.Instance.setAgentPrefVelocity(i, goalVector);
}
}
public void Init()
{
Simulator.Instance.setTimeStep(0.15f);
Simulator.Instance.setAgentDefaults(neibourDist, maxNeibours, timeHorizon, obstacleTimeHorizon, radius, maxSpeed, new RVO.Vector2(0.0f, 0.0f));
for (int i = 0; i < agentCount; i++)
{
RVO.Vector2 pos = circleRadius *
new RVO.Vector2((float)Math.Cos(i * 2.0f * Math.PI / agentCount),
(float)Math.Sin(i * 2.0f * Math.PI / agentCount));
Simulator.Instance.addAgent(pos);
dirs.Add(RVOMath.normalize(-Simulator.Instance.getAgentPosition(i) - Simulator.Instance.getAgentPosition(i)));
goals.Add(-Simulator.Instance.getAgentPosition(i));
GameObject go = GameObject.CreatePrimitive(PrimitiveType.Capsule);
go.transform.position = new Vector3(pos.x(), 120, pos.y());
agents.Add(go);
}
ConvertToObstacle();
inited = true;
}
private void Update()
{
if (inited == false)
{
return;
}
//if ((Time.time - lastTime) > 0.25f)
//{
// lastTime = Time.time;
// for (int i = 0; i < goals.Count; i++)
// {
// goals[i] += dirs[i] * 0.8f;
// Debug.DrawLine(goals[i].ToVec3XZ(),goals[i].ToVec3XZ()+Vector3.up*5f,Color.cyan,0.25f);
// }
//}
if (ReachedGoal() == false)
{
SetPreferredVelocities();
Simulator.Instance.doStep();
}
for (int i = 0; i < agents.Count; i++)
{
vec2 = Simulator.Instance.getAgentPosition(i);
realPos.x = vec2.x();
realPos.z = vec2.y();
vec2 = Simulator.Instance.getAgentVelocity(i);
velocity.x = vec2.x_;
velocity.z = vec2.y_;
agentPos = agents[i].transform.position;
Debug.DrawLine(agentPos + Vector3.up * 1f,
agentPos + velocity.normalized * 2.5f + Vector3.up * 1f,
Color.green, Time.deltaTime);
Debug.DrawLine(realPos, realPos + Vector3.up * 10, Color.red, Time.deltaTime);
agents[i].transform.position = Vector3.Lerp(agentPos, realPos, Time.deltaTime * RVOMath.abs(Simulator.Instance.getAgentVelocity(i)));
}
}
private Vector3 ToVec3(RVO.Vector2 vec)
{
return(new Vector3(vec.x(), 0, vec.y()));
}
// Update is called once per frame
void Update()
{
//RVO.Simulator.Instance.setTimeStep(Time.deltaTime);
//if(RVO.Simulator.Instance.getNumAgents()== MAX_AGENT)
RVO.Simulator.Instance.doStep();
foreach (var AgentItem in m_agentIdlist)
{
RVO.Vector2 pos = RVO.Simulator.Instance.getAgentPosition(AgentItem.Key);
RVO.Vector2 vel = RVO.Simulator.Instance.getAgentVelocity(AgentItem.Key);
Vector3 gmPos = AgentItem.Value.position;
gmPos.x = pos.x_; gmPos.z = pos.y_;
AgentItem.Value.position = gmPos;
}
for (int i = 0; i < m_obstacleList.Count; ++i)
{
//Simulator.Instance.getNextObstacleVertexNo(i);
int hObs = i;
RVO.Vector2 v0;
RVO.Vector2 v1;
Vector3 from;
Vector3 to;
from.y = to.y = 0.1f;
do
{
int next = Simulator.Instance.getNextObstacleVertexNo(hObs);
v0 = Simulator.Instance.getObstacleVertex(hObs);
v1 = Simulator.Instance.getObstacleVertex(next);
from.x = v0.x_; from.z = v0.y_;
to.x = v1.x_; to.z = v1.y_;
m_debugDraw.DrawLine(m_hDrawObstacle, from, to, Color.red);
hObs = Simulator.Instance.getNextObstacleVertexNo(hObs);
}while (hObs != i);
}
m_debugDraw.ClearLine(m_hDrawPointList);
Vector3 lineFrom;
Vector3 lineTo;
lineFrom.y = lineTo.y = 0.1f;
int pointCount = m_pointList.Count;
for (int i = 0; i < pointCount; ++i)
{
RVO.Vector2 v0 = m_pointList[i];
RVO.Vector2 v1 = (i + 1) < pointCount ? m_pointList[i + 1] : m_pointList[0];
lineFrom.x = v0.x_; lineFrom.z = v0.y_;
lineTo.x = v1.x_; lineTo.z = v1.y_;
m_debugDraw.DrawLine(m_hDrawPointList, lineFrom, lineTo, Color.blue);
}
if (Input.GetMouseButtonDown(0))
{
do
{
Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
RaycastHit hit;
if (!Physics.Raycast(ray, out hit))
{
break;
}
if (m_mouseFunc == MouseFunc.AddAgent)
{
if (hit.collider.gameObject.layer != LayerMask.NameToLayer("Ground"))
{
break;
}
GameObject gmAgent = Instantiate(m_playerPrefabs, hit.point, Quaternion.identity) as GameObject;
m_agentIdlist.Add(gmAgent.GetComponent <RVO2Agent>().RVOAgentHandle, gmAgent.transform);
}
if (m_mouseFunc == MouseFunc.AddObstacle)
{
RVO.Vector2 hitPos = new RVO.Vector2(hit.point.x, hit.point.z);
if (m_pointList.Count > 2)
{
RVO.Vector2 last = m_pointList[m_pointList.Count - 1];
if (RVO.RVOMath.absSq(hitPos - last) <= 0.05f)
{
int hObstacle = RVO.Simulator.Instance.addObstacle(m_pointList);
m_obstacleList.Add(hObstacle);
m_pointList.Clear();
RVO.Simulator.Instance.processObstacles();
break;
}
}
m_pointList.Add(hitPos);
}
if (m_mouseFunc == MouseFunc.MovAgent)
{
if (hit.collider.gameObject.layer == LayerMask.NameToLayer("Ground"))
{
if (m_selectedAgent)
{
m_selectedAgent.SetDestPos(hit.point);
}
break;
}
if (hit.collider.gameObject.layer == LayerMask.NameToLayer("Player"))
{
m_selectedAgent = hit.collider.gameObject.GetComponent <RVO2Agent>();
}
}
} while (false);
}
if (Input.GetMouseButton(0))
{
Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
RaycastHit hit;
Physics.Raycast(ray, out hit);
Vector3 MAX_VECTOR = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
if (m_min == MAX_VECTOR)
{
m_min = hit.point;
}
m_max = hit.point;
if (m_selectCollider.enabled == false && (m_max - m_min).sqrMagnitude > 1.0f)
{
m_selectCollider.enabled = true;
}
if (m_selectCollider != null)
{
m_selectCollider.center = Vector3.Lerp(m_min, m_max, 0.5f);
Vector3 delta = m_max - m_min;
m_selectCollider.size = new Vector3(Mathf.Abs(delta.x), 2.0f, Mathf.Abs(delta.z));
}
}
else
{
m_min = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
m_max = new Vector3(float.MinValue, float.MinValue, float.MinValue);
m_selectCollider.enabled = false;
}
}
