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);