summaryrefslogtreecommitdiff
path: root/nuttx/lib/math
diff options
context:
space:
mode:
Diffstat (limited to 'nuttx/lib/math')
-rw-r--r--nuttx/lib/math/Make.defs37
-rw-r--r--nuttx/lib/math/lib_b16cos.c64
-rw-r--r--nuttx/lib/math/lib_b16sin.c110
-rw-r--r--nuttx/lib/math/lib_fixedmath.c272
-rw-r--r--nuttx/lib/math/lib_rint.c135
5 files changed, 618 insertions, 0 deletions
diff --git a/nuttx/lib/math/Make.defs b/nuttx/lib/math/Make.defs
new file mode 100644
index 000000000..0c3414b98
--- /dev/null
+++ b/nuttx/lib/math/Make.defs
@@ -0,0 +1,37 @@
+############################################################################
+# lib/math/Make.defs
+#
+# Copyright (C) 2011 Gregory Nutt. All rights reserved.
+# Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name NuttX nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+MATH_SRCS = lib_rint.c lib_fixedmath.c lib_b16sin.c lib_b16cos.c
+
diff --git a/nuttx/lib/math/lib_b16cos.c b/nuttx/lib/math/lib_b16cos.c
new file mode 100644
index 000000000..69cc61042
--- /dev/null
+++ b/nuttx/lib/math/lib_b16cos.c
@@ -0,0 +1,64 @@
+/****************************************************************************
+ * lib/math/lib_b16cos.c
+ *
+ * Copyright (C) 2007, 2008, 2011 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <fixedmath.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Global Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: b16cos
+ ****************************************************************************/
+
+b16_t b16cos(b16_t rad)
+{
+ /* Compute cosine: sin(rad + PI/2) = cos(rad) */
+
+ rad += b16HALFPI;
+ if (rad > b16PI)
+ {
+ rad -= b16TWOPI;
+ }
+ return b16sin(rad);
+}
diff --git a/nuttx/lib/math/lib_b16sin.c b/nuttx/lib/math/lib_b16sin.c
new file mode 100644
index 000000000..fc4b5e566
--- /dev/null
+++ b/nuttx/lib/math/lib_b16sin.c
@@ -0,0 +1,110 @@
+/****************************************************************************
+ * lib/math/lib_b16sin.c
+ *
+ * Copyright (C) 2008, 2011 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <fixedmath.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define b16_P225 0x0000399a
+#define b16_P405284735 0x000067c1
+#define b16_1P27323954 0x000145f3
+
+/****************************************************************************
+ * Global Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: b16sin
+ * Ref: http://lab.polygonal.de/2007/07/18/fast-and-accurate-sinecosine-approximation/
+ ****************************************************************************/
+
+b16_t b16sin(b16_t rad)
+{
+ b16_t tmp1;
+ b16_t tmp2;
+ b16_t tmp3;
+
+ /* Force angle into the good range */
+
+ if (rad < -b16PI)
+ {
+ rad += b16TWOPI;
+ }
+ else if (rad > b16PI)
+ {
+ rad -= b16TWOPI;
+ }
+
+ /* tmp1 = 1.27323954 * rad
+ * tmp2 = .405284735 * rad * rad
+ */
+
+
+ tmp1 = b16mulb16(b16_1P27323954, rad);
+ tmp2 = b16mulb16(b16_P405284735, b16sqr(rad));
+
+ if (rad < 0)
+ {
+ /* tmp3 = 1.27323954 * rad + .405284735 * rad * rad */
+
+ tmp3 = tmp1 + tmp2;
+ }
+ else
+ {
+ /* tmp3 = 1.27323954 * rad - 0.405284735 * rad * rad */
+
+ tmp3 = tmp1 - tmp2;
+ }
+
+ /* tmp1 = tmp3*tmp3 */
+
+ tmp1 = b16sqr(tmp3);
+ if (tmp3 < 0)
+ {
+ /* tmp1 = tmp3 * -tmp3 */
+
+ tmp1 = -tmp1;
+ }
+
+ /* Return sin = .225 * (tmp3 * (+/-tmp3) - tmp3) + tmp3 */
+
+ return b16mulb16(b16_P225, (tmp1 - tmp3)) + tmp3;
+}
diff --git a/nuttx/lib/math/lib_fixedmath.c b/nuttx/lib/math/lib_fixedmath.c
new file mode 100644
index 000000000..b20158dc8
--- /dev/null
+++ b/nuttx/lib/math/lib_fixedmath.c
@@ -0,0 +1,272 @@
+/****************************************************************************
+ * lib/math/lib_fixedmath.c
+ *
+ * Copyright (C) 2008-2009, 2011 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <fixedmath.h>
+
+#ifndef CONFIG_HAVE_LONG_LONG
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Type Declarations
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: fixsign
+ ****************************************************************************/
+
+static void fixsign(b16_t *parg1, b16_t *parg2, bool *pnegate)
+{
+ bool negate = false;
+ b16_t arg;
+
+ arg = *parg1;
+ if (arg < 0)
+ {
+ *parg1 = -arg;
+ negate = true;
+ }
+
+ arg = *parg2;
+ if (arg < 0)
+ {
+ *parg2 = -arg;
+ negate ^= true;
+ }
+
+ *pnegate = negate;
+}
+
+/****************************************************************************
+ * Name: adjustsign
+ ****************************************************************************/
+
+static b16_t adjustsign(b16_t result, bool negate)
+{
+ /* If the product is negative, then we overflowed */
+
+ if (result < 0)
+ {
+ if (result)
+ {
+ return b16MIN;
+ }
+ else
+ {
+ return b16MAX;
+ }
+ }
+
+ /* correct the sign of the result */
+
+ if (negate)
+ {
+ return -result;
+ }
+ return result;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: b16mulb16
+ ****************************************************************************/
+
+b16_t b16mulb16(b16_t m1, b16_t m2)
+{
+ bool negate;
+ b16_t product;
+
+ fixsign(&m1, &m2, &negate);
+ product = (b16_t)ub16mulub16((ub16_t)m1, (ub16_t)m2);
+ return adjustsign(product, negate);
+}
+
+/****************************************************************************
+ * Name: ub16mulub16
+ **************************************************************************/
+
+ub16_t ub16mulub16(ub16_t m1, ub16_t m2)
+{
+ /* Let:
+ *
+ * m1 = m1i*2**16 + m1f (b16)
+ * m2 = m2i*2**16 + m2f (b16)
+ *
+ * Then:
+ *
+ * m1*m2 = (m1i*m2i)*2**32 + (m1i*m2f + m2i*m1f)*2**16 + m1f*m2f (b32)
+ * = (m1i*m2i)*2**16 + (m1i*m2f + m2i*m1f) + m1f*m2f*2**-16 (b16)
+ * = a*2**16 + b + c*2**-16
+ */
+
+ uint32_t m1i = ((uint32_t)m1 >> 16);
+ uint32_t m2i = ((uint32_t)m1 >> 16);
+ uint32_t m1f = ((uint32_t)m1 & 0x0000ffff);
+ uint32_t m2f = ((uint32_t)m2 & 0x0000ffff);
+
+ return (m1i*m2i << 16) + m1i*m2f + m2i*m1f + (((m1f*m2f) + b16HALF) >> 16);
+}
+
+/****************************************************************************
+ * Name: b16divb16
+ **************************************************************************/
+
+b16_t b16sqr(b16_t a)
+{
+ b16_t sq;
+
+ /* The result is always positive. Just take the absolute value */
+
+ if (a < 0)
+ {
+ a = -a;
+ }
+
+ /* Overflow occurred if the result is negative */
+
+ sq = (b16_t)ub16sqr(a);
+ if (sq < 0)
+ {
+ sq = b16MAX;
+ }
+ return sq;
+}
+
+/****************************************************************************
+ * Name: b16divb16
+ **************************************************************************/
+
+ub16_t ub16sqr(ub16_t a)
+{
+ /* Let:
+ *
+ * m = mi*2**16 + mf (b16)
+ *
+ * Then:
+ *
+ * m*m = (mi*mi)*2**32 + 2*(m1*m2)*2**16 + mf*mf (b32)
+ * = (mi*mi)*2**16 + 2*(mi*mf) + mf*mf*2**-16 (b16)
+ */
+
+ uint32_t mi = ((uint32_t)a >> 16);
+ uint32_t mf = ((uint32_t)a & 0x0000ffff);
+
+ return (mi*mi << 16) + (mi*mf << 1) + ((mf*mf + b16HALF) >> 16);
+}
+
+/****************************************************************************
+ * Name: b16divb16
+ **************************************************************************/
+
+b16_t b16divb16(b16_t num, b16_t denom)
+{
+ bool negate;
+ b16_t quotient;
+
+ fixsign(&num, &denom, &negate);
+ quotient = (b16_t)ub16divub16((ub16_t)num, (ub16_t)denom);
+ return adjustsign(quotient, negate);
+}
+
+/****************************************************************************
+ * Name: ub16divub16
+ **************************************************************************/
+
+ub16_t ub16divub16(ub16_t num, ub16_t denom)
+{
+ uint32_t term1;
+ uint32_t numf;
+ uint32_t product;
+
+ /* Let:
+ *
+ * num = numi*2**16 + numf (b16)
+ * den = deni*2**16 + denf (b16)
+ *
+ * Then:
+ *
+ * num/den = numi*2**16 / den + numf / den (b0)
+ * = numi*2**32 / den + numf*2**16 /den (b16)
+ */
+
+ /* Check for overflow in the first part of the quotient */
+
+ term1 = ((uint32_t)num & 0xffff0000) / denom;
+ if (term1 >= 0x00010000)
+ {
+ return ub16MAX; /* Will overflow */
+ }
+
+ /* Finish the division */
+
+ numf = num - term1 * denom;
+ term1 <<= 16;
+ product = term1 + (numf + (denom >> 1)) / denom;
+
+ /* Check for overflow */
+
+ if (product < term1)
+ {
+ return ub16MAX; /* Overflowed */
+ }
+ return product;
+}
+
+#endif
diff --git a/nuttx/lib/math/lib_rint.c b/nuttx/lib/math/lib_rint.c
new file mode 100644
index 000000000..b122870b8
--- /dev/null
+++ b/nuttx/lib/math/lib_rint.c
@@ -0,0 +1,135 @@
+/************************************************************
+ * lib/math/lib_rint.c
+ *
+ * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ************************************************************/
+
+/************************************************************
+ * Compilation Switches
+ ************************************************************/
+
+/************************************************************
+ * Included Files
+ ************************************************************/
+
+#include <nuttx/config.h>
+#include <sys/types.h>
+#include <stdlib.h>
+
+/************************************************************
+ * Definitions
+ ************************************************************/
+
+/************************************************************
+ * Private Type Declarations
+ ************************************************************/
+
+/************************************************************
+ * Private Function Prototypes
+ ************************************************************/
+
+/**********************************************************
+ * Global Constant Data
+ **********************************************************/
+
+/************************************************************
+ * Global Variables
+ ************************************************************/
+
+/**********************************************************
+ * Private Constant Data
+ **********************************************************/
+
+/************************************************************
+ * Private Variables
+ ************************************************************/
+
+double_t rint(double_t x)
+{
+ double_t ret;
+
+ /* If the current rounding mode rounds toward negative
+ * infinity, rint() is identical to floor(). If the current
+ * rounding mode rounds toward positive infinity, rint() is
+ * identical to ceil().
+ */
+
+#if defined(CONFIG_FP_ROUND_POSITIVE) && CONFIG_FP_ROUNDING_POSITIVE != 0
+
+ ret = ceil(x);
+
+#elif defined(CONFIG_FP_ROUND_NEGATIVE) && CONFIG_FP_ROUNDING_NEGATIVE != 0
+
+ ret = floor(x);
+
+#else
+
+ /* In the default rounding mode (round to nearest), rint(x) is the
+ * integer nearest x with the additional stipulation that if
+ * |rint(x)-x|=1/2, then rint(x) is even.
+ */
+
+ long dwinteger = (long)x;
+ double_t fremainder = x - (double_t)dwinteger;
+
+ if (x < 0.0)
+ {
+ /* fremainder should be in range 0 .. -1 */
+
+ if (fremainder == -0.5)
+ {
+ dwinteger = ((dwinteger+1)&~1);
+ }
+ else if (fremainder < -0.5)
+ {
+ dwinteger--;
+ }
+ }
+ else
+ {
+ /* fremainder should be in range 0 .. 1 */
+
+ if (fremainder == 0.5)
+ {
+ dwinteger = ((dwinteger+1)&~1);
+ }
+ else if (fremainder > 0.5)
+ {
+ dwinteger++;
+ }
+ }
+
+ ret = (double_t)dwinteger;
+#endif
+
+ return ret;
+}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-22 00:14:43 +0000