Esempio n. 1
0
    // returns TRUE when CA has determined a threat and sucessfully added a WP to solve conflict
    // False is returned when either CA failed or no threat was found
    bool CollisionAvoidance()
    {
        Vector2 shipProjectedPos = gameObject.GetComponent <Rigidbody2D>().position
                                   + (wpList[wpIndex].wpCoordinates - gameObject.GetComponent <Rigidbody2D>().position).normalized * desiredSpeed;
        Vector2 shipPos      = new Vector2(gameObject.GetComponent <Rigidbody2D>().position.x, gameObject.GetComponent <Rigidbody2D>().position.y);
        Vector2 headingToWP  = wpList[wpIndex].wpCoordinates - shipPos;
        float   distanceToWP = Mathf.Sqrt(Vector2.SqrMagnitude(wpList[wpIndex].wpCoordinates - gameObject.GetComponent <Rigidbody2D>().position));

        Vector2 closestThreat2D         = new Vector2(0f, 0f);
        Vector2 closestThreatVelocity2D = new Vector2(0f, 0f);
        Vector2 newHeadingVector        = new Vector2(0f, 0f);

        // Variables for use in loops, to avoid repeditive declarations
        float timeToInterWP           = 50f / desiredSpeed;
        float distanceToNearestThreat = 0f;
        float newHeadingLeft          = 0f;
        float newHeadingRight         = 0f;
        bool  leftHeading             = true;
        bool  solutionFound           = false;


        // Setting the travel time, by which we set the threat movement amount
        if (distanceToWP >= 50f)
        {
            timeToInterWP = 50f / desiredSpeed;
        }
        else
        {
            timeToInterWP = distanceToWP / desiredSpeed;
        }

        // Sorting the threats, which also fills in missing bits in the threat list
        SortCollisionThreats((wpList[wpIndex].wpCoordinates - gameObject.GetComponent <Rigidbody2D>().position), timeToInterWP);
        // parsing the POTENTIAL threats for REAL threats, the method also returns a BOOL if there are REAL threats at all
        NTools.CollisionThreatsSorted parsedThreats = CheckHeadingClear(shipProjectedPos, wpList[wpIndex].wpCoordinates);

        // If no threats are found, exit CA, else clear temporary wps from the wp list and start meaty bit of CA
        if (!parsedThreats.realThreatsPresent)
        {
            return(false);
        }

        // Determine distance to closest threat and its coordinates -.-- Why do I do it?
        else if (parsedThreats.realThreatsLeft.Count != 0 && parsedThreats.realThreatsRight.Count != 0)
        {
            ClearTemporaryWaypoints();

            if (parsedThreats.realThreatsLeft[0].sqrDistance < parsedThreats.realThreatsRight[0].sqrDistance)
            {
                distanceToNearestThreat = Vector2.Distance(
                    shipPos,
                    parsedThreats.realThreatsLeft[0].threatCoordinates
                    );
                closestThreat2D         = parsedThreats.realThreatsLeft[0].threatCoordinates;
                closestThreatVelocity2D = parsedThreats.realThreatsLeft[0].threatVelocity;
            }
            else
            {
                distanceToNearestThreat = Vector2.Distance(
                    shipPos,
                    parsedThreats.realThreatsRight[0].threatCoordinates
                    );
                closestThreat2D         = parsedThreats.realThreatsRight[0].threatCoordinates;
                closestThreatVelocity2D = parsedThreats.realThreatsRight[0].threatVelocity;
            }
        }
        else if (parsedThreats.realThreatsLeft.Count == 0 && parsedThreats.realThreatsRight.Count != 0)
        {
            ClearTemporaryWaypoints();

            distanceToNearestThreat = Vector2.Distance(
                shipPos,
                parsedThreats.realThreatsRight[0].threatCoordinates
                );
            closestThreat2D         = parsedThreats.realThreatsRight[0].threatCoordinates;
            closestThreatVelocity2D = parsedThreats.realThreatsRight[0].threatVelocity;
        }
        else if (parsedThreats.realThreatsLeft.Count != 0 && parsedThreats.realThreatsRight.Count == 0)
        {
            ClearTemporaryWaypoints();

            distanceToNearestThreat = Vector2.Distance(
                shipPos,
                parsedThreats.realThreatsLeft[0].threatCoordinates
                );
            closestThreat2D         = parsedThreats.realThreatsLeft[0].threatCoordinates;
            closestThreatVelocity2D = parsedThreats.realThreatsLeft[0].threatVelocity;
        }

        Vector2 vectorToThreat = closestThreat2D - shipPos;

        // statusText.text = vectorToThreat.ToString();

        // Ceck if the WP is closer than the threat, in that case, return and stop CA, return false
        if (distanceToWP < distanceToNearestThreat)
        {
            return(false);
        }

        headingToWP = headingToWP.normalized * distanceToNearestThreat;

        // parse intermittently left and right for a clear initial passage
        int iterations = 0;

        do
        {
            switch (leftHeading)
            {
            case true:
                newHeadingLeft -= 1;

                parsedThreats = CheckHeadingClear(shipPos, shipPos + NTools.RotateVector2(headingToWP, newHeadingLeft));
                if (SettingsStatic.debugEnabled)
                {
                    DrawDebugLine(shipPos, shipPos + NTools.RotateVector2(headingToWP, newHeadingLeft));
                }

                leftHeading = false;
                iterations++;
                break;

            case false:
                newHeadingRight += 1;

                parsedThreats = CheckHeadingClear(shipPos, shipPos + NTools.RotateVector2(headingToWP, newHeadingRight));
                if (SettingsStatic.debugEnabled)
                {
                    DrawDebugLine(shipPos, shipPos + NTools.RotateVector2(headingToWP, newHeadingLeft).normalized);
                }

                leftHeading = true;
                iterations++;
                break;
            }
            // exit failsafes:
            if (newHeadingLeft < -90 || newHeadingRight > 90)
            {
                solutionFound = false;
                break;
            }
        } while (parsedThreats.realThreatsPresent);

        if (newHeadingLeft > -90 && newHeadingRight < 90)
        {
            float newHeading;
            if (leftHeading)
            {
                newHeading = newHeadingRight;
            }
            else
            {
                newHeading = newHeadingLeft;
            }
            solutionFound = true;
            //Debug.Log("Clear relative heading found: " + newHeading.ToString());
        }

        if (!solutionFound)
        {
            Debug.Log("Heading not found in " + iterations + " iterations."); return(false);
        }


        // Determine new direction from projected ship position
        if (leftHeading)
        {
            newHeadingVector = NTools.RotateVector2(headingToWP, newHeadingRight);                   /*Debug.Log("New Heading: " + newHeadingRight);   */
        }
        else
        {
            newHeadingVector = NTools.RotateVector2(headingToWP, newHeadingLeft);                   /*Debug.Log("New Heading: " + newHeadingLeft);    */
        }

        float newWPDistance = 1f;

        do
        {
            newWPDistance += 0.1f;
            parsedThreats  = CheckHeadingClear(shipPos + newHeadingVector * newWPDistance, wpList[wpIndex].wpCoordinates);
            // check to limit the possible distance of the intermittent WP to the distance of the original WP
            if (newWPDistance * newHeadingVector.magnitude > distanceToWP)
            {
                Debug.Log("WP solution not found.");
                break;
            }
        } while (parsedThreats.realThreatsPresent);

        if (newWPDistance < 10f)
        {
            solutionFound = true;
        }


        // This happens, when the collisionAvoidance has done its job and can add a new intermediate waypoint
        // also entering this loop will return true - the method has parsed successfully and added a wp
        if (solutionFound)
        {
            NTools.Waypoint newInbetweenWP = new NTools.Waypoint(shipPos + newHeadingVector * (newWPDistance - 0.2f), 4f, false, true);
            wpList.Add(newInbetweenWP);
            wpIndex = wpList.Count - 1;
            return(true);
        }
        return(false);
    }
