public Vector3 GetMovePoint(Vector3 from, Vector3 to) { NavResult result = new NavResult(from, to); foreach (var o in obstacles) { o.GetMovePoint(ref result); } return(result.movePoint); }
public void GetMovePoint(ref NavResult ioR) { Vector3 point = transform.position - ioR.from; float sqrPointDistance = point.sqrMagnitude; float pointDistance = Mathf.Sqrt(sqrPointDistance); float radiusDivDistance = _radius / pointDistance; if (ioR.hit == HitType.Critical) { return; } else if (ioR.hit == HitType.Risky) { if (radiusDivDistance > ioR.radiusDivDistance) { return; } } if (sqrPointDistance < sqrRiskyRadius) { ioR.radiusDivDistance = radiusDivDistance; ioR.pointDistance = pointDistance; ioR.hitDistance = 0; ioR.obstacle = this; if (pointDistance < _radius) { ioR.hit = HitType.Critical; ioR.movePoint = point * -(maneuverRadius / pointDistance); } else { ioR.hit = HitType.Risky; ioR.movePoint = GetXAxis(ioR.from, ioR.to) * maneuverRadius; } return; } float cosa = Vector3.Dot(point, ioR.rayAxis) / pointDistance; if (cosa > 1f) { cosa = 1f; } if (cosa < 0) { return; } float ySide = pointDistance * cosa; float sqrR = sqrPointDistance - ySide * ySide; //g^2 = k1^2 + k2^2 if (sqrR < 0) { sqrR *= -1; } if (sqrR < sqrRiskyRadius) { var y = Mathf.Sqrt(sqrRiskyRadius - sqrR); var hitDistance = ySide - y; if ((hitDistance > ioR.rayDistance) || ((ioR.hit != HitType.None) && (hitDistance > ioR.hitDistance))) { return; } ioR.radiusDivDistance = radiusDivDistance; ioR.pointDistance = pointDistance; ioR.hitDistance = hitDistance; ioR.hit = HitType.Safe; ioR.obstacle = this; ioR.movePoint = (sqrR != 0 ? (ioR.rayAxis * ySide - point) / Mathf.Sqrt(sqrR) : Vector3.Cross(Vector3.up, ioR.rayAxis)) * maneuverRadius; } }