public void Yaw(float angle)
{
LLQuaternion q = new LLQuaternion(angle, zAxis);
LLMatrix3 m = new LLMatrix3(q);
Rotate(m);
if (!xAxis.IsFinite() || !yAxis.IsFinite())
throw new Exception("Non-finite in CoordinateFrame.Yaw()");
}
public void Rotate(LLQuaternion q)
{
LLMatrix3 m = new LLMatrix3(q);
Rotate(m);
}
public void Rotate(LLMatrix3 m)
{
xAxis = LLVector3.Rot(xAxis, m);
yAxis = LLVector3.Rot(yAxis, m);
Orthonormalize();
if (!IsFinite())
throw new Exception("Non-finite in CoordinateFrame.Rotate()");
}
public CoordinateFrame(LLVector3 origin, LLMatrix3 rotation)
{
this.origin = origin;
xAxis = rotation[0];
yAxis = rotation[1];
zAxis = rotation[2];
if (!IsFinite())
throw new ArgumentException("Non-finite in CoordinateFrame constructor");
}
public CoordinateFrame(LLVector3 origin, LLQuaternion rotation)
{
LLMatrix3 m = new LLMatrix3(rotation);
this.origin = origin;
xAxis = m[0];
yAxis = m[1];
zAxis = m[2];
if (!IsFinite())
throw new ArgumentException("Non-finite in CoordinateFrame constructor");
}
public static LLMatrix3 Transpose(LLMatrix3 m)
{
LLMatrix3 t = new LLMatrix3(m);
t.Transpose();
return t;
}
public static LLVector3 Rot(LLVector3 vector, LLMatrix3 rotation)
{
return vector * rotation;
}
public static LLMatrix3 Multiply(LLMatrix3 left, LLMatrix3 right)
{
return new LLMatrix3(
left.M11 * right.M11 + left.M12 * right.M21 + left.M13 * right.M31,
left.M11 * right.M12 + left.M12 * right.M22 + left.M13 * right.M32,
left.M11 * right.M13 + left.M12 * right.M23 + left.M13 * right.M33,
left.M21 * right.M11 + left.M22 * right.M21 + left.M23 * right.M31,
left.M21 * right.M12 + left.M22 * right.M22 + left.M23 * right.M32,
left.M21 * right.M13 + left.M22 * right.M23 + left.M23 * right.M33,
left.M31 * right.M11 + left.M32 * right.M21 + left.M33 * right.M31,
left.M31 * right.M12 + left.M32 * right.M22 + left.M33 * right.M32,
left.M31 * right.M13 + left.M32 * right.M23 + left.M33 * right.M33
);
}
public static LLVector3 Transform(LLVector3 vector, LLMatrix3 matrix)
{
// Operates "from the right" on row vector
return new LLVector3(
vector.X * matrix.M11 + vector.Y * matrix.M21 + vector.Z * matrix.M31,
vector.X * matrix.M12 + vector.Y * matrix.M22 + vector.Z * matrix.M32,
vector.X * matrix.M13 + vector.Y * matrix.M23 + vector.Z * matrix.M33
);
}
public static LLMatrix3 Subtract(LLMatrix3 left, LLMatrix3 right)
{
return new LLMatrix3(
left.M11 - right.M11, left.M12 - right.M12, left.M13 - right.M13,
left.M21 - right.M21, left.M22 - right.M22, left.M23 - right.M23,
left.M31 - right.M31, left.M32 - right.M32, left.M33 - right.M33
);
}
public static LLMatrix3 Subtract(LLMatrix3 matrix, float scalar)
{
return new LLMatrix3(
matrix.M11 - scalar, matrix.M12 - scalar, matrix.M13 - scalar,
matrix.M21 - scalar, matrix.M22 - scalar, matrix.M23 - scalar,
matrix.M31 - scalar, matrix.M32 - scalar, matrix.M33 - scalar
);
}
public static LLMatrix3 Add(LLMatrix3 matrix, float scalar)
{
return new LLMatrix3(
matrix.M11 + scalar, matrix.M12 + scalar, matrix.M13 + scalar,
matrix.M21 + scalar, matrix.M22 + scalar, matrix.M23 + scalar,
matrix.M31 + scalar, matrix.M32 + scalar, matrix.M33 + scalar
);
}
public static LLMatrix3 Add(LLMatrix3 left, LLMatrix3 right)
{
return new LLMatrix3(
left.M11 + right.M11, left.M12 + right.M12, left.M13 + right.M13,
left.M21 + right.M21, left.M22 + right.M22, left.M23 + right.M23,
left.M31 + right.M31, left.M32 + right.M32, left.M33 + right.M33
);
}
public LLMatrix3(LLMatrix3 m)
{
M11 = m.M11;
M12 = m.M12;
M13 = m.M13;
M21 = m.M21;
M22 = m.M22;
M23 = m.M23;
M31 = m.M31;
M32 = m.M32;
M33 = m.M33;
}
public static LLMatrix3 Orthogonalize(LLMatrix3 m)
{
LLVector3 xAxis = m[0];
LLVector3 yAxis = m[1];
LLVector3 zAxis = m[2];
xAxis = LLVector3.Norm(xAxis);
yAxis -= xAxis * (xAxis * yAxis);
yAxis = LLVector3.Norm(yAxis);
zAxis = LLVector3.Cross(xAxis, yAxis);
m[0] = xAxis;
m[1] = yAxis;
m[2] = zAxis;
return m;
}
/// <summary>
/// Transposes a matrix
/// </summary>
/// <param name="m">Matrix to transpose</param>
/// <returns>Transposed matrix</returns>
public static LLMatrix3 Transpose(LLMatrix3 m)
{
Helpers.Swap<float>(ref m.M12, ref m.M21);
Helpers.Swap<float>(ref m.M13, ref m.M31);
Helpers.Swap<float>(ref m.M23, ref m.M32);
return m;
}
