public static SourceQuaternion MatrixQuaternion(SourceMatrix3x4 mat)
{
SourceQuaternion tmp = new SourceQuaternion();
MatrixQuaternion(mat, ref tmp);
return(tmp);
}
public static void MatrixQuaternion(SourceMatrix3x4 mat, ref SourceQuaternion q)
{
SourceQAngle angles = new SourceQAngle();
MatrixAngles(mat, ref angles);
AngleQuaternion(angles, ref q);
}
public static void QuaternionVectorsFLU(SourceQuaternion q, ref SourceVector pForward, ref SourceVector pLeft, ref SourceVector pUp)
{
// Note: it's pretty much identical to just computing the quaternion matrix and assigning its columns to the vectors
pForward = q.GetForward();
pLeft = q.GetLeft();
pUp = q.GetUp();
}
public static void QuaternionMatrix(SourceQuaternion q, SourceVector pos, ref SourceMatrix3x4 matrix)
{
QuaternionMatrix(q, ref matrix);
matrix[0][3] = pos.x;
matrix[1][3] = pos.y;
matrix[2][3] = pos.z;
}
public static void QuaternionAngles(SourceQuaternion q, ref SourceQAngle angles)
{
// FIXME: doing it this way calculates too much data, needs to do an optimized version...
SourceMatrix3x4 matrix = new SourceMatrix3x4();
QuaternionMatrix(q, ref matrix);
MatrixAngles(matrix, ref angles);
}
public SourceRadianEuler(SourceQuaternion q)
{
var t = this;
MathUtils.QuaternionAngles(q, ref t);
this.x = t.x;
this.y = t.y;
this.z = t.z;
}
public SourceQuaternion(SourceRadianEuler angle)
{
var t = new SourceQuaternion();
MathUtils.AngleQuaternion(angle, ref t);
this.x = t.x;
this.y = t.y;
this.z = t.z;
this.w = t.w;
}
public SourceQuaternion(SourceDegreeEuler angle)
{
SourceRadianEuler radians = new SourceRadianEuler(angle);
var t = new SourceQuaternion();
MathUtils.AngleQuaternion(radians, ref t);
this.x = t.x;
this.y = t.y;
this.z = t.z;
this.w = t.w;
}
public static void AngleQuaternion(SourceRadianEuler angles, ref SourceQuaternion outQuat)
{
float sr, sp, sy, cr, cp, cy;
SinCos(angles.z * 0.5f, out sy, out cy);
SinCos(angles.y * 0.5f, out sp, out cp);
SinCos(angles.x * 0.5f, out sr, out cr);
// NJS: for some reason VC6 wasn't recognizing the common subexpressions:
float srXcp = sr * cp, crXsp = cr * sp;
outQuat.x = srXcp * cy - crXsp * sy; // X
outQuat.y = crXsp * cy + srXcp * sy; // Y
float crXcp = cr * cp, srXsp = sr * sp;
outQuat.z = crXcp * sy - srXsp * cy; // Z
outQuat.w = crXcp * cy + srXsp * sy; // W (real component)
}
public static void QuaternionMatrix(SourceQuaternion q, ref SourceMatrix3x4 matrix)
{
// Original code
// This should produce the same code as below with optimization, but looking at the assmebly,
// it doesn't. There are 7 extra multiplies in the release build of this, go figure.
matrix[0][0] = 1.0f - 2.0f * q.y * q.y - 2.0f * q.z * q.z;
matrix[1][0] = 2.0f * q.x * q.y + 2.0f * q.w * q.z;
matrix[2][0] = 2.0f * q.x * q.z - 2.0f * q.w * q.y;
matrix[0][1] = 2.0f * q.x * q.y - 2.0f * q.w * q.z;
matrix[1][1] = 1.0f - 2.0f * q.x * q.x - 2.0f * q.z * q.z;
matrix[2][1] = 2.0f * q.y * q.z + 2.0f * q.w * q.x;
matrix[0][2] = 2.0f * q.x * q.z + 2.0f * q.w * q.y;
matrix[1][2] = 2.0f * q.y * q.z - 2.0f * q.w * q.x;
matrix[2][2] = 1.0f - 2.0f * q.x * q.x - 2.0f * q.y * q.y;
matrix[0][3] = 0.0f;
matrix[1][3] = 0.0f;
matrix[2][3] = 0.0f;
}
public void SetToIdentity()
{
m_vPosition = SourceVector.Empty;
m_orientation = SourceQuaternion.Identity;
}
public void InitFromQuaternion(SourceQuaternion orientation, SourceVector vPosition)
{
m_orientation = orientation;
m_vPosition = vPosition;
}
public void InitFromMatrix(SourceMatrix3x4 transform)
{
m_orientation = MathUtils.MatrixQuaternion(transform);
m_vPosition = transform.GetOrigin();
}
public SourceCTransform(SourceVector v, SourceQuaternion q)
{
m_vPosition = v;
m_orientation = q;
}
public static void QuaternionVectorsForward(SourceQuaternion q, ref SourceVector pForward)
{
pForward = q.GetForward();
}
public SourceDegreeEuler(SourceQuaternion q)
{
SourceRadianEuler radians = new SourceRadianEuler(q);
Init(MathUtils.RAD2DEG(radians.x), MathUtils.RAD2DEG(radians.y), MathUtils.RAD2DEG(radians.z));
}