mbed official / mbed-dsp

CMSIS DSP library

Dependents:   performance_timer Surfboard_ gps2rtty Capstone ... more

Legacy Warning

This is an mbed 2 library. To learn more about mbed OS 5, visit the docs.

cmsis_dsp/MatrixFunctions/arm_mat_inverse_f32.c@3:7a284390b0ce, 2013-11-08 (annotated)

Committer:
mbed_official
Date:
Fri Nov 08 13:45:10 2013 +0000
Revision:
3:7a284390b0ce
Parent:
2:da51fb522205
Child:
5:3762170b6d4d
Synchronized with git revision e69956aba2f68a2a26ac26b051f8d349deaa1ce8

Who changed what in which revision?

UserRevisionLine numberNew contents of line
emilmont 1:fdd22bb7aa52 1 /* ----------------------------------------------------------------------
mbed_official 3:7a284390b0ce 2 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
emilmont 1:fdd22bb7aa52 3 *
mbed_official 3:7a284390b0ce 4 * $Date: 1. March 2013
mbed_official 3:7a284390b0ce 5 * $Revision: V1.4.1
emilmont 1:fdd22bb7aa52 6 *
emilmont 2:da51fb522205 7 * Project: CMSIS DSP Library
emilmont 2:da51fb522205 8 * Title: arm_mat_inverse_f32.c
emilmont 1:fdd22bb7aa52 9 *
emilmont 2:da51fb522205 10 * Description: Floating-point matrix inverse.
emilmont 1:fdd22bb7aa52 11 *
emilmont 1:fdd22bb7aa52 12 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
emilmont 1:fdd22bb7aa52 13 *
mbed_official 3:7a284390b0ce 14 * Redistribution and use in source and binary forms, with or without
mbed_official 3:7a284390b0ce 15 * modification, are permitted provided that the following conditions
mbed_official 3:7a284390b0ce 16 * are met:
mbed_official 3:7a284390b0ce 17 * - Redistributions of source code must retain the above copyright
mbed_official 3:7a284390b0ce 18 * notice, this list of conditions and the following disclaimer.
mbed_official 3:7a284390b0ce 19 * - Redistributions in binary form must reproduce the above copyright
mbed_official 3:7a284390b0ce 20 * notice, this list of conditions and the following disclaimer in
mbed_official 3:7a284390b0ce 21 * the documentation and/or other materials provided with the
mbed_official 3:7a284390b0ce 22 * distribution.
mbed_official 3:7a284390b0ce 23 * - Neither the name of ARM LIMITED nor the names of its contributors
mbed_official 3:7a284390b0ce 24 * may be used to endorse or promote products derived from this
mbed_official 3:7a284390b0ce 25 * software without specific prior written permission.
mbed_official 3:7a284390b0ce 26 *
mbed_official 3:7a284390b0ce 27 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
mbed_official 3:7a284390b0ce 28 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
mbed_official 3:7a284390b0ce 29 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
mbed_official 3:7a284390b0ce 30 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
mbed_official 3:7a284390b0ce 31 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
mbed_official 3:7a284390b0ce 32 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
mbed_official 3:7a284390b0ce 33 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
mbed_official 3:7a284390b0ce 34 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
mbed_official 3:7a284390b0ce 35 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
mbed_official 3:7a284390b0ce 36 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
mbed_official 3:7a284390b0ce 37 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
mbed_official 3:7a284390b0ce 38 * POSSIBILITY OF SUCH DAMAGE.
emilmont 1:fdd22bb7aa52 39 * -------------------------------------------------------------------- */
emilmont 1:fdd22bb7aa52 40
emilmont 1:fdd22bb7aa52 41 #include "arm_math.h"
emilmont 1:fdd22bb7aa52 42
emilmont 1:fdd22bb7aa52 43 /**
emilmont 1:fdd22bb7aa52 44 * @ingroup groupMatrix
emilmont 1:fdd22bb7aa52 45 */
emilmont 1:fdd22bb7aa52 46
emilmont 1:fdd22bb7aa52 47 /**
emilmont 1:fdd22bb7aa52 48 * @defgroup MatrixInv Matrix Inverse
emilmont 1:fdd22bb7aa52 49 *
emilmont 1:fdd22bb7aa52 50 * Computes the inverse of a matrix.
emilmont 1:fdd22bb7aa52 51 *
emilmont 1:fdd22bb7aa52 52 * The inverse is defined only if the input matrix is square and non-singular (the determinant
emilmont 1:fdd22bb7aa52 53 * is non-zero). The function checks that the input and output matrices are square and of the
emilmont 1:fdd22bb7aa52 54 * same size.
emilmont 1:fdd22bb7aa52 55 *
emilmont 1:fdd22bb7aa52 56 * Matrix inversion is numerically sensitive and the CMSIS DSP library only supports matrix
emilmont 1:fdd22bb7aa52 57 * inversion of floating-point matrices.
emilmont 1:fdd22bb7aa52 58 *
emilmont 1:fdd22bb7aa52 59 * \par Algorithm
emilmont 1:fdd22bb7aa52 60 * The Gauss-Jordan method is used to find the inverse.
emilmont 1:fdd22bb7aa52 61 * The algorithm performs a sequence of elementary row-operations till it
emilmont 1:fdd22bb7aa52 62 * reduces the input matrix to an identity matrix. Applying the same sequence
emilmont 1:fdd22bb7aa52 63 * of elementary row-operations to an identity matrix yields the inverse matrix.
emilmont 1:fdd22bb7aa52 64 * If the input matrix is singular, then the algorithm terminates and returns error status
emilmont 1:fdd22bb7aa52 65 * <code>ARM_MATH_SINGULAR</code>.
emilmont 1:fdd22bb7aa52 66 * \image html MatrixInverse.gif "Matrix Inverse of a 3 x 3 matrix using Gauss-Jordan Method"
emilmont 1:fdd22bb7aa52 67 */
emilmont 1:fdd22bb7aa52 68
emilmont 1:fdd22bb7aa52 69 /**
emilmont 1:fdd22bb7aa52 70 * @addtogroup MatrixInv
emilmont 1:fdd22bb7aa52 71 * @{
emilmont 1:fdd22bb7aa52 72 */
emilmont 1:fdd22bb7aa52 73
emilmont 1:fdd22bb7aa52 74 /**
emilmont 1:fdd22bb7aa52 75 * @brief Floating-point matrix inverse.
emilmont 1:fdd22bb7aa52 76 * @param[in] *pSrc points to input matrix structure
emilmont 1:fdd22bb7aa52 77 * @param[out] *pDst points to output matrix structure
emilmont 2:da51fb522205 78 * @return The function returns
emilmont 1:fdd22bb7aa52 79 * <code>ARM_MATH_SIZE_MISMATCH</code> if the input matrix is not square or if the size
emilmont 1:fdd22bb7aa52 80 * of the output matrix does not match the size of the input matrix.
emilmont 1:fdd22bb7aa52 81 * If the input matrix is found to be singular (non-invertible), then the function returns
emilmont 1:fdd22bb7aa52 82 * <code>ARM_MATH_SINGULAR</code>. Otherwise, the function returns <code>ARM_MATH_SUCCESS</code>.
emilmont 1:fdd22bb7aa52 83 */
emilmont 1:fdd22bb7aa52 84
emilmont 1:fdd22bb7aa52 85 arm_status arm_mat_inverse_f32(
emilmont 1:fdd22bb7aa52 86 const arm_matrix_instance_f32 * pSrc,
emilmont 1:fdd22bb7aa52 87 arm_matrix_instance_f32 * pDst)
emilmont 1:fdd22bb7aa52 88 {
emilmont 1:fdd22bb7aa52 89 float32_t *pIn = pSrc->pData; /* input data matrix pointer */
emilmont 1:fdd22bb7aa52 90 float32_t *pOut = pDst->pData; /* output data matrix pointer */
emilmont 1:fdd22bb7aa52 91 float32_t *pInT1, *pInT2; /* Temporary input data matrix pointer */
emilmont 1:fdd22bb7aa52 92 float32_t *pInT3, *pInT4; /* Temporary output data matrix pointer */
emilmont 1:fdd22bb7aa52 93 float32_t *pPivotRowIn, *pPRT_in, *pPivotRowDst, *pPRT_pDst; /* Temporary input and output data matrix pointer */
emilmont 1:fdd22bb7aa52 94 uint32_t numRows = pSrc->numRows; /* Number of rows in the matrix */
emilmont 1:fdd22bb7aa52 95 uint32_t numCols = pSrc->numCols; /* Number of Cols in the matrix */
emilmont 1:fdd22bb7aa52 96
mbed_official 3:7a284390b0ce 97 #ifndef ARM_MATH_CM0_FAMILY
mbed_official 3:7a284390b0ce 98 float32_t maxC; /* maximum value in the column */
emilmont 1:fdd22bb7aa52 99
emilmont 1:fdd22bb7aa52 100 /* Run the below code for Cortex-M4 and Cortex-M3 */
emilmont 1:fdd22bb7aa52 101
emilmont 1:fdd22bb7aa52 102 float32_t Xchg, in = 0.0f, in1; /* Temporary input values */
emilmont 1:fdd22bb7aa52 103 uint32_t i, rowCnt, flag = 0u, j, loopCnt, k, l; /* loop counters */
emilmont 1:fdd22bb7aa52 104 arm_status status; /* status of matrix inverse */
emilmont 1:fdd22bb7aa52 105
emilmont 1:fdd22bb7aa52 106 #ifdef ARM_MATH_MATRIX_CHECK
emilmont 1:fdd22bb7aa52 107
emilmont 1:fdd22bb7aa52 108
emilmont 1:fdd22bb7aa52 109 /* Check for matrix mismatch condition */
emilmont 1:fdd22bb7aa52 110 if((pSrc->numRows != pSrc->numCols) || (pDst->numRows != pDst->numCols)
emilmont 1:fdd22bb7aa52 111 || (pSrc->numRows != pDst->numRows))
emilmont 1:fdd22bb7aa52 112 {
emilmont 1:fdd22bb7aa52 113 /* Set status as ARM_MATH_SIZE_MISMATCH */
emilmont 1:fdd22bb7aa52 114 status = ARM_MATH_SIZE_MISMATCH;
emilmont 1:fdd22bb7aa52 115 }
emilmont 1:fdd22bb7aa52 116 else
emilmont 1:fdd22bb7aa52 117 #endif /* #ifdef ARM_MATH_MATRIX_CHECK */
emilmont 1:fdd22bb7aa52 118
emilmont 1:fdd22bb7aa52 119 {
emilmont 1:fdd22bb7aa52 120
emilmont 1:fdd22bb7aa52 121 /*--------------------------------------------------------------------------------------------------------------
emilmont 2:da51fb522205 122 * Matrix Inverse can be solved using elementary row operations.
emilmont 2:da51fb522205 123 *
emilmont 2:da51fb522205 124 * Gauss-Jordan Method:
emilmont 2:da51fb522205 125 *
emilmont 2:da51fb522205 126 * 1. First combine the identity matrix and the input matrix separated by a bar to form an
emilmont 2:da51fb522205 127 * augmented matrix as follows:
emilmont 2:da51fb522205 128 * _ _ _ _
emilmont 2:da51fb522205 129 * | a11 a12 | 1 0 | | X11 X12 |
emilmont 2:da51fb522205 130 * | | | = | |
emilmont 2:da51fb522205 131 * |_ a21 a22 | 0 1 _| |_ X21 X21 _|
emilmont 2:da51fb522205 132 *
emilmont 2:da51fb522205 133 * 2. In our implementation, pDst Matrix is used as identity matrix.
emilmont 2:da51fb522205 134 *
emilmont 2:da51fb522205 135 * 3. Begin with the first row. Let i = 1.
emilmont 2:da51fb522205 136 *
mbed_official 3:7a284390b0ce 137 * 4. Check to see if the pivot for column i is the greatest of the column.
emilmont 2:da51fb522205 138 * The pivot is the element of the main diagonal that is on the current row.
emilmont 2:da51fb522205 139 * For instance, if working with row i, then the pivot element is aii.
mbed_official 3:7a284390b0ce 140 * If the pivot is not the most significant of the coluimns, exchange that row with a row
mbed_official 3:7a284390b0ce 141 * below it that does contain the most significant value in column i. If the most
mbed_official 3:7a284390b0ce 142 * significant value of the column is zero, then an inverse to that matrix does not exist.
mbed_official 3:7a284390b0ce 143 * The most significant value of the column is the absolut maximum.
emilmont 2:da51fb522205 144 *
emilmont 2:da51fb522205 145 * 5. Divide every element of row i by the pivot.
emilmont 2:da51fb522205 146 *
emilmont 2:da51fb522205 147 * 6. For every row below and row i, replace that row with the sum of that row and
emilmont 2:da51fb522205 148 * a multiple of row i so that each new element in column i below row i is zero.
emilmont 2:da51fb522205 149 *
emilmont 2:da51fb522205 150 * 7. Move to the next row and column and repeat steps 2 through 5 until you have zeros
emilmont 2:da51fb522205 151 * for every element below and above the main diagonal.
emilmont 2:da51fb522205 152 *
emilmont 2:da51fb522205 153 * 8. Now an identical matrix is formed to the left of the bar(input matrix, pSrc).
emilmont 2:da51fb522205 154 * Therefore, the matrix to the right of the bar is our solution(pDst matrix, pDst).
emilmont 2:da51fb522205 155 *----------------------------------------------------------------------------------------------------------------*/
emilmont 1:fdd22bb7aa52 156
emilmont 1:fdd22bb7aa52 157 /* Working pointer for destination matrix */
emilmont 1:fdd22bb7aa52 158 pInT2 = pOut;
emilmont 1:fdd22bb7aa52 159
emilmont 1:fdd22bb7aa52 160 /* Loop over the number of rows */
emilmont 1:fdd22bb7aa52 161 rowCnt = numRows;
emilmont 1:fdd22bb7aa52 162
emilmont 1:fdd22bb7aa52 163 /* Making the destination matrix as identity matrix */
emilmont 1:fdd22bb7aa52 164 while(rowCnt > 0u)
emilmont 1:fdd22bb7aa52 165 {
emilmont 1:fdd22bb7aa52 166 /* Writing all zeroes in lower triangle of the destination matrix */
emilmont 1:fdd22bb7aa52 167 j = numRows - rowCnt;
emilmont 1:fdd22bb7aa52 168 while(j > 0u)
emilmont 1:fdd22bb7aa52 169 {
emilmont 1:fdd22bb7aa52 170 *pInT2++ = 0.0f;
emilmont 1:fdd22bb7aa52 171 j--;
emilmont 1:fdd22bb7aa52 172 }
emilmont 1:fdd22bb7aa52 173
emilmont 1:fdd22bb7aa52 174 /* Writing all ones in the diagonal of the destination matrix */
emilmont 1:fdd22bb7aa52 175 *pInT2++ = 1.0f;
emilmont 1:fdd22bb7aa52 176
emilmont 1:fdd22bb7aa52 177 /* Writing all zeroes in upper triangle of the destination matrix */
emilmont 1:fdd22bb7aa52 178 j = rowCnt - 1u;
emilmont 1:fdd22bb7aa52 179 while(j > 0u)
emilmont 1:fdd22bb7aa52 180 {
emilmont 1:fdd22bb7aa52 181 *pInT2++ = 0.0f;
emilmont 1:fdd22bb7aa52 182 j--;
emilmont 1:fdd22bb7aa52 183 }
emilmont 1:fdd22bb7aa52 184
emilmont 1:fdd22bb7aa52 185 /* Decrement the loop counter */
emilmont 1:fdd22bb7aa52 186 rowCnt--;
emilmont 1:fdd22bb7aa52 187 }
emilmont 1:fdd22bb7aa52 188
emilmont 1:fdd22bb7aa52 189 /* Loop over the number of columns of the input matrix.
emilmont 1:fdd22bb7aa52 190 All the elements in each column are processed by the row operations */
emilmont 1:fdd22bb7aa52 191 loopCnt = numCols;
emilmont 1:fdd22bb7aa52 192
emilmont 1:fdd22bb7aa52 193 /* Index modifier to navigate through the columns */
emilmont 1:fdd22bb7aa52 194 l = 0u;
emilmont 1:fdd22bb7aa52 195
emilmont 1:fdd22bb7aa52 196 while(loopCnt > 0u)
emilmont 1:fdd22bb7aa52 197 {
emilmont 1:fdd22bb7aa52 198 /* Check if the pivot element is zero..
emilmont 1:fdd22bb7aa52 199 * If it is zero then interchange the row with non zero row below.
emilmont 1:fdd22bb7aa52 200 * If there is no non zero element to replace in the rows below,
emilmont 1:fdd22bb7aa52 201 * then the matrix is Singular. */
emilmont 1:fdd22bb7aa52 202
emilmont 1:fdd22bb7aa52 203 /* Working pointer for the input matrix that points
emilmont 1:fdd22bb7aa52 204 * to the pivot element of the particular row */
emilmont 1:fdd22bb7aa52 205 pInT1 = pIn + (l * numCols);
emilmont 1:fdd22bb7aa52 206
emilmont 1:fdd22bb7aa52 207 /* Working pointer for the destination matrix that points
emilmont 1:fdd22bb7aa52 208 * to the pivot element of the particular row */
emilmont 1:fdd22bb7aa52 209 pInT3 = pOut + (l * numCols);
emilmont 1:fdd22bb7aa52 210
emilmont 1:fdd22bb7aa52 211 /* Temporary variable to hold the pivot value */
emilmont 1:fdd22bb7aa52 212 in = *pInT1;
emilmont 1:fdd22bb7aa52 213
emilmont 1:fdd22bb7aa52 214 /* Destination pointer modifier */
emilmont 1:fdd22bb7aa52 215 k = 1u;
emilmont 1:fdd22bb7aa52 216
mbed_official 3:7a284390b0ce 217 /* Grab the most significant value from column l */
mbed_official 3:7a284390b0ce 218 maxC = 0;
mbed_official 3:7a284390b0ce 219 for (i = 0; i < numRows; i++)
mbed_official 3:7a284390b0ce 220 {
mbed_official 3:7a284390b0ce 221 maxC = *pInT1 > 0 ? (*pInT1 > maxC ? *pInT1 : maxC) : (-*pInT1 > maxC ? -*pInT1 : maxC);
mbed_official 3:7a284390b0ce 222 pInT1 += numCols;
mbed_official 3:7a284390b0ce 223 }
mbed_official 3:7a284390b0ce 224
mbed_official 3:7a284390b0ce 225 /* Update the status if the matrix is singular */
mbed_official 3:7a284390b0ce 226 if(maxC == 0.0f)
mbed_official 3:7a284390b0ce 227 {
mbed_official 3:7a284390b0ce 228 status = ARM_MATH_SINGULAR;
mbed_official 3:7a284390b0ce 229 break;
mbed_official 3:7a284390b0ce 230 }
mbed_official 3:7a284390b0ce 231
mbed_official 3:7a284390b0ce 232 /* Restore pInT1 */
mbed_official 3:7a284390b0ce 233 pInT1 -= numRows * numCols;
mbed_official 3:7a284390b0ce 234
mbed_official 3:7a284390b0ce 235 /* Check if the pivot element is the most significant of the column */
mbed_official 3:7a284390b0ce 236 if( (in > 0.0f ? in : -in) != maxC)
emilmont 1:fdd22bb7aa52 237 {
emilmont 1:fdd22bb7aa52 238 /* Loop over the number rows present below */
emilmont 1:fdd22bb7aa52 239 i = numRows - (l + 1u);
emilmont 1:fdd22bb7aa52 240
emilmont 1:fdd22bb7aa52 241 while(i > 0u)
emilmont 1:fdd22bb7aa52 242 {
emilmont 1:fdd22bb7aa52 243 /* Update the input and destination pointers */
emilmont 1:fdd22bb7aa52 244 pInT2 = pInT1 + (numCols * l);
emilmont 1:fdd22bb7aa52 245 pInT4 = pInT3 + (numCols * k);
emilmont 1:fdd22bb7aa52 246
mbed_official 3:7a284390b0ce 247 /* Look for the most significant element to
emilmont 1:fdd22bb7aa52 248 * replace in the rows below */
mbed_official 3:7a284390b0ce 249 if((*pInT2 > 0.0f ? *pInT2: -*pInT2) == maxC)
emilmont 1:fdd22bb7aa52 250 {
emilmont 1:fdd22bb7aa52 251 /* Loop over number of columns
emilmont 1:fdd22bb7aa52 252 * to the right of the pilot element */
emilmont 1:fdd22bb7aa52 253 j = numCols - l;
emilmont 1:fdd22bb7aa52 254
emilmont 1:fdd22bb7aa52 255 while(j > 0u)
emilmont 1:fdd22bb7aa52 256 {
emilmont 1:fdd22bb7aa52 257 /* Exchange the row elements of the input matrix */
emilmont 1:fdd22bb7aa52 258 Xchg = *pInT2;
emilmont 1:fdd22bb7aa52 259 *pInT2++ = *pInT1;
emilmont 1:fdd22bb7aa52 260 *pInT1++ = Xchg;
emilmont 1:fdd22bb7aa52 261
emilmont 1:fdd22bb7aa52 262 /* Decrement the loop counter */
emilmont 1:fdd22bb7aa52 263 j--;
emilmont 1:fdd22bb7aa52 264 }
emilmont 1:fdd22bb7aa52 265
emilmont 1:fdd22bb7aa52 266 /* Loop over number of columns of the destination matrix */
emilmont 1:fdd22bb7aa52 267 j = numCols;
emilmont 1:fdd22bb7aa52 268
emilmont 1:fdd22bb7aa52 269 while(j > 0u)
emilmont 1:fdd22bb7aa52 270 {
emilmont 1:fdd22bb7aa52 271 /* Exchange the row elements of the destination matrix */
emilmont 1:fdd22bb7aa52 272 Xchg = *pInT4;
emilmont 1:fdd22bb7aa52 273 *pInT4++ = *pInT3;
emilmont 1:fdd22bb7aa52 274 *pInT3++ = Xchg;
emilmont 1:fdd22bb7aa52 275
emilmont 1:fdd22bb7aa52 276 /* Decrement the loop counter */
emilmont 1:fdd22bb7aa52 277 j--;
emilmont 1:fdd22bb7aa52 278 }
emilmont 1:fdd22bb7aa52 279
emilmont 1:fdd22bb7aa52 280 /* Flag to indicate whether exchange is done or not */
emilmont 1:fdd22bb7aa52 281 flag = 1u;
emilmont 1:fdd22bb7aa52 282
emilmont 1:fdd22bb7aa52 283 /* Break after exchange is done */
emilmont 1:fdd22bb7aa52 284 break;
emilmont 1:fdd22bb7aa52 285 }
emilmont 1:fdd22bb7aa52 286
emilmont 1:fdd22bb7aa52 287 /* Update the destination pointer modifier */
emilmont 1:fdd22bb7aa52 288 k++;
emilmont 1:fdd22bb7aa52 289
emilmont 1:fdd22bb7aa52 290 /* Decrement the loop counter */
emilmont 1:fdd22bb7aa52 291 i--;
emilmont 1:fdd22bb7aa52 292 }
emilmont 1:fdd22bb7aa52 293 }
emilmont 1:fdd22bb7aa52 294
emilmont 1:fdd22bb7aa52 295 /* Update the status if the matrix is singular */
emilmont 1:fdd22bb7aa52 296 if((flag != 1u) && (in == 0.0f))
emilmont 1:fdd22bb7aa52 297 {
emilmont 1:fdd22bb7aa52 298 status = ARM_MATH_SINGULAR;
emilmont 1:fdd22bb7aa52 299
emilmont 1:fdd22bb7aa52 300 break;
emilmont 1:fdd22bb7aa52 301 }
emilmont 1:fdd22bb7aa52 302
emilmont 1:fdd22bb7aa52 303 /* Points to the pivot row of input and destination matrices */
emilmont 1:fdd22bb7aa52 304 pPivotRowIn = pIn + (l * numCols);
emilmont 1:fdd22bb7aa52 305 pPivotRowDst = pOut + (l * numCols);
emilmont 1:fdd22bb7aa52 306
emilmont 1:fdd22bb7aa52 307 /* Temporary pointers to the pivot row pointers */
emilmont 1:fdd22bb7aa52 308 pInT1 = pPivotRowIn;
emilmont 1:fdd22bb7aa52 309 pInT2 = pPivotRowDst;
emilmont 1:fdd22bb7aa52 310
emilmont 1:fdd22bb7aa52 311 /* Pivot element of the row */
mbed_official 3:7a284390b0ce 312 in = *pPivotRowIn;
emilmont 1:fdd22bb7aa52 313
emilmont 1:fdd22bb7aa52 314 /* Loop over number of columns
emilmont 1:fdd22bb7aa52 315 * to the right of the pilot element */
emilmont 1:fdd22bb7aa52 316 j = (numCols - l);
emilmont 1:fdd22bb7aa52 317
emilmont 1:fdd22bb7aa52 318 while(j > 0u)
emilmont 1:fdd22bb7aa52 319 {
emilmont 1:fdd22bb7aa52 320 /* Divide each element of the row of the input matrix
emilmont 1:fdd22bb7aa52 321 * by the pivot element */
emilmont 1:fdd22bb7aa52 322 in1 = *pInT1;
emilmont 1:fdd22bb7aa52 323 *pInT1++ = in1 / in;
emilmont 1:fdd22bb7aa52 324
emilmont 1:fdd22bb7aa52 325 /* Decrement the loop counter */
emilmont 1:fdd22bb7aa52 326 j--;
emilmont 1:fdd22bb7aa52 327 }
emilmont 1:fdd22bb7aa52 328
emilmont 1:fdd22bb7aa52 329 /* Loop over number of columns of the destination matrix */
emilmont 1:fdd22bb7aa52 330 j = numCols;
emilmont 1:fdd22bb7aa52 331
emilmont 1:fdd22bb7aa52 332 while(j > 0u)
emilmont 1:fdd22bb7aa52 333 {
emilmont 1:fdd22bb7aa52 334 /* Divide each element of the row of the destination matrix
emilmont 1:fdd22bb7aa52 335 * by the pivot element */
emilmont 1:fdd22bb7aa52 336 in1 = *pInT2;
emilmont 1:fdd22bb7aa52 337 *pInT2++ = in1 / in;
emilmont 1:fdd22bb7aa52 338
emilmont 1:fdd22bb7aa52 339 /* Decrement the loop counter */
emilmont 1:fdd22bb7aa52 340 j--;
emilmont 1:fdd22bb7aa52 341 }
emilmont 1:fdd22bb7aa52 342
emilmont 1:fdd22bb7aa52 343 /* Replace the rows with the sum of that row and a multiple of row i
emilmont 1:fdd22bb7aa52 344 * so that each new element in column i above row i is zero.*/
emilmont 1:fdd22bb7aa52 345
emilmont 1:fdd22bb7aa52 346 /* Temporary pointers for input and destination matrices */
emilmont 1:fdd22bb7aa52 347 pInT1 = pIn;
emilmont 1:fdd22bb7aa52 348 pInT2 = pOut;
emilmont 1:fdd22bb7aa52 349
emilmont 1:fdd22bb7aa52 350 /* index used to check for pivot element */
emilmont 1:fdd22bb7aa52 351 i = 0u;
emilmont 1:fdd22bb7aa52 352
emilmont 1:fdd22bb7aa52 353 /* Loop over number of rows */
emilmont 1:fdd22bb7aa52 354 /* to be replaced by the sum of that row and a multiple of row i */
emilmont 1:fdd22bb7aa52 355 k = numRows;
emilmont 1:fdd22bb7aa52 356
emilmont 1:fdd22bb7aa52 357 while(k > 0u)
emilmont 1:fdd22bb7aa52 358 {
emilmont 1:fdd22bb7aa52 359 /* Check for the pivot element */
emilmont 1:fdd22bb7aa52 360 if(i == l)
emilmont 1:fdd22bb7aa52 361 {
emilmont 1:fdd22bb7aa52 362 /* If the processing element is the pivot element,
emilmont 1:fdd22bb7aa52 363 only the columns to the right are to be processed */
emilmont 1:fdd22bb7aa52 364 pInT1 += numCols - l;
emilmont 1:fdd22bb7aa52 365
emilmont 1:fdd22bb7aa52 366 pInT2 += numCols;
emilmont 1:fdd22bb7aa52 367 }
emilmont 1:fdd22bb7aa52 368 else
emilmont 1:fdd22bb7aa52 369 {
emilmont 1:fdd22bb7aa52 370 /* Element of the reference row */
emilmont 1:fdd22bb7aa52 371 in = *pInT1;
emilmont 1:fdd22bb7aa52 372
emilmont 1:fdd22bb7aa52 373 /* Working pointers for input and destination pivot rows */
emilmont 1:fdd22bb7aa52 374 pPRT_in = pPivotRowIn;
emilmont 1:fdd22bb7aa52 375 pPRT_pDst = pPivotRowDst;
emilmont 1:fdd22bb7aa52 376
emilmont 1:fdd22bb7aa52 377 /* Loop over the number of columns to the right of the pivot element,
emilmont 1:fdd22bb7aa52 378 to replace the elements in the input matrix */
emilmont 1:fdd22bb7aa52 379 j = (numCols - l);
emilmont 1:fdd22bb7aa52 380
emilmont 1:fdd22bb7aa52 381 while(j > 0u)
emilmont 1:fdd22bb7aa52 382 {
emilmont 1:fdd22bb7aa52 383 /* Replace the element by the sum of that row
emilmont 1:fdd22bb7aa52 384 and a multiple of the reference row */
emilmont 1:fdd22bb7aa52 385 in1 = *pInT1;
emilmont 1:fdd22bb7aa52 386 *pInT1++ = in1 - (in * *pPRT_in++);
emilmont 1:fdd22bb7aa52 387
emilmont 1:fdd22bb7aa52 388 /* Decrement the loop counter */
emilmont 1:fdd22bb7aa52 389 j--;
emilmont 1:fdd22bb7aa52 390 }
emilmont 1:fdd22bb7aa52 391
emilmont 1:fdd22bb7aa52 392 /* Loop over the number of columns to
emilmont 1:fdd22bb7aa52 393 replace the elements in the destination matrix */
emilmont 1:fdd22bb7aa52 394 j = numCols;
emilmont 1:fdd22bb7aa52 395
emilmont 1:fdd22bb7aa52 396 while(j > 0u)
emilmont 1:fdd22bb7aa52 397 {
emilmont 1:fdd22bb7aa52 398 /* Replace the element by the sum of that row
emilmont 1:fdd22bb7aa52 399 and a multiple of the reference row */
emilmont 1:fdd22bb7aa52 400 in1 = *pInT2;
emilmont 1:fdd22bb7aa52 401 *pInT2++ = in1 - (in * *pPRT_pDst++);
emilmont 1:fdd22bb7aa52 402
emilmont 1:fdd22bb7aa52 403 /* Decrement the loop counter */
emilmont 1:fdd22bb7aa52 404 j--;
emilmont 1:fdd22bb7aa52 405 }
emilmont 1:fdd22bb7aa52 406
emilmont 1:fdd22bb7aa52 407 }
emilmont 1:fdd22bb7aa52 408
emilmont 1:fdd22bb7aa52 409 /* Increment the temporary input pointer */
emilmont 1:fdd22bb7aa52 410 pInT1 = pInT1 + l;
emilmont 1:fdd22bb7aa52 411
emilmont 1:fdd22bb7aa52 412 /* Decrement the loop counter */
emilmont 1:fdd22bb7aa52 413 k--;
emilmont 1:fdd22bb7aa52 414
emilmont 1:fdd22bb7aa52 415 /* Increment the pivot index */
emilmont 1:fdd22bb7aa52 416 i++;
emilmont 1:fdd22bb7aa52 417 }
emilmont 1:fdd22bb7aa52 418
emilmont 1:fdd22bb7aa52 419 /* Increment the input pointer */
emilmont 1:fdd22bb7aa52 420 pIn++;
emilmont 1:fdd22bb7aa52 421
emilmont 1:fdd22bb7aa52 422 /* Decrement the loop counter */
emilmont 1:fdd22bb7aa52 423 loopCnt--;
emilmont 1:fdd22bb7aa52 424
emilmont 1:fdd22bb7aa52 425 /* Increment the index modifier */
emilmont 1:fdd22bb7aa52 426 l++;
emilmont 1:fdd22bb7aa52 427 }
emilmont 1:fdd22bb7aa52 428
emilmont 1:fdd22bb7aa52 429
emilmont 1:fdd22bb7aa52 430 #else
emilmont 1:fdd22bb7aa52 431
emilmont 1:fdd22bb7aa52 432 /* Run the below code for Cortex-M0 */
emilmont 1:fdd22bb7aa52 433
emilmont 1:fdd22bb7aa52 434 float32_t Xchg, in = 0.0f; /* Temporary input values */
emilmont 1:fdd22bb7aa52 435 uint32_t i, rowCnt, flag = 0u, j, loopCnt, k, l; /* loop counters */
emilmont 1:fdd22bb7aa52 436 arm_status status; /* status of matrix inverse */
emilmont 1:fdd22bb7aa52 437
emilmont 1:fdd22bb7aa52 438 #ifdef ARM_MATH_MATRIX_CHECK
emilmont 1:fdd22bb7aa52 439
emilmont 1:fdd22bb7aa52 440 /* Check for matrix mismatch condition */
emilmont 1:fdd22bb7aa52 441 if((pSrc->numRows != pSrc->numCols) || (pDst->numRows != pDst->numCols)
emilmont 1:fdd22bb7aa52 442 || (pSrc->numRows != pDst->numRows))
emilmont 1:fdd22bb7aa52 443 {
emilmont 1:fdd22bb7aa52 444 /* Set status as ARM_MATH_SIZE_MISMATCH */
emilmont 1:fdd22bb7aa52 445 status = ARM_MATH_SIZE_MISMATCH;
emilmont 1:fdd22bb7aa52 446 }
emilmont 1:fdd22bb7aa52 447 else
emilmont 1:fdd22bb7aa52 448 #endif /* #ifdef ARM_MATH_MATRIX_CHECK */
emilmont 1:fdd22bb7aa52 449 {
emilmont 1:fdd22bb7aa52 450
emilmont 1:fdd22bb7aa52 451 /*--------------------------------------------------------------------------------------------------------------
emilmont 2:da51fb522205 452 * Matrix Inverse can be solved using elementary row operations.
emilmont 2:da51fb522205 453 *
emilmont 2:da51fb522205 454 * Gauss-Jordan Method:
emilmont 2:da51fb522205 455 *
emilmont 2:da51fb522205 456 * 1. First combine the identity matrix and the input matrix separated by a bar to form an
emilmont 2:da51fb522205 457 * augmented matrix as follows:
emilmont 2:da51fb522205 458 * _ _ _ _ _ _ _ _
emilmont 2:da51fb522205 459 * | | a11 a12 | | | 1 0 | | | X11 X12 |
emilmont 2:da51fb522205 460 * | | | | | | | = | |
emilmont 2:da51fb522205 461 * |_ |_ a21 a22 _| | |_0 1 _| _| |_ X21 X21 _|
emilmont 2:da51fb522205 462 *
emilmont 2:da51fb522205 463 * 2. In our implementation, pDst Matrix is used as identity matrix.
emilmont 2:da51fb522205 464 *
emilmont 2:da51fb522205 465 * 3. Begin with the first row. Let i = 1.
emilmont 2:da51fb522205 466 *
emilmont 2:da51fb522205 467 * 4. Check to see if the pivot for row i is zero.
emilmont 2:da51fb522205 468 * The pivot is the element of the main diagonal that is on the current row.
emilmont 2:da51fb522205 469 * For instance, if working with row i, then the pivot element is aii.
emilmont 2:da51fb522205 470 * If the pivot is zero, exchange that row with a row below it that does not
emilmont 2:da51fb522205 471 * contain a zero in column i. If this is not possible, then an inverse
emilmont 2:da51fb522205 472 * to that matrix does not exist.
emilmont 2:da51fb522205 473 *
emilmont 2:da51fb522205 474 * 5. Divide every element of row i by the pivot.
emilmont 2:da51fb522205 475 *
emilmont 2:da51fb522205 476 * 6. For every row below and row i, replace that row with the sum of that row and
emilmont 2:da51fb522205 477 * a multiple of row i so that each new element in column i below row i is zero.
emilmont 2:da51fb522205 478 *
emilmont 2:da51fb522205 479 * 7. Move to the next row and column and repeat steps 2 through 5 until you have zeros
emilmont 2:da51fb522205 480 * for every element below and above the main diagonal.
emilmont 2:da51fb522205 481 *
emilmont 2:da51fb522205 482 * 8. Now an identical matrix is formed to the left of the bar(input matrix, src).
emilmont 2:da51fb522205 483 * Therefore, the matrix to the right of the bar is our solution(dst matrix, dst).
emilmont 2:da51fb522205 484 *----------------------------------------------------------------------------------------------------------------*/
emilmont 1:fdd22bb7aa52 485
emilmont 1:fdd22bb7aa52 486 /* Working pointer for destination matrix */
emilmont 1:fdd22bb7aa52 487 pInT2 = pOut;
emilmont 1:fdd22bb7aa52 488
emilmont 1:fdd22bb7aa52 489 /* Loop over the number of rows */
emilmont 1:fdd22bb7aa52 490 rowCnt = numRows;
emilmont 1:fdd22bb7aa52 491
emilmont 1:fdd22bb7aa52 492 /* Making the destination matrix as identity matrix */
emilmont 1:fdd22bb7aa52 493 while(rowCnt > 0u)
emilmont 1:fdd22bb7aa52 494 {
emilmont 1:fdd22bb7aa52 495 /* Writing all zeroes in lower triangle of the destination matrix */
emilmont 1:fdd22bb7aa52 496 j = numRows - rowCnt;
emilmont 1:fdd22bb7aa52 497 while(j > 0u)
emilmont 1:fdd22bb7aa52 498 {
emilmont 1:fdd22bb7aa52 499 *pInT2++ = 0.0f;
emilmont 1:fdd22bb7aa52 500 j--;
emilmont 1:fdd22bb7aa52 501 }
emilmont 1:fdd22bb7aa52 502
emilmont 1:fdd22bb7aa52 503 /* Writing all ones in the diagonal of the destination matrix */
emilmont 1:fdd22bb7aa52 504 *pInT2++ = 1.0f;
emilmont 1:fdd22bb7aa52 505
emilmont 1:fdd22bb7aa52 506 /* Writing all zeroes in upper triangle of the destination matrix */
emilmont 1:fdd22bb7aa52 507 j = rowCnt - 1u;
emilmont 1:fdd22bb7aa52 508 while(j > 0u)
emilmont 1:fdd22bb7aa52 509 {
emilmont 1:fdd22bb7aa52 510 *pInT2++ = 0.0f;
emilmont 1:fdd22bb7aa52 511 j--;
emilmont 1:fdd22bb7aa52 512 }
emilmont 1:fdd22bb7aa52 513
emilmont 1:fdd22bb7aa52 514 /* Decrement the loop counter */
emilmont 1:fdd22bb7aa52 515 rowCnt--;
emilmont 1:fdd22bb7aa52 516 }
emilmont 1:fdd22bb7aa52 517
emilmont 1:fdd22bb7aa52 518 /* Loop over the number of columns of the input matrix.
emilmont 1:fdd22bb7aa52 519 All the elements in each column are processed by the row operations */
emilmont 1:fdd22bb7aa52 520 loopCnt = numCols;
emilmont 1:fdd22bb7aa52 521
emilmont 1:fdd22bb7aa52 522 /* Index modifier to navigate through the columns */
emilmont 1:fdd22bb7aa52 523 l = 0u;
emilmont 1:fdd22bb7aa52 524 //for(loopCnt = 0u; loopCnt < numCols; loopCnt++)
emilmont 1:fdd22bb7aa52 525 while(loopCnt > 0u)
emilmont 1:fdd22bb7aa52 526 {
emilmont 1:fdd22bb7aa52 527 /* Check if the pivot element is zero..
emilmont 1:fdd22bb7aa52 528 * If it is zero then interchange the row with non zero row below.
emilmont 1:fdd22bb7aa52 529 * If there is no non zero element to replace in the rows below,
emilmont 1:fdd22bb7aa52 530 * then the matrix is Singular. */
emilmont 1:fdd22bb7aa52 531
emilmont 1:fdd22bb7aa52 532 /* Working pointer for the input matrix that points
emilmont 1:fdd22bb7aa52 533 * to the pivot element of the particular row */
emilmont 1:fdd22bb7aa52 534 pInT1 = pIn + (l * numCols);
emilmont 1:fdd22bb7aa52 535
emilmont 1:fdd22bb7aa52 536 /* Working pointer for the destination matrix that points
emilmont 1:fdd22bb7aa52 537 * to the pivot element of the particular row */
emilmont 1:fdd22bb7aa52 538 pInT3 = pOut + (l * numCols);
emilmont 1:fdd22bb7aa52 539
emilmont 1:fdd22bb7aa52 540 /* Temporary variable to hold the pivot value */
emilmont 1:fdd22bb7aa52 541 in = *pInT1;
emilmont 1:fdd22bb7aa52 542
emilmont 1:fdd22bb7aa52 543 /* Destination pointer modifier */
emilmont 1:fdd22bb7aa52 544 k = 1u;
emilmont 1:fdd22bb7aa52 545
emilmont 1:fdd22bb7aa52 546 /* Check if the pivot element is zero */
emilmont 1:fdd22bb7aa52 547 if(*pInT1 == 0.0f)
emilmont 1:fdd22bb7aa52 548 {
emilmont 1:fdd22bb7aa52 549 /* Loop over the number rows present below */
emilmont 1:fdd22bb7aa52 550 for (i = (l + 1u); i < numRows; i++)
emilmont 1:fdd22bb7aa52 551 {
emilmont 1:fdd22bb7aa52 552 /* Update the input and destination pointers */
emilmont 1:fdd22bb7aa52 553 pInT2 = pInT1 + (numCols * l);
emilmont 1:fdd22bb7aa52 554 pInT4 = pInT3 + (numCols * k);
emilmont 1:fdd22bb7aa52 555
emilmont 1:fdd22bb7aa52 556 /* Check if there is a non zero pivot element to
emilmont 1:fdd22bb7aa52 557 * replace in the rows below */
emilmont 1:fdd22bb7aa52 558 if(*pInT2 != 0.0f)
emilmont 1:fdd22bb7aa52 559 {
emilmont 1:fdd22bb7aa52 560 /* Loop over number of columns
emilmont 1:fdd22bb7aa52 561 * to the right of the pilot element */
emilmont 1:fdd22bb7aa52 562 for (j = 0u; j < (numCols - l); j++)
emilmont 1:fdd22bb7aa52 563 {
emilmont 1:fdd22bb7aa52 564 /* Exchange the row elements of the input matrix */
emilmont 1:fdd22bb7aa52 565 Xchg = *pInT2;
emilmont 1:fdd22bb7aa52 566 *pInT2++ = *pInT1;
emilmont 1:fdd22bb7aa52 567 *pInT1++ = Xchg;
emilmont 1:fdd22bb7aa52 568 }
emilmont 1:fdd22bb7aa52 569
emilmont 1:fdd22bb7aa52 570 for (j = 0u; j < numCols; j++)
emilmont 1:fdd22bb7aa52 571 {
emilmont 1:fdd22bb7aa52 572 Xchg = *pInT4;
emilmont 1:fdd22bb7aa52 573 *pInT4++ = *pInT3;
emilmont 1:fdd22bb7aa52 574 *pInT3++ = Xchg;
emilmont 1:fdd22bb7aa52 575 }
emilmont 1:fdd22bb7aa52 576
emilmont 1:fdd22bb7aa52 577 /* Flag to indicate whether exchange is done or not */
emilmont 1:fdd22bb7aa52 578 flag = 1u;
emilmont 1:fdd22bb7aa52 579
emilmont 1:fdd22bb7aa52 580 /* Break after exchange is done */
emilmont 1:fdd22bb7aa52 581 break;
emilmont 1:fdd22bb7aa52 582 }
emilmont 1:fdd22bb7aa52 583
emilmont 1:fdd22bb7aa52 584 /* Update the destination pointer modifier */
emilmont 1:fdd22bb7aa52 585 k++;
emilmont 1:fdd22bb7aa52 586 }
emilmont 1:fdd22bb7aa52 587 }
emilmont 1:fdd22bb7aa52 588
emilmont 1:fdd22bb7aa52 589 /* Update the status if the matrix is singular */
emilmont 1:fdd22bb7aa52 590 if((flag != 1u) && (in == 0.0f))
emilmont 1:fdd22bb7aa52 591 {
emilmont 1:fdd22bb7aa52 592 status = ARM_MATH_SINGULAR;
emilmont 1:fdd22bb7aa52 593
emilmont 1:fdd22bb7aa52 594 break;
emilmont 1:fdd22bb7aa52 595 }
emilmont 1:fdd22bb7aa52 596
emilmont 1:fdd22bb7aa52 597 /* Points to the pivot row of input and destination matrices */
emilmont 1:fdd22bb7aa52 598 pPivotRowIn = pIn + (l * numCols);
emilmont 1:fdd22bb7aa52 599 pPivotRowDst = pOut + (l * numCols);
emilmont 1:fdd22bb7aa52 600
emilmont 1:fdd22bb7aa52 601 /* Temporary pointers to the pivot row pointers */
emilmont 1:fdd22bb7aa52 602 pInT1 = pPivotRowIn;
emilmont 1:fdd22bb7aa52 603 pInT2 = pPivotRowDst;
emilmont 1:fdd22bb7aa52 604
emilmont 1:fdd22bb7aa52 605 /* Pivot element of the row */
emilmont 1:fdd22bb7aa52 606 in = *(pIn + (l * numCols));
emilmont 1:fdd22bb7aa52 607
emilmont 1:fdd22bb7aa52 608 /* Loop over number of columns
emilmont 1:fdd22bb7aa52 609 * to the right of the pilot element */
emilmont 1:fdd22bb7aa52 610 for (j = 0u; j < (numCols - l); j++)
emilmont 1:fdd22bb7aa52 611 {
emilmont 1:fdd22bb7aa52 612 /* Divide each element of the row of the input matrix
emilmont 1:fdd22bb7aa52 613 * by the pivot element */
mbed_official 3:7a284390b0ce 614 *pInT1 = *pInT1 / in;
mbed_official 3:7a284390b0ce 615 pInT1++;
emilmont 1:fdd22bb7aa52 616 }
emilmont 1:fdd22bb7aa52 617 for (j = 0u; j < numCols; j++)
emilmont 1:fdd22bb7aa52 618 {
emilmont 1:fdd22bb7aa52 619 /* Divide each element of the row of the destination matrix
emilmont 1:fdd22bb7aa52 620 * by the pivot element */
mbed_official 3:7a284390b0ce 621 *pInT2 = *pInT2 / in;
mbed_official 3:7a284390b0ce 622 pInT2++;
emilmont 1:fdd22bb7aa52 623 }
emilmont 1:fdd22bb7aa52 624
emilmont 1:fdd22bb7aa52 625 /* Replace the rows with the sum of that row and a multiple of row i
emilmont 1:fdd22bb7aa52 626 * so that each new element in column i above row i is zero.*/
emilmont 1:fdd22bb7aa52 627
emilmont 1:fdd22bb7aa52 628 /* Temporary pointers for input and destination matrices */
emilmont 1:fdd22bb7aa52 629 pInT1 = pIn;
emilmont 1:fdd22bb7aa52 630 pInT2 = pOut;
emilmont 1:fdd22bb7aa52 631
emilmont 1:fdd22bb7aa52 632 for (i = 0u; i < numRows; i++)
emilmont 1:fdd22bb7aa52 633 {
emilmont 1:fdd22bb7aa52 634 /* Check for the pivot element */
emilmont 1:fdd22bb7aa52 635 if(i == l)
emilmont 1:fdd22bb7aa52 636 {
emilmont 1:fdd22bb7aa52 637 /* If the processing element is the pivot element,
emilmont 1:fdd22bb7aa52 638 only the columns to the right are to be processed */
emilmont 1:fdd22bb7aa52 639 pInT1 += numCols - l;
emilmont 1:fdd22bb7aa52 640 pInT2 += numCols;
emilmont 1:fdd22bb7aa52 641 }
emilmont 1:fdd22bb7aa52 642 else
emilmont 1:fdd22bb7aa52 643 {
emilmont 1:fdd22bb7aa52 644 /* Element of the reference row */
emilmont 1:fdd22bb7aa52 645 in = *pInT1;
emilmont 1:fdd22bb7aa52 646
emilmont 1:fdd22bb7aa52 647 /* Working pointers for input and destination pivot rows */
emilmont 1:fdd22bb7aa52 648 pPRT_in = pPivotRowIn;
emilmont 1:fdd22bb7aa52 649 pPRT_pDst = pPivotRowDst;
emilmont 1:fdd22bb7aa52 650
emilmont 1:fdd22bb7aa52 651 /* Loop over the number of columns to the right of the pivot element,
emilmont 1:fdd22bb7aa52 652 to replace the elements in the input matrix */
emilmont 1:fdd22bb7aa52 653 for (j = 0u; j < (numCols - l); j++)
emilmont 1:fdd22bb7aa52 654 {
emilmont 1:fdd22bb7aa52 655 /* Replace the element by the sum of that row
emilmont 1:fdd22bb7aa52 656 and a multiple of the reference row */
mbed_official 3:7a284390b0ce 657 *pInT1 = *pInT1 - (in * *pPRT_in++);
mbed_official 3:7a284390b0ce 658 pInT1++;
emilmont 1:fdd22bb7aa52 659 }
emilmont 1:fdd22bb7aa52 660 /* Loop over the number of columns to
emilmont 1:fdd22bb7aa52 661 replace the elements in the destination matrix */
emilmont 1:fdd22bb7aa52 662 for (j = 0u; j < numCols; j++)
emilmont 1:fdd22bb7aa52 663 {
emilmont 1:fdd22bb7aa52 664 /* Replace the element by the sum of that row
emilmont 1:fdd22bb7aa52 665 and a multiple of the reference row */
mbed_official 3:7a284390b0ce 666 *pInT2 = *pInT2 - (in * *pPRT_pDst++);
mbed_official 3:7a284390b0ce 667 pInT2++;
emilmont 1:fdd22bb7aa52 668 }
emilmont 1:fdd22bb7aa52 669
emilmont 1:fdd22bb7aa52 670 }
emilmont 1:fdd22bb7aa52 671 /* Increment the temporary input pointer */
emilmont 1:fdd22bb7aa52 672 pInT1 = pInT1 + l;
emilmont 1:fdd22bb7aa52 673 }
emilmont 1:fdd22bb7aa52 674 /* Increment the input pointer */
emilmont 1:fdd22bb7aa52 675 pIn++;
emilmont 1:fdd22bb7aa52 676
emilmont 1:fdd22bb7aa52 677 /* Decrement the loop counter */
emilmont 1:fdd22bb7aa52 678 loopCnt--;
emilmont 1:fdd22bb7aa52 679 /* Increment the index modifier */
emilmont 1:fdd22bb7aa52 680 l++;
emilmont 1:fdd22bb7aa52 681 }
emilmont 1:fdd22bb7aa52 682
emilmont 1:fdd22bb7aa52 683
mbed_official 3:7a284390b0ce 684 #endif /* #ifndef ARM_MATH_CM0_FAMILY */
emilmont 1:fdd22bb7aa52 685
emilmont 1:fdd22bb7aa52 686 /* Set status as ARM_MATH_SUCCESS */
emilmont 1:fdd22bb7aa52 687 status = ARM_MATH_SUCCESS;
emilmont 1:fdd22bb7aa52 688
emilmont 1:fdd22bb7aa52 689 if((flag != 1u) && (in == 0.0f))
emilmont 1:fdd22bb7aa52 690 {
emilmont 1:fdd22bb7aa52 691 status = ARM_MATH_SINGULAR;
emilmont 1:fdd22bb7aa52 692 }
emilmont 1:fdd22bb7aa52 693 }
emilmont 1:fdd22bb7aa52 694 /* Return to application */
emilmont 1:fdd22bb7aa52 695 return (status);
emilmont 1:fdd22bb7aa52 696 }
emilmont 1:fdd22bb7aa52 697
emilmont 1:fdd22bb7aa52 698 /**
emilmont 1:fdd22bb7aa52 699 * @} end of MatrixInv group
emilmont 1:fdd22bb7aa52 700 */