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