public static FloatL operator -(FloatL a)
{
FloatL ret = new FloatL();
ret.m_numerator = -a.m_numerator;
return(ret);
}
static FloatL[] _Helper(FloatL[][] luMatrix, FloatL[] b) // helper
{
int n = luMatrix.Length;
FloatL[] x = new FloatL[n];
b.CopyTo(x, 0);
for (int i = 1; i < n; ++i)
{
FloatL sum = x[i];
for (int j = 0; j < i; ++j)
{
sum -= luMatrix[i][j] * x[j];
}
x[i] = sum;
}
x[n - 1] /= luMatrix[n - 1][n - 1];
for (int i = n - 2; i >= 0; --i)
{
FloatL sum = x[i];
for (int j = i + 1; j < n; ++j)
{
sum -= luMatrix[i][j] * x[j];
}
x[i] = sum / luMatrix[i][i];
}
return(x);
}
public static FloatL operator *(FloatL a, FloatL b)
{
FloatL ret = new FloatL();
ret.m_numerator = ((a.m_numerator * b.m_numerator) / m_denominator);
return(ret);
}
public static FloatL operator /(FloatL a, FloatL b)
{
FloatL ret = new FloatL();
if (b.m_numerator == 0)
{
UnityEngine.Debug.LogError("FloatL / 0");
if (a.m_numerator >= 0)
{
return(MaxValue);
}
else if (a.m_numerator < 0)
{
return(MinValue);
}
// else
// {
// return 0;
// }
}
// 可以应对小数除大数时不为0
ret.m_numerator = ((a.m_numerator * m_denominator) / b.m_numerator);
return(ret);
}
// 四元数转欧拉角
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);
}
// 欧拉角转四元数
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 void Set(FloatL new_x, FloatL new_y, FloatL new_z, FloatL new_w)
{
this.x = new_x;
this.y = new_y;
this.z = new_z;
this.w = new_w;
}
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 Vector4L(FloatL x, FloatL y, FloatL z, FloatL w)
{
this.x = x;
this.y = y;
this.z = z;
this.w = w;
}
public Vector4L(FloatL x, FloatL y)
{
this.x = x;
this.y = y;
this.z = 0f;
this.w = 0f;
}
// 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);
}
public static FloatL InverseLerp(FloatL from, FloatL to, FloatL value)
{
if (from < to)
{
if (value < from)
{
return(0f);
}
if (value > to)
{
return(1f);
}
value -= from;
value /= to - from;
return(value);
}
else
{
if (from <= to)
{
return(0f);
}
if (value < to)
{
return(1f);
}
if (value > from)
{
return(0f);
}
return(1f - (value - to) / (from - to));
}
}
public static FloatL Cos(FloatL f)
{
//return Math.Cos(f.ToDouble());
int index = SinCosLookupTable.getIndex(f.m_numerator, FloatL.m_denominator);
return(new FloatL(SinCosLookupTable.cos_table[index]) / new FloatL(SinCosLookupTable.FACTOR));
}
public static Model3d Raycast(OctreeNode node, Ray3d ray)
{
IntersectionTest3D.RaycastResult raycast = new IntersectionTest3D.RaycastResult();
IntersectionTest3D.Ray3dWithAABB3d(node.m_bounds, ray, ref raycast);
FloatL t = raycast.m_t;
if (t >= 0)
{
if (node.m_children == null)
{
return(FindClosest(node.m_models, ray));
}
else
{
/*std::vector<Model*> results;*/
List <Model3d> results = new List <Model3d>();
for (int i = 0; i < 8; ++i)
{
Model3d result = Raycast(node.m_children[i], ray);
if (result != null)
{
results.Add(result);
}
}
return(FindClosest(results, ray));
}
}
return(null);
}
public void Scale(Vector4L scale)
{
this.x *= scale.x;
this.y *= scale.y;
this.z *= scale.z;
this.w *= scale.w;
}
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 Vector3L ExtremePoint(Vector3L direction, ref FloatL projectionDistance)
{
Vector3L extremePoint = ExtremePoint(direction);
projectionDistance = Vector3L.Dot(extremePoint, direction);
return(extremePoint);
}
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 FloatL operator +(FloatL a, FloatL b)
{
FloatL ret = new FloatL();
ret.m_numerator = a.m_numerator + b.m_numerator;
return(ret);
}
public QuaternionL(FloatL x, FloatL y, FloatL z, FloatL w)
{
this.x = x;
this.y = y;
this.z = z;
this.w = w;
}
public Model3d Raycast(Ray3d ray)
{
if (m_octree != null)
{
// :: lets the compiler know to look outside class scope
return(Octree3d.Raycast(m_octree, ray));
}
Model3d result = null;
FloatL result_t = -1;
for (int i = 0, size = m_modleList.Count; i < size; ++i)
{
FloatL t = IntersectionTest3D.Ray3dWithModel3d(ray, m_modleList[i]);
if (result == null && t >= 0)
{
result = m_modleList[i];
result_t = t;
}
else if (result != null && t < result_t)
{
result = m_modleList[i];
result_t = t;
}
}
return(result);
}
public static Model3d FindClosest(List <Model3d> set, Ray3d ray)
{
if (set.Count == 0)
{
return(null);
}
Model3d closest = null;
FloatL closest_t = -1;
for (int i = 0, size = set.Count; i < size; ++i)
{
FloatL this_t = IntersectionTest3D.Ray3dWithModel3d(ray, set[i]);
if (this_t < 0)
{
continue;
}
if (closest_t < 0 || this_t < closest_t)
{
closest_t = this_t;
closest = set[i];
}
}
return(closest);
}
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));
}
public FloatL this[int index]
{
get
{
if (index == 0)
{
return(this.x);
}
if (index != 1)
{
throw new IndexOutOfRangeException("Invalid Vector2 index!");
}
return(this.y);
}
set
{
if (index != 0)
{
if (index != 1)
{
throw new IndexOutOfRangeException("Invalid Vector2 index!");
}
this.y = value;
}
else
{
this.x = value;
}
}
}
void Normalize()
{
FloatL scale = 1.0f / this.Length;
xyz *= scale;
w *= scale;
}
static QuaternionL Normalize(QuaternionL q)
{
FloatL scale = 1.0f / q.Length;
QuaternionL result = new QuaternionL(q.xyz * scale, q.w * scale);
return(result);
}
QuaternionL(Vector3L v, FloatL w)
{
this.x = v.x;
this.y = v.y;
this.z = v.z;
this.w = w;
}
public void Set(FloatL new_x, FloatL new_y, FloatL new_z, FloatL new_w)
{
x = new_x;
y = new_y;
z = new_z;
w = new_w;
}
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 Vector2L ClampMagnitude(Vector2L vector, FloatL maxLength)
{
if (vector.sqrMagnitude > maxLength * maxLength)
{
return(vector.normalized * maxLength);
}
return(vector);
}
