// 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); }
// Works pretty well void MaintainFormation(string pos) { if (!formationLead) { return; } bool movingWP = true; bool temporary = false; float wpRadius = 1.5f; GameObject positionObject = formationLead.transform.Find("Formation4(Clone)/" + pos).gameObject; Vector3 posCoords = positionObject.transform.position; Vector2 wpCoordinates = new Vector2(posCoords.x, posCoords.y); NTools.Waypoint formationWP = new NTools.Waypoint(wpCoordinates, wpRadius, movingWP, temporary); if (wpList.Count == 0) { wpList.Add(formationWP); } else if (wpList.Count > 1) { ClearWaypoints(); wpList.Add(formationWP); } else { wpList[0].wpCoordinates = formationWP.wpCoordinates; } // Checks if wingman is on position // if so, applies position keeping lateral thrust and maintains heading of lead // TCAS override implemented //float distanceToPos = Mathf.Sqrt(sqrDistanceToPos); Vector3 wpLocalCoords = gameObject.transform.InverseTransformPoint(posCoords); float sqrDistanceToPos = Vector2.SqrMagnitude(wpLocalCoords); if (wpLocalCoords.x < wpList[wpIndex].wpRadius * 3f && wpLocalCoords.y < wpList[wpIndex].wpRadius * 3f) { FormationPositionCorrection(wpLocalCoords); } /*else if (wpLocalCoords.y > 0f && wpLocalCoords.y < 4f && Mathf.Abs(wpCoordinates.x) > wpList[wpIndex].wpRadius) * { * FormationPositionCorrection(wpLocalCoords); * }*/ else { // ------ Speed adjustment, based on distance from position in formation if (sqrDistanceToPos < 121f) { desiredSpeed = patrolSpeedMax + 0.5f; } if (sqrDistanceToPos > 121f && sqrDistanceToPos < 400f) { desiredSpeed = patrolSpeedMax + 1f; } if (sqrDistanceToPos > 400f) { desiredSpeed = patrolSpeedMax + 3f; } desiredHeading = headingToWaypoint; Move(); // If the Wingman is off position, regular move is used. } }