// 欧拉角转四元数 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))); }