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