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/kinematics.h>
11
12 int
zsl_phy_kin_dist(zsl_real_t vi,zsl_real_t time,zsl_real_t accel,zsl_real_t * dist)13 zsl_phy_kin_dist(zsl_real_t vi, zsl_real_t time, zsl_real_t accel,
14 zsl_real_t *dist)
15 {
16 if (time < 0) {
17 *dist = NAN;
18 return -EINVAL;
19 }
20
21 *dist = vi * time + 0.5 * accel * time * time;
22
23 return 0;
24 }
25
26 int
zsl_phy_kin_init_pos(zsl_real_t vi,zsl_real_t t,zsl_real_t a,zsl_real_t xf,zsl_real_t * xi)27 zsl_phy_kin_init_pos(zsl_real_t vi, zsl_real_t t, zsl_real_t a, zsl_real_t xf,
28 zsl_real_t *xi)
29 {
30 if (t < 0) {
31 *xi = NAN;
32 return -EINVAL;
33 }
34
35 zsl_real_t dist;
36
37 zsl_phy_kin_dist(vi, t, a, &dist);
38
39 *xi = xf - dist;
40
41 return 0;
42 }
43
44 int
zsl_phy_kin_init_pos2(zsl_real_t vi,zsl_real_t vf,zsl_real_t a,zsl_real_t xf,zsl_real_t * xi)45 zsl_phy_kin_init_pos2(zsl_real_t vi, zsl_real_t vf, zsl_real_t a, zsl_real_t xf,
46 zsl_real_t *xi)
47 {
48 if (a == 0) {
49 *xi = NAN;
50 return -EINVAL;
51 }
52
53 *xi = xf - ((vf * vf - vi * vi) / (2.0 * a));
54
55 return 0;
56 }
57
58 int
zsl_phy_kin_time(zsl_real_t vi,zsl_real_t vf,zsl_real_t accel,zsl_real_t * time)59 zsl_phy_kin_time(zsl_real_t vi, zsl_real_t vf, zsl_real_t accel,
60 zsl_real_t *time)
61 {
62 *time = (vf - vi) / accel;
63
64 if (*time < 0 || accel == 0) {
65 *time = NAN;
66 return -EINVAL;
67 }
68
69 return 0;
70 }
71
72 int
zsl_phy_kin_vel(zsl_real_t vi,zsl_real_t time,zsl_real_t accel,zsl_real_t * vf)73 zsl_phy_kin_vel(zsl_real_t vi, zsl_real_t time, zsl_real_t accel,
74 zsl_real_t *vf)
75 {
76 *vf = vi + time * accel;
77
78 if (time < 0) {
79 *vf = NAN;
80 return -EINVAL;
81 }
82
83 return 0;
84 }
85
86 int
zsl_phy_kin_vel2(zsl_real_t vi,zsl_real_t dist,zsl_real_t accel,zsl_real_t * vf)87 zsl_phy_kin_vel2(zsl_real_t vi, zsl_real_t dist, zsl_real_t accel,
88 zsl_real_t *vf)
89 {
90 zsl_real_t vf2 = vi * vi + 2 * accel * dist;
91
92 if (vf2 < 0) {
93 *vf = NAN;
94 return -EINVAL;
95 }
96
97 *vf = ZSL_SQRT(vf2);
98
99 return 0;
100 }
101
102 int
zsl_phy_kin_init_vel(zsl_real_t vf,zsl_real_t a,zsl_real_t t,zsl_real_t * vi)103 zsl_phy_kin_init_vel(zsl_real_t vf, zsl_real_t a, zsl_real_t t, zsl_real_t *vi)
104 {
105 if (t < 0) {
106 *vi = NAN;
107 return -EINVAL;
108 }
109
110 *vi = vf - a * t;
111
112 return 0;
113 }
114
115 int
zsl_phy_kin_init_vel2(zsl_real_t dist,zsl_real_t a,zsl_real_t t,zsl_real_t * vi)116 zsl_phy_kin_init_vel2(zsl_real_t dist, zsl_real_t a, zsl_real_t t,
117 zsl_real_t *vi)
118 {
119 if (t <= 0) {
120 *vi = NAN;
121 return -EINVAL;
122 }
123
124 *vi = (dist - 0.5 * a * t * t) / t;
125
126 return 0;
127 }
128
129 int
zsl_phy_kin_init_vel3(zsl_real_t vf,zsl_real_t a,zsl_real_t dist,zsl_real_t * vi)130 zsl_phy_kin_init_vel3(zsl_real_t vf, zsl_real_t a, zsl_real_t dist,
131 zsl_real_t *vi)
132 {
133 zsl_real_t sq = vf * vf - 2 * a * dist;
134
135 if (sq < 0) {
136 *vi = NAN;
137 return -EINVAL;
138 }
139
140 *vi = ZSL_SQRT(sq);
141
142 return 0;
143 }
144
145 int
zsl_phy_kin_av_vel(zsl_real_t dist,zsl_real_t time,zsl_real_t * vel)146 zsl_phy_kin_av_vel(zsl_real_t dist, zsl_real_t time, zsl_real_t *vel)
147 {
148 if (time <= 0) {
149 *vel = NAN;
150 return -EINVAL;
151 }
152
153 *vel = dist / time;
154
155 return 0;
156 }
157
158 int
zsl_phy_kin_accel(zsl_real_t vi,zsl_real_t vf,zsl_real_t time,zsl_real_t * accel)159 zsl_phy_kin_accel(zsl_real_t vi, zsl_real_t vf, zsl_real_t time,
160 zsl_real_t *accel)
161 {
162 if (time <= 0) {
163 *accel = NAN;
164 return -EINVAL;
165 }
166
167 *accel = (vf - vi) / time;
168
169 return 0;
170 }
171
172 int
zsl_phy_kin_ener(zsl_real_t v,zsl_real_t m,zsl_real_t * ke)173 zsl_phy_kin_ener(zsl_real_t v, zsl_real_t m, zsl_real_t *ke)
174 {
175 if (m < 0) {
176 *ke = NAN;
177 return -EINVAL;
178 }
179
180 *ke = 0.5 * m * v * v;
181
182 return 0;
183 }
184
