master

分支 (1)

管理

管理

master

vector-computation
/
MatrixInformation.cpp

#ifndef __MATRIX_INFORMATION_CLASS_IMPL__
#define __MATRIX_INFORMATION_CLASS_IMPL__
#include "MatrixInformation.h"
#include "EulerAngle.h"

// 矩阵转置:原矩阵行数据变成列的数据,原矩阵列的数据变成行的数据
MatrixInformation MatrixTranspose(MatrixInformation& matrixInfo)
{
    // 获取原矩阵数据
    double OriginMatrixData[MATRIX_ROW_COUNT][MATRIX_COL_COUNT] = { 0 };
    matrixInfo.GetMatrixData(OriginMatrixData);

    // 填充新矩阵数据,并且数据进行转置
    double NewMatrixData[MATRIX_ROW_COUNT][MATRIX_COL_COUNT] = { 0 };
    for (size_t RowIndex = 0; RowIndex < MATRIX_ROW_COUNT; RowIndex++)
    {
        for (size_t ColIndex = 0; ColIndex < MATRIX_COL_COUNT; ColIndex++)
        {
            NewMatrixData[RowIndex][ColIndex] = OriginMatrixData[ColIndex][RowIndex];
        }
    }
    return MatrixInformation(NewMatrixData);
}

// 矩阵相乘,两个矩阵相乘得到一个新的矩阵
MatrixInformation operator*(MatrixInformation& matrixInfo1, MatrixInformation& matrixInfo2)
{
    return MatrixMultiply(matrixInfo1, matrixInfo2);
}


// 向量与矩阵相乘
VectorInformation operator*(VectorInformation& vectorInfo, MatrixInformation& matrixInfo)
{
    return    VectorMatrixMultiply(vectorInfo, matrixInfo);
}

// 计算矩阵的逆,只有行列式不等于0的非奇异矩阵可计算矩阵的逆(奇异矩阵的逆等于0)
// 矩阵的逆需要先计算代数余子式矩阵
// (M^-1)^-1=M :矩阵的逆的逆等于原先的矩阵
// I^-1=I :单位矩阵的逆等于单位矩阵
// (M┯)^-1=(M^-1)┯:矩阵转置的逆等于矩阵的逆的转置
// (A*B)^-1=B^-1*A^-1:A矩阵乘以B矩阵的逆 等于B矩阵的逆乘以A矩阵的逆(BA顺序相反)
// 矩阵的逆几何意义：撤销矩阵的变换

MatrixInformation InverseOfMatrix(MatrixInformation& matrixInfo)
{
    // 计算矩阵的行列式
    double determinantValue = matrixInfo.DeterminantMatrix();
    // 限制矩阵必须是非奇异矩阵
    assert(fabs(determinantValue) > 0.00001f);

    double paramMatrixData[MATRIX_ROW_COUNT][MATRIX_COL_COUNT] = { 0 };
    matrixInfo.GetMatrixData(paramMatrixData);
    // 声明矩阵数据变量
    DEFINE_MATRIX_DATA_VAR(paramMatrixData);
    // 声明代数余子矩阵数据变量
    DECLARE_ALGEBRAIC_COSUBMATRIX_VAR

    double oneOverDet = 1.0f / determinantValue;

    // 计算代数余子阵
    c11 = (m22 * m33 - m23 * m32) * oneOverDet;
    c12 = (m13 * m32 - m12 * m33) * oneOverDet;
    c13 = (m12 * m23 - m13 * m22) * oneOverDet;

    c21 = (m23 * m31 - m21 * m33) * oneOverDet;
    c22 = (m11 * m33 - m13 * m31) * oneOverDet;
    c23 = (m13 * m21 - m11 * m23) * oneOverDet;

    c31 = (m21 * m32 - m22 * m31) * oneOverDet;
    c32 = (m12 * m31 - m11 * m32) * oneOverDet;
    c33 = (m11 * m22 - m12 * m21) * oneOverDet;
    double data[MATRIX_ROW_COUNT][MATRIX_COL_COUNT] = {
                { c11, c12, c13 },
                { c21, c22, c23 },
                { c31, c32, c33 }
    };
    return MatrixInformation(data);
}

