public void FromToRotation(Vector3 from, Vector3 to, ref Matrix33 mtx)
{
Vector3 v = Vector3.Cross(from, to);
tfloat e = Vector3.Dot(from, to);
if (e > (Math.ONE - Math.Epsilon))
{
mtx[0, 0] = Math.ONE; mtx[0, 1] = Math.ZERO; mtx[0, 2] = Math.ZERO;
mtx[0, 0] = Math.ZERO; mtx[0, 1] = Math.ONE; mtx[0, 2] = Math.ZERO;
mtx[0, 0] = Math.ZERO; mtx[0, 1] = Math.ZERO; mtx[0, 2] = Math.ONE;
}
else if (e < (new tfloat(-1) * Math.ONE + Math.Epsilon))
{
Vector3 up, left = Vector3.ZERO;
tfloat invlen;
tfloat fxx, fyy, fzz, fxy, fxz, fyz;
tfloat uxx, uyy, uzz, uxy, uxz, uyz;
tfloat lxx, lyy, lzz, lxy, lxz, lyz;
left[0] = Math.ZERO; left[1] = from[2]; left[2] = -from[1];
if (Vector3.Dot(left, left) < Math.Epsilon)
{
left[0] = -from[2]; left[1] = Math.ZERO; left[2] = from[0];
}
invlen = Math.ONE / Math.Sqrt(Vector3.Dot(left, left));
left[0] *= invlen;
left[1] *= invlen;
left[2] *= invlen;
up = Vector3.Cross(left, from);
fxx = -from[0] * from[0]; fyy = -from[1] * from[1]; fzz = -from[2] * from[2];
fxy = -from[0] * from[1]; fxz = -from[0] * from[2]; fyz = -from[1] * from[2];
uxx = up[0] * up[0]; uyy = up[1] * up[1]; uzz = up[2] * up[2];
uxy = up[0] * up[1]; uxz = up[0] * up[2]; uyz = up[1] * up[2];
lxx = -left[0] * left[0]; lyy = -left[1] * left[1]; lzz = -left[2] * left[2];
lxy = -left[0] * left[1]; lxz = -left[0] * left[2]; lyz = -left[1] * left[2];
/* symmetric matrix */
mtx[0, 0] = fxx + uxx + lxx; mtx[0, 1] = fxy + uxy + lxy; mtx[0, 2] = fxz + uxz + lxz;
mtx[1, 0] = mtx[0, 1]; mtx[1, 1] = fyy + uyy + lyy; mtx[1, 2] = fyz + uyz + lyz;
mtx[2, 0] = mtx[0, 2]; mtx[2, 1] = mtx[1, 2]; mtx[2, 2] = fzz + uzz + lzz;
}
else
{
tfloat hvx, hvz, hvxy, hvxz, hvyz;
tfloat h = (Math.ONE - e) / Vector3.Dot(v, v);
hvx = h * v[0];
hvz = h * v[2];
hvxy = hvx * v[1];
hvxz = hvx * v[2];
hvyz = hvz * v[1];
mtx[0, 0] = e + hvx * v[0]; mtx[0, 1] = hvxy - v[2]; mtx[0, 2] = hvxz + v[1];
mtx[1, 0] = hvxy + v[2]; mtx[1, 1] = e + h * v[1] * v[1]; mtx[1, 2] = hvyz - v[0];
mtx[2, 0] = hvxz - v[1]; mtx[2, 1] = hvyz + v[0]; mtx[2, 2] = e + hvz * v[2];
}
}
public Matrix33 SetFromToRotation(Vector3 from, Vector3 to)
{
Matrix33 mtx = new Matrix33();
this.FromToRotation(from, to, ref mtx);
this[0, 0] = mtx[0, 0]; this[0, 1] = mtx[0, 1]; this[0, 2] = mtx[0, 2];
this[1, 0] = mtx[1, 0]; this[1, 1] = mtx[1, 1]; this[1, 2] = mtx[1, 2];
this[2, 0] = mtx[2, 0]; this[2, 1] = mtx[2, 1]; this[2, 2] = mtx[2, 2];
return(mtx);
}
public static void MatrixToQuaternion(Matrix33 mtx, ref Quaternion q)
{
tfloat fTrace = mtx[0, 0] + mtx[1, 1] + mtx[2, 2];
tfloat fRoot;
if (fTrace > Math.ZERO)
{
fRoot = Math.Sqrt(fTrace + Math.ONE); // 2w
q.w = 0.5f * fRoot;
fRoot = 0.5f / fRoot; // 1/(4w)
q.x = (mtx[2, 1] - mtx[1, 2]) * fRoot;
q.y = (mtx[0, 2] - mtx[2, 0]) * fRoot;
q.z = (mtx[1, 0] - mtx[0, 1]) * fRoot;
}
else
{
int[] next = new int[3] {
1, 2, 0
};
int i = 0;
if (mtx[1, 1] > mtx[0, 0])
{
i = 1;
}
if (mtx[2, 2] > mtx[i, i])
{
i = 2;
}
int j = next[i];
int k = next[j];
fRoot = Math.Sqrt(mtx[i, i] - mtx[j, j] - mtx[k, k] + Math.ONE);
q[i] = 0.5f / fRoot;
q.w = (mtx[k, j] - mtx[j, k]) * fRoot;
q[j] = (mtx[j, i] + mtx[i, j]) * fRoot;
q[k] = (mtx[k, i] + mtx[i, k]) * fRoot;
}
q = Normalize(q);
}
public static Quaternion FromToRotation(Vector3 fromDirection, Vector3 toDirection)
{
tfloat lhsMag = fromDirection.magnitude;
tfloat rhsMag = toDirection.magnitude;
if (lhsMag < Math.Epsilon || rhsMag < Math.Epsilon)
{
return(identity);
}
//nomarl///
Vector3 lhs = fromDirection / lhsMag;
Vector3 rhs = toDirection / rhsMag;
Matrix33 mtx = new Matrix33();
mtx = mtx.SetFromToRotation(lhs, rhs);
Quaternion q = new Quaternion();
MatrixToQuaternion(mtx, ref q);
return(q);
}