1/* $NetBSD: fpu_trig.c,v 1.18 2017/01/16 12:05:40 isaki Exp $ */ 2 3/* 4 * Copyright (c) 1995 Ken Nakata 5 * All rights reserved. 6 * 7 * Redistribution and use in source and binary forms, with or without 8 * modification, are permitted provided that the following conditions 9 * are met: 10 * 1. Redistributions of source code must retain the above copyright 11 * notice, this list of conditions and the following disclaimer. 12 * 2. Redistributions in binary form must reproduce the above copyright 13 * notice, this list of conditions and the following disclaimer in the 14 * documentation and/or other materials provided with the distribution. 15 * 3. Neither the name of the author nor the names of its contributors 16 * may be used to endorse or promote products derived from this software 17 * without specific prior written permission. 18 * 19 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 20 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 23 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 25 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 26 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 27 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 28 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 29 * SUCH DAMAGE. 30 * 31 * @(#)fpu_trig.c 10/24/95 32 */ 33 34/* 35 * Copyright (c) 2011 Tetsuya Isaki. All rights reserved. 36 * 37 * Redistribution and use in source and binary forms, with or without 38 * modification, are permitted provided that the following conditions 39 * are met: 40 * 1. Redistributions of source code must retain the above copyright 41 * notice, this list of conditions and the following disclaimer. 42 * 2. Redistributions in binary form must reproduce the above copyright 43 * notice, this list of conditions and the following disclaimer in the 44 * documentation and/or other materials provided with the distribution. 45 * 46 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 47 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 48 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 49 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 50 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 51 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 52 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 53 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 54 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 55 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 56 * SUCH DAMAGE. 57 */ 58 59#include <sys/cdefs.h> 60__KERNEL_RCSID(0, "$NetBSD: fpu_trig.c,v 1.18 2017/01/16 12:05:40 isaki Exp $"); 61 62#include "fpu_emulate.h" 63 64/* 65 * arccos(x) = pi/2 - arcsin(x) 66 */ 67struct fpn * 68fpu_acos(struct fpemu *fe) 69{ 70 struct fpn *r; 71 72 if (ISNAN(&fe->fe_f2)) 73 return &fe->fe_f2; 74 if (ISINF(&fe->fe_f2)) 75 return fpu_newnan(fe); 76 77 r = fpu_asin(fe); 78 CPYFPN(&fe->fe_f2, r); 79 80 /* pi/2 - asin(x) */ 81 fpu_const(&fe->fe_f1, FPU_CONST_PI); 82 fe->fe_f1.fp_exp--; 83 fe->fe_f2.fp_sign = !fe->fe_f2.fp_sign; 84 r = fpu_add(fe); 85 86 return r; 87} 88 89/* 90 * x 91 * arcsin(x) = arctan(---------------) 92 * sqrt(1 - x^2) 93 */ 94struct fpn * 95fpu_asin(struct fpemu *fe) 96{ 97 struct fpn x; 98 struct fpn *r; 99 100 if (ISNAN(&fe->fe_f2)) 101 return &fe->fe_f2; 102 if (ISZERO(&fe->fe_f2)) 103 return &fe->fe_f2; 104 105 if (ISINF(&fe->fe_f2)) 106 return fpu_newnan(fe); 107 108 CPYFPN(&x, &fe->fe_f2); 109 110 /* x^2 */ 111 CPYFPN(&fe->fe_f1, &fe->fe_f2); 112 r = fpu_mul(fe); 113 114 /* 1 - x^2 */ 115 CPYFPN(&fe->fe_f2, r); 116 fe->fe_f2.fp_sign = 1; 117 fpu_const(&fe->fe_f1, FPU_CONST_1); 118 r = fpu_add(fe); 119 120 /* sqrt(1-x^2) */ 121 CPYFPN(&fe->fe_f2, r); 122 r = fpu_sqrt(fe); 123 124 /* x/sqrt */ 125 CPYFPN(&fe->fe_f2, r); 126 CPYFPN(&fe->fe_f1, &x); 127 r = fpu_div(fe); 128 129 /* arctan */ 130 CPYFPN(&fe->fe_f2, r); 131 return fpu_atan(fe); 132} 133 134/* 135 * arctan(x): 136 * 137 * if (x < 0) { 138 * x = abs(x); 139 * sign = 1; 140 * } 141 * y = arctan(x); 142 * if (sign) { 143 * y = -y; 144 * } 145 */ 146struct fpn * 147fpu_atan(struct fpemu *fe) 148{ 149 struct fpn a; 150 struct fpn x; 151 struct fpn v; 152 153 if (ISNAN(&fe->fe_f2)) 154 return &fe->fe_f2; 155 if (ISZERO(&fe->fe_f2)) 156 return &fe->fe_f2; 157 158 CPYFPN(&a, &fe->fe_f2); 159 160 if (ISINF(&fe->fe_f2)) { 161 /* f2 <- pi/2 */ 162 fpu_const(&fe->fe_f2, FPU_CONST_PI); 163 fe->fe_f2.fp_exp--; 164 165 fe->fe_f2.fp_sign = a.fp_sign; 166 return &fe->fe_f2; 167 } 168 169 fpu_const(&x, FPU_CONST_1); 170 fpu_const(&fe->fe_f2, FPU_CONST_0); 171 CPYFPN(&v, &fe->fe_f2); 172 fpu_cordit1(fe, &x, &a, &fe->fe_f2, &v); 173 174 return &fe->fe_f2; 175} 176 177 178/* 179 * fe_f1 := sin(in) 180 * fe_f2 := cos(in) 181 */ 182static void 183__fpu_sincos_cordic(struct fpemu *fe, const struct fpn *in) 184{ 185 struct fpn a; 186 struct fpn v; 187 188 CPYFPN(&a, in); 189 fpu_const(&fe->fe_f1, FPU_CONST_0); 190 CPYFPN(&fe->fe_f2, &fpu_cordic_inv_gain1); 191 fpu_const(&v, FPU_CONST_1); 192 v.fp_sign = 1; 193 fpu_cordit1(fe, &fe->fe_f2, &fe->fe_f1, &a, &v); 194} 195 196/* 197 * cos(x): 198 * 199 * if (x < 0) { 200 * x = abs(x); 201 * } 202 * if (x > 2*pi) { 203 * x %= 2*pi; 204 * } 205 * if (x > pi) { 206 * x -= pi; 207 * sign inverse; 208 * } 209 * if (x > pi/2) { 210 * y = sin(x - pi/2); 211 * sign inverse; 212 * } else { 213 * y = cos(x); 214 * } 215 * if (sign) { 216 * y = -y; 217 * } 218 */ 219struct fpn * 220fpu_cos(struct fpemu *fe) 221{ 222 struct fpn x; 223 struct fpn p; 224 struct fpn *r; 225 int sign; 226 227 if (ISNAN(&fe->fe_f2)) 228 return &fe->fe_f2; 229 if (ISINF(&fe->fe_f2)) 230 return fpu_newnan(fe); 231 232 /* x = abs(input) */ 233 sign = 0; 234 CPYFPN(&x, &fe->fe_f2); 235 x.fp_sign = 0; 236 237 /* p <- 2*pi */ 238 fpu_const(&p, FPU_CONST_PI); 239 p.fp_exp++; 240 241 /* 242 * if (x > 2*pi*N) 243 * cos(x) is cos(x - 2*pi*N) 244 */ 245 CPYFPN(&fe->fe_f1, &x); 246 CPYFPN(&fe->fe_f2, &p); 247 r = fpu_cmp(fe); 248 if (r->fp_sign == 0) { 249 CPYFPN(&fe->fe_f1, &x); 250 CPYFPN(&fe->fe_f2, &p); 251 r = fpu_mod(fe); 252 CPYFPN(&x, r); 253 } 254 255 /* p <- pi */ 256 p.fp_exp--; 257 258 /* 259 * if (x > pi) 260 * cos(x) is -cos(x - pi) 261 */ 262 CPYFPN(&fe->fe_f1, &x); 263 CPYFPN(&fe->fe_f2, &p); 264 fe->fe_f2.fp_sign = 1; 265 r = fpu_add(fe); 266 if (r->fp_sign == 0) { 267 CPYFPN(&x, r); 268 sign ^= 1; 269 } 270 271 /* p <- pi/2 */ 272 p.fp_exp--; 273 274 /* 275 * if (x > pi/2) 276 * cos(x) is -sin(x - pi/2) 277 * else 278 * cos(x) 279 */ 280 CPYFPN(&fe->fe_f1, &x); 281 CPYFPN(&fe->fe_f2, &p); 282 fe->fe_f2.fp_sign = 1; 283 r = fpu_add(fe); 284 if (r->fp_sign == 0) { 285 __fpu_sincos_cordic(fe, r); 286 r = &fe->fe_f1; 287 sign ^= 1; 288 } else { 289 __fpu_sincos_cordic(fe, &x); 290 r = &fe->fe_f2; 291 } 292 r->fp_sign = sign; 293 return r; 294} 295 296/* 297 * sin(x): 298 * 299 * if (x < 0) { 300 * x = abs(x); 301 * sign = 1; 302 * } 303 * if (x > 2*pi) { 304 * x %= 2*pi; 305 * } 306 * if (x > pi) { 307 * x -= pi; 308 * sign inverse; 309 * } 310 * if (x > pi/2) { 311 * y = cos(x - pi/2); 312 * } else { 313 * y = sin(x); 314 * } 315 * if (sign) { 316 * y = -y; 317 * } 318 */ 319struct fpn * 320fpu_sin(struct fpemu *fe) 321{ 322 struct fpn x; 323 struct fpn p; 324 struct fpn *r; 325 int sign; 326 327 if (ISNAN(&fe->fe_f2)) 328 return &fe->fe_f2; 329 if (ISINF(&fe->fe_f2)) 330 return fpu_newnan(fe); 331 332 /* if x is +0/-0, return +0/-0 */ 333 if (ISZERO(&fe->fe_f2)) 334 return &fe->fe_f2; 335 336 /* x = abs(input) */ 337 sign = fe->fe_f2.fp_sign; 338 CPYFPN(&x, &fe->fe_f2); 339 x.fp_sign = 0; 340 341 /* p <- 2*pi */ 342 fpu_const(&p, FPU_CONST_PI); 343 p.fp_exp++; 344 345 /* 346 * if (x > 2*pi*N) 347 * sin(x) is sin(x - 2*pi*N) 348 */ 349 CPYFPN(&fe->fe_f1, &x); 350 CPYFPN(&fe->fe_f2, &p); 351 r = fpu_cmp(fe); 352 if (r->fp_sign == 0) { 353 CPYFPN(&fe->fe_f1, &x); 354 CPYFPN(&fe->fe_f2, &p); 355 r = fpu_mod(fe); 356 CPYFPN(&x, r); 357 } 358 359 /* p <- pi */ 360 p.fp_exp--; 361 362 /* 363 * if (x > pi) 364 * sin(x) is -sin(x - pi) 365 */ 366 CPYFPN(&fe->fe_f1, &x); 367 CPYFPN(&fe->fe_f2, &p); 368 fe->fe_f2.fp_sign = 1; 369 r = fpu_add(fe); 370 if (r->fp_sign == 0) { 371 CPYFPN(&x, r); 372 sign ^= 1; 373 } 374 375 /* p <- pi/2 */ 376 p.fp_exp--; 377 378 /* 379 * if (x > pi/2) 380 * sin(x) is cos(x - pi/2) 381 * else 382 * sin(x) 383 */ 384 CPYFPN(&fe->fe_f1, &x); 385 CPYFPN(&fe->fe_f2, &p); 386 fe->fe_f2.fp_sign = 1; 387 r = fpu_add(fe); 388 if (r->fp_sign == 0) { 389 __fpu_sincos_cordic(fe, r); 390 r = &fe->fe_f2; 391 } else { 392 __fpu_sincos_cordic(fe, &x); 393 r = &fe->fe_f1; 394 } 395 r->fp_sign = sign; 396 return r; 397} 398 399/* 400 * tan(x) = sin(x) / cos(x) 401 */ 402struct fpn * 403fpu_tan(struct fpemu *fe) 404{ 405 struct fpn x; 406 struct fpn s; 407 struct fpn *r; 408 409 if (ISNAN(&fe->fe_f2)) 410 return &fe->fe_f2; 411 if (ISINF(&fe->fe_f2)) 412 return fpu_newnan(fe); 413 414 /* if x is +0/-0, return +0/-0 */ 415 if (ISZERO(&fe->fe_f2)) 416 return &fe->fe_f2; 417 418 CPYFPN(&x, &fe->fe_f2); 419 420 /* sin(x) */ 421 CPYFPN(&fe->fe_f2, &x); 422 r = fpu_sin(fe); 423 CPYFPN(&s, r); 424 425 /* cos(x) */ 426 CPYFPN(&fe->fe_f2, &x); 427 r = fpu_cos(fe); 428 CPYFPN(&fe->fe_f2, r); 429 430 CPYFPN(&fe->fe_f1, &s); 431 r = fpu_div(fe); 432 return r; 433} 434 435struct fpn * 436fpu_sincos(struct fpemu *fe, int regc) 437{ 438 __fpu_sincos_cordic(fe, &fe->fe_f2); 439 440 /* cos(x) */ 441 fpu_implode(fe, &fe->fe_f2, FTYPE_EXT, &fe->fe_fpframe->fpf_regs[regc * 3]); 442 443 /* sin(x) */ 444 return &fe->fe_f1; 445} 446