// MatrixF64 fo float Array 1D public float[] ArrConverMatrixF64ToFloat1D(MatrixF64 M) { float[] fArrayFloat = new float[M.Rows() * M.Columns()]; for (int i = 0; i < fArrayFloat.Length; i++) { fArrayFloat[i] = (float)M.Entries[i]; } return fArrayFloat; }
public MatrixF64(MatrixF64 other) { this.totalRows = 0; this.totalColumns = 0; this.entries = null; if (null == other) { return; } if ( (null == other.entries) || (0 == other.totalRows) || (0 == other.totalColumns) ) { return; } int totalEntries = (other.totalRows * other.totalColumns); if (other.entries.Length != totalEntries) { return; } this.totalRows = other.totalRows; this.totalColumns = other.totalColumns; this.entries = new double[totalEntries]; for (int k = 0; k < totalEntries; k++) { this.entries[k] = other.entries[k]; } }
CModel TopoModelFile; // Create topological model file #endregion Fields #region Constructors ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Constructor - new public CFEM_CALC(CModel model, bool bDebugging) { // Load Topological model TopoModelFile = new CModel(); TopoModelFile = model; // Generate FEM model data from Topological model // Prepare solver data // Fill local and global matrices of FEM elements FEMModel = new CGenex(TopoModelFile); /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Temp - display matrices if (bDebugging) { for (int i = 0; i < FEMModel.m_arrFemMembers.Length; i++) { // Member ID Console.WriteLine("Member ID: " + FEMModel.m_arrFemMembers[i].ID + "\n"); // kij_0 - local stiffeness matrix 6 x 6 Console.WriteLine("Local stiffeness matrix k" + FEMModel.m_arrFemMembers[i].NodeStart.ID + FEMModel.m_arrFemMembers[i].NodeEnd.ID + "0 - Dimensions: 6 x 6 \n"); FEMModel.m_arrFemMembers[i].m_fkLocMatr.Print2DMatrixFormated(); // A Tranformation Rotation Matrixes 6 x 6 Console.WriteLine("Tranformation rotation matrix A - Dimensions: 6 x 6 \n"); FEMModel.m_arrFemMembers[i].m_fATRMatr3D.Print2DMatrixFormated(); // B Transfer Matrixes 6 x 6 Console.WriteLine("Transfer matrix B - Dimensions: 6 x 6 \n"); FEMModel.m_arrFemMembers[i].m_fBTTMatr3D.Print2DMatrixFormated(); // Kij - global matrix of member 12 x 12 Console.WriteLine("Global stiffeness matrix K" + FEMModel.m_arrFemMembers[i].NodeStart.ID + FEMModel.m_arrFemMembers[i].NodeEnd.ID + "0 - Dimensions: 12 x 12 \n"); FEMModel.m_arrFemMembers[i].m_fKGlobM.Print2DMatrixFormated_ABxCD(FEMModel.m_arrFemMembers[i].m_fKGlobM.m_fArrMembersABxCD); // Element Load Vectors 2 x 6 Console.WriteLine("Member load vector - primary end forces in LCS at start node ID: " + FEMModel.m_arrFemMembers[i].NodeStart.ID + " - Dimensions: 6 x 1 \n"); FEMModel.m_arrFemMembers[i].m_VElemPEF_LCS_StNode.Print1DVector(); Console.WriteLine("Member load vector - primary end forces in LCS at end node ID: " + FEMModel.m_arrFemMembers[i].NodeEnd.ID + " - Dimensions: 6 x 1 \n"); FEMModel.m_arrFemMembers[i].m_VElemPEF_LCS_EnNode.Print1DVector(); Console.WriteLine("Member load vector - primary end forces in GCS at start node ID: " + FEMModel.m_arrFemMembers[i].NodeStart.ID + " - Dimensions: 6 x 1 \n"); FEMModel.m_arrFemMembers[i].m_VElemPEF_GCS_StNode.Print1DVector(); Console.WriteLine("Member load vector - primary end forces in GCS at end node ID: " + FEMModel.m_arrFemMembers[i].NodeEnd.ID + " - Dimensions: 6 x 1 \n"); FEMModel.m_arrFemMembers[i].m_VElemPEF_GCS_EnNode.Print1DVector(); } } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Set Global Code Number of Nodes / Nastavit globalne kodove cisla uzlov // Save indexes of nodes and DOF which are free and represent vector of uknown variables in solution SetNodesGlobCodeNo(); // Nastavi DOF v uzloch a ich globalne kodove cisla, konecne CodeNo urci velkost matice konstrukcie // Fill members of structure global vector of displacement // Now we know number of not restrained DOF, so we can allocate array size m_fDisp_Vector_CN = new int[m_iCodeNo, 3]; // 1st - global DOF code number, 2nd - Node index, 3rd - local code number of DOF in NODE //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Save it as array of arrays n x 2 (1st value is index - node index (0 - n-1) , 2nd value is DOF index (0-5) // n - total number of nodes in model ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// FillGlobalDisplCodeNo(); ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Right side of Equation System ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Global Stiffeness Matrix of Structure - Allocate Memory (Matrix Size) m_M_K_Structure = new CMatrix(m_iCodeNo); // Fill Global Stiffeness Matrix FillGlobalMatrix(); // Global Stiffeness Matrix m_iCodeNo x m_iCodeNo if (bDebugging) { Console.WriteLine("Global stiffeness matrix - Dimensions: " + m_iCodeNo + " x " + m_iCodeNo + "\n"); m_M_K_Structure.Print2DMatrixFormated(); } // Auxiliary temporary transformation from 2D to 1D array / from float do double // Pomocne prevody medzi jednorozmernym, dvojrozmernym polom a triedou Matrix, // bude nutne zladit a urcit jeden format v akom budeme pracovat s datami a potom zmazat CArray objArray = new CArray(); // Convert Size float[] m_M_K_fTemp1D = objArray.ArrTranf2Dto1D(m_M_K_Structure.m_fArrMembers); // Convert Type double[] m_M_K_dTemp1D = objArray.ArrConverFloatToDouble1D(m_M_K_fTemp1D); MatrixF64 objMatrix = new MatrixF64(m_iCodeNo, m_iCodeNo, m_M_K_dTemp1D); // Print Created Matrix of MatrixF64 Class if (bDebugging) { Console.WriteLine("Global stiffeness matrix in F64 Class - Dimensions: " + m_iCodeNo + " x " + m_iCodeNo + "\n"); objMatrix.WriteLine(); } // Get Inverse Global Stiffeness Matrix MatrixF64 objMatrixInv = objMatrix.Inverse(); // Print Inverse Matrix if (bDebugging) { Console.WriteLine("Inverse global stiffeness matrix - Dimensions: " + m_iCodeNo + " x " + m_iCodeNo + "\n"); objMatrixInv.WriteLine(); } // Convert Type float[] m_M_K_Inv_fTemp1D = objArray.ArrConverMatrixF64ToFloat1D(objMatrixInv); // Inverse Global Stiffeness Matrix of Structure - Allocate Memory (Matrix Size) CMatrix m_M_K_Structure_Inv = new CMatrix(m_iCodeNo); m_M_K_Structure_Inv.m_fArrMembers = objArray.ArrTranf1Dto2D(m_M_K_Inv_fTemp1D); ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Left side of Equation System ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Global Load Vector - Allocate Memory (Vector Size) m_V_Load = new CVector(m_iCodeNo); // Fill Global Load Vector FillGlobalLoadVector(); // Display Global Load Vector if (bDebugging) { Console.WriteLine("Global load vector - Dimensions: " + m_iCodeNo + " x 1 \n"); m_V_Load.Print1DVector(); } ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Solution - calculation of unknown displacement of nodes in GCS - system of linear equations // Start Solver ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Global Displacement Vector - Allocate Memory (Vector Size) m_V_Displ = new CVector(m_iCodeNo); // Fill Global Displacement Vector m_V_Displ = VectorF.fMultiplyMatrVectr(m_M_K_Structure_Inv, m_V_Load); ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // End Solver ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Display Global Displacement Vector - solution result if (bDebugging) { Console.WriteLine("Global displacement vector - Dimensions: " + m_iCodeNo + " x 1 \n"); m_V_Displ.Print1DVector(); } // Set displacements and rotations of DOF in GCS to appropriate node DOF acc. to global code numbers for (int i = 0; i < m_iCodeNo; i++) { // Check if DOF is default (free - ) or has some initial value (settlement; soil consolidation etc.) // See default values - float.PositiveInfinity if (FEMModel.m_arrFemNodes[m_fDisp_Vector_CN[i, 1]].m_VDisp.FVectorItems[m_fDisp_Vector_CN[i, 2]] == float.PositiveInfinity) FEMModel.m_arrFemNodes[m_fDisp_Vector_CN[i, 1]].m_VDisp.FVectorItems[m_fDisp_Vector_CN[i, 2]] = m_V_Displ.FVectorItems[i]; // set calculated else // some real initial value exists FEMModel.m_arrFemNodes[m_fDisp_Vector_CN[i, 1]].m_VDisp.FVectorItems[m_fDisp_Vector_CN[i, 2]] += m_V_Displ.FVectorItems[i]; // add calculated (to sum) } // Set default zero displacements or rotations in GCS to fixed DOF for (int i = 0; i < FEMModel.m_arrFemNodes.Length; i++) { for (int j = 0; j < FEMModel.m_arrFemNodes[i].m_VDisp.FVectorItems.Length; j++) // Check each DOF of all nodes { if (FEMModel.m_arrFemNodes[i].m_VDisp.FVectorItems[j] == float.PositiveInfinity) // Check that default infinity value wasn't changed FEMModel.m_arrFemNodes[i].m_VDisp.FVectorItems[j] = 0; // Set zero } } ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Get final end forces at element in global coordinate system GCS ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// if (bDebugging) { for (int i = 0; i < FEMModel.m_arrFemMembers.Length; i++) { FEMModel.m_arrFemMembers[i].GetArrElemEF_GCS_StNode(); Console.WriteLine("Element Index No.: " + i + "; " + "Node No.: " + FEMModel.m_arrFemMembers[i].NodeStart.ID + "; " + "Start Node End Forces in GCS"); FEMModel.m_arrFemMembers[i].m_VElemEF_GCS_StNode.Print1DVector(); FEMModel.m_arrFemMembers[i].GetArrElemEF_GCS_EnNode(); Console.WriteLine("Element Index No.: " + i + "; " + "Node No.: " + FEMModel.m_arrFemMembers[i].NodeEnd.ID + "; " + "End Node End Forces in GCS"); FEMModel.m_arrFemMembers[i].m_VElemEF_GCS_EnNode.Print1DVector(); FEMModel.m_arrFemMembers[i].GetArrElemEF_LCS_StNode(); Console.WriteLine("Element Index No.: " + i + "; " + "Node No.: " + FEMModel.m_arrFemMembers[i].NodeStart.ID + "; " + "Start Node End Forces in LCS"); FEMModel.m_arrFemMembers[i].m_VElemEF_LCS_StNode.Print1DVector(); FEMModel.m_arrFemMembers[i].GetArrElemEF_LCS_EnNode(); Console.WriteLine("Element Index No.: " + i + "; " + "Node No.: " + FEMModel.m_arrFemMembers[i].NodeEnd.ID + "; " + "End Node End Forces in LCS"); FEMModel.m_arrFemMembers[i].m_VElemEF_LCS_EnNode.Print1DVector(); FEMModel.m_arrFemMembers[i].GetArrElemIF_LCS_StNode(); Console.WriteLine("Element Index No.: " + i + "; " + "Node No.: " + FEMModel.m_arrFemMembers[i].NodeStart.ID + "; " + "Start Node Internal Forces in LCS"); FEMModel.m_arrFemMembers[i].m_VElemIF_LCS_StNode.Print1DVector(); FEMModel.m_arrFemMembers[i].GetArrElemIF_LCS_EnNode(); Console.WriteLine("Element Index No.: " + i + "; " + "Node No.: " + FEMModel.m_arrFemMembers[i].NodeEnd.ID + "; " + "End Node Internal Forces in LCS"); FEMModel.m_arrFemMembers[i].m_VElemIF_LCS_EnNode.Print1DVector(); } } // Calculate IF in x-places int inum_cut = 11; int inum_segm = 10; }
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // TEMPORARY EXAMPLE DATA // Consructor - old public CFEM_CALC() { // Geometry float fGeom_a = 4f, fGeom_b = 5f, fGeom_c = 3.5f; // Unit [m] // Material CMaterial m_Mat = new CMaterial(); // Cross-section CCrSc m_CrSc = new CCrSc_3_00(0, 8, 300, 125, 16.2f, 10.8f, 10.8f, 6.5f, 241.6f); // I 300 section // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! m_CrSc.FI_t = 5.69e-07f; m_CrSc.FI_y = 9.79e-05f; m_CrSc.FI_z = 4.49e-06f; m_CrSc.FA_g = 6.90e-03f; m_CrSc.FA_vy = 4.01e-03f; m_CrSc.FA_vz = 2.89e-03f; // Define Nodes Properties for (int i = 0; i < iNNoTot; i++) { // Create auxiliary Node object CFemNode CNode_i = new CFemNode(iNodeDOFNo); // Fill array object item with auxliary Node m_NodeArray[i] = CNode_i; } // Node 1 m_NodeArray[0].ID = 1; m_NodeArray[0].m_fVNodeCoordinates.FVectorItems[(int) e3D_DOF.eUX] = fGeom_a; m_NodeArray[0].m_fVNodeCoordinates.FVectorItems[(int) e3D_DOF.eUY] = 0f; m_NodeArray[0].m_fVNodeCoordinates.FVectorItems[(int) e3D_DOF.eUZ] = 0f; // Node 2 m_NodeArray[1].ID = 2; m_NodeArray[1].m_fVNodeCoordinates.FVectorItems[(int) e3D_DOF.eUX] = 0f; m_NodeArray[1].m_fVNodeCoordinates.FVectorItems[(int) e3D_DOF.eUY] = 0f; m_NodeArray[1].m_fVNodeCoordinates.FVectorItems[(int) e3D_DOF.eUZ] = 0f; // Node 3 m_NodeArray[2].ID = 5; m_NodeArray[2].m_fVNodeCoordinates.FVectorItems[(int) e3D_DOF.eUX] = fGeom_a; m_NodeArray[2].m_fVNodeCoordinates.FVectorItems[(int) e3D_DOF.eUY] = 0f; m_NodeArray[2].m_fVNodeCoordinates.FVectorItems[(int) e3D_DOF.eUZ] = -fGeom_c; // Node 4 m_NodeArray[3].ID = 3; m_NodeArray[3].m_fVNodeCoordinates.FVectorItems[(int) e3D_DOF.eUX] = fGeom_a; m_NodeArray[3].m_fVNodeCoordinates.FVectorItems[(int) e3D_DOF.eUY] = -fGeom_b; m_NodeArray[3].m_fVNodeCoordinates.FVectorItems[(int) e3D_DOF.eUZ] = 0f; // Set Nodal Supports (for restraint set 0f) // Node 1 // Node 2 m_NodeArray[1].m_VDisp.FVectorItems[0] = 0f; m_NodeArray[1].m_VDisp.FVectorItems[1] = 0f; m_NodeArray[1].m_VDisp.FVectorItems[2] = 0f; m_NodeArray[1].m_VDisp.FVectorItems[3] = 0f; m_NodeArray[1].m_VDisp.FVectorItems[4] = 0f; m_NodeArray[1].m_VDisp.FVectorItems[5] = 0f; // Node 3 m_NodeArray[2].m_VDisp.FVectorItems[0] = 0f; m_NodeArray[2].m_VDisp.FVectorItems[1] = 0f; m_NodeArray[2].m_VDisp.FVectorItems[2] = 0f; m_NodeArray[2].m_VDisp.FVectorItems[3] = 0f; m_NodeArray[2].m_VDisp.FVectorItems[4] = 0f; m_NodeArray[2].m_VDisp.FVectorItems[5] = 0f; // Node 4 m_NodeArray[3].m_VDisp.FVectorItems[0] = 0f; m_NodeArray[3].m_VDisp.FVectorItems[1] = 0f; m_NodeArray[3].m_VDisp.FVectorItems[2] = 0f; m_NodeArray[3].m_VDisp.FVectorItems[3] = 0f; m_NodeArray[3].m_VDisp.FVectorItems[4] = 0f; m_NodeArray[3].m_VDisp.FVectorItems[5] = 0f; // Set Global Code Numbers int m_iCodeNo = 0; // Number of unrestrained degrees of freedom - finally gives size of structure global matrix foreach (CFemNode i_CNode in m_NodeArray) // Each Node { for (int i = 0; i < iNodeDOFNo; i++) // Each DOF { if (i_CNode.m_VDisp.FVectorItems[i] != 0) // Perform for not restrained DOF { i_CNode.m_ArrNCodeNo[i] = m_iCodeNo; // Set global code number of degree of freedom (DOF) m_iCodeNo++; } } } // Fill members of structure global vector of displacement // Now we know number of not restrained DOF, so we can allocate array size m_fDisp_Vector_CN = new int[m_iCodeNo,3]; // 1st - global DOF code number, 2nd - Node index, 3rd - local code number of DOF in NODE FillGlobalDisplCodeNoOld(); //////////////////////////////////////////////////////////////////////////////////// // Set Nodal Loads (acting directly in nodes) //////////////////////////////////////////////////////////////////////////////////// // !!!!!! No kind of these loads actually /////////////////////////////////////////////////////////////////////////////////////// // Define FEM 1D elements /////////////////////////////////////////////////////////////////////////////////////// for (int i = 0; i < iElemNoTot; i++) { // Create auxiliary Element object CE_1D CElement_i = new CE_1D(); // Fill array object item m_ELemArray[i] = CElement_i; // Create auxiliary Element Load Object CLoad CLoad_i = new CLoad(); // Fill array object item m_ELoadArray[i] = CLoad_i; } // Member 1 [0] Nodes 1 - 2 ([0] [1]) m_ELemArray[0].NodeStart = m_NodeArray[0]; m_ELemArray[0].NodeEnd = m_NodeArray[1]; // Element Type m_ELemArray[0].m_eSuppType3D = EElemSuppType3D.e3DEl_000000_000000; // Element Material m_ELemArray[0].m_Mat = m_Mat; // Element Corss-section m_ELemArray[0].m_CrSc = m_CrSc; // Fill Basic Element Data m_ELemArray[0].FillBasic2(); // Load of Element only due to Element Transversal Forces m_ELoadArray[0].GetEndLoad_g(m_ELemArray[0], m_fq); // Output // kij_0 - local stiffeness matrix 6 x 6 m_ELemArray[0].m_fkLocMatr.Print2DMatrixFormated(); // A Tranformation Rotation Matrixes 6 x 6 m_ELemArray[0].m_fATRMatr3D.Print2DMatrixFormated(); // B Transfer Matrixes 6 x 6 m_ELemArray[0].m_fBTTMatr3D.Print2DMatrixFormated(); // Kij - global matrix of member 12 x 12 m_ELemArray[0].m_fKGlobM.Print2DMatrixFormated(); // Element Load Vector 2 x 6 m_ELemArray[0].m_ArrElemPEF_LCS.Print2DMatrixFormated(); #region MATRIX TEST ///////////////////////////////////////////////////////////////////////////////////////////////////// // TEST /* float[,] farrk11 = new float[6, 6]; float[,] farrk12 = new float[6, 6]; float[,] farrk21 = new float[6, 6]; float[,] farrk22 = new float[6, 6]; // Fill array for (int i = 0; i < 6; i++) { for (int j = 0; j < 6; j++) { farrk11[i, j] = ((i+1)*10)+1 + j; } } for (int i = 0; i < 6; i++) { for (int j = 0; j < 6; j++) { farrk12[i, j] = ((i + 1) * 10) + 1 + j; } } for (int i = 0; i < 6; i++) { for (int j = 0; j < 6; j++) { farrk21[i, j] = ((i + 1) * 10) + 1 + j; } } for (int i = 0; i < 6; i++) { for (int j = 0; j < 6; j++) { farrk22[i, j] = ((i + 1) * 10) + 1 + j; } } float[,][,] farrK = new float[2, 2][,] {{farrk11, farrk12}, {farrk21, farrk22}}; Console.WriteLine(m_ELemArray[0].CM.Print2DMatrix(farrK, 2, 6)); */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////// #endregion // Member 2 [1] Nodes 1 - 3 ([0] [2]) m_ELemArray[1].NodeStart = m_NodeArray[0]; m_ELemArray[1].NodeEnd = m_NodeArray[2]; // Element Type m_ELemArray[1].m_eSuppType3D = EElemSuppType3D.e3DEl_000000_000000; // Element Material m_ELemArray[1].m_Mat = m_Mat; // Element Corss-section m_ELemArray[1].m_CrSc = m_CrSc; // Fill Basic Element Data m_ELemArray[1].FillBasic2(); // Load of Element only due to Element Transversal Forces m_ELoadArray[1].GetEndLoad_F(m_ELemArray[1], 0f, 0f, m_fF); // Output // kij_0 - local stiffeness matrix 6 x 6 m_ELemArray[1].m_fkLocMatr.Print2DMatrixFormated(); // A Tranformation Rotation Matrixes 6 x 6 m_ELemArray[1].m_fATRMatr3D.Print2DMatrixFormated(); // B Transfer Matrixes 6 x 6 m_ELemArray[1].m_fBTTMatr3D.Print2DMatrixFormated(); // Kij - global matrix of member 12 x 12 m_ELemArray[1].m_fKGlobM.Print2DMatrixFormated(); // Element Load Vector 2 x 6 m_ELemArray[1].m_ArrElemPEF_LCS.Print2DMatrixFormated(); // Member 3 [2] Nodes 1 - 4 ([0] [3]) m_ELemArray[2].NodeStart = m_NodeArray[0]; m_ELemArray[2].NodeEnd = m_NodeArray[3]; // Element Type m_ELemArray[2].m_eSuppType3D = EElemSuppType3D.e3DEl_000000_000___; // Element Material m_ELemArray[2].m_Mat = m_Mat; // Element Corss-section m_ELemArray[2].m_CrSc = m_CrSc; // Fill Basic Element Data m_ELemArray[2].FillBasic2(); // Load of Element only due to Element Transversal Forces m_ELoadArray[2].GetEndLoad_M(m_ELemArray[2], m_fM, 0f, 0f); // Output // kij_0 - local stiffeness matrix 6 x 6 m_ELemArray[2].m_fkLocMatr.Print2DMatrixFormated(); // A Tranformation Rotation Matrixes 6 x 6 m_ELemArray[2].m_fATRMatr3D.Print2DMatrixFormated(); // B Transfer Matrixes 6 x 6 m_ELemArray[2].m_fBTTMatr3D.Print2DMatrixFormated(); // Kij - global matrix of member 12 x 12 m_ELemArray[2].m_fKGlobM.Print2DMatrixFormated(); // Element Load Vector 2 x 6 m_ELemArray[2].m_ArrElemPEF_LCS.Print2DMatrixFormated(); /* // Nodal loads (sum nodal loads and nodal loads due to element loads) m_NodeArray[2].m_sLoad.s_fFZ += -55000; // Add local nodal load to element ends loads due to intermediate load */ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Set Global Code Number of Nodes / Nastavit globalne kodove cisla uzlov // Save indexes of nodes and DOF which are free and represent vector of uknown variables in solution // Save it as array of arrays n x 2 (1st value is index - node index (0 - n-1) , 2nd value is DOF index (0-5) // n - total number of nodes in model ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// SetNodesGlobCodeNoOld(); // Nastavi DOF v uzlov globalne kodove cisla ??? ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Right side of Equation System ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Global Stiffeness Matrix of Structure - Allocate Memory (Matrix Size) m_M_K_Structure = new CMatrix(m_iCodeNo); // Fill Global Stiffeness Matrix FillGlobalMatrixOld(); // Global Stiffeness Matrix m_iCodeNo x m_iCodeNo m_M_K_Structure.Print2DMatrix(); // Auxialiary temporary transformation from 2D to 1D array / from float do double // Pomocne prevody medzi jednorozmernym, dvojroymernym polom a triedom Matrix, // bude nutne zladit a format v akom budeme pracovat s datami a potom zmazat CArray objArray = new CArray(); // Convert Size float[] m_M_K_fTemp1D = objArray.ArrTranf2Dto1D(m_M_K_Structure.m_fArrMembers); // Convert Type double[] m_M_K_dTemp1D = objArray.ArrConverFloatToDouble1D(m_M_K_fTemp1D); MatrixF64 objMatrix = new MatrixF64(6, 6, m_M_K_dTemp1D); // Print Created Matrix of MatrixF64 Class objMatrix.WriteLine(); // Get Inverse Global Stiffeness Matrix MatrixF64 objMatrixInv = objMatrix.Inverse(); // Print Inverse Matrix objMatrixInv.WriteLine(); // Convert Type float[] m_M_K_Inv_fTemp1D = objArray.ArrConverMatrixF64ToFloat1D(objMatrixInv); // Inverse Global Stiffeness Matrix of Structure - Allocate Memory (Matrix Size) CMatrix m_M_K_Structure_Inv = new CMatrix(m_iCodeNo); m_M_K_Structure_Inv.m_fArrMembers = objArray.ArrTranf1Dto2D(m_M_K_Inv_fTemp1D); ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Left side of Equation System ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Global Load Vector - Allocate Memory (Vector Size) m_V_Load = new CVector(m_iCodeNo); // Fill Global Load Vector FillGlobalLoadVectorOld(); // Display Global Load Vector m_V_Load.Print1DVector(); ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Solution - calculation of unknown displacement of nodes in GCS - system of linear equations // Start Solver ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Global Displacement Vector - Allocate Memory (Vector Size) m_V_Displ = new CVector(m_iCodeNo); // Fill Global Displacement Vector m_V_Displ = VectorF.fMultiplyMatrVectr(m_M_K_Structure_Inv, m_V_Load); ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // End Solver ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Display Global Displacemnt Vector - solution result //Console.ForegroundColor = ConsoleColor.Cyan; m_V_Displ.Print1DVector(); //Console.ForegroundColor = ConsoleColor.Green; //Console.BackgroundColor = ConsoleColor.White; // Set displacements and rotations of DOF in GCS to appropriate node DOF acc. to global code numbers for (int i = 0; i < m_iCodeNo; i++) { // Check if DOF is default (free - ) or has some initial value (settlement; soil consolidation etc.) // See Fill_NDisp_InitStr() for default values - float.PositiveInfinity if (m_NodeArray[m_fDisp_Vector_CN[i, 1]].m_VDisp.FVectorItems[m_fDisp_Vector_CN[i, 2]] == float.PositiveInfinity) m_NodeArray[m_fDisp_Vector_CN[i, 1]].m_VDisp.FVectorItems[m_fDisp_Vector_CN[i, 2]] = m_V_Displ.FVectorItems[i]; // set calculated else // some real initial value exists m_NodeArray[m_fDisp_Vector_CN[i, 1]].m_VDisp.FVectorItems[m_fDisp_Vector_CN[i, 2]] += m_V_Displ.FVectorItems[i]; // add calculated (to sum) } ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Get final end forces at element in global coordinate system GCS ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// for (int i = 0; i < iElemNoTot; i++) { m_ELemArray[i].GetArrElemEF_GCS_StNode(); Console.WriteLine("Element Index No.: " + i + "; " + "Node No.: " + m_ELemArray[i].NodeStart.ID + "; " + "Start Node End Forces in GCS"); m_ELemArray[i].m_VElemEF_GCS_StNode.Print1DVector(); m_ELemArray[i].GetArrElemEF_GCS_EnNode(); Console.WriteLine("Element Index No.: " + i + "; " + "Node No.: " + m_ELemArray[i].NodeEnd.ID + "; " + "End Node End Forces in GCS"); m_ELemArray[i].m_VElemEF_GCS_EnNode.Print1DVector(); m_ELemArray[i].GetArrElemEF_LCS_StNode(); Console.WriteLine("Element Index No.: " + i + "; " + "Node No.: " + m_ELemArray[i].NodeStart.ID + "; " + "Start Node End Forces in LCS"); m_ELemArray[i].m_VElemEF_LCS_StNode.Print1DVector(); m_ELemArray[i].GetArrElemEF_LCS_EnNode(); Console.WriteLine("Element Index No.: " + i + "; " + "Node No.: " + m_ELemArray[i].NodeEnd.ID + "; " + "End Node End Forces in LCS"); m_ELemArray[i].m_VElemEF_LCS_EnNode.Print1DVector(); m_ELemArray[i].GetArrElemIF_LCS_StNode(); Console.WriteLine("Element Index No.: " + i + "; " + "Node No.: " + m_ELemArray[i].NodeStart.ID + "; " + "Start Node Internal Forces in LCS"); m_ELemArray[i].m_VElemIF_LCS_StNode.Print1DVector(); m_ELemArray[i].GetArrElemIF_LCS_EnNode(); Console.WriteLine("Element Index No.: " + i + "; " + "Node No.: " + m_ELemArray[i].NodeEnd.ID + "; " + "End Node Internal Forces in LCS"); m_ELemArray[i].m_VElemIF_LCS_EnNode.Print1DVector(); } }
public static void Test() { System.Console.WriteLine(String.Empty.PadRight(78, '#')); System.Console.WriteLine(Environment.NewLine); System.Console.WriteLine("Test of MatrixF64"); System.Console.WriteLine(Environment.NewLine); System.Console.WriteLine(String.Empty.PadRight(78, '#')); System.Console.WriteLine(Environment.NewLine); System.Console.WriteLine(Environment.NewLine); System.Console.WriteLine("Test of matrix initialization:"); System.Console.WriteLine(Environment.NewLine); // A 5x3 matrix (5 rows x 3 columns) with // 64-bit floating-point entries: MatrixF64 m = new MatrixF64 ( 5, 3, 0.0, 1.0, 2.0, // row 0 3.0, 4.0, 5.0, // row 1 6.0, 7.0, 8.0, // row 2 9.0, 10.0, 11.0, // row 3 12.0, 13.0, 14.0 // row 4 ); m.WriteLine(); System.Console.WriteLine(Environment.NewLine); // [ 0, 1, 2 ] // row 0 // [ 3, 4, 5 ] // row 1 // [ 6, 7, 8 ] // row 2 // [ 9, 10, 11 ] // row 3 // [ 12, 13, 14 ] // row 4 // An 8x2 matrix (8 rows x 2 columns) with all 16 // 64-bit floating-point entries set to zero: MatrixF64 z = MatrixF64.Zero(8, 2); z.WriteLine(); System.Console.WriteLine(Environment.NewLine); // [ 0, 0 ] // row 0 // [ 0, 0 ] // row 1 // [ 0, 0 ] // row 2 // [ 0, 0 ] // row 3 // [ 0, 0 ] // row 4 // [ 0, 0 ] // row 5 // [ 0, 0 ] // row 6 // [ 0, 0 ] // row 7 // Examples of matrix addition, subtraction, and multiplication: System.Console.WriteLine(Environment.NewLine); System.Console.WriteLine("Test of matrix addition, subtraction, and multiplication:"); System.Console.WriteLine(Environment.NewLine); MatrixF64 a = new MatrixF64 ( 3, 5, 0.0, 1.0, 2.0, 3.0, 4.0, // row 0 5.0, 6.0, 7.0, 8.0, 9.0, // row 1 10.0, 11.0, 12.0, 13.0, 14.0 // row 2 ); MatrixF64 b = new MatrixF64 ( 3, 5, 4.0, -3.0, 2.0, -1.0, 0.0, // row 0 -9.0, 8.0, -7.0, 6.0, -5.0, // row 1 14.0, -13.0, 12.0, -11.0, 10.0 // row 2 ); MatrixF64 c = new MatrixF64 ( 5, 3, 0.0, 1.0, 2.0, // row 0 3.0, 4.0, 5.0, // row 1 6.0, 7.0, 8.0, // row 2 9.0, 10.0, 11.0, // row 3 12.0, 13.0, 14.0 // row 4 ); MatrixF64 result = new MatrixF64(); result = a + b; result.WriteLine(); System.Console.WriteLine(Environment.NewLine); // [ 4, -2, 4, 2, 4 ] // [ -4, 14, 0, 14, 4 ] // [ 24, -2, 24, 2, 24 ] result = a - b; result.WriteLine(); System.Console.WriteLine(Environment.NewLine); // [ -4, 4, 0, 4, 4 ] // [ 14, -2, 14, 2, 14 ] // [ -4, 24, 0, 24, 4 ] result = -a; result.WriteLine(); System.Console.WriteLine(Environment.NewLine); // [ 0, -1, -2, -3, -4 ] // [ -5, -6, -7, -8, -9 ] // [ -10, -11, -12, -13, -14 ] result = 3.0 * a; result.WriteLine(); System.Console.WriteLine(Environment.NewLine); // [ 0, 3, 6, 9, 12 ] // [ 15, 18, 21, 24, 27 ] // [ 30, 33, 36, 39, 42 ] result = a * c; // (3x5) * (5x3) = (3x3) result.WriteLine(); System.Console.WriteLine(Environment.NewLine); // [ 90, 100, 110 ] // [ 240, 275, 310 ] // [ 390, 450, 510 ] result = c * a; // (5x3) * (3x5) = (5x5) result.WriteLine(); System.Console.WriteLine(Environment.NewLine); // [ 25, 28, 31, 34, 37 ] // [ 70, 82, 94, 106, 118 ] // [ 115, 136, 157, 178, 199 ] // [ 160, 190, 220, 250, 280 ] // [ 205, 244, 283, 322, 361 ] // Examples of multiplying by an identity matrix: System.Console.WriteLine(Environment.NewLine); System.Console.WriteLine("Test of multiplying by an identity matrix:"); System.Console.WriteLine(Environment.NewLine); MatrixF64 identity3x3 = MatrixF64.Identity(3); identity3x3.WriteLine(); System.Console.WriteLine(Environment.NewLine); // [ 1, 0, 0 ] // [ 0, 1, 0 ] // [ 0, 0, 1 ] MatrixF64 s3x3 = new MatrixF64 ( 3, 3, 0.0, 1.0, 2.0, // row 0 3.0, 4.0, 5.0, // row 1 6.0, 7.0, 8.0 // row 2 ); result = s3x3 * identity3x3; result.WriteLine(); System.Console.WriteLine(Environment.NewLine); // [ 0, 1, 2 ] // [ 3, 4, 5 ] // [ 6, 7, 8 ] // (i.e., the same as s3x3) result = identity3x3 * s3x3; result.WriteLine(); System.Console.WriteLine(Environment.NewLine); // [ 0, 1, 2 ] // [ 3, 4, 5 ] // [ 6, 7, 8 ] // (i.e., the same as s3x3) // Example of LU factoring and back-substitution: System.Console.WriteLine(Environment.NewLine); System.Console.WriteLine("Test of LU factoring and back-substitution:"); System.Console.WriteLine(Environment.NewLine); MatrixF64 a3x3 = new MatrixF64 ( 3, 3, 0.0, 1.0, 2.0, // row 0 3.0, 4.0, 5.0, // row 1 6.0, 7.0, 8.0 // row 2 ); int[] rowPermutationOfOriginalMatrix = null; bool rowInterchangeParityIsOdd = false; MatrixF64 a3x3LU = null; MatrixF64.FindLUFactorsOfARowPermutedVersionOfAnOriginalMatrix ( a3x3, ref a3x3LU, ref rowPermutationOfOriginalMatrix, ref rowInterchangeParityIsOdd ); a3x3LU.WriteLine(); System.Console.WriteLine(Environment.NewLine); // [ 6, 7, 8 ] // [ 0, 1, 2 ] // [ 0.5, 0.5, 1e-19 ] // The matrix above is a combination of the L and U // matrices. The L matrix is 0 above the diagonal, // 1 on the diagonal, and the shown values below the // diagonal. The U matrix is 0 below the diagonal, // and the shown values on and above the diagonal. // This LU factoring is of a ROW-PERMUTED version // of the original matrix. // The following L and U matrices are manually formed // by inspecting the LU factoring matrix above. MatrixF64 L3x3 = new MatrixF64 ( 3, 3, 1.0, 0.0, 0.0, // row 0 0.0, 1.0, 0.0, // row 1 0.5, 0.5, 1.0 // row 2 ); MatrixF64 U3x3 = new MatrixF64 ( 3, 3, 6.0, 7.0, 8.0, // row 0 0.0, 1.0, 2.0, // row 1 0.0, 0.0, 0.0 // row 2 ); // The product (L)*(U) produces a ROW-PERMUTED version // of the original matrix: result = L3x3 * U3x3; result.WriteLine(4); System.Console.WriteLine(Environment.NewLine); // [ 6, 7, 8 ] // [ 0, 1, 2 ] // [ 3, 4, 5 ] VectorF64 b3x1 = new VectorF64(1.0, 2.0, 3.0); VectorF64 x3x1 = null; MatrixF64.LUBacksubstitution ( a3x3LU, rowPermutationOfOriginalMatrix, b3x1, ref x3x1 ); // Solution vector x3x1.WriteLine(); System.Console.WriteLine(Environment.NewLine); // ( -0.66666667, 1, 0 ) // Both the given b3x1 and this x3x1 are relative to the // original matrix, a3x3. The a3x3LU for a ROW-PERMUTED // version of a3x3, but LUBacksubstitution accepts a // non-permuted product vector (e.g., b3x1) and produces // a non-permuted solution vector (e.g., x3x1). // Check solution by multiplying the original matrix a3x3 // by the solution vector x3x1, to get a product vector. VectorF64 checkb3x1 = a3x3 * x3x1; checkb3x1.WriteLine(); System.Console.WriteLine(Environment.NewLine); // ( 1, 2, 3 ) // This matches the b3x1 vector we specified above, so // the solution is valid. // Examples of matrix determinants: System.Console.WriteLine(Environment.NewLine); System.Console.WriteLine("Test of matrix determinants:"); System.Console.WriteLine(Environment.NewLine); MatrixF64 d2x2 = new MatrixF64 ( 2, 2, 2.0, 5.0, // row 0 -4.0, -7.0 // row 1 ); double detd2x2 = d2x2.Determinant(); System.Console.WriteLine(detd2x2); System.Console.WriteLine(Environment.NewLine); // detd2x2 = 6 MatrixF64 d3x3 = new MatrixF64 ( 3, 3, 2.0, 5.0, 7.0, // row 0 -4.0, -1.0, 6.0, // row 1 9.0, 8.0, 3.0 // row 2 ); double detd3x3 = d3x3.Determinant(); System.Console.WriteLine(detd3x3); System.Console.WriteLine(Environment.NewLine); // detd3x3 = 67 MatrixF64 d4x4 = new MatrixF64 ( 4, 4, 7.0, -5.0, 2.0, 4.0, // row 0 3.0, 2.0, 6.0, 3.0, // row 1 -9.0, 8.0, -3.0, 2.0, // row 2 5.0, 3.0, 2.0, 5.0 // row 3 ); double detd4x4 = d4x4.Determinant(); System.Console.WriteLine(detd4x4); System.Console.WriteLine(Environment.NewLine); // detd4x4 = 1457 // Examples of matrix inverses: System.Console.WriteLine(Environment.NewLine); System.Console.WriteLine("Test of matrix inverses:"); System.Console.WriteLine(Environment.NewLine); MatrixF64 m2x2 = new MatrixF64 ( 2, 2, 2.0, 5.0, // row 0 -4.0, -7.0 // row 1 ); MatrixF64 inversem2x2 = m2x2.Inverse(); inversem2x2.WriteLine(); System.Console.WriteLine(Environment.NewLine); // [ -1.1666667, -0.83333333 ] // [ 0.66666667, 0.33333333 ] MatrixF64 prodinvm2x2andm2x2 = inversem2x2 * m2x2; prodinvm2x2andm2x2.WriteLine(); System.Console.WriteLine(Environment.NewLine); // [ 1, 8.8817842e-16 ] // [ 0, 1 ] // This product is very close to a // 2x2 identity matrix, as it should be. MatrixF64 prodm2x2andinvm2x2 = m2x2 * inversem2x2; prodm2x2andinvm2x2.WriteLine(); System.Console.WriteLine(Environment.NewLine); // [ 1, 0 ] // [ 0, 1 ] // This product is the 2x2 identity matrix, // as it should be. MatrixF64 m3x3 = new MatrixF64 ( 3, 3, 2.0, 5.0, 7.0, // row 0 -4.0, -1.0, 6.0, // row 1 9.0, 8.0, 3.0 // row 2 ); MatrixF64 inversem3x3 = m3x3.Inverse(); inversem3x3.WriteLine(); System.Console.WriteLine(Environment.NewLine); // [ -0.76119403, 0.6119403, 0.55223881 ] // [ 0.98507463, -0.85074627, -0.59701493 ] // [ -0.34328358, 0.43283582, 0.26865672 ] MatrixF64 prodinvm3x3andm3x3 = inversem3x3 * m3x3; prodinvm3x3andm3x3.WriteLine(); System.Console.WriteLine(Environment.NewLine); // [ 1, 0, 4.4408921e-16 ] // [ 0, 1, 0 ] // [ 0, 0, 1 ] // This product is very close to a 3x3 identity matrix, // as it should be. MatrixF64 prodm3x3andinvm3x3 = m3x3 * inversem3x3; prodm3x3andinvm3x3.WriteLine(); System.Console.WriteLine(Environment.NewLine); // [ 1, -4.4408921e-16, 4.4408921e-16 ] // [ 0, 1, -2.220446e-16 ] // [ -4.4408921e-16, 0, 1 ] // This product is very close to a 3x3 identity matrix, // as it should be. // Examples of a product of a matrix and a vector: System.Console.WriteLine(Environment.NewLine); System.Console.WriteLine("Test of a product of a matrix and a vector:"); System.Console.WriteLine(Environment.NewLine); MatrixF64 t3x3 = new MatrixF64 ( 3, 3, 2.0, 5.0, 7.0, // row 0 -4.0, -1.0, 6.0, // row 1 9.0, 8.0, 3.0 // row 2 ); VectorF64 p3x1 = new VectorF64(1.0, 2.0, 3.0); VectorF64 q3x1 = t3x3 * p3x1; q3x1.WriteLine(); System.Console.WriteLine(Environment.NewLine); // ( 33, 12, 34 ) // The multiplicative product of the inverse // of the matrix and the result vector computed // above should produce the original vector. MatrixF64 inverset3x3 = t3x3.Inverse(); VectorF64 prodinvt3x3andq3x1 = inverset3x3 * q3x1; prodinvt3x3andq3x1.WriteLine(); System.Console.WriteLine(Environment.NewLine); // ( 1, 2, 3 ) // This vector is equal to the original vector, // as expected. // Example of using a homogeneous matrix to // translate a homogeneous vector: System.Console.WriteLine(Environment.NewLine); System.Console.WriteLine("Test of using a homogeneous matrix to translate a homogeneous vector:"); System.Console.WriteLine(Environment.NewLine); // Forming a homogeneous matrix that represents // a translation: VectorF64 homogeneousTranslationVector4x1 = new VectorF64(10.0, 20.0, 30.0, 1.0); MatrixF64 translation4x4 = MatrixF64.FormHomogeneousTranslationMatrix (homogeneousTranslationVector4x1); translation4x4.WriteLine(); System.Console.WriteLine(Environment.NewLine); // [ 1, 0, 0, 10 ] // [ 0, 1, 0, 20 ] // [ 0, 0, 1, 30 ] // [ 0, 0, 0, 1 ] // Using the homogeneous matrix to translate // a homogeneous vector: VectorF64 homogeneousVector = new VectorF64(5.0, 6.0, 7.0, 1.0); VectorF64 translatedVector = translation4x4 * homogeneousVector; translatedVector.WriteLine(); System.Console.WriteLine(Environment.NewLine); // ( 15, 26, 37, 1 ) // The relevant vector components (5,6,7) // were translated by (10,20,30) by the // homogeneous translation matrix. // Example of using a homogeneous matrix // to rotate a homogeneous vector: System.Console.WriteLine(Environment.NewLine); System.Console.WriteLine("Test of using a homogeneous matrix to rotate a homogeneous vector:"); System.Console.WriteLine(Environment.NewLine); // Forming a homogeneous rotation matrix that // rotates a positive component along coordinate // axis 0 to a positive component along // cooridnate axis 1, in 3-dimensional space. // (The homogeneous matrix must be (3+1)=4 // rows by (3+1)=4 columns.) MatrixF64 rotation4x4 = MatrixF64.Rab(4, 0, 1, (0.5 * Math.PI)); rotation4x4.WriteLine(3); System.Console.WriteLine(Environment.NewLine); // [ 6.12e-17, -1, 0, 0 ] // [ 1, 6.12e-17, 0, 0 ] // [ 0, 0, 1, 0 ] // [ 0, 0, 0, 1 ] // (The entries '6.12e-17' are negligible.) // The upper-left 3x3 part of the matrix represents // a counterclockwise rotation of a quarter-turn // such that a positive component along axis 0 // would be rotated to a positive component along // axis 1, in 3-dimensional space. // Using the homogeneous rotation matrix to // rotate a homogeneous vector: VectorF64 homogeneousVector2 = new VectorF64(5.0, 6.0, 7.0, 1.0); VectorF64 rotatedVector = rotation4x4 * homogeneousVector2; rotatedVector.WriteLine(); System.Console.WriteLine(Environment.NewLine); // ( -6, 5, 7, 1 ) // The relevant vector components (5,6,7) were // rotated to (-6,5,7), which is consistent with // a counterclockwise quarter-turn rotation // with a rotation plane parallel to axes 0 and 1. System.Console.WriteLine(Environment.NewLine); System.Console.WriteLine(String.Empty.PadRight(78, '#')); System.Console.WriteLine(Environment.NewLine); }
public static void LUBacksubstitution( MatrixF64 LUFactorsMatrix, int[] rowPermutationOfOriginalMatrix, VectorF64 givenProductVector, ref VectorF64 solutionVector ) { // This implementation is based on pseudo-code appearing in // ''Introduction to Algorithms'' (Cormen; Lieserson; Rivest; // 24th printing, 2000; MIT Press; ISBN 0-262-03141-8). // See the section named ''Overview of LUP decomposition'', in // chapter 31 (''Matrix Operations''). The implementation here // follows the ''LUP-Solve'' pseudo-code appearing in a // section named ''forward and back substitution''. solutionVector = null; if (null == LUFactorsMatrix) { return; // No matrix specified. } if (null == rowPermutationOfOriginalMatrix) { return; // No permutation specified. } if (null == givenProductVector) { return; // No product vector specified. } if ( (null == LUFactorsMatrix.entries) || (LUFactorsMatrix.totalRows <= 0) || (LUFactorsMatrix.totalColumns <= 0) ) { return; // Matrix is empty. } if (LUFactorsMatrix.totalRows != LUFactorsMatrix.totalColumns) { return; // Matrix is not square. } if (givenProductVector.Dimensions() != LUFactorsMatrix.totalRows) { return; // Product vector size not equal to matrix row count. } // Copy the product vector in to the result. solutionVector = new VectorF64(givenProductVector); // (See LUP-Solve, lines 2-3) for (int i = 0; i < LUFactorsMatrix.totalRows; i++) { double sum = givenProductVector[rowPermutationOfOriginalMatrix[i]]; for (int j = 0; j < i; j++) { sum -= (LUFactorsMatrix[i, j] * solutionVector[j]); } solutionVector[i] = sum; } // (See LUP-Solve, lines 4-5) for (int i = (LUFactorsMatrix.totalRows - 1); i >= 0; i--) { double sum = solutionVector[i]; for ( int j = (i + 1); j < LUFactorsMatrix.totalColumns; j++ ) { sum -= (LUFactorsMatrix[i, j] * solutionVector[j]); } double diagonalElement = LUFactorsMatrix[i, i]; if (diagonalElement != 0.0) { solutionVector[i] = (sum / diagonalElement); } } }
public static void FindLUFactorsOfARowPermutedVersionOfAnOriginalMatrix( MatrixF64 originalMatrix, ref MatrixF64 LUFactorsMatrix, ref int[] rowPermutationOfOriginalMatrix, ref bool rowExchangeParityIsOdd ) { // ''LU factoring'' involves factoring a square matrix (M) in to a // lower-triangular matrix (L), and an upper-triangular matrix (U), // such that (L)*(U) = (M). // // However, the specified matrix (originalMatrix) might not be // optimal for direct LU factoring, due to the locations of extreme // values in the matrix. Therefore, this function instead implicitly // factors a row-permuted version of originalMatrix, where the // permutation of rows is determined by the values in the specified // matrix. The specific row permutation selected by this function is // returned in an array of row indices (rowPermutationOfOriginalMatrix). // The resulting (L) and (U) factors are merged in to a single // output matrix (LUFactorsMatrix). Therefore: // // (L part of LUFactorsMatrix) * (U part of LUFactorsMatrix) // = (originalMatrix with rows permuted according to // rowPermutationOfOriginalMatrix). // // Although factoring a row-permuted version of originalMatrix // makes it more difficult to interpret the results of this function, // the resulting LU factoring is likely to be more accurate. In a // sense, this function indicates (via rowPermutationOfOriginalMatrix) // how to permute the rows of originalMatrix to be able to produce // the most accurate LU factoring directly, and this function produces // that factoring (via the LUFactorsMatrix output). // // When using the (L) and (U) matrices (merged in to LUFactorsMatrix) // to solve a system of equations, it is necessary to permute the rows // of the solution vector of the system of equations according to // rowPermutation. // // The output matrix (LUFactorsMatrix) is a combination of // the (L) and (U) matrices: // // To form the (L) matrix from (LUFactorsMatrix): // assume (0) for entries above the diagonal, // assume (1) for entries on the diagonal, // and use (LUFactorsMatrix) entries below the diagonal. // // To form the (U) matrix from (LUFactorsMatrix): // assume (0) for entries below the diagonal, // and use (LUFactorsMatrix) entries on and above the diagonal. // // The output array (rowPermutationOfOriginalMatrix) indicates how the // rows of the original matrix have implicitly (not actually) been // permuted. // // The output boolean (rowExchangeParityIsOdd) indicates if the parity // of the permutation of rows is odd. // // This implementation is partly based on pseudo-code appearing in // ''Introduction to Algorithms'' (Cormen; Lieserson; Rivest; // 24th printing, 2000; MIT Press; ISBN 0-262-03141-8). // See the section named ''Overview of LUP decomposition'', in // chapter 31 (''Matrix Operations''). The implementation here follows // the ''LUP-Decomposition'' pseudo-code appearing in a section named // ''Computing an LUP decomposition''. // // This implementation also uses ideas found in the implementation of // ''ludcmp'' in the book ''Numerical recipes in C : The art of // scientific computing'' (Press; Teukolsky; Vetterling; Flannery; // second edition; 1996 reprinting; Cambridge University Press). // The overall loop structure here resembles that of ''ludcmp''. // The idea of determining and caching row scale factors in advance of // finding pivots is used here. The method in ''ludcmp'' to avoid zero // pivot values has been modified here to handle excessively-small // pivot values, too. HOWEVER, the row permutation indices produced by // the following implementation is a true permutation of // {0,1,...,(totalRows-1)}, whereas ''ludcmp'' (and ''lubksb'') // interpret their ''permutation indices'' as swaps to perform // (roughly, for each (i), swap b[ i ] and b[ indx[i] ]). So, the // following implementation of LU-factoring is not directly compatible // with ''ludcmp'' or ''lubksb''. Also, the following procedure // does not destroy the original matrix (unlike ''ludcmp''). double singularMatrixIfMaxRowElementIsLessThanThis = (1.0e-19); double forcePivotsToHaveAtLeastThisAbsoluteValue = (1.0e-19); LUFactorsMatrix = null; rowPermutationOfOriginalMatrix = null; rowExchangeParityIsOdd = false; if (null == originalMatrix) { return; // No matrix specified. } if ( (originalMatrix.totalRows <= 0) || (originalMatrix.totalColumns <= 0) || (null == originalMatrix.entries) ) { return; // Matrix is empty. } if (originalMatrix.totalRows != originalMatrix.totalColumns) { return; // Matrix is not square. } // Duplicate the original matrix LUFactorsMatrix = new MatrixF64(originalMatrix); // (Lines 2-3 of LUP-Decomposition) // Initialize the row permutation array to the identity // permutation. rowPermutationOfOriginalMatrix = new int[LUFactorsMatrix.totalRows]; for (int i = 0; i < LUFactorsMatrix.totalRows; i++) { rowPermutationOfOriginalMatrix[i] = i; } // For each row, determine the largest absolute value of // the row elements, and use the reciprocal as the scale for // that row. VectorF64 rowScales = VectorF64.Zero(LUFactorsMatrix.totalRows); for (int i = 0; i < LUFactorsMatrix.totalRows; i++) { double largestElementAbsoluteValueInRow = 0.0; for (int j = 0; j < LUFactorsMatrix.totalColumns; j++) { double absoluteElementValue = Math.Abs(LUFactorsMatrix[i, j]); if (absoluteElementValue > largestElementAbsoluteValueInRow) { largestElementAbsoluteValueInRow = absoluteElementValue; } } if (largestElementAbsoluteValueInRow < singularMatrixIfMaxRowElementIsLessThanThis) { return; // Matrix is singular } rowScales[i] = (1.0 / largestElementAbsoluteValueInRow); } // (Lines 4-18 of LUP-Decomposition) // Go through the columns of the matrix. for (int j = 0; j < LUFactorsMatrix.totalColumns; j++) { // (Lines 17-18 of LUP-Decomposition) // (or equation (2.3.12) of NR) for (int i = 0; i < j; i++) { double sum = LUFactorsMatrix[i, j]; for (int k = 0; k < i; k++) { sum -= (LUFactorsMatrix[i, k] * LUFactorsMatrix[k, j]); } LUFactorsMatrix[i, j] = sum; } // Go through all remaining rows and find the best pivot. double largestScaledSum = 0.0; int rowIndexOfLargestScaledSum = j; for (int i = j; i < LUFactorsMatrix.totalRows; i++) { // (equation (2.3.13) of NR) double sum = LUFactorsMatrix[i, j]; for (int k = 0; k < j; k++) { sum -= (LUFactorsMatrix[i, k] * LUFactorsMatrix[k, j]); } LUFactorsMatrix[i, j] = sum; double scaledSum = rowScales[i] * Math.Abs(sum); if (scaledSum >= largestScaledSum) { largestScaledSum = scaledSum; rowIndexOfLargestScaledSum = i; } } // If indeed we found a better pivot, then exchange rows. if (j != rowIndexOfLargestScaledSum) { // (Line 12 of LUP-Decomposition) // Exchange the row permutation indices int tempRowIndex = rowPermutationOfOriginalMatrix[j]; rowPermutationOfOriginalMatrix[j] = rowPermutationOfOriginalMatrix[rowIndexOfLargestScaledSum]; rowPermutationOfOriginalMatrix[rowIndexOfLargestScaledSum] = tempRowIndex; // (Lines 13-14 of LUP-Decomposition) // Exchange the elements of the rows for (int k = 0; k < LUFactorsMatrix.totalColumns; k++) { double temp = LUFactorsMatrix[rowIndexOfLargestScaledSum, k]; LUFactorsMatrix[rowIndexOfLargestScaledSum, k] = LUFactorsMatrix[j, k]; LUFactorsMatrix[j, k] = temp; } // Exchange the row scale factors double scaleFactor = rowScales[rowIndexOfLargestScaledSum]; rowScales[rowIndexOfLargestScaledSum] = rowScales[j]; rowScales[j] = scaleFactor; // Invert the overall row exchange parity rowExchangeParityIsOdd = (!(rowExchangeParityIsOdd)); } // Force the pivot element to have at least a certain // absolute value. if (Math.Abs(LUFactorsMatrix[j, j]) < forcePivotsToHaveAtLeastThisAbsoluteValue) { if (LUFactorsMatrix[j, j] < 0.0) { LUFactorsMatrix[j, j] = (-(forcePivotsToHaveAtLeastThisAbsoluteValue)); } else { LUFactorsMatrix[j, j] = forcePivotsToHaveAtLeastThisAbsoluteValue; } } // If not the final column, then divide all column elements // below the diagonal by the pivot element, matrixLU[j,j]. // (Lines 15-16 of LUP-Decomposition) if (j != (LUFactorsMatrix.totalColumns - 1)) { double reciprocalOfPivot = (1.0 / LUFactorsMatrix[j, j]); for (int i = (j + 1); i < LUFactorsMatrix.totalRows; i++) { LUFactorsMatrix[i, j] *= reciprocalOfPivot; } } } }
public static bool EqualsWithinTolerance( MatrixF64 a, MatrixF64 b, double tolerance ) { if ((null == a) || (null == b)) { return (false); } if ( (null == a.entries) || (null == b.entries) || (a.totalRows != b.totalRows) || (a.totalColumns != b.totalColumns) ) { return (false); } for (int i = 0; i < a.totalRows; i++) { for (int j = 0; j < a.totalColumns; j++) { double absoluteDifference = Math.Abs(a[i, j] - b[i, j]); if (absoluteDifference > tolerance) { return (false); } } } return (true); }
public MatrixF64 TranslateByAHomogeneousVector(VectorF64 v) { // This function assumes a square matrix that represents a // homogeneous transformation (with a final row that is all // zero except for a final entry of one), and the specification // of a homogeneous vector (with a final component of one). // All but the final component of the vector will contribute // to the final column vector of the matrix. if (null == v) { return (MatrixF64.Zero(1, 1)); // Vector is not specified. } if ( (this.totalRows <= 0) || (this.totalColumns <= 0) || (null == this.entries) ) { return (MatrixF64.Zero(1, 1)); // Matrix is empty. } if (v.Dimensions() != this.totalRows) { // The vector size is not equal to the matrix total rows. return (MatrixF64.Zero(this.totalRows, this.totalColumns)); } // Add all but the final component of the specified vector // to the final column of this matrix. The final entry // of the final column of the matrix will remain one (1). MatrixF64 result = new MatrixF64(this); for (int i = 0; i < (result.totalRows - 1); i++) { result[i, (result.totalColumns - 1)] += v[i]; } return (result); }
public MatrixF64 SnapToNearestInteger(double tolerance) { // example tolerance: 1.0e-5 MatrixF64 result = new MatrixF64(this); for (int i = 0; i < result.totalRows; i++) { for (int j = 0; j < result.totalColumns; j++) { double doubleValue = result[i, j]; // double only has 53 bits for mantissa, so we'll // conservatively limit our snap attempts to values // less than 2^50 = 1125899906842624. long maxSnapValue = 1125899906842624; if (Math.Abs(doubleValue) <= maxSnapValue) { long longValue = (long)(doubleValue + 0.5); if (doubleValue < (-0.5)) { // The interval [-(N).499,-(N-1).501] should be // shifted so that truncation produces -(N). longValue = (long)(doubleValue - 0.5); } double absoluteDifference = Math.Abs(doubleValue - (double)longValue); if (absoluteDifference < tolerance) { result[i, j] = (double)longValue; } } } } return (result); }
public MatrixF64 Inverse() { if ( (this.totalRows <= 0) || (this.totalColumns <= 0) || (null == this.entries) ) { return (MatrixF64.Zero(1, 1)); // Matrix is empty. } if (this.totalRows != this.totalColumns) { // Matrix is not square return (MatrixF64.Zero(1, 1)); } // Simple cases if (1 == this.totalRows) { return (this.Inverse1x1()); } if (2 == this.totalRows) { return (this.Inverse2x2()); } if (3 == this.totalRows) { return (this.Inverse3x3()); } if (4 == this.totalRows) { return (this.Inverse4x4()); } int[] rowPermutationOfOriginalMatrix = null; bool rowExchangeParityIsOdd = false; MatrixF64 originalMatrix = new MatrixF64(this); MatrixF64 LUFactorsMatrix = null; MatrixF64.FindLUFactorsOfARowPermutedVersionOfAnOriginalMatrix ( originalMatrix, ref LUFactorsMatrix, ref rowPermutationOfOriginalMatrix, ref rowExchangeParityIsOdd ); if ((null == LUFactorsMatrix) || (null == rowPermutationOfOriginalMatrix)) { return (MatrixF64.Zero(this.totalRows, this.totalColumns)); } VectorF64 columnVector = VectorF64.Zero(totalRows); VectorF64 solutionVector = VectorF64.Zero(totalRows); MatrixF64 inverse = MatrixF64.Zero(this.totalRows, this.totalColumns); for (int j = 0; j < inverse.totalColumns; j++) { for (int i = 0; i < inverse.totalRows; i++) { columnVector[i] = 0.0; } columnVector[j] = 1.0; MatrixF64.LUBacksubstitution ( LUFactorsMatrix, rowPermutationOfOriginalMatrix, columnVector, ref solutionVector ); if (null != solutionVector) { for (int i = 0; i < inverse.totalRows; i++) { inverse[i, j] = solutionVector[i]; } } } return (inverse); }
public double Determinant() { if ( (this.totalRows <= 0) || (this.totalColumns <= 0) || (null == this.entries) ) { return (0.0); // Matrix is empty. } if (this.totalRows != this.totalColumns) { // Matrix is not square return (0.0); } // Simple cases if (1 == this.totalRows) { return (this.Determinant1x1()); } if (2 == this.totalRows) { return (this.Determinant2x2()); } if (3 == this.totalRows) { return (this.Determinant3x3()); } if (4 == this.totalRows) { return (this.Determinant4x4()); } int[] rowPermutationOfOriginalMatrix = null; bool rowExchangeParityIsOdd = false; MatrixF64 originalMatrix = new MatrixF64(this); MatrixF64 LUFactorsMatrix = null; MatrixF64.FindLUFactorsOfARowPermutedVersionOfAnOriginalMatrix ( originalMatrix, ref LUFactorsMatrix, ref rowPermutationOfOriginalMatrix, ref rowExchangeParityIsOdd ); if ((null == LUFactorsMatrix) || (null == rowPermutationOfOriginalMatrix)) { return (0.0); } double determinant = 1.0; if (true == rowExchangeParityIsOdd) { determinant = (-1.0); } for (int j = 0; j < LUFactorsMatrix.totalColumns; j++) { determinant *= LUFactorsMatrix[j, j]; } return (determinant); }
public static MatrixF64 Zero(int rows, int columns) { MatrixF64 zero = new MatrixF64(rows, columns); // The constructor above sets all entries to zero. return (zero); }