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 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)); }
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 Ray3d(Vector3L origin, Vector3L dir) { m_rayOrigin = origin; m_rayDir = dir; m_rayDir.Normalize(); }
public Plane3d(Vector3L normal, Vector3L point) { m_planeNormal = normal; m_planeNormal.Normalize(); m_planeOnePoint = point; }