Esempio n. 1
0
        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();
        }
Esempio n. 2
0
        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();
        }