public Vector3F GetNormal()
{
Vector3F v = GetRawNormal(this);
v.Scale(GetFixFactor());
return(v);
}
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 Vector3F GetNormalDirected(Vector3F v0)
{
Vector3F v = GetNormal();
if (v.x * v0.x + v.y * v0.y + v.z * v0.z < 0)
{
v.Scale(-1);
}
return(v);
}
public float GetThetaDirected(Vector3F vector)
{
float theta = GetTheta();
Vector3F v = GetNormal();
if (vector.x * q1 + vector.y * q2 + vector.z * q3 < 0)
{
v.Scale(-1);
theta = -theta;
}
return(theta);
}
public AxisVector3F ToAxisAngle()
{
double theta = 2 * Math.Acos(Math.Abs(q0));
double sinTheta2 = Math.Sin(theta / 2);
Vector3F v = GetNormal();
if (sinTheta2 < 0)
{
v.Scale(-1);
theta = Math.PI - theta;
}
return(new AxisVector3F(v, (float)theta));
}
public Vector4F GetThetaDirected(Vector4F axisAngle)
{
float theta = GetTheta();
Vector3F v = GetNormal();
if (axisAngle.X * q1 + axisAngle.Y * q2 + axisAngle.Z * q3 < 0)
{
v.Scale(-1);
theta = -theta;
}
axisAngle.Set(v.x, v.y, v.z, theta);
return(axisAngle);
}
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));
}
public Vector3F GetVectorScaled(int i, float scale)
{
if (i == -1)
{
scale *= GetFixFactor();
return(new Vector3F(q1 * scale, q2 * scale, q3 * scale));
}
if (mat == null)
{
SetMatrix();
}
Vector3F v = new Vector3F();
mat.GetColumn(i, v);
if (scale != 1f)
{
v.Scale(scale);
}
return(v);
}