// 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);
}
// 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);
}
