// 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.
}
}
