/// <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); } }
public static Vector3L ClosestPointOfPoint3dWithPlane3d(Vector3L point, Plane3d plane) { FloatL dot = Vector3L.Dot(plane.m_planeNormal, point); FloatL distance = dot - plane.GetDistanceFromOrigin(); return(point - plane.m_planeNormal * distance); }
public static Vector3L ClosestPointOfPoint3dWithLine3d(Vector3L point, Line3d line) { Vector3L lVec = line.m_point2 - line.m_point1; // Line Vector FloatL t = Vector3L.Dot(point - line.m_point1, lVec) / Vector3L.Dot(lVec, lVec); return(line.m_point1 + lVec * t); }
public Vector3L ExtremePoint(Vector3L direction, ref FloatL projectionDistance) { Vector3L extremePoint = ExtremePoint(direction); projectionDistance = Vector3L.Dot(extremePoint, direction); return(extremePoint); }
/// <summary> /// 3d空间中点到直线距离 /// </summary> /// <param name="a"></param> /// <param name="b"></param> /// <param name="s"></param> /// <returns></returns> // public static FloatL DistanceOfPoint3dWithLine3d(Line3d line3d, Vector3L s) // { // FloatL ab = FixPointMath.Sqrt(FixPointMath.Pow((line3d.m_point1.x - line3d.m_point2.x), 2.0f) + FixPointMath.Pow((line3d.m_point1.y - line3d.m_point2.y), 2.0f) + FixPointMath.Pow((line3d.m_point1.z - line3d.m_point2.z), 2.0f)); // FloatL as2 = FixPointMath.Sqrt(FixPointMath.Pow((line3d.m_point1.x - s.x), 2.0f) + FixPointMath.Pow((line3d.m_point1.y - s.y), 2.0f) + FixPointMath.Pow((line3d.m_point1.z - s.z), 2.0f)); // FloatL bs = FixPointMath.Sqrt(FixPointMath.Pow((s.x - line3d.m_point2.x), 2.0f) + FixPointMath.Pow((s.y - line3d.m_point2.y), 2.0f) + FixPointMath.Pow((s.z - line3d.m_point2.z), 2.0f)); // FloatL cos_A = (FixPointMath.Pow(as2, 2.0f) + FixPointMath.Pow(ab, 2.0f) - FixPointMath.Pow(bs, 2.0f)) / (2 * ab * as2); // FloatL sin_A = FixPointMath.Sqrt(1 - FixPointMath.Pow(cos_A, 2.0f)); // return as2 * sin_A; // } public static Vector3L ClosestPointOfPoint3dWithSegment3d(Vector3L point, Segment3d line) { Vector3L lVec = line.m_point2 - line.m_point1; // Line Vector FloatL t = Vector3L.Dot(point - line.m_point1, lVec) / Vector3L.Dot(lVec, lVec); t = FixPointMath.Max(t, 0.0f); // Clamp to 0 t = FixPointMath.Min(t, 1.0f); // Clamp to 1 return(line.m_point1 + lVec * t); }
public static Vector3L Project(Vector3L vector, Vector3L onNormal) { FloatL num = Vector3L.Dot(onNormal, onNormal); if (num < FloatL.Epsilon) { return(Vector3L.zero); } return(onNormal * Vector3L.Dot(vector, onNormal) / num); }
public static FloatL Vector3L_Angle(Vector3L from, Vector3L to) { Debug.Log("x " + to.normalized.x + " y " + to.normalized.y + " z " + to.normalized.z); FloatL dot = Vector3L.Dot(from.normalized, to.normalized); Debug.Log("dotL " + dot); FloatL acos = FixPointMath.Acos(FixPointMath.Clamp(dot, -1f, 1f)); Debug.Log("acosL " + acos); return(acos * 57.29578d); }
public static Vector3L ClosestPointOfPoint3dWithRay3d(Vector3L point, Ray3d ray) { FloatL t = Vector3L.Dot(point - ray.m_rayOrigin, ray.m_rayDir); // We assume the direction of the ray is normalized // If for some reason the direction is not normalized // the below division is needed. So long as the ray // direction is normalized, we don't need this divide // t /= Dot(ray.direction, ray.direction); t = FixPointMath.Max(t, 0.0f); return(ray.m_rayOrigin + ray.m_rayDir * t); }
public static bool Point2dWithSector2d(Vector2L pos, Sector2d sector2d) { Vector2L distance = pos - sector2d.m_pos; if (distance.magnitude > sector2d.m_radius) { return(false); } Vector3L sectorForward = RotateHelper.GetForward(sector2d.m_rotation); FloatL cosTarget = Vector3L.Dot(sectorForward, distance) / sectorForward.magnitude / distance.magnitude; FloatL cosHalfDegree = FixPointMath.Cos((FixPointMath.Deg2Rad * sector2d.m_theraDegree / 2)); return(cosTarget > cosHalfDegree); }
public static FloatL PlaneEquation(Vector3L point, Plane3d plane) { return(Vector3L.Dot(point, plane.m_planeNormal) - plane.GetDistanceFromOrigin()); }
public FloatL GetDistanceFromOrigin() { return(Vector3L.Dot(m_planeOnePoint, m_planeNormal)); }
public static Vector3L Reflect(Vector3L inDirection, Vector3L inNormal) { return(-2f * Vector3L.Dot(inNormal, inDirection) * inNormal + inDirection); }
public static FloatL Angle(Vector3L from, Vector3L to) { return(FixPointMath.Acos(FixPointMath.Clamp(Vector3L.Dot(from.normalized, to.normalized), -1f, 1f)) * 57.29578d); }
private static QuaternionL SlerpUnclamped(QuaternionL a, QuaternionL b, FloatL t) { // if either input is zero, return the other. if (a.LengthSquared == 0.0f) { if (b.LengthSquared == 0.0f) { return(identity); } return(b); } else if (b.LengthSquared == 0.0f) { return(a); } FloatL cosHalfAngle = a.w * b.w + Vector3L.Dot(a.xyz, b.xyz); if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f) { // angle = 0.0f, so just return one input. return(a); } else if (cosHalfAngle < 0.0f) { b.xyz = -b.xyz; b.w = -b.w; cosHalfAngle = -cosHalfAngle; } FloatL blendA; FloatL blendB; if (cosHalfAngle < 0.99f) { // do proper slerp for big angles FloatL halfAngle = FixPointMath.Acos(cosHalfAngle); FloatL sinHalfAngle = FixPointMath.Sin(halfAngle); FloatL oneOverSinHalfAngle = 1.0f / sinHalfAngle; blendA = FixPointMath.Sin(halfAngle * (1.0f - t)) * oneOverSinHalfAngle; blendB = FixPointMath.Sin(halfAngle * t) * oneOverSinHalfAngle; } else { // do lerp if angle is really small. blendA = 1.0f - t; blendB = t; } QuaternionL result = new QuaternionL(blendA * a.xyz + blendB * b.xyz, blendA * a.w + blendB * b.w); if (result.LengthSquared > 0.0f) { return(Normalize(result)); } else { return(identity); } }
public Vector3L ExtremePoint(Vector3L direction) { FloatL len = direction.magnitude; return(Vector3L.Dot(direction, m_point2 - m_point1) >= 0 ? m_point2 : m_point1 + direction * (m_radius / len)); }