protected override void Prepare(ref Unemployed job, float delta)
{
int raycastCount = m_lockedRaycasts.Count;
if (m_outputRaycast.Length != raycastCount)
{
m_outputRaycast.Dispose();
m_outputRaycast = new NativeArray <RaycastData>(raycastCount, Allocator.Persistent);
}
Raycast r;
float3 pos, dir;
if (plane == AxisPair.XY)
{
for (int i = 0; i < raycastCount; i++)
{
r = m_lockedRaycasts[i];
pos = r.pos;
dir = r.m_dir;
m_outputRaycast[i] = new RaycastData()
{
position = float2(pos.x, pos.y), //
distance = r.m_distance,
worldPosition = pos,
baseline = pos.z,
direction = float2(dir.x, dir.y),
worldDir = dir,
layerIgnore = r.m_layerIgnore,
filter = r.m_filter,
twoSided = r.twoSided
};
}
}
else
{
for (int i = 0; i < raycastCount; i++)
{
r = m_lockedRaycasts[i];
pos = r.pos;
dir = r.m_dir;
m_outputRaycast[i] = new RaycastData()
{
position = float2(pos.x, pos.z), //
distance = r.m_distance,
worldPosition = pos,
baseline = pos.y,
direction = float2(dir.x, dir.z),
worldDir = dir,
layerIgnore = r.m_layerIgnore,
filter = r.m_filter,
twoSided = r.twoSided
};
}
}
}
/// <summary>
/// Recursive method for computing the agent neighbors of the specified raycast.
/// </summary>
/// <param name="raycast">The agent for which agent neighbors are to be computed.</param>
/// <param name="raycast">The agent making the initial query</param>
/// <param name="rangeSq">The squared range around the agent.</param>
/// <param name="node">The current agent k-D tree node index.</param>
/// <param name="agentNeighbors">The list of neighbors to be filled up.</param>
private void QueryAgentTreeRecursive(ref RaycastData raycast, ref float rangeSq, int node, ref NativeList <int> agentNeighbors)
{
float2 center = raycast.position;
AgentTreeNode treeNode = m_inputAgentTree[node];
if (treeNode.end - treeNode.begin <= AgentTreeNode.MAX_LEAF_SIZE)
{
AgentData a;
float bottom = raycast.baseline, top = bottom;
for (int i = treeNode.begin; i < treeNode.end; ++i)
{
a = m_inputAgents[i];
if (!a.collisionEnabled ||
(a.layerOccupation & ~raycast.layerIgnore) == 0 ||
(top < a.baseline || bottom > a.baseline + a.height))
{
continue;
}
//if ((distancesq(center, a.position) - lengthsq(a.radius)) < rangeSq)
agentNeighbors.Add(i);
}
}
else
{
AgentTreeNode leftNode = m_inputAgentTree[treeNode.left], rightNode = m_inputAgentTree[treeNode.right];
float distSqLeft = lengthsq(max(0.0f, leftNode.minX - center.x))
+ lengthsq(max(0.0f, center.x - leftNode.maxX))
+ lengthsq(max(0.0f, leftNode.minY - center.y))
+ lengthsq(max(0.0f, center.y - leftNode.maxY));
float distSqRight = lengthsq(max(0.0f, rightNode.minX - center.x))
+ lengthsq(max(0.0f, center.x - rightNode.maxX))
+ lengthsq(max(0.0f, rightNode.minY - center.y))
+ lengthsq(max(0.0f, center.y - rightNode.maxY));
if (distSqLeft < distSqRight)
{
if (distSqLeft < rangeSq)
{
QueryAgentTreeRecursive(ref raycast, ref rangeSq, treeNode.left, ref agentNeighbors);
if (distSqRight < rangeSq)
{
QueryAgentTreeRecursive(ref raycast, ref rangeSq, treeNode.right, ref agentNeighbors);
}
}
}
else
{
if (distSqRight < rangeSq)
{
QueryAgentTreeRecursive(ref raycast, ref rangeSq, treeNode.right, ref agentNeighbors);
if (distSqLeft < rangeSq)
{
QueryAgentTreeRecursive(ref raycast, ref rangeSq, treeNode.left, ref agentNeighbors);
}
}
}
}
}
private void QueryObstacleTreeRecursive(
ref RaycastData raycast,
ref float rangeSq,
int node,
ref NativeList <int> obstacleNeighbors,
ref NativeArray <ObstacleVertexData> obstacles,
ref NativeArray <ObstacleVertexData> refObstacles,
ref NativeArray <ObstacleInfos> obstaclesInfos,
ref NativeArray <ObstacleTreeNode> kdTree)
{
float2 center = raycast.position;
ObstacleTreeNode treeNode = kdTree[node];
if (treeNode.end - treeNode.begin <= ObstacleTreeNode.MAX_LEAF_SIZE)
{
ObstacleVertexData o;
ObstacleInfos infos;
float top = raycast.baseline, bottom = raycast.baseline;
float2 oPos, nPos;
for (int i = treeNode.begin; i < treeNode.end; ++i)
{
o = obstacles[i];
infos = obstaclesInfos[o.infos];
if (!infos.collisionEnabled || (infos.layerOccupation & ~raycast.layerIgnore) == 0)
{
continue;
}
if (top < infos.baseline || bottom > infos.baseline + infos.height)
{
continue;
}
oPos = o.pos; nPos = refObstacles[o.next].pos;
float distSq = DistSqPointLineSegment(oPos, nPos, center);
if (distSq < rangeSq)
{
float raycastLeftOfLine = LeftOf(oPos, nPos, center);
if ((lengthsq(raycastLeftOfLine) / lengthsq(nPos - oPos)) < rangeSq && raycastLeftOfLine < 0.0f)
{
obstacleNeighbors.Add(i);
}
}
}
}
else
{
ObstacleTreeNode leftNode = kdTree[treeNode.left],
rightNode = kdTree[treeNode.right];
float distSqLeft = lengthsq(max(0.0f, leftNode.minX - center.x))
+ lengthsq(max(0.0f, center.x - leftNode.maxX))
+ lengthsq(max(0.0f, leftNode.minY - center.y))
+ lengthsq(max(0.0f, center.y - leftNode.maxY));
float distSqRight = lengthsq(max(0.0f, rightNode.minX - center.x))
+ lengthsq(max(0.0f, center.x - rightNode.maxX))
+ lengthsq(max(0.0f, rightNode.minY - center.y))
+ lengthsq(max(0.0f, center.y - rightNode.maxY));
if (distSqLeft < distSqRight)
{
if (distSqLeft < rangeSq)
{
QueryObstacleTreeRecursive(ref raycast, ref rangeSq, treeNode.left, ref obstacleNeighbors,
ref obstacles, ref refObstacles, ref obstaclesInfos, ref kdTree);
if (distSqRight < rangeSq)
{
QueryObstacleTreeRecursive(ref raycast, ref rangeSq, treeNode.right, ref obstacleNeighbors,
ref obstacles, ref refObstacles, ref obstaclesInfos, ref kdTree);
}
}
}
else
{
if (distSqRight < rangeSq)
{
QueryObstacleTreeRecursive(ref raycast, ref rangeSq, treeNode.right, ref obstacleNeighbors,
ref obstacles, ref refObstacles, ref obstaclesInfos, ref kdTree);
if (distSqLeft < rangeSq)
{
QueryObstacleTreeRecursive(ref raycast, ref rangeSq, treeNode.left, ref obstacleNeighbors,
ref obstacles, ref refObstacles, ref obstaclesInfos, ref kdTree);
}
}
}
}
}
public void Execute(int index)
{
RaycastData raycast = m_inputRaycasts[index];
RaycastResult result = new RaycastResult()
{
hitAgent = -1,
hitObstacle = -1,
dynamicObstacle = false
};
RaycastFilter filter = raycast.filter;
if (filter == 0)
{
m_results[index] = result;
return;
}
float2
r_position = raycast.position,
r_dir = raycast.direction,
hitLocation;
float
a_bottom = raycast.baseline,
a_top = a_bottom,
a_sqRange = lengthsq(raycast.distance);
Segment2D raySegment = new Segment2D(raycast.position, raycast.position + raycast.direction * raycast.distance),
segment;
UIntPair pair = new UIntPair(0, 0);
ObstacleVertexData otherVertex;
bool
twoSidedCast = raycast.twoSided,
alreadyCovered = false,
hit;
#region static obstacles
if ((filter & RaycastFilter.OBSTACLE_STATIC) != 0)
{
NativeList <int> staticObstacleNeighbors = new NativeList <int>(10, Allocator.Temp);
if (m_staticObstacleTree.Length > 0)
{
QueryObstacleTreeRecursive(ref raycast, ref a_sqRange, 0, ref staticObstacleNeighbors,
ref m_staticObstacles, ref m_staticRefObstacles, ref m_staticObstacleInfos, ref m_staticObstacleTree);
}
NativeHashMap <UIntPair, bool> coveredStaticEdges = new NativeHashMap <UIntPair, bool>(m_staticObstacleTree.Length * 2, Allocator.Temp);
for (int i = 0; i < staticObstacleNeighbors.Length; ++i)
{
ObstacleVertexData vertex = m_staticObstacles[staticObstacleNeighbors[i]];
ObstacleInfos infos = m_staticObstacleInfos[vertex.infos];
pair = new UIntPair(vertex.index, vertex.next);
alreadyCovered = coveredStaticEdges.TryGetValue(pair, out hit);
if (!alreadyCovered)
{
otherVertex = m_staticRefObstacles[vertex.next];
segment = new Segment2D(vertex.pos, otherVertex.pos);
alreadyCovered = (!twoSidedCast && dot(r_dir, segment.normal) < 0f);
if (!alreadyCovered)
{
if (raySegment.IsIntersecting(segment, out hitLocation))
{
//Rays intersecting !
if (result.hitObstacle == -1 ||
distancesq(r_position, hitLocation) < distancesq(r_position, result.hitObstacleLocation2D))
{
result.hitObstacleLocation2D = hitLocation;
result.hitObstacle = infos.index;
}
}
}
coveredStaticEdges.TryAdd(pair, true);
}
pair = new UIntPair(vertex.index, vertex.prev);
alreadyCovered = coveredStaticEdges.ContainsKey(pair);
if (!alreadyCovered)
{
otherVertex = m_staticRefObstacles[vertex.prev];
segment = new Segment2D(vertex.pos, otherVertex.pos);
alreadyCovered = (!twoSidedCast && dot(r_dir, segment.normal) < 0f);
if (!alreadyCovered)
{
if (raySegment.IsIntersecting(segment, out hitLocation))
{
//Rays intersecting !
if (result.hitObstacle == -1 ||
distancesq(r_position, hitLocation) < distancesq(r_position, result.hitObstacleLocation2D))
{
result.hitObstacleLocation2D = hitLocation;
result.hitObstacle = infos.index;
}
}
}
coveredStaticEdges.TryAdd(pair, true);
}
}
staticObstacleNeighbors.Dispose();
coveredStaticEdges.Dispose();
}
#endregion
#region dynamic obstacles
if ((filter & RaycastFilter.OBSTACLE_DYNAMIC) != 0)
{
NativeList <int> dynObstacleNeighbors = new NativeList <int>(10, Allocator.Temp);
if (m_dynObstacleTree.Length > 0)
{
QueryObstacleTreeRecursive(ref raycast, ref a_sqRange, 0, ref dynObstacleNeighbors,
ref m_dynObstacles, ref m_dynRefObstacles, ref m_dynObstacleInfos, ref m_dynObstacleTree);
}
NativeHashMap <UIntPair, bool> coveredDynEdges = new NativeHashMap <UIntPair, bool>(m_dynObstacleTree.Length * 2, Allocator.Temp);
for (int i = 0; i < dynObstacleNeighbors.Length; ++i)
{
ObstacleVertexData vertex = m_dynObstacles[dynObstacleNeighbors[i]];
ObstacleInfos infos = m_dynObstacleInfos[vertex.infos];
pair = new UIntPair(vertex.index, vertex.next);
alreadyCovered = coveredDynEdges.TryGetValue(pair, out hit);
if (!alreadyCovered)
{
otherVertex = m_dynRefObstacles[vertex.next];
segment = new Segment2D(vertex.pos, otherVertex.pos);
alreadyCovered = (!twoSidedCast && dot(r_dir, segment.normal) < 0f);
if (!alreadyCovered)
{
if (raySegment.IsIntersecting(segment, out hitLocation))
{
//Rays intersecting !
if (result.hitObstacle == -1 ||
distancesq(r_position, hitLocation) < distancesq(r_position, result.hitObstacleLocation2D))
{
result.hitObstacleLocation2D = hitLocation;
result.hitObstacle = infos.index;
result.dynamicObstacle = true;
}
}
}
coveredDynEdges.TryAdd(pair, true);
}
pair = new UIntPair(vertex.index, vertex.prev);
alreadyCovered = coveredDynEdges.ContainsKey(pair);
if (!alreadyCovered)
{
otherVertex = m_dynRefObstacles[vertex.prev];
segment = new Segment2D(vertex.pos, otherVertex.pos);
alreadyCovered = (!twoSidedCast && dot(r_dir, segment.normal) < 0f);
if (!alreadyCovered)
{
if (raySegment.IsIntersecting(segment, out hitLocation))
{
//Rays intersecting !
if (result.hitObstacle == -1 ||
distancesq(r_position, hitLocation) < distancesq(r_position, result.hitObstacleLocation2D))
{
result.hitObstacleLocation2D = hitLocation;
result.hitObstacle = infos.index;
result.dynamicObstacle = true;
}
}
}
coveredDynEdges.TryAdd(pair, true);
}
}
dynObstacleNeighbors.Dispose();
coveredDynEdges.Dispose();
}
#endregion
#region Agents
if ((filter & RaycastFilter.AGENTS) != 0)
{
NativeList <int> agentNeighbors = new NativeList <int>(10, Allocator.Temp);
float radSq = a_sqRange + lengthsq(m_maxAgentRadius);
if (m_inputAgents.Length > 0)
{
QueryAgentTreeRecursive(ref raycast, ref radSq, 0, ref agentNeighbors);
}
AgentData agent;
int iResult, agentIndex;
float2 center, i1, i2, rayEnd = r_position + normalize(r_dir) * raycast.distance;
float
a_radius, a_sqRadius, cx, cy,
Ax = r_position.x,
Ay = r_position.y,
Bx = rayEnd.x,
By = rayEnd.y,
dx = Bx - Ax,
dy = By - Ay,
magA = dx * dx + dy * dy,
distSq;
int TryGetIntersection(out float2 intersection1, out float2 intersection2)
{
float
magB = 2f * (dx * (Ax - cx) + dy * (Ay - cy)),
magC = (Ax - cx) * (Ax - cx) + (Ay - cy) * (Ay - cy) - a_sqRadius,
det = magB * magB - 4f * magA * magC,
sqDet, t;
if ((magA <= float.Epsilon) || (det < 0f))
{
// No real solutions.
intersection1 = float2(float.NaN, float.NaN);
intersection2 = float2(float.NaN, float.NaN);
return(0);
}
if (det == 0)
{
// One solution.
t = -magB / (2f * magA);
intersection1 = float2(Ax + t * dx, Ay + t * dy);
intersection2 = float2(float.NaN, float.NaN);
return(1);
}
else
{
// Two solutions.
sqDet = sqrt(det);
t = ((-magB + sqDet) / (2f * magA));
intersection1 = float2(Ax + t * dx, Ay + t * dy);
t = ((-magB - sqDet) / (2f * magA));
intersection2 = float2(Ax + t * dx, Ay + t * dy);
return(2);
}
}
for (int i = 0; i < agentNeighbors.Length; ++i)
{
agentIndex = agentNeighbors[i];
agent = m_inputAgents[agentIndex];
center = agent.position;
cx = center.x;
cy = center.y;
a_radius = agent.radius;
a_sqRadius = a_radius * a_radius;
if (dot(center - r_position, r_dir) < 0f)
{
continue;
}
iResult = TryGetIntersection(out i1, out i2);
if (iResult == 0)
{
continue;
}
distSq = distancesq(r_position, i1);
if (distSq < a_sqRange &&
(result.hitAgent == -1 || distSq < distancesq(r_position, result.hitAgentLocation2D)) &&
IsBetween(r_position, center, i1))
{
result.hitAgentLocation2D = i1;
result.hitAgent = agentIndex;
}
if (iResult == 2)
{
distSq = distancesq(r_position, i2);
if (distSq < a_sqRange &&
(result.hitAgent == -1 || distSq < distancesq(r_position, result.hitAgentLocation2D)) &&
IsBetween(r_position, center, i2))
{
result.hitAgentLocation2D = i2;
result.hitAgent = agentIndex;
}
}
}
agentNeighbors.Dispose();
}
#endregion
if (m_plane == AxisPair.XY)
{
float baseline = raycast.worldPosition.z;
if (result.hitAgent != -1)
{
result.hitAgentLocation = float3(result.hitAgentLocation2D, baseline);
}
if (result.hitObstacle != -1)
{
result.hitObstacleLocation = float3(result.hitObstacleLocation2D, baseline);
}
}
else
{
float baseline = raycast.worldPosition.y;
if (result.hitAgent != -1)
{
result.hitAgentLocation = float3(result.hitAgentLocation2D.x, baseline, result.hitAgentLocation2D.y);
}
if (result.hitObstacle != -1)
{
result.hitObstacleLocation = float3(result.hitObstacleLocation2D.x, baseline, result.hitObstacleLocation2D.y);
}
}
m_results[index] = result;
}
