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);
}
public void Multiply(Matrix3X3F m1, Matrix3X3F m2)
{
Set(m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20, m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21,
m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22, m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20,
m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21, m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22,
m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20, m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21,
m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22);
}
public override bool Equals(object o)
{
if (!(o is Matrix3X3F))
{
return(false);
}
Matrix3X3F m = (Matrix3X3F)o;
return(m00 == m.m00 && m01 == m.m01 && m02 == m.m02 && m10 == m.m10 && m11 == m.m11 && m12 == m.m12 && m20 == m.m20 &&
m21 == m.m21 && m22 == m.m22);
}
private void SetImpl(Matrix3X3F m1)
{
m00 = m1.m00;
m01 = m1.m01;
m02 = m1.m02;
m10 = m1.m10;
m11 = m1.m11;
m12 = m1.m12;
m20 = m1.m20;
m21 = m1.m21;
m22 = m1.m22;
}
public void Subtract(Matrix3X3F m1)
{
m00 -= m1.m00;
m01 -= m1.m01;
m02 -= m1.m02;
m10 -= m1.m10;
m11 -= m1.m11;
m12 -= m1.m12;
m20 -= m1.m20;
m21 -= m1.m21;
m22 -= m1.m22;
}
public void Add(Matrix3X3F m1)
{
m00 += m1.m00;
m01 += m1.m01;
m02 += m1.m02;
m10 += m1.m10;
m11 += m1.m11;
m12 += m1.m12;
m20 += m1.m20;
m21 += m1.m21;
m22 += m1.m22;
}
private void SetMatrix()
{
mat = new Matrix3X3F {
m00 = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3,
m01 = 2 * q1 * q2 - 2 * q0 * q3,
m02 = 2 * q1 * q3 + 2 * q0 * q2,
m10 = 2 * q1 * q2 + 2 * q0 * q3,
m11 = q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3,
m12 = 2 * q2 * q3 - 2 * q0 * q1,
m20 = 2 * q1 * q3 - 2 * q0 * q2,
m21 = 2 * q2 * q3 + 2 * q0 * q1,
m22 = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3
};
}
public Matrix3X3F(Matrix3X3F m1)
{
if (m1 == null)
{
SetScale(1);
return;
}
m00 = m1.m00;
m01 = m1.m01;
m02 = m1.m02;
m10 = m1.m10;
m11 = m1.m11;
m12 = m1.m12;
m20 = m1.m20;
m21 = m1.m21;
m22 = m1.m22;
}
private void Set(Matrix3X3F mat1)
{
mat = mat1;
double trace = mat1.m00 + mat1.m11 + mat1.m22;
double temp;
double w, x, y, z;
if (trace >= 0.5)
{
w = Math.Sqrt(1.0 + trace);
x = (mat1.m21 - mat1.m12) / w;
y = (mat1.m02 - mat1.m20) / w;
z = (mat1.m10 - mat1.m01) / w;
}
else if ((temp = mat1.m00 + mat1.m00 - trace) >= 0.5)
{
x = Math.Sqrt(1.0 + temp);
w = (mat1.m21 - mat1.m12) / x;
y = (mat1.m10 + mat1.m01) / x;
z = (mat1.m20 + mat1.m02) / x;
}
else if ((temp = mat1.m11 + mat1.m11 - trace) >= 0.5 || mat1.m11 > mat1.m22)
{
y = Math.Sqrt(1.0 + temp);
w = (mat1.m02 - mat1.m20) / y;
x = (mat1.m10 + mat1.m01) / y;
z = (mat1.m21 + mat1.m12) / y;
}
else
{
z = Math.Sqrt(1.0 + mat1.m22 + mat1.m22 - trace);
w = (mat1.m10 - mat1.m01) / z;
x = (mat1.m20 + mat1.m02) / z; // was -
y = (mat1.m21 + mat1.m12) / z; // was -
}
q0 = (float)(w * 0.5);
q1 = (float)(x * 0.5);
q2 = (float)(y * 0.5);
q3 = (float)(z * 0.5);
}
public Quat(Matrix3X3F mat)
{
Set(new Matrix3X3F(mat));
}
public void Multiply(Matrix3X3F m1)
{
Multiply(this, m1);
}
public void Set(Matrix3X3F m1)
{
SetImpl(m1);
}
public void Invert(Matrix3X3F m1)
{
SetImpl(m1);
Invert();
}
public void Transpose(Matrix3X3F m1)
{
SetImpl(m1);
TransposeImpl();
}
public void Set(Matrix3X3F m1)
{
SetFromMat(m1.m00, m1.m01, m1.m02, m1.m10, m1.m11, m1.m12, m1.m20, m1.m21, m1.m22);
}
