Exemplo n.º 1
0
        public bool Init(int maxAgents, float maxAgentRadius, NavMesh nav)
        {
            Purge();
            _maxAgents      = maxAgents;
            _maxAgentRadius = maxAgentRadius;

            Helper.VSet(ref _ext, _maxAgentRadius * 2.0f, _maxAgentRadius * 1.5f, _maxAgentRadius * 2.0f);

            _grid = new ProximityGrid();
            _grid.Init(_maxAgents * 4, maxAgentRadius * 3);

            _obstacleQuery = new ObstacleAvoidanceQuery();
            _obstacleQuery.Init(6, 8);

            for (int i = 0; i < _obstacleQueryParams.Length; i++)
            {
                _obstacleQueryParams[i] = new ObstacleAvoidanceParams
                {
                    velBias       = 0.4f,
                    weightDesVel  = 2.0f,
                    weightCurVel  = 0.75f,
                    weightSide    = 0.75f,
                    weightToi     = 2.5f,
                    horizTime     = 2.5f,
                    gridSize      = 33,
                    adaptiveDivs  = 7,
                    adaptiveRings = 2,
                    adaptiveDepth = 5
                };
            }

            _maxPathResult = 256;
            _pathResult    = new long[_maxPathResult];

            _pathq = new PathQueue();
            _pathq.Init(_maxPathResult, MaxPathQueueNodes, nav);

            _agents       = new CrowdAgent[_maxAgents];
            _activeAgents = new CrowdAgent[_maxAgents];
            _agentAnims   = new CrowdAgentAnimation[_maxAgents];

            for (int i = 0; i < _maxAgents; i++)
            {
                _agents[i]        = new CrowdAgent();
                _agents[i].Active = false;
                _agents[i].Corridor.Init(_maxPathResult);
            }

            for (int i = 0; i < _maxAgents; i++)
            {
                _agentAnims[i]        = new CrowdAgentAnimation();
                _agentAnims[i].Active = false;
            }

            _navQuery = new NavMeshQuery();
            _navQuery.Init(nav, MaxCommonNodes);

            return(true);
        }
