private static Vector3F GetRawNormal(Quat q)
{
Vector3F v = new Vector3F(q.q1, q.q2, q.q3);
if (v.Norm() == 0)
{
return(new Vector3F(0, 0, 1));
}
v.Normalize();
return(v);
}
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));
}