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@5:3762170b6d4d, 2015-11-20 (annotated)

Committer:
mbed_official
Date:
Fri Nov 20 08:45:18 2015 +0000
Revision:
5:3762170b6d4d
Parent:
3:7a284390b0ce 
Synchronized with git revision 2eb940b9a73af188d3004a2575fdfbb05febe62b



Full URL: https://github.com/mbedmicro/mbed/commit/2eb940b9a73af188d3004a2575fdfbb05febe62b/



Added option to build rpc library. closes #1426

Who changed what in which revision?

UserRevisionLine numberNew contents of line
emilmont 1:fdd22bb7aa52 1 /* ----------------------------------------------------------------------
mbed_official 5:3762170b6d4d 2 * Copyright (C) 2010-2014 ARM Limited. All rights reserved.
emilmont 1:fdd22bb7aa52 3 *
mbed_official 5:3762170b6d4d 4 * $Date: 19. March 2015
mbed_official 5:3762170b6d4d 5 * $Revision: V.1.4.5
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.
mbed_official 5:3762170b6d4d 61 * The algorithm performs a sequence of elementary row-operations until 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 */
mbed_official 5:3762170b6d4d 92 float32_t *pOutT1, *pOutT2; /* 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 5:3762170b6d4d 140 * If the pivot is not the most significant of the columns, 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 5:3762170b6d4d 143 * The most significant value of the column is the absolute 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 */
mbed_official 5:3762170b6d4d 158 pOutT1 = 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 {
mbed_official 5:3762170b6d4d 170 *pOutT1++ = 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 */
mbed_official 5:3762170b6d4d 175 *pOutT1++ = 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 {
mbed_official 5:3762170b6d4d 181 *pOutT1++ = 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 */
mbed_official 5:3762170b6d4d 209 pOutT1 = 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
mbed_official 5:3762170b6d4d 214 /* Grab the most significant value from column l */
mbed_official 3:7a284390b0ce 215 maxC = 0;
mbed_official 5:3762170b6d4d 216 for (i = l; i < numRows; i++)
mbed_official 3:7a284390b0ce 217 {
mbed_official 3:7a284390b0ce 218 maxC = *pInT1 > 0 ? (*pInT1 > maxC ? *pInT1 : maxC) : (-*pInT1 > maxC ? -*pInT1 : maxC);
mbed_official 3:7a284390b0ce 219 pInT1 += numCols;
mbed_official 3:7a284390b0ce 220 }
mbed_official 3:7a284390b0ce 221
mbed_official 3:7a284390b0ce 222 /* Update the status if the matrix is singular */
mbed_official 3:7a284390b0ce 223 if(maxC == 0.0f)
mbed_official 3:7a284390b0ce 224 {
mbed_official 5:3762170b6d4d 225 return ARM_MATH_SINGULAR;
mbed_official 3:7a284390b0ce 226 }
mbed_official 3:7a284390b0ce 227
mbed_official 3:7a284390b0ce 228 /* Restore pInT1 */
mbed_official 5:3762170b6d4d 229 pInT1 = pIn;
mbed_official 5:3762170b6d4d 230
mbed_official 5:3762170b6d4d 231 /* Destination pointer modifier */
mbed_official 5:3762170b6d4d 232 k = 1u;
mbed_official 3:7a284390b0ce 233
mbed_official 3:7a284390b0ce 234 /* Check if the pivot element is the most significant of the column */
mbed_official 3:7a284390b0ce 235 if( (in > 0.0f ? in : -in) != maxC)
emilmont 1:fdd22bb7aa52 236 {
emilmont 1:fdd22bb7aa52 237 /* Loop over the number rows present below */
emilmont 1:fdd22bb7aa52 238 i = numRows - (l + 1u);
emilmont 1:fdd22bb7aa52 239
emilmont 1:fdd22bb7aa52 240 while(i > 0u)
emilmont 1:fdd22bb7aa52 241 {
emilmont 1:fdd22bb7aa52 242 /* Update the input and destination pointers */
emilmont 1:fdd22bb7aa52 243 pInT2 = pInT1 + (numCols * l);
mbed_official 5:3762170b6d4d 244 pOutT2 = pOutT1 + (numCols * k);
emilmont 1:fdd22bb7aa52 245
mbed_official 3:7a284390b0ce 246 /* Look for the most significant element to
emilmont 1:fdd22bb7aa52 247 * replace in the rows below */
mbed_official 3:7a284390b0ce 248 if((*pInT2 > 0.0f ? *pInT2: -*pInT2) == maxC)
emilmont 1:fdd22bb7aa52 249 {
emilmont 1:fdd22bb7aa52 250 /* Loop over number of columns
emilmont 1:fdd22bb7aa52 251 * to the right of the pilot element */
emilmont 1:fdd22bb7aa52 252 j = numCols - l;
emilmont 1:fdd22bb7aa52 253
emilmont 1:fdd22bb7aa52 254 while(j > 0u)
emilmont 1:fdd22bb7aa52 255 {
emilmont 1:fdd22bb7aa52 256 /* Exchange the row elements of the input matrix */
emilmont 1:fdd22bb7aa52 257 Xchg = *pInT2;
emilmont 1:fdd22bb7aa52 258 *pInT2++ = *pInT1;
emilmont 1:fdd22bb7aa52 259 *pInT1++ = Xchg;
emilmont 1:fdd22bb7aa52 260
emilmont 1:fdd22bb7aa52 261 /* Decrement the loop counter */
emilmont 1:fdd22bb7aa52 262 j--;
emilmont 1:fdd22bb7aa52 263 }
emilmont 1:fdd22bb7aa52 264
emilmont 1:fdd22bb7aa52 265 /* Loop over number of columns of the destination matrix */
emilmont 1:fdd22bb7aa52 266 j = numCols;
emilmont 1:fdd22bb7aa52 267
emilmont 1:fdd22bb7aa52 268 while(j > 0u)
emilmont 1:fdd22bb7aa52 269 {
emilmont 1:fdd22bb7aa52 270 /* Exchange the row elements of the destination matrix */
mbed_official 5:3762170b6d4d 271 Xchg = *pOutT2;
mbed_official 5:3762170b6d4d 272 *pOutT2++ = *pOutT1;
mbed_official 5:3762170b6d4d 273 *pOutT1++ = Xchg;
emilmont 1:fdd22bb7aa52 274
emilmont 1:fdd22bb7aa52 275 /* Decrement the loop counter */
emilmont 1:fdd22bb7aa52 276 j--;
emilmont 1:fdd22bb7aa52 277 }
emilmont 1:fdd22bb7aa52 278
emilmont 1:fdd22bb7aa52 279 /* Flag to indicate whether exchange is done or not */
emilmont 1:fdd22bb7aa52 280 flag = 1u;
emilmont 1:fdd22bb7aa52 281
emilmont 1:fdd22bb7aa52 282 /* Break after exchange is done */
emilmont 1:fdd22bb7aa52 283 break;
emilmont 1:fdd22bb7aa52 284 }
emilmont 1:fdd22bb7aa52 285
emilmont 1:fdd22bb7aa52 286 /* Update the destination pointer modifier */
emilmont 1:fdd22bb7aa52 287 k++;
emilmont 1:fdd22bb7aa52 288
emilmont 1:fdd22bb7aa52 289 /* Decrement the loop counter */
emilmont 1:fdd22bb7aa52 290 i--;
emilmont 1:fdd22bb7aa52 291 }
emilmont 1:fdd22bb7aa52 292 }
emilmont 1:fdd22bb7aa52 293
emilmont 1:fdd22bb7aa52 294 /* Update the status if the matrix is singular */
emilmont 1:fdd22bb7aa52 295 if((flag != 1u) && (in == 0.0f))
emilmont 1:fdd22bb7aa52 296 {
mbed_official 5:3762170b6d4d 297 return ARM_MATH_SINGULAR;
emilmont 1:fdd22bb7aa52 298 }
emilmont 1:fdd22bb7aa52 299
emilmont 1:fdd22bb7aa52 300 /* Points to the pivot row of input and destination matrices */
emilmont 1:fdd22bb7aa52 301 pPivotRowIn = pIn + (l * numCols);
emilmont 1:fdd22bb7aa52 302 pPivotRowDst = pOut + (l * numCols);
emilmont 1:fdd22bb7aa52 303
emilmont 1:fdd22bb7aa52 304 /* Temporary pointers to the pivot row pointers */
emilmont 1:fdd22bb7aa52 305 pInT1 = pPivotRowIn;
emilmont 1:fdd22bb7aa52 306 pInT2 = pPivotRowDst;
emilmont 1:fdd22bb7aa52 307
emilmont 1:fdd22bb7aa52 308 /* Pivot element of the row */
mbed_official 3:7a284390b0ce 309 in = *pPivotRowIn;
emilmont 1:fdd22bb7aa52 310
emilmont 1:fdd22bb7aa52 311 /* Loop over number of columns
emilmont 1:fdd22bb7aa52 312 * to the right of the pilot element */
emilmont 1:fdd22bb7aa52 313 j = (numCols - l);
emilmont 1:fdd22bb7aa52 314
emilmont 1:fdd22bb7aa52 315 while(j > 0u)
emilmont 1:fdd22bb7aa52 316 {
emilmont 1:fdd22bb7aa52 317 /* Divide each element of the row of the input matrix
emilmont 1:fdd22bb7aa52 318 * by the pivot element */
emilmont 1:fdd22bb7aa52 319 in1 = *pInT1;
emilmont 1:fdd22bb7aa52 320 *pInT1++ = in1 / in;
emilmont 1:fdd22bb7aa52 321
emilmont 1:fdd22bb7aa52 322 /* Decrement the loop counter */
emilmont 1:fdd22bb7aa52 323 j--;
emilmont 1:fdd22bb7aa52 324 }
emilmont 1:fdd22bb7aa52 325
emilmont 1:fdd22bb7aa52 326 /* Loop over number of columns of the destination matrix */
emilmont 1:fdd22bb7aa52 327 j = numCols;
emilmont 1:fdd22bb7aa52 328
emilmont 1:fdd22bb7aa52 329 while(j > 0u)
emilmont 1:fdd22bb7aa52 330 {
emilmont 1:fdd22bb7aa52 331 /* Divide each element of the row of the destination matrix
emilmont 1:fdd22bb7aa52 332 * by the pivot element */
emilmont 1:fdd22bb7aa52 333 in1 = *pInT2;
emilmont 1:fdd22bb7aa52 334 *pInT2++ = in1 / in;
emilmont 1:fdd22bb7aa52 335
emilmont 1:fdd22bb7aa52 336 /* Decrement the loop counter */
emilmont 1:fdd22bb7aa52 337 j--;
emilmont 1:fdd22bb7aa52 338 }
emilmont 1:fdd22bb7aa52 339
emilmont 1:fdd22bb7aa52 340 /* Replace the rows with the sum of that row and a multiple of row i
emilmont 1:fdd22bb7aa52 341 * so that each new element in column i above row i is zero.*/
emilmont 1:fdd22bb7aa52 342
emilmont 1:fdd22bb7aa52 343 /* Temporary pointers for input and destination matrices */
emilmont 1:fdd22bb7aa52 344 pInT1 = pIn;
emilmont 1:fdd22bb7aa52 345 pInT2 = pOut;
emilmont 1:fdd22bb7aa52 346
emilmont 1:fdd22bb7aa52 347 /* index used to check for pivot element */
emilmont 1:fdd22bb7aa52 348 i = 0u;
emilmont 1:fdd22bb7aa52 349
emilmont 1:fdd22bb7aa52 350 /* Loop over number of rows */
emilmont 1:fdd22bb7aa52 351 /* to be replaced by the sum of that row and a multiple of row i */
emilmont 1:fdd22bb7aa52 352 k = numRows;
emilmont 1:fdd22bb7aa52 353
emilmont 1:fdd22bb7aa52 354 while(k > 0u)
emilmont 1:fdd22bb7aa52 355 {
emilmont 1:fdd22bb7aa52 356 /* Check for the pivot element */
emilmont 1:fdd22bb7aa52 357 if(i == l)
emilmont 1:fdd22bb7aa52 358 {
emilmont 1:fdd22bb7aa52 359 /* If the processing element is the pivot element,
emilmont 1:fdd22bb7aa52 360 only the columns to the right are to be processed */
emilmont 1:fdd22bb7aa52 361 pInT1 += numCols - l;
emilmont 1:fdd22bb7aa52 362
emilmont 1:fdd22bb7aa52 363 pInT2 += numCols;
emilmont 1:fdd22bb7aa52 364 }
emilmont 1:fdd22bb7aa52 365 else
emilmont 1:fdd22bb7aa52 366 {
emilmont 1:fdd22bb7aa52 367 /* Element of the reference row */
emilmont 1:fdd22bb7aa52 368 in = *pInT1;
emilmont 1:fdd22bb7aa52 369
emilmont 1:fdd22bb7aa52 370 /* Working pointers for input and destination pivot rows */
emilmont 1:fdd22bb7aa52 371 pPRT_in = pPivotRowIn;
emilmont 1:fdd22bb7aa52 372 pPRT_pDst = pPivotRowDst;
emilmont 1:fdd22bb7aa52 373
emilmont 1:fdd22bb7aa52 374 /* Loop over the number of columns to the right of the pivot element,
emilmont 1:fdd22bb7aa52 375 to replace the elements in the input matrix */
emilmont 1:fdd22bb7aa52 376 j = (numCols - l);
emilmont 1:fdd22bb7aa52 377
emilmont 1:fdd22bb7aa52 378 while(j > 0u)
emilmont 1:fdd22bb7aa52 379 {
emilmont 1:fdd22bb7aa52 380 /* Replace the element by the sum of that row
emilmont 1:fdd22bb7aa52 381 and a multiple of the reference row */
emilmont 1:fdd22bb7aa52 382 in1 = *pInT1;
emilmont 1:fdd22bb7aa52 383 *pInT1++ = in1 - (in * *pPRT_in++);
emilmont 1:fdd22bb7aa52 384
emilmont 1:fdd22bb7aa52 385 /* Decrement the loop counter */
emilmont 1:fdd22bb7aa52 386 j--;
emilmont 1:fdd22bb7aa52 387 }
emilmont 1:fdd22bb7aa52 388
emilmont 1:fdd22bb7aa52 389 /* Loop over the number of columns to
emilmont 1:fdd22bb7aa52 390 replace the elements in the destination matrix */
emilmont 1:fdd22bb7aa52 391 j = numCols;
emilmont 1:fdd22bb7aa52 392
emilmont 1:fdd22bb7aa52 393 while(j > 0u)
emilmont 1:fdd22bb7aa52 394 {
emilmont 1:fdd22bb7aa52 395 /* Replace the element by the sum of that row
emilmont 1:fdd22bb7aa52 396 and a multiple of the reference row */
emilmont 1:fdd22bb7aa52 397 in1 = *pInT2;
emilmont 1:fdd22bb7aa52 398 *pInT2++ = in1 - (in * *pPRT_pDst++);
emilmont 1:fdd22bb7aa52 399
emilmont 1:fdd22bb7aa52 400 /* Decrement the loop counter */
emilmont 1:fdd22bb7aa52 401 j--;
emilmont 1:fdd22bb7aa52 402 }
emilmont 1:fdd22bb7aa52 403
emilmont 1:fdd22bb7aa52 404 }
emilmont 1:fdd22bb7aa52 405
emilmont 1:fdd22bb7aa52 406 /* Increment the temporary input pointer */
emilmont 1:fdd22bb7aa52 407 pInT1 = pInT1 + l;
emilmont 1:fdd22bb7aa52 408
emilmont 1:fdd22bb7aa52 409 /* Decrement the loop counter */
emilmont 1:fdd22bb7aa52 410 k--;
emilmont 1:fdd22bb7aa52 411
emilmont 1:fdd22bb7aa52 412 /* Increment the pivot index */
emilmont 1:fdd22bb7aa52 413 i++;
emilmont 1:fdd22bb7aa52 414 }
emilmont 1:fdd22bb7aa52 415
emilmont 1:fdd22bb7aa52 416 /* Increment the input pointer */
emilmont 1:fdd22bb7aa52 417 pIn++;
emilmont 1:fdd22bb7aa52 418
emilmont 1:fdd22bb7aa52 419 /* Decrement the loop counter */
emilmont 1:fdd22bb7aa52 420 loopCnt--;
emilmont 1:fdd22bb7aa52 421
emilmont 1:fdd22bb7aa52 422 /* Increment the index modifier */
emilmont 1:fdd22bb7aa52 423 l++;
emilmont 1:fdd22bb7aa52 424 }
emilmont 1:fdd22bb7aa52 425
emilmont 1:fdd22bb7aa52 426
emilmont 1:fdd22bb7aa52 427 #else
emilmont 1:fdd22bb7aa52 428
emilmont 1:fdd22bb7aa52 429 /* Run the below code for Cortex-M0 */
emilmont 1:fdd22bb7aa52 430
emilmont 1:fdd22bb7aa52 431 float32_t Xchg, in = 0.0f; /* Temporary input values */
emilmont 1:fdd22bb7aa52 432 uint32_t i, rowCnt, flag = 0u, j, loopCnt, k, l; /* loop counters */
emilmont 1:fdd22bb7aa52 433 arm_status status; /* status of matrix inverse */
emilmont 1:fdd22bb7aa52 434
emilmont 1:fdd22bb7aa52 435 #ifdef ARM_MATH_MATRIX_CHECK
emilmont 1:fdd22bb7aa52 436
emilmont 1:fdd22bb7aa52 437 /* Check for matrix mismatch condition */
emilmont 1:fdd22bb7aa52 438 if((pSrc->numRows != pSrc->numCols) || (pDst->numRows != pDst->numCols)
emilmont 1:fdd22bb7aa52 439 || (pSrc->numRows != pDst->numRows))
emilmont 1:fdd22bb7aa52 440 {
emilmont 1:fdd22bb7aa52 441 /* Set status as ARM_MATH_SIZE_MISMATCH */
emilmont 1:fdd22bb7aa52 442 status = ARM_MATH_SIZE_MISMATCH;
emilmont 1:fdd22bb7aa52 443 }
emilmont 1:fdd22bb7aa52 444 else
emilmont 1:fdd22bb7aa52 445 #endif /* #ifdef ARM_MATH_MATRIX_CHECK */
emilmont 1:fdd22bb7aa52 446 {
emilmont 1:fdd22bb7aa52 447
emilmont 1:fdd22bb7aa52 448 /*--------------------------------------------------------------------------------------------------------------
emilmont 2:da51fb522205 449 * Matrix Inverse can be solved using elementary row operations.
emilmont 2:da51fb522205 450 *
emilmont 2:da51fb522205 451 * Gauss-Jordan Method:
emilmont 2:da51fb522205 452 *
emilmont 2:da51fb522205 453 * 1. First combine the identity matrix and the input matrix separated by a bar to form an
emilmont 2:da51fb522205 454 * augmented matrix as follows:
emilmont 2:da51fb522205 455 * _ _ _ _ _ _ _ _
emilmont 2:da51fb522205 456 * | | a11 a12 | | | 1 0 | | | X11 X12 |
emilmont 2:da51fb522205 457 * | | | | | | | = | |
emilmont 2:da51fb522205 458 * |_ |_ a21 a22 _| | |_0 1 _| _| |_ X21 X21 _|
emilmont 2:da51fb522205 459 *
emilmont 2:da51fb522205 460 * 2. In our implementation, pDst Matrix is used as identity matrix.
emilmont 2:da51fb522205 461 *
emilmont 2:da51fb522205 462 * 3. Begin with the first row. Let i = 1.
emilmont 2:da51fb522205 463 *
emilmont 2:da51fb522205 464 * 4. Check to see if the pivot for row i is zero.
emilmont 2:da51fb522205 465 * The pivot is the element of the main diagonal that is on the current row.
emilmont 2:da51fb522205 466 * For instance, if working with row i, then the pivot element is aii.
emilmont 2:da51fb522205 467 * If the pivot is zero, exchange that row with a row below it that does not
emilmont 2:da51fb522205 468 * contain a zero in column i. If this is not possible, then an inverse
emilmont 2:da51fb522205 469 * to that matrix does not exist.
emilmont 2:da51fb522205 470 *
emilmont 2:da51fb522205 471 * 5. Divide every element of row i by the pivot.
emilmont 2:da51fb522205 472 *
emilmont 2:da51fb522205 473 * 6. For every row below and row i, replace that row with the sum of that row and
emilmont 2:da51fb522205 474 * a multiple of row i so that each new element in column i below row i is zero.
emilmont 2:da51fb522205 475 *
emilmont 2:da51fb522205 476 * 7. Move to the next row and column and repeat steps 2 through 5 until you have zeros
emilmont 2:da51fb522205 477 * for every element below and above the main diagonal.
emilmont 2:da51fb522205 478 *
emilmont 2:da51fb522205 479 * 8. Now an identical matrix is formed to the left of the bar(input matrix, src).
emilmont 2:da51fb522205 480 * Therefore, the matrix to the right of the bar is our solution(dst matrix, dst).
emilmont 2:da51fb522205 481 *----------------------------------------------------------------------------------------------------------------*/
emilmont 1:fdd22bb7aa52 482
emilmont 1:fdd22bb7aa52 483 /* Working pointer for destination matrix */
mbed_official 5:3762170b6d4d 484 pOutT1 = pOut;
emilmont 1:fdd22bb7aa52 485
emilmont 1:fdd22bb7aa52 486 /* Loop over the number of rows */
emilmont 1:fdd22bb7aa52 487 rowCnt = numRows;
emilmont 1:fdd22bb7aa52 488
emilmont 1:fdd22bb7aa52 489 /* Making the destination matrix as identity matrix */
emilmont 1:fdd22bb7aa52 490 while(rowCnt > 0u)
emilmont 1:fdd22bb7aa52 491 {
emilmont 1:fdd22bb7aa52 492 /* Writing all zeroes in lower triangle of the destination matrix */
emilmont 1:fdd22bb7aa52 493 j = numRows - rowCnt;
emilmont 1:fdd22bb7aa52 494 while(j > 0u)
emilmont 1:fdd22bb7aa52 495 {
mbed_official 5:3762170b6d4d 496 *pOutT1++ = 0.0f;
emilmont 1:fdd22bb7aa52 497 j--;
emilmont 1:fdd22bb7aa52 498 }
emilmont 1:fdd22bb7aa52 499
emilmont 1:fdd22bb7aa52 500 /* Writing all ones in the diagonal of the destination matrix */
mbed_official 5:3762170b6d4d 501 *pOutT1++ = 1.0f;
emilmont 1:fdd22bb7aa52 502
emilmont 1:fdd22bb7aa52 503 /* Writing all zeroes in upper triangle of the destination matrix */
emilmont 1:fdd22bb7aa52 504 j = rowCnt - 1u;
emilmont 1:fdd22bb7aa52 505 while(j > 0u)
emilmont 1:fdd22bb7aa52 506 {
mbed_official 5:3762170b6d4d 507 *pOutT1++ = 0.0f;
emilmont 1:fdd22bb7aa52 508 j--;
emilmont 1:fdd22bb7aa52 509 }
emilmont 1:fdd22bb7aa52 510
emilmont 1:fdd22bb7aa52 511 /* Decrement the loop counter */
emilmont 1:fdd22bb7aa52 512 rowCnt--;
emilmont 1:fdd22bb7aa52 513 }
emilmont 1:fdd22bb7aa52 514
emilmont 1:fdd22bb7aa52 515 /* Loop over the number of columns of the input matrix.
emilmont 1:fdd22bb7aa52 516 All the elements in each column are processed by the row operations */
emilmont 1:fdd22bb7aa52 517 loopCnt = numCols;
emilmont 1:fdd22bb7aa52 518
emilmont 1:fdd22bb7aa52 519 /* Index modifier to navigate through the columns */
emilmont 1:fdd22bb7aa52 520 l = 0u;
emilmont 1:fdd22bb7aa52 521 //for(loopCnt = 0u; loopCnt < numCols; loopCnt++)
emilmont 1:fdd22bb7aa52 522 while(loopCnt > 0u)
emilmont 1:fdd22bb7aa52 523 {
emilmont 1:fdd22bb7aa52 524 /* Check if the pivot element is zero..
emilmont 1:fdd22bb7aa52 525 * If it is zero then interchange the row with non zero row below.
emilmont 1:fdd22bb7aa52 526 * If there is no non zero element to replace in the rows below,
emilmont 1:fdd22bb7aa52 527 * then the matrix is Singular. */
emilmont 1:fdd22bb7aa52 528
emilmont 1:fdd22bb7aa52 529 /* Working pointer for the input matrix that points
emilmont 1:fdd22bb7aa52 530 * to the pivot element of the particular row */
emilmont 1:fdd22bb7aa52 531 pInT1 = pIn + (l * numCols);
emilmont 1:fdd22bb7aa52 532
emilmont 1:fdd22bb7aa52 533 /* Working pointer for the destination matrix that points
emilmont 1:fdd22bb7aa52 534 * to the pivot element of the particular row */
mbed_official 5:3762170b6d4d 535 pOutT1 = pOut + (l * numCols);
emilmont 1:fdd22bb7aa52 536
emilmont 1:fdd22bb7aa52 537 /* Temporary variable to hold the pivot value */
emilmont 1:fdd22bb7aa52 538 in = *pInT1;
emilmont 1:fdd22bb7aa52 539
emilmont 1:fdd22bb7aa52 540 /* Destination pointer modifier */
emilmont 1:fdd22bb7aa52 541 k = 1u;
emilmont 1:fdd22bb7aa52 542
emilmont 1:fdd22bb7aa52 543 /* Check if the pivot element is zero */
emilmont 1:fdd22bb7aa52 544 if(*pInT1 == 0.0f)
emilmont 1:fdd22bb7aa52 545 {
emilmont 1:fdd22bb7aa52 546 /* Loop over the number rows present below */
emilmont 1:fdd22bb7aa52 547 for (i = (l + 1u); i < numRows; i++)
emilmont 1:fdd22bb7aa52 548 {
emilmont 1:fdd22bb7aa52 549 /* Update the input and destination pointers */
emilmont 1:fdd22bb7aa52 550 pInT2 = pInT1 + (numCols * l);
mbed_official 5:3762170b6d4d 551 pOutT2 = pOutT1 + (numCols * k);
emilmont 1:fdd22bb7aa52 552
emilmont 1:fdd22bb7aa52 553 /* Check if there is a non zero pivot element to
emilmont 1:fdd22bb7aa52 554 * replace in the rows below */
emilmont 1:fdd22bb7aa52 555 if(*pInT2 != 0.0f)
emilmont 1:fdd22bb7aa52 556 {
emilmont 1:fdd22bb7aa52 557 /* Loop over number of columns
emilmont 1:fdd22bb7aa52 558 * to the right of the pilot element */
emilmont 1:fdd22bb7aa52 559 for (j = 0u; j < (numCols - l); j++)
emilmont 1:fdd22bb7aa52 560 {
emilmont 1:fdd22bb7aa52 561 /* Exchange the row elements of the input matrix */
emilmont 1:fdd22bb7aa52 562 Xchg = *pInT2;
emilmont 1:fdd22bb7aa52 563 *pInT2++ = *pInT1;
emilmont 1:fdd22bb7aa52 564 *pInT1++ = Xchg;
emilmont 1:fdd22bb7aa52 565 }
emilmont 1:fdd22bb7aa52 566
emilmont 1:fdd22bb7aa52 567 for (j = 0u; j < numCols; j++)
emilmont 1:fdd22bb7aa52 568 {
mbed_official 5:3762170b6d4d 569 Xchg = *pOutT2;
mbed_official 5:3762170b6d4d 570 *pOutT2++ = *pOutT1;
mbed_official 5:3762170b6d4d 571 *pOutT1++ = Xchg;
emilmont 1:fdd22bb7aa52 572 }
emilmont 1:fdd22bb7aa52 573
emilmont 1:fdd22bb7aa52 574 /* Flag to indicate whether exchange is done or not */
emilmont 1:fdd22bb7aa52 575 flag = 1u;
emilmont 1:fdd22bb7aa52 576
emilmont 1:fdd22bb7aa52 577 /* Break after exchange is done */
emilmont 1:fdd22bb7aa52 578 break;
emilmont 1:fdd22bb7aa52 579 }
emilmont 1:fdd22bb7aa52 580
emilmont 1:fdd22bb7aa52 581 /* Update the destination pointer modifier */
emilmont 1:fdd22bb7aa52 582 k++;
emilmont 1:fdd22bb7aa52 583 }
emilmont 1:fdd22bb7aa52 584 }
emilmont 1:fdd22bb7aa52 585
emilmont 1:fdd22bb7aa52 586 /* Update the status if the matrix is singular */
emilmont 1:fdd22bb7aa52 587 if((flag != 1u) && (in == 0.0f))
emilmont 1:fdd22bb7aa52 588 {
mbed_official 5:3762170b6d4d 589 return ARM_MATH_SINGULAR;
emilmont 1:fdd22bb7aa52 590 }
emilmont 1:fdd22bb7aa52 591
emilmont 1:fdd22bb7aa52 592 /* Points to the pivot row of input and destination matrices */
emilmont 1:fdd22bb7aa52 593 pPivotRowIn = pIn + (l * numCols);
emilmont 1:fdd22bb7aa52 594 pPivotRowDst = pOut + (l * numCols);
emilmont 1:fdd22bb7aa52 595
emilmont 1:fdd22bb7aa52 596 /* Temporary pointers to the pivot row pointers */
emilmont 1:fdd22bb7aa52 597 pInT1 = pPivotRowIn;
mbed_official 5:3762170b6d4d 598 pOutT1 = pPivotRowDst;
emilmont 1:fdd22bb7aa52 599
emilmont 1:fdd22bb7aa52 600 /* Pivot element of the row */
emilmont 1:fdd22bb7aa52 601 in = *(pIn + (l * numCols));
emilmont 1:fdd22bb7aa52 602
emilmont 1:fdd22bb7aa52 603 /* Loop over number of columns
emilmont 1:fdd22bb7aa52 604 * to the right of the pilot element */
emilmont 1:fdd22bb7aa52 605 for (j = 0u; j < (numCols - l); j++)
emilmont 1:fdd22bb7aa52 606 {
emilmont 1:fdd22bb7aa52 607 /* Divide each element of the row of the input matrix
emilmont 1:fdd22bb7aa52 608 * by the pivot element */
mbed_official 3:7a284390b0ce 609 *pInT1 = *pInT1 / in;
mbed_official 3:7a284390b0ce 610 pInT1++;
emilmont 1:fdd22bb7aa52 611 }
emilmont 1:fdd22bb7aa52 612 for (j = 0u; j < numCols; j++)
emilmont 1:fdd22bb7aa52 613 {
emilmont 1:fdd22bb7aa52 614 /* Divide each element of the row of the destination matrix
emilmont 1:fdd22bb7aa52 615 * by the pivot element */
mbed_official 5:3762170b6d4d 616 *pOutT1 = *pOutT1 / in;
mbed_official 5:3762170b6d4d 617 pOutT1++;
emilmont 1:fdd22bb7aa52 618 }
emilmont 1:fdd22bb7aa52 619
emilmont 1:fdd22bb7aa52 620 /* Replace the rows with the sum of that row and a multiple of row i
emilmont 1:fdd22bb7aa52 621 * so that each new element in column i above row i is zero.*/
emilmont 1:fdd22bb7aa52 622
emilmont 1:fdd22bb7aa52 623 /* Temporary pointers for input and destination matrices */
emilmont 1:fdd22bb7aa52 624 pInT1 = pIn;
mbed_official 5:3762170b6d4d 625 pOutT1 = pOut;
emilmont 1:fdd22bb7aa52 626
emilmont 1:fdd22bb7aa52 627 for (i = 0u; i < numRows; i++)
emilmont 1:fdd22bb7aa52 628 {
emilmont 1:fdd22bb7aa52 629 /* Check for the pivot element */
emilmont 1:fdd22bb7aa52 630 if(i == l)
emilmont 1:fdd22bb7aa52 631 {
emilmont 1:fdd22bb7aa52 632 /* If the processing element is the pivot element,
emilmont 1:fdd22bb7aa52 633 only the columns to the right are to be processed */
emilmont 1:fdd22bb7aa52 634 pInT1 += numCols - l;
mbed_official 5:3762170b6d4d 635 pOutT1 += numCols;
emilmont 1:fdd22bb7aa52 636 }
emilmont 1:fdd22bb7aa52 637 else
emilmont 1:fdd22bb7aa52 638 {
emilmont 1:fdd22bb7aa52 639 /* Element of the reference row */
emilmont 1:fdd22bb7aa52 640 in = *pInT1;
emilmont 1:fdd22bb7aa52 641
emilmont 1:fdd22bb7aa52 642 /* Working pointers for input and destination pivot rows */
emilmont 1:fdd22bb7aa52 643 pPRT_in = pPivotRowIn;
emilmont 1:fdd22bb7aa52 644 pPRT_pDst = pPivotRowDst;
emilmont 1:fdd22bb7aa52 645
emilmont 1:fdd22bb7aa52 646 /* Loop over the number of columns to the right of the pivot element,
emilmont 1:fdd22bb7aa52 647 to replace the elements in the input matrix */
emilmont 1:fdd22bb7aa52 648 for (j = 0u; j < (numCols - l); j++)
emilmont 1:fdd22bb7aa52 649 {
emilmont 1:fdd22bb7aa52 650 /* Replace the element by the sum of that row
emilmont 1:fdd22bb7aa52 651 and a multiple of the reference row */
mbed_official 3:7a284390b0ce 652 *pInT1 = *pInT1 - (in * *pPRT_in++);
mbed_official 3:7a284390b0ce 653 pInT1++;
emilmont 1:fdd22bb7aa52 654 }
emilmont 1:fdd22bb7aa52 655 /* Loop over the number of columns to
emilmont 1:fdd22bb7aa52 656 replace the elements in the destination matrix */
emilmont 1:fdd22bb7aa52 657 for (j = 0u; j < numCols; j++)
emilmont 1:fdd22bb7aa52 658 {
emilmont 1:fdd22bb7aa52 659 /* Replace the element by the sum of that row
emilmont 1:fdd22bb7aa52 660 and a multiple of the reference row */
mbed_official 5:3762170b6d4d 661 *pOutT1 = *pOutT1 - (in * *pPRT_pDst++);
mbed_official 5:3762170b6d4d 662 pOutT1++;
emilmont 1:fdd22bb7aa52 663 }
emilmont 1:fdd22bb7aa52 664
emilmont 1:fdd22bb7aa52 665 }
emilmont 1:fdd22bb7aa52 666 /* Increment the temporary input pointer */
emilmont 1:fdd22bb7aa52 667 pInT1 = pInT1 + l;
emilmont 1:fdd22bb7aa52 668 }
emilmont 1:fdd22bb7aa52 669 /* Increment the input pointer */
emilmont 1:fdd22bb7aa52 670 pIn++;
emilmont 1:fdd22bb7aa52 671
emilmont 1:fdd22bb7aa52 672 /* Decrement the loop counter */
emilmont 1:fdd22bb7aa52 673 loopCnt--;
emilmont 1:fdd22bb7aa52 674 /* Increment the index modifier */
emilmont 1:fdd22bb7aa52 675 l++;
emilmont 1:fdd22bb7aa52 676 }
emilmont 1:fdd22bb7aa52 677
emilmont 1:fdd22bb7aa52 678
mbed_official 3:7a284390b0ce 679 #endif /* #ifndef ARM_MATH_CM0_FAMILY */
emilmont 1:fdd22bb7aa52 680
emilmont 1:fdd22bb7aa52 681 /* Set status as ARM_MATH_SUCCESS */
emilmont 1:fdd22bb7aa52 682 status = ARM_MATH_SUCCESS;
emilmont 1:fdd22bb7aa52 683
emilmont 1:fdd22bb7aa52 684 if((flag != 1u) && (in == 0.0f))
emilmont 1:fdd22bb7aa52 685 {
mbed_official 5:3762170b6d4d 686 pIn = pSrc->pData;
mbed_official 5:3762170b6d4d 687 for (i = 0; i < numRows * numCols; i++)
mbed_official 5:3762170b6d4d 688 {
mbed_official 5:3762170b6d4d 689 if (pIn[i] != 0.0f)
mbed_official 5:3762170b6d4d 690 break;
mbed_official 5:3762170b6d4d 691 }
mbed_official 5:3762170b6d4d 692
mbed_official 5:3762170b6d4d 693 if (i == numRows * numCols)
mbed_official 5:3762170b6d4d 694 status = ARM_MATH_SINGULAR;
emilmont 1:fdd22bb7aa52 695 }
emilmont 1:fdd22bb7aa52 696 }
emilmont 1:fdd22bb7aa52 697 /* Return to application */
emilmont 1:fdd22bb7aa52 698 return (status);
emilmont 1:fdd22bb7aa52 699 }
emilmont 1:fdd22bb7aa52 700
emilmont 1:fdd22bb7aa52 701 /**
emilmont 1:fdd22bb7aa52 702 * @} end of MatrixInv group
emilmont 1:fdd22bb7aa52 703 */