public static QuaternionRightHand LookRotation(Vector3RightHand forward, Vector3RightHand up)
{
forward = Vector3RightHand.Normalize(forward);
Vector3RightHand right = Vector3RightHand.Normalize(Vector3RightHand.Cross(up, forward));
up = Vector3RightHand.Cross(forward, right);
var m00 = right.x;
var m01 = right.y;
var m02 = right.z;
var m10 = up.x;
var m11 = up.y;
var m12 = up.z;
var m20 = forward.x;
var m21 = forward.y;
var m22 = forward.z;
float num8 = (m00 + m11) + m22;
var quaternion = new QuaternionRightHand();
if (num8 > 0f)
{
var num = (float)Mathf.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))
{
var num7 = (float)Mathf.Sqrt(((1f + m00) - m11) - m22);
var 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)
{
var num6 = (float)Mathf.Sqrt(((1f + m11) - m00) - m22);
var 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);
}
var num5 = (float)Mathf.Sqrt(((1f + m22) - m00) - m11);
var 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 QuaternionRightHand AngleAxis(float degress, Vector3RightHand axis)
{
if (axis.sqrMagnitude == 0.0f)
{
return(identity);
}
QuaternionRightHand result = identity;
var radians = degress * Mathf.Deg2Rad;
radians *= 0.5f;
axis.Normalize();
axis = axis * (float)Mathf.Sin(radians);
result.x = axis.x;
result.y = axis.y;
result.z = axis.z;
result.w = (float)Mathf.Cos(radians);
return(Normalize(result));
}
/// <summary>
/// 将向量from向向量to旋转角度angle
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <param name="angle"></param>
/// <returns></returns>
static Vector3RightHand RotateTo(Vector3RightHand from, Vector3RightHand to, float angle)
{
//如果两向量角度为0
if (Vector3RightHand.Angle(from, to) == 0)
{
return(from);
}
//旋转轴
Vector3RightHand n = Vector3RightHand.Cross(from, to);
//旋转轴规范化
n.Normalize();
//旋转矩阵
Matrix4x4RightHand rotateMatrix = new Matrix4x4RightHand();
//旋转的弧度
double radian = angle * Mathf.PI / 180;
float cosAngle = (float)Mathf.Cos((float)radian);
float sinAngle = (float)Mathf.Sin((float)radian);
//矩阵的数据
//这里看不懂的自行科普矩阵知识
rotateMatrix.SetRow(0, new Vector4(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 Vector4(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 Vector4(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 Vector4(0, 0, 0, 1));
Vector4 v = Vector3RightHand.ToVector4(from);
Vector3RightHand vector = new Vector3RightHand();
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 Test()
{
QuaternionRightHand rotate = QuaternionRightHand.identity;
rotate.eulerAngles = new Vector3RightHand(40, 50, 60);
Debug.Log("测试euler to Quaternion \n" + rotate);
Vector3RightHand euler = rotate.eulerAngles;
Debug.Log("测试quaternion to euler \n" + euler);
Vector3RightHand dir = new Vector3RightHand(0.3f, 0.4f, 0.5f);
dir.Normalize();
QuaternionRightHand lookRotation = QuaternionRightHand.LookRotation(dir, Vector3RightHand.up);
Debug.Log("测试look rotation \n" + lookRotation);
QuaternionRightHand rotate2 = QuaternionRightHand.identity;
rotate2.eulerAngles = new Vector3RightHand(70, 80, 90);
QuaternionRightHand slerp = QuaternionRightHand.Slerp(rotate, rotate2, 0.5f);
Debug.Log("测试slerp \n" + slerp);
//Console.ReadLine();
float angle = Angle(rotate, rotate2);
Debug.Log("测试angle " + angle);
QuaternionRightHand angleAxis = AngleAxis(10, dir);
Debug.Log("测试angleAxis " + angleAxis);
Vector3RightHand dir2 = new Vector3RightHand(0.6f, 0.7f, 0.8f);
dir2.Normalize();
QuaternionRightHand rotateTowards = QuaternionRightHand.RotateTowards(QuaternionRightHand.LookRotation(dir), QuaternionRightHand.LookRotation(dir2), float.MaxValue);
Debug.Log("测试RotateTowards " + rotateTowards);
Vector3RightHand dir3 = new Vector3RightHand(1, 0, 0);
Vector3RightHand dir4 = new Vector3RightHand(0, 0, 1);
QuaternionRightHand fromToRotation = FromToRotation(dir, dir2);
//Quaternion fromToRotation = FromToRotation(dir4, dir3);
Debug.Log("测试FromToRotation " + fromToRotation);
QuaternionRightHand inverstQua = QuaternionRightHand.Inverse(fromToRotation);
Debug.Log("测试inverst " + inverstQua);
QuaternionRightHand lerpQua = QuaternionRightHand.Lerp(rotate, rotate2, 0.5f);
Debug.Log("测试lerp " + lerpQua);
float angle2 = 0;
Vector3RightHand axis = Vector3RightHand.zero;
lerpQua.ToAngleAxis(out angle2, out axis);
Debug.Log("测试ToAngleAxis " + angle2 + " " + axis);
//Console.ReadLine();
}