public static void ComputeNewVelocity(ref NavigationData agent, Translation agentTranslation, Entity agentEntity,
NativeArray <NavigationData> agentNeighbors,
NativeArray <Translation> translations,
NativeArray <Entity> entities,
float simulatorTimeStep,
float maxDistance)
{
NativeList <Line> orcaLines = new NativeList <Line>(1, Allocator.Temp);
float invTimeHorizon = 1.0f / agent.TimeHorizon;
float maxDistanceSq = maxDistance * maxDistance;
var agentPosition = agentTranslation.Value.xy;
/* Create agent ORCA lines. */
for (int i = 0; i < agentNeighbors.Length; ++i)
{
NavigationData other = agentNeighbors[i];
float2 otherPosition = translations[i].Value.xy;
Entity otherEntity = entities[i];
if (otherEntity == agentEntity)
{
continue;
}
var distanceSqr = (agentPosition - otherPosition).LengthSquared();
if (distanceSqr > maxDistanceSq)
{
continue;
}
var line = ORCAConstrainLogic.CalculateConstrain(agent, other, otherPosition, agentPosition, invTimeHorizon, simulatorTimeStep);
orcaLines.Add(line);
}
int lineFail = ORCALinearProgramsSolver.linearProgram2(orcaLines, agent.MaxSpeed, agent.PreferredVelocity, false, ref agent.Velocity);
if (lineFail < orcaLines.Length)
{
ORCALinearProgramsSolver.linearProgram3(orcaLines, lineFail, agent.MaxSpeed, ref agent.Velocity);
}
orcaLines.Dispose();
}
public static void Compute(NavigationBehaviour navigation, NavigationBehaviour[] agentNeighbors, float simulatorTimeStep, float maxDistance)
{
NativeList <Line> orcaLines = new NativeList <Line>(1, Allocator.Temp);
float invTimeHorizon = 1.0f / navigation.Data.TimeHorizon;
float maxDistanceSq = maxDistance * maxDistance;
Vector2 agentPosition = navigation.transform.position;
/* Create agent ORCA lines. */
for (int i = 0; i < agentNeighbors.Length; ++i)
{
NavigationData other = agentNeighbors[i].Data;
Vector2 otherPosition = agentNeighbors[i].transform.position;
if (navigation == agentNeighbors[i])
{
continue;
}
var distanceSqr = (agentPosition - otherPosition).sqrMagnitude;
if (distanceSqr > maxDistanceSq)
{
continue;
}
var line = ORCAConstrainLogic.CalculateConstrain(navigation.Data, other, otherPosition, agentPosition, invTimeHorizon, simulatorTimeStep);
orcaLines.Add(line);
}
int lineFail = ORCALinearProgramsSolver.linearProgram2(orcaLines, navigation.Data.MaxSpeed, navigation.Data.PreferredVelocity, false, ref navigation.Data.Velocity);
if (lineFail < orcaLines.Length)
{
ORCALinearProgramsSolver.linearProgram3(orcaLines, lineFail, navigation.Data.MaxSpeed, ref navigation.Data.Velocity);
}
orcaLines.Dispose();
}