source: svn/trunk/newcon3bcm2_21bu/magnum/commonutils/csc/bcsc.c @ 2

Last change on this file since 2 was 2, checked in by jglee, 11 years ago

first commit
  • Property svn:executable set to *
File size: 23.3 KB
Line 
1/***************************************************************************
2 *     Copyright (c) 2003-2007, 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: bcsc.c $
11 * $brcm_Revision: Hydra_Software_Devel/2 $
12 * $brcm_Date: 6/11/07 6:48p $
13 *
14 * Module Description:
15 *
16 * Revision History:
17 *
18 * $brcm_Log: /magnum/commonutils/csc/bcsc.c $
19 *
20 * Hydra_Software_Devel/2   6/11/07 6:48p albertl
21 * PR31093:  Fixed some fixed math errors.
22 *
23 * Hydra_Software_Devel/1   5/31/07 7:29p albertl
24 * PR31093:  Initial version.
25 *
26 ***************************************************************************/
27#include "bstd.h"              /* standard types */
28#include "bkni.h"              /* memcpy calls */
29#include "bmth_fix.h"
30#include "bcsc.h"
31#include "bdbg.h"
32
33BDBG_MODULE(BCSC);
34
35#define BCSC_P_FIX_MAX_BITS (31)
36
37#define BCSC_P_FIX_CONVERT(x, infract, outfract) \
38        (BMTH_FIX_SIGNED_CONVERT(x, (BCSC_P_FIX_MAX_BITS - infract), infract, (BCSC_P_FIX_MAX_BITS - outfract), outfract))
39
40/* fixed to integer */
41#define BCSC_P_FIXTOI(x, infract) \
42        (BMTH_FIX_SIGNED_FIXTOI(x, (BCSC_P_FIX_MAX_BITS - infract), infract))
43
44/* integer to fixed */
45#define BCSC_P_ITOFIX(x, outfract) \
46        (BMTH_FIX_SIGNED_ITOFIX(x, (BCSC_P_FIX_MAX_BITS - outfract), outfract))
47
48/* fixed point operation multiply */
49#define BCSC_P_FIX_MUL(x, y, fract) \
50        (BMTH_FIX_SIGNED_MUL(x, y, (BCSC_P_FIX_MAX_BITS - fract), fract, \
51                               (BCSC_P_FIX_MAX_BITS - fract), fract, \
52                               (BCSC_P_FIX_MAX_BITS - fract), fract))
53
54/* fixed point operation divide */
55#define BCSC_P_FIX_DIV(x, y, fract) \
56        (BMTH_FIX_SIGNED_DIV(x, y, (BCSC_P_FIX_MAX_BITS - fract), fract, \
57                               (BCSC_P_FIX_MAX_BITS - fract), fract, \
58                               (BCSC_P_FIX_MAX_BITS - fract), fract))
59
60typedef struct BCSC_P_Matrix
61{
62        uint32_t data[4][4];
63        uint32_t ulSize;
64        uint32_t ulFractBits;
65} BCSC_P_Matrix;
66
67typedef struct BCSC_P_Vector
68{
69        uint32_t data[4];
70        uint32_t ulSize;
71        uint32_t ulFractBits;
72} BCSC_P_Vector;
73
74
75/* Prototypes */
76void BCSC_P_Matrix_RGBW_To_NPM
77        ( uint32_t                              *pulRGBW,
78          uint32_t                               ulRGBWFractBits,
79          BCSC_P_Matrix                         *pNPM,
80          uint32_t                               ulNPMFractBits);
81
82void BCSC_P_Matrix_NPM_To_RGBPrime_To_YCbCrPrime
83        ( BCSC_P_Matrix                         *pNPM,
84          BCSC_P_Matrix                         *pRetMatrix);
85
86void BCSC_P_Matrix_Mult
87        ( BCSC_P_Matrix                         *pMatrix1,
88          BCSC_P_Matrix                         *pMatrix2,
89          BCSC_P_Matrix                         *pRetMatrix);
90
91void BCSC_P_Matrix_MultVector
92        ( BCSC_P_Matrix                         *pMatrix,
93          BCSC_P_Vector                         *pVector,
94          BCSC_P_Vector                         *pRetVector);
95
96void BCSC_P_Matrix_Make4x4
97        ( BCSC_P_Matrix                         *pMatrix,
98          BCSC_P_Matrix                         *pRetMatrix);
99
100void BCSC_P_Matrix_GScale
101        ( uint32_t                               ulN,
102          BCSC_P_Matrix                         *pRetMatrix,
103          uint32_t                               ulMatrixFractBits);
104
105void BCSC_P_Matrix_GScaleInverse
106        ( uint32_t                               ulN,
107          BCSC_P_Matrix                         *pRetMatrix,
108          uint32_t                               ulMatrixFractBits);
109
110void BCSC_P_Matrix_GGain
111        ( uint32_t                               ulN,
112          BCSC_P_Matrix                         *pRetMatrix,
113          uint32_t                               ulMatrixFractBits);
114
115uint32_t BCSC_P_Matrix_Cofactor
116        ( BCSC_P_Matrix                         *pMatrix,
117          uint32_t                               ulRow,
118          uint32_t                               ulCol);
119
120uint32_t BCSC_P_Matrix_Determinant
121        ( BCSC_P_Matrix                         *pMatrix,
122          BCSC_P_Matrix                         *pCofactors);
123
124void BCSC_P_Matrix_CofactorMatrix
125        ( BCSC_P_Matrix                         *pMatrix,
126          BCSC_P_Matrix                         *pCofactors);
127
128void BCSC_P_Matrix_Transpose
129        ( BCSC_P_Matrix                         *pMatrix,
130          BCSC_P_Matrix                         *pRetMatrix);
131
132void BCSC_P_Matrix_Inverse
133        ( BCSC_P_Matrix                         *pMatrix,
134          BCSC_P_Matrix                         *pRetMatrix);
135
136void BCSC_P_Matrix_Dump
137        ( BCSC_P_Matrix                         *pMatrix);
138
139/*  Given source and display (xR, yR), (xG, yG), (xB, yB), (xW, yW), derive CMP matrix;
140    Output: CMP matrix in array of 12 fixed point coefficients;
141    inputs:
142        pul_SrcRGBW       - Source gamut in (xR, yR), (xG, yG), (xB, yB), (xW, yW) array, 8 values;
143        pul_DispRGBW:     - Display gamut in (xR, yR), (xG, yG), (xB, yB), (xW, yW) array, 8 values;
144        ulXyFractBits     - Fixed-point fractional bits of input gamut values in (x, y) array;
145        ulMatrixFractBits - Fixed-point fractional bits of output matrix coefficients; */
146BERR_Code BCSC_RGBW2CmpMatrix
147        ( uint32_t                              *pul_SrcRGBW,
148          uint32_t                              *pul_DispRGBW,
149          uint32_t                               ulXyFractBits,
150          uint32_t                               ulMatrixFractBits,
151          uint32_t                               ulN,
152          int32_t                               *plCmpMatrix,
153          int32_t                               *plDvoMatrix)
154{
155        BCSC_P_Matrix stMatrix3_SrcNPM;
156        BCSC_P_Matrix stMatrix3_Src_RGBPrime_To_YCbCrPrime;
157        BCSC_P_Matrix stMatrix3_Src_YCbCrPrime_To_RGBPrime;
158        BCSC_P_Matrix stMatrix4_Src_YCbCrPrime_To_RGBPrime;
159        BCSC_P_Matrix stMatrix3_DispNPM;
160        BCSC_P_Matrix stMatrix3_DispNPMInv;
161        BCSC_P_Matrix stMatrix3_Disp_RGBPrime_To_YCbCrPrime;
162        BCSC_P_Matrix stMatrix4_Disp_RGBPrime_To_YCbCrPrime;
163        BCSC_P_Matrix stMatrix4_Disp_YCbCrPrime_To_RGBPrime;
164        BCSC_P_Matrix stMatrix4_RGB_S_To_D;
165        BCSC_P_Matrix stMatrix4_GScale;
166        BCSC_P_Matrix stMatrix4_GScaleInv;
167        BCSC_P_Matrix stMatrix4_GGain;
168        BCSC_P_Matrix stMatrix4_CMP_CSC;
169        BCSC_P_Matrix stMatrix4_DVI_CSC;
170        BCSC_P_Matrix stMatrix3_Temp;
171
172        uint32_t i = 0;
173        uint32_t j = 0;
174
175        if(!pul_SrcRGBW || !pul_DispRGBW || !plCmpMatrix || !plDvoMatrix)
176        {
177                return BERR_INVALID_PARAMETER;
178        }
179
180        stMatrix3_SrcNPM.ulSize                      = 3;
181        stMatrix3_Src_RGBPrime_To_YCbCrPrime.ulSize  = 3;
182        stMatrix3_DispNPM.ulSize                     = 3;
183        stMatrix3_DispNPMInv.ulSize                  = 3;
184        stMatrix3_Disp_RGBPrime_To_YCbCrPrime.ulSize = 3;
185        stMatrix3_Temp.ulSize                        = 3;
186
187        stMatrix4_Src_YCbCrPrime_To_RGBPrime.ulSize  = 4;
188        stMatrix4_Disp_RGBPrime_To_YCbCrPrime.ulSize = 4;
189        stMatrix4_Disp_YCbCrPrime_To_RGBPrime.ulSize = 4;
190        stMatrix4_RGB_S_To_D.ulSize                  = 4;
191        stMatrix4_GScale.ulSize                      = 4;
192        stMatrix4_GScaleInv.ulSize                   = 4;
193        stMatrix4_GGain.ulSize                       = 4;
194        stMatrix4_CMP_CSC.ulSize                     = 4;
195        stMatrix4_DVI_CSC.ulSize                     = 4;
196
197        /* calculate 3x3 Source NPM */
198        BCSC_P_Matrix_RGBW_To_NPM(pul_SrcRGBW, ulXyFractBits, &stMatrix3_SrcNPM, ulMatrixFractBits);
199
200        /* calculate 3x3 Source R'G'B' to Y'Cb'Cr' matrix */
201        BCSC_P_Matrix_NPM_To_RGBPrime_To_YCbCrPrime(&stMatrix3_SrcNPM, &stMatrix3_Src_RGBPrime_To_YCbCrPrime);
202
203        /* calculate 4x4 Source Y'Cb'Cr' to R'G'B' matrix */
204        BCSC_P_Matrix_Inverse(&stMatrix3_Src_RGBPrime_To_YCbCrPrime, &stMatrix3_Src_YCbCrPrime_To_RGBPrime);
205        BCSC_P_Matrix_Make4x4(&stMatrix3_Src_YCbCrPrime_To_RGBPrime, &stMatrix4_Src_YCbCrPrime_To_RGBPrime);
206
207        /* calculate 3x3 Display NPM */
208        BCSC_P_Matrix_RGBW_To_NPM(pul_DispRGBW, ulXyFractBits, &stMatrix3_DispNPM, ulMatrixFractBits);
209
210        /* calculate 4x4 Display R'G'B' to Y'Cb'Cr' matrix */
211        BCSC_P_Matrix_NPM_To_RGBPrime_To_YCbCrPrime(&stMatrix3_DispNPM, &stMatrix3_Disp_RGBPrime_To_YCbCrPrime);
212        BCSC_P_Matrix_Make4x4(&stMatrix3_Disp_RGBPrime_To_YCbCrPrime, &stMatrix4_Disp_RGBPrime_To_YCbCrPrime);
213
214        /* calculate 4x4 Display Y'Cb'Cr' to R'G'B' matrix */
215        BCSC_P_Matrix_Inverse(&stMatrix4_Disp_RGBPrime_To_YCbCrPrime, &stMatrix4_Disp_YCbCrPrime_To_RGBPrime);
216
217        /* calculate 4x4 RGB source to RGB display matrix */
218        BCSC_P_Matrix_Inverse(&stMatrix3_DispNPM, &stMatrix3_DispNPMInv);
219        BCSC_P_Matrix_Mult(&stMatrix3_DispNPMInv, &stMatrix3_SrcNPM, &stMatrix3_Temp);
220        BCSC_P_Matrix_Make4x4(&stMatrix3_Temp, &stMatrix4_RGB_S_To_D);
221
222        /* calculate 4x4 G scale and G gain matrices */
223        BCSC_P_Matrix_GScale(ulN, &stMatrix4_GScale, ulMatrixFractBits);
224/*      BCSC_P_Matrix_Inverse(&stMatrix4_GScale, &stMatrix4_GScaleInv);*/
225        BCSC_P_Matrix_GScaleInverse(ulN, &stMatrix4_GScaleInv, ulMatrixFractBits);
226        BCSC_P_Matrix_GGain(ulN, &stMatrix4_GGain, ulMatrixFractBits);
227
228        /* calculate final CMP CSC matrix */
229        BCSC_P_Matrix_Mult(&stMatrix4_Src_YCbCrPrime_To_RGBPrime,  &stMatrix4_GScale,  &stMatrix4_CMP_CSC);
230        BCSC_P_Matrix_Mult(&stMatrix4_RGB_S_To_D,                  &stMatrix4_CMP_CSC, &stMatrix4_CMP_CSC);
231        BCSC_P_Matrix_Mult(&stMatrix4_Disp_RGBPrime_To_YCbCrPrime, &stMatrix4_CMP_CSC, &stMatrix4_CMP_CSC);
232        BCSC_P_Matrix_Mult(&stMatrix4_GScaleInv,                   &stMatrix4_CMP_CSC, &stMatrix4_CMP_CSC);
233
234        /* calculate final DVI CSC matrix */
235        BCSC_P_Matrix_Mult(&stMatrix4_Disp_YCbCrPrime_To_RGBPrime, &stMatrix4_GScale,  &stMatrix4_DVI_CSC);
236        BCSC_P_Matrix_Mult(&stMatrix4_GGain,                       &stMatrix4_DVI_CSC, &stMatrix4_DVI_CSC);
237
238        for (i = 0; i < 4; i++)
239        {
240                for (j = 0; j < 4; j++)
241                {
242                        plCmpMatrix[i*4 + j] = stMatrix4_CMP_CSC.data[i][j];
243                        plDvoMatrix[i*4 + j] = stMatrix4_DVI_CSC.data[i][j];
244                }
245        }
246
247        return BERR_SUCCESS;
248}
249
250/***************************************************************************
251 * {private}
252 *
253 * Takes a RGBW array and outputs a 3x3 NPM matrix.
254 */
255void BCSC_P_Matrix_RGBW_To_NPM(uint32_t *pulRGBW, uint32_t ulRGBWFractBits, BCSC_P_Matrix *pNPM, uint32_t ulNPMFractBits)
256{
257        uint32_t ulSize = 3;
258        uint32_t ulRx = BCSC_P_FIX_CONVERT(pulRGBW[0], ulRGBWFractBits, ulNPMFractBits);
259        uint32_t ulRy = BCSC_P_FIX_CONVERT(pulRGBW[1], ulRGBWFractBits, ulNPMFractBits);
260        uint32_t ulGx = BCSC_P_FIX_CONVERT(pulRGBW[2], ulRGBWFractBits, ulNPMFractBits);
261        uint32_t ulGy = BCSC_P_FIX_CONVERT(pulRGBW[3], ulRGBWFractBits, ulNPMFractBits);
262        uint32_t ulBx = BCSC_P_FIX_CONVERT(pulRGBW[4], ulRGBWFractBits, ulNPMFractBits);
263        uint32_t ulBy = BCSC_P_FIX_CONVERT(pulRGBW[5], ulRGBWFractBits, ulNPMFractBits);
264        uint32_t ulWx = BCSC_P_FIX_CONVERT(pulRGBW[6], ulRGBWFractBits, ulNPMFractBits);
265        uint32_t ulWy = BCSC_P_FIX_CONVERT(pulRGBW[7], ulRGBWFractBits, ulNPMFractBits);
266        BCSC_P_Matrix stMatrixP;
267        BCSC_P_Matrix stMatrixPInv;
268        BCSC_P_Matrix stMatrixC;
269        BCSC_P_Vector stVectorW;
270        BCSC_P_Vector stVectorCRGB;
271
272        uint32_t ulFixOne = BCSC_P_ITOFIX(1, ulNPMFractBits);
273
274        /* Matrix P */
275        stMatrixP.ulSize = ulSize;
276        stMatrixP.ulFractBits = ulNPMFractBits;
277        stMatrixP.data[0][0] = ulRx;
278        stMatrixP.data[0][1] = ulRy;
279        stMatrixP.data[0][2] = ulGx;
280        stMatrixP.data[1][0] = ulGy;
281        stMatrixP.data[1][1] = ulBx;
282        stMatrixP.data[1][2] = ulBy;
283        stMatrixP.data[2][0] = ulFixOne - (ulRx + ulRy);
284        stMatrixP.data[2][1] = ulFixOne - (ulGx + ulGy);
285        stMatrixP.data[2][2] = ulFixOne - (ulBx + ulBy);
286
287        /* Vector W */
288        stVectorW.ulSize = ulSize;
289        stVectorW.ulFractBits = ulNPMFractBits;
290        /* ulWx / ulWy */
291        stVectorW.data[0] = BCSC_P_FIX_DIV(ulWx, ulWy, ulNPMFractBits);
292        /* 1 */
293        stVectorW.data[1] = ulFixOne;
294        /* (1 - ulWx - ulWy) / ulWy */
295        stVectorW.data[2] = BCSC_P_FIX_DIV((ulFixOne - ulWx - ulWy), ulWy, ulNPMFractBits);
296       
297        /* Vector C RGB */
298        stVectorCRGB.ulSize = ulSize;
299        stVectorCRGB.ulFractBits = ulNPMFractBits;
300        BCSC_P_Matrix_Inverse(&stMatrixP, &stMatrixPInv);
301        BCSC_P_Matrix_MultVector(&stMatrixPInv, &stVectorW, &stVectorCRGB);
302
303        /* Matrix C */
304        stMatrixC.ulSize = ulSize;
305        stMatrixC.ulFractBits = ulNPMFractBits;
306        stMatrixC.data[0][0] = stVectorCRGB.data[0];
307        stMatrixC.data[0][1] = 0;
308        stMatrixC.data[0][2] = 0;
309        stMatrixC.data[1][0] = 0;
310        stMatrixC.data[1][1] = stVectorCRGB.data[1];
311        stMatrixC.data[1][2] = 0;
312        stMatrixC.data[2][0] = 0;
313        stMatrixC.data[2][1] = 0;
314        stMatrixC.data[2][2] = stVectorCRGB.data[2];
315
316        BCSC_P_Matrix_Mult(&stMatrixP, &stMatrixC, pNPM);
317}
318
319/***************************************************************************
320 * {private}
321 *
322 * Takes a 3x3 NPM matrix and outputs a 3x3 RGBPrime_To_YCbCrPrime matrix.
323 */
324void BCSC_P_Matrix_NPM_To_RGBPrime_To_YCbCrPrime(BCSC_P_Matrix *pNPM, BCSC_P_Matrix *pRetMatrix)
325{
326        uint32_t ulFractBits = pNPM->ulFractBits;
327        uint32_t ulFixOne = BCSC_P_ITOFIX(1, ulFractBits);
328
329        pRetMatrix->ulSize = 3;
330        pRetMatrix->ulFractBits = ulFractBits;
331        pRetMatrix->data[0][0] = pNPM->data[1][0];
332        pRetMatrix->data[0][1] = pNPM->data[1][1];
333        pRetMatrix->data[0][2] = pNPM->data[1][2];
334        pRetMatrix->data[1][0] = BCSC_P_FIX_DIV(-(pNPM->data[1][0]), 2*(ulFixOne - pNPM->data[1][2]), ulFractBits);
335        pRetMatrix->data[1][1] = pNPM->data[1][1];
336        pRetMatrix->data[1][2] = ulFixOne / 2;
337        pRetMatrix->data[2][0] = ulFixOne / 2;
338        pRetMatrix->data[2][1] = BCSC_P_FIX_DIV(-(pNPM->data[1][1]), 2*(ulFixOne - pNPM->data[1][0]), ulFractBits);
339        pRetMatrix->data[2][2] = BCSC_P_FIX_DIV(-(pNPM->data[1][2]), 2*(ulFixOne - pNPM->data[1][0]), ulFractBits);
340}
341
342/***************************************************************************
343 * {private}
344 *
345 * Takes two matrices and multiplies them together.  Result is a matrix
346 * with the same size and fixed point fractional bits.
347 */
348void BCSC_P_Matrix_Mult(BCSC_P_Matrix *pMatrix1, BCSC_P_Matrix *pMatrix2, BCSC_P_Matrix *pRetMatrix)
349{
350        BCSC_P_Matrix stMatrixTemp;
351        uint32_t ulSize = pMatrix1->ulSize;
352        uint32_t ulFractBits = pMatrix1->ulFractBits;
353        uint32_t i = 0;
354        uint32_t j = 0;
355        uint32_t k = 0;
356
357        BDBG_ASSERT(pMatrix1->ulSize == pMatrix2->ulSize);
358        BDBG_ASSERT(pMatrix1->ulFractBits == pMatrix2->ulFractBits);
359
360        stMatrixTemp.ulSize = ulSize;
361        stMatrixTemp.ulFractBits = ulFractBits;
362
363        for (i = 0; i < ulSize; i++)
364        {
365                for (j = 0; j < ulSize; j++)
366                {
367                        stMatrixTemp.data[i][j] = 0;
368                        for (k = 0; k < ulSize; k++)
369                        {
370                                stMatrixTemp.data[i][j] += (unsigned)BCSC_P_FIX_MUL((signed)pMatrix1->data[i][k],  (signed)pMatrix2->data[k][j], ulFractBits);
371                        }
372                }
373        }
374
375        BKNI_Memcpy(pRetMatrix, &stMatrixTemp, sizeof(stMatrixTemp));
376}
377
378/***************************************************************************
379 * {private}
380 *
381 * Multiplies a Matrix with a Vector.  Result is a vector with the same
382 * size as the original vector and same fixed point fractional bits.
383 */
384void BCSC_P_Matrix_MultVector(BCSC_P_Matrix *pMatrix, BCSC_P_Vector *pVector, BCSC_P_Vector *pRetVector)
385{
386        BCSC_P_Vector stTempVector;
387        uint32_t ulSize = pMatrix->ulSize;
388        uint32_t ulFractBits = pMatrix->ulFractBits;
389        uint32_t i = 0;
390        uint32_t k = 0;
391
392        BDBG_ASSERT(pMatrix->ulSize == pVector->ulSize);
393        BDBG_ASSERT(pMatrix->ulFractBits == pVector->ulFractBits);
394
395        stTempVector.ulSize = ulSize;
396        stTempVector.ulFractBits = ulFractBits;
397
398        for (i = 0; i < ulSize; i++)
399        {
400                stTempVector.data[i] = 0;
401                for (k = 0; k < ulSize; k++)
402                {
403                        stTempVector.data[i] += BCSC_P_FIX_MUL(pMatrix->data[i][k],  pVector->data[k], ulFractBits);
404                }
405        }
406
407        BKNI_Memcpy(pRetVector, &stTempVector, sizeof(stTempVector));
408}
409
410/***************************************************************************
411 * {private}
412 *
413 * Takes a 3x3 matrix and converts it to a 4x4 matrix.
414 */
415void BCSC_P_Matrix_Make4x4(BCSC_P_Matrix *pMatrix, BCSC_P_Matrix *pRetMatrix)
416{
417        uint32_t ulFractBits = pMatrix->ulFractBits;
418        uint32_t i = 0;
419        uint32_t j = 0;
420
421        BDBG_ASSERT(pMatrix->ulSize == 3);
422
423        BKNI_Memset(pRetMatrix, 0, sizeof(BCSC_P_Matrix));
424        pRetMatrix->ulSize = 4;
425        pRetMatrix->ulFractBits = ulFractBits;
426        pRetMatrix->data[3][3] = BCSC_P_ITOFIX(1, ulFractBits);
427
428        for (i = 0; i < 3; i++)
429        {
430                for (j = 0; j < 3; j++)
431                {
432                        pRetMatrix->data[i][j] = pMatrix->data[i][j];
433                }
434        }
435}
436
437/***************************************************************************
438 * {private}
439 *
440 * Produces a 4x4 GScale matrix.
441 */
442void BCSC_P_Matrix_GScale(uint32_t ulN, BCSC_P_Matrix *pRetMatrix, uint32_t ulFractBits)
443{
444        uint32_t ulFixOne = BCSC_P_ITOFIX(1, ulFractBits);
445
446        BKNI_Memset(pRetMatrix, 0, sizeof(BCSC_P_Matrix));
447        pRetMatrix->ulSize = 4;
448        pRetMatrix->ulFractBits = ulFractBits;
449        pRetMatrix->data[0][0] = ulFixOne / (219 * (1 << (ulN - 8)));
450        pRetMatrix->data[1][1] = ulFixOne / (224 * (1 << (ulN - 8)));
451        pRetMatrix->data[2][2] = ulFixOne / (224 * (1 << (ulN - 8)));
452        pRetMatrix->data[0][3] = (signed) BCSC_P_ITOFIX(-16, ulFractBits) / 219;
453        pRetMatrix->data[1][3] = (signed) BCSC_P_ITOFIX(-128, ulFractBits) / 224;
454        pRetMatrix->data[2][3] = (signed) BCSC_P_ITOFIX(-128, ulFractBits) / 224;
455        pRetMatrix->data[3][3] = ulFixOne;
456}
457
458/***************************************************************************
459 * {private}
460 *
461 * Produces a 4x4 inverted GScale matrix.  We calculate the GScale
462 * inverse matrix directly because it's easy to calculate and the
463 * result is more accurate than using the inverse matrix algorithm.
464 * This also avoids the problem of the determinant being too small
465 * for the fixed-point representation.
466 */
467void BCSC_P_Matrix_GScaleInverse(uint32_t ulN, BCSC_P_Matrix *pRetMatrix, uint32_t ulFractBits)
468{
469        uint32_t ulFixOne = BCSC_P_ITOFIX(1, ulFractBits);
470
471        BKNI_Memset(pRetMatrix, 0, sizeof(BCSC_P_Matrix));
472        pRetMatrix->ulSize = 4;
473        pRetMatrix->ulFractBits = ulFractBits;
474        pRetMatrix->data[0][0] = BCSC_P_ITOFIX(219 * (1 << (ulN - 8)), ulFractBits);
475        pRetMatrix->data[1][1] = BCSC_P_ITOFIX(224 * (1 << (ulN - 8)), ulFractBits);
476        pRetMatrix->data[2][2] = BCSC_P_ITOFIX(224 * (1 << (ulN - 8)), ulFractBits);
477        pRetMatrix->data[0][3] = BCSC_P_ITOFIX((1 << (ulN - 4)), ulFractBits);
478        pRetMatrix->data[1][3] = BCSC_P_ITOFIX((1 << (ulN - 1)), ulFractBits);
479        pRetMatrix->data[2][3] = BCSC_P_ITOFIX((1 << (ulN - 1)), ulFractBits);
480        pRetMatrix->data[3][3] = ulFixOne;
481}
482
483/***************************************************************************
484 * {private}
485 *
486 * Produces a 4x4 GGain matrix.
487 */
488void BCSC_P_Matrix_GGain(uint32_t ulN, BCSC_P_Matrix *pRetMatrix, uint32_t ulFractBits)
489{
490        uint32_t ulFixOne = BCSC_P_ITOFIX(1, ulFractBits);
491
492        BKNI_Memset(pRetMatrix, 0, sizeof(BCSC_P_Matrix));
493        pRetMatrix->ulSize = 4;
494        pRetMatrix->ulFractBits = ulFractBits;
495        pRetMatrix->data[0][0] = BCSC_P_ITOFIX(1 >> ulN, ulFractBits) - ulFixOne;
496        pRetMatrix->data[1][1] = BCSC_P_ITOFIX(1 >> ulN, ulFractBits) - ulFixOne;
497        pRetMatrix->data[2][2] = BCSC_P_ITOFIX(1 >> ulN, ulFractBits) - ulFixOne;
498        pRetMatrix->data[3][3] = ulFixOne;
499}
500
501/***************************************************************************
502 * {private}
503 *
504 * Calculates the cofactor of a matrix at a specific position.
505 */
506uint32_t BCSC_P_Matrix_Cofactor(BCSC_P_Matrix *pMatrix, uint32_t ulRow, uint32_t ulCol)
507{
508        uint32_t ulSignPos = (ulCol + ulRow) & 1;
509        uint32_t ulSize = pMatrix->ulSize;
510        uint32_t ulFractBits = pMatrix->ulFractBits;
511        uint32_t i = 0;
512        uint32_t j = 0;
513        uint32_t ulSrcRow = 0;
514        uint32_t ulSrcCol = 0;
515        uint32_t ulCofactor = 0;
516
517        BCSC_P_Matrix stTempMatrix;
518
519        stTempMatrix.ulSize = ulSize - 1;
520        stTempMatrix.ulFractBits = ulFractBits;
521
522        /* build 3x3 matrix for calculating determinant */
523        for (i = 0, ulSrcRow = 0; i < ulSize; i++, ulSrcRow++)
524        {
525                if (i == ulRow)
526                {
527                        ulSrcRow++;
528                }
529                for (j = 0, ulSrcCol = 0; j < ulSize; j++, ulSrcCol++)
530                {
531                        if (j == ulCol)
532                        {
533                                ulSrcCol++;
534                        }
535                        stTempMatrix.data[i][j] = pMatrix->data[ulSrcRow][ulSrcCol];
536                }
537        }
538
539        /* get determinant */
540        ulCofactor = BCSC_P_Matrix_Determinant(&stTempMatrix, NULL);
541
542        /* apply sign based on position */
543        ulCofactor *= (ulSignPos) ? -1 : 1;
544
545        return ulCofactor;
546}
547
548/***************************************************************************
549 * {private}
550 *
551 * Calculates the determinant of a matrix.  If cofactor matrix is present,
552 * precaculated cofactors from the table are used.
553 */
554uint32_t BCSC_P_Matrix_Determinant(BCSC_P_Matrix *pMatrix, BCSC_P_Matrix *pCofactors)
555{
556        uint32_t ulCol;
557        uint32_t ulDeterminant = 0;
558        uint32_t ulSize = pMatrix->ulSize;
559        uint32_t ulFractBits = pMatrix->ulFractBits;
560
561        if (ulSize == 1)
562        {
563                ulDeterminant = pMatrix->data[0][0];
564        }
565        else if (ulSize == 2)
566        {
567                ulDeterminant = BCSC_P_FIX_MUL(pMatrix->data[0][0], pMatrix->data[1][1], ulFractBits) -
568                                    BCSC_P_FIX_MUL(pMatrix->data[0][1], pMatrix->data[1][0], ulFractBits);
569        }
570        else
571        {
572                /* add cofactors along top row */
573                for (ulCol = 0; ulCol < ulSize; ulCol++)
574                {
575                        if (pCofactors)
576                        {
577                                /* use precomputed cofactors */
578                                ulDeterminant += BCSC_P_FIX_MUL(pMatrix->data[0][ulCol], pCofactors->data[0][ulCol], ulFractBits);
579                        }
580                        else
581                        {
582                                ulDeterminant += BCSC_P_FIX_MUL(pMatrix->data[0][ulCol], BCSC_P_Matrix_Cofactor(pMatrix, 0, ulCol), ulFractBits);
583                        }
584                }
585        }
586
587        return ulDeterminant;
588}
589
590/***************************************************************************
591 * {private}
592 *
593 * Creates a matrix holding all cofactors of a matrix.
594 */
595void BCSC_P_Matrix_CofactorMatrix(BCSC_P_Matrix *pMatrix, BCSC_P_Matrix *pCofactors)
596{
597        uint32_t ulRow;
598        uint32_t ulCol;
599        uint32_t ulSize = pMatrix->ulSize;
600        uint32_t ulFractBits = pMatrix->ulFractBits;
601
602        for (ulRow = 0; ulRow < ulSize; ulRow++)
603        {
604                for (ulCol = 0; ulCol < ulSize; ulCol++)
605                {
606                        pCofactors->data[ulRow][ulCol] = BCSC_P_Matrix_Cofactor(pMatrix, ulRow, ulCol);
607                }
608        }
609
610        pCofactors->ulSize = ulSize;
611        pCofactors->ulFractBits = ulFractBits;
612}
613
614/***************************************************************************
615 * {private}
616 *
617 * Transposes a matrix.
618 */
619void BCSC_P_Matrix_Transpose(BCSC_P_Matrix *pMatrix, BCSC_P_Matrix *pRetMatrix)
620{
621        uint32_t ulRow;
622        uint32_t ulCol;
623        uint32_t ulSize = pMatrix->ulSize;
624        uint32_t ulFractBits = pMatrix->ulFractBits;
625
626        for (ulRow = 0; ulRow < ulSize; ulRow++)
627        {
628                for (ulCol = 0; ulCol < ulSize; ulCol++)
629                {
630                        pRetMatrix->data[ulRow][ulCol] = pMatrix->data[ulCol][ulRow];
631                }
632        }
633
634        pRetMatrix->ulSize = ulSize;
635        pRetMatrix->ulFractBits = ulFractBits;
636}
637
638/***************************************************************************
639 * {private}
640 *
641 * Multiplies a matrix by a scalar.
642 */
643void BCSC_P_Matrix_MultScalar(BCSC_P_Matrix *pMatrix, uint32_t ulScalar, BCSC_P_Matrix *pRetMatrix)
644{
645        uint32_t ulRow;
646        uint32_t ulCol;
647        uint32_t ulSize = pMatrix->ulSize;
648        uint32_t ulFractBits = pMatrix->ulFractBits;
649
650        for (ulRow = 0; ulRow < ulSize; ulRow++)
651        {
652                for (ulCol = 0; ulCol < ulSize; ulCol++)
653                {
654                        pRetMatrix->data[ulRow][ulCol] = BCSC_P_FIX_MUL(pMatrix->data[ulRow][ulCol], ulScalar, ulFractBits);
655                }
656        }
657
658        pRetMatrix->ulSize = ulSize;
659        pRetMatrix->ulFractBits = ulFractBits;
660}
661
662/***************************************************************************
663 * {private}
664 *
665 * Calculates the inverse matrix.
666 */
667void BCSC_P_Matrix_Inverse(BCSC_P_Matrix *pMatrix, BCSC_P_Matrix *pRetMatrix)
668{
669        uint32_t ulFractBits = pMatrix->ulFractBits;
670        BCSC_P_Matrix stCofactorMatrix;
671        BCSC_P_Matrix stAdjointMatrix;
672        uint32_t ulDeterminant;
673        uint32_t ulOneOverDeterminant;
674        uint32_t ulFixOne = BCSC_P_ITOFIX(1, ulFractBits);
675
676        /* InvM = 1/det(M) * Transpose(Cofactor(M)) */
677
678        /* compute all cofactors */
679        BCSC_P_Matrix_CofactorMatrix(pMatrix, &stCofactorMatrix);
680        ulDeterminant = BCSC_P_Matrix_Determinant(pMatrix, &stCofactorMatrix);
681        BDBG_ASSERT(ulDeterminant);
682        ulOneOverDeterminant = BCSC_P_FIX_DIV(ulFixOne, ulDeterminant, ulFractBits);
683        BCSC_P_Matrix_Transpose(&stCofactorMatrix, &stAdjointMatrix);
684        BCSC_P_Matrix_MultScalar(&stAdjointMatrix, ulOneOverDeterminant, pRetMatrix);
685}
686
687void BCSC_P_Matrix_Dump(BCSC_P_Matrix *pMatrix)
688{
689        uint32_t i = 0;
690        uint32_t j = 0;
691
692        for (i = 0; i < pMatrix->ulSize; i++)
693        {
694                for (j = 0; j < pMatrix->ulSize; j++)
695                {
696                        BKNI_Printf("0x%x ", pMatrix->data[i][j]);
697                }
698                BKNI_Printf("\n");
699        }
700        BKNI_Printf("\n");
701}
702
703/* End of File */
Note: See TracBrowser for help on using the repository browser.