added initial port of robotics cape math

src/rcmath/CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(rcmath)
+
+find_package(catkin REQUIRED COMPONENTS)
+
+catkin_package(
+	INCLUDE_DIRS include
+	LIBRARIES rcmath
+)
+
+
+include_directories(include)
+add_library(serial
+	    src/rcmath/rc_linear_algebra.c
+        src/rcmath/rc_linear_filter.c
+        src/rcmath/rc_linear_matrix.c
+        src/rcmath/rc_linear_neon_functions.c
+        src/rcmath/rc_polynomial.c
+        src/rcmath/rc_ring_buffer.c
+        src/rcmath/rc_vector.c
+        src/rcmath/rc_quaternion.c
+        )
+
+
+

src/rcmath/include/rcmath/preprocessor_macros.h

@@ -0,0 +1,42 @@
+/*******************************************************************************
+* preprocessor_macros.h
+*
+* This is a list of macros to clean up GNU-C extensions in a safe and portable
+* way. Thank you Robert Love for providing this in "Linux System Programming",
+* it is an excellent book.
+*******************************************************************************/
+
+
+#if __GNUC__ >= 3
+# undef inline
+# define inline			inline __attribute__ ((always_inline))
+# define __noinline		__attribute__ ((noinline))
+# define __pure			__attribute__ ((pure))
+# define __const		__attribute__ ((const))
+# define __noreturn		__attribute__ ((noreturn))
+# define __malloc		__attribute__ ((malloc))
+# define __must_check	__attribute__ ((warn_unused_result))
+# define __deprecated	__attribute__ ((deprecated))
+# define __used			__attribute__ ((used))
+# define __unused		__attribute__ ((unused))
+# define __packed		__attribute__ ((packed))
+# define __align(x)		__attribute__ ((aligned (x)))
+# define __align_max	__attribute__ ((aligned))
+# define likely(x)		__builtin_expect (!!(x), 1)
+# define unlikely(x)	__builtin_expect (!!(x), 0)
+#else
+# define __noinline		/* no noinline */
+# define __pure			/* pure */
+# define __const		/* const */
+# define __noreturn		/* noreturn */
+# define __malloc		/* malloc */
+# define __must_check	/* warn_unused_result */
+# define __deprecated	/* deprecated */
+# define __used			/* used */
+# define __unused		/* unused */
+# define __packed		/* packed */
+# define __align(x)		/* aligned */
+# define __align_max	/* align_max */
+# define likely(x) (x)
+# define unlikely(x) (x)
+#endif

src/rcmath/include/rcmath/rc_algebra_common.h

@@ -0,0 +1,26 @@
+/*******************************************************************************
+* rc_algebra_common.h
+*
+* all things shared between rc_vector.c, rc_matrix.c, and rc_linear_algebra.c
+*******************************************************************************/
+#include "roboticscape.h"
+#include "preprocessor_macros.h"
+#include <stdio.h>	// for fprintf
+#include <stdlib.h>	// for malloc,calloc,free
+#include <math.h>	// for sqrt, pow, etc
+#include <float.h>	// for FLT_MAX
+#include <string.h>	// for memcpy
+
+#define ZERO_TOLERANCE 1e-6 // consider v to be zero if fabs(v)<ZERO_TOLERANCE
+
+/*******************************************************************************
+* float rc_mult_accumulate(float * __restrict__ a, float * __restrict__ b, int n)
+* 
+* Performs a vector dot product on the contents of a and b over n values.
+* This is a dangerous function that could segfault if not used properly. Hence
+* it is only for internal use in the RC library. the 'restrict' attributes tell
+* the C compiler that the pointers are not aliased which helps the vectorization
+* process for optimization with the NEON FPU.
+*******************************************************************************/
+float rc_mult_accumulate(float * __restrict__ a, float * __restrict__ b, int n);
+

src/rcmath/include/rcmath/rc_algebra_common.h~

@@ -0,0 +1,26 @@
+/*******************************************************************************
+* rc_algebra_common.h
+*
+* all things shared between rc_vector.c, rc_matrix.c, and rc_linear_algebra.c
+*******************************************************************************/
+#include "../roboticscape.h"
+#include "../preprocessor_macros.h"
+#include <stdio.h>	// for fprintf
+#include <stdlib.h>	// for malloc,calloc,free
+#include <math.h>	// for sqrt, pow, etc
+#include <float.h>	// for FLT_MAX
+#include <string.h>	// for memcpy
+
+#define ZERO_TOLERANCE 1e-6 // consider v to be zero if fabs(v)<ZERO_TOLERANCE
+
+/*******************************************************************************
+* float rc_mult_accumulate(float * __restrict__ a, float * __restrict__ b, int n)
+* 
+* Performs a vector dot product on the contents of a and b over n values.
+* This is a dangerous function that could segfault if not used properly. Hence
+* it is only for internal use in the RC library. the 'restrict' attributes tell
+* the C compiler that the pointers are not aliased which helps the vectorization
+* process for optimization with the NEON FPU.
+*******************************************************************************/
+float rc_mult_accumulate(float * __restrict__ a, float * __restrict__ b, int n);
+