summaryrefslogtreecommitdiff
path: root/common/math_util.c
diff options
context:
space:
mode:
Diffstat (limited to 'common/math_util.c')
-rw-r--r--common/math_util.c110
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__)
View Lorry Controller Status