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)));
}
public static Quat GetQuaternionFrameV(Vector3F vA, Vector3F vB, Vector3F vC, bool yBased)
{
if (vC == null)
{
vC = new Vector3F();
vC.Cross(vA, vB);
if (yBased)
{
vA.Cross(vB, vC);
}
}
Vector3F vBprime = new Vector3F();
vBprime.Cross(vC, vA);
vA.Normalize();
vBprime.Normalize();
vC.Normalize();
Matrix3X3F mat = new Matrix3X3F();
mat.SetColumn(0, vA);
mat.SetColumn(1, vBprime);
mat.SetColumn(2, vC);
Quat q = new Quat(mat);
return(q);
}
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);
}