diff options
Diffstat (limited to 'common/math_util.c')
-rw-r--r-- | common/math_util.c | 110 |
1 files changed, 52 insertions, 58 deletions
diff --git a/common/math_util.c b/common/math_util.c index ff305438eb..c0a279b825 100644 --- a/common/math_util.c +++ b/common/math_util.c @@ -1,4 +1,4 @@ -/* Copyright 2014 The Chromium OS Authors. All rights reserved. +/* Copyright 2014 The ChromiumOS Authors * Use of this source code is governed by a BSD-style license that can be * found in the LICENSE file. */ @@ -11,18 +11,18 @@ #include "util.h" /* For cosine lookup table, define the increment and the size of the table. */ -#define COSINE_LUT_INCR_DEG 5 -#define COSINE_LUT_SIZE ((180 / COSINE_LUT_INCR_DEG) + 1) +#define COSINE_LUT_INCR_DEG 5 +#define COSINE_LUT_SIZE ((180 / COSINE_LUT_INCR_DEG) + 1) /* Lookup table for the value of cosine from 0 degrees to 180 degrees. */ static const fp_t cos_lut[] = { - FLOAT_TO_FP( 1.00000), FLOAT_TO_FP( 0.99619), FLOAT_TO_FP( 0.98481), - FLOAT_TO_FP( 0.96593), FLOAT_TO_FP( 0.93969), FLOAT_TO_FP( 0.90631), - FLOAT_TO_FP( 0.86603), FLOAT_TO_FP( 0.81915), FLOAT_TO_FP( 0.76604), - FLOAT_TO_FP( 0.70711), FLOAT_TO_FP( 0.64279), FLOAT_TO_FP( 0.57358), - FLOAT_TO_FP( 0.50000), FLOAT_TO_FP( 0.42262), FLOAT_TO_FP( 0.34202), - FLOAT_TO_FP( 0.25882), FLOAT_TO_FP( 0.17365), FLOAT_TO_FP( 0.08716), - FLOAT_TO_FP( 0.00000), FLOAT_TO_FP(-0.08716), FLOAT_TO_FP(-0.17365), + FLOAT_TO_FP(1.00000), FLOAT_TO_FP(0.99619), FLOAT_TO_FP(0.98481), + FLOAT_TO_FP(0.96593), FLOAT_TO_FP(0.93969), FLOAT_TO_FP(0.90631), + FLOAT_TO_FP(0.86603), FLOAT_TO_FP(0.81915), FLOAT_TO_FP(0.76604), + FLOAT_TO_FP(0.70711), FLOAT_TO_FP(0.64279), FLOAT_TO_FP(0.57358), + FLOAT_TO_FP(0.50000), FLOAT_TO_FP(0.42262), FLOAT_TO_FP(0.34202), + FLOAT_TO_FP(0.25882), FLOAT_TO_FP(0.17365), FLOAT_TO_FP(0.08716), + FLOAT_TO_FP(0.00000), FLOAT_TO_FP(-0.08716), FLOAT_TO_FP(-0.17365), FLOAT_TO_FP(-0.25882), FLOAT_TO_FP(-0.34202), FLOAT_TO_FP(-0.42262), FLOAT_TO_FP(-0.50000), FLOAT_TO_FP(-0.57358), FLOAT_TO_FP(-0.64279), FLOAT_TO_FP(-0.70711), FLOAT_TO_FP(-0.76604), FLOAT_TO_FP(-0.81915), @@ -32,7 +32,6 @@ static const fp_t cos_lut[] = { }; BUILD_ASSERT(ARRAY_SIZE(cos_lut) == COSINE_LUT_SIZE); - fp_t arc_cos(fp_t x) { int i; @@ -48,8 +47,8 @@ fp_t arc_cos(fp_t x) * interpolate for precision. */ /* TODO(crosbug.com/p/25600): Optimize with binary search. */ - for (i = 0; i < COSINE_LUT_SIZE-1; i++) { - if (x >= cos_lut[i+1]) { + for (i = 0; i < COSINE_LUT_SIZE - 1; i++) { + if (x >= cos_lut[i + 1]) { const fp_t interp = fp_div(cos_lut[i] - x, cos_lut[i] - cos_lut[i + 1]); @@ -104,7 +103,7 @@ int int_sqrtf(fp_inter_t x) * infrequently enough it doesn't matter. */ if (x <= 0) - return 0; /* Yeah, for imaginary numbers too */ + return 0; /* Yeah, for imaginary numbers too */ else if (x >= (fp_inter_t)rmax * rmax) return rmax; @@ -138,9 +137,8 @@ fp_t fp_sqrtf(fp_t x) int vector_magnitude(const intv3_t v) { - fp_inter_t sum = (fp_inter_t)v[0] * v[0] + - (fp_inter_t)v[1] * v[1] + - (fp_inter_t)v[2] * v[2]; + fp_inter_t sum = (fp_inter_t)v[0] * v[0] + (fp_inter_t)v[1] * v[1] + + (fp_inter_t)v[2] * v[2]; return int_sqrtf(sum); } @@ -155,8 +153,7 @@ void cross_product(const intv3_t v1, const intv3_t v2, intv3_t v) fp_inter_t dot_product(const intv3_t v1, const intv3_t v2) { - return (fp_inter_t)v1[X] * v2[X] + - (fp_inter_t)v1[Y] * v2[Y] + + return (fp_inter_t)v1[X] * v2[X] + (fp_inter_t)v1[Y] * v2[Y] + (fp_inter_t)v1[Z] * v2[Z]; } @@ -215,17 +212,13 @@ void rotate(const intv3_t v, const mat33_fp_t R, intv3_t res) return; } - /* Rotate */ - t[0] = (fp_inter_t)v[0] * R[0][0] + - (fp_inter_t)v[1] * R[1][0] + - (fp_inter_t)v[2] * R[2][0]; - t[1] = (fp_inter_t)v[0] * R[0][1] + - (fp_inter_t)v[1] * R[1][1] + - (fp_inter_t)v[2] * R[2][1]; - t[2] = (fp_inter_t)v[0] * R[0][2] + - (fp_inter_t)v[1] * R[1][2] + - (fp_inter_t)v[2] * R[2][2]; + t[0] = (fp_inter_t)v[0] * R[0][0] + (fp_inter_t)v[1] * R[1][0] + + (fp_inter_t)v[2] * R[2][0]; + t[1] = (fp_inter_t)v[0] * R[0][1] + (fp_inter_t)v[1] * R[1][1] + + (fp_inter_t)v[2] * R[2][1]; + t[2] = (fp_inter_t)v[0] * R[0][2] + (fp_inter_t)v[1] * R[1][2] + + (fp_inter_t)v[2] * R[2][2]; /* Scale by fixed point shift when writing back to result */ res[0] = FP_TO_INT(t[0]); @@ -244,38 +237,39 @@ void rotate_inv(const intv3_t v, const mat33_fp_t R, intv3_t res) return; } - deter = fp_mul(R[0][0], (fp_mul(R[1][1], R[2][2]) - - fp_mul(R[2][1], R[1][2]))) - - fp_mul(R[0][1], (fp_mul(R[1][0], R[2][2]) - - fp_mul(R[1][2], R[2][0]))) + - fp_mul(R[0][2], (fp_mul(R[1][0], R[2][1]) - - fp_mul(R[1][1], R[2][0]))); + deter = fp_mul(R[0][0], + (fp_mul(R[1][1], R[2][2]) - fp_mul(R[2][1], R[1][2]))) - + fp_mul(R[0][1], + (fp_mul(R[1][0], R[2][2]) - fp_mul(R[1][2], R[2][0]))) + + fp_mul(R[0][2], + (fp_mul(R[1][0], R[2][1]) - fp_mul(R[1][1], R[2][0]))); /* * invert the matrix: from * http://stackoverflow.com/questions/983999/ * simple-3x3-matrix-inverse-code-c */ - t[0] = (fp_inter_t)v[0] * (fp_mul(R[1][1], R[2][2]) - - fp_mul(R[2][1], R[1][2])) - - (fp_inter_t)v[1] * (fp_mul(R[1][0], R[2][2]) - - fp_mul(R[1][2], R[2][0])) + - (fp_inter_t)v[2] * (fp_mul(R[1][0], R[2][1]) - - fp_mul(R[2][0], R[1][1])); - - t[1] = (fp_inter_t)v[0] * (fp_mul(R[0][1], R[2][2]) - - fp_mul(R[0][2], R[2][1])) * -1 + - (fp_inter_t)v[1] * (fp_mul(R[0][0], R[2][2]) - - fp_mul(R[0][2], R[2][0])) - - (fp_inter_t)v[2] * (fp_mul(R[0][0], R[2][1]) - - fp_mul(R[2][0], R[0][1])); - - t[2] = (fp_inter_t)v[0] * (fp_mul(R[0][1], R[1][2]) - - fp_mul(R[0][2], R[1][1])) - - (fp_inter_t)v[1] * (fp_mul(R[0][0], R[1][2]) - - fp_mul(R[1][0], R[0][2])) + - (fp_inter_t)v[2] * (fp_mul(R[0][0], R[1][1]) - - fp_mul(R[1][0], R[0][1])); + t[0] = (fp_inter_t)v[0] * + (fp_mul(R[1][1], R[2][2]) - fp_mul(R[2][1], R[1][2])) - + (fp_inter_t)v[1] * + (fp_mul(R[1][0], R[2][2]) - fp_mul(R[1][2], R[2][0])) + + (fp_inter_t)v[2] * + (fp_mul(R[1][0], R[2][1]) - fp_mul(R[2][0], R[1][1])); + + t[1] = (fp_inter_t)v[0] * + (fp_mul(R[0][1], R[2][2]) - fp_mul(R[0][2], R[2][1])) * + -1 + + (fp_inter_t)v[1] * + (fp_mul(R[0][0], R[2][2]) - fp_mul(R[0][2], R[2][0])) - + (fp_inter_t)v[2] * + (fp_mul(R[0][0], R[2][1]) - fp_mul(R[2][0], R[0][1])); + + t[2] = (fp_inter_t)v[0] * + (fp_mul(R[0][1], R[1][2]) - fp_mul(R[0][2], R[1][1])) - + (fp_inter_t)v[1] * + (fp_mul(R[0][0], R[1][2]) - fp_mul(R[1][0], R[0][2])) + + (fp_inter_t)v[2] * + (fp_mul(R[0][0], R[1][1]) - fp_mul(R[1][0], R[0][1])); /* Scale by fixed point shift when writing back to result */ res[0] = FP_TO_INT(fp_div(t[0], deter)); @@ -287,8 +281,8 @@ void rotate_inv(const intv3_t v, const mat33_fp_t R, intv3_t res) int round_divide(int64_t dividend, int divisor) { return (dividend > 0) ^ (divisor > 0) ? - (dividend - divisor / 2) / divisor : - (dividend + divisor / 2) / divisor; + (dividend - divisor / 2) / divisor : + (dividend + divisor / 2) / divisor; } #if ULONG_MAX == 0xFFFFFFFFUL @@ -303,7 +297,7 @@ uint64_t bitmask_uint64(int offset) { union mask64_t { struct { -#if (__BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__) +#if (__BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__) uint32_t lo; uint32_t hi; #elif (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__) |