Unity3d半圆绕开障碍物算法

float r = 1.5f;
float x0 = ObstacleObject.transform.position.x;
float z0 = ObstacleObject.transform.position.z;

Vector3 vec1 = new Vector3(0.0f, 0.0f, 1.0f);
Vector3 vec2 = new Vector3(LandPosition.x - x0, 0.0f, LandPosition.z - z0);
Vector3 vec3 = Vector3.Project(vec2, vec1);
float signedAngle = Vector3.SignedAngle(vec2, vec1, Vector3.up);

float dot = Vector3.Dot(vec1, vec2);
float sign = dot > 0 ? 1.0f : -1.0f;
float angle = Mathf.Rad2Deg * Mathf.Acos(-sign * vec3.magnitude / r);

//两个异号
if ((signedAngle * angle) < 0.0f)
{
	angle = -1.0f * angle;
}

Vector3 vec4 = Quaternion.AngleAxis(angle, Vector3.up) * (-vec1.normalized);
Vector2 pt1 = new Vector2(x0, z0);
Vector2 pt2 = new Vector2(LandPosition.x, LandPosition.z);
float dist = Vector2.Distance(pt1, pt2);
if (dist < r && vec4.magnitude > 0.0001f)
{
	Debug.Log(string.Format("dist = {0}, vec4.magnitude= {1}", dist, vec4.magnitude));
	Vector3 pt5 = new Vector3(x0, LandPosition.y, z0);
	uavGroup.transform.position = pt5 + r * vec4.normalized;
}
else
{
	Debug.Log("isAvoidObstacle = false");
	uavGroup.isAvoidObstacle = false;
}