Esempio n. 2
0
    // Determines real threats and sorts them by distance
    NTools.CollisionThreatsSorted CheckHeadingClear(Vector2 initialPosition, Vector2 aimPoint)
    {
        //The ship Projected velocity is from initial position to aimpoint, NOT the real velocity vector!!!
        Vector3 shipProjectedVel = new Vector3((aimPoint - initialPosition).normalized.x, (aimPoint - initialPosition).normalized.y, 0f) * desiredSpeed;
        Vector2 shipPos          = gameObject.GetComponent <Rigidbody2D>().position;

        Vector3 threatLocalPos;

        List <NTools.CollisionThreat> realThreatsLeft  = new List <NTools.CollisionThreat>(20);
        List <NTools.CollisionThreat> realThreatsRight = new List <NTools.CollisionThreat>(20);

        NTools.CollisionThreatsSorted ctSorted = new NTools.CollisionThreatsSorted(false, realThreatsLeft, realThreatsRight);

        float separation = 0f; float separation2 = 0f;


        // Parse for real threats and add them to ctSorted
        for (int i = 0; i < collisionThreatList.Count; i++)
        {
            threatLocalPos = gameObject.transform.InverseTransformPoint(new Vector3(
                                                                            collisionThreatList[i].threatCoordinates.x,
                                                                            collisionThreatList[i].threatCoordinates.y,
                                                                            0f
                                                                            ));

            if (threatLocalPos.y < 0)
            {
                continue;
            }

            // Debug.Log("Threat pos" + gameObject.transform.InverseTransformPoint(collisionThreatList[i].threatCoordinates.x, collisionThreatList[i].threatCoordinates.y,0f));

            separation = Vector3.Cross(
                new Vector3(collisionThreatList[i].threatCoordinates.x - shipPos.x, collisionThreatList[i].threatCoordinates.y - shipPos.y, 0f),
                shipProjectedVel
                ).magnitude / shipProjectedVel.magnitude;
            // Debug.Log("Sep1: " + separation);

            separation2 = Vector3.Cross(
                new Vector3(collisionThreatList[i].threatCoordinates2.x - shipPos.x, collisionThreatList[i].threatCoordinates2.y - shipPos.y, 0f),
                shipProjectedVel
                ).magnitude / shipProjectedVel.magnitude;

            // Debug.Log("Sep2: " + separation2);

            // if threat is valid push to appropriate list for side, also sorts so, that the nearest threat has lowest index
            if (separation < minSeparation || separation2 < minSeparation)
            {
                ctSorted.realThreatsPresent = true;

                if (collisionThreatList[i].leftRightOfHeading < 0f)
                {
                    if (realThreatsLeft.Count == 0)
                    {
                        realThreatsLeft.Add(collisionThreatList[i]);
                    }
                    else if (realThreatsLeft[realThreatsLeft.Count - 1].sqrDistance < collisionThreatList[i].sqrDistance)
                    {
                        realThreatsLeft.Add(collisionThreatList[i]);
                    }
                    else if (realThreatsLeft[realThreatsLeft.Count - 1].sqrDistance > collisionThreatList[i].sqrDistance)
                    {
                        NTools.CollisionThreat tempFromLefts = realThreatsLeft[realThreatsLeft.Count - 1];
                        realThreatsLeft.RemoveAt(realThreatsLeft.Count - 1);
                        realThreatsLeft.Add(collisionThreatList[i]);
                        realThreatsLeft.Add(tempFromLefts);
                    }
                }
                else
                {
                    if (realThreatsRight.Count == 0)
                    {
                        realThreatsRight.Add(collisionThreatList[i]);
                    }
                    else if (realThreatsRight[realThreatsRight.Count - 1].sqrDistance < collisionThreatList[i].sqrDistance)
                    {
                        realThreatsRight.Add(collisionThreatList[i]);
                    }
                    else if (realThreatsRight[realThreatsRight.Count - 1].sqrDistance > collisionThreatList[i].sqrDistance)
                    {
                        NTools.CollisionThreat tempFromLefts = realThreatsRight[realThreatsRight.Count - 1];
                        realThreatsRight.RemoveAt(realThreatsRight.Count - 1);
                        realThreatsRight.Add(collisionThreatList[i]);
                        realThreatsRight.Add(tempFromLefts);
                    }
                }
            }
        }

        return(ctSorted);
    }