1a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes/*- 2a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * Copyright (c) 2007 Steven G. Kargl 3a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * All rights reserved. 4a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * 5a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * Redistribution and use in source and binary forms, with or without 6a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * modification, are permitted provided that the following conditions 7a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * are met: 8a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * 1. Redistributions of source code must retain the above copyright 9a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * notice unmodified, this list of conditions, and the following 10a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * disclaimer. 11a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * 2. Redistributions in binary form must reproduce the above copyright 12a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * notice, this list of conditions and the following disclaimer in the 13a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * documentation and/or other materials provided with the distribution. 14a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * 15a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 16a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 17a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 18a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 19a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 20a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 21a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 22a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 24a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes */ 26a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes 27a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes#include <sys/cdefs.h> 28a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes__FBSDID("$FreeBSD$"); 29a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes 30a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes#include <fenv.h> 31a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes#include <float.h> 32a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes 33a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes#include "fpmath.h" 34a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes#include "math.h" 35a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes 36a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes/* Return (x + ulp) for normal positive x. Assumes no overflow. */ 37a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughesstatic inline long double 38a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughesinc(long double x) 39a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes{ 40a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes union IEEEl2bits u; 41a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes 42a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes u.e = x; 43a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes if (++u.bits.manl == 0) { 44a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes if (++u.bits.manh == 0) { 45a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes u.bits.exp++; 46a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes u.bits.manh |= LDBL_NBIT; 47a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes } 48a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes } 49a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes return (u.e); 50a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes} 51a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes 52a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes/* Return (x - ulp) for normal positive x. Assumes no underflow. */ 53a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughesstatic inline long double 54a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughesdec(long double x) 55a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes{ 56a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes union IEEEl2bits u; 57a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes 58a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes u.e = x; 59a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes if (u.bits.manl-- == 0) { 60a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes if (u.bits.manh-- == LDBL_NBIT) { 61a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes u.bits.exp--; 62a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes u.bits.manh |= LDBL_NBIT; 63a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes } 64a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes } 65a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes return (u.e); 66a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes} 67a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes 68a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes#pragma STDC FENV_ACCESS ON 69a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes 70a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes/* 71a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * This is slow, but simple and portable. You should use hardware sqrt 72a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * if possible. 73a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes */ 74a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes 75a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hugheslong double 76a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughessqrtl(long double x) 77a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes{ 78a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes union IEEEl2bits u; 79a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes int k, r; 80a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes long double lo, xn; 81a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes fenv_t env; 82a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes 83a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes u.e = x; 84a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes 85a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes /* If x = NaN, then sqrt(x) = NaN. */ 86a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes /* If x = Inf, then sqrt(x) = Inf. */ 87a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes /* If x = -Inf, then sqrt(x) = NaN. */ 88a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes if (u.bits.exp == LDBL_MAX_EXP * 2 - 1) 89a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes return (x * x + x); 90a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes 91a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes /* If x = +-0, then sqrt(x) = +-0. */ 92a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes if ((u.bits.manh | u.bits.manl | u.bits.exp) == 0) 93a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes return (x); 94a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes 95a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes /* If x < 0, then raise invalid and return NaN */ 96a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes if (u.bits.sign) 97a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes return ((x - x) / (x - x)); 98a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes 99a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes feholdexcept(&env); 100a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes 101a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes if (u.bits.exp == 0) { 102a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes /* Adjust subnormal numbers. */ 103a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes u.e *= 0x1.0p514; 104a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes k = -514; 105a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes } else { 106a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes k = 0; 107a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes } 108a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes /* 109a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * u.e is a normal number, so break it into u.e = e*2^n where 110a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * u.e = (2*e)*2^2k for odd n and u.e = (4*e)*2^2k for even n. 111a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes */ 112a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes if ((u.bits.exp - 0x3ffe) & 1) { /* n is odd. */ 113a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes k += u.bits.exp - 0x3fff; /* 2k = n - 1. */ 114a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes u.bits.exp = 0x3fff; /* u.e in [1,2). */ 115a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes } else { 116a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes k += u.bits.exp - 0x4000; /* 2k = n - 2. */ 117a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes u.bits.exp = 0x4000; /* u.e in [2,4). */ 118a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes } 119a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes 120a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes /* 121a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * Newton's iteration. 122a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes * Split u.e into a high and low part to achieve additional precision. 123a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes */ 124a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes xn = sqrt(u.e); /* 53-bit estimate of sqrtl(x). */ 125a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes#if LDBL_MANT_DIG > 100 126a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes xn = (xn + (u.e / xn)) * 0.5; /* 106-bit estimate. */ 127a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes#endif 128a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes lo = u.e; 129a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes u.bits.manl = 0; /* Zero out lower bits. */ 130a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes lo = (lo - u.e) / xn; /* Low bits divided by xn. */ 131a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes xn = xn + (u.e / xn); /* High portion of estimate. */ 132a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes u.e = xn + lo; /* Combine everything. */ 133a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes u.bits.exp += (k >> 1) - 1; 134a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes 135a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes feclearexcept(FE_INEXACT); 136a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes r = fegetround(); 137a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes fesetround(FE_TOWARDZERO); /* Set to round-toward-zero. */ 138a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes xn = x / u.e; /* Chopped quotient (inexact?). */ 139a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes 140a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes if (!fetestexcept(FE_INEXACT)) { /* Quotient is exact. */ 141a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes if (xn == u.e) { 142a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes fesetenv(&env); 143a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes return (u.e); 144a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes } 145a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes /* Round correctly for inputs like x = y**2 - ulp. */ 146a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes xn = dec(xn); /* xn = xn - ulp. */ 147a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes } 148a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes 149a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes if (r == FE_TONEAREST) { 150a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes xn = inc(xn); /* xn = xn + ulp. */ 151a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes } else if (r == FE_UPWARD) { 152a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes u.e = inc(u.e); /* u.e = u.e + ulp. */ 153a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes xn = inc(xn); /* xn = xn + ulp. */ 154a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes } 155a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes u.e = u.e + xn; /* Chopped sum. */ 156a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes feupdateenv(&env); /* Restore env and raise inexact */ 157a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes u.bits.exp--; 158a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes return (u.e); 159a0ee07829a9ba7e99ef68e8c12551301cc797f0fElliott Hughes} 160