/** Calculates target point from the current line segment.
* \param p Current position
* \param a Line segment start
* \param b Line segment end
* The returned point will lie somewhere on the line segment.
* \see #forwardLook
* \todo This function uses .magnitude quite a lot, can it be optimized?
*/
protected Vector3 CalculateTargetPoint(Vector3 p, Vector3 a, Vector3 b)
{
a.y = p.y;
b.y = p.y;
float magn = (a - b).magnitude;
if (magn == 0)
{
return(a);
}
float closest = Mathfx.Clamp01(Mathfx.NearestPointFactor(a, b, p));
Vector3 point = (b - a) * closest + a;
float distance = (point - p).magnitude;
float lookAhead = Mathf.Clamp(forwardLook - distance, 0.0F, forwardLook);
float offset = lookAhead / magn;
offset = Mathf.Clamp(offset + closest, 0.0F, 1.0F);
return((b - a) * offset + a);
}
public bool FinalInts(Vector3 target, Vector3 closeEdgeConstraint, bool drawGizmos, out Vector3 closest)
{
ints1.Sort();
ints2.Sort();
ints3.Sort();
float a = (float)Math.Atan2(direction.z, direction.x);
Vector3 cross = Vector3.Cross(direction, Vector3.up);
Vector3 p1 = cross * (float)Math.Tan(angle) * limit;
Vector3 pLeft = origin + direction * limit + p1;
Vector3 pRight = origin + direction * limit - p1;
Vector3 pLeft2 = pLeft + new Vector3((float)Math.Cos(a + angle), 0, (float)Math.Sin(a + angle)) * 100;
Vector3 pRight2 = pRight + new Vector3((float)Math.Cos(a - angle), 0, (float)Math.Sin(a - angle)) * 100;
bool anyCloseFound = false;
closest = Vector3.zero;
int IgnoreDirection = Vector3.Dot(closeEdgeConstraint - origin, cross) > 0 ? 2 : 1; //(pRight-(transform.position+velocity)).sqrMagnitude < (pLeft-(transform.position+velocity)).sqrMagnitude ? 2 : 1;//Vector3.Dot (pRight-pLeft,target-pLeft) > 0 ? 1 : 2;
for (int j = 1; j <= 3; j++)
{
if (j == IgnoreDirection)
{
continue;
}
List <IntersectionPair> ints = j == 1 ? ints1 : (j == 2 ? ints2 : ints3);
/*IntersectionState prevInside = ints[0].state;
* if (ints[0].state == IntersectionState.Outside) {
* ints[0].SetState (IntersectionState.Exit);
* }*/
/*for (int i=1;i<ints.Count;i++) {
* Debug.Log ("Intersection at "+j+", "+i+" : "+ints[i].factor);
* prevInside = ints[i].state;
* if (prevInside == IntersectionState.Outside) {
* ints[i].SetState (IntersectionState.Enter);
* } else if (prevInside == IntersectionState.Inside) {
* ints[i].SetState (ints[i].state == IntersectionState.Inside ? IntersectionState.Inside : IntersectionState.Exit);
* }
* }
* if (ints[ints.Count-1].state == IntersectionState.Exit) {
* ints.Add (new IntersectionPair (1,false));
* }*/
Vector3 start = (j == 1 || j == 3 ? pLeft : pRight);
Vector3 end = (j == 1 ? pLeft2 : (j == 2 ? pRight2 : pRight));
float closestFactor = Mathfx.NearestPointFactor(start, end, target);
float closeMin = float.PositiveInfinity;
float closeMax = float.NegativeInfinity;
bool anySegmClose = false;
for (int i = 0; i < ints.Count - (j == 3 ? 1 : 0); i++)
{
if (drawGizmos)
{
Debug.DrawRay(start + (end - start) * ints[i].factor, Vector3.down, ints[i].state == IntersectionState.Outside ? Color.green : Color.red);
}
if (ints[i].state == IntersectionState.Outside && ((i == ints.Count - 1 && (i == 0 || ints[i - 1].state != IntersectionState.Outside)) || (i < ints.Count - 1 && ints[i + 1].state == IntersectionState.Outside)))
{
anySegmClose = true;
float startFactor = ints[i].factor;
float endFactor = i == ints.Count - 1 ? (j == 3 ? 1 : float.PositiveInfinity) : ints[i + 1].factor;
if (drawGizmos)
{
Debug.DrawLine(start + (end - start) * startFactor + Vector3.up, start + (end - start) * Mathf.Clamp01(endFactor) + Vector3.up, Color.green);
}
if (startFactor <= closestFactor && endFactor >= closestFactor)
{
closeMin = closestFactor;
closeMax = closestFactor;
break;
}
else if (endFactor < closestFactor && endFactor > closeMax)
{
closeMax = endFactor;
}
else if (startFactor > closestFactor && startFactor < closeMin)
{
closeMin = startFactor;
}
}
}
if (anySegmClose)
{
//The closest factor
float closeV = closeMin == float.NegativeInfinity ? closeMax : (closeMax == float.PositiveInfinity ? closeMin : (Mathf.Abs(closestFactor - closeMin) < Mathf.Abs(closestFactor - closeMax) ? closeMin : closeMax));
/*if (Mathf.Abs(closestFactor-closeMin) < Mathf.Abs(closestFactor-closeMax)) {
* segmClose = start+ (end-start)*closeMin;
* } else {
* segmClose = start+ (end-start)*closeMax;
* }*/
Vector3 segmClose = start + (end - start) * closeV;
if (!anyCloseFound || (segmClose - target).sqrMagnitude < (closest - target).sqrMagnitude)
{
closest = segmClose;
}
if (drawGizmos)
{
Debug.DrawLine(target, closest, Color.yellow);
}
anyCloseFound = true;
}
}
return(anyCloseFound);
}
