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