1  /*
  2   *
  3   * Copyright (C) 2020 Google, Inc.
  4   *
  5   * Redistribution and use in source and binary forms, with or without
  6   * modification, are permitted provided that the following conditions
  7   * are met:
  8   * 1. Redistributions of source code must retain the above copyright
  9   *    notice, this list of conditions and the following disclaimer.
 10   * 2. Redistributions in binary form must reproduce the above copyright
 11   *    notice, this list of conditions and the following disclaimer in the
 12   *    documentation and/or other materials provided with the distribution.
 13   * 3. The name of the author may not be used to endorse or promote products
 14   *    derived from this software without specific prior written permission.
 15   *
 16   * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 17   * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 18   * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 19   * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 20   * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 21   * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 22   * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 23   * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 24   * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 25   * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 26   * SUCH DAMAGE.
 27   */
 28  
 29  #include <stdint.h>
 30  
 31  /*
 32   * This file implements operations for a simple 32.32 fixed-point math type.
 33   * This is intended for speed-critical stuff (e.g. graphics) so there are
 34   * intentionally no overflow checks or assertions, and operations are written
 35   * to prefer speed over precision (e.g. multiplying by 1 may lose precision).
 36   * For best results, only use for applications where 16.16 would fit.
 37   */
 38  
 39  typedef struct {		/* wrap in struct to prevent direct access */
 40  	int64_t v;
 41  } fpmath_t;
 42  
 43  #define FPMATH_SHIFT 32		/* define where to place the decimal point */
 44  
 45  /* Turn an integer into an fpmath_t. */
 46  static inline fpmath_t fp(int32_t a)
 47  {
 48  	return (fpmath_t){ .v = (int64_t)a << FPMATH_SHIFT };
 49  }
 50  
 51  /* Create an fpmath_t from a fraction. (numerator / denominator)  */
 52  static inline fpmath_t fpfrac(int32_t numerator, int32_t denominator)
 53  {
 54  	return (fpmath_t){ .v = ((int64_t)numerator << FPMATH_SHIFT) / denominator };
 55  }
 56  
 57  /* Turn an fpmath_t back into an integer, rounding towards -INF. */
 58  static inline int32_t fpfloor(fpmath_t a)
 59  {
 60  	return a.v >> FPMATH_SHIFT;
 61  }
 62  
 63  /* Turn an fpmath_t back into an integer, rounding towards nearest. */
 64  static inline int32_t fpround(fpmath_t a)
 65  {
 66  	return (a.v + ((int64_t)1 << (FPMATH_SHIFT - 1))) >> FPMATH_SHIFT;
 67  }
 68  
 69  /* Turn an fpmath_t back into an integer, rounding towards +INF. */
 70  static inline int32_t fpceil(fpmath_t a)
 71  {
 72  	return (a.v + ((int64_t)1 << FPMATH_SHIFT) - 1) >> FPMATH_SHIFT;
 73  }
 74  
 75  /* Add two fpmath_t. (a + b) */
 76  static inline fpmath_t fpadd(fpmath_t a, fpmath_t b)
 77  {
 78  	return (fpmath_t){ .v = a.v + b.v };
 79  }
 80  
 81  /* Add an fpmath_t and an integer. (a + b) */
 82  static inline fpmath_t fpaddi(fpmath_t a, int32_t b)
 83  {
 84  	return (fpmath_t){ .v = a.v + ((int64_t)b << FPMATH_SHIFT) };
 85  }
 86  
 87  /* Subtract one fpmath_t from another. (a + b) */
 88  static inline fpmath_t fpsub(fpmath_t a, fpmath_t b)
 89  {
 90  	return (fpmath_t){ .v = a.v - b.v };
 91  }
 92  
 93  /* Subtract an integer from an fpmath_t. (a - b) */
 94  static inline fpmath_t fpsubi(fpmath_t a, int32_t b)
 95  {
 96  	return (fpmath_t){ .v = a.v - ((int64_t)b << FPMATH_SHIFT) };
 97  }
 98  
 99  /* Subtract an fpmath_t from an integer. (a - b) */
