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