/// <summary>
/// Find the 3D distance between a point (x, y, z) and a segment PQ
/// </summary>
/// <param name="pt">The coordinate of the point.</param>
/// <param name="p">The coordinate of point P in the segment PQ.</param>
/// <param name="q">The coordinate of point Q in the segment PQ.</param>
/// <returns>The distance between the point and the segment.</returns>
internal static float PointToSegmentSquared(ref Vector3 pt, ref Vector3 p, ref Vector3 q)
{
//distance from P to Q
Vector3 pq = q - p;
//disance from P to the lone point
float dx = pt.X - p.X;
float dy = pt.Y - p.Y;
float dz = pt.Z - p.Z;
float segmentMagnitudeSquared = pq.LengthSquared();
float t = pq.X * dx + pq.Y * dy + pq.Z * dz;
if (segmentMagnitudeSquared > 0)
{
t /= segmentMagnitudeSquared;
}
//keep t between 0 and 1
if (t < 0)
{
t = 0;
}
else if (t > 1)
{
t = 1;
}
dx = p.X + t * pq.X - pt.X;
dy = p.Y + t * pq.Y - pt.Y;
dz = p.Z + t * pq.Z - pt.Z;
return(dx * dx + dy * dy + dz * dz);
}
public void CollideAgainst(ShapeCollection shapeCollection, bool isCloudCollision)
{
Vector3 lastPosition = this.Position;
Vector3 lastVelocity = this.Velocity;
float lastY = this.Y;
mIsOnGround = false;
mHitHead = false;
if (shapeCollection.CollideAgainstBounceWithoutSnag(this.Collision, 0))
{
bool wasJumpCancelled = false;
if (isCloudCollision)
{
// Cloud can't modify X and XVelocity
this.X = lastPosition.X;
this.Velocity.X = lastVelocity.X;
Vector3 change = this.Position - lastPosition;
if (change.LengthSquared() > MaximumCloudRepositionLength * MaximumCloudRepositionLength || change.Y <= 0 || lastVelocity.Y > 0)
{
// We only want to do cloud collision, but we didn't fall within the cloud criteria, so let's ignore the collision
this.Position = lastPosition;
this.Velocity = lastVelocity;
wasJumpCancelled = true;
}
// Since we may have repositioned this, we want to reposition the rect for future collisions and for
// rendering.
this.Collision.ForceUpdateDependencies();
}
if (!wasJumpCancelled)
{
if (this.Y > lastY)
{
mIsOnGround = true;
}
if (this.Y < lastY)
{
mHitHead = true;
}
}
}
}
/// <summary>
/// Update the crowd pathfinding periodically
/// </summary>
/// <param name="dt">Th time until the next update</param>
public void Update(float dt)
{
velocitySampleCount = 0;
int numAgents = GetActiveAgents(agents);
//check that all agents have valid paths
CheckPathValidity(agents, numAgents, dt);
//update async move requests and path finder
UpdateMoveRequest();
//optimize path topology
UpdateTopologyOptimization(agents, numAgents, dt);
//register agents to proximity grid
grid.Clear();
for (int i = 0; i < numAgents; i++)
{
Agent a = agents[i];
Vector3 p = a.Position;
float r = a.Parameters.Radius;
grid.AddItem(a, p.X - r, p.Z - r, p.X + r, p.Z + r);
}
//get nearby navmesh segments and agents to collide with
for (int i = 0; i < numAgents; i++)
{
if (agents[i].State != AgentState.Walking)
continue;
//update the collision boundary after certain distance has passed or if it has become invalid
float updateThr = agents[i].Parameters.CollisionQueryRange * 0.25f;
if (Vector3Extensions.Distance2D(agents[i].Position, agents[i].Boundary.Center) > updateThr * updateThr || !agents[i].Boundary.IsValid(navQuery))
{
agents[i].Boundary.Update(agents[i].Corridor.GetFirstPoly(), agents[i].Position, agents[i].Parameters.CollisionQueryRange, navQuery);
}
//query neighbor agents
agents[i].NeighborCount = GetNeighbors(agents[i].Position, agents[i].Parameters.Height, agents[i].Parameters.CollisionQueryRange, agents[i], agents[i].Neighbors, AgentMaxNeighbors, agents, grid);
for (int j = 0; j < agents[i].NeighborCount; j++)
agents[i].Neighbors[j].Index = GetAgentIndex(agents[agents[i].Neighbors[j].Index]);
}
//find the next corner to steer to
for (int i = 0; i < numAgents; i++)
{
if (agents[i].State != AgentState.Walking)
continue;
if (agents[i].TargetState == TargetState.None ||
agents[i].TargetState == TargetState.Velocity)
continue;
//find corners for steering
agents[i].Corridor.FindCorners(agents[i].Corners, navQuery);
//check to see if the corner after the next corner is directly visible
if (((agents[i].Parameters.UpdateFlags & UpdateFlags.OptimizeVis) != 0) && agents[i].Corners.Count > 0)
{
Vector3 target = agents[i].Corners[Math.Min(1, agents[i].Corners.Count - 1)].Point.Position;
agents[i].Corridor.OptimizePathVisibility(target, agents[i].Parameters.PathOptimizationRange, navQuery);
}
}
//trigger off-mesh connections (depends on corners)
for (int i = 0; i < numAgents; i++)
{
if (agents[i].State != AgentState.Walking)
continue;
if (agents[i].TargetState == TargetState.None ||
agents[i].TargetState == TargetState.Velocity)
continue;
//check
float triggerRadius = agents[i].Parameters.Radius * 2.25f;
if (OverOffmeshConnection(agents[i], triggerRadius))
{
//prepare to off-mesh connection
int idx = i;
//adjust the path over the off-mesh connection
NavPolyId[] refs = new NavPolyId[2];
if (agents[i].Corridor.MoveOverOffmeshConnection(agents[i].Corners[agents[i].Corners.Count - 1].Point.Polygon, refs, ref agentAnims[idx].StartPos, ref agentAnims[idx].EndPos, navQuery))
{
agentAnims[idx].InitPos = agents[i].Position;
agentAnims[idx].PolyRef = refs[1];
agentAnims[idx].Active = true;
agentAnims[idx].T = 0.0f;
agentAnims[idx].TMax = (Vector3Extensions.Distance2D(agentAnims[idx].StartPos, agentAnims[idx].EndPos)
/ agents[i].Parameters.MaxSpeed) * 0.5f;
agents[i].State = AgentState.Offmesh;
agents[i].Corners.Clear();
agents[i].NeighborCount = 0;
continue;
}
}
}
//calculate steering
for (int i = 0; i < numAgents; i++)
{
if (agents[i].State != AgentState.Walking)
continue;
if (agents[i].TargetState == TargetState.None)
continue;
Vector3 dvel = new Vector3(0, 0, 0);
if (agents[i].TargetState == TargetState.Velocity)
{
dvel = agents[i].TargetPosition;
agents[i].DesiredSpeed = agents[i].TargetPosition.Length();
}
else
{
//calculate steering direction
if ((agents[i].Parameters.UpdateFlags & UpdateFlags.AnticipateTurns) != 0)
CalcSmoothSteerDirection(agents[i], ref dvel);
else
CalcStraightSteerDirection(agents[i], ref dvel);
//calculate speed scale, which tells the agent to slowdown at the end of the path
float slowDownRadius = agents[i].Parameters.Radius * 2;
float speedScale = GetDistanceToGoal(agents[i], slowDownRadius) / slowDownRadius;
agents[i].DesiredSpeed = agents[i].Parameters.MaxSpeed;
dvel = dvel * (agents[i].DesiredSpeed * speedScale);
}
//separation
if ((agents[i].Parameters.UpdateFlags & UpdateFlags.Separation) != 0)
{
float separationDist = agents[i].Parameters.CollisionQueryRange;
float invSeparationDist = 1.0f / separationDist;
float separationWeight = agents[i].Parameters.SeparationWeight;
float w = 0;
Vector3 disp = new Vector3(0, 0, 0);
for (int j = 0; j < agents[i].NeighborCount; j++)
{
Agent nei = agents[agents[i].Neighbors[j].Index];
Vector3 diff = agents[i].Position - nei.Position;
diff.Y = 0;
float distSqr = diff.LengthSquared();
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));
disp = disp + diff * (weight / dist);
w += 1.0f;
}
if (w > 0.0001f)
{
//adjust desired veloctiy
dvel = dvel + disp * (1.0f / w);
//clamp desired velocity to desired speed
float speedSqr = dvel.LengthSquared();
float desiredSqr = agents[i].DesiredSpeed * agents[i].DesiredSpeed;
if (speedSqr > desiredSqr)
dvel = dvel * (desiredSqr / speedSqr);
}
}
//set the desired velocity
agents[i].DesiredVel = dvel;
}
//velocity planning
for (int i = 0; i < numAgents; i++)
{
if (agents[i].State != AgentState.Walking)
continue;
if ((agents[i].Parameters.UpdateFlags & UpdateFlags.ObstacleAvoidance) != 0)
{
this.obstacleQuery.Reset();
//add neighhbors as obstacles
for (int j = 0; j < agents[i].NeighborCount; j++)
{
Agent nei = agents[agents[i].Neighbors[j].Index];
obstacleQuery.AddCircle(nei.Position, nei.Parameters.Radius, nei.Vel, nei.DesiredVel);
}
//append neighbor segments as obstacles
for (int j = 0; j < agents[i].Boundary.SegCount; j++)
{
LocalBoundary.Segment s = agents[i].Boundary.Segs[j];
if (Triangle3.Area2D(agents[i].Position, s.Start, s.End) < 0.0f)
continue;
obstacleQuery.AddSegment(s.Start, s.End);
}
//sample new safe velocity
bool adaptive = true;
int ns = 0;
ObstacleAvoidanceQuery.ObstacleAvoidanceParams parameters = obstacleQueryParams[agents[i].Parameters.ObstacleAvoidanceType];
if (adaptive)
{
ns = obstacleQuery.SampleVelocityAdaptive(agents[i].Position, agents[i].Parameters.Radius, agents[i].DesiredSpeed, agents[i].Vel, agents[i].DesiredVel, ref agents[i].NVel, parameters);
}
else
{
ns = obstacleQuery.SampleVelocityGrid(agents[i].Position, agents[i].Parameters.Radius, agents[i].DesiredSpeed, agents[i].Vel, agents[i].DesiredVel, ref agents[i].NVel, parameters);
}
this.velocitySampleCount += ns;
}
else
{
//if not using velocity planning, new velocity is directly the desired velocity
agents[i].NVel = agents[i].DesiredVel;
}
}
//integrate
for (int i = 0; i < numAgents; i++)
{
Agent ag = agents[i];
if (ag.State != AgentState.Walking)
continue;
ag.Integrate(dt);
}
//handle collisions
const float COLLISION_RESOLVE_FACTOR = 0.7f;
for (int iter = 0; iter < 4; iter++)
{
for (int i = 0; i < numAgents; i++)
{
int idx0 = GetAgentIndex(agents[i]);
if (agents[i].State != AgentState.Walking)
continue;
agents[i].Disp = new Vector3(0, 0, 0);
float w = 0;
for (int j = 0; j < agents[i].NeighborCount; j++)
{
Agent nei = agents[agents[i].Neighbors[j].Index];
int idx1 = GetAgentIndex(nei);
Vector3 diff = agents[i].Position - nei.Position;
diff.Y = 0;
float dist = diff.LengthSquared();
if (dist > (agents[i].Parameters.Radius + nei.Parameters.Radius) * (agents[i].Parameters.Radius + nei.Parameters.Radius))
continue;
dist = (float)Math.Sqrt(dist);
float pen = (agents[i].Parameters.Radius + nei.Parameters.Radius) - dist;
if (dist < 0.0001f)
{
//agents on top of each other, try to choose diverging separation directions
if (idx0 > idx1)
diff = new Vector3(-agents[i].DesiredVel.Z, 0, agents[i].DesiredVel.X);
else
diff = new Vector3(agents[i].DesiredVel.Z, 0, -agents[i].DesiredVel.X);
pen = 0.01f;
}
else
{
pen = (1.0f / dist) * (pen * 0.5f) * COLLISION_RESOLVE_FACTOR;
}
agents[i].Disp = agents[i].Disp + diff * pen;
w += 1.0f;
}
if (w > 0.0001f)
{
float iw = 1.0f / w;
agents[i].Disp = agents[i].Disp * iw;
}
}
for (int i = 0; i < numAgents; i++)
{
if (agents[i].State != AgentState.Walking)
continue;
//move along navmesh
agents[i].Corridor.MovePosition(agents[i].Position, navQuery);
//get valid constrained position back
agents[i].Position = agents[i].Corridor.Pos;
//if not using path, truncate the corridor to just one poly
if (agents[i].TargetState == TargetState.None ||
agents[i].TargetState == TargetState.Velocity)
{
agents[i].Corridor.Reset(agents[i].Corridor.GetFirstPoly(), agents[i].Position);
agents[i].IsPartial = false;
}
}
//update agents using offmesh connections
for (int i = 0; i < maxAgents; i++)
{
if (!agentAnims[i].Active)
continue;
agentAnims[i].T += dt;
if (agentAnims[i].T > agentAnims[i].TMax)
{
//reset animation
agentAnims[i].Active = false;
//prepare agent for walking
agents[i].State = AgentState.Walking;
continue;
}
//update position
float ta = agentAnims[i].TMax * 0.15f;
float tb = agentAnims[i].TMax;
if (agentAnims[i].T < ta)
{
float u = MathHelper.Normalize(agentAnims[i].T, 0.0f, ta);
Vector3 lerpOut;
Vector3.Lerp(ref agentAnims[i].InitPos, ref agentAnims[i].StartPos, u, out lerpOut);
agents[i].Position = lerpOut;
}
else
{
float u = MathHelper.Normalize(agentAnims[i].T, ta, tb);
Vector3 lerpOut;
Vector3.Lerp(ref agentAnims[i].StartPos, ref agentAnims[i].EndPos, u, out lerpOut);
agents[i].Position = lerpOut;
}
agents[i].Vel = new Vector3(0, 0, 0);
agents[i].DesiredVel = new Vector3(0, 0, 0);
}
}
}
public float LengthSquared()
{
return(_vector.LengthSquared());
}