private static Quat SimpleAverage(Quat[] ndata)
{
Vector3F mean = new Vector3F(0, 0, 1);
// using the directed normal ensures that the mean is
// continually added to and never subtracted from
Vector3F v = ndata[0].GetNormal();
mean.Add(v);
for (int i = ndata.Length; --i >= 0;)
{
mean.Add(ndata[i].GetNormalDirected(mean));
}
mean.Subtract(v);
mean.Normalize();
float f = 0;
// the 3D projection of the quaternion is [sin(theta/2)]*n
// so dotted with the normalized mean gets us an approximate average for sin(theta/2)
for (int i = ndata.Length; --i >= 0;)
{
f += Math.Abs(ndata[i].Get3DProjection(v).Dot(mean));
}
if (f != 0)
{
mean.Scale(f / ndata.Length);
}
// now convert f to the corresponding cosine instead of sine
f = (float)Math.Sqrt(1 - mean.NormSquared());
if (float.IsNaN(f))
{
f = 0;
}
return(new Quat(new Vector4F(mean.x, mean.y, mean.z, f)));
}
private static Quat NewMean(Quat[] data, Quat mean)
{
Vector3F sum = new Vector3F();
for (int i = data.Length; --i >= 0;)
{
Quat q = data[i];
Quat dq = q.Divide(mean);
Vector3F v = dq.GetNormal();
v.Scale(dq.GetTheta());
sum.Add(v);
}
sum.Scale(1f / data.Length);
Quat dqMean = new Quat(sum, sum.Norm());
return(dqMean.Multiply(mean));
}