1 /*
2 * Copyright (c) 2019 Kevin Townsend (KTOWN)
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <math.h>
8 #include <errno.h>
9 #include <zsl/zsl.h>
10 #include <zsl/physics/mass.h>
11
12
13 int
zsl_phy_mass_center(struct zsl_vec * m,struct zsl_vec * x,struct zsl_vec * y,struct zsl_vec * z,zsl_real_t * mx,zsl_real_t * my,zsl_real_t * mz)14 zsl_phy_mass_center(struct zsl_vec *m, struct zsl_vec *x,
15 struct zsl_vec *y, struct zsl_vec *z, zsl_real_t *mx,
16 zsl_real_t *my, zsl_real_t *mz)
17 {
18 /* TO DO: Check if the number of arguments in m, x, y and z
19 * is equal to n. */
20
21 int rc;
22 zsl_real_t mt = 0.0;
23 zsl_real_t mtx = 0.0;
24 zsl_real_t mty = 0.0;
25 zsl_real_t mtz = 0.0;
26
27 #if CONFIG_ZSL_BOUNDS_CHECKS
28 /* Ensure that all vectors have the same size. */
29 if ((m->sz != x->sz) || (x->sz != y->sz) || (y->sz != z->sz)) {
30 *mx = NAN;
31 *my = NAN;
32 *mz = NAN;
33 return -EINVAL;
34 }
35 #endif
36
37 rc = zsl_vec_dot(m, x, &mtx);
38 if (rc) {
39 return -EINVAL;
40 }
41
42 rc = zsl_vec_dot(m, y, &mty);
43 if (rc) {
44 return -EINVAL;
45 }
46
47 rc = zsl_vec_dot(m, z, &mtz);
48 if (rc) {
49 return -EINVAL;
50 }
51
52 /* Calculate arithematic mean of all masses in vector m. */
53 rc = zsl_vec_ar_mean(m, &mt);
54 if (rc) {
55 return -EINVAL;
56 }
57
58 /* Ensure there are no negative values for mass. */
59 if (zsl_vec_is_nonneg(m) == false) {
60 *mx = NAN;
61 *my = NAN;
62 *mz = NAN;
63 return -EINVAL;
64 }
65
66 mt *= m->sz;
67
68 /* Avoid divide by zero errors. */
69 if (mt == 0.0) {
70 *mx = NAN;
71 *my = NAN;
72 *mz = NAN;
73 return -EINVAL;
74 }
75
76 *mx = mtx / mt;
77 *my = mty / mt;
78 *mz = mtz / mt;
79
80 return 0;
81 }
82
