QuaternionL(Vector3L v, FloatL w)
{
this.x = v.x;
this.y = v.y;
this.z = v.z;
this.w = w;
}
// 欧拉角转四元数
static QuaternionL FromEulerRad(Vector3L euler)
{
FloatL yaw = euler.z;
FloatL pitch = euler.x;
FloatL roll = euler.y;
FloatL yawOver2 = yaw * 0.5f;
FloatL sinYawOver2 = FixPointMath.Sin(yawOver2);
FloatL cosYawOver2 = FixPointMath.Cos(yawOver2);
FloatL pitchOver2 = pitch * 0.5f;
FloatL sinPitchOver2 = FixPointMath.Sin(pitchOver2);
FloatL cosPitchOver2 = FixPointMath.Cos(pitchOver2);
FloatL rollOver2 = roll * 0.5f;
FloatL sinRollOver2 = FixPointMath.Sin(rollOver2);
FloatL cosRollOver2 = FixPointMath.Cos(rollOver2);
QuaternionL result;
result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
result.x = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
result.y = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
result.z = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
return(result);
}
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);
}
static Vector3L NormalizeAngles(Vector3L angles)
{
angles.x = NormalizeAngle(angles.x);
angles.y = NormalizeAngle(angles.y);
angles.z = NormalizeAngle(angles.z);
return(angles);
}
public Vector3L ExtremePoint(Vector3L direction, ref FloatL projectionDistance)
{
Vector3L extremePoint = ExtremePoint(direction);
projectionDistance = Vector3L.Dot(extremePoint, direction);
return(extremePoint);
}
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 static Matrix4x4L TRS(Vector3L pos, QuaternionL q, Vector3L s)
{
Matrix4x4L posMatrix = Matrix4x4L.identity;
posMatrix.m03 = pos.x;
posMatrix.m13 = pos.y;
posMatrix.m23 = pos.z;
Matrix4x4L rotateMatrix = Matrix4x4L.identity;
rotateMatrix.m00 = 1 - 2 * q.y * q.y - 2 * q.z * q.z;
rotateMatrix.m10 = 2 * q.x * q.y + 2 * q.w * q.z;
rotateMatrix.m20 = 2 * q.x * q.z - 2 * q.w * q.y;
rotateMatrix.m01 = 2 * q.x * q.y - 2 * q.w * q.z;
rotateMatrix.m11 = 1 - 2 * q.x * q.x - 2 * q.z * q.z;
rotateMatrix.m21 = 2 * q.y * q.z + 2 * q.w * q.x;
rotateMatrix.m02 = 2 * q.x * q.z + 2 * q.w * q.y;
rotateMatrix.m12 = 2 * q.y * q.z - 2 * q.w * q.x;
rotateMatrix.m22 = 1 - 2 * q.x * q.x - 2 * q.y * q.y;
Matrix4x4L scaleMatrix = Scale(s);
Matrix4x4L ret = posMatrix * rotateMatrix * scaleMatrix;
return(ret);
}
public static Vector3L ClosestPointOfPoint3dWithTriangle3d(Triangle3d t, Vector3L p)
{
Plane3d plane = FromTriangle(t);
Vector3L closest = ClosestPointOfPoint3dWithPlane3d(p, plane);
// Closest point was inside triangle
if (IntersectionTest3D.Point3dWithTriangle(closest, t))
{
return(closest);
}
Vector3L c1 = ClosestPointOfPoint3dWithSegment3d(closest, new Segment3d(t.m_point0, t.m_point1)); // Line AB
Vector3L c2 = ClosestPointOfPoint3dWithSegment3d(closest, new Segment3d(t.m_point1, t.m_point2)); // Line BC
Vector3L c3 = ClosestPointOfPoint3dWithSegment3d(closest, new Segment3d(t.m_point2, t.m_point0)); // Line CA
FloatL magSq1 = (closest - c1).sqrMagnitude;
FloatL magSq2 = (closest - c2).sqrMagnitude;
FloatL magSq3 = (closest - c3).sqrMagnitude;
if (magSq1 < magSq2 && magSq1 < magSq3)
{
return(c1);
}
else if (magSq2 < magSq1 && magSq2 < magSq3)
{
return(c2);
}
return(c3);
}
public static void Test()
{
Debug.Log("IsLittleEndian " + BitConverter.IsLittleEndian);
Debug.Log(FixPointMathTest.ConvertFloatToBinStr(1f));
Debug.Log(FixPointMathTest.ConvertFloatToBinStr(17.625f));
Debug.Log(FixPointMath.SqrtOld(new FloatL(0.955d)));
Debug.Log(Mathf.Sqrt(0.955f));
Debug.Log(FixPointMath.Sqrt(new FloatL(0.955d)));
Vector3L dir = new Vector3L(-0.093d, -0.093d, 0.988d);
FloatL angle1 = Vector3L_Angle(Vector3L.forward, dir);
float angle2 = Vector3_Angle(Vector3.forward, dir.Convert());
Debug.Log("angle1 " + angle1 + " angle2 " + angle2);
Debug.Log("sin 10 " + Mathf.Sin(10 * Mathf.Deg2Rad));
Debug.Log("sinL 10 " + FixPointMath.Sin(10 * Mathf.Deg2Rad));
Debug.Log("cos 10 " + Mathf.Cos(10 * Mathf.Deg2Rad));
Debug.Log("cosL 10 " + FixPointMath.Cos(10 * Mathf.Deg2Rad));
Debug.Log("acos 0.1 " + Mathf.Acos(0.1f));
Debug.Log("acosL 0.1 " + FixPointMath.Acos(0.1f));
Debug.Log("atan2 3/2 " + Mathf.Atan2(3, 2));
Debug.Log("atan2L 3/2 " + FixPointMath.Atan2(3, 2));
Debug.Log("asin 0.6 " + Mathf.Asin(0.6f));
Debug.Log("asinL 0.6 " + FixPointMath.Asin(0.6f));
}
/// <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 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);
}
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 Vector3L ClampMagnitude(Vector3L vector, FloatL maxLength)
{
if (vector.sqrMagnitude > maxLength * maxLength)
{
return(vector.normalized * maxLength);
}
return(vector);
}
public Vector3L MultiplyVector(Vector3L v)
{
Vector3L result;
result.x = this.m00 * v.x + this.m01 * v.y + this.m02 * v.z;
result.y = this.m10 * v.x + this.m11 * v.y + this.m12 * v.z;
result.z = this.m20 * v.x + this.m21 * v.y + this.m22 * v.z;
return(result);
}
public static Vector3L operator +(Vector3L a, Vector3L b)
{
Vector3L ret = new Vector3L();
ret.x = a.x + b.x;
ret.y = a.y + b.y;
ret.z = a.z + b.z;
return(ret);
}
public static Vector3L operator *(Vector3L a, FloatL d)
{
Vector3L ret = new Vector3L();
ret.x = a.x * d;
ret.y = a.y * d;
ret.z = a.z * d;
return(ret);
}
public static Vector3L operator /(Vector3L a, FloatL d)
{
Vector3L ret = new Vector3L();
ret.x = a.x / d;
ret.y = a.y / d;
ret.z = a.z / d;
return(ret);
}
public static Vector3L operator -(Vector3L a)
{
Vector3L ret = new Vector3L();
ret.x = -a.x;
ret.y = -a.y;
ret.z = -a.z;
return(ret);
}
/// <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 bool Segment2dWithCircle2d(Segment2d segment, Circle2d circle)
{
Segment3d segment3d = new Segment3d(
new Vector3L(segment.m_point1.x, 0, segment.m_point1.y),
new Vector3L(segment.m_point2.x, 0, segment.m_point2.y));
Vector3L closestPoint = Distance3d.ClosestPointOfPoint3dWithSegment3d(new Vector3L(circle.m_pos.x, 0, circle.m_pos.y), segment3d);
Vector2L distance = circle.m_pos - new Vector2L(closestPoint.x, closestPoint.z);
return(distance.magnitude <= circle.m_radius);
}
public OBB3d(Vector3L pos, QuaternionL qua, Vector3L size)
{
m_pos = pos;
m_rotation = qua;
//m_xLength = size.x;
//m_yLength = size.y;
//m_zLength = size.z;
m_size = size;
}
public override bool Equals(object other)
{
if (!(other is Vector3L))
{
return(false);
}
Vector3L vector = (Vector3L)other;
return(this.x.Equals(vector.x) && this.y.Equals(vector.y) && this.z.Equals(vector.z));
}
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 Vector3L Normalize(Vector3L value)
{
FloatL num = Vector3L.Magnitude(value);
if (num > FloatL.Epsilon)
{
return(value / num);
}
return(Vector3L.zero);
}
public OBB3d(Vector3L pos, QuaternionL qua, FloatL xLength, FloatL yLength, FloatL zLength)
{
m_pos = pos;
m_rotation = qua;
//m_xLength = xLength;
//m_yLength = yLength;
//m_zLength = zLength;
m_size.x = xLength;
m_size.y = yLength;
m_size.z = zLength;
}
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 MoveTowards(Vector3L current, Vector3L target, FloatL maxDistanceDelta)
{
Vector3L a = target - current;
FloatL magnitude = a.magnitude;
if (magnitude <= maxDistanceDelta || magnitude == 0f)
{
return(target);
}
return(current + a / magnitude * maxDistanceDelta);
}
public Vector3L ExtremePoint(Vector3L direction)
{
Vector3L maxPoint = GetMax();
Vector3L minPoint = GetMin();
return(new Vector3L(
direction.x >= 0 ? maxPoint.x : minPoint.x,
direction.y >= 0 ? maxPoint.y : minPoint.y,
direction.z >= 0 ? maxPoint.z : minPoint.z
));
}
