protected void GetAvoidanceVectorsForBox(Intersection intersection)
{
//0. Convert Intersection Point
Vector3 vLocalInter = intersection.Point;
Bounds ObsBounds = intersection.Obstacle.MeshBounds;
Vector3 vDesired = intersection.Point - Vehicle.Position;
vLocalInter = intersection.Obstacle.Transform.InverseTransformPoint(vLocalInter);
vLocalInter.x = Mathf.Clamp(vLocalInter.x, ObsBounds.min.x, ObsBounds.max.x);
vLocalInter.y = Mathf.Clamp(vLocalInter.y, ObsBounds.min.y, ObsBounds.max.y);
vLocalInter.z = Mathf.Clamp(vLocalInter.z, ObsBounds.min.z, ObsBounds.max.z);
//1. Find Closest Side
Box3DSide boxSide = intersection.Obstacle.FindIntersectedSide(vLocalInter);
//2. Get the Normal
Vector3 vTempFor = (boxSide.vUR - boxSide.vUL).normalized;
Vector3 vTempRight = (boxSide.vLL - boxSide.vUL).normalized;
Vector3 vNormal = Vector3.Cross(vTempFor, vTempRight);
//3. Get the closest points of intersection
Vector3 vInter1 = Mathfx.ClosestPtToPointOnSegment(vLocalInter, boxSide.vUL, boxSide.vUR);
Vector3 vInter2 = Mathfx.ClosestPtToPointOnSegment(vLocalInter, boxSide.vUL, boxSide.vLL);
Vector3 vInter3 = Mathfx.ClosestPtToPointOnSegment(vLocalInter, boxSide.vLL, boxSide.vLR);
Vector3 vInter4 = Mathfx.ClosestPtToPointOnSegment(vLocalInter, boxSide.vUR, boxSide.vLR);
//4. Do we run parallel to the shape or try to move around it
Vector3 vLocalVehPos = intersection.Obstacle.Transform.InverseTransformPoint(Vehicle.Position);
Vector3 vLocalDir1 = DecideDirection(vInter1, vLocalInter, vNormal, vLocalVehPos);
Vector3 vLocalDir2 = DecideDirection(vInter2, vLocalInter, vNormal, vLocalVehPos);
Vector3 vLocalDir3 = DecideDirection(vInter3, vLocalInter, vNormal, vLocalVehPos);
Vector3 vLocalDir4 = DecideDirection(vInter4, vLocalInter, vNormal, vLocalVehPos);
//5. Commit to the priority queue
vLocalDir1 = intersection.Obstacle.Transform.TransformDirection(vLocalDir1);
vLocalDir2 = intersection.Obstacle.Transform.TransformDirection(vLocalDir2);
vLocalDir3 = intersection.Obstacle.Transform.TransformDirection(vLocalDir3);
vLocalDir4 = intersection.Obstacle.Transform.TransformDirection(vLocalDir4);
m_pqPotentialDirections.Enqueue(new PotentialDirection(vLocalDir1),
AssignHeuristicValueToDirection(vLocalDir1, vDesired));
m_pqPotentialDirections.Enqueue(new PotentialDirection(vLocalDir2),
AssignHeuristicValueToDirection(vLocalDir2, vDesired));
m_pqPotentialDirections.Enqueue(new PotentialDirection(vLocalDir3),
AssignHeuristicValueToDirection(vLocalDir3, vDesired));
m_pqPotentialDirections.Enqueue(new PotentialDirection(vLocalDir4),
AssignHeuristicValueToDirection(vLocalDir4, vDesired));
}