Exemplo n.º 2
0
        public void Update(float dt, ref CrowdAgentDebugInfo debug)
        {
            _velocitySampleCount = 0;
            int debugIdx = debug != null ? debug.Idx : -1;

            CrowdAgent[] agents  = _activeAgents;
            int          nagents = GetActiveAgents(ref agents, _maxAgents);

            CheckPathValitidy(agents, nagents, dt);

            UpdateMoveRequest(dt);

            UpdateTopologyOptimization(agents, nagents, dt);

            _grid.Clear();
            for (int i = 0; i < nagents; i++)
            {
                CrowdAgent ag = agents[i];
                float[]    p  = ag.npos;
                float      r  = ag.Param.Radius;
                _grid.AddItem(i, p[0] - r, p[2] - r, p[0] + r, p[2] + r);
            }

            for (int i = 0; i < nagents; i++)
            {
                CrowdAgent ag = agents[i];
                if (ag.State != CrowdAgentState.Walking)
                {
                    continue;
                }

                float updateThr = ag.Param.CollisionQueryRange * 0.25f;
                if (Helper.VDist2DSqr(ag.npos[0], ag.npos[1], ag.npos[2], ag.Boundary.Center[0], ag.Boundary.Center[1], ag.Boundary.Center[2]) > updateThr * updateThr ||
                    !ag.Boundary.IsValid(_navQuery, _filter))
                {
                    ag.Boundary.Update(ag.Corridor.FirstPoly, ag.npos, ag.Param.CollisionQueryRange, _navQuery, _filter);
                }

                ag.NNeis = GetNeighbors(ag.npos, ag.Param.Height, ag.Param.CollisionQueryRange, ag, ref ag.Neis,
                                        CrowdAgent.CrowdAgentMaxNeighbors, agents, nagents, _grid);

                for (int j = 0; j < ag.NNeis; j++)
                {
                    ag.Neis[j].Idx = AgentIndex(agents[ag.Neis[j].Idx]);
                }
            }

            // find next corner to steer to.
            for (int i = 0; i < nagents; i++)
            {
                CrowdAgent ag = agents[i];

                if (ag.State != CrowdAgentState.Walking)
                {
                    continue;
                }
                if (ag.TargetState == MoveRequestState.TargetNone || ag.TargetState == MoveRequestState.TargetVelocity)
                {
                    continue;
                }

                ag.NCorners = ag.Corridor.FindCorners(ref ag.CornerVerts, ref ag.CornerFlags, ref ag.CornerPolys,
                                                      CrowdAgent.CrowdAgentMaxCorners, _navQuery, _filter);

                if ((ag.Param.UpdateFlags & UpdateFlags.OptimizeVisibility) != 0 && ag.NCorners > 0)
                {
                    float[] target = new float[3];
                    Array.Copy(ag.CornerVerts, Math.Min(1, ag.NCorners - 1) * 3, target, 0, 3);
                    ag.Corridor.OptimizePathVisibility(target, ag.Param.PathOptimizationRange, _navQuery, _filter);
                    if (debugIdx == i)
                    {
                        Helper.VCopy(ref debug.OptStart, ag.Corridor.Pos);
                        Helper.VCopy(ref debug.OptEnd, target);
                    }
                }
                else
                {
                    if (debugIdx == i)
                    {
                        Helper.VSet(ref debug.OptStart, 0, 0, 0);
                        Helper.VSet(ref debug.OptEnd, 0, 0, 0);
                    }
                }
            }

            // trigger off-mesh connections
            for (int i = 0; i < nagents; i++)
            {
                CrowdAgent ag = agents[i];

                if (ag.State != CrowdAgentState.Walking)
                {
                    continue;
                }
                if (ag.TargetState == MoveRequestState.TargetNone || ag.TargetState == MoveRequestState.TargetVelocity)
                {
                    continue;
                }

                float triggerRadius = ag.Param.Radius * 2.25f;
                if (ag.OverOffMeshConnection(triggerRadius))
                {
                    int idx = AgentIndex(ag);
                    CrowdAgentAnimation anim = _agentAnims[idx];

                    long[] refs = new long[2];
                    if (ag.Corridor.MoveOverOffmeshConnection(ag.CornerPolys[ag.NCorners - 1], ref refs, ref anim.StartPos, ref anim.EndPos, _navQuery))
                    {
                        Helper.VCopy(ref anim.InitPos, ag.npos);
                        anim.PolyRef = refs[1];
                        anim.Active  = true;
                        anim.T       = 0.0f;
                        anim.TMax    = (Helper.VDist2D(anim.StartPos, anim.EndPos) / ag.Param.MaxSpeed) * 0.5f;

                        ag.State    = CrowdAgentState.OffMesh;
                        ag.NCorners = 0;
                        ag.NNeis    = 0;
                        continue;
                    }
                }
            }

            // Calculate Steering
            for (int i = 0; i < nagents; i++)
            {
                CrowdAgent ag = agents[i];

                if (ag.State != CrowdAgentState.Walking)
                {
                    continue;
                }
                if (ag.TargetState == MoveRequestState.TargetNone)
                {
                    continue;
                }

                float[] dvel = { 0f, 0f, 0f };

                if (ag.TargetState == MoveRequestState.TargetVelocity)
                {
                    Helper.VCopy(ref dvel, ag.TargetPos);
                    ag.DesiredSpeed = Helper.VLen(ag.TargetPos);
                }
                else
                {
                    if ((ag.Param.UpdateFlags & UpdateFlags.AnticipateTurns) != 0)
                    {
                        ag.CalcSmoothSteerDirection(ref dvel);
                    }
                    else
                    {
                        ag.CalcStraightSteerDirection(ref dvel);
                    }

                    float slowDownRadius = ag.Param.Radius * 2;
                    float speedScale     = ag.GetDistanceToGoal(slowDownRadius) / slowDownRadius;

                    ag.DesiredSpeed = ag.Param.MaxSpeed;
                    dvel            = Helper.VScale(dvel[0], dvel[1], dvel[2], ag.DesiredSpeed * speedScale);
                }

                //separation
                if ((ag.Param.UpdateFlags & UpdateFlags.Separation) != 0)
                {
                    float separationDist    = ag.Param.CollisionQueryRange;
                    float invSeparationDist = 1.0f / separationDist;
                    float separationWeight  = ag.Param.SeparationWeight;

                    float   w    = 0;
                    float[] disp = { 0, 0, 0 };

                    for (int j = 0; j < ag.NNeis; j++)
                    {
                        CrowdAgent nei = _agents[ag.Neis[j].Idx];

                        float[] diff = Helper.VSub(ag.npos[0], ag.npos[1], ag.npos[2], nei.npos[0], nei.npos[1], nei.npos[2]);
                        diff[1] = 0;

                        float distSqr = Helper.VLenSqr(diff);
                        if (distSqr < 0.00001f)
                        {
                            continue;
                        }
                        if (distSqr > separationDist * separationDist)
                        {
                            continue;
                        }
                        float dist   = (float)Math.Sqrt(distSqr);
                        float weight = separationWeight * (1.0f - (dist * invSeparationDist * dist * invSeparationDist));

                        Helper.VMad(ref disp, disp, diff, weight / dist);
                        w += 1.0f;
                    }

                    if (w > 0.0001f)
                    {
                        Helper.VMad(ref dvel, dvel, disp, 1.0f / w);
                        float speedSqr     = Helper.VLenSqr(dvel);
                        float desiredSpeed = (ag.DesiredSpeed * ag.DesiredSpeed);
                        if (speedSqr > desiredSpeed)
                        {
                            dvel = Helper.VScale(dvel[0], dvel[1], dvel[2], desiredSpeed / speedSqr);
                        }
                    }
                }

                Helper.VCopy(ref ag.dvel, dvel);
            }

            // Velocity Planning
            for (int i = 0; i < nagents; i++)
            {
                CrowdAgent ag = agents[i];
                if (ag.State != CrowdAgentState.Walking)
                {
                    continue;
                }

                if ((ag.Param.UpdateFlags & UpdateFlags.ObstacleAvoidance) != 0)
                {
                    _obstacleQuery.Reset();

                    for (int j = 0; j < ag.NNeis; j++)
                    {
                        CrowdAgent nei = _agents[ag.Neis[j].Idx];
                        _obstacleQuery.AddCircle(nei.npos, nei.Param.Radius, nei.vel, nei.dvel);
                    }

                    for (int j = 0; j < ag.Boundary.SegmentCount; j++)
                    {
                        float[] s0 = new float[3];
                        float[] s1 = new float[3];
                        Array.Copy(ag.Boundary.GetSegment(j), 0, s0, 0, 3);
                        Array.Copy(ag.Boundary.GetSegment(j), 3, s1, 0, 3);
                        if (Helper.TriArea2D(ag.npos, s0, s1) < 0.0f)
                        {
                            continue;
                        }
                        _obstacleQuery.AddSegment(s0, s1);
                    }

                    ObstacleAvoidanceDebugData vod = null;
                    if (debugIdx == i)
                    {
                        vod = debug.Vod;
                    }

                    bool adaptive = true;
                    int  ns       = 0;

                    ObstacleAvoidanceParams param = _obstacleQueryParams[ag.Param.ObstacleAvoidanceType];

                    if (adaptive)
                    {
                        ns = _obstacleQuery.SampleVelocityAdaptive(ag.npos, ag.Param.Radius, ag.DesiredSpeed, ag.vel,
                                                                   ag.dvel, ref ag.nvel, param, vod);
                    }
                    else
                    {
                        ns = _obstacleQuery.SampleVelocityGrid(ag.npos, ag.Param.Radius, ag.DesiredSpeed, ag.vel,
                                                               ag.dvel, ref ag.nvel, param, vod);
                    }
                    _velocitySampleCount += ns;
                }
                else
                {
                    Helper.VCopy(ref ag.nvel, ag.dvel);
                }
            }

            // Integrate
            for (int i = 0; i < nagents; i++)
            {
                CrowdAgent ag = agents[i];
                if (ag.State != CrowdAgentState.Walking)
                {
                    continue;
                }

                ag.Integrate(dt);
            }

            // Handle Collisions
            float CollisionResolveFactor = 0.7f;

            for (int iter = 0; iter < 4; iter++)
            {
                for (int i = 0; i < nagents; i++)
                {
                    CrowdAgent ag   = agents[i];
                    int        idx0 = AgentIndex(ag);

                    if (ag.State != CrowdAgentState.Walking)
                    {
                        continue;
                    }

                    Helper.VSet(ref ag.disp, 0, 0, 0);

                    float w = 0;

                    for (int j = 0; j < ag.NNeis; j++)
                    {
                        CrowdAgent nei  = _agents[ag.Neis[j].Idx];
                        int        idx1 = AgentIndex(nei);

                        float[] diff = Helper.VSub(ag.npos[0], ag.npos[1], ag.npos[2], nei.npos[0], nei.npos[1],
                                                   nei.npos[2]);
                        diff[1] = 0;

                        float dist = Helper.VLenSqr(diff);
                        if (dist > (ag.Param.Radius + nei.Param.Radius) * (ag.Param.Radius + nei.Param.Radius))
                        {
                            continue;
                        }
                        dist = (float)Math.Sqrt(dist);
                        float pen = (ag.Param.Radius + nei.Param.Radius) - dist;
                        if (dist < 0.0001f)
                        {
                            if (idx0 > idx1)
                            {
                                Helper.VSet(ref diff, -ag.dvel[2], 0, ag.dvel[0]);
                            }
                            else
                            {
                                Helper.VSet(ref diff, ag.dvel[2], 0, -ag.vel[0]);
                            }
                            pen = 0.01f;
                        }
                        else
                        {
                            pen = (1.0f / dist) * (pen * 0.5f) * CollisionResolveFactor;
                        }

                        Helper.VMad(ref ag.disp, ag.disp, diff, pen);

                        w += 1.0f;
                    }

                    if (w > 0.0001f)
                    {
                        float iw = 1.0f / w;
                        ag.disp = Helper.VScale(ag.disp[0], ag.disp[1], ag.disp[2], iw);
                    }
                }

                for (int i = 0; i < nagents; i++)
                {
                    CrowdAgent ag = agents[i];
                    if (ag.State != CrowdAgentState.Walking)
                    {
                        continue;
                    }

                    ag.npos = Helper.VAdd(ag.npos[0], ag.npos[1], ag.npos[2], ag.disp[0], ag.disp[1], ag.disp[2]);
                }
            }

            for (int i = 0; i < nagents; i++)
            {
                CrowdAgent ag = agents[i];
                if (ag.State != CrowdAgentState.Walking)
                {
                    continue;
                }

                ag.Corridor.MovePosition(ag.npos, _navQuery, _filter);
                Helper.VCopy(ref ag.npos, ag.Corridor.Pos);

                if (ag.TargetState == MoveRequestState.TargetNone || ag.TargetState == MoveRequestState.TargetVelocity)
                {
                    ag.Corridor.Reset(ag.Corridor.FirstPoly, ag.npos);
                }
            }

            // Update agents using off-mesh connection
            for (int i = 0; i < _maxAgents; i++)
            {
                CrowdAgentAnimation anim = _agentAnims[i];
                if (!anim.Active)
                {
                    continue;
                }
                CrowdAgent ag = agents[i];

                anim.T += dt;
                if (anim.T > anim.TMax)
                {
                    anim.Active = false;
                    ag.State    = CrowdAgentState.Walking;
                    continue;
                }

                float ta = anim.TMax * 0.15f;
                float tb = anim.TMax;
                if (anim.T < ta)
                {
                    float u = Tween(anim.T, 0.0f, ta);
                    Helper.VLerp(ref ag.npos, anim.InitPos[0], anim.InitPos[1], anim.InitPos[2], anim.StartPos[0], anim.StartPos[1], anim.StartPos[2], u);
                }
                else
                {
                    float u = Tween(anim.T, ta, tb);
                    Helper.VLerp(ref ag.npos, anim.StartPos[0], anim.StartPos[1], anim.StartPos[2], anim.EndPos[0], anim.EndPos[1], anim.EndPos[2], u);
                }

                Helper.VSet(ref ag.vel, 0, 0, 0);
                Helper.VSet(ref ag.dvel, 0, 0, 0);
            }
        }