MatrixInformation MatrixMultiply(MatrixInformation& matrixInfor1, MatrixInformation& matrixInfor2)
{
    // 定义新矩阵的数据
    double NewMatrixData[MATRIX_ROW_COUNT][MATRIX_COL_COUNT] = { 0 };
    // 获取两个原矩阵的数据
    double MatrixData1[MATRIX_ROW_COUNT][MATRIX_COL_COUNT] = { 0 };
    double MatrixData2[MATRIX_ROW_COUNT][MATRIX_COL_COUNT] = { 0 };
    matrixInfor1.GetMatrixData(MatrixData1);
    matrixInfor2.GetMatrixData(MatrixData2);


    // 控制矩阵行循环次数
    for (size_t rowLoopCount = 0; rowLoopCount < MATRIX_ROW_COUNT; rowLoopCount++)
    {
        // 控制矩阵列循环次数
        for (size_t colLoopCount = 0; colLoopCount < MATRIX_COL_COUNT; colLoopCount++)
        {
            {
                double sum = 0.0f;
                for (size_t ColIndex = 0; ColIndex < MATRIX_COL_COUNT; ColIndex++)
                {
                    // 第一个矩阵行数据乘以第二个矩阵列数据
                    // 矩阵1行中的每一个数据
                    double m1r = MatrixData1[rowLoopCount][ColIndex];
                    // 矩阵2列中的每一个数据
                    double m2c = MatrixData2[ColIndex][colLoopCount];
                    sum += m1r * m2c;
                }
                // 循环对新矩阵每个元素进行赋值
                NewMatrixData[rowLoopCount][colLoopCount] = sum;
            }
        }
    }
    return MatrixInformation(NewMatrixData);
}

VectorInformation VectorMatrixMultiply(VectorInformation& vectorInfo, MatrixInformation& matrixInfo)
{
    double x = vectorInfo.GetX();
    double y = vectorInfo.GetY();
    double z = vectorInfo.GetZ();
    double data[MATRIX_ROW_COUNT][MATRIX_COL_COUNT] = { 0 };
    matrixInfo.GetMatrixData(data);
    double NewVectorX = x * data[0][0] + y * data[1][0] + z * data[2][0];
    double NewVectorY = x * data[0][1] + y * data[1][1] + z * data[2][1];
    double NewVectorZ = x * data[0][2] + y * data[1][2] + z * data[2][2];
    return VectorInformation(NewVectorX, NewVectorY, NewVectorZ);
}


MatrixInformation::MatrixInformation()
{
    InitializationMatrixData();
    ZeroMatrixData();
}

MatrixInformation::~MatrixInformation()
{
    SafeDeletePoint(pNormalMatrixData);
    // cout << "MatrixInformation::~MatrixInformation" << endl;
}

MatrixInformation::MatrixInformation(double data[MATRIX_ROW_COUNT][MATRIX_COL_COUNT])
{
    InitializationMatrixData();
    ZeroMatrixData();
    SetMatrixData(data);
}

