payloads/libpayload/include/fpmath.h

   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 }