100  static inline fpmath_t fpisub(int32_t a, fpmath_t b)
101  {
102  	return (fpmath_t){ .v = ((int64_t)a << FPMATH_SHIFT) - b.v };
103  }
104  
105  /* Multiply two fpmath_t. (a * b)
106     Looses 16 bits fractional precision on each. */
107  static inline fpmath_t fpmul(fpmath_t a, fpmath_t b)
108  {
109  	return (fpmath_t){ .v = (a.v >> (FPMATH_SHIFT/2)) * (b.v >> (FPMATH_SHIFT/2)) };
110  }
111  
112  /* Multiply an fpmath_t and an integer. (a * b) */
113  static inline fpmath_t fpmuli(fpmath_t a, int32_t b)
114  {
115  	return (fpmath_t){ .v = a.v * b };
116  }
117  
118  /* Divide an fpmath_t by another. (a / b)
119     Truncates integral part of a to 16 bits! Careful with this one! */
120  static inline fpmath_t fpdiv(fpmath_t a, fpmath_t b)
121  {
122  	return (fpmath_t){ .v = (a.v << (FPMATH_SHIFT/2)) / (b.v >> (FPMATH_SHIFT/2)) };
123  }
124  
125  /* Divide an fpmath_t by an integer. (a / b) */
126  static inline fpmath_t fpdivi(fpmath_t a, int32_t b)
127  {
128  	return (fpmath_t){ .v = a.v / b };
129  }
130  
131  /* Calculate absolute value of an fpmath_t. (ABS(a)) */
132  static inline fpmath_t fpabs(fpmath_t a)
133  {
134  	return (fpmath_t){ .v = (a.v < 0 ? -a.v : a.v) };
135  }
136  
137  /* Return true iff two fpmath_t are exactly equal. (a == b)
138     Like with floats, you probably don't want to use this most of the time. */
139  static inline int fpequals(fpmath_t a, fpmath_t b)
140  {
141  	return a.v == b.v;
142  }
143  
144  /* Return true iff one fpmath_t is less than another. (a < b) */
145  static inline int fpless(fpmath_t a, fpmath_t b)
146  {
147  	return a.v < b.v;
148  }
149  
150  /* Return true iff one fpmath_t is more than another. (a > b) */
151  static inline int fpmore(fpmath_t a, fpmath_t b)
152  {
153  	return a.v > b.v;
154  }
155  
156  /* Return the smaller of two fpmath_t. (MIN(a, b)) */
157  static inline fpmath_t fpmin(fpmath_t a, fpmath_t b)
158  {
159  	if (a.v < b.v)
160  		return a;
161  	else
162  		return b;
163  }
164  
165  /* Return the larger of two fpmath_t. (MAX(a, b)) */
166  static inline fpmath_t fpmax(fpmath_t a, fpmath_t b)
167  {
168  	if (a.v > b.v)
169  		return a;
170  	else
171  		return b;
172  }
173  
174  /* Return the constant PI as an fpmath_t. */
175  static inline fpmath_t fppi(void)
176  {
177  	/* Rounded (uint64_t)(M_PI * (1UL << 60)) to nine hex digits. */
178  	return (fpmath_t){ .v = 0x3243f6a89 };
179  }
180  
181  /*
182   * Returns the "one-based" sine of an fpmath_t, meaning the input is interpreted as if the range
183   * 0.0-1.0 corresponded to 0.0-PI/2 for radians. This is mostly here as the base primitives for
184   * the other trig stuff, but it may be useful to use directly if your input value already needs
185   * to be multiplied by some factor of PI and you want to save the instructions (and precision)
186   * for multiplying it in just so that the trig functions can divide it right out again.
187   */
188  fpmath_t fpsin1(fpmath_t x);
189  
190  /* Returns the "one-based" cosine of an fpmath_t (analogous definition to fpsin1()). */
191  static inline fpmath_t fpcos1(fpmath_t x)
192  {
193  	return fpsin1(fpaddi(x, 1));
194  }
195  
196  /* Returns the sine of an fpmath_t interpreted as radians. */
197  static inline fpmath_t fpsinr(fpmath_t radians)
198  {
199  	return fpsin1(fpdiv(radians, fpdivi(fppi(), 2)));
200  }
201  
202  /* Returns the sine of an fpmath_t interpreted as degrees. */
203  static inline fpmath_t fpsind(fpmath_t degrees)
204  {
205  	return fpsin1(fpdivi(degrees, 90));
206  }
207  
208  /* Returns the cosine of an fpmath_t interpreted as radians. */
209  static inline fpmath_t fpcosr(fpmath_t radians)
210  {
211  	return fpcos1(fpdiv(radians, fpdivi(fppi(), 2)));
212  }
213  
214  /* Returns the cosine of an fpmath_t interpreted as degrees. */
215  static inline fpmath_t fpcosd(fpmath_t degrees)
216  {
217  	return fpcos1(fpdivi(degrees, 90));
218  }
219  
220  /* Returns the tangent of an fpmath_t interpreted as radians.
221     No guard rails, don't call this at the poles or you'll divide by 0! */
222  static inline fpmath_t fptanr(fpmath_t radians)
223  {
224  	fpmath_t one_based = fpdiv(radians, fpdivi(fppi(), 2));
225  	return fpdiv(fpsin1(one_based), fpcos1(one_based));
226  }
227  
228  /* Returns the tangent of an fpmath_t interpreted as degrees.
229     No guard rails, don't call this at the poles or you'll divide by 0! */
230  static inline fpmath_t fptand(fpmath_t degrees)
231  {
232  	fpmath_t one_based = fpdivi(degrees, 90);
233  	return fpdiv(fpsin1(one_based), fpcos1(one_based));
234  }