]>
git.gir.st - tmk_keyboard.git/blob - tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/MatrixFunctions/arm_mat_inverse_f32.c
1 /* ----------------------------------------------------------------------
2 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
7 * Project: CMSIS DSP Library
8 * Title: arm_mat_inverse_f32.c
10 * Description: Floating-point matrix inverse.
12 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
14 * Redistribution and use in source and binary forms, with or without
15 * modification, are permitted provided that the following conditions
17 * - Redistributions of source code must retain the above copyright
18 * notice, this list of conditions and the following disclaimer.
19 * - Redistributions in binary form must reproduce the above copyright
20 * notice, this list of conditions and the following disclaimer in
21 * the documentation and/or other materials provided with the
23 * - Neither the name of ARM LIMITED nor the names of its contributors
24 * may be used to endorse or promote products derived from this
25 * software without specific prior written permission.
27 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
28 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
29 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
30 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
31 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
32 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
33 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
34 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
35 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
36 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
37 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
38 * POSSIBILITY OF SUCH DAMAGE.
39 * -------------------------------------------------------------------- */
44 * @ingroup groupMatrix
48 * @defgroup MatrixInv Matrix Inverse
50 * Computes the inverse of a matrix.
52 * The inverse is defined only if the input matrix is square and non-singular (the determinant
53 * is non-zero). The function checks that the input and output matrices are square and of the
56 * Matrix inversion is numerically sensitive and the CMSIS DSP library only supports matrix
57 * inversion of floating-point matrices.
60 * The Gauss-Jordan method is used to find the inverse.
61 * The algorithm performs a sequence of elementary row-operations till it
62 * reduces the input matrix to an identity matrix. Applying the same sequence
63 * of elementary row-operations to an identity matrix yields the inverse matrix.
64 * If the input matrix is singular, then the algorithm terminates and returns error status
65 * <code>ARM_MATH_SINGULAR</code>.
66 * \image html MatrixInverse.gif "Matrix Inverse of a 3 x 3 matrix using Gauss-Jordan Method"
70 * @addtogroup MatrixInv
75 * @brief Floating-point matrix inverse.
76 * @param[in] *pSrc points to input matrix structure
77 * @param[out] *pDst points to output matrix structure
78 * @return The function returns
79 * <code>ARM_MATH_SIZE_MISMATCH</code> if the input matrix is not square or if the size
80 * of the output matrix does not match the size of the input matrix.
81 * If the input matrix is found to be singular (non-invertible), then the function returns
82 * <code>ARM_MATH_SINGULAR</code>. Otherwise, the function returns <code>ARM_MATH_SUCCESS</code>.
85 arm_status
arm_mat_inverse_f32(
86 const arm_matrix_instance_f32
* pSrc
,
87 arm_matrix_instance_f32
* pDst
)
89 float32_t
*pIn
= pSrc
->pData
; /* input data matrix pointer */
90 float32_t
*pOut
= pDst
->pData
; /* output data matrix pointer */
91 float32_t
*pInT1
, *pInT2
; /* Temporary input data matrix pointer */
92 float32_t
*pInT3
, *pInT4
; /* Temporary output data matrix pointer */
93 float32_t
*pPivotRowIn
, *pPRT_in
, *pPivotRowDst
, *pPRT_pDst
; /* Temporary input and output data matrix pointer */
94 uint32_t numRows
= pSrc
->numRows
; /* Number of rows in the matrix */
95 uint32_t numCols
= pSrc
->numCols
; /* Number of Cols in the matrix */
97 #ifndef ARM_MATH_CM0_FAMILY
98 float32_t maxC
; /* maximum value in the column */
100 /* Run the below code for Cortex-M4 and Cortex-M3 */
102 float32_t Xchg
, in
= 0.0f
, in1
; /* Temporary input values */
103 uint32_t i
, rowCnt
, flag
= 0u, j
, loopCnt
, k
, l
; /* loop counters */
104 arm_status status
; /* status of matrix inverse */
106 #ifdef ARM_MATH_MATRIX_CHECK
109 /* Check for matrix mismatch condition */
110 if((pSrc
->numRows
!= pSrc
->numCols
) || (pDst
->numRows
!= pDst
->numCols
)
111 || (pSrc
->numRows
!= pDst
->numRows
))
113 /* Set status as ARM_MATH_SIZE_MISMATCH */
114 status
= ARM_MATH_SIZE_MISMATCH
;
117 #endif /* #ifdef ARM_MATH_MATRIX_CHECK */
121 /*--------------------------------------------------------------------------------------------------------------
122 * Matrix Inverse can be solved using elementary row operations.
124 * Gauss-Jordan Method:
126 * 1. First combine the identity matrix and the input matrix separated by a bar to form an
127 * augmented matrix as follows:
129 * | a11 a12 | 1 0 | | X11 X12 |
131 * |_ a21 a22 | 0 1 _| |_ X21 X21 _|
133 * 2. In our implementation, pDst Matrix is used as identity matrix.
135 * 3. Begin with the first row. Let i = 1.
137 * 4. Check to see if the pivot for column i is the greatest of the column.
138 * The pivot is the element of the main diagonal that is on the current row.
139 * For instance, if working with row i, then the pivot element is aii.
140 * If the pivot is not the most significant of the coluimns, exchange that row with a row
141 * below it that does contain the most significant value in column i. If the most
142 * significant value of the column is zero, then an inverse to that matrix does not exist.
143 * The most significant value of the column is the absolut maximum.
145 * 5. Divide every element of row i by the pivot.
147 * 6. For every row below and row i, replace that row with the sum of that row and
148 * a multiple of row i so that each new element in column i below row i is zero.
150 * 7. Move to the next row and column and repeat steps 2 through 5 until you have zeros
151 * for every element below and above the main diagonal.
153 * 8. Now an identical matrix is formed to the left of the bar(input matrix, pSrc).
154 * Therefore, the matrix to the right of the bar is our solution(pDst matrix, pDst).
155 *----------------------------------------------------------------------------------------------------------------*/
157 /* Working pointer for destination matrix */
160 /* Loop over the number of rows */
163 /* Making the destination matrix as identity matrix */
166 /* Writing all zeroes in lower triangle of the destination matrix */
167 j
= numRows
- rowCnt
;
174 /* Writing all ones in the diagonal of the destination matrix */
177 /* Writing all zeroes in upper triangle of the destination matrix */
185 /* Decrement the loop counter */
189 /* Loop over the number of columns of the input matrix.
190 All the elements in each column are processed by the row operations */
193 /* Index modifier to navigate through the columns */
198 /* Check if the pivot element is zero..
199 * If it is zero then interchange the row with non zero row below.
200 * If there is no non zero element to replace in the rows below,
201 * then the matrix is Singular. */
203 /* Working pointer for the input matrix that points
204 * to the pivot element of the particular row */
205 pInT1
= pIn
+ (l
* numCols
);
207 /* Working pointer for the destination matrix that points
208 * to the pivot element of the particular row */
209 pInT3
= pOut
+ (l
* numCols
);
211 /* Temporary variable to hold the pivot value */
214 /* Destination pointer modifier */
217 /* Grab the most significant value from column l */
219 for (i
= 0; i
< numRows
; i
++)
221 maxC
= *pInT1
> 0 ? (*pInT1
> maxC
? *pInT1
: maxC
) : (-*pInT1
> maxC
? -*pInT1
: maxC
);
225 /* Update the status if the matrix is singular */
228 status
= ARM_MATH_SINGULAR
;
233 pInT1
-= numRows
* numCols
;
235 /* Check if the pivot element is the most significant of the column */
236 if( (in
> 0.0f
? in
: -in
) != maxC
)
238 /* Loop over the number rows present below */
239 i
= numRows
- (l
+ 1u);
243 /* Update the input and destination pointers */
244 pInT2
= pInT1
+ (numCols
* l
);
245 pInT4
= pInT3
+ (numCols
* k
);
247 /* Look for the most significant element to
248 * replace in the rows below */
249 if((*pInT2
> 0.0f
? *pInT2
: -*pInT2
) == maxC
)
251 /* Loop over number of columns
252 * to the right of the pilot element */
257 /* Exchange the row elements of the input matrix */
262 /* Decrement the loop counter */
266 /* Loop over number of columns of the destination matrix */
271 /* Exchange the row elements of the destination matrix */
276 /* Decrement the loop counter */
280 /* Flag to indicate whether exchange is done or not */
283 /* Break after exchange is done */
287 /* Update the destination pointer modifier */
290 /* Decrement the loop counter */
295 /* Update the status if the matrix is singular */
296 if((flag
!= 1u) && (in
== 0.0f
))
298 status
= ARM_MATH_SINGULAR
;
303 /* Points to the pivot row of input and destination matrices */
304 pPivotRowIn
= pIn
+ (l
* numCols
);
305 pPivotRowDst
= pOut
+ (l
* numCols
);
307 /* Temporary pointers to the pivot row pointers */
309 pInT2
= pPivotRowDst
;
311 /* Pivot element of the row */
314 /* Loop over number of columns
315 * to the right of the pilot element */
320 /* Divide each element of the row of the input matrix
321 * by the pivot element */
325 /* Decrement the loop counter */
329 /* Loop over number of columns of the destination matrix */
334 /* Divide each element of the row of the destination matrix
335 * by the pivot element */
339 /* Decrement the loop counter */
343 /* Replace the rows with the sum of that row and a multiple of row i
344 * so that each new element in column i above row i is zero.*/
346 /* Temporary pointers for input and destination matrices */
350 /* index used to check for pivot element */
353 /* Loop over number of rows */
354 /* to be replaced by the sum of that row and a multiple of row i */
359 /* Check for the pivot element */
362 /* If the processing element is the pivot element,
363 only the columns to the right are to be processed */
364 pInT1
+= numCols
- l
;
370 /* Element of the reference row */
373 /* Working pointers for input and destination pivot rows */
374 pPRT_in
= pPivotRowIn
;
375 pPRT_pDst
= pPivotRowDst
;
377 /* Loop over the number of columns to the right of the pivot element,
378 to replace the elements in the input matrix */
383 /* Replace the element by the sum of that row
384 and a multiple of the reference row */
386 *pInT1
++ = in1
- (in
* *pPRT_in
++);
388 /* Decrement the loop counter */
392 /* Loop over the number of columns to
393 replace the elements in the destination matrix */
398 /* Replace the element by the sum of that row
399 and a multiple of the reference row */
401 *pInT2
++ = in1
- (in
* *pPRT_pDst
++);
403 /* Decrement the loop counter */
409 /* Increment the temporary input pointer */
412 /* Decrement the loop counter */
415 /* Increment the pivot index */
419 /* Increment the input pointer */
422 /* Decrement the loop counter */
425 /* Increment the index modifier */
432 /* Run the below code for Cortex-M0 */
434 float32_t Xchg
, in
= 0.0f
; /* Temporary input values */
435 uint32_t i
, rowCnt
, flag
= 0u, j
, loopCnt
, k
, l
; /* loop counters */
436 arm_status status
; /* status of matrix inverse */
438 #ifdef ARM_MATH_MATRIX_CHECK
440 /* Check for matrix mismatch condition */
441 if((pSrc
->numRows
!= pSrc
->numCols
) || (pDst
->numRows
!= pDst
->numCols
)
442 || (pSrc
->numRows
!= pDst
->numRows
))
444 /* Set status as ARM_MATH_SIZE_MISMATCH */
445 status
= ARM_MATH_SIZE_MISMATCH
;
448 #endif /* #ifdef ARM_MATH_MATRIX_CHECK */
451 /*--------------------------------------------------------------------------------------------------------------
452 * Matrix Inverse can be solved using elementary row operations.
454 * Gauss-Jordan Method:
456 * 1. First combine the identity matrix and the input matrix separated by a bar to form an
457 * augmented matrix as follows:
459 * | | a11 a12 | | | 1 0 | | | X11 X12 |
460 * | | | | | | | = | |
461 * |_ |_ a21 a22 _| | |_0 1 _| _| |_ X21 X21 _|
463 * 2. In our implementation, pDst Matrix is used as identity matrix.
465 * 3. Begin with the first row. Let i = 1.
467 * 4. Check to see if the pivot for row i is zero.
468 * The pivot is the element of the main diagonal that is on the current row.
469 * For instance, if working with row i, then the pivot element is aii.
470 * If the pivot is zero, exchange that row with a row below it that does not
471 * contain a zero in column i. If this is not possible, then an inverse
472 * to that matrix does not exist.
474 * 5. Divide every element of row i by the pivot.
476 * 6. For every row below and row i, replace that row with the sum of that row and
477 * a multiple of row i so that each new element in column i below row i is zero.
479 * 7. Move to the next row and column and repeat steps 2 through 5 until you have zeros
480 * for every element below and above the main diagonal.
482 * 8. Now an identical matrix is formed to the left of the bar(input matrix, src).
483 * Therefore, the matrix to the right of the bar is our solution(dst matrix, dst).
484 *----------------------------------------------------------------------------------------------------------------*/
486 /* Working pointer for destination matrix */
489 /* Loop over the number of rows */
492 /* Making the destination matrix as identity matrix */
495 /* Writing all zeroes in lower triangle of the destination matrix */
496 j
= numRows
- rowCnt
;
503 /* Writing all ones in the diagonal of the destination matrix */
506 /* Writing all zeroes in upper triangle of the destination matrix */
514 /* Decrement the loop counter */
518 /* Loop over the number of columns of the input matrix.
519 All the elements in each column are processed by the row operations */
522 /* Index modifier to navigate through the columns */
524 //for(loopCnt = 0u; loopCnt < numCols; loopCnt++)
527 /* Check if the pivot element is zero..
528 * If it is zero then interchange the row with non zero row below.
529 * If there is no non zero element to replace in the rows below,
530 * then the matrix is Singular. */
532 /* Working pointer for the input matrix that points
533 * to the pivot element of the particular row */
534 pInT1
= pIn
+ (l
* numCols
);
536 /* Working pointer for the destination matrix that points
537 * to the pivot element of the particular row */
538 pInT3
= pOut
+ (l
* numCols
);
540 /* Temporary variable to hold the pivot value */
543 /* Destination pointer modifier */
546 /* Check if the pivot element is zero */
549 /* Loop over the number rows present below */
550 for (i
= (l
+ 1u); i
< numRows
; i
++)
552 /* Update the input and destination pointers */
553 pInT2
= pInT1
+ (numCols
* l
);
554 pInT4
= pInT3
+ (numCols
* k
);
556 /* Check if there is a non zero pivot element to
557 * replace in the rows below */
560 /* Loop over number of columns
561 * to the right of the pilot element */
562 for (j
= 0u; j
< (numCols
- l
); j
++)
564 /* Exchange the row elements of the input matrix */
570 for (j
= 0u; j
< numCols
; j
++)
577 /* Flag to indicate whether exchange is done or not */
580 /* Break after exchange is done */
584 /* Update the destination pointer modifier */
589 /* Update the status if the matrix is singular */
590 if((flag
!= 1u) && (in
== 0.0f
))
592 status
= ARM_MATH_SINGULAR
;
597 /* Points to the pivot row of input and destination matrices */
598 pPivotRowIn
= pIn
+ (l
* numCols
);
599 pPivotRowDst
= pOut
+ (l
* numCols
);
601 /* Temporary pointers to the pivot row pointers */
603 pInT2
= pPivotRowDst
;
605 /* Pivot element of the row */
606 in
= *(pIn
+ (l
* numCols
));
608 /* Loop over number of columns
609 * to the right of the pilot element */
610 for (j
= 0u; j
< (numCols
- l
); j
++)
612 /* Divide each element of the row of the input matrix
613 * by the pivot element */
614 *pInT1
= *pInT1
/ in
;
617 for (j
= 0u; j
< numCols
; j
++)
619 /* Divide each element of the row of the destination matrix
620 * by the pivot element */
621 *pInT2
= *pInT2
/ in
;
625 /* Replace the rows with the sum of that row and a multiple of row i
626 * so that each new element in column i above row i is zero.*/
628 /* Temporary pointers for input and destination matrices */
632 for (i
= 0u; i
< numRows
; i
++)
634 /* Check for the pivot element */
637 /* If the processing element is the pivot element,
638 only the columns to the right are to be processed */
639 pInT1
+= numCols
- l
;
644 /* Element of the reference row */
647 /* Working pointers for input and destination pivot rows */
648 pPRT_in
= pPivotRowIn
;
649 pPRT_pDst
= pPivotRowDst
;
651 /* Loop over the number of columns to the right of the pivot element,
652 to replace the elements in the input matrix */
653 for (j
= 0u; j
< (numCols
- l
); j
++)
655 /* Replace the element by the sum of that row
656 and a multiple of the reference row */
657 *pInT1
= *pInT1
- (in
* *pPRT_in
++);
660 /* Loop over the number of columns to
661 replace the elements in the destination matrix */
662 for (j
= 0u; j
< numCols
; j
++)
664 /* Replace the element by the sum of that row
665 and a multiple of the reference row */
666 *pInT2
= *pInT2
- (in
* *pPRT_pDst
++);
671 /* Increment the temporary input pointer */
674 /* Increment the input pointer */
677 /* Decrement the loop counter */
679 /* Increment the index modifier */
684 #endif /* #ifndef ARM_MATH_CM0_FAMILY */
686 /* Set status as ARM_MATH_SUCCESS */
687 status
= ARM_MATH_SUCCESS
;
689 if((flag
!= 1u) && (in
== 0.0f
))
691 status
= ARM_MATH_SINGULAR
;
694 /* Return to application */
699 * @} end of MatrixInv group