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/rotation.h>
11
12 int
zsl_phy_rot_angle(zsl_real_t oi,zsl_real_t time,zsl_real_t a,zsl_real_t * phi)13 zsl_phy_rot_angle(zsl_real_t oi, zsl_real_t time, zsl_real_t a, zsl_real_t *phi)
14 {
15 if (time < 0) {
16 *phi = NAN;
17 return -EINVAL;
18 }
19
20 *phi = oi * time + 0.5 * a * time * time;
21
22 return 0;
23 }
24
25 int
zsl_phy_rot_dist(zsl_real_t phi,zsl_real_t r,zsl_real_t * dist)26 zsl_phy_rot_dist(zsl_real_t phi, zsl_real_t r, zsl_real_t *dist)
27 {
28 if (r < 0) {
29 *dist = NAN;
30 return -EINVAL;
31 }
32
33 *dist = phi * r;
34
35 return 0;
36 }
37
38 int
zsl_phy_rot_turn(zsl_real_t phi,zsl_real_t * turn)39 zsl_phy_rot_turn(zsl_real_t phi, zsl_real_t *turn)
40 {
41 *turn = phi / (2.0 * ZSL_PI);
42
43 return 0;
44 }
45
46 int
zsl_phy_rot_time(zsl_real_t oi,zsl_real_t of,zsl_real_t a,zsl_real_t * time)47 zsl_phy_rot_time(zsl_real_t oi, zsl_real_t of, zsl_real_t a, zsl_real_t *time)
48 {
49 *time = (of - oi) / a;
50
51 if (*time < 0 || a == 0) {
52 *time = NAN;
53 return -EINVAL;
54 }
55
56 return 0;
57 }
58
59 int
zsl_phy_rot_omega(zsl_real_t oi,zsl_real_t time,zsl_real_t a,zsl_real_t * of)60 zsl_phy_rot_omega(zsl_real_t oi, zsl_real_t time, zsl_real_t a, zsl_real_t *of)
61 {
62 *of = oi + time * a;
63
64 if (time < 0) {
65 *of = NAN;
66 return -EINVAL;
67 }
68
69 return 0;
70 }
71
72 int
zsl_phy_rot_omega2(zsl_real_t oi,zsl_real_t phi,zsl_real_t a,zsl_real_t * of)73 zsl_phy_rot_omega2(zsl_real_t oi, zsl_real_t phi, zsl_real_t a, zsl_real_t *of)
74 {
75 zsl_real_t of2 = oi * oi + 2.0 * a * phi;
76
77 if (of2 < 0) {
78 *of = NAN;
79 return -EINVAL;
80 }
81
82 *of = ZSL_SQRT(of2);
83
84 return 0;
85 }
86
87 int
zsl_phy_rot_av_omega(zsl_real_t phi,zsl_real_t time,zsl_real_t * omega)88 zsl_phy_rot_av_omega(zsl_real_t phi, zsl_real_t time, zsl_real_t *omega)
89 {
90 if (time <= 0) {
91 *omega = NAN;
92 return -EINVAL;
93 }
94
95 *omega = phi / time;
96
97 return 0;
98 }
99
100 int
zsl_phy_rot_vel(zsl_real_t omega,zsl_real_t r,zsl_real_t * vel)101 zsl_phy_rot_vel(zsl_real_t omega, zsl_real_t r, zsl_real_t *vel)
102 {
103 if (r < 0) {
104 *vel = NAN;
105 return -EINVAL;
106 }
107
108 *vel = omega * r;
109
110 return 0;
111 }
112
113 int
zsl_phy_rot_ang_accel(zsl_real_t oi,zsl_real_t of,zsl_real_t time,zsl_real_t * a)114 zsl_phy_rot_ang_accel(zsl_real_t oi, zsl_real_t of, zsl_real_t time,
115 zsl_real_t *a)
116 {
117 if (time <= 0) {
118 *a = NAN;
119 return -EINVAL;
120 }
121
122 *a = (of - oi) / time;
123
124 return 0;
125 }
126
127 int
zsl_phy_rot_accel(zsl_real_t a,zsl_real_t r,zsl_real_t * accel)128 zsl_phy_rot_accel(zsl_real_t a, zsl_real_t r, zsl_real_t *accel)
129 {
130 if (r < 0) {
131 *accel = NAN;
132 return -EINVAL;
133 }
134
135 *accel = a * r;
136
137 return 0;
138 }
139
140 int
zsl_phy_rot_ener(zsl_real_t omega,zsl_real_t i,zsl_real_t * rke)141 zsl_phy_rot_ener(zsl_real_t omega, zsl_real_t i, zsl_real_t *rke)
142 {
143 if (i < 0) {
144 *rke = NAN;
145 return -EINVAL;
146 }
147
148 *rke = 0.5 * i * omega * omega;
149
150 return 0;
151 }
152
153 int
zsl_phy_rot_period(zsl_real_t omega,zsl_real_t * t)154 zsl_phy_rot_period(zsl_real_t omega, zsl_real_t *t)
155 {
156 if (omega <= 0) {
157 *t = NAN;
158 return -EINVAL;
159 }
160
161 *t = (2.0 * ZSL_PI) / omega;
162
163 return 0;
164 }
165
166 int
zsl_phy_rot_frequency(zsl_real_t omega,zsl_real_t * f)167 zsl_phy_rot_frequency(zsl_real_t omega, zsl_real_t *f)
168 {
169 if (omega < 0) {
170 *f = NAN;
171 return -EINVAL;
172 }
173
174 *f = omega / (2.0 * ZSL_PI);
175
176 return 0;
177 }
178
179 int
zsl_phy_rot_cent_accel(zsl_real_t vel,zsl_real_t r,zsl_real_t * ca)180 zsl_phy_rot_cent_accel(zsl_real_t vel, zsl_real_t r, zsl_real_t *ca)
181 {
182 if (r <= 0) {
183 *ca = NAN;
184 return -EINVAL;
185 }
186
187 *ca = (vel * vel) / r;
188
189 return 0;
190 }
191
192 int
zsl_phy_rot_cent_accel2(zsl_real_t t,zsl_real_t r,zsl_real_t * ca)193 zsl_phy_rot_cent_accel2(zsl_real_t t, zsl_real_t r, zsl_real_t *ca)
194 {
195 if (r < 0 || t <= 0) {
196 *ca = NAN;
197 return -EINVAL;
198 }
199
200 *ca = (4.0 * ZSL_PI * ZSL_PI * r) / (t * t);
201
202 return 0;
203 }
204
205 int
zsl_phy_rot_total_accel(zsl_real_t accel,zsl_real_t ca,zsl_real_t * at)206 zsl_phy_rot_total_accel(zsl_real_t accel, zsl_real_t ca, zsl_real_t *at)
207 {
208 *at = ZSL_SQRT(accel * accel + ca * ca);
209
210 return 0;
211 }
212
213 int
zsl_phy_rot_power(zsl_real_t torque,zsl_real_t omega,zsl_real_t * power)214 zsl_phy_rot_power(zsl_real_t torque, zsl_real_t omega, zsl_real_t *power)
215 {
216 *power = torque * omega;
217
218 return 0;
219 }
220
