1 /* vector.c: vector/point operations. */
2
3 #ifdef HAVE_CONFIG_H
4 #include "config.h"
5 #endif /* Def: HAVE_CONFIG_H */
6
7 #include "vector.h"
8 #include "message.h"
9 #include "epsilon-equal.h"
10 #include <math.h>
11 #include <errno.h>
12 #include <assert.h>
13 #include <string.h>
14
15 static at_real acos_d (at_real, at_exception_type * excep);
16
17 #ifndef M_PI
18 #define M_PI 3.14159265
19 #endif
20
21 #define SIGN(x) ((x) > 0 ? 1 : (x) < 0 ? -1 : 0)
22 #define ROUND(x) ((int) ((int) (x) + .5 * SIGN (x)))
23
24 /* Given the point COORD, return the corresponding vector. */
25
26 vector_type
make_vector(const at_real_coord c)27 make_vector (const at_real_coord c)
28 {
29 vector_type v;
30
31 v.dx = c.x;
32 v.dy = c.y;
33 v.dz = c.z;
34
35 return v;
36 }
37
38
39 /* And the converse: given a vector, return the corresponding point. */
40
41 at_real_coord
vector_to_point(const vector_type v)42 vector_to_point (const vector_type v)
43 {
44 at_real_coord coord;
45
46 coord.x = v.dx;
47 coord.y = v.dy;
48
49 return coord;
50 }
51
52
53 at_real
magnitude(const vector_type v)54 magnitude (const vector_type v)
55 {
56 return (at_real) sqrt (v.dx * v.dx + v.dy * v.dy + v.dz * v.dz);
57 }
58
59
60 vector_type
normalize(const vector_type v)61 normalize (const vector_type v)
62 {
63 vector_type new_v;
64 at_real m = magnitude (v);
65
66 /* assert (m > 0.0); */
67
68 if (m > 0.0)
69 {
70 new_v.dx = v.dx / m;
71 new_v.dy = v.dy / m;
72 new_v.dz = v.dz / m;
73 }
74 else
75 {
76 new_v.dx = v.dx;
77 new_v.dy = v.dy;
78 new_v.dz = v.dz;
79 }
80
81 return new_v;
82 }
83
84
85 vector_type
Vadd(const vector_type v1,const vector_type v2)86 Vadd (const vector_type v1, const vector_type v2)
87 {
88 vector_type new_v;
89
90 new_v.dx = v1.dx + v2.dx;
91 new_v.dy = v1.dy + v2.dy;
92 new_v.dz = v1.dz + v2.dz;
93
94 return new_v;
95 }
96
97
98 at_real
Vdot(const vector_type v1,const vector_type v2)99 Vdot (const vector_type v1, const vector_type v2)
100 {
101 return v1.dx * v2.dx + v1.dy * v2.dy + v1.dz * v2.dz;
102 }
103
104
105 vector_type
Vmult_scalar(const vector_type v,const at_real r)106 Vmult_scalar (const vector_type v, const at_real r)
107 {
108 vector_type new_v;
109
110 new_v.dx = v.dx * r;
111 new_v.dy = v.dy * r;
112 new_v.dz = v.dz * r;
113
114 return new_v;
115 }
116
117
118 /* Given the IN_VECTOR and OUT_VECTOR, return the angle between them in
119 degrees, in the range zero to 180. */
120
121 at_real
Vangle(const vector_type in_vector,const vector_type out_vector,at_exception_type * exp)122 Vangle (const vector_type in_vector,
123 const vector_type out_vector,
124 at_exception_type * exp)
125 {
126 vector_type v1 = normalize (in_vector);
127 vector_type v2 = normalize (out_vector);
128
129 return acos_d (Vdot (v2, v1), exp);
130 }
131
132
133 at_real_coord
Vadd_point(const at_real_coord c,const vector_type v)134 Vadd_point (const at_real_coord c, const vector_type v)
135 {
136 at_real_coord new_c;
137
138 new_c.x = c.x + v.dx;
139 new_c.y = c.y + v.dy;
140 new_c.z = c.z + v.dz;
141 return new_c;
142 }
143
144
145 at_real_coord
Vsubtract_point(const at_real_coord c,const vector_type v)146 Vsubtract_point (const at_real_coord c, const vector_type v)
147 {
148 at_real_coord new_c;
149
150 new_c.x = c.x - v.dx;
151 new_c.y = c.y - v.dy;
152 new_c.z = c.z - v.dz;
153 return new_c;
154 }
155
156
157 at_coord
Vadd_int_point(const at_coord c,const vector_type v)158 Vadd_int_point (const at_coord c, const vector_type v)
159 {
160 at_coord a;
161
162 a.x = (unsigned short) ROUND ((at_real) c.x + v.dx);
163 a.y = (unsigned short) ROUND ((at_real) c.y + v.dy);
164 return a;
165 }
166
167
168 vector_type
Vabs(const vector_type v)169 Vabs (const vector_type v)
170 {
171 vector_type new_v;
172
173 new_v.dx = (at_real) fabs (v.dx);
174 new_v.dy = (at_real) fabs (v.dy);
175 new_v.dz = (at_real) fabs (v.dz);
176 return new_v;
177 }
178
179
180 /* Operations on points. */
181
182 at_real_coord
Padd(const at_real_coord coord1,const at_real_coord coord2)183 Padd (const at_real_coord coord1, const at_real_coord coord2)
184 {
185 at_real_coord sum;
186
187 sum.x = coord1.x + coord2.x;
188 sum.y = coord1.y + coord2.y;
189 sum.z = coord1.z + coord2.z;
190
191 return sum;
192 }
193
194
195 at_real_coord
Pmult_scalar(const at_real_coord coord,const at_real r)196 Pmult_scalar (const at_real_coord coord, const at_real r)
197 {
198 at_real_coord answer;
199
200 answer.x = coord.x * r;
201 answer.y = coord.y * r;
202 answer.z = coord.z * r;
203
204 return answer;
205 }
206
207
208 vector_type
Psubtract(const at_real_coord c1,const at_real_coord c2)209 Psubtract (const at_real_coord c1, const at_real_coord c2)
210 {
211 vector_type v;
212
213 v.dx = c1.x - c2.x;
214 v.dy = c1.y - c2.y;
215 v.dz = c1.z - c2.z;
216
217 return v;
218 }
219
220
221
222 /* Operations on integer points. */
223
224 vector_type
IPsubtract(const at_coord coord1,const at_coord coord2)225 IPsubtract (const at_coord coord1, const at_coord coord2)
226 {
227 vector_type v;
228
229 v.dx = (at_real) (coord1.x - coord2.x);
230 v.dy = (at_real) (coord1.y - coord2.y);
231 v.dz = 0.0;
232
233 return v;
234 }
235
236
237 at_coord
IPsubtractP(const at_coord c1,const at_coord c2)238 IPsubtractP (const at_coord c1, const at_coord c2)
239 {
240 at_coord c;
241
242 c.x = c1.x - c2.x;
243 c.y = c1.y - c2.y;
244
245 return c;
246 }
247
248
249 at_coord
IPadd(const at_coord c1,const at_coord c2)250 IPadd (const at_coord c1, const at_coord c2)
251 {
252 at_coord c;
253
254 c.x = c1.x + c2.x;
255 c.y = c1.y + c2.y;
256
257 return c;
258 }
259
260
261 at_coord
IPmult_scalar(const at_coord c,const int i)262 IPmult_scalar (const at_coord c, const int i)
263 {
264 at_coord a;
265
266 a.x = (unsigned short) (c.x * i);
267 a.y = (unsigned short) (c.y * i);
268
269 return a;
270 }
271
272
273 at_real_coord
IPmult_real(const at_coord c,const at_real r)274 IPmult_real (const at_coord c, const at_real r)
275 {
276 at_real_coord a;
277
278 a.x = c.x * r;
279 a.y = c.y * r;
280
281 return a;
282 }
283
284
285 at_bool
IPequal(const at_coord c1,const at_coord c2)286 IPequal (const at_coord c1, const at_coord c2)
287 {
288 if ((c1.x == c2.x) && (c1.y == c2.y))
289 return true;
290 else
291 return false;
292 }
293
294 static at_real
acos_d(at_real v,at_exception_type * excep)295 acos_d (at_real v, at_exception_type * excep)
296 {
297 at_real a;
298
299 if (epsilon_equal (v, 1.0))
300 v = 1.0;
301 else if (epsilon_equal (v, -1.0))
302 v = -1.0;
303
304 errno = 0;
305 a = (at_real) acos (v);
306 if (errno == ERANGE || errno == EDOM)
307 {
308 at_exception_fatal(excep, strerror(errno));
309 return 0.0;
310 }
311
312
313 return a * (at_real) 180.0 / (at_real) M_PI;
314 }
315