public TreeNode(TreeSettings aSettings, List <TreePoint> aPoints, int aDepth)
            {
                int axis = aSettings.GetAxis(aDepth);

                if (axis == 0)
                {
                    aPoints.Sort(sortX);
                }
                else if (axis == 1)
                {
                    aPoints.Sort(sortY);
                }
                else if (axis == 2)
                {
                    aPoints.Sort(sortZ);
                }

                int median = aPoints.Count / 2;

                point = aPoints[median];

                List <TreePoint> leftList  = aPoints.GetRange(0, median);
                List <TreePoint> rightList = aPoints.GetRange(median + 1, aPoints.Count - (median + 1));

                if (leftList.Count > 0)
                {
                    left = new TreeNode(aSettings, leftList, aDepth + 1);
                }
                if (rightList.Count > 0)
                {
                    right = new TreeNode(aSettings, rightList, aDepth + 1);
                }
            }
        public TreePoint Get(Vector3 aAt)
        {
            TreePoint pt   = null;
            float     dist = 0;

            root.GetNearest(settings, 0, aAt, ref pt, ref dist);
            return(pt);
        }
            public void GetNearest(TreeSettings aSettings, int aDepth, Vector3 aPt, ref TreePoint aClosest, ref float aClosestDist)
            {
                if (IsLeaf)
                {
                    float dist = (point.point - aPt).sqrMagnitude;
                    if (aClosest == null || dist < aClosestDist)
                    {
                        aClosest     = point;
                        aClosestDist = dist;
                    }
                    return;
                }

                int  axis   = aSettings.GetAxis(aDepth);
                bool goLeft = false;

                if (axis == 0)
                {
                    goLeft = aPt.x <= point.point.x ? true : false;
                }
                else if (axis == 1)
                {
                    goLeft = aPt.y <= point.point.y ? true : false;
                }
                else if (axis == 2)
                {
                    goLeft = aPt.z <= point.point.z ? true : false;
                }

                TreeNode first = goLeft ? left : right;
                TreeNode other = goLeft ? right: left;

                if (first == null)
                {
                    first = other;
                    other = null;
                }

                first.GetNearest(aSettings, aDepth + 1, aPt, ref aClosest, ref aClosestDist);

                float thisDist = (point.point - aPt).sqrMagnitude;

                if (thisDist < aClosestDist)
                {
                    aClosest     = point;
                    aClosestDist = thisDist;
                }

                if (other != null)
                {
                    float axisDist = aSettings.AxisDist(axis, point.point, aPt);
                    if (axisDist * axisDist <= aClosestDist)
                    {
                        other.GetNearest(aSettings, aDepth + 1, aPt, ref aClosest, ref aClosestDist);
                    }
                }
            }
Esempio n. 4
0
    public TreePoint(Vector3 position, float theta, float velocity, float cost, TreePoint parent)
    {
        this.position = position;
        this.theta    = theta;
        this.velocity = velocity;
        this.cost     = cost;

        this.parent = parent;
        children    = new List <TreePoint>();
    }
Esempio n. 5
0
    /// <summary>
    /// Generates the heightmap thread.
    /// </summary>
    private void    GenerateHeightmapThread()
    {
        lock (HeightmapThread)
        {
            Heightmap = NoiseProvider.GetHeightmapData(Setting.HeightmapResolution, Setting.HeightmapResolution, Position.X, Position.Z);

            if (Setting.Trees.Count > 0)
            {
                for (var zRes = 0; zRes < Setting.HeightmapResolution; zRes++)
                {
                    for (var xRes = 0; xRes < Setting.HeightmapResolution; xRes++)
                    {
                        int height = Mathf.FloorToInt(Heightmap[zRes, xRes] * 100);
                        if (height > 30 && zRes % 20 == 0 && xRes % 20 == 0)
                        {
                            var xCoordinate = (float)xRes / (Setting.HeightmapResolution - 1);
                            var zCoordinate = (float)zRes / (Setting.HeightmapResolution - 1);

                            Vector3 point = new Vector3(xCoordinate, zCoordinate, height);
                            TreePoint.Add(point);
                        }
                        else
                        {
                            if (height == 0 && zRes % 10 == 0 && xRes % 10 == 0)
                            {
                                var xCoordinate = (float)xRes / (Setting.HeightmapResolution - 1);
                                var zCoordinate = (float)zRes / (Setting.HeightmapResolution - 1);

                                Vector3 point = new Vector3(xCoordinate, zCoordinate, height);
                                ChopPoint.Add(point);
                            }
                        }
                    }
                }
            }
        }
    }
