protected override void Prepare(ref Unemployed job, float delta)
{
int agentCount = m_lockedAgents.Count;
if (m_outputAgents.Length != agentCount)
{
m_outputAgents.Dispose();
m_outputAgents = new NativeArray <AgentData>(agentCount, Allocator.Persistent);
}
m_maxRadius = 0f;
Agent a;
float3 pos, prefVel, vel;
if (plane == AxisPair.XY)
{
for (int i = 0; i < agentCount; i++)
{
a = m_lockedAgents[i];
m_maxRadius = max(m_maxRadius, a.radius);
pos = a.pos;
prefVel = a.m_prefVelocity;
vel = a.m_velocity;
m_outputAgents[i] = new AgentData()
{
index = i,
kdIndex = i,
position = float2(pos.x, pos.y), //
worldPosition = pos,
baseline = pos.z,
prefVelocity = float2(prefVel.x, prefVel.y),
velocity = float2(vel.x, vel.y),
worldVelocity = vel,
height = a.m_height,
radius = a.m_radius,
radiusObst = a.m_radiusObst,
maxSpeed = a.m_maxSpeed,
maxNeighbors = a.m_maxNeighbors,
neighborDist = a.m_neighborDist,
neighborElev = a.m_neighborElev,
timeHorizon = a.m_timeHorizon,
timeHorizonObst = a.m_timeHorizonObst,
navigationEnabled = a.m_navigationEnabled,
collisionEnabled = a.m_collisionEnabled,
layerOccupation = a.m_layerOccupation,
layerIgnore = a.m_layerIgnore
};
}
}
else
{
for (int i = 0; i < agentCount; i++)
{
a = m_lockedAgents[i];
m_maxRadius = max(m_maxRadius, a.radius);
pos = a.pos;
prefVel = a.m_prefVelocity;
vel = a.m_velocity;
m_outputAgents[i] = new AgentData()
{
index = i,
position = float2(pos.x, pos.z), //
worldPosition = pos,
baseline = pos.y,
prefVelocity = float2(prefVel.x, prefVel.z),
velocity = float2(vel.x, vel.z),
worldVelocity = vel,
height = a.m_height,
radius = a.m_radius,
radiusObst = a.m_radiusObst,
maxSpeed = a.m_maxSpeed,
maxNeighbors = a.m_maxNeighbors,
neighborDist = a.m_neighborDist,
neighborElev = a.m_neighborElev,
timeHorizon = a.m_timeHorizon,
timeHorizonObst = a.m_timeHorizonObst,
navigationEnabled = a.m_navigationEnabled,
collisionEnabled = a.m_collisionEnabled,
layerOccupation = a.m_layerOccupation,
layerIgnore = a.m_layerIgnore
};
}
}
}
/// <summary>
/// Recursive method for building an agent k-D tree.
/// </summary>
/// <param name="begin">The beginning agent k-D tree node node index.</param>
/// <param name="end">The ending agent k-D tree node index.</param>
/// <param name="node">The current agent k-D tree node index.</param>
private void BuildAgentTreeRecursive(int begin, int end, int node)
{
AgentTreeNode treeNode = m_outputTree[node];
AgentData agent = m_inputAgents[begin];
float2 pos;
treeNode.begin = begin;
treeNode.end = end;
treeNode.minX = treeNode.maxX = agent.position.x;
treeNode.minY = treeNode.maxY = agent.position.y;
for (int i = begin + 1; i < end; ++i)
{
pos = m_inputAgents[i].position;
treeNode.maxX = max(treeNode.maxX, pos.x);
treeNode.minX = min(treeNode.minX, pos.x);
treeNode.maxY = max(treeNode.maxY, pos.y);
treeNode.minY = min(treeNode.minY, pos.y);
}
m_outputTree[node] = treeNode;
if (end - begin > AgentTreeNode.MAX_LEAF_SIZE)
{
// No leaf node.
bool isVertical = treeNode.maxX - treeNode.minX > treeNode.maxY - treeNode.minY;
float splitValue = 0.5f * (isVertical ? treeNode.maxX + treeNode.minX : treeNode.maxY + treeNode.minY);
int left = begin;
int right = end;
while (left < right)
{
while (left < right && (isVertical ? m_inputAgents[left].position.x : m_inputAgents[left].position.y) < splitValue)
{
++left;
}
while (right > left && (isVertical ? m_inputAgents[right - 1].position.x : m_inputAgents[right - 1].position.y) >= splitValue)
{
--right;
}
if (left < right)
{
AgentData tempAgent = m_inputAgents[left], rep = m_inputAgents[right - 1];
tempAgent.kdIndex = right - 1;
rep.kdIndex = left;
m_inputAgents[left] = rep;
m_inputAgents[right - 1] = tempAgent;
++left;
--right;
}
}
int leftSize = left - begin;
if (leftSize == 0)
{
++leftSize;
++left;
++right;
}
treeNode.left = node + 1;
treeNode.right = node + 2 * leftSize;
m_outputTree[node] = treeNode;
BuildAgentTreeRecursive(begin, left, treeNode.left);
BuildAgentTreeRecursive(left, end, treeNode.right);
}
}