VOID MatrixInformation::ShowMatrixInformation()
{
    cout << "Matrix Print:" << endl;
    for (size_t RowIndex = 0; RowIndex < MATRIX_ROW_COUNT; RowIndex++)
    {
        cout << "\t[";
        for (size_t ColIndex = 0; ColIndex < MATRIX_COL_COUNT; ColIndex++)
        {
            SetCoutWidth();
            double oValue = pNormalMatrixData[RowIndex][ColIndex];
            cout << FormatOutputFloatValue(oValue);
            // 最后一列
            if (ColIndex != (MATRIX_COL_COUNT - 1))
            {
                cout << ", ";
            }
        }
        cout << "]" << endl;
    }
}
// 矩阵数据置0
VOID MatrixInformation::ZeroMatrixData()
{
    for (size_t RowIndex = 0; RowIndex < MATRIX_ROW_COUNT; RowIndex++)
    {
        for (size_t ColIndex = 0; ColIndex < MATRIX_COL_COUNT; ColIndex++)
        {
            pNormalMatrixData[RowIndex][ColIndex] = 0;
        }
    }
}
VOID MatrixInformation::SetCoutWidth(int width)
{
    cout.width(width);
    cout.fill(' ');
    // 设置浮点数固定输出小数点后两位
    // cout << setiosflags(ios::fixed);
    // cout<<setprecision(1);
}
VOID MatrixInformation::GetMatrixData(double data[MATRIX_ROW_COUNT][MATRIX_COL_COUNT])
{
    for (size_t RowIndex = 0; RowIndex < MATRIX_ROW_COUNT; RowIndex++)
    {
        for (size_t ColIndex = 0; ColIndex < MATRIX_COL_COUNT; ColIndex++)
        {
            data[RowIndex][ColIndex] = pNormalMatrixData[RowIndex][ColIndex];
        }
    }
}
// 填充数组的数据到矩阵中
VOID MatrixInformation::SetMatrixData(double data[MATRIX_ROW_COUNT][MATRIX_COL_COUNT])
{
    for (size_t RowIndex = 0; RowIndex < MATRIX_ROW_COUNT; RowIndex++)
    {
        for (size_t ColIndex = 0; ColIndex < MATRIX_COL_COUNT; ColIndex++)
        {
            pNormalMatrixData[RowIndex][ColIndex] = data[RowIndex][ColIndex];
        }
    }
}

/**
@descriptor:旋转矩阵
@type:旋转轴的类型,可以为xyz轴旋转
@radian旋转的角度
*/
VOID MatrixInformation::RotationMatrix(ROTATE_TYPE type, double angle)
{
    double cosValue = cos(angle);
    double sinValue = sin(angle);

    switch (type)
    {
    case ROTATE_TYPE::ROTATE_X:
    {
        double data[MATRIX_ROW_COUNT][MATRIX_COL_COUNT] = {
                    { 1,     0    ,    0     },
                    { 0, cosValue , sinValue },
                    { 0, -sinValue, cosValue }
        };
        SetMatrixData(data);
        break;
    }
    case ROTATE_TYPE::ROTATE_Y:
    {
        double data[MATRIX_ROW_COUNT][MATRIX_COL_COUNT] = {
                    { cosValue, 0,-sinValue },
                    { 0,        1,    0     },
                    { sinValue, 0, cosValue }
        };
        SetMatrixData(data);
        break;
    }
    case ROTATE_TYPE::ROTATE_Z:
    {
        double data[MATRIX_ROW_COUNT][MATRIX_COL_COUNT] = {
                    { cosValue, sinValue ,  0 },
                    { -sinValue, cosValue,  0 },
                    { 0,          0      ,  1 }
        };
        SetMatrixData(data);
        break;
    }
    default:
        assert(false);
        break;
    }
}
// 缩放矩阵
VOID MatrixInformation::SaclingMatrix(double x, double y, double z)
{
    double data[MATRIX_ROW_COUNT][MATRIX_COL_COUNT] = {
                { x, 0, 0 },
                { 0, y, 0 },
                { 0, 0, z }
    };
    SetMatrixData(data);
}
// 缩放矩阵
VOID MatrixInformation::SaclingMatrix(VectorInformation& vectorInformation)
{
    SaclingMatrix(vectorInformation.GetX(), vectorInformation.GetY(), vectorInformation.GetZ());
}
// 投影矩阵
VOID MatrixInformation::ProjectionMatrix(double x, double y, double z)
{
    VectorInformation vectorInformation(x, y, z);
    // 向量必须为单位向量
    assert(fabs(vectorInformation * vectorInformation) - 1.0f < 0.000001);

    DECLARE_MATRIX_DATA_VAR;

    m11 = 1.0f - x * x;
    m22 = 1.0f - y * y;
    m33 = 1.0f - z * z;

    m12 = m21 = -x * y;
    m13 = m31 = -x * z;
    m23 = m32 = -y * z;

    double data[MATRIX_ROW_COUNT][MATRIX_COL_COUNT] =
    { { m11,m12,m13 },{ m21,m22,m23 },{ m31,m32,m33 } };
    SetMatrixData(data);
}

