nuttx/include/fixedmath.h

/****************************************************************************
 * include/fixedmath.h
 *
 * Licensed to the Apache Software Foundation (ASF) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.  The
 * ASF licenses this file to you under the Apache License, Version 2.0 (the
 * "License"); you may not use this file except in compliance with the
 * License.  You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
 * License for the specific language governing permissions and limitations
 * under the License.
 *
 ****************************************************************************/

#ifndef __INCLUDE_FIXEDMATH_H
#define __INCLUDE_FIXEDMATH_H

/****************************************************************************
 * Included Files
 ****************************************************************************/

#include <nuttx/compiler.h>
#include <stdint.h>

/****************************************************************************
 * Pre-processor Definitions
 ****************************************************************************/

/* Common numbers */

#define b8HUNDRED       0x6400                   /* 100 */
#define b8TEN           0x0a00                   /* 10 */
#define b8ONE           0x0100                   /* 1 */
#define b8HALF          0x0080                   /* 0.5 */
#define b8ONETENTH      0x001a                   /* 0.1 (actually 0.1015625) */
#define b8ONEHUNDRTH    0x0003                   /* 0.01 (actually 0.0117198765) */
#define b8HALFPI        0x0192                   /* 1.5703125 */
#define b8PI            0x0324                   /* 3.1406250 */
#define b8TWOPI         0x0648                   /* 6.2812500 */

#define b8MAX           0x7fff                   /* Max value of b8_t */
#define ub8MAX          0xffff                   /* Max value of rb8_t */
#define b8MIN           0x8000                   /* Min value of b8_t */
#define ub8MIN          0x0000                   /* Min value of ub8_t */

#define b16THOUSAND     0x03e80000               /* 1000 */
#define b16HUNDRED      0x00640000               /* 100 */
#define b16TEN          0x000a0000               /* 10 */
#define b16ONE          0x00010000               /* 1 */
#define b16HALF         0x00008000               /* 0.5 */
#define b16ONETENTH     0x0000199a               /* 0.1 (actually 0.100006..) */
#define b16ONEHUNDRTH   0x0000028f               /* 0.01 (actually 0.0099945..) */
#define b16ONETHOUSTH   0x00000042               /* 0.001 (actually 0.000100708..)*/
#define b16HALFPI       0x0001921f               /* 1.57078552246 */
#define b16PI           0x0003243f               /* 3.14158630371 */
#define b16TWOPI        0x0006487b               /* 6.28312683105 */

#define b16MAX          0x7fffffff               /* Max value of b16_t */
#define ub16MAX         0xffffffff               /* Max value of ub16_t */
#define b16MIN          0x80000000               /* Min value of b16_t */
#define ub16MIN         0x00000000               /* Min value of ub16_t */

#define b32MILLION      0x000f424000000000LL     /* 1000000 */
#define b32THOUSAND     0x000003e800000000LL     /* 1000 */
#define b32HUNDRED      0x0000006400000000LL     /* 100 */
#define b32TEN          0x0000000a00000000LL     /* 10 */
#define b32ONE          0x0000000100000000LL     /* 1 */
#define b32HALF         0x0000000080000000LL     /* 0.5 */
#define b32ONETENTH     0x000000001999999aLL     /* 0.1 */
#define b32ONEHUNDRTH   0x00000000028f5c29LL     /* 0.01 */
#define b32ONETHOUSTH   0x0000000000418937LL     /* 0.001 */
#define b32ONETENTHOU   0x0000000000068db9LL     /* 0.0001 */
#define b32HALFPI       0x00000001921eb9ffLL     /* 1.57078134990 */
#define b32PI           0x00000003243f6b4fLL     /* 3.14159269980 */
#define b32TWOPI        0x00000006487ae7fdLL     /* 6.28312539984 */

#define b32MAX          0x7fffffffffffffffLL     /* Max value of b32_t */
#define ub32MAX         0xffffffffffffffffLL     /* Max value of ub32_t */
#define b32MIN          0x8000000000000000LL     /* Min value of b32_t */
#define ub32MIN         0x0000000000000000LL     /* Min value of ub32_t */

/* Conversions between b32, b16, and b8 *************************************/

#define b8tob16(b)      (((b16_t)(b)) << 8)
#define ub8toub16(b)    (((ub16_t)(b)) << 8)
#define b16tob8(b)      (b8_t)(((b)+0x0080)>>8)
#define ub16toub8(b)    (ub8_t)(((b)+0x0080)>>8)

