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