public Quaternion Predict(Quaternion value)
{
var filteredOrientation = Quaternion.Zero;
var trend = Quaternion.Zero;
var diffJitter = Quaternion.RotationBetween(value, prevFilteredOrientation);
var diffJitterValue = Math.Abs(Quaternion.Angle(diffJitter));
if (diffJitterValue <= parameters.JitterRadius)
filteredOrientation = Quaternion.SlerpNeighborhood(prevFilteredOrientation, value,
diffJitterValue/parameters.JitterRadius);
else
filteredOrientation = value;
filteredOrientation = Quaternion.SlerpNeighborhood(filteredOrientation,
prevFilteredOrientation*prevTrend,
parameters.Smoothing);
diffJitter = Quaternion.RotationBetween(filteredOrientation, prevFilteredOrientation);
trend = Quaternion.SlerpNeighborhood(prevTrend, diffJitter,
parameters.Correction);
var predictedOrientation = filteredOrientation*
Quaternion.SlerpNeighborhood(Quaternion.Identity, trend,
parameters.Prediction);
var diff = Quaternion.RotationBetween(predictedOrientation, filteredOrientation);
var diffValue = Math.Abs(Quaternion.Angle(diff));
if (diffValue > parameters.MaxDeviationRadius)
predictedOrientation = Quaternion.SlerpNeighborhood(filteredOrientation, predictedOrientation,
parameters.MaxDeviationRadius/diffValue);
predictedOrientation = Quaternion.Normalize(predictedOrientation);
filteredOrientation = Quaternion.Normalize(filteredOrientation);
trend = Quaternion.Normalize(trend);
prevFilteredOrientation = filteredOrientation;
prevTrend = trend;
return predictedOrientation;
}
public static Quaternion Lerp(Quaternion q1, Quaternion q2, float amount)
{
var num = amount;
var num2 = 1 - num;
var ret = new Quaternion();
var num5 = q1.X*q2.X + q1.Y*q2.Y + q1.Z*q2.Z +
q1.W*q2.W;
if (num5 >= 0)
{
ret.X = num2*q1.X + num*q2.X;
ret.Y = num2*q1.Y + num*q2.Y;
ret.Z = num2*q1.Z + num*q2.Z;
ret.W = num2*q1.W + num*q2.W;
}
else
{
ret.X = num2*q1.X - num*q2.X;
ret.Y = num2*q1.Y - num*q2.Y;
ret.Z = num2*q1.Z - num*q2.Z;
ret.W = num2*q1.W - num*q2.W;
}
var num4 = ret.X*ret.X + ret.Y*ret.Y + ret.Z*ret.Z +
ret.W*ret.W;
var num3 = (float) (1/Math.Sqrt(num4));
ret.X *= num3;
ret.Y *= num3;
ret.Z *= num3;
ret.W *= num3;
return ret;
}
public static float LengthSquared(Quaternion q)
{
return q.X*q.X + q.Y*q.Y + q.Z*q.Z + q.W*q.W;
}
public static void Transform(ref Vector3 v, ref Quaternion q, out Vector3 result)
{
var x = 2*(q.Y*v.Z - q.Z*v.Y);
var y = 2*(q.Z*v.X - q.X*v.Z);
var z = 2*(q.X*v.Y - q.Y*v.X);
result.X = v.X + x*q.W + q.Y*z - q.Z*y;
result.Y = v.Y + y*q.W + q.Z*x - q.X*z;
result.Z = v.Z + z*q.W + q.X*y - q.Y*x;
}
public static Vector3 ToEuler(Quaternion q)
{
var sqw = q.W*q.W;
var sqx = q.X*q.X;
var sqy = q.Y*q.Y;
var sqz = q.Z*q.Z;
var unit = sqx + sqy + sqz + sqw;
var test = q.X*q.W - q.Y*q.Z;
Vector3 ret;
if (test > 0.499f*unit)
{
ret.Y = 2*(float) Math.Atan2(q.Y, q.X);
ret.X = MathHelper.PiOver2;
ret.Z = 0;
return ret*MathHelper.Radians2Degrees;
}
if (test < -0.499f*unit)
{
ret.Y = -2*(float) Math.Atan2(q.Y, q.X);
ret.X = -MathHelper.PiOver2;
ret.Z = 0;
return ret*MathHelper.Radians2Degrees;
}
q = new Quaternion(q.W, q.Z, q.X, q.Y);
ret.Y = (float) Math.Atan2(2*q.X*q.W + 2*q.Y*q.Z, 1 - 2*(q.Z*q.Z + q.W*q.W));
ret.X = (float) Math.Asin(2*(q.X*q.Z - q.W*q.Y));
ret.Z = (float) Math.Atan2(2*q.X*q.Y + 2*q.Z*q.W, 1 - 2*(q.Y*q.Y + q.Z*q.Z));
return ret*MathHelper.Radians2Degrees;
}
public static Quaternion SlerpNeighborhood(Quaternion q1, Quaternion q2, float amount)
{
q2 = EnsureNeighborhood(q1, q2);
return Slerp(q1, q2, amount);
}
public static Quaternion RotationBetween(Quaternion q1, Quaternion q2)
{
q2 = EnsureNeighborhood(q1, q2);
return Multiply(Inverse(q1), q2);
}
public static Quaternion Negate(Quaternion q)
{
Quaternion ret;
ret.X = -q.X;
ret.Y = -q.Y;
ret.Z = -q.Z;
ret.W = -q.W;
return ret;
}
public static Quaternion Constrain(Quaternion q, Vector3 minAngles, Vector3 maxAngles)
{
var e = ToEuler(q);
e.X = MathHelper.Clamp(e.X, minAngles.X, maxAngles.X);
e.Y = MathHelper.Clamp(e.Y, minAngles.Y, maxAngles.Y);
e.Z = MathHelper.Clamp(e.Z, minAngles.Z, maxAngles.Z);
return FromEuler(e);
}
public static Quaternion Average(params Quaternion[] values)
{
if (values == null || values.Length == 0)
throw new Exception("Empty values list!");
var amount = 1/(float) values.Length;
var ret = new Quaternion();
for (var i = 0; i < values.Length; i++)
{
var crt = values[i];
if (!AreClose(crt, ret))
crt = Negate(crt);
ret.X += crt.X*amount;
ret.Y += crt.Y*amount;
ret.Z += crt.Z*amount;
ret.W += crt.W*amount;
}
return ret;
}
public static bool AreClose(Quaternion q1, Quaternion q2)
{
return Dot(q1, q2) >= 0;
}
public static float Angle(Quaternion q)
{
q = Normalize(q);
return (float) (2*Math.Acos(q.W))*MathHelper.Radians2Degrees;
}
public MoCapBoneCalibrationUnit(Quaternion unit)
{
Unit = unit;
}
public static Quaternion Multiply(Quaternion q1, Quaternion q2)
{
Quaternion ret;
var x = q1.X;
var y = q1.Y;
var z = q1.Z;
var w = q1.W;
var num4 = q2.X;
var num3 = q2.Y;
var num2 = q2.Z;
var num = q2.W;
var num12 = y*num2 - z*num3;
var num11 = z*num4 - x*num2;
var num10 = x*num3 - y*num4;
var num9 = x*num4 + y*num3 + z*num2;
ret.X = x*num + num4*w + num12;
ret.Y = y*num + num3*w + num11;
ret.Z = z*num + num2*w + num10;
ret.W = w*num - num9;
return ret;
}
public static Quaternion Divide(Quaternion q1, Quaternion q2)
{
Quaternion ret;
var x = q1.X;
var y = q1.Y;
var z = q1.Z;
var w = q1.W;
var num14 = q2.X*q2.X + q2.Y*q2.Y + q2.Z*q2.Z +
q2.W*q2.W;
var num5 = 1/num14;
var num4 = -q2.X*num5;
var num3 = -q2.Y*num5;
var num2 = -q2.Z*num5;
var num = q2.W*num5;
var num13 = y*num2 - z*num3;
var num12 = z*num4 - x*num2;
var num11 = x*num3 - y*num4;
var num10 = x*num4 + y*num3 + z*num2;
ret.X = x*num + num4*w + num13;
ret.Y = y*num + num3*w + num12;
ret.Z = z*num + num2*w + num11;
ret.W = w*num - num10;
return ret;
}
public static Quaternion Multiply(Quaternion q, float scalar)
{
Quaternion ret;
ret.X = q.X*scalar;
ret.Y = q.Y*scalar;
ret.Z = q.Z*scalar;
ret.W = q.W*scalar;
return ret;
}
public static float Dot(Quaternion q1, Quaternion q2)
{
return q1.X*q2.X + q1.Y*q2.Y + q1.Z*q2.Z + q1.W*q2.W;
}
public static Quaternion Normalize(Quaternion q)
{
Quaternion ret;
var num2 = q.X*q.X + q.Y*q.Y + q.Z*q.Z + q.W*q.W;
var num = (float) (1/Math.Sqrt(num2));
ret.X = q.X*num;
ret.Y = q.Y*num;
ret.Z = q.Z*num;
ret.W = q.W*num;
return ret;
}
public static Quaternion EnsureNeighborhood(Quaternion q1, Quaternion q2)
{
if (!AreClose(q1, q2))
return Negate(q2);
return q2;
}
public static Quaternion Slerp(Quaternion q1, Quaternion q2, float amount)
{
float num2;
float num3;
Quaternion ret;
var num = amount;
var num4 = q1.X*q2.X + q1.Y*q2.Y + q1.Z*q2.Z +
q1.W*q2.W;
var flag = false;
if (num4 < 0)
{
flag = true;
num4 = -num4;
}
if (num4 > 0.999999f)
{
num3 = 1 - num;
num2 = flag ? -num : num;
}
else
{
var num5 = (float) Math.Acos(num4);
var num6 = (float) (1/Math.Sin(num5));
num3 = (float) (Math.Sin((1 - num)*num5)*num6);
num2 = flag ? (float) (-Math.Sin(num*num5)*num6) : (float) (Math.Sin(num*num5)*num6);
}
ret.X = num3*q1.X + num2*q2.X;
ret.Y = num3*q1.Y + num2*q2.Y;
ret.Z = num3*q1.Z + num2*q2.Z;
ret.W = num3*q1.W + num2*q2.W;
return ret;
}
public static Quaternion Inverse(Quaternion q)
{
Quaternion ret;
var num2 = q.X*q.X + q.Y*q.Y + q.Z*q.Z + q.W*q.W;
var num = 1/num2;
ret.X = -q.X*num;
ret.Y = -q.Y*num;
ret.Z = -q.Z*num;
ret.W = q.W*num;
return ret;
}
public static Quaternion Subtract(Quaternion q1, Quaternion q2)
{
Quaternion ret;
ret.X = q1.X - q2.X;
ret.Y = q1.Y - q2.Y;
ret.Z = q1.Z - q2.Z;
ret.W = q1.W - q2.W;
return ret;
}
public static float Length(Quaternion q)
{
var num = q.X*q.X + q.Y*q.Y + q.Z*q.Z + q.W*q.W;
return (float) Math.Sqrt(num);
}
public static Quaternion Add(Quaternion q1, Quaternion q2)
{
Quaternion ret;
ret.X = q1.X + q2.X;
ret.Y = q1.Y + q2.Y;
ret.Z = q1.Z + q2.Z;
ret.W = q1.W + q2.W;
return ret;
}
public static Vector3 Transform(Vector3 v, Quaternion q)
{
var x = 2*(q.Y*v.Z - q.Z*v.Y);
var y = 2*(q.Z*v.X - q.X*v.Z);
var z = 2*(q.X*v.Y - q.Y*v.X);
var ret = new Vector3
{
X = v.X + x*q.W + q.Y*z - q.Z*y,
Y = v.Y + y*q.W + q.Z*x - q.X*z,
Z = v.Z + z*q.W + q.X*y - q.Y*x
};
return ret;
}