#ifdef CONFIG_HAVE_LONG_LONG
#  define b8tob32(b)    (((b32_t)(b)) << 24)
#  define ub8toub32(b)  (((ub32_t)(b)) << 24)
#  define b16tob32(b)   (((b32_t)(b)) << 16)
#  define ub16toub32(b) (((ub32_t)(b)) << 16)
#  define b32tob16(b)   (b16_t)(((b) + 0x0000000000008000)>>16)
#  define ub32toub16(b) (ub16_t)(((b) + 0x0000000000008000)>>16)
#  define b32tob8(b)    (b8_t)(((b) + 0x0000000000000080)>>8)
#endif

/* 16-bit values with 8 bits of precision ***********************************/

/* Conversions */

#define b8toi(a)        ((a) >> 8)               /* Conversion to integer */
#define ub8toi(a)       ((a) >> 8)               /* Conversion to unsigned integer */
#define itob8(i)        (((b8_t)(i)) << 8)       /* Conversion from integer */
#define uitoub8(i)      (((ub8_t)(i)) << 8)      /* Conversion from unsigned integer */
#define b8tof(b)        (((float)(b))/256.0f)    /* Conversion to float */
#define ftob8(f)        (b8_t)(((f)*256.0f))     /* Conversion from float */
#define b8trunc(a)      ((a) & 0xff00)           /* Truncate to integer b8 */
#define b8round(a)      (((a)+0x0080) & 0xff00)  /* Round to integer b8 */
#define b8frac(a)       ((a) & 0x00ff)           /* Take fractional part */

/* Operators */

#define ub8inv(b)       (0x8000/((b)>>1))                   /* Inversion (b8=b15/b7) */
#define b8inv(b)        (0x4000/((b)>>2))                   /* Inversion (b8=b14/b6) */
#define b8addb8(a,b)    ((a)+(b))                           /* Addition */
#define b8addi(a,i)     ((a)+itob8(i))                      /* Add integer from b16 */
#define b8subb8(a,b)    ((a)-(b))                           /* Subtraction */
#define b8subi(a,i)     ((a)-itob8(i))                      /* Subtract integer from b8 */
#define b8mulb8(a,b)    (b16tob8((b16_t)(a)*(b16_t)(b))     /* Muliplication */
#define ub8mulub8(a,b)  (ub16toub8((ub16_t)(a)*(ub16_t)(b)) /* Muliplication */
#define b8muli(a,i)     ((a)*(i))                           /* Simple multiplication by integer */
#define b8sqr(a)        b8mulb8(a,a)                        /* Square */
#define ub8sqr(a)       ub8mulub8(a,a)                      /* Square */
#define b8divb8(a,b)    (b8tob16(a)/(b16_t)(b))             /* Division */
#define ub8divub8(a,b)  (ub8toub16(a)/(ub16_t)(b))          /* Division */
#define b8divi(a,i)     ((a)/(i))                           /* Simple division by integer */
#define b8idiv(i,j)     (((i)<<8)/j)                        /* Division of integer, b8 result */
#define b8abs(b)        ((b < 0) ? (-b) : (b))              /* Get the absolute value */
#define b8sign(b)       ((b > 0) ? (b8ONE) : (-b8ONE))      /* Get the sign */

/* 32-bit values with 16 bits of precision **********************************/

/* Conversions */

#define b16toi(a)       ((a) >> 16)              /* Conversion to integer */
#define ub16toi(a)      ((a) >> 16)              /* Conversion to unsgined integer */
#define itob16(i)       (((b16_t)(i)) << 16)     /* Conversion from integer */
#define uitoub16(i)     (((ub16_t)(i)) << 16)    /* Conversion from unsigned integer */
#define b16tof(b)       (((float)(b))/65536.0f)  /* Conversion to float */
#define ftob16(f)       (b16_t)(((f)*65536.0f))  /* Conversion from float */
#define b16tod(b)       (((double)(b))/65536.0)  /* Conversion to double */
#define dtob16(f)       (b16_t)(((f)*65536.0))   /* Conversion from double */
#define b16trunc(a)     ((a) & 0xffff0000)       /* Truncate to integer */
#define b16round(a)     (((a)+0x00008000) & 0xffff0000)
#define b16frac(a)      ((a) & 0x0000ffff)       /* Take fractional part */

/* Operators */

