本算法实现2D下的射线检测,可以检测射线段到障碍物的距离。
// An highlighted block
/// <summary>
/// 计算线段(M,P)与圆(C,R)的交点距离目标点(M)最近的点的距离
/// </summary>
/// <param name="P">线段另一顶点</param>
/// <param name="C">圆心</param>
/// <param name="R">圆半径</param>
/// <param name="M">目标点</param>
/// <returns></returns>
private float GetLineTOCirclePoint(Vector M, Vector P, Vector C, float R)
{
float _K; //直线斜率
float _B; //直线截距
float _A = C.X; //圆心X坐标 (X-_A)^2+(Y-_C)^2=R^2
float _C = C.Y; //圆心Y坐标
float X1 = 0.0f; //两点坐标
float X2 = 0.0f;
float Y1 = 0.0f;
float Y2 = 0.0f;
float Dis = Vector.Distance(new Vector(M.X, M.Y), new Vector(P.X, P.Y)); //获取视野距离
if (P.Y == M.Y) //情况一 斜率为0时也就是垂直Y轴
{
if (P.Y > C.Y + R || P.Y < C.Y - R) //如果没有交点则直接返回
return Dis;
else if (P.Y == C.Y + R || P.Y == C.Y - R) //如果有一个交点
{
Vector m = new Vector(C.X, P.Y);
float d = Vector.Distance(M, m);
if (Vector.AngleBetween(new Vector(m.X - M.X, m.Y - M.Y), new Vector(P.X - M.X, P.Y - M.Y)) < Math.PI / 2)
return d > Dis ? Dis : d;
}
else //有两个交点
{
Y1 = M.Y;
Y2 = M.Y;
X1 = _A + (float)Math.Sqrt(R * R - (M.Y - _C) * (M.Y - _C));
X2 = _A - (float)Math.Sqrt(R * R - (M.Y - _C) * (M.Y - _C));
}
}
else if (M.X == P.X) //情况二 斜率不存在也就是垂直X轴
{
if (P.X > C.X + R || P.X < C.X - R) //如果没有交点则直接返回
return Dis;
else if (P.X == C.X + R || P.X == C.X - R) //如果有一个交点
{
Vector m = new Vector(P.X, C.Y);
float d = Vector.Distance(M, m);
if (Vector.AngleBetween(new Vector(m.X - M.X, m.Y - M.Y), new Vector(P.X - M.X, P.Y - M.Y)) < Math.PI / 2)
return d > Dis ? Dis : d;
}
else //有两个交点
{
X1 = M.X;
X2 = M.X;
Y1 = _C + (float)Math.Sqrt(R * R - (M.X - _A) * (M.X - _A));
Y2 = _C - (float)Math.Sqrt(R * R - (M.X - _A) * (M.X - _A));
}
}
else
{
//情况三 斜率存在不为0
_K = (P.Y - M.Y) / (P.X - M.X); //计算斜率
_B = M.Y - (M.X * _K); //由y=kx-x1k+y1计算得出 b=y1-x1*k
float B11 = (2 * _K * (_B - _C) - 2 * _A); //计算一元二次方程的b系数
float A11 = (_K * _K + 1); //计算b
float C11 = ((_B - _C) * (_B - _C) - R * R + _A * _A); //计算c
if (B11 * B11 - 4 * A11 * C11 < 0) //无解表示没有交点,所以距离为0
return Dis;
else if (B11 * B11 - 4 * A11 * C11 == 0) //只有一个交点
{
float X = -(B11) / (2 * A11); //韦达定理
float Y = _K * X + _B; //计算Y坐标
Vector m = new Vector(X, Y);
float d = Vector.Distance(M, m);
if (Vector.AngleBetween(new Vector(m.X - M.X, m.Y - M.Y), new Vector(P.X - M.X, P.Y - M.Y)) < Math.PI / 2)
return d > Dis ? Dis : d;
}
else
{
//求根公式
X1 = (-B11 + (float)Math.Sqrt(B11 * B11 - 4 * A11 * C11)) / (2 * A11);
X2 = (-B11 - (float)Math.Sqrt(B11 * B11 - 4 * A11 * C11)) / (2 * A11);
Y1 = _K * X1 + _B;
Y2 = _K * X2 + _B;
}
}
float Dis1 = Vector.Distance(new Vector(M.X, M.Y), new Vector(X1, Y1));
float Dis2 = Vector.Distance(new Vector(M.X, M.Y), new Vector(X2, Y2));
//如果两个点都在圆内
if (Vector.Distance(new Vector(P.X, P.Y), new Vector(C.X, C.Y)) < R &&
Vector.Distance(new Vector(M.X, M.Y), new Vector(C.X, C.Y)) < R)
{
return Dis;
}
else if (Vector.Distance(new Vector(P.X, P.Y), new Vector(C.X, C.Y)) < R &&
Vector.Distance(new Vector(M.X, M.Y), new Vector(C.X, C.Y)) > R)
{
//当另一点在圆内,起始点在圆外
return Dis1 > Dis2 ? Dis2 : Dis1;
}
else if (Vector.Distance(new Vector(P.X, P.Y), new Vector(C.X, C.Y)) > R &&
Vector.Distance(new Vector(M.X, M.Y), new Vector(C.X, C.Y)) < R)
{
//当起始点在圆内,另一点在圆外
Vector r1 = new Vector(X1 - M.X, Y1 - M.Y);
Vector r2 = new Vector(P.X - M.X, P.Y - M.Y);
if (Vector.AngleBetween(r1, r2) < Math.PI / 2)
{
//如果起始点到第一个交点的向量与起始点与另一点所构成的角度为锐角,则当前交点为最终交点
return Dis1;
}
else
{
return Dis2;
}
}
else if (Vector.Distance(new Vector(M.X, M.Y), new Vector(C.X, C.Y)) == R &&
Vector.Distance(new Vector(P.X, P.Y), new Vector(C.X, C.Y)) != R)
{
//如果起始原点在圆上,另一点不再圆上
Vector m = new Vector(P.X - M.X, P.Y - M.Y);
float ARC = m.GetAngle_in_RCS();
if (ARC >= 180 && ARC <= 360)
{
return Dis;
}
else
return 0.0f;
}
else if (Vector.Distance(new Vector(M.X, M.Y), new Vector(C.X, C.Y)) != R &&
Vector.Distance(new Vector(P.X, P.Y), new Vector(C.X, C.Y)) == R)
{
//如果另一原点在圆上,起始点不再圆上
Vector r1 = new Vector(X1 - M.X, Y1 - M.Y);
Vector r2 = new Vector(P.X - M.X, P.Y - M.Y);
if (Vector.AngleBetween(r1, r2) < Math.PI / 2)
{
//如果起始点到第一个交点的向量与起始点与另一点所构成的角度为锐角,则当前交点为最终交点
return Dis1;
}
else
{
return Dis2;
}
}
else if (Vector.Distance(new Vector(M.X, M.Y), new Vector(C.X, C.Y)) == R &&
Vector.Distance(new Vector(P.X, P.Y), new Vector(C.X, C.Y)) == R)
{
//如果两个点都在圆上,则距离为0
return 0.0f;
}
else
{
//如果两点都在圆外
//如果另一原点在圆上,起始点不再圆上
Vector r1 = new Vector(X1 - M.X, Y1 - M.Y);
Vector r2 = new Vector(P.X - M.X, P.Y - M.Y);
Vector r3 = new Vector(X2 - M.X, Y2 - M.Y);
float ARC1 = Vector.AngleBetween(r1, r2);
float ARC2 = Vector.AngleBetween(r3, r2);
if (ARC1 < Math.PI / 2 && ARC1 > -Math.PI / 2 && ARC2 > -Math.PI / 2 &&
ARC2 < Math.PI / 2)
{
//如果起始点到第一个交点的向量与起始点与另一点所构成的角度为锐角,则当前交点为最终交点
if (Dis1 > Dis2)
{
if (Dis2 > Dis)
{
return Dis;
}
else
{
return Dis2;
}
}
else
{
if (Dis1 > Dis)
{
return Dis;
}
else
{
return Dis1;
}
}
}
else
{
return Dis;
}
}
}
/// <summary>
/// 射线对线段检测
/// </summary>
/// <param name="M">起点坐标</param>
/// <param name="P">端点坐标</param>
/// <param name="V1">线段的一个坐标</param>
/// <param name="V2">线段另一个坐标</param>
private float ABenCountCD(Vector M, Vector P, Vector V1, Vector V2)
{
float Dis = Vector.Distance(M, P);
//情况一 斜率为0
if (P.Y == M.Y)
{
//如果另一条直线也是斜率为0,即两者相互平行
if (V1.Y == V2.Y)
{
return Dis; //直接返回当前长度
}
else if (V1.X == V2.X)
{
float X1 = V1.X;
float Y1 = M.Y;
float Dis1 = Vector.Distance(M, new Vector(X1, Y1));
return Dis1 > Dis ? Dis : Dis1;
}
else
{
//计算交点
float K = (V1.Y - V2.Y) / (V1.X - V2.X); //计算V1 V2斜率
float X = V1.X + (1 / K) * (M.Y - V1.Y); //x=x1+(1/k)(y-y1)
float Y = M.Y;
float d1 = Math.Abs(X - V1.X);
float d2 = Math.Abs(X - V2.X);
float d3 = Math.Abs(V1.X - V2.X);
//if(d1+d2==d3&&)
if (!(((X > M.X) && (X > P.X)) || ((X < M.X) && (X < P.X))) &&
(X != M.X) || (Y != M.Y)) //如果交点在左边或在右边,则没有交点
{
return Vector.Distance(M, new Vector(X, Y));
}
else
return Dis;
}
}
//情况二 斜率不存在
else if (P.X == M.X)
{
//如果另一条直线也是斜率不存在,即两者相互平行
if (V1.X == V2.X)
{
return Dis; //直接返回当前长度
}
else if (V1.Y == V2.Y)
{
float X1 = P.X;
float Y1 = V1.Y;
float Dis1 = Vector.Distance(M, new Vector(X1, Y1));
return Dis1 > Dis ? Dis : Dis1;
}
else
{
//计算交点
float K = (V1.Y - V2.Y) / (V1.X - V2.X);
float B = V1.Y - K * V1.X; //y=kx-kx1+y1
float X = M.X;
float Y = M.X * K + B;
if (!(((Y > M.Y) && (Y > P.Y)) || ((Y < M.Y) && (Y < P.Y))) &&
(X != M.X) || (Y != M.Y)) //如果交点在上边或在下边,则没有交点
{
return Vector.Distance(M, new Vector(X, Y));
}
else
return Dis;
}
}else{
float X;
float Y;
float K1 = (M.Y - P.Y) / (M.X - P.X);
float B1 = M.Y - K1 * M.X;
//如果另一条线段斜率不存在
if (V1.X == V2.X)
{
X = V1.X;
Y = K1 * X + B1;
}else if(V1.Y == V2.Y)
{
X = (1 / K1) * (V1.Y - B1);
Y = V1.Y;
}
else
{
float K2 = (V1.Y - V2.Y) / (V1.X - V2.X);
float B2 = V1.Y - K2 * V1.X; //y=kx-kx1+y1
//如果两个直线斜率相等,说明平行
if (K1 == K2)
return Dis;
X = (B2 - B1) / (K1 - K2);
Y = K1 * X + B1;
}
Vector G = new Vector(X, Y);
float Dis1 = Vector.Distance(M, G);
Vector v1 = new Vector(P.X - M.X, P.Y - M.Y);
Vector v2 = new Vector(X - M.X, Y - M.Y);
float arc = Vector.AngleBetween(v1, v2);
if (arc > Math.PI / 2 && G != M) //如果交点与与射线方向相反
{
return Dis;
}
else if (arc >= 0 && arc < Math.PI / 2 && G != M) //如果交点与与射线方向相同
{
///如果交点在四边形内
if (!(((X > M.X) && (X > P.X)) || ((X < M.X) && (X < P.X))) &&
!(((Y > M.Y) && (Y > P.Y)) || ((Y < M.Y) && (Y < P.Y))) &&
!(((X > V1.X) && (X > V2.X)) || ((X < V1.X) && (X < V2.X))) &&
!(((Y > V1.Y) && (Y > V2.Y)) || ((Y < V1.Y) && (Y < V2.Y))))
{
return Dis1;
}
else
return Dis;
}
else
{
return Dis;
}
}
}
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace WindowsFormsApp3
{
class Vector
{
//向量的X,Y值
private float _X;
private float _Y;
public float X { get => _X; set => _X = value; }
public float Y { get => _Y; set => _Y = value; }
/// <summary>
/// 向量的模
/// </summary>
public float Magnitude { get => (float)Math.Sqrt(_X * _X + _Y * _Y); }
/// <summary>
/// 单位方向向量
/// </summary>
public Vector UnitVector { get => new Vector(_X / (float)Math.Sqrt(_X * _X + _Y * _Y), _Y / (float)Math.Sqrt(_X * _X + _Y * _Y)); }
/// <summary>
/// 通过X,Y创建向量
/// </summary>
/// <param name="X"></param>
/// <param name="Y"></param>
public Vector(float X, float Y)
{
_Y = Y;
_X = X;
}
/// <summary>
/// 创建零向量
/// </summary>
public Vector()
{
_X = 0f;
_Y = 0f;
}
/// <summary>
/// 求向量
/// </summary>
/// <param name="v1"></param>
/// <param name="v2"></param>
/// <returns></returns>
public static Vector operator -(Vector v1, Vector v2)
{
Vector d = new Vector(v1.X - v2.X, v1.Y - v2.Y);
return d;
}
/// <summary>
/// 向量相加
/// </summary>
/// <param name="v1"></param>
/// <param name="v2"></param>
/// <returns></returns>
public static Vector operator +(Vector v1, Vector v2)
{
Vector d = new Vector(v1.X + v2.X, v1.Y + v2.Y);
return d;
}
/// <summary>
/// 向量乘以一个数
/// </summary>
/// <param name="v"></param>
/// <param name="k"></param>
/// <returns></returns>
public static Vector operator *(Vector v, float k)
{
Vector d = new Vector(k * v.X, k * v.Y);
return d;
}
/// <summary>
/// 向量乘以一个数
/// </summary>
/// <param name="k"></param>
/// <param name="v"></param>
/// <returns></returns>
public static Vector operator *(float k, Vector v)
{
Vector d = new Vector(k * v.X, k * v.Y);
return d;
}
/// <summary>
/// 向量除以一个数
/// </summary>
/// <param name="v"></param>
/// <param name="k"></param>
/// <returns></returns>
public static Vector operator /(Vector v, float k)
{
Vector d = new Vector(v.X / k, v.Y / k);
d.X = v.X / k;
d.Y = v.Y / k;
return d;
}
/// <summary>
/// 两向量所在点的距离
/// </summary>
/// <param name="v1"></param>
/// <param name="v2"></param>
/// <returns></returns>
public static float Distance(Vector v1, Vector v2)
{
return (float)Math.Sqrt(Math.Pow((v1.X - v2.X), 2) + Math.Pow((v1.Y - v2.Y), 2));
}
/// <summary>
/// 两点之间的距离
/// </summary>
/// <param name="x1"></param>
/// <param name="y1"></param>
/// <param name="x2"></param>
/// <param name="y2"></param>
/// <returns></returns>
public static float PDistance(float x1, float y1, float x2, float y2)
{
return (float)Math.Sqrt(Math.Pow((x2 - x1), 2) + Math.Pow((y2 - y1), 2));
}
public static float Dot(Vector V1, Vector V2)
{
return V1.X * V2.X + V1.Y * V2.Y;
}
/// <summary>
/// AngleBetween - the angle between 2 vectors
/// </summary>
/// <returns>
/// Returns the the angle in degrees between vector1 and vector2
/// </returns>
/// <param name="vector1"> The first Vector </param>
/// <param name="vector2"> The second Vector </param>
public static float AngleBetween(Vector vector1, Vector vector2)
{
float sin = vector1.X * vector2.Y - vector2.X * vector1.Y;
float cos = vector1.X * vector2.X + vector1.Y * vector2.Y;
return (float)Math.Abs(Math.Atan2(sin, cos));
}
/// <summary>
/// 获得向量在直角坐标系的角度
/// </summary>
/// <param name="V1"></param>
/// <returns></returns>
public float GetAngle_in_RCS()
{
float VP = 0;
if (X > 0f && Y >= 0f)
{
float DA = 1 * X;
float DL = new Vector(0, 1).Magnitude;
float GS = Magnitude;
float An = (float)Math.Acos(DA / (DL * GS));
VP = An / (float)Math.PI * 180.0f;
}
else if (X <= 0f && Y > 0f)
{
float DA = 1 * Y;
float DL = new Vector(0, 1).Magnitude;
float GS = Magnitude;
float An = (float)Math.Acos(DA / (DL * GS));
VP = 90.0f + An / (float)Math.PI * 180.0f;
}
else if (X < 0f && Y <= 0f)
{
float DA = -1 * X;
float DL = new Vector(-1, 0).Magnitude;
float GS = Magnitude;
float An = (float)Math.Acos(DA / (DL * GS));
VP = 180.0f + An / (float)Math.PI * 180.0f;
}
else if (X >= 0f && Y < 0f)
{
float DA = -1 * Y;
float DL = new Vector(0, -1).Magnitude;
float GS = Magnitude;
float An = (float)Math.Acos(DA / (DL * GS));
VP = 270.0f + An / (float)Math.PI * 180.0f;
}
else if (X == 0f && Y == 0f)
{
VP = 0.0f;
}
return VP;
}
}
}