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