Esempio n. 6
0
        private void Awake()
        {
            Time.timeScale = 1;
            // get the car controller
            m_Car           = GetComponent <CarController>();
            terrain_manager = terrain_manager_game_object.GetComponent <TerrainManager>();

            maxVelocity  = 150;
            acceleration = 1f;

            InitializeCSpace();

            RRTIterations     = 500000; // Limits the number of iterations allowed in the RRT before running the car.
            maxTimesFoundGoal = 10;     // Limits the times that we find goal before running the car.
            maxShortcutAngle  = 5;      // Angle deviance accepted for points that can give a straight line shortcut.
            maxShortcutTheta  = 15;
            maxDistTarget     = 5;      // Maximum accepted distance from a target point.
            timeStep          = 0.05f;
            numberOfSteps     = 5;
            time = 0;

            steerDirection        = 0;
            accelerationDirection = 0;
            brake     = 0;
            handBrake = 0;

            carLength = FindCarLength();

            world_width  = (terrain_manager.myInfo.x_high - terrain_manager.myInfo.x_low);
            world_height = (terrain_manager.myInfo.z_high - terrain_manager.myInfo.z_low);
            gridSize     = Mathf.Min(
                world_width / terrain_manager.myInfo.x_N,
                world_height / terrain_manager.myInfo.z_N
                );
            gridAmountX = (int)(world_width / gridSize);
            gridAmountZ = (int)(world_height / gridSize);
            start       = new TreePoint(terrain_manager.myInfo.start_pos, m_Car.transform.eulerAngles.y, 0, 0);
            treePoints  = new List <TreePoint> [gridAmountX, gridAmountZ];
            for (int i = 0; i < gridAmountX; ++i)
            {
                for (int j = 0; j < gridAmountZ; ++j)
                {
                    if (NearestNeighbourTraversable(i, j))
                    {
                        treePoints[i, j] = new List <TreePoint>();
                    }
                }
            }
            treePoints[NearestNeighbourGetIndexI(start.position.x), NearestNeighbourGetIndexJ(start.position.z)].Add(start);
            treeSize = 1;

            shortestPointToGoalDistance = Vector3.Distance(start.position, terrain_manager.myInfo.goal_pos);
            path            = new List <TreePoint>();
            pathIndex       = 1;
            goalFoundAmount = 0;

            path = RRT();

            if (path == null)
            {
                Debug.Log("No path found!");
            }
            nextPoint = path[pathIndex];

            crashed          = false;
            crashTime        = 0;
            crashCheckTime   = 0.5f;
            crashDirection   = 0;
            previousPosition = Vector3.up;
        }
Esempio n. 7
0
        //Simulate movement from a point in the tree to a point in the plane.
        public TreePoint SimulateMovement(TreePoint from, Vector3 to, int timeFactor)
        {
            Vector3 position = from.position;
            float   theta    = from.theta;
            float   velocity = from.velocity;
            float   cost     = from.cost;

            for (int step = 0; step < timeFactor * numberOfSteps; step++)
            {
                //Get steering angle
                float delta   = m_Car.m_MaximumSteerAngle * SteerInput(position, theta, to);
                float braking = 1;
                if (AccelerationInput(position, theta, to) < 0)
                {
                    delta = -delta;
                }
                if (Mathf.Abs(delta) <= 0.4f * m_Car.m_MaximumSteerAngle)
                {
                    delta = 0; //Do we need this?
                }
                else if (Mathf.Abs(delta) > 0.8f * m_Car.m_MaximumSteerAngle)
                {
                    if (Mathf.Abs(velocity) > maxVelocity / 10)
                    {
                        braking = -1;
                    }
                }

                /*
                 * //Calculate motion model values according to kinematic car model
                 * float xDiff = velocity * Mathf.Sin(Mathf.Deg2Rad * theta);
                 * float zDiff = velocity * Mathf.Cos(Mathf.Deg2Rad * theta);
                 * float thetaDiff = velocity / carLength * Mathf.Tan(Mathf.Deg2Rad * delta) * Mathf.Rad2Deg;
                 *
                 * //Get new position and orientation using Euler's method
                 * Vector3 newPosition = new Vector3(Euler(position.x, xDiff, timeStep), 0, Euler(position.z, zDiff, timeStep));
                 * cost += Vector3.Distance(position, newPosition);
                 * position = newPosition;
                 * theta = Euler(theta, thetaDiff, timeStep);
                 */

                //Get new position and orientation using RK4
                float[] nextState = RK4Step(position.x, position.z, theta, delta, carLength, velocity, timeStep);
                position.x = nextState[0];
                position.z = nextState[1];
                theta      = nextState[2];


                //If collision, this point is not traversable
                if (configurationSpace.Collision(position.x, position.z, theta))
                {
                    return(null);
                }

                //If close enough to end position, stop iterating
                if (Vector3.Distance(position, to) <= 0.1f)
                {
                    break;
                }

                velocity += Mathf.Clamp(AccelerationInput(position, theta, to) * acceleration * timeStep * braking, -maxVelocity, maxVelocity);
            }

            TreePoint result = new TreePoint(position, theta, velocity, cost);

            return(result);
        }
