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