private void Update()
{
if (reachedGoal)
{
return;
}
SetTimeStep(sim, Time.deltaTime);
var nAgents = GetNumAgents(sim);
for (ulong i = 0; i < nAgents; ++i)
{
RvoVector3 pos = RvoVector3.Zero;
RvoVector3 vel = RvoVector3.Zero;
if (GetAgentPosition(ref pos, sim, i) &&
GetAgentVelocity(ref vel, sim, i))
{
//Debug.Log($"{i}:{pos.x}, {pos.y}, {pos.z}");
// update go position
var v3Pos = new Vector3(pos.x, pos.y, pos.z);
gos[(int)i].position = v3Pos;
var v3Vel = new Vector3(vel.x, vel.y, vel.z);
if (v3Vel.sqrMagnitude > float.Epsilon)
{
gos[(int)i].rotation = Quaternion.LookRotation(v3Vel);
}
var goalVector = goals[(int)i] - v3Pos;
if (goalVector.sqrMagnitude > 1.0f)
{
goalVector = goalVector.normalized * speed;
}
SetAgentPrefVelocity(sim, i, new RvoVector3(goalVector));
}
}
DoStep(sim);
reachedGoal = ReachGoal();
}
private bool ReachGoal()
{
RvoVector3 pos = RvoVector3.Zero;
if (GetAgentPosition(ref pos, sim, 0))
{
float radius = 0.0f;
if (GetAgentRadius(ref radius, sim, 0))
{
var v3Pos = new Vector3(pos.x, pos.y, pos.z);
var goalVector = goals[0] - v3Pos;
if (goalVector.sqrMagnitude < radius * radius)
{
return(true);
}
}
}
return(false);
}
public static extern void SetAgentPrefVelocity(IntPtr sim, ulong agentNo, RvoVector3 prefVelocity);
public static extern bool GetAgentVelocity(ref RvoVector3 velocity, IntPtr sim, ulong agentNo);
public static extern bool GetAgentPosition(ref RvoVector3 pos, IntPtr sim, ulong agentNo);
public static extern ulong AddAgent(IntPtr sim, RvoVector3 position);
public static extern void SetAgentDefaults(IntPtr sim,
float neighborDist, ulong maxNeighbors,
float timeHorizon, float radius, float maxSpeed,
RvoVector3 velocity);