Esempio n. 8
0
        public List <TreePoint> RRT()
        {
            bool      foundGoal = false;
            TreePoint pathPoint = null;

            for (int i = 0; i < RRTIterations; i++)
            {
                Vector3 randomPoint;
                do
                {
                    float probability = foundGoal ? 1 : UnityEngine.Random.Range(0f, 1f);
                    if (probability < 0.1f)
                    {
                        Vector2 p = UnityEngine.Random.insideUnitCircle * shortestPointToGoalDistance;
                        randomPoint = new Vector3(
                            p.x,
                            0,
                            p.y
                            );
                    }
                    else
                    {
                        randomPoint = new Vector3(
                            UnityEngine.Random.Range(terrain_manager.myInfo.x_low, terrain_manager.myInfo.x_high),
                            0,
                            UnityEngine.Random.Range(terrain_manager.myInfo.z_low, terrain_manager.myInfo.z_high)
                            );
                    }
                }while (configurationSpace.Collision(randomPoint.x, randomPoint.z, 0));

                List <TreePoint> nearestPoints = kNearestNeighbours(randomPoint, 1);
                TreePoint        nearPoint     = nearestPoints.Count > 0 ? nearestPoints[0] : null;
                if (nearPoint == null)
                {
                    continue;
                }
                TreePoint newPoint = SimulateMovement(nearPoint, randomPoint, 1);
                if (newPoint != null)
                {
                    //Check traversability
                    int newIIndex = NearestNeighbourGetIndexI(newPoint.position.x);
                    int newJIndex = NearestNeighbourGetIndexJ(newPoint.position.z);
                    if (!NearestNeighbourTraversable(newIIndex, newJIndex))
                    {
                        continue;
                    }
                    nearestPoints = kNearestNeighbours(newPoint.position, Mathf.Min(10 + treeSize / 1000, treeSize));
                    treePoints[newIIndex, newJIndex].Add(newPoint);
                    treeSize++;

                    //RRT*
                    TreePoint minPoint = nearPoint;
                    float     minCost  = newPoint.cost;

                    foreach (TreePoint point in nearestPoints)
                    {
                        TreePoint p = SimulateMovement(point, newPoint.position, 3);
                        if (p != null)
                        {
                            if (Vector3.Distance(p.position, newPoint.position) <= 0.1f && p.cost < minCost)
                            {
                                minCost  = p.cost;
                                minPoint = point;
                            }
                        }
                    }

                    newPoint.parent = minPoint;
                    minPoint.children.Add(newPoint);

                    //Update value for sampling heuristic
                    if (Vector3.Distance(newPoint.position, terrain_manager.myInfo.goal_pos) < shortestPointToGoalDistance)
                    {
                        shortestPointToGoalDistance = Vector3.Distance(newPoint.position, terrain_manager.myInfo.goal_pos);
                    }

                    //Found goal
                    if (Vector3.Distance(newPoint.position, terrain_manager.myInfo.goal_pos) <= 5)
                    {
                        foundGoal = true;
                        if (pathPoint == null)
                        {
                            pathPoint = newPoint;
                        }
                        else if (newPoint.cost < pathPoint.cost)
                        {
                            pathPoint = newPoint;
                        }
                        goalFoundAmount++;
                        if (goalFoundAmount >= maxTimesFoundGoal)
                        {
                            Debug.Log("Found goal " + maxTimesFoundGoal + "  times, stopping.");
                            break;
                        }
                    }
                }
            }

            if (foundGoal)
            {
                List <TreePoint> goalPath = new List <TreePoint> {
                    pathPoint
                };
                while (pathPoint.parent != null)
                {
                    goalPath.Insert(0, pathPoint.parent);
                    pathPoint = pathPoint.parent;
                }
                Debug.Log("Reached " + RRTIterations + " RRT-iterations with at least one goal, stopping.");
                return(goalPath);
            }
            Debug.Log("Reached " + RRTIterations + " RRT-iterations but found no goal, stopping.");
            return(null);
        }