#define ub16inv(b)      (0x80000000/((b)>>1))    /* Inversion (b16=b31/b15) */
#define b16inv(b)       (0x40000000/((b)>>2))    /* Inversion (b16=b30/b14) */
#define b16addb16(a,b)  ((a)+(b))                /* Addition */
#define b16addi(a,i)    ((a)+itob16(i))          /* Add integer to b16 */
#define b16subb16(a,b)  ((a)-(b))                /* Subtraction */
#define b16subi(a,i)    ((a)-itob16(i))          /* Subtract integer from b16 */
#define b16muli(a,i)    ((a)*(i))                /* Simple multiplication by integer */
#define b16divi(a,i)    ((a)/(i))                /* Simple division by integer*/
#define b16idiv(i,j)    (((i)<<16)/j)            /* Division of integer, b16 result */
#define b16abs(b)       ((b < 0) ? (-b) : (b))   /* Get the absolute value */
#define b16sign(b)      ((b > 0) ? (b16ONE) : (-b16ONE))

#ifdef CONFIG_HAVE_LONG_LONG
/* Multiplication operators */

#  define b16mulb16(a,b)   b32tob16((b32_t)(a)*(b32_t)(b))
#  define ub16mulub16(a,b) ub32toub16((ub32_t)(a)*(ub32_t)(b))

/* Square operators */

#  define b16sqr(a)        b16mulb16(a,a)
#  define ub16sqr(a)       ub16mulub16(a,a)

/* Division operators */

#  define b16divb16(a,b)   (b16_t)(b16tob32(a)/(b32_t)(b))
#  define ub16divub16(a,b) (ub16_t)(ub16toub32(a)/(ub32_t)(b))

/* Square root operators */

#  define ub16sqrtub16(a)  ub32sqrtub16(ub16toub32(a))
#else
#  define ub16sqrtub16(a)  ub8toub16(ub16sqrtub8(a))
#endif

/* 64-bit values with 32 bits of precision **********************************/

#ifdef CONFIG_HAVE_LONG_LONG
/* Conversions */

#define b32toi(a)       ((a) >> 32)                   /* Conversion to integer */
#define itob32(i)       (((b32_t)(i)) << 32)          /* Conversion from integer */
#define uitoub32(i)     (((ub32_t)(i)) << 32)         /* Conversion from unsigned integer */
#define b32tod(b)       (((double)(b))/b32ONE)        /* Conversion to double */
#define dtob32(f)       (b32_t)(((f)*(double)b32ONE)) /* Conversion from double */
#define b32trunc(a)     ((a) & 0xffffffff00000000)    /* Truncate to integer */
#define b32round(a)     (((a)+0x0000000080000000) & 0xffffffff00000000)
#define b32frac(a)      ((a) & 0x00000000ffffffff)    /* Take fractional part */
#define b32abs(b)       ((b < 0) ? (-b) : (b))        /* Get the absolute value */
#define b32sign(b)      ((b > 0) ? (b32ONE) : (-b32ONE))
#endif

/****************************************************************************
 * Public Types
 ****************************************************************************/

typedef int16_t  b8_t;
typedef uint16_t ub8_t;
typedef int32_t  b16_t;
typedef uint32_t ub16_t;
#ifdef CONFIG_HAVE_LONG_LONG
typedef int64_t  b32_t;
typedef uint64_t ub32_t;
#endif

/****************************************************************************
 * Public Function Prototypes
 ****************************************************************************/

#undef EXTERN
#if defined(__cplusplus)
#define EXTERN extern "C"
extern "C"
{
#else
#define EXTERN extern
#endif

#ifndef CONFIG_HAVE_LONG_LONG
/* Multiplication operators */

b16_t  b16mulb16(b16_t m1, b16_t m2);
ub16_t ub16mulub16(ub16_t m1, ub16_t m2);

/* Square operators */

b16_t  b16sqr(b16_t a);
ub16_t ub16sqr(ub16_t a);

/* Division operators */

b16_t  b16divb16(b16_t num, b16_t denom);
ub16_t ub16divub16(ub16_t num, ub16_t denom);
#endif

/* Trigonometric Functions */

b16_t  b16sin(b16_t rad);
b16_t  b16cos(b16_t rad);
b16_t  b16atan2(b16_t y, b16_t x);

/* Square root operators */

#ifdef CONFIG_HAVE_LONG_LONG
ub16_t ub32sqrtub16(ub32_t a);
#endif
ub8_t ub16sqrtub8(ub16_t a);

#undef EXTERN
#if defined(__cplusplus)
}
#endif

#endif /* __INCLUDE_FIXEDMATH_H */