private static Matrix4x4d LookRotationToMatrix(Vector3d viewVec, Vector3d upVec)
{
Vector3d z = viewVec;
Matrix4x4d m = new Matrix4x4d();
double mag = Vector3d.Magnitude(z);
if (mag < 0)
{
m = Matrix4x4d.identity;
}
z /= mag;
Vector3d x = Vector3d.Cross(upVec, z);
mag = Vector3d.Magnitude(x);
if (mag < 0)
{
m = Matrix4x4d.identity;
}
x /= mag;
Vector3d y = Vector3d.Cross(z, x);
m[0, 0] = x.x; m[0, 1] = y.x; m[0, 2] = z.x;
m[1, 0] = x.y; m[1, 1] = y.y; m[1, 2] = z.y;
m[2, 0] = x.z; m[2, 1] = y.z; m[2, 2] = z.z;
return(m);
}
public static Matrix4x4d SetAxisAngle(Vector3d rotationAxis, double radians)
{
Matrix4x4d m = new Matrix4x4d();
double s, c;
double xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
s = Math.Sin(radians);
c = Math.Cos(radians);
xx = rotationAxis.x * rotationAxis.x;
yy = rotationAxis.y * rotationAxis.y;
zz = rotationAxis.z * rotationAxis.z;
xy = rotationAxis.x * rotationAxis.y;
yz = rotationAxis.y * rotationAxis.z;
zx = rotationAxis.z * rotationAxis.x;
xs = rotationAxis.x * s;
ys = rotationAxis.y * s;
zs = rotationAxis.z * s;
one_c = 1 - c;
m[0, 0] = (one_c * xx) + c;
m[0, 1] = (one_c * xy) - zs;
m[0, 2] = (one_c * zx) + ys;
m[1, 0] = (one_c * xy) + zs;
m[1, 1] = (one_c * yy) + c;
m[1, 2] = (one_c * yz) - xs;
m[2, 0] = (one_c * zx) - ys;
m[2, 1] = (one_c * yz) + xs;
m[2, 2] = (one_c * zz) + c;
return(m);
}
private static Quaterniond MatrixToQuaternion(Matrix4x4d m)
{
Quaterniond quat = new Quaterniond();
double fTrace = m[0, 0] + m[1, 1] + m[2, 2];
double root;
if (fTrace > 0)
{
root = Math.Sqrt(fTrace + 1);
quat.w = 0.5D * root;
root = 0.5D / root;
quat.x = (m[2, 1] - m[1, 2]) * root;
quat.y = (m[0, 2] - m[2, 0]) * root;
quat.z = (m[1, 0] - m[0, 1]) * root;
}
else
{
int[] s_iNext = new int[] { 1, 2, 0 };
int i = 0;
if (m[1, 1] > m[0, 0])
{
i = 1;
}
if (m[2, 2] > m[i, i])
{
i = 2;
}
int j = s_iNext[i];
int k = s_iNext[j];
root = Math.Sqrt(m[i, i] - m[j, j] - m[k, k] + 1);
if (root < 0)
{
throw new IndexOutOfRangeException("error!");
}
quat[i] = 0.5 * root;
root = 0.5f / root;
quat.w = (m[k, j] - m[j, k]) * root;
quat[j] = (m[j, i] + m[i, j]) * root;
quat[k] = (m[k, i] + m[i, k]) * root;
}
double nor = Math.Sqrt(Dot(quat, quat));
quat = new Quaterniond(quat.x / nor, quat.y / nor, quat.z / nor, quat.w / nor);
return(quat);
}
/// <summary>
/// 正交矩阵
/// </summary>
/// <param name="left"></param>
/// <param name="right"></param>
/// <param name="bottom"></param>
/// <param name="top"></param>
/// <param name="zNear"></param>
/// <param name="zFar"></param>
/// <returns></returns>
public static Matrix4x4d Ortho(double left, double right, double bottom, double top, double zNear, double zFar)
{
Matrix4x4d result = identity;
double deltax = right - left;
double deltay = top - bottom;
double deltaz = zFar - zNear;
result[0, 0] = 2.0F / deltax;
result[0, 3] = -(right + left) / deltax;
result[1, 1] = 2.0F / deltay;
result[1, 3] = -(top + bottom) / deltay;
result[2, 2] = -2.0F / deltaz;
result[2, 3] = -(zFar + zNear) / deltaz;
return(result);
}
/// <summary>
/// 透视矩阵
/// </summary>
/// <param name="fov"></param>
/// <param name="aspect"></param>
/// <param name="zNear"></param>
/// <param name="zFar"></param>
/// <returns></returns>
public static Matrix4x4d Perspective(double fov, double aspect, double zNear, double zFar)
{
Matrix4x4d result = new Matrix4x4d();
double cotangent, deltaZ;
double radians = (fov / 2.0) * (Math.PI / 180);
cotangent = Math.Cos(radians) / Math.Sin(radians);
deltaZ = zNear - zFar;
result[0, 0] = cotangent / aspect; result[0, 1] = 0; result[0, 2] = 0; result[0, 3] = 0;
result[1, 0] = 0; result[1, 1] = cotangent; result[1, 2] = 0; result[1, 3] = 0;
result[2, 0] = 0; result[2, 1] = 0; result[2, 2] = (zFar + zNear) / deltaZ; result[2, 3] = 2 * zNear * zFar / deltaZ;
result[3, 0] = 0; result[3, 1] = 0; result[3, 2] = -1; result[3, 3] = 0;
return(result);
}
/// <summary>
/// 平移旋转缩放矩阵
/// </summary>
/// <param name="pos"></param>
/// <param name="q"></param>
/// <param name="s"></param>
/// <returns></returns>
public static Matrix4x4d TRS(Vector3d pos, Quaterniond q, Vector3d s)
{
Matrix4x4d m = Quaterniond.QuaternionToMatrix(q);
m[0] *= s[0];
m[1] *= s[0];
m[2] *= s[0];
m[4] *= s[1];
m[5] *= s[1];
m[6] *= s[1];
m[8] *= s[2];
m[9] *= s[2];
m[10] *= s[2];
m[12] = pos[0];
m[13] = pos[1];
m[14] = pos[2];
return(m);
}
private Vector3d MatrixToEuler(Matrix4x4d m)
{
Vector3d v = new Vector3d();
if (m[1, 2] < 1)
{
if (m[1, 2] > -1)
{
v.x = Math.Asin(-m[1, 2]);
v.y = Math.Atan2(m[0, 2], m[2, 2]);
v.z = Math.Atan2(m[1, 0], m[1, 1]);
}
else
{
v.x = Math.PI * 0.5;
v.y = Math.Atan2(m[0, 1], m[0, 0]);
v.z = 0;
}
}
else
{
v.x = -Math.PI * 0.5;
v.y = Math.Atan2(-m[0, 1], m[0, 0]);
v.z = 0;
}
for (int i = 0; i < 3; i++)
{
if (v[i] < 0)
{
v[i] += 2 * Math.PI;
}
else if (v[i] > 2 * Math.PI)
{
v[i] -= 2 * Math.PI;
}
}
return(v);
}
/// <summary>
/// 球形插值(无限制)
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="t"></param>
/// <returns></returns>
public static Vector3d SlerpUnclamped(Vector3d lhs, Vector3d rhs, double t)
{
double lhsMag = Magnitude(lhs);
double rhsMag = Magnitude(rhs);
if (lhsMag < 0 || rhsMag < 0)
{
return(Lerp(lhs, rhs, t));
}
double lerpedMagnitude = rhsMag * t + lhsMag * (1 - t);
double dot = Dot(lhs, rhs) / (lhsMag * rhsMag);
if (dot > 1)
{
return(Lerp(lhs, rhs, t));
}
else if (dot < -1)
{
Vector3d lhsNorm = lhs / lhsMag;
Vector3d axis = OrthoNormalVectorFast(lhsNorm);
Matrix4x4d m = SetAxisAngle(axis, Math.PI * t);
Vector3d slerped = m * lhsNorm;
slerped *= lerpedMagnitude;
return(slerped);
}
else
{
Vector3d axis = Cross(lhs, rhs);
Vector3d lhsNorm = lhs / lhsMag;
axis = Normalize(axis);
double angle = Math.Acos(dot) * t;
Matrix4x4d m = SetAxisAngle(axis, angle);
Vector3d slerped = m * lhsNorm;
slerped *= lerpedMagnitude;
return(slerped);
}
}
public static Matrix4x4d QuaternionToMatrix(Quaterniond quat)
{
Matrix4x4d m = new Matrix4x4d();
double x = quat.x * 2;
double y = quat.y * 2;
double z = quat.z * 2;
double xx = quat.x * x;
double yy = quat.y * y;
double zz = quat.z * z;
double xy = quat.x * y;
double xz = quat.x * z;
double yz = quat.y * z;
double wx = quat.w * x;
double wy = quat.w * y;
double wz = quat.w * z;
m[0] = 1.0f - (yy + zz);
m[1] = xy + wz;
m[2] = xz - wy;
m[3] = 0.0F;
m[4] = xy - wz;
m[5] = 1.0f - (xx + zz);
m[6] = yz + wx;
m[7] = 0.0F;
m[8] = xz + wy;
m[9] = yz - wx;
m[10] = 1.0f - (xx + yy);
m[11] = 0.0F;
m[12] = 0.0F;
m[13] = 0.0F;
m[14] = 0.0F;
m[15] = 1.0F;
return(m);
}
/// <summary>
/// 向目标向量旋转
/// </summary>
/// <param name="current"></param>
/// <param name="target"></param>
/// <param name="maxRadiansDelta"></param>
/// <param name="maxMagnitudeDelta"></param>
/// <returns></returns>
public static Vector3d RotateTowards(Vector3d current, Vector3d target, double maxRadiansDelta, double maxMagnitudeDelta)
{
double currentMag = Magnitude(current);
double targetMag = Magnitude(target);
if (currentMag > 0 && targetMag > 0)
{
Vector3d currentNorm = current / currentMag;
Vector3d targetNorm = target / targetMag;
double dot = Dot(currentNorm, targetNorm);
if (dot > 1)
{
return(MoveTowards(current, target, maxMagnitudeDelta));
}
else if (dot < -1)
{
Vector3d axis = OrthoNormalVectorFast(currentNorm);
Matrix4x4d m = SetAxisAngle(axis, maxRadiansDelta);
Vector3d rotated = m * currentNorm;
rotated *= ClampedMove(currentMag, targetMag, maxMagnitudeDelta);
return(rotated);
}
else
{
double angle = Math.Acos(dot);
Vector3d axis = Normalize(Cross(currentNorm, targetNorm));
Matrix4x4d m = SetAxisAngle(axis, Math.Min(maxRadiansDelta, angle));
Vector3d rotated = m * currentNorm;
rotated *= ClampedMove(currentMag, targetMag, maxMagnitudeDelta);
return(rotated);
}
}
else
{
return(MoveTowards(current, target, maxMagnitudeDelta));
}
}
/// <summary>
/// 注视旋转
/// </summary>
/// <param name="forward"></param>
/// <param name="upwards"></param>
/// <returns></returns>
public static Quaterniond LookRotation(Vector3d forward, [DefaultValue("Vector3d.up")] Vector3d upwards)
{
Matrix4x4d m = LookRotationToMatrix(forward, upwards);
return(MatrixToQuaternion(m));
}
/// <summary>
/// 转置矩阵
/// </summary>
/// <param name="m"></param>
/// <returns></returns>
public static Matrix4x4d Transpose(Matrix4x4d m)
{
return(m.transpose);
}
/// <summary>
/// 逆矩阵
/// </summary>
/// <param name="m"></param>
/// <returns></returns>
public static Matrix4x4d Inverse(Matrix4x4d m)
{
return(m.inverse);
}
/// <summary>
/// 行列式
/// </summary>
/// <param name="m"></param>
/// <returns></returns>
public static double Determinant(Matrix4x4d m)
{
return(m.determinant);
}
