Subversion Repositories dashGPS

Rev

Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | Download | RSS feed

  1. #include "ref.h"
  2.  
  3. float32_t ref_detrm(float32_t *pSrc, float32_t *temp, uint32_t size)
  4. {
  5.    float32_t s = 1, det = 0;
  6.    int i, j, m, n, c;
  7.  
  8.    if ( size == 1 )
  9.    {
  10.       return ( pSrc[ 0 ] );
  11.    }
  12.    else
  13.    {
  14.       det = 0;
  15.  
  16.       for ( c = 0;c < size;c++ )
  17.       {
  18.          m = 0;
  19.          n = 0;
  20.  
  21.          for ( i = 0;i < size;i++ )
  22.          {
  23.             for ( j = 0;j < size;j++ )
  24.             {
  25.                temp[ i*size + j ] = 0;
  26.  
  27.                if ( i != 0 && j != c )
  28.                {
  29.                   temp[ m*(size-1) + n ] = pSrc[ i*size + j ];
  30.  
  31.                   if ( n < ( size - 2 ) )
  32.                   {
  33.                      n++;
  34.                   }
  35.                   else
  36.                   {
  37.                      n = 0;
  38.                      m++;
  39.                   }
  40.                }
  41.             }
  42.          }
  43.  
  44.          det += s * ( pSrc[ c ] * ref_detrm( temp, temp + size*size, size - 1 ) );
  45.          s = -s;
  46.       }
  47.    }
  48.    
  49.    return ( det );
  50. }
  51.  
  52.  
  53. void ref_cofact(float32_t *pSrc, float32_t *pDst, float32_t *temp, uint32_t size)
  54. {
  55.    int p, q, m, n, i, j;
  56.        
  57.          if (size == 1)
  58.          {
  59.                  pDst[0] = 1;
  60.                  return;
  61.          }
  62.  
  63.    for ( q = 0;q < size;q++ )
  64.    {
  65.       for ( p = 0;p < size;p++ )
  66.       {
  67.          m = 0;
  68.          n = 0;
  69.  
  70.          for ( i = 0;i < size;i++ )
  71.          {
  72.             for ( j = 0;j < size;j++ )
  73.             {
  74.                temp[ i*size + j ] = 0;
  75.  
  76.                if ( i != q && j != p )
  77.                {
  78.                   temp[ m*(size-1) + n ] = pSrc[ i*size + j ];
  79.  
  80.                   if ( n < ( size - 2 ) )
  81.                                                                         {
  82.                                                                                 n++;
  83.                                                                         }
  84.                   else
  85.                   {
  86.                      n = 0;
  87.                      m++;
  88.                   }
  89.                }
  90.             }
  91.          }
  92.  
  93.          pDst[ q*size + p ] = ref_pow( -1, q + p ) * ref_detrm( temp, temp + (size-1)*(size-1), size - 1 );
  94.       }
  95.    }
  96. }
  97.  
  98.  
  99.  
  100. float64_t ref_detrm64(float64_t *pSrc, float64_t *temp, uint32_t size)
  101. {
  102.    float64_t s = 1, det = 0;
  103.    int i, j, m, n, c;
  104.  
  105.    if ( size == 1 )
  106.    {
  107.       return ( pSrc[ 0 ] );
  108.    }
  109.    else
  110.    {
  111.       det = 0;
  112.  
  113.       for ( c = 0;c < size;c++ )
  114.       {
  115.          m = 0;
  116.          n = 0;
  117.  
  118.          for ( i = 0;i < size;i++ )
  119.          {
  120.             for ( j = 0;j < size;j++ )
  121.             {
  122.                temp[ i*size + j ] = 0;
  123.  
  124.                if ( i != 0 && j != c )
  125.                {
  126.                   temp[ m*(size-1) + n ] = pSrc[ i*size + j ];
  127.  
  128.                   if ( n < ( size - 2 ) )
  129.                   {
  130.                      n++;
  131.                   }
  132.                   else
  133.                   {
  134.                      n = 0;
  135.                      m++;
  136.                   }
  137.                }
  138.             }
  139.          }
  140.  
  141.          det += s * ( pSrc[ c ] * ref_detrm64( temp, temp + size*size, size - 1 ) );
  142.          s = -s;
  143.       }
  144.    }
  145.    
  146.    return ( det );
  147. }
  148.  
  149.  
  150. void ref_cofact64(float64_t *pSrc, float64_t *pDst, float64_t *temp, uint32_t size)
  151. {
  152.    int p, q, m, n, i, j;
  153.        
  154.          if (size == 1)
  155.          {
  156.                  pDst[0] = 1;
  157.                  return;
  158.          }
  159.  
  160.    for ( q = 0;q < size;q++ )
  161.    {
  162.       for ( p = 0;p < size;p++ )
  163.       {
  164.          m = 0;
  165.          n = 0;
  166.  
  167.          for ( i = 0;i < size;i++ )
  168.          {
  169.             for ( j = 0;j < size;j++ )
  170.             {
  171.                temp[ i*size + j ] = 0;
  172.  
  173.                if ( i != q && j != p )
  174.                {
  175.                   temp[ m*(size-1) + n ] = pSrc[ i*size + j ];
  176.  
  177.                   if ( n < ( size - 2 ) )
  178.                                                                         {
  179.                                                                                 n++;
  180.                                                                         }
  181.                   else
  182.                   {
  183.                      n = 0;
  184.                      m++;
  185.                   }
  186.                }
  187.             }
  188.          }
  189.  
  190.          pDst[ q*size + p ] = ref_pow( -1, q + p ) * ref_detrm64( temp, temp + (size-1)*(size-1), size - 1 );
  191.       }
  192.    }
  193. }
  194.