modules/base/math/fixed_point/f64_sqrt.c

   1 /*
   2  *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
   3  *
   4  *  This program is free software; you can redistribute it and/or modify
   5  *  it under the terms of the GNU General Public License as published by
   6  *  the Free Software Foundation; either version 2 of the License, or
   7  *  (at your option) any later version.
   8  *
   9  *  This program is distributed in the hope that it will be useful,
  10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  *  GNU General Public License for more details.
  13  *
  14  *  You should have received a copy of the GNU General Public License
  15  *  along with this program; if not, write to the Free Software
  16  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  17  *
  18  *  Revision : $Id: f64_sqrt.c,v 1.5.4.4 2009-02-02 22:21:20 zer0 Exp $
  19  *
  20  */
  21
  22 #include <stdio.h>
  23 #include <f64.h>
  24 #include <s64_to_f64.h>
  25
  26 #define NEXT(n, i)  (((n) + (i)/(n)) >> 1)
  27
  28 static uint64_t u64_sqrt(uint64_t number) {
  29         uint64_t n  = 1;
  30         uint64_t n1 = NEXT(n, number);
  31
  32         if (number == 0)
  33                 return 0;
  34
  35         while(ABS(n1 - n) > 1) {
  36                 n  = n1;
  37                 n1 = NEXT(n, number);
  38         }
  39         while((n1*n1) > number) {
  40                 n1 -= 1;
  41         }
  42         return n1;
  43 }
  44
  45 f64 f64_sqrt(f64 f)
  46 {
  47         uint64_t a,b,c,d;
  48
  49         if (F64_IS_ZERO(f))
  50                 return F64_ZERO;
  51
  52         if (F64_IS_NEG(f))
  53                 return F64_NAN;
  54
  55         if(f.f64_integer) {
  56                 /* sqrt(a+b) = sqrt(a)*sqrt(1+b/a) */
  57                 a=(uint64_t)(f.f64_integer) << 32 ;
  58                 b=(uint64_t)(f.f64_decimal) << 32 ;
  59                 c=u64_sqrt(a);
  60                 d=u64_sqrt(0x100000000 + (b/a));
  61                 return f64_mul(s64_to_f64( c<<16 ), s64_to_f64( d<<16 ));
  62         }
  63         else {
  64                 b=(uint64_t)(f.f64_decimal) << 32 ;
  65                 return s64_to_f64(u64_sqrt(b));
  66         }
  67 }