//4x4矩阵的与标量的乘积
public static void Matrix4fMulRotationScale(ref Matrix4fT NewObj, float scale)
{
NewObj.s.XX *= scale; NewObj.s.YX *= scale; NewObj.s.ZX *= scale;
NewObj.s.XY *= scale; NewObj.s.YY *= scale; NewObj.s.ZY *= scale;
NewObj.s.XZ *= scale; NewObj.s.YZ *= scale; NewObj.s.ZZ *= scale;
}
//设置4x4矩阵为单位矩阵
public static void Matrix4fSetIdentity(ref Matrix4fT NewObj)
{
Matrix4fSetZero(ref NewObj);
NewObj.s.M00 = 1.0f;
NewObj.s.M11 = 1.0f;
NewObj.s.M22 = 1.0f;
NewObj.s.M33=1.0f;
}
public object Clone()
{
Matrix4fT matrx = new Matrix4fT();
matrx.s = (matr44)this.s.Clone();
return matrx;
}
//进行矩阵的奇异值分解，旋转矩阵被保存到rot3和rot4中，返回矩阵的缩放因子
public static float Matrix4fSVD( Matrix4fT NewObj/*, ref Matrix3fT rot3, ref Matrix4fT rot4*/)
{
float s;
s = Convert.ToSingle(Math.Sqrt(
( (NewObj.s.XX * NewObj.s.XX) + (NewObj.s.XY * NewObj.s.XY) + (NewObj.s.XZ * NewObj.s.XZ) +
(NewObj.s.YX * NewObj.s.YX) + (NewObj.s.YY * NewObj.s.YY) + (NewObj.s.YZ * NewObj.s.YZ) +
(NewObj.s.ZX * NewObj.s.ZX) + (NewObj.s.ZY * NewObj.s.ZY) + (NewObj.s.ZZ * NewObj.s.ZZ) ) / 3.0f ));
/*
//if (rot3 != null)
{
rot3.s.XX = NewObj.s.XX; rot3.s.XY = NewObj.s.XY; rot3.s.XZ = NewObj.s.XZ;
rot3.s.YX = NewObj.s.YX; rot3.s.YY = NewObj.s.YY; rot3.s.YZ = NewObj.s.YZ;
rot3.s.ZX = NewObj.s.ZX; rot3.s.ZY = NewObj.s.ZY; rot3.s.ZZ = NewObj.s.ZZ;
n = 1.0f / Math.Sqrt( (NewObj.s.XX * NewObj.s.XX) +
(NewObj.s.XY * NewObj.s.XY) +
(NewObj.s.XZ * NewObj.s.XZ) );
rot3.s.XX *= n;
rot3.s.XY *= n;
rot3.s.XZ *= n;
n = 1.0f / Math.Sqrt( (NewObj.s.YX * NewObj.s.YX) +
(NewObj.s.YY * NewObj.s.YY) +
(NewObj.s.YZ * NewObj.s.YZ) );
rot3.s.YX *= n;
rot3.s.YY *= n;
rot3.s.YZ *= n;
n = 1.0f / Math.Sqrt( (NewObj.s.ZX * NewObj.s.ZX) +
(NewObj.s.ZY * NewObj.s.ZY) +
(NewObj.s.ZZ * NewObj.s.ZZ) );
rot3.s.ZX *= n;
rot3.s.ZY *= n;
rot3.s.ZZ *= n;
}
//if (rot4)
{
//if (rot4 != NewObj)
{
Matrix4fSetRotationScaleFromMatrix4f(ref rot4, NewObj);
}
n = 1.0f / Math.Sqrt( (NewObj.s.XX * NewObj.s.XX) +
(NewObj.s.XY * NewObj.s.XY) +
(NewObj.s.XZ * NewObj.s.XZ) );
rot4.s.XX *= n;
rot4.s.XY *= n;
rot4.s.XZ *= n;
n = 1.0f / Math.Sqrt( (NewObj.s.YX * NewObj.s.YX) +
(NewObj.s.YY * NewObj.s.YY) +
(NewObj.s.YZ * NewObj.s.YZ) );
rot4.s.YX *= n;
rot4.s.YY *= n;
rot4.s.YZ *= n;
n = 1.0f / Math.Sqrt( (NewObj.s.ZX * NewObj.s.ZX) +
(NewObj.s.ZY * NewObj.s.ZY) +
(NewObj.s.ZZ * NewObj.s.ZZ) );
rot4.s.ZX *= n;
rot4.s.ZY *= n;
rot4.s.ZZ *= n;
}
*/
return s;
}
//设置4x4矩阵为0矩阵
public static void Matrix4fSetZero(ref Matrix4fT NewObj)
{
NewObj.s.M00 = NewObj.s.M01 = NewObj.s.M02 =NewObj.s.M03 =
NewObj.s.M10 = NewObj.s.M11 = NewObj.s.M12 =NewObj.s.M13 =
NewObj.s.M20 = NewObj.s.M21 = NewObj.s.M22 =NewObj.s.M23 =
NewObj.s.M30 = NewObj.s.M31 = NewObj.s.M32 =NewObj.s.M33 = 0.0f;
}
//4x4矩阵相乘
public static void Matrix4fSetRotationScaleFromMatrix4f(ref Matrix4fT NewObj, Matrix4fT m1)
{
NewObj.s.XX = m1.s.XX; NewObj.s.YX = m1.s.YX; NewObj.s.ZX = m1.s.ZX;
NewObj.s.XY = m1.s.XY; NewObj.s.YY = m1.s.YY; NewObj.s.ZY = m1.s.ZY;
NewObj.s.XZ = m1.s.XZ; NewObj.s.YZ = m1.s.YZ; NewObj.s.ZZ = m1.s.ZZ;
}
//设置旋转矩阵
public static void Matrix4fSetRotationFromMatrix3f(ref Matrix4fT NewObj, Matrix3fT m1)
{
float scale;
scale = Matrix4fSVD(NewObj);
Matrix4fSetRotationScaleFromMatrix3f(ref NewObj, m1);
Matrix4fMulRotationScale(ref NewObj, scale);
}
