public void SetObstacleAvoidanceParams(int idx, ObstacleAvoidanceParams param)
{
if (idx >= 0 && idx < CrowdMaxObstAvoidanceParams)
{
_obstacleQueryParams[idx] = param;
}
}
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);
}
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);
}
}
