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