void LateUpdate()
{
sim.setTimeStep(Time.deltaTime);
//desired vels
for (int i = 0; i < units.Count; i++)
{
Vector3 pos = units[i].GetFeetPosition();
var v = units[i].GetVelocity();
var vdes = units[i].DesiredVelocity;
var rvoAgent = sim.agents_[i];
rvoAgent.position_ = new RVO.Vector2(pos.x, pos.z);
rvoAgent.velocity_ = new RVO.Vector2(v.x, v.z);
rvoAgent.prefVelocity_ = new RVO.Vector2(vdes.x, vdes.z);
}
//simulate
sim.doStep();
//set new vels
for (int i = 0; i < units.Count; i++)
{
var vnew = sim.agents_[i].velocity_;
units[i].Move(new Vector3(vnew.x_, 0, vnew.y_));
}
}
public void Update()
{
m_timeSum += Time.deltaTime;
if (sim != null)
{
if (m_timeSum > sim.timeStep_)
{
sim.doStep();
m_timeSum -= sim.timeStep_;
}
}
m_sb.Length = 0;
foreach (var i in m_agentIds)
{
var pos = RVO.Simulator.Instance.getAgentPosition(i);
var vel = RVO.Simulator.Instance.getAgentVelocity(i);
var radius = RVO.Simulator.Instance.getAgentRadius(i);
float orient = Mathf.Atan2(vel.y(), vel.x());
m_sb.Append(string.Format("id:{0} pos:{1} vel:{2} orieng:{3}", i, pos, vel, orient));
m_sb.AppendLine("");
var target = new RVO.Vector2(goals[i].x, goals[i].z);
RVO.Simulator.Instance.setAgentPrefVelocity(i, RVO.RVOMath.normalize(target - pos) * m_maxSpeed);
Vector3 forward = new Vector3(Mathf.Cos(orient), 0, Mathf.Sin(orient)).normalized *radius;
if (forward == Vector3.zero)
{
forward = new Vector3(0, 0, radius);
}
Vector3 right = Vector3.Cross(Vector3.up, forward);
Vector3 orig = new Vector3(pos.x(), 0, pos.y()) + renderingOffset;
int vc = 4 * i;
int tc = 2 * 3 * i;
verts[vc + 0] = (orig + forward - right);
verts[vc + 1] = (orig + forward + right);
verts[vc + 2] = (orig - forward + right);
verts[vc + 3] = (orig - forward - right);
uv[vc + 0] = (new Vector2(0, 1));
uv[vc + 1] = (new Vector2(1, 1));
uv[vc + 2] = (new Vector2(1, 0));
uv[vc + 3] = (new Vector2(0, 0));
meshColors[vc + 0] = colors[i];
meshColors[vc + 1] = colors[i];
meshColors[vc + 2] = colors[i];
meshColors[vc + 3] = colors[i];
tris[tc + 0] = (vc + 0);
tris[tc + 1] = (vc + 1);
tris[tc + 2] = (vc + 2);
tris[tc + 3] = (vc + 0);
tris[tc + 4] = (vc + 2);
tris[tc + 5] = (vc + 3);
}
Debug.Log(m_sb.ToString());
//Update the mesh
mesh.Clear();
mesh.vertices = verts;
mesh.uv = uv;
mesh.colors = meshColors;
mesh.triangles = tris;
mesh.RecalculateNormals();
}