// xyz轴的镜像
VOID MatrixInformation::MatrixImageXYZ(ROTATE_TYPE type)
{
    switch (type)
    {
        // x轴镜像
    case ROTATE_TYPE::ROTATE_X:
    {
        double data[MATRIX_ROW_COUNT][MATRIX_COL_COUNT] = {
                    { -1, 0, 0 },
                    { 0 , 1, 0 },
                    { 0 , 0, 1 }
        };
        SetMatrixData(data);
        break;
    }
    // y轴镜像
    case ROTATE_TYPE::ROTATE_Y:
    {
        double data[MATRIX_ROW_COUNT][MATRIX_COL_COUNT] = {
                    { 1, 0, 0 },
                    { 0,-1, 0 },
                    { 0, 0, 1 }
        };
        SetMatrixData(data);
        break;
    }
    // z轴镜像
    case ROTATE_TYPE::ROTATE_Z:
    {
        double data[MATRIX_ROW_COUNT][MATRIX_COL_COUNT] = {
                    { 1, 0, 0  },
                    { 0, 1, 0  },
                    { 0, 0, -1 }
        };
        SetMatrixData(data);
        break;
    }
    default:
        assert(false);
        break;
    }
}
// 任意轴的镜像
VOID MatrixInformation::ArbitraryMatrixImage(VectorInformation& vectorInformation)
{
    // 向量必须为单位向量
    assert(fabs(vectorInformation * vectorInformation) - 1.0f < 0.000001);
    double xValue = vectorInformation.GetX();
    double yValue = vectorInformation.GetY();
    double zValue = vectorInformation.GetZ();

    DOUBLE dx = -2.0f * xValue;
    DOUBLE dy = -2.0f * yValue;
    DOUBLE dz = -2.0f * zValue;
    DECLARE_MATRIX_DATA_VAR;
    m11 = 1.0f + dx * xValue;
    m22 = 1.0f + dy * yValue;
    m33 = 1.0f + dz * zValue;

    m12 = m21 = dx * yValue;
    m13 = m31 = dx * zValue;
    m23 = m32 = dy * zValue;
    double data[MATRIX_ROW_COUNT][MATRIX_COL_COUNT] = {
                { m11,m12,m13 } ,
                { m21,m22,m23 } ,
                { m31,m32,m33 }
    };
    SetMatrixData(data);
}
// 转换指定的矩阵数据为单位矩阵
VOID MatrixInformation::ConvertSpecificArrayToIdentityMatrix(double** data)
{
    data[0][0] = 1;
    data[0][1] = 0;
    data[0][2] = 0;

    data[1][0] = 0;
    data[1][1] = 1;
    data[1][2] = 0;

    data[2][0] = 0;
    data[2][1] = 0;
    data[2][2] = 1;

}
VectorInformation MatrixInformation::InertialToObject(VectorInformation& vectorInfo)
{
    return VectorMatrixMultiply(vectorInfo, *this);
}
VectorInformation MatrixInformation::ObjectToInertial(VectorInformation& vectorInfo)
{
    MatrixInformation tMatrix = MatrixTranspose(*this);
    return VectorMatrixMultiply(vectorInfo, tMatrix);
}
VOID MatrixInformation::SetEulerAngle(EulerAngle& eulerAngle)
{
    // 初始化欧拉角变量
    DEFINE_EULER_ANGLE_VAR(eulerAngle);
    // 声明矩阵的数据
    DECLARE_MATRIX_DATA_VAR;
    m11 = cosHeading * cosBank+sinHeading*sinPitch * sinBank;
    m12 = -cosHeading * sinBank + sinHeading * sinPitch * cosBank;
    m13 = sinHeading * cosPitch;

    m21 = sinBank * cosPitch;
    m22= cosBank * cosPitch;
    m23 = -sinPitch;

    m31 = -sinHeading*cosBank + cosHeading * sinPitch * sinBank;
    m32 = sinBank*sinHeading + cosHeading * sinPitch * cosBank;
    m33 = cosHeading * cosPitch;

    double data[MATRIX_ROW_COUNT][MATRIX_COL_COUNT] = {
            { m11,m12,m13 } ,
            { m21,m22,m23 } ,
            { m31,m32,m33 }
    };
    SetMatrixData(data);
}
// 转换当前矩阵为单位矩阵
VOID MatrixInformation::ConvertToIdentityMatrix()
{
    double* p[MATRIX_COL_COUNT];
    for (size_t i = 0; i < MATRIX_ROW_COUNT; i++)
    {
        p[i] = pNormalMatrixData[i];
    }
    ConvertSpecificArrayToIdentityMatrix(p);
}

