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 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]); } } // 查找用于转向的下一个Corner 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); } } } // 触发离地连接 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; } } } // 计算转向 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); } // 速度计划 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); } } // 整合 for (int i = 0; i < nagents; i++) { CrowdAgent ag = agents[i]; if (ag.State != CrowdAgentState.Walking) { continue; } ag.Integrate(dt); } // 操纵碰撞 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); } }