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); }
private float ProcessSample(float[] vcand, float cs, float[] pos, float rad, float[] vel, float[] dvel, ObstacleAvoidanceDebugData debug = null) { float tmin = _params.horizTime; float side = 0; int nside = 0; for (int i = 0; i < _nCircles; i++) { ObstacleCircle cir = _circles[i]; float[] vab = Helper.VScale(vcand[0], vcand[1], vcand[2], 2); vab = Helper.VSub(vab[0], vab[1], vab[2], vel[0], vel[1], vel[2]); side += (float)Math.Max(0.0, Math.Min(1.0, Math.Min(Helper.VDot2D(cir.dp, vab) * 0.5f + 0.5f, Helper.VDot2D(cir.np, vab) * 2f))); nside++; float htmin = 0, htmax = 0; if (SweetCircleCircle(pos, rad, vab, cir.p, cir.rad, ref htmin, ref htmax) == 0) { continue; } if (htmin < 0.0f && htmax > 0.0f) { htmin = -htmin * 0.5f; } if (htmin >= 0.0f) { if (htmin < tmin) { tmin = htmin; } } } for (int i = 0; i < _nSegments; i++) { ObstacleSegment seg = _segments[i]; float htmin = 0; if (seg.touch) { float[] sdir = Helper.VSub(seg.q[0], seg.q[1], seg.q[2], seg.p[0], seg.p[1], seg.p[2]); float[] snorm = new float[3]; snorm[0] = -sdir[2]; snorm[2] = sdir[0]; if (Helper.VDot2D(snorm, vcand) < 0.0f) { continue; } htmin = 0.0f; } else { if (isectRaySeg(pos, vcand, seg.p, seg.q, ref htmin) == 0) { continue; } } htmin *= 2.0f; if (htmin < tmin) { tmin = htmin; } } if (nside > 0) { side /= nside; } float vpen = _params.weightDesVel * (Helper.VDist2D(vcand, dvel) * _invVmax); float vcpen = _params.weightCurVel * (Helper.VDist2D(vcand, vel) * _invVmax); float spen = _params.weightSide * side; float tpen = _params.weightToi * (1.0f / (0.1f + tmin * _invHorizTime)); float penalty = vpen + vcpen + spen + tpen; if (debug != null) { debug.AddSample(vcand, cs, penalty, vpen, vcpen, spen, tpen); } return(penalty); }
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); } }