// 欧拉角转四元数
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 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));
}
// 四元数转欧拉角
static Vector3L ToEulerRad(QuaternionL rotation)
{
FloatL sqw = rotation.w * rotation.w;
FloatL sqx = rotation.x * rotation.x;
FloatL sqy = rotation.y * rotation.y;
FloatL sqz = rotation.z * rotation.z;
FloatL unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
FloatL test = rotation.x * rotation.w - rotation.y * rotation.z;
Vector3L v;
if (test > 0.4995d * unit)
{ // singularity at north pole
v.y = 2f * FixPointMath.Atan2(rotation.y, rotation.x);
v.x = FixPointMath.PI / 2;
v.z = 0;
return(NormalizeAngles(v * FixPointMath.Rad2Deg));
}
if (test < -0.4995d * unit)
{ // singularity at south pole
v.y = -2f * FixPointMath.Atan2(rotation.y, rotation.x);
v.x = -FixPointMath.PI / 2;
v.z = 0;
return(NormalizeAngles(v * FixPointMath.Rad2Deg));
}
QuaternionL q = new QuaternionL(rotation.w, rotation.z, rotation.x, rotation.y);
v.y = FixPointMath.Atan2(2f * q.x * q.w + 2f * q.y * q.z, 1 - 2f * (q.z * q.z + q.w * q.w)); // Yaw
v.x = FixPointMath.Asin(2f * (q.x * q.z - q.w * q.y)); // Pitch
v.z = FixPointMath.Atan2(2f * q.x * q.y + 2f * q.z * q.w, 1 - 2f * (q.y * q.y + q.z * q.z)); // Roll
return(NormalizeAngles(v * FixPointMath.Rad2Deg) * FixPointMath.Deg2Rad);
}
// public static FloatL SinOld(FloatL f)
// {
// return Math.Sin(f.ToDouble());
// }
/// <summary>
/// 牛顿法求平方根
/// </summary>
/// <param name="c"></param>
/// <returns></returns>
public static FloatL SqrtOld(FloatL c)
{
if (c == 0)
{
return(0);
}
if (c < new FloatL(0))
{
return(new FloatL(-1));
}
FloatL err = FloatL.Epsilon;
FloatL t = c;
int count = 0;
while (FixPointMath.Abs(t - c / t) > err * t)
{
count++;
t = (c / t + t) / new FloatL(2.0f);
if (count >= 20)
{
UnityEngine.Debug.LogWarning("FixPoint Sqrt " + c + " current sqrt " + t);
break;
}
}
return(t);
}
/// <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 FloatL Acos(FloatL f)
{
//return Math.Acos(f.ToDouble());
int num = (int)FixPointMath.Divide(f.m_numerator * (long)AcosLookupTable.HALF_COUNT, FloatL.m_denominator) + AcosLookupTable.HALF_COUNT;
num = FixPointMath.Clamp(num, 0, AcosLookupTable.COUNT);
return(new FloatL((long)AcosLookupTable.table[num]) / new FloatL(10000));
}
public static FloatL MoveTowards(FloatL current, FloatL target, FloatL maxDelta)
{
if (FixPointMath.Abs(target - current) <= maxDelta)
{
return(target);
}
return(current + FixPointMath.Sign(target - current) * maxDelta);
}
/// <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 FloatL LerpAngle(FloatL a, FloatL b, FloatL t)
{
FloatL num = FixPointMath.Repeat(b - a, 360f);
if (num > 180f)
{
num -= 360f;
}
return(a + num * FixPointMath.Clamp01(t));
}
public static FloatL DeltaAngle(FloatL current, FloatL target)
{
FloatL num = FixPointMath.Repeat(target - current, 360f);
if (num > 180f)
{
num -= 360f;
}
return(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 AABB3d Merge(AABB3d other)
{
Vector3L selfMin = GetMin();
Vector3L otherMin = other.GetMin();
Vector3L selfMax = GetMax();
Vector3L otherMax = GetMax();
Vector3L totalMin = new Vector3L(FixPointMath.Min(selfMin.x, otherMin.x), FixPointMath.Min(selfMin.y, otherMin.y), FixPointMath.Min(selfMin.z, otherMin.z));
Vector3L totalMax = new Vector3L(FixPointMath.Max(selfMax.x, otherMax.x), FixPointMath.Max(selfMax.y, otherMax.y), FixPointMath.Max(selfMax.z, otherMax.z));
return(new AABB3d((totalMin + totalMax) / 2, totalMax - totalMin));
}
public static QuaternionL RotateTowards(QuaternionL from, QuaternionL to, FloatL maxDegreesDelta)
{
FloatL num = QuaternionL.Angle(from, to);
if (num == 0f)
{
return(to);
}
FloatL t = FixPointMath.Min(1f, maxDegreesDelta / num);
return(QuaternionL.SlerpUnclamped(from, to, t));
}
public static bool Point2dWithLine2d(Vector2L point, Line2d line2d)
{
// Find the slope
FloatL dy = (line2d.m_point2.y - line2d.m_point1.y);
FloatL dx = (line2d.m_point2.x - line2d.m_point1.x);
FloatL M = dy / dx;
// Find the Y-Intercept
FloatL B = line2d.m_point1.y - M * line2d.m_point1.x;
// Check line equation
return(FixPointMath.Approximately(point.y, M * point.x + B));
}
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 Matrix4x4L Perspective(FloatL fov, FloatL aspect /*纵横比*/, FloatL zNear, FloatL zFar)
{
// 为什么这里用的是opengl的透视矩阵？
Matrix4x4L ret = Matrix4x4L.zero;
FloatL fovRad = FixPointMath.Deg2Rad * fov;
ret.m00 = 1 / FixPointMath.Tan(fovRad * 0.5f) / aspect;
ret.m11 = 1 / FixPointMath.Tan(fovRad * 0.5f);
ret.m22 = -(zFar + zNear) / (zFar - zNear);
ret.m23 = -2 * zNear * zFar / (zFar - zNear);
ret.m32 = -1;
return(ret);
}
public static FloatL Atan2(FloatL yL, FloatL xL)
{
//return Math.Atan2(y.ToDouble(), x.ToDouble());
long y = yL.m_numerator;
long x = xL.m_numerator;
int num;
int num2;
if (x < 0)
{
if (y < 0)
{
x = -x;
y = -y;
num = 1;
}
else
{
x = -x;
num = -1;
}
num2 = -31416;
}
else
{
if (y < 0)
{
y = -y;
num = -1;
}
else
{
num = 1;
}
num2 = 0;
}
int dIM = Atan2LookupTable.DIM;
long num3 = (long)(dIM - 1);
long b = (long)((x >= y) ? x : y);
int num4 = (int)FixPointMath.Divide((long)x * num3, b);
int num5 = (int)FixPointMath.Divide((long)y * num3, b);
int num6 = Atan2LookupTable.table[num5 * dIM + num4];
return(new FloatL((num6 + num2) * num) / new FloatL(10000));
}
public static QuaternionL AngleAxis(FloatL degress, Vector3L axis)
{
if (axis.sqrMagnitude == 0.0f)
{
return(identity);
}
QuaternionL result = identity;
FloatL radians = degress * FixPointMath.Deg2Rad;
radians *= 0.5f;
axis.Normalize();
axis = axis * FixPointMath.Sin(radians);
result.x = axis.x;
result.y = axis.y;
result.z = axis.z;
result.w = FixPointMath.Cos(radians);
return(Normalize(result));
}
public void SetContent(Mesh3d mesh)
{
m_mesh3d = mesh;
Vector3L[] vertices = mesh.GetVertices();
Vector3L min = vertices[0];
Vector3L max = vertices[0];
for (int i = 1; i < mesh.m_triangleList.Count * 3; ++i)
{
min.x = FixPointMath.Min(vertices[i].x, min.x);
min.y = FixPointMath.Min(vertices[i].y, min.y);
min.z = FixPointMath.Min(vertices[i].z, min.z);
max.x = FixPointMath.Max(vertices[i].x, max.x);
max.y = FixPointMath.Max(vertices[i].y, max.y);
max.z = FixPointMath.Max(vertices[i].z, max.z);
}
m_aabb = Mesh3d.FromMinMax(min, max);
}
static void ToAxisAngleRad(QuaternionL q, out Vector3L axis, out FloatL angle)
{
if (FixPointMath.Abs(q.w) > 1.0f)
{
q.Normalize();
}
angle = 2.0f * FixPointMath.Acos(q.w); // angle
FloatL den = FixPointMath.Sqrt(1.0f - q.w * q.w);
if (den > 0.0001f)
{
axis = q.xyz / den;
}
else
{
// This occurs when the angle is zero.
// Not a problem: just set an arbitrary normalized axis.
axis = new Vector3L(1, 0, 0);
}
}
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 void AccelerateMesh(Mesh3d mesh)
{
if (mesh.m_accelerator != null)
{
return;
}
Vector3L[] vertices = mesh.GetVertices();
Vector3L min = vertices[0];
Vector3L max = vertices[0];
for (int i = 1; i < mesh.m_triangleList.Count * 3; ++i)
{
min.x = FixPointMath.Min(vertices[i].x, min.x);
min.y = FixPointMath.Min(vertices[i].y, min.y);
min.z = FixPointMath.Min(vertices[i].z, min.z);
max.x = FixPointMath.Max(vertices[i].x, max.x);
max.y = FixPointMath.Max(vertices[i].y, max.y);
max.z = FixPointMath.Max(vertices[i].z, max.z);
}
mesh.m_accelerator = new BVHNode();
mesh.m_accelerator.m_bounds = FromMinMax(min, max);
mesh.m_accelerator.m_children = null;
// this.m_accelerator.m_triangles = this.m_triangleList.Count;
// this.m_accelerator->triangles = new int[mesh.numTriangles];
// for (int i = 0; i < mesh.numTriangles; ++i) {
// mesh.accelerator->triangles[i] = i;
// }
//this.m_accelerator.m_triangles = this.m_triangleList;
// for (int i = 0; i < mesh.m_triangleList.Count; ++i)
// {
// mesh.m_accelerator.m_triangles.Add(mesh.m_triangleList);
// }
mesh.m_accelerator.m_triangles.AddRange(mesh.m_triangleList);
SplitBVHNode(mesh.m_accelerator, mesh, 3);
}
public static Vector2L Max(Vector2L lhs, Vector2L rhs)
{
return(new Vector2L(FixPointMath.Max(lhs.x, rhs.x), FixPointMath.Max(lhs.y, rhs.y)));
}
public static FloatL Angle(Vector2L from, Vector2L to)
{
return(FixPointMath.Acos(FixPointMath.Clamp(Vector2L.Dot(from.normalized, to.normalized), -1, 1)) * 57.29578d);
}
public static Vector2L Lerp(Vector2L from, Vector2L to, FloatL t)
{
t = FixPointMath.Clamp01(t);
return(new Vector2L(from.x + (to.x - from.x) * t, from.y + (to.y - from.y) * t));
}
public static FloatL Distance(Vector3L a, Vector3L b)
{
Vector3L vector = new Vector3L(a.x - b.x, a.y - b.y, a.z - b.z);
return(FixPointMath.Sqrt(vector.x * vector.x + vector.y * vector.y + vector.z * vector.z));
}
public static FloatL Magnitude(Vector3L a)
{
return(FixPointMath.Sqrt(a.x * a.x + a.y * a.y + a.z * a.z));
}
public static Vector3L Min(Vector3L lhs, Vector3L rhs)
{
return(new Vector3L(FixPointMath.Min(lhs.x, rhs.x), FixPointMath.Min(lhs.y, rhs.y), FixPointMath.Min(lhs.z, rhs.z)));
}
