// 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 MoveTowards(FloatL current, FloatL target, FloatL maxDelta)
{
if (FixPointMath.Abs(target - current) <= maxDelta)
{
return(target);
}
return(current + FixPointMath.Sign(target - current) * maxDelta);
}
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);
}
}
public static FloatL Angle(QuaternionL a, QuaternionL b)
{
FloatL f = QuaternionL.Dot(a, b);
return(FixPointMath.Acos(FixPointMath.Min(FixPointMath.Abs(f), 1f)) * 2f * 57.29578d);
}
static int _MatrixDecompose(FloatL[][] m, out FloatL[][] lum, out int[] perm)
{
// Crout's LU decomposition for matrix determinant and inverse
// stores combined lower & upper in lum[][]
// stores row permuations into perm[]
// returns +1 or -1 according to even or odd number of row permutations
// lower gets dummy 1.0s on diagonal (0.0s above)
// upper gets lum values on diagonal (0.0s below)
int toggle = +1; // even (+1) or odd (-1) row permutatuions
int n = m.Length;
// make a copy of m[][] into result lu[][]
lum = _MatrixCreate(n, n);
for (int i = 0; i < n; ++i)
{
for (int j = 0; j < n; ++j)
{
lum[i][j] = m[i][j];
}
}
// make perm[]
perm = new int[n];
for (int i = 0; i < n; ++i)
{
perm[i] = i;
}
for (int j = 0; j < n - 1; ++j) // process by column. note n-1
{
FloatL max = FixPointMath.Abs(lum[j][j]);
int piv = j;
for (int i = j + 1; i < n; ++i) // find pivot index
{
FloatL xij = FixPointMath.Abs(lum[i][j]);
if (xij > max)
{
max = xij;
piv = i;
}
} // i
if (piv != j)
{
FloatL[] tmp = lum[piv]; // swap rows j, piv
lum[piv] = lum[j];
lum[j] = tmp;
int t = perm[piv]; // swap perm elements
perm[piv] = perm[j];
perm[j] = t;
toggle = -toggle;
}
FloatL xjj = lum[j][j];
if (xjj != 0.0)
{
for (int i = j + 1; i < n; ++i)
{
FloatL xij = lum[i][j] / xjj;
lum[i][j] = xij;
for (int k = j + 1; k < n; ++k)
{
lum[i][k] -= xij * lum[j][k];
}
}
}
} // j
return(toggle);
}
public static FloatL PingPong(FloatL t, FloatL length)
{
t = FixPointMath.Repeat(t, length * 2f);
return(length - FixPointMath.Abs(t - length));
}
