Exemple #1
0
 public void SetObstacleAvoidanceParams(int idx, ObstacleAvoidanceParams param)
 {
     if (idx >= 0 && idx < CrowdMaxObstAvoidanceParams)
     {
         _obstacleQueryParams[idx] = param;
     }
 }
Exemple #2
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);
        }
        public int SampleVelocityGrid(float[] pos, float rad, float vmax, float[] vel, float[] dvel, ref float[] nvel,
                                      ObstacleAvoidanceParams param, ObstacleAvoidanceDebugData debug = null)
        {
            Prepare(pos, dvel);

            _params       = new ObstacleAvoidanceParams(param);
            _invHorizTime = 1.0f / _params.horizTime;
            _vmax         = vmax;
            _invVmax      = 1.0f / vmax;

            Helper.VSet(ref nvel, 0, 0, 0);

            if (debug != null)
            {
                debug.Reset();
            }

            float cvx  = dvel[0] * _params.velBias;
            float cvz  = dvel[2] * _params.velBias;
            float cs   = vmax * 2 * (1 - _params.velBias) / (float)(_params.gridSize - 1);
            float half = (_params.gridSize - 1) * cs * 0.5f;

            float minPenalty = float.MaxValue;
            int   ns         = 0;

            for (int y = 0; y < _params.gridSize; y++)
            {
                for (int x = 0; x < _params.gridSize; x++)
                {
                    float[] vcand = new float[3];
                    vcand[0] = cvx + x * cs - half;
                    vcand[1] = 0;
                    vcand[2] = cvz + y * cs - half;

                    if (vcand[0] * vcand[0] + vcand[2] * vcand[2] > (vmax + cs / 2) * (vmax + cs / 2))
                    {
                        continue;
                    }

                    float penalty = ProcessSample(vcand, cs, pos, rad, vel, dvel, debug);
                    ns++;
                    if (penalty < minPenalty)
                    {
                        minPenalty = penalty;
                        Helper.VCopy(ref nvel, vcand);
                    }
                }
            }
            return(ns);
        }
        public int SampleVelocityAdaptive(float[] pos, float rad, float vmax, float[] vel, float[] dvel,
                                          ref float[] nvel, ObstacleAvoidanceParams param,
                                          ObstacleAvoidanceDebugData debug = null)
        {
            Prepare(pos, dvel);

            _params       = new ObstacleAvoidanceParams(param);
            _invHorizTime = 1.0f / _params.horizTime;
            _vmax         = vmax;
            _invVmax      = 1.0f / vmax;

            Helper.VSet(ref nvel, 0, 0, 0);

            if (debug != null)
            {
                debug.Reset();
            }

            float[] pat  = new float[(MaxPatternDivs * MaxPatternRings + 1) * 2];
            int     npat = 0;

            int ndivs  = _params.adaptiveDivs;
            int nrings = _params.adaptiveRings;
            int depth  = _params.adaptiveDepth;

            int   nd   = Math.Max(1, Math.Min(MaxPatternDivs, ndivs));
            int   nr   = Math.Max(1, Math.Min(MaxPatternRings, nrings));
            float da   = (1.0f / nd) * (float)Math.PI * 2f;
            float dang = (float)Math.Atan2(dvel[2], dvel[0]);

            pat[npat * 2 + 0] = 0;
            pat[npat * 2 + 1] = 0;
            npat++;

            for (int j = 0; j < nr; j++)
            {
                float r = (nr - j) / (float)nr;
                float a = dang + (j & 1) * 0.5f * da;
                for (int i = 0; i < nd; i++)
                {
                    pat[npat * 2 + 0] = (float)Math.Cos(a) * r;
                    pat[npat * 2 + 1] = (float)Math.Sin(a) * r;
                    npat++;
                    a += da;
                }
            }

            float cr = vmax * (1.0f - _params.velBias);

            float[] res = new float[3];
            Helper.VSet(ref res, dvel[0] * _params.velBias, 0, dvel[2] * _params.velBias);
            int ns = 0;

            for (int k = 0; k < depth; k++)
            {
                float   minPenalty = float.MaxValue;
                float[] bvel       = new float[3];
                Helper.VSet(ref bvel, 0, 0, 0);

                for (int i = 0; i < npat; i++)
                {
                    float[] vcand = { res[0] + pat[i * 2 + 0] * cr, 0, res[2] + pat[i * 2 + 1] * cr };

                    if (vcand[0] * vcand[0] + vcand[2] * vcand[2] > (vmax + 0.001f) * (vmax + 0.001f))
                    {
                        continue;
                    }

                    float penalty = ProcessSample(vcand, cr / 10f, pos, rad, vel, dvel, debug);
                    ns++;
                    if (penalty < minPenalty)
                    {
                        minPenalty = penalty;
                        Helper.VCopy(ref bvel, vcand);
                    }
                }
                Helper.VCopy(ref res, bvel);

                cr *= 0.5f;
            }

            Helper.VCopy(ref nvel, res);

            return(ns);
        }
Exemple #5
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);
            }
        }