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; }