public Quaternion(double angle, Vector unit_axis)
{
double sa = Math.Sin(angle * 0.5);
w = Math.Cos(angle * 0.5);
x = unit_axis.x * sa;
y = unit_axis.y * sa;
z = unit_axis.z * sa;
}
public TMatrix Translated(Vector v)
{
var x = new TMatrix( (double[])this.m.Clone() );
x.m[12] += v.x;
x.m[13] += v.y;
x.m[14] += v.z;
return x;
}
public Quaternion GetRotation()
{
// Decompose the transformation to a rotation followed by translation, and return
// the rotation. Throws InvalidOperationException if the matrix is not of that form.
if (m[3] != 0 || m[7] != 0 || m[11] != 0 || m[15] != 1)
throw new InvalidOperationException("Not a rotation+translation.");
// Check for any scale, which we don't decompose (actually TMatrix is never supposed to
// have a scale)
for(int i=0; i<3; i++)
if (Math.Abs(m[0+i]*m[0+i] + m[4+i]*m[4+i] + m[8+i]*m[8+i] - 1.0) > .0001)
throw new InvalidOperationException("Not a rotation+translation.");
// Extract rotation matrix
double[] r = new double[9];
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
r[i * 3 + j] = m[i * 4 + j];
// Test that it is (approximately) special orthogonal. If it isn't, the input matrix didn't meet the desired
// form.
double det = r[0] * (r[4] * r[8] - r[7] * r[5]) - r[1] * (r[3] * r[8] - r[6] * r[5]) + r[2] * (r[3] * r[7] - r[6] * r[4]);
if (det < 0.99999 || det > 1.00001)
throw new InvalidOperationException("Not a rotation+translation.");
// Rotation -> quaternion
var rotation = new Quaternion();
var T = r[0] + r[3 + 1] + r[6 + 2];
if (T > 0.0)
{
var S = 0.5 / Math.Sqrt(T + 1);
rotation.w = 0.25 / S;
rotation.x = (r[6 + 1] - r[3 + 2]) * S;
rotation.y = (r[0 + 2] - r[6 + 0]) * S;
rotation.z = (r[3 + 0] - r[0 + 1]) * S;
}
else if (r[0] > r[3 + 1] && r[0] > r[6 + 2])
{
var S = Math.Sqrt(1.0 + r[0] - r[3 + 1] - r[6 + 2]);
rotation.x = S * 0.5;
S = 0.5 / S;
rotation.w = (r[6 + 1] - r[3 + 2]) * S;
rotation.y = (r[0 + 1] + r[3 + 0]) * S;
rotation.z = (r[0 + 2] + r[6 + 0]) * S;
}
else if (r[3 + 1] > r[6 + 2])
{
var S = Math.Sqrt(1.0 + r[3 + 1] - r[0 + 0] - r[6 + 2]);
rotation.y = S * 0.5;
S = 0.5 / S;
rotation.w = (r[0 + 2] - r[6 + 0]) * S;
rotation.x = (r[0 + 1] + r[3 + 0]) * S;
rotation.z = (r[3 + 2] + r[6 + 1]) * S;
}
else
{
var S = Math.Sqrt(1.0 + r[6 + 2] - r[0 + 0] - r[3 + 1]);
rotation.z = S * 0.5;
S = 0.5 / S;
rotation.w = (r[3 + 0] - r[0 + 1]) * S;
rotation.x = (r[0 + 2] + r[6 + 0]) * S;
rotation.y = (r[3 + 2] + r[6 + 1]) * S;
}
/*var tr = r[0] + r[4] + r[8];
if (tr >= 0.0)
{
var s = Math.Sqrt(tr + 1.0);
rotation.w = s * 0.5;
s = 0.5 / s;
rotation.x = (r[7] - r[5]) * s;
rotation.y = (r[2] - r[6]) * s;
rotation.z = (r[3] - r[1]) * s;
} else {
int h = 0;
if (r[4] > */
/*
// Shoemake '85
var rotation = new Quaternion();
var w2 = 0.25 * (1 + r[0] + r[4] + r[8]);
if (w2 > double.Epsilon)
{
rotation.w = Math.Sqrt(w2);
rotation.x = (r[7] - r[5]) / (4 * rotation.w);
rotation.y = (r[2] - r[6]) / (4 * rotation.w);
rotation.z = (r[3] - r[1]) / (4 * rotation.w);
}
else
{
rotation.w = 0;
var x2 = -0.5 * (r[4] + r[8]);
if (x2 > double.Epsilon)
{
rotation.x = Math.Sqrt(x2);
rotation.y = r[3] / (2 * rotation.x);
rotation.z = r[6] / (2 * rotation.x);
}
else
{
rotation.x = 0;
var y2 = 0.5 * (1 - r[8]);
if (y2 > double.Epsilon)
{
rotation.y = Math.Sqrt(y2);
rotation.z = r[7] / (2 * rotation.y);
}
else
{
rotation.y = 0;
rotation.z = 1;
}
}
}
rotation.Renormalize();*/
var tv = new Vector(1,2,3);
var tp = rotation.ToMatrix().Inverse() * this * tv;
if ((tp-tv)*(tp-tv) > .0001)
Console.WriteLine("GetRotation error.");
rotation.Renormalize();
return rotation;
/*rotation.w = Math.Sqrt(Math.Max(0, 1 + r[0] + r[4] + r[8])) * 0.5;
rotation.x = Math.Sqrt(Math.Max(0, 1 + r[0] - r[4] - r[8])) * 0.5;
rotation.y = Math.Sqrt(Math.Max(0, 1 - r[0] + r[4] - r[8])) * 0.5;
rotation.z = Math.Sqrt(Math.Max(0, 1 - r[0] - r[4] + r[8])) * 0.5;
if (r[6 + 1] - r[3 + 2] < 0) rotation.x = -rotation.x;
if (r[0 + 2] - r[6 + 0] < 0) rotation.y = -rotation.y;
if (r[3 + 0] - r[0 + 1] < 0) rotation.z = -rotation.z;
rotation.Renormalize();
return rotation;*/
/*var T = r[0] + r[3 + 1] + r[6 + 2] + 1;
var rotation = new Quaternion();
if (T > 0.00000001)
{
var S = 0.5 / Math.Sqrt(T);
rotation.w = 0.25 / S;
rotation.x = (r[6 + 1] - r[3 + 2]) * S;
rotation.y = (r[0 + 2] - r[6 + 0]) * S;
rotation.z = (r[3 + 0] - r[0 + 1]) * S;
}
else if (r[0] > r[3 + 1] && r[0] > r[6 + 2])
{
var S = 0.5 / Math.Sqrt(1.0 + r[0] - r[3 + 1] - r[6 + 2]);
rotation.w = (r[6 + 1] - r[3 + 2]) * S;
rotation.x = 0.25 / S;
rotation.y = (r[0 + 1] + r[3 + 0]) * S;
rotation.z = (r[0 + 2] + r[6 + 0]) * S;
}
else if (r[3 + 1] > r[6 + 2])
{
var S = 0.5 / Math.Sqrt(1.0 + r[3 + 1] - r[0 + 0] - r[6 + 2]);
rotation.w = (r[0 + 2] - r[6 + 0]) * S;
rotation.x = (r[0 + 1] + r[3 + 0]) * S;
rotation.y = 0.25 * S;
rotation.z = (r[3 + 2] + r[6 + 1]) * S;
}
else
{
var S = 0.5 / Math.Sqrt(1.0 + r[6 + 2] - r[0 + 0] - r[3 + 1]);
rotation.w = (r[3 + 0] - r[0 + 1]) * S;
rotation.x = (r[0 + 2] + r[6 + 0]) * S;
rotation.y = (r[3 + 2] + r[6 + 1]) * S;
rotation.z = 0.25 * S;
}
rotation.Renormalize(); // Rounding error can make w>1, yielding NAN for rotation.Angle
return rotation;*/
// translation = R^T * (-this.w_column)
/*for (int i = 0; i < 3; i++)
translation[i] = -(r[0 + i] * m[12] + r[3 + i] * m[13] + r[6 + i] * m[14]);*/
}
public static TMatrix Translation(Vector v)
{
return new TMatrix( new double[] { 1,0,0,0, 0,1,0,0, 0,0,1,0, v.x,v.y,v.z,1 } );
}
static void TestMath()
{
var rand = new Random();
for (int i = 0; i < 1000; i++)
{
var u = new Vector( rand.NextDouble()-0.5, rand.NextDouble()-0.5, rand.NextDouble()-0.5 ).Normalized();
var t1 = new Vector( rand.NextDouble()-0.5, rand.NextDouble()-0.5, rand.NextDouble()-0.5 ) * 10;
var t2 = new Vector(rand.NextDouble() - 0.5, rand.NextDouble() - 0.5, rand.NextDouble() - 0.5) * 10;
var p1 = new Vector(rand.NextDouble() - 0.5, rand.NextDouble() - 0.5, rand.NextDouble() - 0.5) * 10;
var q = new Quaternion(rand.NextDouble() * Math.PI, u);
var m = q.ToMatrix();
var mt = TMatrix.Translation(t1) * m * TMatrix.Translation(t2);
var q2 = mt.GetRotation();
var m2 = q2.ToMatrix();
if (((m * p1) - (m2 * p1)) * (m * p1 - m2 * p1) > .0001)
Console.WriteLine("math error.");
}
var badq = new Quaternion(3.1414934204542249,
new Vector(-0.920567453, -0.390583634, -0));
badq = badq.ToMatrix().GetRotation();
Console.WriteLine(badq.Angle);
Console.WriteLine(badq.Axis);
}