public static void IRotateAABB(SourceMatrix3x4 transform, SourceVector vecMinsIn, SourceVector vecMaxsIn, ref SourceVector vecMinsOut, ref SourceVector vecMaxsOut)
{
throw new NotImplementedException(); //can't find some of methods
//SourceVector oldCenter;
//VectorAdd(vecMinsIn, vecMaxsIn, oldCenter);
//oldCenter *= 0.5f;
//SourceVector oldExtents;
//VectorSubtract(vecMaxsIn, oldCenter, oldExtents);
//SourceVector newCenter;
//VectorIRotate(oldCenter, transform, newCenter);
//SourceVector newExtents;
//newExtents.x = FloatMakePositive(oldExtents.x * transform[0][0]) +
// FloatMakePositive(oldExtents.y * transform[1][0]) +
// FloatMakePositive(oldExtents.z * transform[2][0]);
//newExtents.y = FloatMakePositive(oldExtents.x * transform[0][1]) +
// FloatMakePositive(oldExtents.y * transform[1][1]) +
// FloatMakePositive(oldExtents.z * transform[2][1]);
//newExtents.z = FloatMakePositive(oldExtents.x * transform[0][2]) +
// FloatMakePositive(oldExtents.y * transform[1][2]) +
// FloatMakePositive(oldExtents.z * transform[2][2]);
//VectorSubtract(newCenter, newExtents, vecMinsOut);
//VectorAdd(newCenter, newExtents, vecMaxsOut);
}
public SourceMatrix3x4 ToMatrix()
{
SourceMatrix3x4 mat = new SourceMatrix3x4();
mat.InitFromQuaternion(this);
return(mat);
}
public static void MatrixInvert(SourceMatrix3x4 _in, ref SourceMatrix3x4 _out)
{
if (_in == _out)
{
V_swap(ref _out[0][1], ref _out[1][0]);
V_swap(ref _out[0][2], ref _out[2][0]);
V_swap(ref _out[1][2], ref _out[2][1]);
}
else
{
// transpose the matrix
_out[0][0] = _in[0][0];
_out[0][1] = _in[1][0];
_out[0][2] = _in[2][0];
_out[1][0] = _in[0][1];
_out[1][1] = _in[1][1];
_out[1][2] = _in[2][1];
_out[2][0] = _in[0][2];
_out[2][1] = _in[1][2];
_out[2][2] = _in[2][2];
}
// now fix up the translation to be in the other space
float[] tmp = new float[3];
tmp[0] = _in[0][3];
tmp[1] = _in[1][3];
tmp[2] = _in[2][3];
_out[0][3] = -DotProduct(tmp, _out[0]);
_out[1][3] = -DotProduct(tmp, _out[1]);
_out[2][3] = -DotProduct(tmp, _out[2]);
}
public static void AngleMatrix(SourceQAngle angles, ref SourceMatrix3x4 matrix)
{
float sr, sp, sy, cr, cp, cy;
SinCos(DEG2RAD(angles[1]), out sy, out cy);
SinCos(DEG2RAD(angles[0]), out sp, out cp);
SinCos(DEG2RAD(angles[2]), out sr, out cr);
// matrix = (YAW * PITCH) * ROLL
matrix[0][0] = cp * cy;
matrix[1][0] = cp * sy;
matrix[2][0] = -sp;
// NOTE: Do not optimize this to reduce multiplies! optimizer bug will screw this up.
matrix[0][1] = sr * sp * cy + cr * -sy;
matrix[1][1] = sr * sp * sy + cr * cy;
matrix[2][1] = sr * cp;
matrix[0][2] = (cr * sp * cy + -sr * -sy);
matrix[1][2] = (cr * sp * sy + -sr * cy);
matrix[2][2] = cr * cp;
matrix[0][3] = 0.0f;
matrix[1][3] = 0.0f;
matrix[2][3] = 0.0f;
}
public static SourceCTransform MatrixTransform(SourceMatrix3x4 _in)
{
SourceCTransform _out = new SourceCTransform();
MatrixTransform(_in, ref _out);
return(_out);
}
public static void ITransformAABB(SourceMatrix3x4 transform, SourceVector vecMinsIn, SourceVector vecMaxsIn, ref SourceVector vecMinsOut, ref SourceVector vecMaxsOut)
{
throw new NotImplementedException(); //can't find some of methods
// SourceVector worldCenter;
// VectorAdd(vecMinsIn, vecMaxsIn, worldCenter );
// worldCenter *= 0.5f;
// SourceVector worldExtents;
// VectorSubtract(vecMaxsIn, worldCenter, worldExtents );
// Vector localCenter;
// VectorITransform(worldCenter, transform, localCenter );
// Vector localExtents;
// localExtents.x = FloatMakePositive(worldExtents.x* transform[0][0] ) +
//FloatMakePositive(worldExtents.y* transform[1][0] ) +
//FloatMakePositive(worldExtents.z* transform[2][0] );
// localExtents.y = FloatMakePositive(worldExtents.x* transform[0][1] ) +
//FloatMakePositive(worldExtents.y* transform[1][1] ) +
//FloatMakePositive(worldExtents.z* transform[2][1] );
// localExtents.z = FloatMakePositive(worldExtents.x* transform[0][2] ) +
//FloatMakePositive(worldExtents.y* transform[1][2] ) +
//FloatMakePositive(worldExtents.z* transform[2][2] );
// VectorSubtract(localCenter, localExtents, vecMinsOut );
// VectorAdd(localCenter, localExtents, vecMaxsOut );
}
public static void MatrixQuaternion(SourceMatrix3x4 mat, ref SourceQuaternion q)
{
SourceQAngle angles = new SourceQAngle();
MatrixAngles(mat, ref angles);
AngleQuaternion(angles, ref q);
}
public static SourceQuaternion MatrixQuaternion(SourceMatrix3x4 mat)
{
SourceQuaternion tmp = new SourceQuaternion();
MatrixQuaternion(mat, ref tmp);
return(tmp);
}
public static SourceMatrix3x4 TransformMatrix(SourceCTransform _in)
{
SourceMatrix3x4 _out = new SourceMatrix3x4();
TransformMatrix(_in, ref _out);
return(_out);
}
public static void VectorIRotate(SourceVector v, SourceCTransform t, ref SourceVector _out)
{
// FIXME: Make work directly with the transform
SourceMatrix3x4 m = new SourceMatrix3x4();
TransformMatrix(t, ref m);
VectorIRotate(v, m, ref _out);
}
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 static void MatrixAngles(SourceMatrix3x4 matrix, ref SourceRadianEuler angles)
{
float[] a = new float[3];
for (int i = 0; i < 3; i++)
{
a[i] = angles[i];
}
MatrixAngles(matrix, ref a);
for (int i = 0; i < 3; i++)
{
angles[i] = a[i];
}
}
public static void MatrixAngles(SourceMatrix3x4 matrix, ref float[] angles)
{
float[] forward = new float[3];
float[] left = new float[3];
float[] up = new float[3];
//
// Extract the basis vectors from the matrix. Since we only need the Z
// component of the up vector, we don't get X and Y.
//
forward[0] = matrix[0][0];
forward[1] = matrix[1][0];
forward[2] = matrix[2][0];
left[0] = matrix[0][1];
left[1] = matrix[1][1];
left[2] = matrix[2][1];
up[2] = matrix[2][2];
float xyDist = (float)(Math.Sqrt(forward[0] * forward[0] + forward[1] * forward[1]));
// enough here to get angles?
if (xyDist > 0.001f)
{
// (yaw) y = ATAN( forward.y, forward.x ); -- in our space, forward is the X axis
angles[1] = RAD2DEG((float)Math.Atan2(forward[1], forward[0]));
// (pitch) x = ATAN( -forward.z, sqrt(forward.x*forward.x+forward.y*forward.y) );
angles[0] = RAD2DEG((float)Math.Atan2(-forward[2], xyDist));
// (roll) z = ATAN( left.z, up.z );
angles[2] = RAD2DEG((float)Math.Atan2(left[2], up[2]));
}
else // forward is mostly Z, gimbal lock-
{
// (yaw) y = ATAN( -left.x, left.y ); -- forward is mostly z, so use right for yaw
angles[1] = RAD2DEG((float)Math.Atan2(-left[0], left[1]));
// (pitch) x = ATAN( -forward.z, sqrt(forward.x*forward.x+forward.y*forward.y) );
angles[0] = RAD2DEG((float)Math.Atan2(-forward[2], xyDist));
// Assume no roll in this case as one degree of freedom has been lost (i.e. yaw == roll)
angles[2] = 0;
}
}
public static void RotateAABB(SourceMatrix3x4 transform, SourceVector vecMinsIn, SourceVector vecMaxsIn, ref SourceVector vecMinsOut, ref SourceVector vecMaxsOut)
{
throw new NotImplementedException(); //can't find some of methods
//SourceVector localCenter;
//VectorAdd(vecMinsIn, vecMaxsIn, localCenter);
//localCenter *= 0.5f;
//SourceVector localExtents;
//VectorSubtract(vecMaxsIn, localCenter, localExtents);
//SourceVector newCenter;
//VectorRotate(localCenter, transform, newCenter);
//VecSourceVectortor newExtents;
//newExtents.x = DotProductAbs(localExtents, transform[0]);
//newExtents.y = DotProductAbs(localExtents, transform[1]);
//newExtents.z = DotProductAbs(localExtents, transform[2]);
//VectorSubtract(newCenter, newExtents, vecMinsOut);
//VectorAdd(newCenter, newExtents, vecMaxsOut);
}
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 static void AngleMatrix(SourceRadianEuler angles, SourceVector position, ref SourceMatrix3x4 matrix)
{
AngleMatrix(angles, ref matrix);
MatrixSetColumn(position, 3, ref matrix);
}
//inline void TransformPlane( const cplane_t &inPlane, cplane_t &outPlane ) const;
//inline void InverseTransformPlane( const cplane_t &inPlane, cplane_t &outPlane ) const;
/// Computes an inverse. Uses the 'TR' naming to be consistent with the same method in matrix3x4_t (which only works with orthonormal matrices)
public void InverseTR(ref SourceCTransform _out)
{
SourceMatrix3x4 xForm = ToMatrix();
_out = xForm.InverseTR().ToCTransform();
}
public void InitFromMatrix(SourceMatrix3x4 transform)
{
m_orientation = MathUtils.MatrixQuaternion(transform);
m_vPosition = transform.GetOrigin();
}
public static void TransformMatrix(SourceCTransform _in, ref SourceMatrix3x4 _out)
{
QuaternionMatrix(_in.m_orientation, _in.m_vPosition, ref _out);
}
public static void MatrixAngles(SourceMatrix3x4 matrix, ref SourceQAngle angles)
{
//MatrixAngles(matrix, out angles.x);
throw new NotImplementedException();
}
public static void VectorIRotate(SourceVector in1, SourceMatrix3x4 in2, ref SourceVector _out)
{
throw new NotImplementedException();
//VectorIRotate( in1.x, in2, ref _out.x );
}
public static void MatrixGetColumn(SourceMatrix3x4 _in, int column, ref SourceVector _out)
{
_out.x = _in[0][column];
_out.y = _in[1][column];
_out.z = _in[2][column];
}
public static void MatrixTransform(SourceMatrix3x4 _in, ref SourceCTransform _out)
{
MatrixQuaternion(_in, ref _out.m_orientation);
MatrixGetColumn(_in, 3, ref _out.m_vPosition);
}
public static void MatrixSetColumn(SourceVector _in, int column, ref SourceMatrix3x4 _out)
{
_out[0][column] = _in.x;
_out[1][column] = _in.y;
_out[2][column] = _in.z;
}
public static void AngleMatrix(SourceRadianEuler angles, ref SourceMatrix3x4 matrix)
{
SourceQAngle quakeEuler = new SourceQAngle(RAD2DEG(angles.y), RAD2DEG(angles.z), RAD2DEG(angles.x));
AngleMatrix(quakeEuler, ref matrix);
}
