1176720Sdas/*- 2176720Sdas * Copyright (c) 2007 Steven G. Kargl 3176720Sdas * All rights reserved. 4176720Sdas * 5176720Sdas * Redistribution and use in source and binary forms, with or without 6176720Sdas * modification, are permitted provided that the following conditions 7176720Sdas * are met: 8176720Sdas * 1. Redistributions of source code must retain the above copyright 9176720Sdas * notice unmodified, this list of conditions, and the following 10176720Sdas * disclaimer. 11176720Sdas * 2. Redistributions in binary form must reproduce the above copyright 12176720Sdas * notice, this list of conditions and the following disclaimer in the 13176720Sdas * documentation and/or other materials provided with the distribution. 14176720Sdas * 15176720Sdas * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 16176720Sdas * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 17176720Sdas * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 18176720Sdas * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 19176720Sdas * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 20176720Sdas * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 21176720Sdas * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 22176720Sdas * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23176720Sdas * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 24176720Sdas * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25176720Sdas */ 26176720Sdas 27176720Sdas#include <sys/cdefs.h> 28176720Sdas__FBSDID("$FreeBSD$"); 29176720Sdas 30176720Sdas#include <fenv.h> 31176720Sdas#include <float.h> 32176720Sdas 33176720Sdas#include "fpmath.h" 34176720Sdas#include "math.h" 35176720Sdas 36176720Sdas/* Return (x + ulp) for normal positive x. Assumes no overflow. */ 37176720Sdasstatic inline long double 38176720Sdasinc(long double x) 39176720Sdas{ 40176720Sdas union IEEEl2bits u; 41176720Sdas 42176720Sdas u.e = x; 43176720Sdas if (++u.bits.manl == 0) { 44176720Sdas if (++u.bits.manh == 0) { 45176720Sdas u.bits.exp++; 46176720Sdas u.bits.manh |= LDBL_NBIT; 47176720Sdas } 48176720Sdas } 49176720Sdas return (u.e); 50176720Sdas} 51176720Sdas 52176720Sdas/* Return (x - ulp) for normal positive x. Assumes no underflow. */ 53176720Sdasstatic inline long double 54176720Sdasdec(long double x) 55176720Sdas{ 56176720Sdas union IEEEl2bits u; 57176720Sdas 58176720Sdas u.e = x; 59176720Sdas if (u.bits.manl-- == 0) { 60176720Sdas if (u.bits.manh-- == LDBL_NBIT) { 61176720Sdas u.bits.exp--; 62176720Sdas u.bits.manh |= LDBL_NBIT; 63176720Sdas } 64176720Sdas } 65176720Sdas return (u.e); 66176720Sdas} 67176720Sdas 68176720Sdas#pragma STDC FENV_ACCESS ON 69176720Sdas 70176720Sdas/* 71176720Sdas * This is slow, but simple and portable. You should use hardware sqrt 72176720Sdas * if possible. 73176720Sdas */ 74176720Sdas 75176720Sdaslong double 76176720Sdassqrtl(long double x) 77176720Sdas{ 78176720Sdas union IEEEl2bits u; 79176720Sdas int k, r; 80176720Sdas long double lo, xn; 81176720Sdas fenv_t env; 82176720Sdas 83176720Sdas u.e = x; 84176720Sdas 85176720Sdas /* If x = NaN, then sqrt(x) = NaN. */ 86176720Sdas /* If x = Inf, then sqrt(x) = Inf. */ 87176720Sdas /* If x = -Inf, then sqrt(x) = NaN. */ 88176720Sdas if (u.bits.exp == LDBL_MAX_EXP * 2 - 1) 89176720Sdas return (x * x + x); 90176720Sdas 91176720Sdas /* If x = +-0, then sqrt(x) = +-0. */ 92176720Sdas if ((u.bits.manh | u.bits.manl | u.bits.exp) == 0) 93176720Sdas return (x); 94176720Sdas 95176720Sdas /* If x < 0, then raise invalid and return NaN */ 96176720Sdas if (u.bits.sign) 97176720Sdas return ((x - x) / (x - x)); 98176720Sdas 99176720Sdas feholdexcept(&env); 100176720Sdas 101176720Sdas if (u.bits.exp == 0) { 102176720Sdas /* Adjust subnormal numbers. */ 103176720Sdas u.e *= 0x1.0p514; 104176720Sdas k = -514; 105176720Sdas } else { 106176720Sdas k = 0; 107176720Sdas } 108176720Sdas /* 109176720Sdas * u.e is a normal number, so break it into u.e = e*2^n where 110176720Sdas * u.e = (2*e)*2^2k for odd n and u.e = (4*e)*2^2k for even n. 111176720Sdas */ 112176720Sdas if ((u.bits.exp - 0x3ffe) & 1) { /* n is odd. */ 113176720Sdas k += u.bits.exp - 0x3fff; /* 2k = n - 1. */ 114176720Sdas u.bits.exp = 0x3fff; /* u.e in [1,2). */ 115176720Sdas } else { 116176720Sdas k += u.bits.exp - 0x4000; /* 2k = n - 2. */ 117176720Sdas u.bits.exp = 0x4000; /* u.e in [2,4). */ 118176720Sdas } 119176720Sdas 120176720Sdas /* 121176720Sdas * Newton's iteration. 122176720Sdas * Split u.e into a high and low part to achieve additional precision. 123176720Sdas */ 124176720Sdas xn = sqrt(u.e); /* 53-bit estimate of sqrtl(x). */ 125176720Sdas#if LDBL_MANT_DIG > 100 126176720Sdas xn = (xn + (u.e / xn)) * 0.5; /* 106-bit estimate. */ 127176720Sdas#endif 128176720Sdas lo = u.e; 129176720Sdas u.bits.manl = 0; /* Zero out lower bits. */ 130176720Sdas lo = (lo - u.e) / xn; /* Low bits divided by xn. */ 131176720Sdas xn = xn + (u.e / xn); /* High portion of estimate. */ 132176720Sdas u.e = xn + lo; /* Combine everything. */ 133176720Sdas u.bits.exp += (k >> 1) - 1; 134176720Sdas 135176720Sdas feclearexcept(FE_INEXACT); 136176720Sdas r = fegetround(); 137176720Sdas fesetround(FE_TOWARDZERO); /* Set to round-toward-zero. */ 138176720Sdas xn = x / u.e; /* Chopped quotient (inexact?). */ 139176720Sdas 140176720Sdas if (!fetestexcept(FE_INEXACT)) { /* Quotient is exact. */ 141176720Sdas if (xn == u.e) { 142176720Sdas fesetenv(&env); 143176720Sdas return (u.e); 144176720Sdas } 145176720Sdas /* Round correctly for inputs like x = y**2 - ulp. */ 146176720Sdas xn = dec(xn); /* xn = xn - ulp. */ 147176720Sdas } 148176720Sdas 149176720Sdas if (r == FE_TONEAREST) { 150176720Sdas xn = inc(xn); /* xn = xn + ulp. */ 151176720Sdas } else if (r == FE_UPWARD) { 152176720Sdas u.e = inc(u.e); /* u.e = u.e + ulp. */ 153176720Sdas xn = inc(xn); /* xn = xn + ulp. */ 154176720Sdas } 155176720Sdas u.e = u.e + xn; /* Chopped sum. */ 156176720Sdas feupdateenv(&env); /* Restore env and raise inexact */ 157176720Sdas u.bits.exp--; 158176720Sdas return (u.e); 159176720Sdas} 160