From c4203a8ab1b3d77ee0681ab7567e84eb23c8f0e2 Mon Sep 17 00:00:00 2001 From: Scott Kovach Date: Mon, 28 Mar 2016 16:48:30 -0700 Subject: [PATCH 1/3] update cmake with new plover compiler, example code --- include/libswiftnav/plover/ambiguity_test.h | 13 - plover/AmbiguityTest.hs | 33 -- plover/CMakeLists.txt | 83 ++- plover/Main.hs | 18 - plover/README | 2 + plover/libswiftnav-generate.cabal | 14 - plover/prelude.plv | 555 ++++++++++++++++++++ plover/qr.plv | 82 +++ src/CMakeLists.txt | 6 +- src/plover/ambiguity_test.c | 9 - 10 files changed, 698 insertions(+), 117 deletions(-) delete mode 100644 include/libswiftnav/plover/ambiguity_test.h delete mode 100644 plover/AmbiguityTest.hs delete mode 100644 plover/Main.hs delete mode 100644 plover/libswiftnav-generate.cabal create mode 100644 plover/prelude.plv create mode 100644 plover/qr.plv delete mode 100644 src/plover/ambiguity_test.c diff --git a/include/libswiftnav/plover/ambiguity_test.h b/include/libswiftnav/plover/ambiguity_test.h deleted file mode 100644 index dc0caea6..00000000 --- a/include/libswiftnav/plover/ambiguity_test.h +++ /dev/null @@ -1,13 +0,0 @@ -#ifndef PLOVER_GENERATED_AMBIGUITY_TEST_H -#define PLOVER_GENERATED_AMBIGUITY_TEST_H - -#include - -typedef struct { - int n; - double xs[22]; -} test_struct_t; -int hello_world(void); - - -#endif /* PLOVER_GENERATED_AMBIGUITY_TEST_H */ diff --git a/plover/AmbiguityTest.hs b/plover/AmbiguityTest.hs deleted file mode 100644 index 2d606c0f..00000000 --- a/plover/AmbiguityTest.hs +++ /dev/null @@ -1,33 +0,0 @@ -{-# LANGUAGE OverloadedStrings #-} - -module AmbiguityTest (cu) where - -import Plover.Types -import Plover.Macros -import Plover.Compile - -testStruct = StructDecl "test_struct_t" ( - ST Generated [ - ("n", IntType), - ("xs", VecType [22] NumType) - ] - ) - -helloPlover :: FunctionDefinition -helloPlover = ("hello_world", FnT [] [] IntType, body) - where - body :: CExpr - body = seqList $ [ - Ref "printf" :$ StrLit "hello world\n", - Return 0 - ] - -cu :: CompilationUnit -cu = CU - { unitName = "ambiguity_test" - , sourceDefs = [helloPlover] - , sourceIncs = ["stdio.h", "plover/ambiguity_test.h"] - , headerDefs = [testStruct] - , headerIncs = ["common.h"] - } - diff --git a/plover/CMakeLists.txt b/plover/CMakeLists.txt index 3d60985e..3ff1c779 100644 --- a/plover/CMakeLists.txt +++ b/plover/CMakeLists.txt @@ -8,47 +8,76 @@ set (PLOVER_GENERATED_H_DIR ${CMAKE_SOURCE_DIR}/include/libswiftnav/plover CACHE file(MAKE_DIRECTORY ${PLOVER_GENERATED_H_DIR}) # Find default cabal -find_program(CMAKE_cabal NAMES cabal) +find_program(CMAKE_CABAL NAMES cabal) +# Find plover executable +find_program(CMAKE_PLOVER NAMES plover) if (DEFINED ENV{CABAL}) - message("Saw cabal flag: $ENV{CABAL}") - set (CMAKE_cabal $ENV{CABAL}) + message("Saw CABAL flag: $ENV{CABAL}") + set (CMAKE_CABAL $ENV{CABAL}) else () - message("Environment variable 'CABAL' not set. Defaulting to ${CMAKE_cabal}.") + message("Environment variable 'CABAL' not set. Defaulting to ${CMAKE_CABAL}.") +endif () +if (DEFINED ENV{PLOVER}) + message("Saw PLOVER flag. Using $ENV{PLOVER}") + set (CMAKE_PLOVER $ENV{PLOVER}) +else () + message("Environment variable 'PLOVER' not set. Defaulting to ${CMAKE_PLOVER}.") endif () -set (plover_HS_SRCS - Main.hs - AmbiguityTest.hs +# Haskell files with macro plover definitions +# TODO implement build step +set (PLOVER_HS_SRCS ) -set (plover_SRCS - ${PLOVER_GENERATED_C_DIR}/ambiguity_test.c - CACHE INTERNAL "" +# For each unit in PLOVER_UNITS: +# Expects ${unit}.plv in PLOVER_DIR +# Generates ${unit}.c in PLOVER_GENERATED_C_DIR +# and ${unit}.h in PLOVER_GENERATED_H_DIR +set (PLOVER_UNITS + prelude + qr ) -set (plover_HDRS - ${PLOVER_GENERATED_H_DIR}/ambiguity_test.h - CACHE INTERNAL "" -) +foreach(unit ${PLOVER_UNITS}) + list(APPEND PLOVER_CFILES "${PLOVER_GENERATED_C_DIR}/${unit}.c") + list(APPEND PLOVER_HFILES "${PLOVER_GENERATED_H_DIR}/${unit}.h") + list(APPEND PLOVER_SRC "${PLOVER_DIR}/${unit}.plv") +endforeach(unit) + +# Make available to src/ CMakeLists +set(PLOVER_CFILES ${PLOVER_CFILES} PARENT_SCOPE) +set(PLOVER_HFILES ${PLOVER_HFILES} PARENT_SCOPE) + +# Main project depends on generate. generate depends on generated files. +add_custom_target(generate DEPENDS ${PLOVER_CFILES} ${PLOVER_HFILES}) + +if (CMAKE_CABAL AND CMAKE_PLOVER) + message("\nFound Plover tools.") + # Don't add step if no UNITS are specified + if (PLOVER_CFILES OR PLOVER_HFILES) + message("Adding code generation steps, one for each plv unit.\n\ +Will run if plover subproject is modified and after make clean or clean-generate.") + foreach(unit ${PLOVER_UNITS}) + add_custom_command( + OUTPUT "${PLOVER_GENERATED_C_DIR}/${unit}.c" "${PLOVER_GENERATED_H_DIR}/${unit}.h" + COMMAND ${CMAKE_PLOVER} + -I "${PLOVER_DIR}" + --cdir "${PLOVER_GENERATED_C_DIR}" --hdir "${PLOVER_GENERATED_H_DIR}" + --libprefix "libswiftnav/plover" + "${PLOVER_DIR}/${unit}.plv" + WORKING_DIRECTORY ${PLOVER_DIR} + DEPENDS "${PLOVER_DIR}/${unit}.plv") + endforeach(unit) + endif () + message("") -if (CMAKE_cabal) - message("\nFound Haskell tools. Adding code generation step (to be run if Haskell subproject is modified).\n") - add_custom_command( - OUTPUT ${plover_SRCS} ${plover_HDRS} - COMMAND ${CMAKE_cabal} run "${PLOVER_GENERATED_C_DIR}" "${PLOVER_GENERATED_H_DIR}" - WORKING_DIRECTORY ${PLOVER_DIR} - DEPENDS ${plover_HS_SRCS}) else () - message("\nNo Haskell toolchain (cabal) found. Code generation with Plover is disabled.\n") + message("\nNo Plover toolchain found. Code generation is disabled.\n") endif () -add_custom_target(clean_generate +add_custom_target(clean-generate COMMAND ${CMAKE_COMMAND} -E remove_directory ${PLOVER_DIR}/dist COMMAND ${CMAKE_COMMAND} -E remove ${PLOVER_GENERATED_C_DIR}/* COMMAND ${CMAKE_COMMAND} -E remove ${PLOVER_GENERATED_H_DIR}/* ) - -# Main project depends on generated code -add_custom_target(generate DEPENDS ${plover_SRCS} ${plover_HDRS}) - diff --git a/plover/Main.hs b/plover/Main.hs deleted file mode 100644 index 959768e0..00000000 --- a/plover/Main.hs +++ /dev/null @@ -1,18 +0,0 @@ -{-# LANGUAGE OverloadedStrings #-} -import System.Environment - -import Plover.Types -import Plover.Compile - -import qualified AmbiguityTest - -units :: [CompilationUnit] -units = [AmbiguityTest.cu] - -main = do - args <- getArgs - case args of - [cdir, hdir] -> do - mapM_ (generateMain hdir cdir) units - _ -> error "Requires exactly two arguments: C and H file output directories" - diff --git a/plover/README b/plover/README index a55d77a3..e812c721 100644 --- a/plover/README +++ b/plover/README @@ -1 +1,3 @@ requires installation of https://github.com/swift-nav/plover for code generation + +new plover files should be added to the local CMakeLists.txt file. see `PLOVER_UNITS` diff --git a/plover/libswiftnav-generate.cabal b/plover/libswiftnav-generate.cabal deleted file mode 100644 index 8b0c72a4..00000000 --- a/plover/libswiftnav-generate.cabal +++ /dev/null @@ -1,14 +0,0 @@ --- Initial plover.cabal generated by cabal init. For further --- documentation, see http://haskell.org/cabal/users-guide/ -name: libswiftnav-generate -version: 0.1.0.0 -author: Scott Kovach -maintainer: dskovach@gmail.com -build-type: Simple -cabal-version: >=1.10 - -executable plover - main-is: Main.hs - other-extensions: OverloadedStrings - build-depends: base >=4.7 && <4.8, plover - default-language: Haskell2010 diff --git a/plover/prelude.plv b/plover/prelude.plv new file mode 100644 index 00000000..bc624cfa --- /dev/null +++ b/plover/prelude.plv @@ -0,0 +1,555 @@ +-- prelude.plv +-- The standard library for plover + + +__C__ "#include "; + +--- +--- Integers +--- +__C__ + "\n#include \n\ + \#include \n\ + \#include \n\ + \#ifndef COMMON_INT_TYPES\n\ + \#define COMMON_INT_TYPES\n\ + \/** \\defgroup common_inttypes Integer types\n\ + \ * Specified-width integer type definitions for shorter and nicer code.\n\ + \ *\n\ + \ * These should be used in preference to unspecified width types such as\n\ + \ * `int` which can lead to portability issues between different platforms.\n\ + \ * \\{ */\n\ + \\n\ + \/** Signed 8-bit integer. */\n\ + \typedef int8_t s8;\n\ + \/** Signed 16-bit integer. */\n\ + \typedef int16_t s16;\n\ + \/** Signed 32-bit integer. */\n\ + \typedef int32_t s32;\n\ + \/** Signed 64-bit integer. */\n\ + \typedef int64_t s64;\n\ + \/** Unsigned 8-bit integer. */\n\ + \typedef uint8_t u8;\n\ + \/** Unsigned 16-bit integer. */\n\ + \typedef uint16_t u16;\n\ + \/** Unsigned 32-bit integer. */\n\ + \typedef uint32_t u32;\n\ + \/** Unsigned 64-bit integer. */\n\ + \typedef uint64_t u64;\n\ + \\n\ + \#endif\n\ + \\n\ + \/** \\} */"; + +--- +--- Floats +--- +__C__ + "\n#include "; + +--- +--- stdio.h +--- + +__C__ "#include "; +extern ( + printf (x :: string) __VARARGS__ :: int; +); + + +--- +--- stdlib.h +--- + +__C__ "#include "; +extern ( + struct div_t ( + quot :: int; + rem :: int; + ); + RAND_MAX :: int; + -- skipping __compar_fn_t + abort :: (); + abs (i :: int) :: int; + -- skipping bsearch + div (n :: int) (denom :: int) :: div_t; + -- skipping ldiv + -- skipping qsort + strtol (nptr :: string) (inout endptr :: string) (base :: int) :: int; + strtoul (nptr :: string) (inout endptr :: string) (base :: int) :: u32; -- unsigned long + atol (s :: string) :: int; -- long + atoi (s :: string) :: int; + exit (status :: int) :: (); + -- skipping malloc, free, calloc, realloc + strtod (nptr :: string) (inout endptr :: string) :: double; + atof (s :: string) :: double; + rand () :: int; + srand (seed :: u32) :: (); + rand_r (ctxt :: *u32) :: int; -- argument is unsigned long + + -- non-standard +-- random () :: int; -- long +-- srandom (seed :: u32) :: (); -- u32 is unsigned long +-- random_r (ctxt :: *u32) :: int; -- u32 is unsigned long +-- RANDOM_MAX :: int; +); + +--- +--- math.h +--- + +__C__ "#include "; +extern ( + fpclassify (x :: double) :: int; + FP_INFINITE :: int; + FP_NAN :: int; + FP_NORMAL :: int; + FP_SUBNORMAL :: int; + FP_ZERO :: int; + + isfinite (x :: double) :: bool; + isgreater (x :: double) (y :: double) :: bool; + isgreaterequal (x :: double) (y :: double) :: bool; + isinf (x :: double) :: bool; + isless (x :: double) (y :: double) :: bool; + islessequal (x :: double) (y :: double) :: bool; + islessgreater (x :: double) (y :: double) :: bool; + isnan (x :: double) :: bool; + isnormal (x :: double) :: bool; + isunordered (x :: double) :: bool; + signbit (x :: double) :: bool; + + M_E :: double; + M_LOG2E :: double; + M_LOG10E :: double; + M_LN2 :: double; + M_LN10 :: double; + M_PI :: double; + M_PI_2 :: double; + M_PI_4 :: double; + M_1_PI :: double; + M_2_PI :: double; + M_2_SQRTPI :: double; + M_SQRT2 :: double; + M_SQRT_1_2 :: double; + + FP_ILOGB0 :: int; + FP_ILOGBNAN :: int; + + acos (x :: double) :: double; + acosf (x :: float) :: float; + acosh (x :: double) :: double; + acoshf (x :: float) :: float; + asin (x :: double) :: double; + asinf (x :: float) :: float; + asinh (x :: double) :: double; + asinhf (x :: float) :: float; + atan (x :: double) :: double; + atan2 (x :: double) (y :: double) :: double; + atan2f (x :: float) (y :: float) :: float; + atanf (x :: float) :: float; + atanh (x :: double) :: double; + atanhf (x :: float) :: float; + cbrt (x :: double) :: double; + cbrtf (x :: float) :: float; + ceil (x :: double) :: double; + ceilf (x :: float) :: float; + copysign (x :: double) (y :: double) :: double; + copysignf (x :: float) (y :: float) :: float; + cos (x :: double) :: double; + cosf (x :: float) :: float; + cosh (x :: double) :: double; + coshf (x :: float) :: float; + erf (x :: double) :: double; + erfc (x :: double) :: double; + erfcf (x :: float) :: float; + erff (x :: float) :: float; + exp (x :: double) :: double; + exp2 (x :: double) :: double; + exp2f (x :: float) :: float; + expf (x :: float) :: float; + expm1 (x :: double) :: double; + expm1f (x :: float) :: float; + fabs (x :: double) :: double; + fabsf (x :: float) :: float; + fdim (x :: double) (y :: double) :: double; + fdimf (x :: float) (y :: float) :: float; + floor (x :: double) :: double; + floorf (x :: float) :: float; + fma (x :: double) (y :: double) (z :: double) :: double; + fmaf (x :: float) (y :: float) (z :: float) :: float; + fmax (x :: double) (y :: double) :: double; + fmaxf (x :: float) (y :: float) :: float; + fmin (x :: double) (y :: double) :: double; + fminf (x :: float) (y :: float) :: float; + fmod (x :: double) (y :: double) :: double; + fmodf (x :: float) (y :: float) :: float; + frexp (x :: double) (out y :: int) :: double; + frexpf (x :: float) (out y :: int) :: float; + hypot (x :: double) (y :: double) :: double; + hypotf (x :: float) (y :: float) :: float; + ilogb (x :: double) :: int; + ilogbf (x :: float) :: int; + j0 (x :: double) :: double; + j1 (x :: double) :: double; + jn (x :: int) (y :: double) :: double; + ldexp (x :: double) (y :: int) :: double; + ldexpf (x :: float) (y :: int) :: float; + lgamma (x :: double) :: double; + lgammaf (x :: float) :: float; + llrint (x :: double) :: s64; + llrintf (x :: float) :: s64; + llround (x :: double) :: s64; + llroundf (x :: float) :: s64; + log (x :: double) :: double; + log10 (x :: double) :: double; + log10f (x :: float) :: float; + log1p (x :: double) :: double; + log1pf (x :: float) :: float; + log2 (x :: double) :: double; + log2f (x :: float) :: float; + logb (x :: double) :: double; + logbf (x :: float) :: float; + logf (x :: float) :: float; + lrint (x :: double) :: s32; + lrintf (x :: float) :: s32; + lround (x :: double) :: s32; + lroundf (x :: float) :: s32; + modf (x :: double) (out y :: double) :: double; + modff (x :: float) (out y :: float) :: float; + nan (x :: string) :: double; + nanf (x :: string) :: float; + nearbyint (x :: double) :: double; + nearbyintf (x :: float) :: float; + nextafter (x :: double) (y :: double) :: double; + nextafterf (x :: float) (y :: float) :: float; + pow (x :: double) (y :: double) :: double; + powf (x :: float) (y :: float) :: float; + remainder (x :: double) (y :: double) :: double; + remainderf (x :: float) (y :: float) :: float; + remquo (x :: double) (y :: double) (out z :: int) :: double; + remquof (x :: float) (y :: float) (out z :: int) :: float; + rint (x :: double) :: double; + rintf (x :: float) :: float; + round (x :: double) :: double; + roundf (x :: float) :: float; + scalbln (x :: double) (y :: s32) :: double; + scalblnf (x :: float) (y :: s32) :: float; + scalbn (x :: double) (y :: int) :: double; + scalbnf (x :: float) (y :: int) :: float; + sin (x :: double) :: double; + sinf (x :: float) :: float; + sinh (x :: double) :: double; + sinhf (x :: float) :: float; + sqrt (x :: double) :: double; + sqrtf (x :: float) :: float; + tan (x :: double) :: double; + tanf (x :: float) :: float; + tanh (x :: double) :: double; + tanhf (x :: float) :: float; + tgamma (x :: double) :: double; + tgammaf (x :: float) :: float; + trunc (x :: double) :: double; + truncf (x :: float) :: float; + y0 (x :: double) :: double; + y1 (x :: double) :: double; + yn (x :: int) (y :: double) :: double; + signgam :: int; +); + +extern ipow (base :: int) (exp :: int) :: int; +__C__ "int ipow(int base, int exp);"; +static __C__ "int ipow(int base, int exp) {\n\ +\ int result = 1;\n\ +\ while (exp) {\n\ +\ if (exp & 1) result *= base;\n\ +\ exp >>= 1;\n\ +\ base *= base;\n\ +\ }\n\ +\ return result;\n\ +\}"; +extern dipow (base :: double) (exp :: int) :: double; +__C__ "double dipow(double base, int exp);"; +static __C__ "double dipow(double base, int exp) {\n\ +\ double result = 1;\n\ +\ while (exp) {\n\ +\ if (exp & 1) result *= base;\n\ +\ exp >>= 1;\n\ +\ base *= base;\n\ +\ }\n\ +\ return result;\n\ +\}"; + + +-- Generates a uniform random double from 0 to 1, saturating all +-- mantissa bits. bits. Computes it by taking a random double in +-- [1,2) then subtracting 1. +extern rand_uniform() :: double; +__C__ "double rand_uniform(void);"; +static __C__ "double rand_uniform(void) {\n\ +\ union { double d; u64 i; } di;\n\ +\ di.i = 0;\n\ +\ const int width = 32 - __builtin_clz((u32)RAND_MAX);\n\ +\ for (int j = 0; j < DBL_MANT_DIG; j += width) {\n\ +\ di.i = (di.i << width) | rand();\n\ +\ }\n\ +\ di.i = (di.i << (64 - DBL_MANT_DIG)) >> (64 - DBL_MANT_DIG);\n\ +\ di.i |= 0x3ff0000000000000; // zero exponent\n\ +\ return di.d-1;\n\ +\}"; + +-- Computes a random normally-distributed number using the Box-Muller +-- transform. +-- Source: http://www.design.caltech.edu/erik/Misc/Gaussian.html +rand_normal() :: double + := (x1 :: double; x2 :: double; + w :: double; + while (x1 <- 2 * rand_uniform() - 1.0; + x2 <- 2 * rand_uniform() - 1.0; + w <- x1 ^ 2 + x2 ^ 2; + w >= 1.0); + w <- sqrt( -2 * log w / w ); + return x1 * w; + -- waste x2 * w; + ); + +--- +--- Vector stuff +--- +static __C__ "#define MATRIX_EPSILON (1e-60)"; +extern MATRIX_EPSILON :: double; + + +norm {n} (v :: double[n]) :: double + := sqrt (v * v); +normalize {n} (v :: double[n]) :: double[n] + := v / norm v; + +print_vec {n} (v :: double[n]) :: () := ( + printf "vec("; + for i in n -> ( + if i > 0 then printf "," else 0; + printf "% 12lf" v[i]; + ); + printf ")\n"; + (); +); + +print_mat {n,m} (A :: double[n,m]) :: () := ( + printf "mat("; + for i in n -> ( + if i > 0 then printf ";\n " else 0; + for j in m -> ( + if j > 0 then printf "," else 0; + printf "% 12lf" A[i,j]; + ); + ); + printf ")\n"; + (); +); + + +matrix_inv {n} (A :: double[n,n]) (out B :: double[n, n]) :: int + := specialize n ( + 0 -> 0; -- then B is already set. it's technically an inverse + 1 -> inv1 A (out B); + 2 -> inv2 A (out B); + 3 -> inv3 A (out B); + 4 -> inv4 A (out B); + _ -> gen_inv_qr A (out B); + ); + +det {n} (A :: double[n,n]) :: double + := specialize n ( + 0 -> 1; + 1 -> A[0,0]; + 2 -> det2 A; + 3 -> det3 A; + 4 -> det4 A; + _ -> gen_det_qr A; + ); + +static ( + inv1 (A :: double[1,1]) (out B :: double[1,1]) :: int + := ( if fabs A[0,0] < MATRIX_EPSILON then return -1; + B <- mat( 1 / A[0, 0] ); + return 0; + ); + det2 (A :: double[2,2]) :: double + := A[0,0] * A[1,1] - A[0,1] * A[1,0]; + inv2 (A :: double[2,2]) (out B :: double[2,2]) :: int + := ( d := det2 A; + if fabs d < MATRIX_EPSILON then return -1; + + B <- mat( A[1,1] / d, -A[0,1] / d; + -A[1,0] / d, A[0,0] / d); + + return 0; + ); + det3 (A :: double[3,3]) :: double + := -A[1,0] * (A[0,1] * A[2,2] - A[0,2] * A[2,1]) + +A[1,1] * (A[0,0] * A[2,2] - A[0,2] * A[2,0]) + -A[1,2] * (A[0,0] * A[2,1] - A[0,1] * A[2,0]); + inv3 (A :: double[3,3]) (out B :: double[3,3]) :: int + := ( d := det3 A; + if fabs d < MATRIX_EPSILON then return -1; + + B[0,0] <- (A[1,1]*A[2,2]-A[1,2]*A[2,1])/d; + B[1,0] <- -(A[1,0]*A[2,2]-A[1,2]*A[2,0])/d; + B[2,0] <- (A[1,0]*A[2,1]-A[1,1]*A[2,0])/d; + + B[0,1] <- -(A[0,1]*A[2,2]-A[0,2]*A[2,1])/d; + B[1,1] <- (A[0,0]*A[2,2]-A[0,2]*A[2,0])/d; + B[2,1] <- -(A[0,0]*A[2,1]-A[0,1]*A[2,0])/d; + + B[0,2] <- (A[0,1]*A[1,2]-A[0,2]*A[1,1])/d; + B[1,2] <- -(A[0,0]*A[1,2]-A[0,2]*A[1,0])/d; + B[2,2] <- (A[0,0]*A[1,1]-A[0,1]*A[1,0])/d; + + return 0; + ); + + det4 (A :: double[4,4]) :: double + := A[1,0]*(A[2,1]*(A[0,2]*A[3,3]-A[0,3]*A[3,2]) + -A[2,2]*(A[0,1]*A[3,3]-A[0,3]*A[3,1]) + +A[2,3]*(A[0,1]*A[3,2]-A[0,2]*A[3,1])) + -A[1,1]*(A[2,0]*(A[0,2]*A[3,3]-A[0,3]*A[3,2]) + -A[2,2]*(A[0,0]*A[3,3]-A[0,3]*A[3,0]) + +A[2,3]*(A[0,0]*A[3,2]-A[0,2]*A[3,0])) + +A[1,2]*(A[2,0]*(A[0,1]*A[3,3]-A[0,3]*A[3,1]) + -A[2,1]*(A[0,0]*A[3,3]-A[0,3]*A[3,0]) + +A[2,3]*(A[0,0]*A[3,1]-A[0,1]*A[3,0])) + -A[1,3]*(A[2,0]*(A[0,1]*A[3,2]-A[0,2]*A[3,1]) + -A[2,1]*(A[0,0]*A[3,2]-A[0,2]*A[3,0]) + +A[2,2]*(A[0,0]*A[3,1]-A[0,1]*A[3,0])); + + inv4 (A :: double[4,4]) (out B :: double[4,4]) :: int + := ( d := det4 A; + if fabs d < MATRIX_EPSILON then return -1; + + B[0,0] <- (-A[2,1]*(A[1,2]*A[3,3]-A[1,3]*A[3,2]) + +A[2,2]*(A[1,1]*A[3,3]-A[1,3]*A[3,1]) + -A[2,3]*(A[1,1]*A[3,2]-A[1,2]*A[3,1]))/d; + B[1,0] <- -(-A[2,0]*(A[1,2]*A[3,3]-A[1,3]*A[3,2]) + +A[2,2]*(A[1,0]*A[3,3]-A[1,3]*A[3,0]) + -A[2,3]*(A[1,0]*A[3,2]-A[1,2]*A[3,0]))/d; + B[2,0] <- (-A[2,0]*(A[1,1]*A[3,3]-A[1,3]*A[3,1]) + +A[2,1]*(A[1,0]*A[3,3]-A[1,3]*A[3,0]) + -A[2,3]*(A[1,0]*A[3,1]-A[1,1]*A[3,0]))/d; + B[3,0] <- -(-A[2,0]*(A[1,1]*A[3,2]-A[1,2]*A[3,1]) + +A[2,1]*(A[1,0]*A[3,2]-A[1,2]*A[3,0]) + -A[2,2]*(A[1,0]*A[3,1]-A[1,1]*A[3,0]))/d; + + B[0,1] <- -(-A[2,1]*(A[0,2]*A[3,3]-A[0,3]*A[3,2]) + +A[2,2]*(A[0,1]*A[3,3]-A[0,3]*A[3,1]) + -A[2,3]*(A[0,1]*A[3,2]-A[0,2]*A[3,1]))/d; + B[1,1] <- (-A[2,0]*(A[0,2]*A[3,3]-A[0,3]*A[3,2]) + +A[2,2]*(A[0,0]*A[3,3]-A[0,3]*A[3,0]) + -A[2,3]*(A[0,0]*A[3,2]-A[0,2]*A[3,0]))/d; + B[2,1] <- -(-A[2,0]*(A[0,1]*A[3,3]-A[0,3]*A[3,1]) + +A[2,1]*(A[0,0]*A[3,3]-A[0,3]*A[3,0]) + -A[2,3]*(A[0,0]*A[3,1]-A[0,1]*A[3,0]))/d; + B[3,1] <- (-A[2,0]*(A[0,1]*A[3,2]-A[0,2]*A[3,1]) + +A[2,1]*(A[0,0]*A[3,2]-A[0,2]*A[3,0]) + -A[2,2]*(A[0,0]*A[3,1]-A[0,1]*A[3,0]))/d; + + B[0,2] <- (-A[1,1]*(A[0,2]*A[3,3]-A[0,3]*A[3,2]) + +A[1,2]*(A[0,1]*A[3,3]-A[0,3]*A[3,1]) + -A[1,3]*(A[0,1]*A[3,2]-A[0,2]*A[3,1]))/d; + B[1,2] <- -(-A[1,0]*(A[0,2]*A[3,3]-A[0,3]*A[3,2]) + +A[1,2]*(A[0,0]*A[3,3]-A[0,3]*A[3,0]) + -A[1,3]*(A[0,0]*A[3,2]-A[0,2]*A[3,0]))/d; + B[2,2] <- (-A[1,0]*(A[0,1]*A[3,3]-A[0,3]*A[3,1]) + +A[1,1]*(A[0,0]*A[3,3]-A[0,3]*A[3,0]) + -A[1,3]*(A[0,0]*A[3,1]-A[0,1]*A[3,0]))/d; + B[3,2] <- -(-A[1,0]*(A[0,1]*A[3,2]-A[0,2]*A[3,1]) + +A[1,1]*(A[0,0]*A[3,2]-A[0,2]*A[3,0]) + -A[1,2]*(A[0,0]*A[3,1]-A[0,1]*A[3,0]))/d; + + B[0,3] <- -(-A[1,1]*(A[0,2]*A[2,3]-A[0,3]*A[2,2]) + +A[1,2]*(A[0,1]*A[2,3]-A[0,3]*A[2,1]) + -A[1,3]*(A[0,1]*A[2,2]-A[0,2]*A[2,1]))/d; + B[1,3] <- (-A[1,0]*(A[0,2]*A[2,3]-A[0,3]*A[2,2]) + +A[1,2]*(A[0,0]*A[2,3]-A[0,3]*A[2,0]) + -A[1,3]*(A[0,0]*A[2,2]-A[0,2]*A[2,0]))/d; + B[2,3] <- -(-A[1,0]*(A[0,1]*A[2,3]-A[0,3]*A[2,1]) + +A[1,1]*(A[0,0]*A[2,3]-A[0,3]*A[2,0]) + -A[1,3]*(A[0,0]*A[2,1]-A[0,1]*A[2,0]))/d; + B[3,3] <- (-A[1,0]*(A[0,1]*A[2,2]-A[0,2]*A[2,1]) + +A[1,1]*(A[0,0]*A[2,2]-A[0,2]*A[2,0]) + -A[1,2]*(A[0,0]*A[2,1]-A[0,1]*A[2,0]))/d; + + return 0; + ); + + givens (a :: double) (b :: double) :: double[2,2] := ( + c := 0.0; + s := 0.0; + if b == 0 then ( + c <- 1; s <- 0 + ) else if fabs b > fabs a then ( + tau := -a/b; s <- 1/sqrt(1+tau*tau); c <- s*tau + ) else ( + tau := -b/a; c <- 1/sqrt(1+tau*tau); s <- c*tau; + ); + mat( c, s ; + -s, c ); + ); + + -- | Compute the determinant by doing part of the QR factorization, + -- then multiplying the elements of the diagonal. + gen_det_qr {n} (U :: double[n,n]) :: double := ( + A := U; + -- do QR factorization + for j in 1 .. n, + i in n .. j+1 : -1 -> ( + rot := givens A[i-2,j-1] A[i-1,j-1]; + -- Rotate one column at a time + for k in j .. n -> ( + v := A[i-2 .. i-1, k-1]; + A[i-2 .. i-1, k-1] <- rot^T * v; + ); + ); + -- multiply diagonal since now A is upper triangular + d := 1.0; + for i in 0:n -> ( + d <- d * A[i,i]; + ); + return d; + ); + + -- | Computes the general inverse of a matrix U by applying the + -- transforms from the QR decomposition to an identity matrix + gen_inv_qr {n} (U :: double[n,n]) (out B :: double[n,n]) :: int := ( + -- Initialize B to identity matrix + A := U; + B <- vec i in n, j in n -> if i == j then 1 else 0; + -- do QR factorization + for j in 1 .. n, + i in n .. j+1 : -1 -> ( + rot := givens A[i-2,j-1] A[i-1,j-1]; + -- Rotate one column at a time + for k in j .. n -> ( + v := A[i-2 .. i-1, k-1]; + A[i-2 .. i-1, k-1] <- rot^T * v; + ); + for k in 1 .. n -> ( + w := B[i-2 .. i-1, k-1]; + B[i-2 .. i-1, k-1] <- rot^T * w; + ); + ); + -- Back substitution + for i in 0:n -> ( + if A[i,i] == 0 then return -1; + scale := A[i,i]; + A[i] <- A[i] / scale; + B[i] <- B[i] / scale; + for j in 0:i -> ( + c := A[j,i]; + A[j] <- A[j] - c * A[i]; + B[j] <- B[j] - c * B[i]; + ); + ); + return 0; + ); +); diff --git a/plover/qr.plv b/plover/qr.plv new file mode 100644 index 00000000..f1fce796 --- /dev/null +++ b/plover/qr.plv @@ -0,0 +1,82 @@ +import prelude; + +static givens (a :: double) (b :: double) :: double[2,2] := ( + c :: double; + s :: double; + if b == 0 then ( + c <- 1; s <- 0 + ) else if fabs b > fabs a then ( + tau := -a/b; s <- 1/sqrt(1+tau*tau); c <- s*tau + ) else ( + tau := -b/a; c <- 1/sqrt(1+tau*tau); s <- c*tau; + ); + + mat( c, s ; + -s, c ); +); + +-- Assumes m >= n +-- See "Matrix Computations" 4th ed. Golub and Van Loan +qr_solve {m, n} + (inout A :: double[m, n]) + (inout b :: double[m]) + + (out solution :: double[n]) + (out residual :: double) + + :: s8 := ( + + qr_update (inout b) (inout A); + + -- A is now upper triangular; backsolve it into b + code := backsolve A[0:n, 0:n] (inout b[0:n]); + + -- Solution stored in first n elements + solution <- b[0:n]; + + -- Norm of error = norm of last m-n elements + residual <- norm b[n:m]; + + return code; +); + +qr_update {m, n} + (inout b :: double[m]) + (inout A :: double[m, n]) + :: Void := ( + + for j in 1 .. n, + i in m .. j+1 : -1 -> ( + + -- Givens rotation + rot := givens A[i-2,j-1] A[i-1,j-1]; + -- Rotate one column at a time + for k in j..n -> ( + v := A[i-2 .. i-1, k-1]; + A[i-2 .. i-1, k-1] <- rot^T * v; + ); + + -- Rotate b vector + v := b[i-2 .. i-1]; + b[i-2 .. i-1] <- rot^T * v; + + ); +); + +-- Back substitution for upper triangular U +static backsolve {n} + (U :: double[n,n]) + (inout b :: double[n]) + :: s8 := ( + for i in 0:n -> + if U[i,i] == 0 then + return -1; + + b[n-1] <- b[n-1]/U[n-1, n-1]; + + for i in n-1 .. 1 : -1 -> ( + b[i-1] <- (b[i-1] - U[i-1, i : n] * b[i : n]) / U[i-1, i-1]; + ); + + return 0; +); diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 3258c1f5..d1ad5dce 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -12,8 +12,8 @@ include_directories("${PROJECT_SOURCE_DIR}/libfec/include") include_directories("${PROJECT_SOURCE_DIR}/include") -set_source_files_properties(${plover_SRCS} PROPERTIES GENERATED TRUE) -set_source_files_properties(${plover_HDRS} PROPERTIES GENERATED TRUE) +set_source_files_properties(${PLOVER_CFILES} PROPERTIES GENERATED TRUE) +set_source_files_properties(${PLOVER_HFILES} PROPERTIES GENERATED TRUE) set(libswiftnav_SRCS logging.c @@ -46,7 +46,7 @@ set(libswiftnav_SRCS ionosphere.c bit_sync.c cnav_msg.c - ${plover_SRCS} + ${PLOVER_CFILES} CACHE INTERNAL "" ) diff --git a/src/plover/ambiguity_test.c b/src/plover/ambiguity_test.c deleted file mode 100644 index cba8aa49..00000000 --- a/src/plover/ambiguity_test.c +++ /dev/null @@ -1,9 +0,0 @@ -#include - -#include - -int hello_world(void) -{ - printf("hello world\n"); - return 0; -} From 754963ad1bff89d8df96bccb48bda8516461d9e9 Mon Sep 17 00:00:00 2001 From: Scott Kovach Date: Mon, 28 Mar 2016 16:48:50 -0700 Subject: [PATCH 2/3] add generated code --- include/libswiftnav/plover/prelude.h | 56 +++ include/libswiftnav/plover/qr.h | 10 + src/plover/prelude.c | 643 +++++++++++++++++++++++++++ src/plover/qr.c | 123 +++++ 4 files changed, 832 insertions(+) create mode 100644 include/libswiftnav/plover/prelude.h create mode 100644 include/libswiftnav/plover/qr.h create mode 100644 src/plover/prelude.c create mode 100644 src/plover/qr.c diff --git a/include/libswiftnav/plover/prelude.h b/include/libswiftnav/plover/prelude.h new file mode 100644 index 00000000..95be5c82 --- /dev/null +++ b/include/libswiftnav/plover/prelude.h @@ -0,0 +1,56 @@ +#ifndef PLOVER_GENERATED_prelude +#define PLOVER_GENERATED_prelude + + +#include + +#include +#include +#include +#ifndef COMMON_INT_TYPES +#define COMMON_INT_TYPES +/** \defgroup common_inttypes Integer types + * Specified-width integer type definitions for shorter and nicer code. + * + * These should be used in preference to unspecified width types such as + * `int` which can lead to portability issues between different platforms. + * \{ */ + +/** Signed 8-bit integer. */ +typedef int8_t s8; +/** Signed 16-bit integer. */ +typedef int16_t s16; +/** Signed 32-bit integer. */ +typedef int32_t s32; +/** Signed 64-bit integer. */ +typedef int64_t s64; +/** Unsigned 8-bit integer. */ +typedef uint8_t u8; +/** Unsigned 16-bit integer. */ +typedef uint16_t u16; +/** Unsigned 32-bit integer. */ +typedef uint32_t u32; +/** Unsigned 64-bit integer. */ +typedef uint64_t u64; + +#endif + +/** \} */ + +#include +#include +#include +#include +int ipow(int base, int exp); +double dipow(double base, int exp); +double rand_uniform(void); +double rand_normal (void); +double norm (const s32 n, const double * v); +void normalize (const s32 n, const double * v, double * result); +void print_vec (const s32 n, const double * v); +void print_mat (const s32 n, const s32 m, const double * A); +s32 matrix_inv (const s32 n, const double * A, double * B); +double det (const s32 n, const double * A); + + +#endif /* PLOVER_GENERATED_prelude */ diff --git a/include/libswiftnav/plover/qr.h b/include/libswiftnav/plover/qr.h new file mode 100644 index 00000000..7d55ea84 --- /dev/null +++ b/include/libswiftnav/plover/qr.h @@ -0,0 +1,10 @@ +#ifndef PLOVER_GENERATED_qr +#define PLOVER_GENERATED_qr + +#include "libswiftnav/plover/prelude.h" + +s8 qr_solve (const s32 m, const s32 n, double * A, double * b, double * solution, double * const residual); +void qr_update (const s32 m, const s32 n, double * b, double * A); + + +#endif /* PLOVER_GENERATED_qr */ diff --git a/src/plover/prelude.c b/src/plover/prelude.c new file mode 100644 index 00000000..c07c4135 --- /dev/null +++ b/src/plover/prelude.c @@ -0,0 +1,643 @@ +#include "libswiftnav/plover/prelude.h" + + +int ipow(int base, int exp) { + int result = 1; + while (exp) { + if (exp & 1) result *= base; + exp >>= 1; + base *= base; + } + return result; +} +double dipow(double base, int exp) { + double result = 1; + while (exp) { + if (exp & 1) result *= base; + exp >>= 1; + base *= base; + } + return result; +} +double rand_uniform(void) { + union { double d; u64 i; } di; + di.i = 0; + const int width = 32 - __builtin_clz((u32)RAND_MAX); + for (int j = 0; j < DBL_MANT_DIG; j += width) { + di.i = (di.i << width) | rand(); + } + di.i = (di.i << (64 - DBL_MANT_DIG)) >> (64 - DBL_MANT_DIG); + di.i |= 0x3ff0000000000000; // zero exponent + return di.d-1; +} +#define MATRIX_EPSILON (1e-60) +static s32 inv1 (const double * A, double * B); +static double det2 (const double * A); +static s32 inv2 (const double * A, double * B); +static double det3 (const double * A); +static s32 inv3 (const double * A, double * B); +static double det4 (const double * A); +static s32 inv4 (const double * A, double * B); +static void givens (const double a, const double b, double * result); +static double gen_det_qr (const s32 n, const double * U); +static s32 gen_inv_qr (const s32 n, const double * U, double * B); +double rand_normal (void) +{ + double x1; + double x2; + double w; + + do { + x1 = 2 * rand_uniform() - 1.0; + x2 = 2 * rand_uniform() - 1.0; + w = x1 * x1 + x2 * x2; + } while(1.0 <= w); + w = sqrt(-(2 * log(w) / w)); + return x1 * w; +} +double norm (const s32 n, const double * v) +{ + double sum = 0; + + for (s32 idx = 0; idx < n; idx++) { + sum += v[idx] * v[idx]; + } + return sqrt(sum); +} +void normalize (const s32 n, const double * v, double * result) +{ + double tmp; + + tmp = norm(n, v); + for (s32 idx = 0; idx < n; idx++) { + result[idx] = v[idx] / tmp; + } +} +void print_vec (const s32 n, const double * v) +{ + printf("vec("); + for (s32 i = 0, idx = 0; idx < n; i += 1, idx++) { + if (0 < i) { + printf(","); + } + printf("% 12lf", v[i]); + } + printf(")\n"); +} +void print_mat (const s32 n, const s32 m, const double * A) +{ + printf("mat("); + for (s32 i = 0, idx = 0; idx < n; i += 1, idx++) { + if (0 < i) { + printf(";\n "); + } + for (s32 j = 0, idx2 = 0; idx2 < m; j += 1, idx2++) { + if (0 < j) { + printf(","); + } + printf("% 12lf", A[m * i + j]); + } + } + printf(")\n"); +} +s32 matrix_inv (const s32 n, const double * A, double * B) +{ + s32 tmp; + + switch (n) { + + + case 0: + { + tmp = 0; + break; + } + + + case 1: + { + tmp = inv1(A, B); + break; + } + + + case 2: + { + tmp = inv2(A, B); + break; + } + + + case 3: + { + tmp = inv3(A, B); + break; + } + + + case 4: + { + tmp = inv4(A, B); + break; + } + + default: + { + tmp = gen_inv_qr(n, A, B); + } + } + return tmp; +} +double det (const s32 n, const double * A) +{ + double tmp; + + switch (n) { + + + case 0: + { + tmp = 1; + break; + } + + + case 1: + { + tmp = A[n * 0]; + break; + } + + + case 2: + { + tmp = det2(A); + break; + } + + + case 3: + { + tmp = det3(A); + break; + } + + + case 4: + { + tmp = det4(A); + break; + } + + default: + { + tmp = gen_det_qr(n, A); + } + } + return tmp; +} +s32 inv1 (const double * A, double * B) +{ + if (fabs(A[0]) < MATRIX_EPSILON) { + return -1; + } + B[0] = 1 / A[0]; + return 0; +} +double det2 (const double * A) +{ + return A[2 * 0] * A[2 * 1 + 1] - A[2 * 0 + 1] * A[2 * 1]; +} +s32 inv2 (const double * A, double * B) +{ + double d; + + d = det2(A); + if (fabs(d) < MATRIX_EPSILON) { + return -1; + } + B[2 * 0] = A[2 * 1 + 1] / d; + B[2 * 0 + 1] = -(A[2 * 0 + 1] / d); + B[2 * 1] = -(A[2 * 1] / d); + B[2 * 1 + 1] = A[2 * 0] / d; + return 0; +} +double det3 (const double * A) +{ + return -(A[3 * 1] * (A[3 * 0 + 1] * A[3 * 2 + 2] - A[3 * 0 + 2] * A[3 * 2 + 1])) + A[3 * 1 + 1] * (A[3 * 0] * A[3 * + 2 + + 2] - + A[3 * 0 + 2] * + A[3 * 2]) - A[3 * + 1 + + 2] * + (A[3 * 0] * A[3 * 2 + 1] - A[3 * 0 + 1] * A[3 * 2]); +} +s32 inv3 (const double * A, double * B) +{ + double d; + + d = det3(A); + if (fabs(d) < MATRIX_EPSILON) { + return -1; + } + B[3 * 0] = (A[3 * 1 + 1] * A[3 * 2 + 2] - A[3 * 1 + 2] * A[3 * 2 + 1]) / d; + B[3 * 1] = -((A[3 * 1] * A[3 * 2 + 2] - A[3 * 1 + 2] * A[3 * 2]) / d); + B[3 * 2] = (A[3 * 1] * A[3 * 2 + 1] - A[3 * 1 + 1] * A[3 * 2]) / d; + B[3 * 0 + 1] = -((A[3 * 0 + 1] * A[3 * 2 + 2] - A[3 * 0 + 2] * A[3 * 2 + 1]) / d); + B[3 * 1 + 1] = (A[3 * 0] * A[3 * 2 + 2] - A[3 * 0 + 2] * A[3 * 2]) / d; + B[3 * 2 + 1] = -((A[3 * 0] * A[3 * 2 + 1] - A[3 * 0 + 1] * A[3 * 2]) / d); + B[3 * 0 + 2] = (A[3 * 0 + 1] * A[3 * 1 + 2] - A[3 * 0 + 2] * A[3 * 1 + 1]) / d; + B[3 * 1 + 2] = -((A[3 * 0] * A[3 * 1 + 2] - A[3 * 0 + 2] * A[3 * 1]) / d); + B[3 * 2 + 2] = (A[3 * 0] * A[3 * 1 + 1] - A[3 * 0 + 1] * A[3 * 1]) / d; + return 0; +} +double det4 (const double * A) +{ + return A[4 * 1] * (A[4 * 2 + 1] * (A[4 * 0 + 2] * A[4 * 3 + 3] - A[4 * 0 + 3] * A[4 * 3 + 2]) - A[4 * 2 + 2] * + (A[4 * 0 + 1] * A[4 * 3 + 3] - A[4 * 0 + 3] * A[4 * 3 + 1]) + A[4 * 2 + 3] * (A[4 * 0 + 1] * + A[4 * 3 + 2] - + A[4 * 0 + 2] * + A[4 * 3 + 1])) - + A[4 * 1 + 1] * (A[4 * 2] * (A[4 * 0 + 2] * A[4 * 3 + 3] - A[4 * 0 + 3] * A[4 * 3 + 2]) - A[4 * 2 + 2] * (A[4 * + 0] * + A[4 * + 3 + + 3] - + A[4 * + 0 + + 3] * + A[4 * + 3]) + + A[4 * 2 + 3] * (A[4 * 0] * A[4 * 3 + 2] - A[4 * 0 + 2] * A[4 * 3])) + A[4 * 1 + 2] * (A[4 * 2] * + (A[4 * 0 + + 1] * + A[4 * 3 + + 3] - + A[4 * 0 + + 3] * + A[4 * 3 + + 1]) - + A[4 * 2 + + 1] * + (A[4 * + 0] * + A[4 * 3 + + 3] - + A[4 * 0 + + 3] * + A[4 * + 3]) + + A[4 * 2 + + 3] * + (A[4 * + 0] * + A[4 * 3 + + 1] - + A[4 * 0 + + 1] * + A[4 * + 3])) - + A[4 * 1 + 3] * (A[4 * 2] * (A[4 * 0 + 1] * A[4 * 3 + 2] - A[4 * 0 + 2] * A[4 * 3 + 1]) - A[4 * 2 + 1] * (A[4 * + 0] * + A[4 * + 3 + + 2] - + A[4 * + 0 + + 2] * + A[4 * + 3]) + + A[4 * 2 + 2] * (A[4 * 0] * A[4 * 3 + 1] - A[4 * 0 + 1] * A[4 * 3])); +} +s32 inv4 (const double * A, double * B) +{ + double d; + + d = det4(A); + if (fabs(d) < MATRIX_EPSILON) { + return -1; + } + B[4 * 0] = (-(A[4 * 2 + 1] * (A[4 * 1 + 2] * A[4 * 3 + 3] - A[4 * 1 + 3] * A[4 * 3 + 2])) + A[4 * 2 + 2] * (A[4 * + 1 + + 1] * + A[4 * + 3 + + 3] - + A[4 * + 1 + + 3] * + A[4 * + 3 + + 1]) - + A[4 * 2 + 3] * (A[4 * 1 + 1] * A[4 * 3 + 2] - A[4 * 1 + 2] * A[4 * 3 + 1])) / d; + B[4 * 1] = -((-(A[4 * 2] * (A[4 * 1 + 2] * A[4 * 3 + 3] - A[4 * 1 + 3] * A[4 * 3 + 2])) + A[4 * 2 + 2] * (A[4 * 1] * + A[4 * 3 + + 3] - + A[4 * 1 + + 3] * + A[4 * + 3]) - + A[4 * 2 + 3] * (A[4 * 1] * A[4 * 3 + 2] - A[4 * 1 + 2] * A[4 * 3])) / d); + B[4 * 2] = (-(A[4 * 2] * (A[4 * 1 + 1] * A[4 * 3 + 3] - A[4 * 1 + 3] * A[4 * 3 + 1])) + A[4 * 2 + 1] * (A[4 * 1] * + A[4 * 3 + + 3] - A[4 * + 1 + + 3] * + A[4 * 3]) - + A[4 * 2 + 3] * (A[4 * 1] * A[4 * 3 + 1] - A[4 * 1 + 1] * A[4 * 3])) / d; + B[4 * 3] = -((-(A[4 * 2] * (A[4 * 1 + 1] * A[4 * 3 + 2] - A[4 * 1 + 2] * A[4 * 3 + 1])) + A[4 * 2 + 1] * (A[4 * 1] * + A[4 * 3 + + 2] - + A[4 * 1 + + 2] * + A[4 * + 3]) - + A[4 * 2 + 2] * (A[4 * 1] * A[4 * 3 + 1] - A[4 * 1 + 1] * A[4 * 3])) / d); + B[4 * 0 + 1] = -((-(A[4 * 2 + 1] * (A[4 * 0 + 2] * A[4 * 3 + 3] - A[4 * 0 + 3] * A[4 * 3 + 2])) + A[4 * 2 + 2] * + (A[4 * 0 + 1] * A[4 * 3 + 3] - A[4 * 0 + 3] * A[4 * 3 + 1]) - A[4 * 2 + 3] * (A[4 * 0 + 1] * A[4 * + 3 + + 2] - + A[4 * 0 + 2] * A[4 * + 3 + + 1])) / + d); + B[4 * 1 + 1] = (-(A[4 * 2] * (A[4 * 0 + 2] * A[4 * 3 + 3] - A[4 * 0 + 3] * A[4 * 3 + 2])) + A[4 * 2 + 2] * (A[4 * + 0] * + A[4 * + 3 + + 3] - + A[4 * + 0 + + 3] * + A[4 * + 3]) - + A[4 * 2 + 3] * (A[4 * 0] * A[4 * 3 + 2] - A[4 * 0 + 2] * A[4 * 3])) / d; + B[4 * 2 + 1] = -((-(A[4 * 2] * (A[4 * 0 + 1] * A[4 * 3 + 3] - A[4 * 0 + 3] * A[4 * 3 + 1])) + A[4 * 2 + 1] * (A[4 * + 0] * + A[4 * + 3 + + 3] - + A[4 * + 0 + + 3] * + A[4 * + 3]) - + A[4 * 2 + 3] * (A[4 * 0] * A[4 * 3 + 1] - A[4 * 0 + 1] * A[4 * 3])) / d); + B[4 * 3 + 1] = (-(A[4 * 2] * (A[4 * 0 + 1] * A[4 * 3 + 2] - A[4 * 0 + 2] * A[4 * 3 + 1])) + A[4 * 2 + 1] * (A[4 * + 0] * + A[4 * + 3 + + 2] - + A[4 * + 0 + + 2] * + A[4 * + 3]) - + A[4 * 2 + 2] * (A[4 * 0] * A[4 * 3 + 1] - A[4 * 0 + 1] * A[4 * 3])) / d; + B[4 * 0 + 2] = (-(A[4 * 1 + 1] * (A[4 * 0 + 2] * A[4 * 3 + 3] - A[4 * 0 + 3] * A[4 * 3 + 2])) + A[4 * 1 + 2] * + (A[4 * 0 + 1] * A[4 * 3 + 3] - A[4 * 0 + 3] * A[4 * 3 + 1]) - A[4 * 1 + 3] * (A[4 * 0 + 1] * A[4 * + 3 + + 2] - + A[4 * 0 + 2] * A[4 * + 3 + + 1])) / + d; + B[4 * 1 + 2] = -((-(A[4 * 1] * (A[4 * 0 + 2] * A[4 * 3 + 3] - A[4 * 0 + 3] * A[4 * 3 + 2])) + A[4 * 1 + 2] * (A[4 * + 0] * + A[4 * + 3 + + 3] - + A[4 * + 0 + + 3] * + A[4 * + 3]) - + A[4 * 1 + 3] * (A[4 * 0] * A[4 * 3 + 2] - A[4 * 0 + 2] * A[4 * 3])) / d); + B[4 * 2 + 2] = (-(A[4 * 1] * (A[4 * 0 + 1] * A[4 * 3 + 3] - A[4 * 0 + 3] * A[4 * 3 + 1])) + A[4 * 1 + 1] * (A[4 * + 0] * + A[4 * + 3 + + 3] - + A[4 * + 0 + + 3] * + A[4 * + 3]) - + A[4 * 1 + 3] * (A[4 * 0] * A[4 * 3 + 1] - A[4 * 0 + 1] * A[4 * 3])) / d; + B[4 * 3 + 2] = -((-(A[4 * 1] * (A[4 * 0 + 1] * A[4 * 3 + 2] - A[4 * 0 + 2] * A[4 * 3 + 1])) + A[4 * 1 + 1] * (A[4 * + 0] * + A[4 * + 3 + + 2] - + A[4 * + 0 + + 2] * + A[4 * + 3]) - + A[4 * 1 + 2] * (A[4 * 0] * A[4 * 3 + 1] - A[4 * 0 + 1] * A[4 * 3])) / d); + B[4 * 0 + 3] = -((-(A[4 * 1 + 1] * (A[4 * 0 + 2] * A[4 * 2 + 3] - A[4 * 0 + 3] * A[4 * 2 + 2])) + A[4 * 1 + 2] * + (A[4 * 0 + 1] * A[4 * 2 + 3] - A[4 * 0 + 3] * A[4 * 2 + 1]) - A[4 * 1 + 3] * (A[4 * 0 + 1] * A[4 * + 2 + + 2] - + A[4 * 0 + 2] * A[4 * + 2 + + 1])) / + d); + B[4 * 1 + 3] = (-(A[4 * 1] * (A[4 * 0 + 2] * A[4 * 2 + 3] - A[4 * 0 + 3] * A[4 * 2 + 2])) + A[4 * 1 + 2] * (A[4 * + 0] * + A[4 * + 2 + + 3] - + A[4 * + 0 + + 3] * + A[4 * + 2]) - + A[4 * 1 + 3] * (A[4 * 0] * A[4 * 2 + 2] - A[4 * 0 + 2] * A[4 * 2])) / d; + B[4 * 2 + 3] = -((-(A[4 * 1] * (A[4 * 0 + 1] * A[4 * 2 + 3] - A[4 * 0 + 3] * A[4 * 2 + 1])) + A[4 * 1 + 1] * (A[4 * + 0] * + A[4 * + 2 + + 3] - + A[4 * + 0 + + 3] * + A[4 * + 2]) - + A[4 * 1 + 3] * (A[4 * 0] * A[4 * 2 + 1] - A[4 * 0 + 1] * A[4 * 2])) / d); + B[4 * 3 + 3] = (-(A[4 * 1] * (A[4 * 0 + 1] * A[4 * 2 + 2] - A[4 * 0 + 2] * A[4 * 2 + 1])) + A[4 * 1 + 1] * (A[4 * + 0] * + A[4 * + 2 + + 2] - + A[4 * + 0 + + 2] * + A[4 * + 2]) - + A[4 * 1 + 2] * (A[4 * 0] * A[4 * 2 + 1] - A[4 * 0 + 1] * A[4 * 2])) / d; + return 0; +} +void givens (const double a, const double b, double * result) +{ + double c; + + c = 0.0; + + double s; + + s = 0.0; + if (b == 0) { + c = 1; + s = 0; + } else { + if (fabs(a) < fabs(b)) { + double tau; + + tau = -(a / b); + s = 1 / sqrt(1 + tau * tau); + c = s * tau; + } else { + double tau; + + tau = -(b / a); + c = 1 / sqrt(1 + tau * tau); + s = c * tau; + } + } + result[2 * 0] = c; + result[2 * 0 + 1] = s; + result[2 * 1] = -s; + result[2 * 1 + 1] = c; +} +double gen_det_qr (const s32 n, const double * U) +{ + double A [n * n]; + + for (s32 idx = 0; idx < n; idx++) { + for (s32 idx2 = 0; idx2 < n; idx2++) { + A[n * idx + idx2] = U[n * idx + idx2]; + } + } + for (s32 j = 1, idx = 0; idx < n; j += 1, idx++) { + for (s32 i = n, idx2 = 0; idx2 < -j + n; i += -1, idx2++) { + double rot [2 * 2]; + + givens(A[n * (i - 2) + (j - 1)], A[n * (i - 1) + (j - 1)], rot); + for (s32 k = j, idx3 = 0; idx3 < 1 - j + n; k += 1, idx3++) { + double v [2]; + + for (s32 idx4 = 0; idx4 < 2; idx4++) { + v[idx4] = A[n * (i - 2 + idx4) + (k - 1)]; + } + for (s32 idx4 = 0; idx4 < 2; idx4++) { + double sum = 0; + + for (s32 idx5 = 0; idx5 < 2; idx5++) { + sum += rot[2 * idx5 + idx4] * v[idx5]; + } + A[n * (i - 2 + idx4) + (k - 1)] = sum; + } + } + } + } + + double d; + + d = 1.0; + for (s32 i = 0, idx = 0; idx < n; i += 1, idx++) { + d = d * A[n * i + i]; + } + return d; +} +s32 gen_inv_qr (const s32 n, const double * U, double * B) +{ + double A [n * n]; + + for (s32 idx = 0; idx < n; idx++) { + for (s32 idx2 = 0; idx2 < n; idx2++) { + A[n * idx + idx2] = U[n * idx + idx2]; + } + } + for (s32 idx = 0; idx < n; idx++) { + s32 i = idx; + + for (s32 idx2 = 0; idx2 < n; idx2++) { + s32 j = idx2; + s32 loc; + + if (i == j) { + loc = 1; + } else { + loc = 0; + } + B[n * idx + idx2] = loc; + } + } + for (s32 j = 1, idx = 0; idx < n; j += 1, idx++) { + for (s32 i = n, idx2 = 0; idx2 < -j + n; i += -1, idx2++) { + double rot [2 * 2]; + + givens(A[n * (i - 2) + (j - 1)], A[n * (i - 1) + (j - 1)], rot); + for (s32 k = j, idx3 = 0; idx3 < 1 - j + n; k += 1, idx3++) { + double v [2]; + + for (s32 idx4 = 0; idx4 < 2; idx4++) { + v[idx4] = A[n * (i - 2 + idx4) + (k - 1)]; + } + for (s32 idx4 = 0; idx4 < 2; idx4++) { + double sum = 0; + + for (s32 idx5 = 0; idx5 < 2; idx5++) { + sum += rot[2 * idx5 + idx4] * v[idx5]; + } + A[n * (i - 2 + idx4) + (k - 1)] = sum; + } + } + for (s32 k = 1, idx3 = 0; idx3 < n; k += 1, idx3++) { + double w [2]; + + for (s32 idx4 = 0; idx4 < 2; idx4++) { + w[idx4] = B[n * (i - 2 + idx4) + (k - 1)]; + } + for (s32 idx4 = 0; idx4 < 2; idx4++) { + double sum = 0; + + for (s32 idx5 = 0; idx5 < 2; idx5++) { + sum += rot[2 * idx5 + idx4] * w[idx5]; + } + B[n * (i - 2 + idx4) + (k - 1)] = sum; + } + } + } + } + for (s32 i = 0, idx = 0; idx < n; i += 1, idx++) { + if (A[n * i + i] == 0) { + return -1; + } + + double scale; + + scale = A[n * i + i]; + for (s32 idx2 = 0; idx2 < n; idx2++) { + A[n * i + idx2] = A[n * i + idx2] / scale; + } + for (s32 idx2 = 0; idx2 < n; idx2++) { + B[n * i + idx2] = B[n * i + idx2] / scale; + } + for (s32 j = 0, idx2 = 0; idx2 < i; j += 1, idx2++) { + double c; + + c = A[n * j + i]; + for (s32 idx3 = 0; idx3 < n; idx3++) { + A[n * j + idx3] = A[n * j + idx3] - c * A[n * i + idx3]; + } + for (s32 idx3 = 0; idx3 < n; idx3++) { + B[n * j + idx3] = B[n * j + idx3] - c * B[n * i + idx3]; + } + } + } + return 0; +} + diff --git a/src/plover/qr.c b/src/plover/qr.c new file mode 100644 index 00000000..68c9f3c4 --- /dev/null +++ b/src/plover/qr.c @@ -0,0 +1,123 @@ +#include "libswiftnav/plover/qr.h" + + +static void givens (const double a, const double b, double * result); +static s8 backsolve (const s32 n, const double * U, double * b); +void givens (const double a, const double b, double * result) +{ + double c; + double s; + + if (b == 0) { + c = 1; + s = 0; + } else { + if (fabs(a) < fabs(b)) { + double tau; + + tau = -(a / b); + s = 1 / sqrt(1 + tau * tau); + c = s * tau; + } else { + double tau; + + tau = -(b / a); + c = 1 / sqrt(1 + tau * tau); + s = c * tau; + } + } + result[2 * 0] = c; + result[2 * 0 + 1] = s; + result[2 * 1] = -s; + result[2 * 1 + 1] = c; +} +s8 qr_solve (const s32 m, const s32 n, double * A, double * b, double * solution, double * const residual) +{ + qr_update(m, n, b, A); + + s8 code; + double arg [n * n]; + double arg2 [n]; + + for (s32 idx = 0; idx < n; idx++) { + for (s32 idx2 = 0; idx2 < n; idx2++) { + arg[n * idx + idx2] = A[n * idx + idx2]; + } + } + for (s32 idx = 0; idx < n; idx++) { + arg2[idx] = b[idx]; + } + code = backsolve(n, arg, arg2); + for (s32 idx = 0; idx < n; idx++) { + b[idx] = arg2[idx]; + } + for (s32 idx = 0; idx < n; idx++) { + solution[idx] = b[idx]; + } + + double arg3 [m - n]; + + for (s32 idx = 0; idx < m - n; idx++) { + arg3[idx] = b[n + idx]; + } + *residual = norm(m - n, arg3); + return code; +} +void qr_update (const s32 m, const s32 n, double * b, double * A) +{ + for (s32 j = 1, idx = 0; idx < n; j += 1, idx++) { + for (s32 i = m, idx2 = 0; idx2 < -j + m; i += -1, idx2++) { + double rot [2 * 2]; + + givens(A[n * (i - 2) + (j - 1)], A[n * (i - 1) + (j - 1)], rot); + for (s32 k = j, idx3 = 0; idx3 < 1 - j + n; k += 1, idx3++) { + double v [2]; + + for (s32 idx4 = 0; idx4 < 2; idx4++) { + v[idx4] = A[n * (i - 2 + idx4) + (k - 1)]; + } + for (s32 idx4 = 0; idx4 < 2; idx4++) { + double sum = 0; + + for (s32 idx5 = 0; idx5 < 2; idx5++) { + sum += rot[2 * idx5 + idx4] * v[idx5]; + } + A[n * (i - 2 + idx4) + (k - 1)] = sum; + } + } + + double v [2]; + + for (s32 idx3 = 0; idx3 < 2; idx3++) { + v[idx3] = b[i - 2 + idx3]; + } + for (s32 idx3 = 0; idx3 < 2; idx3++) { + double sum = 0; + + for (s32 idx4 = 0; idx4 < 2; idx4++) { + sum += rot[2 * idx4 + idx3] * v[idx4]; + } + b[i - 2 + idx3] = sum; + } + } + } +} +s8 backsolve (const s32 n, const double * U, double * b) +{ + for (s32 i = 0, idx = 0; idx < n; i += 1, idx++) { + if (U[n * i + i] == 0) { + return -1; + } + } + b[n - 1] = b[n - 1] / U[n * (n - 1) + (n - 1)]; + for (s32 i = n - 1, idx = 0; idx < -1 + n; i += -1, idx++) { + double sum = 0; + + for (s32 idx2 = 0; idx2 < -i + n; idx2++) { + sum += U[n * (i - 1) + (i + idx2)] * b[i + idx2]; + } + b[i - 1] = (b[i - 1] - sum) / U[n * (i - 1) + (i - 1)]; + } + return 0; +} + From d570f3a63682a34afec854ffe6dbe9640b6630cf Mon Sep 17 00:00:00 2001 From: Scott Kovach Date: Mon, 28 Mar 2016 16:49:22 -0700 Subject: [PATCH 3/3] add check_qr test --- tests/CMakeLists.txt | 1 + tests/check_main.c | 2 + tests/check_qr.c | 111 +++++++++++++++++++++++++++++++++++++++++++ tests/check_suites.h | 1 + 4 files changed, 115 insertions(+) create mode 100644 tests/check_qr.c diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 69183c2d..18604c3a 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -47,6 +47,7 @@ else (CMAKE_CROSSCOMPILING) check_signal.c check_track.c check_cnav.c + check_qr.c ) target_link_libraries(test_libswiftnav ${TEST_LIBS}) diff --git a/tests/check_main.c b/tests/check_main.c index d52d68f7..9d3c40bb 100644 --- a/tests/check_main.c +++ b/tests/check_main.c @@ -34,6 +34,8 @@ int main(void) srunner_add_suite(sr, track_test_suite()); srunner_add_suite(sr, cnav_test_suite()); + srunner_add_suite(sr, qr_test_suite()); + srunner_set_fork_status(sr, CK_NOFORK); srunner_run_all(sr, CK_NORMAL); number_failed = srunner_ntests_failed(sr); diff --git a/tests/check_qr.c b/tests/check_qr.c new file mode 100644 index 00000000..46daed37 --- /dev/null +++ b/tests/check_qr.c @@ -0,0 +1,111 @@ +#include +#include + +#include + +#include +#include +#include + +#include "check_utils.h" + +START_TEST(test_ok) +{ + double m1[] = {1, 0, 2, + 1, 1, -1, + 0, 0, 1, + 0, 0, 22 }; + double b[] = {5, 4, 1.1, 22}; + double soln[3]; + double residual; + + s8 code = qr_solve(4, 3, m1, b, soln, &residual); + + fail_unless(code == 0, "Solver error code: %d\n", code); + fail_unless(residual < 0.1, "Residual too large: %f\n", residual); +} +END_TEST + +/* Deficient */ +START_TEST(test_bad) +{ + double m1[] = {1, 0, 0, + 0, 1, 0, + 0, 0, 0 }; + double b[] = {1,1,1}; + double soln[3]; + double residual; + + s8 code = qr_solve(3, 3, m1, b, soln, &residual); + + fail_unless(code == -1, "Wrong solver code: %d\n", code); +} +END_TEST + +double randd() { + int max = 22; + return (double)(rand()) / RAND_MAX * max - (double)max / 2; +} + +void test_qr(int rows, int cols) +{ + double mat[rows*cols]; + double vec[cols]; + + for(int r = 0; r < rows; r++) { + for(int c = 0; c < cols; c++) { + mat[r*cols + c] = randd(); + } + } + for(int c = 0; c < cols; c++) { + vec[c] = randd(); + } + + double m1[rows*cols]; + memcpy(m1, mat, sizeof(m1)); + + //printf("%d %d\n", rows, cols); + //print_double_mtx(mat, rows, cols); + + double out[rows]; + matrix_multiply(rows, cols, 1, mat, vec, out); + double soln[cols]; + double residual; + int code = qr_solve(rows, cols, mat, out, soln, &residual); + fail_unless(residual < 0.0000001, "Residual too large: %f\n", residual); + if (!arr_within_epsilon(cols, soln, vec) && code != -1) { + printf("original matrix: "); + print_double_mtx(m1, rows, cols); + printf("reduced R matrix: "); + print_double_mtx(mat, rows, cols); + printf("solution: "); + print_double_mtx(soln, 1, cols); + printf("vec: "); + print_double_mtx(vec, 1, cols); + fail_unless(false, "Solution wrong\n"); + } +} + +/* Performs 10000 trials. */ +START_TEST(test_rand) +{ + srand(0); + for (int i = 0; i < 10000; i++) { + int rows = rand() % 25 + 1; + int cols = rand() % rows + 1; + test_qr(rows, cols); + } +} +END_TEST + +Suite* qr_test_suite(void) +{ + Suite *s = suite_create("Generated QR solver"); + + TCase *tc_core = tcase_create("Core"); + tcase_add_test(tc_core, test_ok); + tcase_add_test(tc_core, test_bad); + tcase_add_test(tc_core, test_rand); + suite_add_tcase(s, tc_core); + return s; +} diff --git a/tests/check_suites.h b/tests/check_suites.h index 71c6ec07..97c1d124 100644 --- a/tests/check_suites.h +++ b/tests/check_suites.h @@ -23,5 +23,6 @@ Suite* ionosphere_suite(void); Suite* signal_test_suite(void); Suite* track_test_suite(void); Suite* cnav_test_suite(void); +Suite* qr_test_suite(void); #endif /* CHECK_SUITES_H */