source: svn/newcon3bcm2_21bu/magnum/commonutils/mth/bmth_fix_matrix.c @ 66

Last change on this file since 66 was 66, checked in by megakiss, 11 years ago

키패드 기능 연결

전원키 누르고 리모콘 누르면 학습
CH+ 간격 1초 증가 MAX:15
CH- 간격 1초 감소 MIN :1
  • Property svn:executable set to *
File size: 11.9 KB
Line 
1/***************************************************************************
2 *     Copyright (c) 2003-2010, Broadcom Corporation
3 *     All Rights Reserved
4 *     Confidential Property of Broadcom Corporation
5 *
6 *  THIS SOFTWARE MAY ONLY BE USED SUBJECT TO AN EXECUTED SOFTWARE LICENSE
7 *  AGREEMENT  BETWEEN THE USER AND BROADCOM.  YOU HAVE NO RIGHT TO USE OR
8 *  EXPLOIT THIS MATERIAL EXCEPT SUBJECT TO THE TERMS OF SUCH AN AGREEMENT.
9 *
10 * $brcm_Workfile: bmth_fix_matrix.c $
11 * $brcm_Revision: Hydra_Software_Devel/3 $
12 * $brcm_Date: 10/4/10 3:20p $
13 *
14 * Module Description:
15 *
16 * Revision History:
17 *
18 * $brcm_Log: /magnum/commonutils/mth/bmth_fix_matrix.c $
19 *
20 * Hydra_Software_Devel/3   10/4/10 3:20p albertl
21 * SW7340-213: Initialized matrices to remove coverity error.
22 *
23 * Hydra_Software_Devel/2   10/1/10 2:42p albertl
24 * SW7405-4556: Removed debug dump messages.
25 *
26 * Hydra_Software_Devel/1   8/26/10 7:48p albertl
27 * SW7405-4556, SW7405-4515: Initial revision.
28 *
29 *
30 ***************************************************************************/
31#include "bmth_fix_matrix.h"
32
33/*************************************************************************
34 * BMTH_FIX_SIGNED_CONVERT
35 *
36 *************************************************************************/
37
38BDBG_MODULE(BMTH_FIX_MATRIX);
39
40#define BMTH_P_FIX_MATRIX_MAX_BITS (31)
41
42#define BMTH_P_FIX_MATRIX_CONVERT(x, infract, outfract) \
43        (BMTH_FIX_SIGNED_CONVERT(x, (BMTH_P_FIX_MATRIX_MAX_BITS - infract), infract, (BMTH_P_FIX_MATRIX_MAX_BITS - outfract), outfract))
44
45/* fixed to integer */
46#define BMTH_P_FIX_MATRIX_FIXTOI(x, infract) \
47        (BMTH_FIX_SIGNED_FIXTOI(x, (BMTH_P_FIX_MATRIX_MAX_BITS - infract), infract))
48
49/* integer to fixed */
50#define BMTH_P_FIX_MATRIX_ITOFIX(x, outfract) \
51        (BMTH_FIX_SIGNED_ITOFIX(x, (BMTH_P_FIX_MATRIX_MAX_BITS - outfract), outfract))
52
53/* fixed point operation multiply */
54#define BMTH_P_FIX_MATRIX_MUL(x, y, fract) \
55        (BMTH_FIX_SIGNED_MUL(x, y, (BMTH_P_FIX_MATRIX_MAX_BITS - fract), fract, \
56                               (BMTH_P_FIX_MATRIX_MAX_BITS - fract), fract, \
57                               (BMTH_P_FIX_MATRIX_MAX_BITS - fract), fract))
58
59/* fixed point operation divide */
60#define BMTH_P_FIX_MATRIX_DIV(x, y, fract) \
61        (BMTH_FIX_SIGNED_DIV(x, y, (BMTH_P_FIX_MATRIX_MAX_BITS - fract), fract, \
62                               (BMTH_P_FIX_MATRIX_MAX_BITS - fract), fract, \
63                               (BMTH_P_FIX_MATRIX_MAX_BITS - fract), fract))
64
65
66
67/* Prototypes */
68uint32_t BMTH_P_FIX_Matrix_Cofactor
69        ( BMTH_FIX_Matrix                           *pMatrix,
70          uint32_t                                   ulRow,
71          uint32_t                                   ulCol);
72
73void BMTH_P_FIX_Matrix_CofactorMatrix
74        ( BMTH_FIX_Matrix                           *pMatrix,
75          BMTH_FIX_Matrix                           *pCofactors);
76
77
78/***************************************************************************
79 * {private}
80 *
81 * Takes two matrices and multiplies them together.  Result is a matrix
82 * with the same size and fixed point fractional bits.
83 */
84void BMTH_FIX_Matrix_Mult(BMTH_FIX_Matrix *pMatrix1, BMTH_FIX_Matrix *pMatrix2, BMTH_FIX_Matrix *pRetMatrix)
85{
86        BMTH_FIX_Matrix stMatrixTemp;
87        uint32_t ulSize = pMatrix1->ulSize;
88        uint32_t ulFractBits = pMatrix1->ulFractBits;
89        uint32_t i = 0;
90        uint32_t j = 0;
91        uint32_t k = 0;
92
93        BDBG_ASSERT(pMatrix1->ulSize == pMatrix2->ulSize);
94        BDBG_ASSERT(pMatrix1->ulFractBits == pMatrix2->ulFractBits);
95
96        stMatrixTemp.ulSize = ulSize;
97        stMatrixTemp.ulFractBits = ulFractBits;
98
99        for (i = 0; i < ulSize; i++)
100        {
101                for (j = 0; j < ulSize; j++)
102                {
103                        stMatrixTemp.data[i][j] = 0;
104                        for (k = 0; k < ulSize; k++)
105                        {
106                                stMatrixTemp.data[i][j] += (unsigned)BMTH_P_FIX_MATRIX_MUL((signed)pMatrix1->data[i][k],  (signed)pMatrix2->data[k][j], ulFractBits);
107                        }
108                }
109        }
110
111        BKNI_Memcpy(pRetMatrix, &stMatrixTemp, sizeof(stMatrixTemp));
112}
113
114/***************************************************************************
115 * {private}
116 *
117 * Multiplies a Matrix with a Vector.  Result is a vector with the same
118 * size as the original vector and same fixed point fractional bits.
119 */
120void BMTH_FIX_Matrix_MultVector(BMTH_FIX_Matrix *pMatrix, BMTH_FIX_Vector *pVector, BMTH_FIX_Vector *pRetVector)
121{
122        BMTH_FIX_Vector stTempVector;
123        uint32_t ulSize = pMatrix->ulSize;
124        uint32_t ulFractBits = pMatrix->ulFractBits;
125        uint32_t i = 0;
126        uint32_t k = 0;
127
128        BDBG_ASSERT(pMatrix->ulSize == pVector->ulSize);
129        BDBG_ASSERT(pMatrix->ulFractBits == pVector->ulFractBits);
130
131        stTempVector.ulSize = ulSize;
132        stTempVector.ulFractBits = ulFractBits;
133
134        for (i = 0; i < ulSize; i++)
135        {
136                stTempVector.data[i] = 0;
137                for (k = 0; k < ulSize; k++)
138                {
139                        stTempVector.data[i] += BMTH_P_FIX_MATRIX_MUL(pMatrix->data[i][k],  pVector->data[k], ulFractBits);
140                }
141        }
142
143        BKNI_Memcpy(pRetVector, &stTempVector, sizeof(stTempVector));
144}
145
146/***************************************************************************
147 * {private}
148 *
149 * Takes a 3x3 matrix and converts it to a 4x4 matrix.
150 */
151void BMTH_FIX_Matrix_Make4x4(BMTH_FIX_Matrix *pMatrix, BMTH_FIX_Matrix *pRetMatrix)
152{
153        uint32_t ulFractBits = pMatrix->ulFractBits;
154        uint32_t i = 0;
155        uint32_t j = 0;
156
157        BDBG_ASSERT(pMatrix->ulSize == 3);
158
159        BKNI_Memset(pRetMatrix, 0, sizeof(BMTH_FIX_Matrix));
160        pRetMatrix->ulSize = 4;
161        pRetMatrix->ulFractBits = ulFractBits;
162        pRetMatrix->data[3][3] = BMTH_P_FIX_MATRIX_ITOFIX(1, ulFractBits);
163
164        for (i = 0; i < 3; i++)
165        {
166                for (j = 0; j < 3; j++)
167                {
168                        pRetMatrix->data[i][j] = pMatrix->data[i][j];
169                }
170        }
171}
172
173/***************************************************************************
174 * {private}
175 *
176 * Calculates the cofactor of a matrix at a specific position.
177 */
178uint32_t BMTH_P_FIX_Matrix_Cofactor(BMTH_FIX_Matrix *pMatrix, uint32_t ulRow, uint32_t ulCol)
179{
180        uint32_t ulSignPos = (ulCol + ulRow) & 1;
181        uint32_t ulSize = pMatrix->ulSize;
182        uint32_t ulFractBits = pMatrix->ulFractBits;
183        uint32_t i = 0;
184        uint32_t j = 0;
185        uint32_t ulSrcRow = 0;
186        uint32_t ulSrcCol = 0;
187        uint32_t ulCofactor = 0;
188
189        BMTH_FIX_Matrix stTempMatrix;
190
191        BKNI_Memset(&stTempMatrix, 0, sizeof(BMTH_FIX_Matrix));
192        stTempMatrix.ulSize = ulSize - 1;
193        stTempMatrix.ulFractBits = ulFractBits;
194
195        /* build 3x3 matrix for calculating determinant */
196        for (i = 0, ulSrcRow = 0; i < ulSize; i++, ulSrcRow++)
197        {
198                if (i == ulRow)
199                {
200                        ulSrcRow++;
201                }
202                for (j = 0, ulSrcCol = 0; j < ulSize; j++, ulSrcCol++)
203                {
204                        if (j == ulCol)
205                        {
206                                ulSrcCol++;
207                        }
208                        stTempMatrix.data[i][j] = pMatrix->data[ulSrcRow][ulSrcCol];
209                }
210        }
211
212        /* get determinant */
213        ulCofactor = BMTH_FIX_Matrix_Determinant(&stTempMatrix, NULL);
214
215        /* apply sign based on position */
216        ulCofactor *= (ulSignPos) ? -1 : 1;
217
218        return ulCofactor;
219}
220
221/***************************************************************************
222 * {private}
223 *
224 * Calculates the determinant of a matrix.  If cofactor matrix is present,
225 * precaculated cofactors from the table are used.
226 */
227uint32_t BMTH_FIX_Matrix_Determinant(BMTH_FIX_Matrix *pMatrix, BMTH_FIX_Matrix *pCofactors)
228{
229        uint32_t ulCol;
230        uint32_t ulDeterminant = 0;
231        uint32_t ulSize = pMatrix->ulSize;
232        uint32_t ulFractBits = pMatrix->ulFractBits;
233
234        if (ulSize == 1)
235        {
236                ulDeterminant = pMatrix->data[0][0];
237        }
238        else if (ulSize == 2)
239        {
240                ulDeterminant = BMTH_P_FIX_MATRIX_MUL(pMatrix->data[0][0], pMatrix->data[1][1], ulFractBits) -
241                                    BMTH_P_FIX_MATRIX_MUL(pMatrix->data[0][1], pMatrix->data[1][0], ulFractBits);
242/*              printf("a: %f * d: %f - b: %f * c: %f \n",
243                        BMTH_FIX_SIGNED_FIXTOF(pMatrix->data[0][0], 31 - pMatrix->ulFractBits, pMatrix->ulFractBits),
244                        BMTH_FIX_SIGNED_FIXTOF(pMatrix->data[1][1], 31 - pMatrix->ulFractBits, pMatrix->ulFractBits),
245                        BMTH_FIX_SIGNED_FIXTOF(pMatrix->data[0][1], 31 - pMatrix->ulFractBits, pMatrix->ulFractBits),
246                        BMTH_FIX_SIGNED_FIXTOF(pMatrix->data[1][0], 31 - pMatrix->ulFractBits, pMatrix->ulFractBits));
247                printf("a * d (0x%x) %f - b * c (0x%x) %f\n",
248                        BMTH_P_FIX_MATRIX_MUL(pMatrix->data[0][0], pMatrix->data[1][1], ulFractBits),
249                        BMTH_FIX_SIGNED_FIXTOF(BMTH_P_FIX_MATRIX_MUL(pMatrix->data[0][0], pMatrix->data[1][1], ulFractBits), 31 - pMatrix->ulFractBits, pMatrix->ulFractBits),
250                        BMTH_P_FIX_MATRIX_MUL(pMatrix->data[0][1], pMatrix->data[1][0], ulFractBits),
251                        BMTH_FIX_SIGNED_FIXTOF(BMTH_P_FIX_MATRIX_MUL(pMatrix->data[0][1], pMatrix->data[1][0], ulFractBits), 31 - pMatrix->ulFractBits, pMatrix->ulFractBits));
252*/
253        }
254        else
255        {
256                /* add cofactors along top row */
257                for (ulCol = 0; ulCol < ulSize; ulCol++)
258                {
259                        if (pCofactors)
260                        {
261                                /* use precomputed cofactors */
262                                ulDeterminant += BMTH_P_FIX_MATRIX_MUL(pMatrix->data[0][ulCol], pCofactors->data[0][ulCol], ulFractBits);
263                        }
264                        else
265                        {
266                                ulDeterminant += BMTH_P_FIX_MATRIX_MUL(pMatrix->data[0][ulCol], BMTH_P_FIX_Matrix_Cofactor(pMatrix, 0, ulCol), ulFractBits);
267                        }
268                }
269        }
270
271        return ulDeterminant;
272}
273
274/***************************************************************************
275 * {private}
276 *
277 * Creates a matrix holding all cofactors of a matrix.
278 */
279void BMTH_P_FIX_Matrix_CofactorMatrix(BMTH_FIX_Matrix *pMatrix, BMTH_FIX_Matrix *pCofactors)
280{
281        uint32_t ulRow;
282        uint32_t ulCol;
283        uint32_t ulSize = pMatrix->ulSize;
284        uint32_t ulFractBits = pMatrix->ulFractBits;
285
286        for (ulRow = 0; ulRow < ulSize; ulRow++)
287        {
288                for (ulCol = 0; ulCol < ulSize; ulCol++)
289                {
290                        pCofactors->data[ulRow][ulCol] = BMTH_P_FIX_Matrix_Cofactor(pMatrix, ulRow, ulCol);
291                }
292        }
293
294        pCofactors->ulSize = ulSize;
295        pCofactors->ulFractBits = ulFractBits;
296}
297
298/***************************************************************************
299 * {private}
300 *
301 * Transposes a matrix.
302 */
303void BMTH_FIX_Matrix_Transpose(BMTH_FIX_Matrix *pMatrix, BMTH_FIX_Matrix *pRetMatrix)
304{
305        uint32_t ulRow;
306        uint32_t ulCol;
307        uint32_t ulSize = pMatrix->ulSize;
308        uint32_t ulFractBits = pMatrix->ulFractBits;
309
310        for (ulRow = 0; ulRow < ulSize; ulRow++)
311        {
312                for (ulCol = 0; ulCol < ulSize; ulCol++)
313                {
314                        pRetMatrix->data[ulRow][ulCol] = pMatrix->data[ulCol][ulRow];
315                }
316        }
317
318        pRetMatrix->ulSize = ulSize;
319        pRetMatrix->ulFractBits = ulFractBits;
320}
321
322/***************************************************************************
323 * {private}
324 *
325 * Multiplies a matrix by a scalar.
326 */
327void BMTH_FIX_Matrix_MultScalar(BMTH_FIX_Matrix *pMatrix, uint32_t ulScalar, BMTH_FIX_Matrix *pRetMatrix)
328{
329        uint32_t ulRow;
330        uint32_t ulCol;
331        uint32_t ulSize = pMatrix->ulSize;
332        uint32_t ulFractBits = pMatrix->ulFractBits;
333
334        for (ulRow = 0; ulRow < ulSize; ulRow++)
335        {
336                for (ulCol = 0; ulCol < ulSize; ulCol++)
337                {
338                        pRetMatrix->data[ulRow][ulCol] = BMTH_P_FIX_MATRIX_MUL(pMatrix->data[ulRow][ulCol], ulScalar, ulFractBits);
339                }
340        }
341
342        pRetMatrix->ulSize = ulSize;
343        pRetMatrix->ulFractBits = ulFractBits;
344}
345
346/***************************************************************************
347 * {private}
348 *
349 * Calculates the inverse matrix.
350 */
351void BMTH_FIX_Matrix_Inverse(BMTH_FIX_Matrix *pMatrix, BMTH_FIX_Matrix *pRetMatrix)
352{
353        uint32_t ulFractBits = pMatrix->ulFractBits;
354        BMTH_FIX_Matrix stCofactorMatrix;
355        BMTH_FIX_Matrix stAdjointMatrix;
356        uint32_t ulDeterminant;
357        uint32_t ulOneOverDeterminant;
358        uint32_t ulFixOne = BMTH_P_FIX_MATRIX_ITOFIX(1, ulFractBits);
359
360        BKNI_Memset(&stCofactorMatrix, 0, sizeof(BMTH_FIX_Matrix));
361        BKNI_Memset(&stAdjointMatrix, 0, sizeof(BMTH_FIX_Matrix));
362
363        /* InvM = 1/det(M) * Transpose(Cofactor(M)) */
364
365        /* compute all cofactors */
366        BMTH_P_FIX_Matrix_CofactorMatrix(pMatrix, &stCofactorMatrix);
367        ulDeterminant = BMTH_FIX_Matrix_Determinant(pMatrix, &stCofactorMatrix);
368        BDBG_ASSERT(ulDeterminant);
369        ulOneOverDeterminant = BMTH_P_FIX_MATRIX_DIV(ulFixOne, ulDeterminant, ulFractBits);
370        BMTH_FIX_Matrix_Transpose(&stCofactorMatrix, &stAdjointMatrix);
371        BMTH_FIX_Matrix_MultScalar(&stAdjointMatrix, ulOneOverDeterminant, pRetMatrix);
372}
373
374/***************************************************************************
375 * {private}
376 *
377 * Prints a matrix.
378 */
379void BMTH_FIX_Matrix_Dump(BMTH_FIX_Matrix *pMatrix)
380{
381        uint32_t i = 0;
382        uint32_t j = 0;
383
384        for (i = 0; i < pMatrix->ulSize; i++)
385        {
386                for (j = 0; j < pMatrix->ulSize; j++)
387                {
388                        BKNI_Printf("0x%x ", pMatrix->data[i][j]);
389/*                      BKNI_Printf("%f ", BMTH_FIX_SIGNED_FIXTOF(pMatrix->data[i][j], 31 - pMatrix->ulFractBits, pMatrix->ulFractBits));*/
390                }
391                BKNI_Printf("\n");
392        }
393        BKNI_Printf("\n");
394}
395
396/* end of file */
Note: See TracBrowser for help on using the repository browser.