// @returns the rotation matrix corresponding to the inverse rotation by this
// quaternion.
public RotationMatrix RotationMatrixTranspose()
{
RotationMatrix R = new RotationMatrix();
float x2 = _x * _x;
float y2 = _y * _y;
float z2 = _z * _z;
float r2 = _r * _r;
R[0, 0] = r2 + x2 - y2 - z2; // fill diagonal terms
R[1, 1] = r2 - x2 + y2 - z2;
R[2, 2] = r2 - x2 - y2 + z2;
float xy = _x * _y;
float yz = _y * _z;
float zx = _z * _x;
float rx = _r * _x;
float ry = _r * _y;
float rz = _r * _z;
R[0, 1] = 2 * (xy + rz); // fill off diagonal terms
R[0, 2] = 2 * (zx - ry);
R[1, 2] = 2 * (yz + rx);
R[1, 0] = 2 * (xy - rz);
R[2, 0] = 2 * (zx + ry);
R[2, 1] = 2 * (yz - rx);
return(R);
}
/// <summary>
///
/// </summary>
/// <param name="R">the rotation matrix from which the quaternion will be extracted</param>
public void Set(RotationMatrix R)
{
// The trace determines the angle
float diag = 0.5f * (R[0, 0] + R[1, 1] + R[2, 2] - 1.0f);
if (diag >= 1.0)
{
// zero angle
_x = _y = _z = 0.0f;
return;
}
else
{
float angle = (float)Math.Acos(diag);
_x = R[2, 1] - R[1, 2];
_y = R[0, 2] - R[2, 0];
_z = R[1, 0] - R[0, 1];
float axisnorm = (float)Math.Sqrt(_x * _x + _y * _y + _z * _z);
if (axisnorm < 1e-8)
{
// Axis is undefined
_x = _y = _z = 0.0f;
return;
}
else
{
// All OK, so scale it
float scale = angle / axisnorm;
_x *= scale;
_y *= scale;
_z *= scale;
}
}
}
public override MatrixFixed Transpose()
{
RotationMatrix R = new RotationMatrix();
R[0, 0] = this[0, 0]; R[0, 1] = this[1, 0]; R[0, 2] = this[2, 0];
R[1, 0] = this[0, 1]; R[1, 1] = this[1, 1]; R[1, 2] = this[2, 1];
R[2, 0] = this[0, 2]; R[2, 1] = this[1, 2]; R[2, 2] = this[2, 2];
return(R);
}
//param R the rotation matrix from which the quaternion will be extracted
public void Set(RotationMatrix R)
{
float d0 = R[0, 0], d1 = R[1, 1], d2 = R[2, 2];
// The trace determines the method of decomposition
float rr = 1.0f + d0 + d1 + d2;
if (rr > 0)
{
float s = 0.5f / (float)Math.Sqrt(rr);
_x = (R[2, 1] - R[1, 2]) * s;
_y = (R[0, 2] - R[2, 0]) * s;
_z = (R[1, 0] - R[0, 1]) * s;
_r = 0.25f / s;
}
else
{
// Trace is less than zero, so need to determine which
// major diagonal is largest
if ((d0 > d1) && (d0 > d2))
{
float s = 0.5f / (float)Math.Sqrt(1 + d0 - d1 - d2);
_x = 0.5f * s;
_y = (R[0, 1] + R[1, 0]) * s;
_z = (R[0, 2] + R[2, 0]) * s;
_r = (R[1, 2] + R[2, 1]) * s;
}
else
if (d1 > d2)
{
float s = 0.5f / (float)Math.Sqrt(1 + d0 - d1 - d2);
_x = (R[0, 1] + R[1, 0]) * s;
_y = 0.5f * s;
_z = (R[1, 2] + R[2, 1]) * s;
_r = (R[0, 2] + R[2, 0]) * s;
}
else
{
float s = 0.5f / (float)Math.Sqrt(1 + d0 - d1 - d2);
_x = (R[0, 2] + R[2, 0]) * s;
_y = (R[1, 2] + R[2, 1]) * s;
_z = 0.5f * s;
_r = (R[0, 1] + R[1, 0]) * s;
}
}
}
/// <summary>
///
/// </summary>
/// <returns>the rotation matrix corresponding to the inverse rotation by this quaternion</returns>
public RotationMatrix RotationMatrixTranspose()
{
// start with an identity matrix.
RotationMatrix R = new RotationMatrix();
float angle = (float)Math.Sqrt(_x * _x + _y * _y + _z * _z);
// Identity is all that can be deduced from zero angle
if (angle == 0)
{
return(R);
}
float c = (float)Math.Cos(angle) - 1.0f;
float s = (float)Math.Sin(angle);
// normalize axis to a unit vector u
float norm = 1.0f / angle;
float nx = _x * norm;
float ny = _y * norm;
float nz = _z * norm;
// add (cos(angle)-1)*(1 - u u').
R[0, 0] += c * (nx * nx - 1.0f);
R[0, 1] += c * nx * ny;
R[0, 2] += c * nx * nz;
R[1, 0] += c * ny * nx;
R[1, 1] += c * (ny * ny - 1.0f);
R[1, 2] += c * ny * nz;
R[2, 0] += c * nz * nx;
R[2, 1] += c * nz * ny;
R[2, 2] += c * (nz * nz - 1.0f);
// add sin(angle) * [u]
R[0, 1] += s * nz;
R[0, 2] -= s * ny;
R[1, 0] -= s * nz;
R[1, 2] += s * nx;
R[2, 0] += s * ny;
R[2, 1] -= s * nx;
return(R);
}
public RotationMatrix(RotationMatrix R) : base(3, 3)
{
this.Equals(R);
}
public Quaternion(RotationMatrix R)
{
Set(R);
}
// @returns the rotation matrix corresponding to the inverse rotation by this
// quaternion.
public RotationMatrix RotationMatrixTranspose()
{
RotationMatrix R = new RotationMatrix();
float x2 = _x * _x;
float y2 = _y * _y;
float z2 = _z * _z;
float r2 = _r * _r;
R[0,0] = r2 + x2 - y2 - z2; // fill diagonal terms
R[1,1] = r2 - x2 + y2 - z2;
R[2,2] = r2 - x2 - y2 + z2;
float xy = _x * _y;
float yz = _y * _z;
float zx = _z * _x;
float rx = _r * _x;
float ry = _r * _y;
float rz = _r * _z;
R[0,1] = 2 * (xy + rz); // fill off diagonal terms
R[0,2] = 2 * (zx - ry);
R[1,2] = 2 * (yz + rx);
R[1,0] = 2 * (xy - rz);
R[2,0] = 2 * (zx + ry);
R[2,1] = 2 * (yz - rx);
return(R);
}
//param R the rotation matrix from which the quaternion will be extracted
public void Set(RotationMatrix R)
{
float d0 = R[0,0], d1 = R[1,1], d2 = R[2,2];
// The trace determines the method of decomposition
float rr = 1.0f + d0 + d1 + d2;
if (rr > 0)
{
float s = 0.5f / (float)Math.Sqrt(rr);
_x = (R[2,1] - R[1,2]) * s;
_y = (R[0,2] - R[2,0]) * s;
_z = (R[1,0] - R[0,1]) * s;
_r = 0.25f / s;
}
else
{
// Trace is less than zero, so need to determine which
// major diagonal is largest
if ((d0 > d1) && (d0 > d2))
{
float s = 0.5f / (float)Math.Sqrt(1 + d0 - d1 - d2);
_x = 0.5f * s;
_y = (R[0,1] + R[1,0]) * s;
_z = (R[0,2] + R[2,0]) * s;
_r = (R[1,2] + R[2,1]) * s;
}
else
if (d1 > d2)
{
float s = 0.5f / (float)Math.Sqrt(1 + d0 - d1 - d2);
_x = (R[0,1] + R[1,0]) * s;
_y = 0.5f * s;
_z = (R[1,2] + R[2,1]) * s;
_r = (R[0,2] + R[2,0]) * s;
}
else
{
float s = 0.5f / (float)Math.Sqrt(1 + d0 - d1 - d2);
_x = (R[0,2] + R[2,0]) * s;
_y = (R[1,2] + R[2,1]) * s;
_z = 0.5f * s;
_r = (R[0,1] + R[1,0]) * s;
}
}
}
public Quaternion(RotationMatrix R)
{
Set(R);
}
public override MatrixFixed Transpose()
{
RotationMatrix R = new RotationMatrix();
R[0, 0] = this[0, 0]; R[0, 1] = this[1, 0]; R[0, 2] = this[2, 0];
R[1, 0] = this[0, 1]; R[1, 1] = this[1, 1]; R[1, 2] = this[2, 1];
R[2, 0] = this[0, 2]; R[2, 1] = this[1, 2]; R[2, 2] = this[2, 2];
return R;
}
public RotationMatrix(RotationMatrix R) : base(3,3)
{
this.Equals(R);
}
public AngleAxis(RotationMatrix R)
{
Set(R);
}
public AngleAxis(RotationMatrix R)
{
Set(R);
}
/// <summary>
///
/// </summary>
/// <returns>the rotation matrix corresponding to the inverse rotation by this quaternion</returns>
public RotationMatrix RotationMatrixTranspose()
{
// start with an identity matrix.
RotationMatrix R = new RotationMatrix();
float angle = (float)Math.Sqrt(_x * _x + _y * _y + _z * _z);
// Identity is all that can be deduced from zero angle
if (angle == 0)
return R;
float c = (float)Math.Cos(angle) - 1.0f;
float s = (float)Math.Sin(angle);
// normalize axis to a unit vector u
float norm = 1.0f/angle;
float nx = _x*norm;
float ny = _y*norm;
float nz = _z*norm;
// add (cos(angle)-1)*(1 - u u').
R[0,0] += c* (nx*nx - 1.0f);
R[0,1] += c* nx*ny;
R[0,2] += c* nx*nz;
R[1,0] += c* ny*nx;
R[1,1] += c* (ny*ny - 1.0f);
R[1,2] += c* ny*nz;
R[2,0] += c* nz*nx;
R[2,1] += c* nz*ny;
R[2,2] += c* (nz*nz - 1.0f);
// add sin(angle) * [u]
R[0,1] += s*nz;
R[0,2] -= s*ny;
R[1,0] -= s*nz;
R[1,2] += s*nx;
R[2,0] += s*ny;
R[2,1] -= s*nx;
return(R);
}
/// <summary>
///
/// </summary>
/// <param name="R">the rotation matrix from which the quaternion will be extracted</param>
public void Set(RotationMatrix R)
{
// The trace determines the angle
float diag = 0.5f*(R[0,0] + R[1,1] + R[2,2] - 1.0f);
if (diag >= 1.0)
{
// zero angle
_x = _y = _z = 0.0f;
return;
}
else
{
float angle = (float)Math.Acos(diag);
_x = R[2,1] - R[1,2];
_y = R[0,2] - R[2,0];
_z = R[1,0] - R[0,1];
float axisnorm = (float)Math.Sqrt(_x * _x + _y * _y + _z * _z);
if (axisnorm < 1e-8)
{
// Axis is undefined
_x = _y = _z = 0.0f;
return;
}
else
{
// All OK, so scale it
float scale = angle/axisnorm;
_x *= scale;
_y *= scale;
_z *= scale;
}
}
}
