public void Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex) { NativeArray <Translation> translations = chunk.GetNativeArray(translationType); NativeArray <int> quadrantKeys = new NativeArray <int>(MAX_QUADRANT_NEIGHBORS, Allocator.Temp); //TODO make something that is dynamic float radius = 0.5f; float combinedRadius = radius * 2.0f; for (int entityIdx = 0; entityIdx < chunk.ChunkEntityCount; entityIdx++) { float2 currentPosition = translations[entityIdx].Value.xz; int countNeighborQuadrant = 0; QuadrantSystem.GetCurrentCellAndNeighborsKeys(currentPosition, ref quadrantKeys, ref countNeighborQuadrant); //Get nearest neighbors for (int i = 0; i < countNeighborQuadrant; i++) { if (!quadrantMap.TryGetFirstValue(quadrantKeys[i], out var neighbor, out var nativeMultiHashMapIterator)) { continue; } do { float distance = math.distance(neighbor.position, currentPosition); if (distance < combinedRadius && distance > 0.0001f) { var entity = ecb.CreateEntity(chunkIndex); Collision collision = new Collision() { // percentage = ((combinedRadius - distance) * 0.5f) / radius, percentage = distance, position = (neighbor.position + currentPosition) / 2.0f, frame = key }; ecb.AddComponent(chunkIndex, entity, collision); } } while (quadrantMap.TryGetNextValue(out neighbor, ref nativeMultiHashMapIterator)); } } }
public void Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex) { NativeArray <Translation> translations = chunk.GetNativeArray(translationType); NativeArray <Velocity> velocities = chunk.GetNativeArray(velocityType); NativeArray <Line> orcaLines = new NativeArray <Line>(maxNeighbors, Allocator.Temp); NativeArray <KeyValuePair <float, AgentNeighbor> > agentNeighbors = new NativeArray <KeyValuePair <float, AgentNeighbor> >(maxNeighbors, Allocator.Temp); NativeArray <int> quadrantKeys = new NativeArray <int>(MAX_QUADRANT_NEIGHBORS, Allocator.Temp); float invTimeStep = 1.0f / dt; float combinedRadius = radius * 2.0f; float combinedRadiusSqr = math.pow(combinedRadius, 2); float rangeSqr = neighborsDist * neighborsDist; for (int entityIdx = 0; entityIdx < chunk.ChunkEntityCount; entityIdx++) { float2 velocity = velocities[entityIdx].Value; //Early exit if the agent is not moving if (math.lengthsq(velocity) < 0.001f) { continue; } float2 position = translations[entityIdx].Value.xz; int countNeighborQuadrant = 0; QuadrantSystem.GetCurrentCellAndNeighborsKeys(position, ref quadrantKeys, ref countNeighborQuadrant); //ORCA setup int neighborsCount = 0; int nbObstacleLine = 0; //Get nearest neighbors for (int i = 0; i < countNeighborQuadrant; i++) { if (!quadrantMap.TryGetFirstValue(quadrantKeys[i], out var neighbor, out var nativeMultiHashMapIterator)) { continue; } do { float2 dir = position - neighbor.position; float distSqr = math.dot(dir, dir); //Condition to avoid self if (distSqr > 0.001f) { //If the other agent is under the minimum range => add it if (!(distSqr < rangeSqr)) { continue; } //If there is a free space, add it immediately if (neighborsCount < maxNeighbors) { agentNeighbors[neighborsCount] = new KeyValuePair <float, AgentNeighbor>(distSqr, new AgentNeighbor() { position = neighbor.position, velocity = neighbor.velocity }); neighborsCount++; } //Make sure the list is sorted int j = neighborsCount - 1; while (j != 0 && distSqr < agentNeighbors[j - 1].Key) { agentNeighbors[j] = agentNeighbors[j - 1]; j--; } //Once a spot with a further agent is found, place if agentNeighbors[j] = new KeyValuePair <float, AgentNeighbor>(distSqr, new AgentNeighbor() { position = neighbor.position, velocity = neighbor.velocity }); //If the list is full, only check agent nearer than the farrest neighbor. if (neighborsCount == maxNeighbors) { rangeSqr = agentNeighbors[maxNeighbors - 1].Key; } } } while (quadrantMap.TryGetNextValue(out neighbor, ref nativeMultiHashMapIterator)); } //Evaluate each neighbors for (int neighborIdx = 0; neighborIdx < neighborsCount; neighborIdx++) { AgentNeighbor otherAgent = agentNeighbors[neighborIdx].Value; float2 relativePosition = otherAgent.position - position; float2 relativeVelocity = velocity - otherAgent.velocity; float distSqr = math.lengthsq(relativePosition); Line line; float2 u; if (distSqr > combinedRadiusSqr) { // No Collision float2 w = relativeVelocity - invTimeHorizon * relativePosition; // Vector from center to relative velocity float wLengthSqr = math.lengthsq(w); float dotProduct1 = math.dot(w, relativePosition); if (dotProduct1 < 0.0f && math.pow(dotProduct1, 2) > combinedRadiusSqr * wLengthSqr) { // Project on circle float wLength = math.sqrt(wLengthSqr); float2 unitW = w / wLength; line.direction = new float2(unitW.y, -unitW.x); u = (combinedRadius * invTimeHorizon - wLength) * unitW; } else { // Projection on legs float leg = math.sqrt(distSqr - combinedRadiusSqr); if (Det(relativePosition, w) > 0.0f) { line.direction = new float2( relativePosition.x * leg - relativePosition.y * combinedRadius, relativePosition.x * combinedRadius + relativePosition.y * leg) / distSqr; } else { line.direction = -new float2( relativePosition.x * leg - relativePosition.y * combinedRadius, -relativePosition.x * combinedRadius + relativePosition.y * leg) / distSqr; } float dotProduct2 = math.dot(relativeVelocity, line.direction); u = dotProduct2 * line.direction - relativeVelocity; } } else { //Collision float2 w = relativeVelocity - invTimeStep * relativePosition; float wLength = math.length(w); float2 wUnit = w / wLength; line.direction = new float2(wUnit.y, -wUnit.x); u = (combinedRadius * invTimeStep - wLength) * wUnit; } line.point = velocity + 0.5f * u; orcaLines[neighborIdx] = line; } float2 optimalVel = velocity; float2 vel = float2.zero; float maxSpeed = velocities[entityIdx].maxSpeed; int lineFail = LinearProgram2(orcaLines, neighborsCount, maxSpeed, optimalVel, false, ref vel); if (lineFail < neighborsCount) { LinearProgram3(orcaLines, neighborsCount, nbObstacleLine, lineFail, maxSpeed, ref vel); } velocities[entityIdx] = new Velocity() { Value = vel, maxSpeed = maxSpeed }; } quadrantKeys.Dispose(); orcaLines.Dispose(); agentNeighbors.Dispose(); }