ti/dsplib/src/DSPF_dp_lud_inv_cmplx/c66/DSPF_dp_lud_inv_cmplx_cn.c

   1 /* ======================================================================= */
   2 /* DSPF_dp_lud_inv_cmplx_cn.c - complex matrix inversion by LUD double precision*/
   3 /*                 Natural C Implementation                                */
   4 /*                                                                         */
   5 /* Rev 1.0.0                                                               */
   6 /*                                                                         */
   7 /* Copyright (C) 2013 Texas Instruments Incorporated - http://www.ti.com/  */
   8 /*                                                                         */
   9 /*                                                                         */
  10 /*  Redistribution and use in source and binary forms, with or without     */
  11 /*  modification, are permitted provided that the following conditions     */
  12 /*  are met:                                                               */
  13 /*                                                                         */
  14 /*    Redistributions of source code must retain the above copyright       */
  15 /*    notice, this list of conditions and the following disclaimer.        */
  16 /*                                                                         */
  17 /*    Redistributions in binary form must reproduce the above copyright    */
  18 /*    notice, this list of conditions and the following disclaimer in the  */
  19 /*    documentation and/or other materials provided with the               */
  20 /*    distribution.                                                        */
  21 /*                                                                         */
  22 /*    Neither the name of Texas Instruments Incorporated nor the names of  */
  23 /*    its contributors may be used to endorse or promote products derived  */
  24 /*    from this software without specific prior written permission.        */
  25 /*                                                                         */
  26 /*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS    */
  27 /*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT      */
  28 /*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR  */
  29 /*  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT   */
  30 /*  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,  */
  31 /*  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT       */
  32 /*  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,  */
  33 /*  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY  */
  34 /*  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT    */
  35 /*  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE  */
  36 /*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.   */
  37 /*                                                                         */
  38 /* ======================================================================= */\
  39 #include <math.h>
  40 #include <string.h>
  41
  42 #include "DSPF_dp_lud_inv_cmplx_cn.h"
  43
  44 static void complex_dp_div_cn(double x_real,double x_imag,double y_real,
  45                               double y_imag,double *z_real,double *z_imag);
  46
  47 int DSPF_dp_lud_inverse_cmplx_cn(const int Nrows,unsigned short *P,double *L,
  48                                  double *U,double *inv_A) {
  49
  50   int row,col,Ncols,k;
  51   double factor_real,factor_imag,sum_real,sum_imag;
  52   double xreal,ximag,yreal,yimag,zreal,zimag;
  53   double *inv_L,*inv_U,*inv_U_x_inv_L;
  54
  55   /* set inv_A matrix to identity */
  56   inv_L=&inv_A[0];
  57   Ncols=2*Nrows;
  58   memset(inv_L,0.0,sizeof(double)*Nrows*Ncols);
  59   for (row=0;row<Nrows;row++) {
  60         inv_L[2*row  +row*Ncols]=1;
  61   }
  62
  63   /* use Gauss Jordan algorithm to invert L whose result is in inv_L */
  64   for (col=0;col<Nrows-1;col++) {
  65     for (row=col+1;row<Nrows;row++) {
  66       xreal=L[2*col  +row*Ncols];
  67       ximag=L[2*col+1+row*Ncols];
  68       yreal=L[2*col  +col*Ncols];
  69       yimag=L[2*col+1+col*Ncols];
  70       complex_dp_div_cn(xreal,ximag,yreal,yimag,&zreal,&zimag);
  71       factor_real=zreal;
  72       factor_imag=zimag;
  73           for (k=0;k<Nrows;k++) {
  74                 xreal=factor_real;
  75                 ximag=factor_imag;
  76                 yreal=inv_L[2*k  +col*Ncols];
  77                 yimag=inv_L[2*k+1+col*Ncols];
  78                 inv_L[2*k  +row*Ncols]-=xreal*yreal-ximag*yimag;
  79                 inv_L[2*k+1+row*Ncols]-=xreal*yimag+ximag*yreal;
  80                 xreal=factor_real;
  81                 ximag=factor_imag;
  82                 yreal=L[2*k  +col*Ncols];
  83                 yimag=L[2*k+1+col*Ncols];
  84                 L[2*k  +row*Ncols]-=xreal*yreal-ximag*yimag;
  85                 L[2*k+1+row*Ncols]-=xreal*yimag+ximag*yreal;
  86           }
  87         }
  88   }
  89
  90   /* set inv_U matrix to identity */
  91   inv_U=&L[0];
  92   memset(inv_U,0.0,sizeof(double)*Nrows*Ncols);
  93   for (row=0;row<Nrows;row++) {
  94         inv_U[2*row  +row*Ncols]=1;
  95   }
  96
  97   /* use Gauss Jordan algorithm to invert U whose result is in L */
  98   for (col=Nrows-1;col>=1;col--) {
  99         for (row=col-1;row>=0;row--) {
 100       xreal=U[2*col  +row*Ncols];
 101       ximag=U[2*col+1+row*Ncols];
 102       yreal=U[2*col  +col*Ncols];
 103       yimag=U[2*col+1+col*Ncols];
 104       complex_dp_div_cn(xreal,ximag,yreal,yimag,&zreal,&zimag);
 105       factor_real=zreal;
 106       factor_imag=zimag;
 107           for (k=0;k<Nrows;k++) {
 108                 xreal=factor_real;
 109                 ximag=factor_imag;
 110                 yreal=inv_U[2*k  +col*Ncols];
 111                 yimag=inv_U[2*k+1+col*Ncols];
 112                 complex_dp_div_cn(xreal,ximag,yreal,yimag,&zreal,&zimag);
 113                 inv_U[2*k  +row*Ncols]-=xreal*yreal-ximag*yimag;
 114                 inv_U[2*k+1+row*Ncols]-=xreal*yimag+ximag*yreal;
 115                 xreal=factor_real;
 116                 ximag=factor_imag;
 117                 yreal=U[2*k  +col*Ncols];
 118                 yimag=U[2*k+1+col*Ncols];
 119                 U[2*k  +row*Ncols]-=xreal*yreal-ximag*yimag;
 120                 U[2*k+1+row*Ncols]-=xreal*yimag+ximag*yreal;
 121           }
 122         }
 123   }
 124
 125   /* scale U & L to get identity matrix in U */
 126   for (row=Nrows-1;row>=0;row--) {
 127         factor_real=U[2*row  +row*Ncols];
 128         factor_imag=U[2*row+1+row*Ncols];
 129         for (col=0;col<Nrows;col++) {
 130       xreal=L[2*col  +row*Ncols];
 131       ximag=L[2*col+1+row*Ncols];
 132       yreal=factor_real;
 133       yimag=factor_imag;
 134       complex_dp_div_cn(xreal,ximag,yreal,yimag,&zreal,&zimag);
 135       L[2*col  +row*Ncols]=zreal;
 136       L[2*col+1+row*Ncols]=zimag;
 137       xreal=U[2*col  +row*Nrows];
 138       ximag=U[2*col+1+row*Nrows];
 139       yreal=factor_real;
 140       yimag=factor_imag;
 141       complex_dp_div_cn(xreal,ximag,yreal,yimag,&zreal,&zimag);
 142       U[2*col  +row*Ncols]=zreal;
 143       U[2*col+1+row*Ncols]=zimag;
 144         }
 145   }
 146
 147   /* compute inv_U_x_inv_L=inv(U)*inv(L) */
 148   inv_U_x_inv_L=&L[0];
 149   for (row=0;row<Nrows;row++) {
 150     for (col=0;col<Nrows;col++) {
 151     sum_real=0;
 152     sum_imag=0;
 153         for (k=0;k<Nrows;k++) {
 154           xreal=inv_U[2*k  +row*Ncols];
 155           ximag=inv_U[2*k+1+row*Ncols];
 156           yreal=inv_L[2*col  +k*Ncols];
 157           yimag=inv_L[2*col+1+k*Ncols];
 158           sum_real+=xreal*yreal-ximag*yimag;
 159           sum_imag+=xreal*yimag+ximag*yreal;
 160         }
 161         inv_U_x_inv_L[2*col  +row*Ncols]=sum_real;
 162         inv_U_x_inv_L[2*col+1+row*Ncols]=sum_imag;
 163     }
 164   }
 165   /* compute inv_A=inv(U)*inv(L)*P */
 166   for (row=0;row<Nrows;row++) {
 167     for (col=0;col<Nrows;col++) {
 168     sum_real=0;
 169     sum_imag=0;
 170         for (k=0;k<Nrows;k++) {
 171           xreal=inv_U_x_inv_L[2*k  +row*Ncols];
 172           ximag=inv_U_x_inv_L[2*k+1+row*Ncols];
 173           yreal=P[col+k*Nrows];
 174           sum_real+=xreal*yreal;
 175           sum_imag+=ximag*yreal;
 176         }
 177     inv_A[2*col  +row*Ncols]=sum_real;
 178     inv_A[2*col+1+row*Ncols]=sum_imag;
 179     }
 180   }
 181
 182   return 0;
 183 }
 184
 185 void complex_dp_div_cn(double x_real,double x_imag,double y_real,double y_imag,double *z_real,double *z_imag) {
 186
 187   double x_mag,y_mag,z_mag;
 188   double x_angle,y_angle,z_angle;
 189
 190   /* magnitude */
 191   x_mag=sqrt(x_real*x_real+x_imag*x_imag);
 192   y_mag=sqrt(y_real*y_real+y_imag*y_imag);
 193   z_mag=x_mag/y_mag;
 194
 195   /* angle */
 196   x_angle=atan2(x_imag,x_real);
 197   y_angle=atan2(y_imag,y_real);
 198   z_angle=x_angle-y_angle;
 199
 200   /* results */
 201   *z_real=cos(z_angle)*z_mag;
 202   *z_imag=sin(z_angle)*z_mag;
 203 }
 204
 205
 206 /* ======================================================================= */
 207 /*  End of file:  DSPF_dp_lud_inv_cmplx_cn.c                               */
 208 /* ----------------------------------------------------------------------- */
 209 /*            Copyright (c) 2013 Texas Instruments, Incorporated.          */
 210 /*                           All Rights Reserved.                          */
 211 /* ======================================================================= */