public static bool Line2dWithOrientedRectangle2d(Line2d line, OrientedRectangle2d rectangle2d)
{
Vector3L forward = RotateHelper.GetForward(rectangle2d.m_rotation);
Vector3L right = Vector3L.Cross(Vector3L.up, forward);
Vector3L pos = new Vector3L(rectangle2d.m_pos.x, 0, rectangle2d.m_pos.y);
Vector3L a = pos + forward * rectangle2d.m_length * 0.5f + -right * rectangle2d.m_width * 0.5f;
Vector3L b = pos + forward * rectangle2d.m_length * 0.5f + right * rectangle2d.m_width * 0.5f;
Vector3L c = pos + -forward * rectangle2d.m_length * 0.5f + right * rectangle2d.m_width * 0.5f;
Vector3L d = pos + -forward * rectangle2d.m_length * 0.5f + -right * rectangle2d.m_width * 0.5f;
List <Vector2L> lineList = new List <Vector2L>();
lineList.Add(new Vector2L(a.x, a.z));
lineList.Add(new Vector2L(b.x, b.z));
lineList.Add(new Vector2L(c.x, c.z));
lineList.Add(new Vector2L(d.x, d.z));
for (int i = 0; i < lineList.Count; ++i)
{
if (IsTwoSegmentIntersection(lineList[i], lineList[(i + 1) % lineList.Count], line.m_point1, line.m_point2))
{
return(true);
}
}
return(false);
}
public static bool Circle2dWithOrientedRectangle2d(Circle2d circle, OrientedRectangle2d rectangle)
{
Vector3L forward = RotateHelper.GetForward(rectangle.m_rotation);
Vector3L right = Vector3L.Cross(Vector3L.up, forward);
Vector3L pos = new Vector3L(rectangle.m_pos.x, 0, rectangle.m_pos.y);
Vector3L a = pos + forward * rectangle.m_length * 0.5f + -right * rectangle.m_width * 0.5f;
Vector3L b = pos + forward * rectangle.m_length * 0.5f + right * rectangle.m_width * 0.5f;
Vector3L c = pos + -forward * rectangle.m_length * 0.5f + right * rectangle.m_width * 0.5f;
Vector3L d = pos + -forward * rectangle.m_length * 0.5f + -right * rectangle.m_width * 0.5f;
List <Vector2L> lineList = new List <Vector2L>();
lineList.Add(new Vector2L(a.x, a.z));
lineList.Add(new Vector2L(b.x, b.z));
lineList.Add(new Vector2L(c.x, c.z));
lineList.Add(new Vector2L(d.x, d.z));
for (int i = 0; i < lineList.Count; ++i)
{
Segment2d segment = new Segment2d();
segment.m_point1 = lineList[i];
segment.m_point2 = lineList[(i + 1) % lineList.Count];
if (Segment2dWithCircle2d(segment, circle))
{
return(true);
}
}
return(false);
}
/// <summary>
/// 注意abcd为绕序
/// </summary>
/// <param name="p"></param>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="c"></param>
/// <param name="d"></param>
/// <returns></returns>
static bool IsInRectangle2d(Vector3L p, Vector3L a, Vector3L b, Vector3L c, Vector3L d)
{
Vector3L v11 = p - a;
Vector3L v12 = b - a;
Vector3L v21 = p - b;
Vector3L v22 = c - b;
Vector3L v31 = p - c;
Vector3L v32 = d - c;
Vector3L v41 = p - d;
Vector3L v42 = a - d;
Vector3L cross1 = Vector3L.Cross(v11, v12);
Vector3L cross2 = Vector3L.Cross(v21, v22);
Vector3L cross3 = Vector3L.Cross(v31, v32);
Vector3L cross4 = Vector3L.Cross(v41, v42);
if (Vector3L.Dot(cross1, cross2) > 0 &&
Vector3L.Dot(cross2, cross3) > 0 &&
Vector3L.Dot(cross3, cross4) > 0)
{
return(true);
}
else
{
return(false);
}
}
/// <summary>
/// 判断两条线段是否相交
/// 现在有线段AB和线段CB
//用线段AB的方向和C，D两点分别做差乘比较。如果C,D在同侧则return跳出
//用线段CD的方向和A，B两点分别做差乘比较。如果A,B在同侧则return跳出
//最终返回相交
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="c"></param>
/// <param name="d"></param>
/// <returns></returns>
static bool IsTwoSegmentIntersection(Vector2L a2d, Vector2L b2d, Vector2L c2d, Vector2L d2d)
{
Vector3L a = new Vector3L(a2d.x, 0, a2d.y);
Vector3L b = new Vector3L(b2d.x, 0, b2d.y);
Vector3L c = new Vector3L(c2d.x, 0, c2d.y);
Vector3L d = new Vector3L(d2d.x, 0, d2d.y);
var crossA = FixPointMath.Sign(Vector3L.Cross(d - c, a - c).y);
var crossB = FixPointMath.Sign(Vector3L.Cross(d - c, b - c).y);
if (FixPointMath.Approximately(crossA, crossB))
{
return(false);
}
var crossC = FixPointMath.Sign(Vector3L.Cross(b - a, c - a).y);
var crossD = FixPointMath.Sign(Vector3L.Cross(b - a, d - a).y);
if (FixPointMath.Approximately(crossC, crossD))
{
return(false);
}
return(true);
}
public static QuaternionL LookRotation(Vector3L forward, Vector3L up)
{
forward = Vector3L.Normalize(forward);
Vector3L right = Vector3L.Normalize(Vector3L.Cross(up, forward));
up = Vector3L.Cross(forward, right);
FloatL m00 = right.x;
FloatL m01 = right.y;
FloatL m02 = right.z;
FloatL m10 = up.x;
FloatL m11 = up.y;
FloatL m12 = up.z;
FloatL m20 = forward.x;
FloatL m21 = forward.y;
FloatL m22 = forward.z;
FloatL num8 = (m00 + m11) + m22;
QuaternionL quaternion = new QuaternionL();
if (num8 > 0f)
{
FloatL num = FixPointMath.Sqrt(num8 + 1f);
quaternion.w = num * 0.5f;
num = 0.5f / num;
quaternion.x = (m12 - m21) * num;
quaternion.y = (m20 - m02) * num;
quaternion.z = (m01 - m10) * num;
return(quaternion);
}
if ((m00 >= m11) && (m00 >= m22))
{
FloatL num7 = FixPointMath.Sqrt(((1f + m00) - m11) - m22);
FloatL num4 = 0.5f / num7;
quaternion.x = 0.5f * num7;
quaternion.y = (m01 + m10) * num4;
quaternion.z = (m02 + m20) * num4;
quaternion.w = (m12 - m21) * num4;
return(quaternion);
}
if (m11 > m22)
{
FloatL num6 = FixPointMath.Sqrt(((1f + m11) - m00) - m22);
FloatL num3 = 0.5f / num6;
quaternion.x = (m10 + m01) * num3;
quaternion.y = 0.5f * num6;
quaternion.z = (m21 + m12) * num3;
quaternion.w = (m20 - m02) * num3;
return(quaternion);
}
FloatL num5 = FixPointMath.Sqrt(((1f + m22) - m00) - m11);
FloatL num2 = 0.5f / num5;
quaternion.x = (m20 + m02) * num2;
quaternion.y = (m21 + m12) * num2;
quaternion.z = 0.5f * num5;
quaternion.w = (m01 - m10) * num2;
return(quaternion);
}
public static bool Point2dWithRectangle2d(Vector2L point2d, OrientedRectangle2d rectangle2d)
{
Vector3L forward = RotateHelper.GetForward(rectangle2d.m_rotation);
Vector3L right = Vector3L.Cross(Vector3L.up, forward);
Vector3L pos = new Vector3L(rectangle2d.m_pos.x, 0, rectangle2d.m_pos.y);
Vector3L a = pos + forward * rectangle2d.m_length * 0.5f + -right * rectangle2d.m_width * 0.5f;
Vector3L b = pos + forward * rectangle2d.m_length * 0.5f + right * rectangle2d.m_width * 0.5f;
Vector3L c = pos + -forward * rectangle2d.m_length * 0.5f + right * rectangle2d.m_width * 0.5f;
Vector3L d = pos + -forward * rectangle2d.m_length * 0.5f + -right * rectangle2d.m_width * 0.5f;
return(IsInRectangle2d(a, b, c, d, new Vector3L(point2d.x, 0, point2d.y)));
}
static Vector3L RotateTo(Vector3L from, Vector3L to, FloatL angle)
{
//如果两向量角度为0
if (Vector3L.Angle(from, to) == 0)
{
return(from);
}
//旋转轴
Vector3L n = Vector3L.Cross(from, to);
//旋转轴规范化
n.Normalize();
//旋转矩阵
Matrix4x4L rotateMatrix = new Matrix4x4L();
//旋转的弧度
double radian = (angle * FixPointMath.PI / 180).ToDouble();
FloatL cosAngle = FixPointMath.Cos(radian);
FloatL sinAngle = FixPointMath.Sin(radian);
//矩阵的数据
//这里看不懂的自行科普矩阵知识
rotateMatrix.SetRow(0, new Vector4L(n.x * n.x * (1 - cosAngle) + cosAngle, n.x * n.y * (1 - cosAngle) + n.z * sinAngle, n.x * n.z * (1 - cosAngle) - n.y * sinAngle, 0));
rotateMatrix.SetRow(1, new Vector4L(n.x * n.y * (1 - cosAngle) - n.z * sinAngle, n.y * n.y * (1 - cosAngle) + cosAngle, n.y * n.z * (1 - cosAngle) + n.x * sinAngle, 0));
rotateMatrix.SetRow(2, new Vector4L(n.x * n.z * (1 - cosAngle) + n.y * sinAngle, n.y * n.z * (1 - cosAngle) - n.x * sinAngle, n.z * n.z * (1 - cosAngle) + cosAngle, 0));
rotateMatrix.SetRow(3, new Vector4L(0, 0, 0, 1));
Vector4L v = Vector3L.ToVector4(from);
Vector3L vector = new Vector3L();
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; j++)
{
vector[i] += v[j] * rotateMatrix[j, i];
}
}
return(vector);
}
public static QuaternionL FromToRotation(Vector3L v1, Vector3L v2)
{
FloatL angle = Vector3L.Angle(v1, v2);
return(QuaternionL.AngleAxis(angle, Vector3L.Cross(v1, v2)));
}
public static Plane3d FromTriangle(Triangle3d t)
{
Plane3d result = new Plane3d(Vector3L.Cross(t.m_point1 - t.m_point0, t.m_point2 - t.m_point0), t.m_point0);
return(result);
}