Esempio n. 9
0
        private void FixedUpdate()
        {
            if (!crashed)
            {
                time += Time.deltaTime;
                if (time >= crashCheckTime && Vector3.Distance(m_Car.transform.position, terrain_manager.myInfo.start_pos) > 5)
                {
                    time = 0;
                    if (Vector3.Distance(previousPosition, m_Car.transform.position) < 0.1f)
                    {
                        crashed = true;
                        if (Physics.BoxCast(
                                m_Car.transform.position,
                                new Vector3(configurationSpace.BoxSize.x / 2, configurationSpace.BoxSize.y / 2, 0.5f),
                                m_Car.transform.forward,
                                Quaternion.LookRotation(m_Car.transform.forward),
                                configurationSpace.BoxSize.z / 2
                                ))
                        {
                            crashDirection = -1;
                        }
                        else
                        {
                            crashDirection = 1;
                        }
                    }
                    else
                    {
                        previousPosition = m_Car.transform.position;
                    }
                }


                //Improve path by straightening unnecessary curves
                if (pathIndex < path.Count - 1)
                {
                    for (int i = path.Count - 1; i >= pathIndex + 1; --i)
                    {
                        Vector3 position  = m_Car.transform.position;
                        Vector3 direction = path[i].position - position;
                        float   angle     = Vector3.Angle(m_Car.transform.forward, direction);

                        //If somewhat straight to point on path, check if it is possible to go there
                        if (angle <= maxShortcutAngle && Quaternion.Angle(Quaternion.Euler(0, path[i].theta, 0), Quaternion.LookRotation(direction)) <= maxShortcutTheta)
                        {
                            bool collision = false;
                            for (float h = 0; h <= 1; h += 0.005f)
                            {
                                float x = (position + direction * h).x;
                                float z = (position + direction * h).z;
                                if (configurationSpace.Collision(x, z, Quaternion.LookRotation(direction).y))
                                {
                                    collision = true;
                                }
                            }

                            //No collision, go to this point instead
                            if (!collision)
                            {
                                nextPoint = path[pathIndex];
                                pathIndex = i;
                                break;
                            }
                        }
                    }
                }

                steerDirection = SteerInput(m_Car.transform.position, m_Car.transform.eulerAngles.y, nextPoint.position);
                if (Mathf.Abs(steerDirection) < 0.2f)
                {
                    steerDirection = 0;
                }
                brake = 0;
                if (Mathf.Abs(steerDirection) > 0.8f && m_Car.CurrentSpeed > maxVelocity / 10)
                {
                    accelerationDirection = 0;
                    if (m_Car.CurrentSpeed > maxVelocity / 5)
                    {
                        handBrake = 1;
                    }
                    else
                    {
                        handBrake = 0;
                    }
                }
                else
                {
                    accelerationDirection = AccelerationInput(m_Car.transform.position, m_Car.transform.eulerAngles.y, nextPoint.position);
                    handBrake             = 0;
                }
                if (pathIndex < path.Count - 1)
                {
                    int stepsToCheck = Mathf.Min(3 + (int)(m_Car.CurrentSpeed * 1.6f * 1.6f * m_Car.CurrentSpeed / 500), path.Count - 1 - pathIndex);
                    for (int i = 1; i <= stepsToCheck; ++i)
                    {
                        float steerCheck = SteerInput(m_Car.transform.position, m_Car.transform.eulerAngles.y, path[pathIndex + i].position);
                        if (Mathf.Abs(steerCheck) > 0.8f && (m_Car.CurrentSpeed * 1.6f * 1.6f * m_Car.CurrentSpeed) >= Vector3.Distance(m_Car.transform.position, path[pathIndex + i].position) * 250 * 0.8f)
                        {
                            accelerationDirection = 0;
                            brake = 1;
                            break;
                        }
                    }
                }

                if (m_Car.CurrentSpeed >= maxVelocity)
                {
                    accelerationDirection = 0;
                }

                if (accelerationDirection < 0)
                {
                    m_Car.Move(-steerDirection, brake, accelerationDirection * acceleration, handBrake);
                }
                else
                {
                    m_Car.Move(steerDirection, accelerationDirection * acceleration, -brake, handBrake);
                }
            }
            else
            {
                crashTime += Time.deltaTime;
                if (crashTime <= 1f)
                {
                    steerDirection = SteerInput(m_Car.transform.position, m_Car.transform.eulerAngles.y, nextPoint.position);
                    if (crashDirection > 0)
                    {
                        m_Car.Move(steerDirection, acceleration, 0, 0);
                    }
                    else
                    {
                        m_Car.Move(-steerDirection, 0, -acceleration, 0);
                    }
                }
                else
                {
                    crashTime = 0;
                    crashed   = false;
                }
            }

            //Update point if close enough to current one
            if (Vector3.Distance(m_Car.transform.position, nextPoint.position) <= maxDistTarget + m_Car.CurrentSpeed / 40)
            {
                pathIndex = Mathf.Min(pathIndex + 1, path.Count - 1);
                nextPoint = path[pathIndex];
            }
        }