//Get an average (mean) from more then two quaternions (with two, slerp would be used).
//Note: this only works if all the quaternions are relatively close together.
//Usage:
//-Cumulative is an external Vector4 which holds all the added x y z and w components.
//-newRotation is the next rotation to be added to the average pool
//-firstRotation is the first quaternion of the array to be averaged
//-addAmount holds the total amount of quaternions which are currently added
//This function returns the current average quaternion
public static Quaternion AverageQuaternion(ref Vector4 cumulative, Quaternion newRotation, Quaternion firstRotation, int addAmount)
{
float w = 0.0f;
float x = 0.0f;
float y = 0.0f;
float z = 0.0f;
//Before we add the new rotation to the average (mean), we have to check whether the quaternion has to be inverted. Because
//q and -q are the same rotation, but cannot be averaged, we have to make sure they are all the same.
if (!ExtQuaternion.IsClose(newRotation, firstRotation))
{
newRotation = ExtQuaternion.InverseSignQuaternion(newRotation);
}
//Average the values
float addDet = 1f / (float)addAmount;
cumulative.w += newRotation.w;
w = cumulative.w * addDet;
cumulative.x += newRotation.x;
x = cumulative.x * addDet;
cumulative.y += newRotation.y;
y = cumulative.y * addDet;
cumulative.z += newRotation.z;
z = cumulative.z * addDet;
//note: if speed is an issue, you can skip the normalization step
return(ExtQuaternion.NormalizeQuaternion(x, y, z, w));
}
public static void GetShortestAngleAxisBetween(Quaternion a, Quaternion b, out Vector3 axis, out float angle)
{
var dq = Quaternion.Inverse(a) * b;
if (dq.w > 1)
{
dq = ExtQuaternion.Normalize(dq);
}
//get as doubles for precision
var qw = (double)dq.w;
var qx = (double)dq.x;
var qy = (double)dq.y;
var qz = (double)dq.z;
var ratio = System.Math.Sqrt(1.0d - qw * qw);
angle = (float)(2.0d * System.Math.Acos(qw)) * Mathf.Rad2Deg;
if (ratio < 0.001d)
{
axis = new Vector3(1f, 0f, 0f);
}
else
{
axis = new Vector3(
(float)(qx / ratio),
(float)(qy / ratio),
(float)(qz / ratio));
axis.Normalize();
}
}
public static void GetAngleAxis(this Quaternion q, out Vector3 axis, out float angle)
{
if (q.w > 1)
{
q = ExtQuaternion.Normalize(q);
}
//get as doubles for precision
var qw = (double)q.w;
var qx = (double)q.x;
var qy = (double)q.y;
var qz = (double)q.z;
var ratio = System.Math.Sqrt(1.0d - qw * qw);
angle = (float)(2.0d * System.Math.Acos(qw)) * Mathf.Rad2Deg;
if (ratio < 0.001d)
{
axis = new Vector3(1f, 0f, 0f);
}
else
{
axis = new Vector3(
(float)(qx / ratio),
(float)(qy / ratio),
(float)(qz / ratio));
axis.Normalize();
}
}
//Rotate a vector as if it is attached to an object with rotation "from", which is then rotated to rotation "to".
//Similar to TransformWithParent(), but rotating a vector instead of a transform.
public static Vector3 RotateVectorFromTo(Quaternion from, Quaternion to, Vector3 vector)
{
//Note: comments are in case all inputs are in World Space.
Quaternion Q = ExtQuaternion.SubtractRotation(to, from); //Output is in object space.
Vector3 A = ExtQuaternion.InverseTransformDirectionMath(from, vector); //Output is in object space.
Vector3 B = Q * A; //Output is in local space.
Vector3 C = ExtQuaternion.TransformDirectionMath(from, B); //Output is in world space.
return(C);
}