VOID MatrixInformation::MatrixShear(ROTATE_TYPE ShearType, double s, double t)
{
    switch (ShearType)
    {
    case ROTATE_TYPE::ROTATE_X:
    {
        // x轴切变y和z轴,x轴不变
        double data[MATRIX_ROW_COUNT][MATRIX_COL_COUNT] = {
                    { 1, s, t },
                    { 0, 1, 0 },
                    { 0, 0, 1 }
        };
        SetMatrixData(data);
        break;
    }
    case ROTATE_TYPE::ROTATE_Y:
    {
        // y轴切变x和z轴,y轴不变
        double data[MATRIX_ROW_COUNT][MATRIX_COL_COUNT] = {
                    { 1, 0, 0 },
                    { s, 1, t },
                    { 0, 0, 1 }
        };
        SetMatrixData(data);
        break;
    }
    case ROTATE_TYPE::ROTATE_Z:
    {
        // z轴切变x和y轴,z轴不变
        double data[MATRIX_ROW_COUNT][MATRIX_COL_COUNT] = {
                    { 1, 0, 0 },
                    { 0, 1, 0 },
                    { s, t, 1 }
        };
        SetMatrixData(data);
        break;
    }
    default:
        break;
    }
}
/*
矩阵的行列式
几何意义：2D为矩阵的面积,3D为矩阵的体积
计算公式：

2*2的矩阵|M| =     |m11 m12| = m11*m22-m12*m21
                |m21 m22|

                | m11 m12 m13 |
3*3的矩阵|M|=    | m21 m22 m23 |= m11*m22*m33+m12*m23*m31+m13*m21*m32-m13*m22*m31-m12*m21*m33-m11*m23*m32
                | m31 m32 m33 |= m11*(m22*m33 - m23*m32)+ m12* (m23*m31 - m21*m33)+ m13* (m21*m32 - m22*m31);

*/
DOUBLE MatrixInformation::DeterminantMatrix()
{
    double data[MATRIX_ROW_COUNT][MATRIX_COL_COUNT] = { 0 };
    GetMatrixData(data);
    DEFINE_MATRIX_DATA_VAR(data);
    double sum = m11 * (m22 * m33 - m23 * m32)
        + m12 * (m23 * m31 - m21 * m33)
        + m13 * (m21 * m32 - m22 * m31);
    return sum;
}

// 投影矩阵
VOID MatrixInformation::ProjectionMatrix(VectorInformation& vectorInformation)
{
    // 向量必须为单位向量
    assert(fabs(vectorInformation * vectorInformation) - 1.0f < 0.000001);
    ProjectionMatrix(vectorInformation.GetX(), vectorInformation.GetY(), vectorInformation.GetZ());
}


/*
设置矩阵的行数据
*/
VOID MatrixInformation::SetMatrixRowData(MATRIX_DATA_INDEX rowIndex, double rowData[3])
{
    assert(rowIndex < MATRIX_ROW_COUNT);
    for (size_t row = 0; row < MATRIX_ROW_COUNT; row++)
    {
        if (row == rowIndex)
        {
            for (size_t col = 0; col < MATRIX_COL_COUNT; col++)
            {
                pNormalMatrixData[row][col] = rowData[col];
            }
            return;
        }
    }
}
/*
设置矩阵的列数据
*/
VOID MatrixInformation::SetMatrixColData(MATRIX_DATA_INDEX colIndex, double rowData[3])
{
    assert(colIndex < MATRIX_COL_COUNT);
    for (size_t col = 0; col < MATRIX_COL_COUNT; col++)
    {
        if (col == colIndex)
        {
            for (size_t row = 0; row < MATRIX_ROW_COUNT; row++)
            {
                pNormalMatrixData[row][col] = rowData[row];
            }
            return;
        }
    }
}

#endif // !MATRIX_INFORMATION_CLASS_IMPL