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