1 /* $NetBSD: fpu_trig.c,v 1.15 2013/04/20 07:32:45 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.15 2013/04/20 07:32:45 isaki Exp $"); 61 62 #include "fpu_emulate.h" 63 64 /* 65 * arccos(x) = pi/2 - arcsin(x) 66 */ 67 struct fpn * 68 fpu_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 */ 94 struct fpn * 95 fpu_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 */ 146 struct fpn * 147 fpu_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 */ 182 static 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 */ 219 struct fpn * 220 fpu_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 CPYFPN(&x, &fe->fe_f2); 233 234 /* x = abs(input) */ 235 x.fp_sign = 0; 236 sign = 0; 237 238 /* p <- 2*pi */ 239 fpu_const(&p, FPU_CONST_PI); 240 p.fp_exp++; 241 242 /* 243 * if (x > 2*pi*N) 244 * cos(x) is cos(x - 2*pi*N) 245 */ 246 CPYFPN(&fe->fe_f1, &x); 247 CPYFPN(&fe->fe_f2, &p); 248 r = fpu_cmp(fe); 249 if (r->fp_sign == 0) { 250 CPYFPN(&fe->fe_f1, &x); 251 CPYFPN(&fe->fe_f2, &p); 252 r = fpu_mod(fe); 253 CPYFPN(&x, r); 254 } 255 256 /* p <- pi */ 257 p.fp_exp--; 258 259 /* 260 * if (x > pi) 261 * cos(x) is -cos(x - pi) 262 */ 263 CPYFPN(&fe->fe_f1, &x); 264 CPYFPN(&fe->fe_f2, &p); 265 fe->fe_f2.fp_sign = 1; 266 r = fpu_add(fe); 267 if (r->fp_sign == 0) { 268 CPYFPN(&x, r); 269 sign ^= 1; 270 } 271 272 /* p <- pi/2 */ 273 p.fp_exp--; 274 275 /* 276 * if (x > pi/2) 277 * cos(x) is -sin(x - pi/2) 278 * else 279 * cos(x) 280 */ 281 CPYFPN(&fe->fe_f1, &x); 282 CPYFPN(&fe->fe_f2, &p); 283 fe->fe_f2.fp_sign = 1; 284 r = fpu_add(fe); 285 if (r->fp_sign == 0) { 286 __fpu_sincos_cordic(fe, r); 287 r = &fe->fe_f1; 288 sign ^= 1; 289 } else { 290 __fpu_sincos_cordic(fe, &x); 291 r = &fe->fe_f2; 292 } 293 r->fp_sign = sign; 294 return r; 295 } 296 297 /* 298 * sin(x): 299 * 300 * if (x < 0) { 301 * x = abs(x); 302 * sign = 1; 303 * } 304 * if (x > 2*pi) { 305 * x %= 2*pi; 306 * } 307 * if (x > pi) { 308 * x -= pi; 309 * sign inverse; 310 * } 311 * if (x > pi/2) { 312 * y = cos(x - pi/2); 313 * } else { 314 * y = sin(x); 315 * } 316 * if (sign) { 317 * y = -y; 318 * } 319 */ 320 struct fpn * 321 fpu_sin(struct fpemu *fe) 322 { 323 struct fpn x; 324 struct fpn p; 325 struct fpn *r; 326 int sign; 327 328 if (ISNAN(&fe->fe_f2)) 329 return &fe->fe_f2; 330 if (ISINF(&fe->fe_f2)) 331 return fpu_newnan(fe); 332 333 /* if x is +0/-0, return +0/-0 */ 334 if (ISZERO(&fe->fe_f2)) 335 return &fe->fe_f2; 336 337 CPYFPN(&x, &fe->fe_f2); 338 339 /* x = abs(input) */ 340 sign = x.fp_sign; 341 x.fp_sign = 0; 342 343 /* p <- 2*pi */ 344 fpu_const(&p, FPU_CONST_PI); 345 p.fp_exp++; 346 347 /* 348 * if (x > 2*pi*N) 349 * sin(x) is sin(x - 2*pi*N) 350 */ 351 CPYFPN(&fe->fe_f1, &x); 352 CPYFPN(&fe->fe_f2, &p); 353 r = fpu_cmp(fe); 354 if (r->fp_sign == 0) { 355 CPYFPN(&fe->fe_f1, &x); 356 CPYFPN(&fe->fe_f2, &p); 357 r = fpu_mod(fe); 358 CPYFPN(&x, r); 359 } 360 361 /* p <- pi */ 362 p.fp_exp--; 363 364 /* 365 * if (x > pi) 366 * sin(x) is -sin(x - pi) 367 */ 368 CPYFPN(&fe->fe_f1, &x); 369 CPYFPN(&fe->fe_f2, &p); 370 fe->fe_f2.fp_sign = 1; 371 r = fpu_add(fe); 372 if (r->fp_sign == 0) { 373 CPYFPN(&x, r); 374 sign ^= 1; 375 } 376 377 /* p <- pi/2 */ 378 p.fp_exp--; 379 380 /* 381 * if (x > pi/2) 382 * sin(x) is cos(x - pi/2) 383 * else 384 * sin(x) 385 */ 386 CPYFPN(&fe->fe_f1, &x); 387 CPYFPN(&fe->fe_f2, &p); 388 fe->fe_f2.fp_sign = 1; 389 r = fpu_add(fe); 390 if (r->fp_sign == 0) { 391 __fpu_sincos_cordic(fe, r); 392 r = &fe->fe_f2; 393 } else { 394 __fpu_sincos_cordic(fe, &x); 395 r = &fe->fe_f1; 396 } 397 r->fp_sign = sign; 398 return r; 399 } 400 401 /* 402 * tan(x) = sin(x) / cos(x) 403 */ 404 struct fpn * 405 fpu_tan(struct fpemu *fe) 406 { 407 struct fpn x; 408 struct fpn s; 409 struct fpn *r; 410 411 if (ISNAN(&fe->fe_f2)) 412 return &fe->fe_f2; 413 if (ISINF(&fe->fe_f2)) 414 return fpu_newnan(fe); 415 416 /* if x is +0/-0, return +0/-0 */ 417 if (ISZERO(&fe->fe_f2)) 418 return &fe->fe_f2; 419 420 CPYFPN(&x, &fe->fe_f2); 421 422 /* sin(x) */ 423 CPYFPN(&fe->fe_f2, &x); 424 r = fpu_sin(fe); 425 CPYFPN(&s, r); 426 427 /* cos(x) */ 428 CPYFPN(&fe->fe_f2, &x); 429 r = fpu_cos(fe); 430 CPYFPN(&fe->fe_f2, r); 431 432 CPYFPN(&fe->fe_f1, &s); 433 r = fpu_div(fe); 434 return r; 435 } 436 437 struct fpn * 438 fpu_sincos(struct fpemu *fe, int regc) 439 { 440 __fpu_sincos_cordic(fe, &fe->fe_f2); 441 442 /* cos(x) */ 443 fpu_implode(fe, &fe->fe_f2, FTYPE_EXT, &fe->fe_fpframe->fpf_regs[regc]); 444 445 /* sin(x) */ 446 return &fe->fe_f1; 447 } 448