1 | |
---|
2 | |
---|
3 | /* |
---|
4 | orxonox - the future of 3D-vertical-scrollers |
---|
5 | |
---|
6 | Copyright (C) 2004 orx |
---|
7 | |
---|
8 | This program is free software; you can redistribute it and/or modify |
---|
9 | it under the terms of the GNU General Public License as published by |
---|
10 | the Free Software Foundation; either version 2, or (at your option) |
---|
11 | any later version. |
---|
12 | |
---|
13 | ### File Specific: |
---|
14 | main-programmer: Christian Meyer |
---|
15 | co-programmer: Patrick Boenzli : Vector::scale() |
---|
16 | Vector::abs() |
---|
17 | |
---|
18 | Quaternion code borrowed from an Gamasutra article by Nick Bobick and Ken Shoemake |
---|
19 | */ |
---|
20 | |
---|
21 | |
---|
22 | #include "vector.h" |
---|
23 | |
---|
24 | |
---|
25 | using namespace std; |
---|
26 | |
---|
27 | /** |
---|
28 | \brief add two vectors |
---|
29 | \param v: the other vector |
---|
30 | \return the sum of both vectors |
---|
31 | */ |
---|
32 | Vector Vector::operator+ (const Vector& v) const |
---|
33 | { |
---|
34 | Vector r; |
---|
35 | |
---|
36 | r.x = x + v.x; |
---|
37 | r.y = y + v.y; |
---|
38 | r.z = z + v.z; |
---|
39 | |
---|
40 | return r; |
---|
41 | } |
---|
42 | |
---|
43 | /** |
---|
44 | \brief subtract a vector from another |
---|
45 | \param v: the other vector |
---|
46 | \return the difference between the vectors |
---|
47 | */ |
---|
48 | Vector Vector::operator- (const Vector& v) const |
---|
49 | { |
---|
50 | Vector r; |
---|
51 | |
---|
52 | r.x = x - v.x; |
---|
53 | r.y = y - v.y; |
---|
54 | r.z = z - v.z; |
---|
55 | |
---|
56 | return r; |
---|
57 | } |
---|
58 | |
---|
59 | /** |
---|
60 | \brief calculate the dot product of two vectors |
---|
61 | \param v: the other vector |
---|
62 | \return the dot product of the vectors |
---|
63 | */ |
---|
64 | float Vector::operator* (const Vector& v) const |
---|
65 | { |
---|
66 | return x*v.x+y*v.y+z*v.z; |
---|
67 | } |
---|
68 | |
---|
69 | /** |
---|
70 | \brief multiply a vector with a float |
---|
71 | \param f: the factor |
---|
72 | \return the vector multipied by f |
---|
73 | */ |
---|
74 | Vector Vector::operator* (float f) const |
---|
75 | { |
---|
76 | Vector r; |
---|
77 | |
---|
78 | r.x = x * f; |
---|
79 | r.y = y * f; |
---|
80 | r.z = z * f; |
---|
81 | |
---|
82 | return r; |
---|
83 | } |
---|
84 | |
---|
85 | /** |
---|
86 | \brief divide a vector with a float |
---|
87 | \param f: the divisor |
---|
88 | \return the vector divided by f |
---|
89 | */ |
---|
90 | Vector Vector::operator/ (float f) const |
---|
91 | { |
---|
92 | Vector r; |
---|
93 | |
---|
94 | if( f == 0.0) |
---|
95 | { |
---|
96 | // Prevent divide by zero |
---|
97 | return Vector (0,0,0); |
---|
98 | } |
---|
99 | |
---|
100 | r.x = x / f; |
---|
101 | r.y = y / f; |
---|
102 | r.z = z / f; |
---|
103 | |
---|
104 | return r; |
---|
105 | } |
---|
106 | |
---|
107 | /** |
---|
108 | \brief calculate the dot product of two vectors |
---|
109 | \param v: the other vector |
---|
110 | \return the dot product of the vectors |
---|
111 | */ |
---|
112 | float Vector::dot (const Vector& v) const |
---|
113 | { |
---|
114 | return x*v.x+y*v.y+z*v.z; |
---|
115 | } |
---|
116 | |
---|
117 | /** |
---|
118 | \brief calculate the cross product of two vectors |
---|
119 | \param v: the other vector |
---|
120 | \return the cross product of the vectors |
---|
121 | */ |
---|
122 | Vector Vector::cross (const Vector& v) const |
---|
123 | { |
---|
124 | Vector r; |
---|
125 | |
---|
126 | r.x = y * v.z - z * v.y; |
---|
127 | r.y = z * v.x - x * v.z; |
---|
128 | r.z = x * v.y - y * v.x; |
---|
129 | |
---|
130 | return r; |
---|
131 | } |
---|
132 | |
---|
133 | /** |
---|
134 | \brief normalizes the vector to lenght 1.0 |
---|
135 | */ |
---|
136 | void Vector::normalize () |
---|
137 | { |
---|
138 | float l = len(); |
---|
139 | |
---|
140 | if( l == 0.0) |
---|
141 | { |
---|
142 | // Prevent divide by zero |
---|
143 | return; |
---|
144 | } |
---|
145 | |
---|
146 | x = x / l; |
---|
147 | y = y / l; |
---|
148 | z = z / l; |
---|
149 | } |
---|
150 | |
---|
151 | |
---|
152 | /** |
---|
153 | \bref returns the voctor normalized to length 1.0 |
---|
154 | */ |
---|
155 | |
---|
156 | Vector* Vector::getNormalized() |
---|
157 | { |
---|
158 | float l = len(); |
---|
159 | if(l != 1.0) |
---|
160 | { |
---|
161 | return this; |
---|
162 | } |
---|
163 | else if(l == 0.0) |
---|
164 | { |
---|
165 | return 0; |
---|
166 | } |
---|
167 | |
---|
168 | Vector *normalizedVector = new Vector(x / l, y /l, z / l); |
---|
169 | return normalizedVector; |
---|
170 | } |
---|
171 | |
---|
172 | |
---|
173 | void Vector::scale(const Vector& v) |
---|
174 | { |
---|
175 | x *= v.x; |
---|
176 | y *= v.y; |
---|
177 | z *= v.z; |
---|
178 | } |
---|
179 | |
---|
180 | |
---|
181 | /** |
---|
182 | \brief calculates the lenght of the vector |
---|
183 | \return the lenght of the vector |
---|
184 | */ |
---|
185 | float Vector::len () const |
---|
186 | { |
---|
187 | return sqrt (x*x+y*y+z*z); |
---|
188 | } |
---|
189 | |
---|
190 | |
---|
191 | |
---|
192 | Vector Vector::abs() |
---|
193 | { |
---|
194 | Vector v(fabs(x), fabs(y), fabs(z)); |
---|
195 | return v; |
---|
196 | } |
---|
197 | |
---|
198 | |
---|
199 | /** |
---|
200 | \brief calculate the angle between two vectors in radiances |
---|
201 | \param v1: a vector |
---|
202 | \param v2: another vector |
---|
203 | \return the angle between the vectors in radians |
---|
204 | */ |
---|
205 | float angle_rad (const Vector& v1, const Vector& v2) |
---|
206 | { |
---|
207 | return acos( v1 * v2 / (v1.len() * v2.len())); |
---|
208 | } |
---|
209 | |
---|
210 | |
---|
211 | /** |
---|
212 | \brief calculate the angle between two vectors in degrees |
---|
213 | \param v1: a vector |
---|
214 | \param v2: another vector |
---|
215 | \return the angle between the vectors in degrees |
---|
216 | */ |
---|
217 | float angle_deg (const Vector& v1, const Vector& v2) |
---|
218 | { |
---|
219 | float f; |
---|
220 | f = acos( v1 * v2 / (v1.len() * v2.len())); |
---|
221 | return f * 180 / PI; |
---|
222 | } |
---|
223 | |
---|
224 | /** |
---|
225 | \brief creates a multiplicational identity Quaternion |
---|
226 | */ |
---|
227 | Quaternion::Quaternion () |
---|
228 | { |
---|
229 | w = 1; |
---|
230 | v = Vector(0,0,0); |
---|
231 | } |
---|
232 | |
---|
233 | /** |
---|
234 | \brief turns a rotation along an axis into a Quaternion |
---|
235 | \param angle: the amount of radians to rotate |
---|
236 | \param axis: the axis to rotate around |
---|
237 | */ |
---|
238 | Quaternion::Quaternion (float angle, const Vector& axis) |
---|
239 | { |
---|
240 | w = cos(angle/2); |
---|
241 | v = axis * sin(angle/2); |
---|
242 | } |
---|
243 | |
---|
244 | /** |
---|
245 | \brief calculates a look_at rotation |
---|
246 | \param dir: the direction you want to look |
---|
247 | \param up: specify what direction up should be |
---|
248 | |
---|
249 | Mathematically this determines the rotation a (0,0,1)-Vector has to undergo to point |
---|
250 | the same way as dir. If you want to use this with cameras, you'll have to reverse the |
---|
251 | dir Vector (Vector(0,0,0) - your viewing direction) or you'll point the wrong way. You |
---|
252 | can use this for meshes as well (then you do not have to reverse the vector), but keep |
---|
253 | in mind that if you do that, the model's front has to point in +z direction, and left |
---|
254 | and right should be -x or +x respectively or the mesh wont rotate correctly. |
---|
255 | */ |
---|
256 | Quaternion::Quaternion (const Vector& dir, const Vector& up) |
---|
257 | { |
---|
258 | Vector z = dir; |
---|
259 | z.normalize(); |
---|
260 | Vector x = up.cross(z); |
---|
261 | x.normalize(); |
---|
262 | Vector y = z.cross(x); |
---|
263 | |
---|
264 | float m[4][4]; |
---|
265 | m[0][0] = x.x; |
---|
266 | m[0][1] = x.y; |
---|
267 | m[0][2] = x.z; |
---|
268 | m[0][3] = 0; |
---|
269 | m[1][0] = y.x; |
---|
270 | m[1][1] = y.y; |
---|
271 | m[1][2] = y.z; |
---|
272 | m[1][3] = 0; |
---|
273 | m[2][0] = z.x; |
---|
274 | m[2][1] = z.y; |
---|
275 | m[2][2] = z.z; |
---|
276 | m[2][3] = 0; |
---|
277 | m[3][0] = 0; |
---|
278 | m[3][1] = 0; |
---|
279 | m[3][2] = 0; |
---|
280 | m[3][3] = 1; |
---|
281 | |
---|
282 | *this = Quaternion (m); |
---|
283 | } |
---|
284 | |
---|
285 | /** |
---|
286 | \brief calculates a rotation from euler angles |
---|
287 | \param roll: the roll in radians |
---|
288 | \param pitch: the pitch in radians |
---|
289 | \param yaw: the yaw in radians |
---|
290 | |
---|
291 | I DO HONESTLY NOT EXACTLY KNOW WHICH ANGLE REPRESENTS WHICH ROTATION. And I do not know |
---|
292 | in what order they are applied, I just copy-pasted the code. |
---|
293 | */ |
---|
294 | Quaternion::Quaternion (float roll, float pitch, float yaw) |
---|
295 | { |
---|
296 | float cr, cp, cy, sr, sp, sy, cpcy, spsy; |
---|
297 | |
---|
298 | // calculate trig identities |
---|
299 | cr = cos(roll/2); |
---|
300 | cp = cos(pitch/2); |
---|
301 | cy = cos(yaw/2); |
---|
302 | |
---|
303 | sr = sin(roll/2); |
---|
304 | sp = sin(pitch/2); |
---|
305 | sy = sin(yaw/2); |
---|
306 | |
---|
307 | cpcy = cp * cy; |
---|
308 | spsy = sp * sy; |
---|
309 | |
---|
310 | w = cr * cpcy + sr * spsy; |
---|
311 | v.x = sr * cpcy - cr * spsy; |
---|
312 | v.y = cr * sp * cy + sr * cp * sy; |
---|
313 | v.z = cr * cp * sy - sr * sp * cy; |
---|
314 | } |
---|
315 | |
---|
316 | /** |
---|
317 | \brief rotates one Quaternion by another |
---|
318 | \param q: another Quaternion to rotate this by |
---|
319 | \return a quaternion that represents the first one rotated by the second one (WARUNING: this operation is not commutative! e.g. (A*B) != (B*A)) |
---|
320 | */ |
---|
321 | Quaternion Quaternion::operator*(const Quaternion& q) const |
---|
322 | { |
---|
323 | float A, B, C, D, E, F, G, H; |
---|
324 | Quaternion r; |
---|
325 | |
---|
326 | A = (w + v.x)*(q.w + q.v.x); |
---|
327 | B = (v.z - v.y)*(q.v.y - q.v.z); |
---|
328 | C = (w - v.x)*(q.v.y + q.v.z); |
---|
329 | D = (v.y + v.z)*(q.w - q.v.x); |
---|
330 | E = (v.x + v.z)*(q.v.x + q.v.y); |
---|
331 | F = (v.x - v.z)*(q.v.x - q.v.y); |
---|
332 | G = (w + v.y)*(q.w - q.v.z); |
---|
333 | H = (w - v.y)*(q.w + q.v.z); |
---|
334 | |
---|
335 | r.w = B + (-E - F + G + H)/2; |
---|
336 | r.v.x = A - (E + F + G + H)/2; |
---|
337 | r.v.y = C + (E - F + G - H)/2; |
---|
338 | r.v.z = D + (E - F - G + H)/2; |
---|
339 | |
---|
340 | return r; |
---|
341 | } |
---|
342 | |
---|
343 | /** |
---|
344 | \brief add two Quaternions |
---|
345 | \param q: another Quaternion |
---|
346 | \return the sum of both Quaternions |
---|
347 | */ |
---|
348 | Quaternion Quaternion::operator+(const Quaternion& q) const |
---|
349 | { |
---|
350 | Quaternion r(*this); |
---|
351 | r.w = r.w + q.w; |
---|
352 | r.v = r.v + q.v; |
---|
353 | return r; |
---|
354 | } |
---|
355 | |
---|
356 | /** |
---|
357 | \brief subtract two Quaternions |
---|
358 | \param q: another Quaternion |
---|
359 | \return the difference of both Quaternions |
---|
360 | */ |
---|
361 | Quaternion Quaternion::operator- (const Quaternion& q) const |
---|
362 | { |
---|
363 | Quaternion r(*this); |
---|
364 | r.w = r.w - q.w; |
---|
365 | r.v = r.v - q.v; |
---|
366 | return r; |
---|
367 | } |
---|
368 | |
---|
369 | /** |
---|
370 | \brief rotate a Vector by a Quaternion |
---|
371 | \param v: the Vector |
---|
372 | \return a new Vector representing v rotated by the Quaternion |
---|
373 | */ |
---|
374 | Vector Quaternion::apply (Vector& v) const |
---|
375 | { |
---|
376 | Quaternion q; |
---|
377 | q.v = v; |
---|
378 | q.w = 0; |
---|
379 | q = *this * q * conjugate(); |
---|
380 | return q.v; |
---|
381 | } |
---|
382 | |
---|
383 | /** |
---|
384 | \brief multiply a Quaternion with a real value |
---|
385 | \param f: a real value |
---|
386 | \return a new Quaternion containing the product |
---|
387 | */ |
---|
388 | Quaternion Quaternion::operator*(const float& f) const |
---|
389 | { |
---|
390 | Quaternion r(*this); |
---|
391 | r.w = r.w*f; |
---|
392 | r.v = r.v*f; |
---|
393 | return r; |
---|
394 | } |
---|
395 | |
---|
396 | /** |
---|
397 | \brief divide a Quaternion by a real value |
---|
398 | \param f: a real value |
---|
399 | \return a new Quaternion containing the quotient |
---|
400 | */ |
---|
401 | Quaternion Quaternion::operator/(const float& f) const |
---|
402 | { |
---|
403 | if( f == 0) return Quaternion(); |
---|
404 | Quaternion r(*this); |
---|
405 | r.w = r.w/f; |
---|
406 | r.v = r.v/f; |
---|
407 | return r; |
---|
408 | } |
---|
409 | |
---|
410 | /** |
---|
411 | \brief calculate the conjugate value of the Quaternion |
---|
412 | \return the conjugate Quaternion |
---|
413 | */ |
---|
414 | Quaternion Quaternion::conjugate() const |
---|
415 | { |
---|
416 | Quaternion r(*this); |
---|
417 | r.v = Vector() - r.v; |
---|
418 | return r; |
---|
419 | } |
---|
420 | |
---|
421 | /** |
---|
422 | \brief calculate the norm of the Quaternion |
---|
423 | \return the norm of The Quaternion |
---|
424 | */ |
---|
425 | float Quaternion::norm() const |
---|
426 | { |
---|
427 | return w*w + v.x*v.x + v.y*v.y + v.z*v.z; |
---|
428 | } |
---|
429 | |
---|
430 | /** |
---|
431 | \brief calculate the inverse value of the Quaternion |
---|
432 | \return the inverse Quaternion |
---|
433 | |
---|
434 | Note that this is equal to conjugate() if the Quaternion's norm is 1 |
---|
435 | */ |
---|
436 | Quaternion Quaternion::inverse() const |
---|
437 | { |
---|
438 | float n = norm(); |
---|
439 | if (n != 0) |
---|
440 | { |
---|
441 | return conjugate() / norm(); |
---|
442 | } |
---|
443 | else return Quaternion(); |
---|
444 | } |
---|
445 | |
---|
446 | /** |
---|
447 | \brief convert the Quaternion to a 4x4 rotational glMatrix |
---|
448 | \param m: a buffer to store the Matrix in |
---|
449 | */ |
---|
450 | void Quaternion::matrix (float m[4][4]) const |
---|
451 | { |
---|
452 | float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2; |
---|
453 | |
---|
454 | // calculate coefficients |
---|
455 | x2 = v.x + v.x; |
---|
456 | y2 = v.y + v.y; |
---|
457 | z2 = v.z + v.z; |
---|
458 | xx = v.x * x2; xy = v.x * y2; xz = v.x * z2; |
---|
459 | yy = v.y * y2; yz = v.y * z2; zz = v.z * z2; |
---|
460 | wx = w * x2; wy = w * y2; wz = w * z2; |
---|
461 | |
---|
462 | m[0][0] = 1.0 - (yy + zz); m[1][0] = xy - wz; |
---|
463 | m[2][0] = xz + wy; m[3][0] = 0.0; |
---|
464 | |
---|
465 | m[0][1] = xy + wz; m[1][1] = 1.0 - (xx + zz); |
---|
466 | m[2][1] = yz - wx; m[3][1] = 0.0; |
---|
467 | |
---|
468 | m[0][2] = xz - wy; m[1][2] = yz + wx; |
---|
469 | m[2][2] = 1.0 - (xx + yy); m[3][2] = 0.0; |
---|
470 | |
---|
471 | m[0][3] = 0; m[1][3] = 0; |
---|
472 | m[2][3] = 0; m[3][3] = 1; |
---|
473 | } |
---|
474 | |
---|
475 | |
---|
476 | void Quaternion::quatSlerp(const Quaternion* from, const Quaternion* to, float t, Quaternion* res) |
---|
477 | { |
---|
478 | float tol[4]; |
---|
479 | double omega, cosom, sinom, scale0, scale1; |
---|
480 | DELTA = 0.2; |
---|
481 | |
---|
482 | cosom = from->v.x * to->v.x + from->v.y * to->v.y + from->v.z * to->v.z + from->w * to->w; |
---|
483 | |
---|
484 | if( cosom < 0.0 ) |
---|
485 | { |
---|
486 | cosom = -cosom; |
---|
487 | tol[0] = -to->v.x; |
---|
488 | tol[1] = -to->v.y; |
---|
489 | tol[2] = -to->v.z; |
---|
490 | tol[3] = -to->w; |
---|
491 | } |
---|
492 | else |
---|
493 | { |
---|
494 | tol[0] = to->v.x; |
---|
495 | tol[1] = to->v.y; |
---|
496 | tol[2] = to->v.z; |
---|
497 | tol[3] = to->w; |
---|
498 | } |
---|
499 | |
---|
500 | //if( (1.0 - cosom) > DELTA ) |
---|
501 | //{ |
---|
502 | omega = acos(cosom); |
---|
503 | sinom = sin(omega); |
---|
504 | scale0 = sin((1.0 - t) * omega) / sinom; |
---|
505 | scale1 = sin(t * omega) / sinom; |
---|
506 | //} |
---|
507 | /* |
---|
508 | else |
---|
509 | { |
---|
510 | scale0 = 1.0 - t; |
---|
511 | scale1 = t; |
---|
512 | } |
---|
513 | */ |
---|
514 | res->v.x = scale0 * from->v.x + scale1 * tol[0]; |
---|
515 | res->v.y = scale0 * from->v.y + scale1 * tol[1]; |
---|
516 | res->v.z = scale0 * from->v.z + scale1 * tol[2]; |
---|
517 | res->w = scale0 * from->w + scale1 * tol[3]; |
---|
518 | } |
---|
519 | |
---|
520 | |
---|
521 | /** |
---|
522 | \brief convert a rotational 4x4 glMatrix into a Quaternion |
---|
523 | \param m: a 4x4 matrix in glMatrix order |
---|
524 | */ |
---|
525 | Quaternion::Quaternion (float m[4][4]) |
---|
526 | { |
---|
527 | |
---|
528 | float tr, s, q[4]; |
---|
529 | int i, j, k; |
---|
530 | |
---|
531 | int nxt[3] = {1, 2, 0}; |
---|
532 | |
---|
533 | tr = m[0][0] + m[1][1] + m[2][2]; |
---|
534 | |
---|
535 | // check the diagonal |
---|
536 | if (tr > 0.0) |
---|
537 | { |
---|
538 | s = sqrt (tr + 1.0); |
---|
539 | w = s / 2.0; |
---|
540 | s = 0.5 / s; |
---|
541 | v.x = (m[1][2] - m[2][1]) * s; |
---|
542 | v.y = (m[2][0] - m[0][2]) * s; |
---|
543 | v.z = (m[0][1] - m[1][0]) * s; |
---|
544 | } |
---|
545 | else |
---|
546 | { |
---|
547 | // diagonal is negative |
---|
548 | i = 0; |
---|
549 | if (m[1][1] > m[0][0]) i = 1; |
---|
550 | if (m[2][2] > m[i][i]) i = 2; |
---|
551 | j = nxt[i]; |
---|
552 | k = nxt[j]; |
---|
553 | |
---|
554 | s = sqrt ((m[i][i] - (m[j][j] + m[k][k])) + 1.0); |
---|
555 | |
---|
556 | q[i] = s * 0.5; |
---|
557 | |
---|
558 | if (s != 0.0) s = 0.5 / s; |
---|
559 | |
---|
560 | q[3] = (m[j][k] - m[k][j]) * s; |
---|
561 | q[j] = (m[i][j] + m[j][i]) * s; |
---|
562 | q[k] = (m[i][k] + m[k][i]) * s; |
---|
563 | |
---|
564 | v.x = q[0]; |
---|
565 | v.y = q[1]; |
---|
566 | v.z = q[2]; |
---|
567 | w = q[3]; |
---|
568 | } |
---|
569 | } |
---|
570 | |
---|
571 | /** |
---|
572 | \brief create a rotation from a vector |
---|
573 | \param v: a vector |
---|
574 | */ |
---|
575 | Rotation::Rotation (const Vector& v) |
---|
576 | { |
---|
577 | Vector x = Vector( 1, 0, 0); |
---|
578 | Vector axis = x.cross( v); |
---|
579 | axis.normalize(); |
---|
580 | float angle = angle_rad( x, v); |
---|
581 | float ca = cos(angle); |
---|
582 | float sa = sin(angle); |
---|
583 | m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f); |
---|
584 | m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y; |
---|
585 | m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z; |
---|
586 | m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y; |
---|
587 | m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f); |
---|
588 | m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z; |
---|
589 | m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z; |
---|
590 | m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z; |
---|
591 | m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f); |
---|
592 | } |
---|
593 | |
---|
594 | /** |
---|
595 | \brief creates a rotation from an axis and an angle (radians!) |
---|
596 | \param axis: the rotational axis |
---|
597 | \param angle: the angle in radians |
---|
598 | */ |
---|
599 | Rotation::Rotation (const Vector& axis, float angle) |
---|
600 | { |
---|
601 | float ca, sa; |
---|
602 | ca = cos(angle); |
---|
603 | sa = sin(angle); |
---|
604 | m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f); |
---|
605 | m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y; |
---|
606 | m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z; |
---|
607 | m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y; |
---|
608 | m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f); |
---|
609 | m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z; |
---|
610 | m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z; |
---|
611 | m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z; |
---|
612 | m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f); |
---|
613 | } |
---|
614 | |
---|
615 | /** |
---|
616 | \brief creates a rotation from euler angles (pitch/yaw/roll) |
---|
617 | \param pitch: rotation around z (in radians) |
---|
618 | \param yaw: rotation around y (in radians) |
---|
619 | \param roll: rotation around x (in radians) |
---|
620 | */ |
---|
621 | Rotation::Rotation ( float pitch, float yaw, float roll) |
---|
622 | { |
---|
623 | float cy, sy, cr, sr, cp, sp; |
---|
624 | cy = cos(yaw); |
---|
625 | sy = sin(yaw); |
---|
626 | cr = cos(roll); |
---|
627 | sr = sin(roll); |
---|
628 | cp = cos(pitch); |
---|
629 | sp = sin(pitch); |
---|
630 | m[0] = cy*cr; |
---|
631 | m[1] = -cy*sr; |
---|
632 | m[2] = sy; |
---|
633 | m[3] = cp*sr+sp*sy*cr; |
---|
634 | m[4] = cp*cr-sp*sr*sy; |
---|
635 | m[5] = -sp*cy; |
---|
636 | m[6] = sp*sr-cp*sy*cr; |
---|
637 | m[7] = sp*cr+cp*sy*sr; |
---|
638 | m[8] = cp*cy; |
---|
639 | } |
---|
640 | |
---|
641 | /** |
---|
642 | \brief creates a nullrotation (an identity rotation) |
---|
643 | */ |
---|
644 | Rotation::Rotation () |
---|
645 | { |
---|
646 | m[0] = 1.0f; |
---|
647 | m[1] = 0.0f; |
---|
648 | m[2] = 0.0f; |
---|
649 | m[3] = 0.0f; |
---|
650 | m[4] = 1.0f; |
---|
651 | m[5] = 0.0f; |
---|
652 | m[6] = 0.0f; |
---|
653 | m[7] = 0.0f; |
---|
654 | m[8] = 1.0f; |
---|
655 | } |
---|
656 | |
---|
657 | /** |
---|
658 | \brief fills the specified buffer with a 4x4 glmatrix |
---|
659 | \param buffer: Pointer to an array of 16 floats |
---|
660 | |
---|
661 | Use this to get the rotation in a gl-compatible format |
---|
662 | */ |
---|
663 | void Rotation::glmatrix (float* buffer) |
---|
664 | { |
---|
665 | buffer[0] = m[0]; |
---|
666 | buffer[1] = m[3]; |
---|
667 | buffer[2] = m[6]; |
---|
668 | buffer[3] = m[0]; |
---|
669 | buffer[4] = m[1]; |
---|
670 | buffer[5] = m[4]; |
---|
671 | buffer[6] = m[7]; |
---|
672 | buffer[7] = m[0]; |
---|
673 | buffer[8] = m[2]; |
---|
674 | buffer[9] = m[5]; |
---|
675 | buffer[10] = m[8]; |
---|
676 | buffer[11] = m[0]; |
---|
677 | buffer[12] = m[0]; |
---|
678 | buffer[13] = m[0]; |
---|
679 | buffer[14] = m[0]; |
---|
680 | buffer[15] = m[1]; |
---|
681 | } |
---|
682 | |
---|
683 | /** |
---|
684 | \brief multiplies two rotational matrices |
---|
685 | \param r: another Rotation |
---|
686 | \return the matrix product of the Rotations |
---|
687 | |
---|
688 | Use this to rotate one rotation by another |
---|
689 | */ |
---|
690 | Rotation Rotation::operator* (const Rotation& r) |
---|
691 | { |
---|
692 | Rotation p; |
---|
693 | |
---|
694 | p.m[0] = m[0]*r.m[0] + m[1]*r.m[3] + m[2]*r.m[6]; |
---|
695 | p.m[1] = m[0]*r.m[1] + m[1]*r.m[4] + m[2]*r.m[7]; |
---|
696 | p.m[2] = m[0]*r.m[2] + m[1]*r.m[5] + m[2]*r.m[8]; |
---|
697 | |
---|
698 | p.m[3] = m[3]*r.m[0] + m[4]*r.m[3] + m[5]*r.m[6]; |
---|
699 | p.m[4] = m[3]*r.m[1] + m[4]*r.m[4] + m[5]*r.m[7]; |
---|
700 | p.m[5] = m[3]*r.m[2] + m[4]*r.m[5] + m[5]*r.m[8]; |
---|
701 | |
---|
702 | p.m[6] = m[6]*r.m[0] + m[7]*r.m[3] + m[8]*r.m[6]; |
---|
703 | p.m[7] = m[6]*r.m[1] + m[7]*r.m[4] + m[8]*r.m[7]; |
---|
704 | p.m[8] = m[6]*r.m[2] + m[7]*r.m[5] + m[8]*r.m[8]; |
---|
705 | |
---|
706 | return p; |
---|
707 | } |
---|
708 | |
---|
709 | |
---|
710 | /** |
---|
711 | \brief rotates the vector by the given rotation |
---|
712 | \param v: a vector |
---|
713 | \param r: a rotation |
---|
714 | \return the rotated vector |
---|
715 | */ |
---|
716 | Vector rotate_vector( const Vector& v, const Rotation& r) |
---|
717 | { |
---|
718 | Vector t; |
---|
719 | |
---|
720 | t.x = v.x * r.m[0] + v.y * r.m[1] + v.z * r.m[2]; |
---|
721 | t.y = v.x * r.m[3] + v.y * r.m[4] + v.z * r.m[5]; |
---|
722 | t.z = v.x * r.m[6] + v.y * r.m[7] + v.z * r.m[8]; |
---|
723 | |
---|
724 | return t; |
---|
725 | } |
---|
726 | |
---|
727 | /** |
---|
728 | \brief calculate the distance between two lines |
---|
729 | \param l: the other line |
---|
730 | \return the distance between the lines |
---|
731 | */ |
---|
732 | float Line::distance (const Line& l) const |
---|
733 | { |
---|
734 | float q, d; |
---|
735 | Vector n = a.cross(l.a); |
---|
736 | q = n.dot(r-l.r); |
---|
737 | d = n.len(); |
---|
738 | if( d == 0.0) return 0.0; |
---|
739 | return q/d; |
---|
740 | } |
---|
741 | |
---|
742 | /** |
---|
743 | \brief calculate the distance between a line and a point |
---|
744 | \param v: the point |
---|
745 | \return the distance between the Line and the point |
---|
746 | */ |
---|
747 | float Line::distance_point (const Vector& v) const |
---|
748 | { |
---|
749 | Vector d = v-r; |
---|
750 | Vector u = a * d.dot( a); |
---|
751 | return (d - u).len(); |
---|
752 | } |
---|
753 | |
---|
754 | /** |
---|
755 | \brief calculate the two points of minimal distance of two lines |
---|
756 | \param l: the other line |
---|
757 | \return a Vector[2] (!has to be deleted after use!) containing the two points of minimal distance |
---|
758 | */ |
---|
759 | Vector* Line::footpoints (const Line& l) const |
---|
760 | { |
---|
761 | Vector* fp = new Vector[2]; |
---|
762 | Plane p = Plane (r + a.cross(l.a), r, r + a); |
---|
763 | fp[1] = p.intersect_line (l); |
---|
764 | p = Plane (fp[1], l.a); |
---|
765 | fp[0] = p.intersect_line (*this); |
---|
766 | return fp; |
---|
767 | } |
---|
768 | |
---|
769 | /** |
---|
770 | \brief calculate the length of a line |
---|
771 | \return the lenght of the line |
---|
772 | */ |
---|
773 | float Line::len() const |
---|
774 | { |
---|
775 | return a.len(); |
---|
776 | } |
---|
777 | |
---|
778 | /** |
---|
779 | \brief rotate the line by given rotation |
---|
780 | \param rot: a rotation |
---|
781 | */ |
---|
782 | void Line::rotate (const Rotation& rot) |
---|
783 | { |
---|
784 | Vector t = a + r; |
---|
785 | t = rotate_vector( t, rot); |
---|
786 | r = rotate_vector( r, rot), |
---|
787 | a = t - r; |
---|
788 | } |
---|
789 | |
---|
790 | /** |
---|
791 | \brief create a plane from three points |
---|
792 | \param a: first point |
---|
793 | \param b: second point |
---|
794 | \param c: third point |
---|
795 | */ |
---|
796 | Plane::Plane (Vector a, Vector b, Vector c) |
---|
797 | { |
---|
798 | n = (a-b).cross(c-b); |
---|
799 | k = -(n.x*b.x+n.y*b.y+n.z*b.z); |
---|
800 | } |
---|
801 | |
---|
802 | /** |
---|
803 | \brief create a plane from anchor point and normal |
---|
804 | \param n: normal vector |
---|
805 | \param p: anchor point |
---|
806 | */ |
---|
807 | Plane::Plane (Vector norm, Vector p) |
---|
808 | { |
---|
809 | n = norm; |
---|
810 | k = -(n.x*p.x+n.y*p.y+n.z*p.z); |
---|
811 | } |
---|
812 | |
---|
813 | /** |
---|
814 | \brief returns the intersection point between the plane and a line |
---|
815 | \param l: a line |
---|
816 | */ |
---|
817 | Vector Plane::intersect_line (const Line& l) const |
---|
818 | { |
---|
819 | if (n.x*l.a.x+n.y*l.a.y+n.z*l.a.z == 0.0) return Vector(0,0,0); |
---|
820 | float t = (n.x*l.r.x+n.y*l.r.y+n.z*l.r.z+k) / (n.x*l.a.x+n.y*l.a.y+n.z*l.a.z); |
---|
821 | return l.r + (l.a * t); |
---|
822 | } |
---|
823 | |
---|
824 | /** |
---|
825 | \brief returns the distance between the plane and a point |
---|
826 | \param p: a Point |
---|
827 | \return the distance between the plane and the point (can be negative) |
---|
828 | */ |
---|
829 | float Plane::distance_point (const Vector& p) const |
---|
830 | { |
---|
831 | float l = n.len(); |
---|
832 | if( l == 0.0) return 0.0; |
---|
833 | return (n.dot(p) + k) / n.len(); |
---|
834 | } |
---|
835 | |
---|
836 | /** |
---|
837 | \brief returns the side a point is located relative to a Plane |
---|
838 | \param p: a Point |
---|
839 | \return 0 if the point is contained within the Plane, positive(negative) if the point is in the positive(negative) semi-space of the Plane |
---|
840 | */ |
---|
841 | float Plane::locate_point (const Vector& p) const |
---|
842 | { |
---|
843 | return (n.dot(p) + k); |
---|
844 | } |
---|
845 | |
---|
846 | |
---|
847 | /** |
---|
848 | \brief Creates a new BezierCurve |
---|
849 | */ |
---|
850 | BezierCurve::BezierCurve (void) |
---|
851 | { |
---|
852 | nodeCount = 0; |
---|
853 | firstNode = new PathNode; |
---|
854 | currentNode = firstNode; |
---|
855 | |
---|
856 | firstNode->position = Vector (.0, .0, .0); |
---|
857 | firstNode->number = 0; |
---|
858 | firstNode->next = 0; // not sure if this really points to NULL!! |
---|
859 | |
---|
860 | return; |
---|
861 | } |
---|
862 | |
---|
863 | /** |
---|
864 | \brief Deletes a BezierCurve. |
---|
865 | It does this by freeing all the space taken over from the nodes |
---|
866 | */ |
---|
867 | BezierCurve::~BezierCurve (void) |
---|
868 | { |
---|
869 | PathNode* tmpNode; |
---|
870 | currentNode = firstNode; |
---|
871 | while (tmpNode != 0) |
---|
872 | { |
---|
873 | tmpNode = currentNode; |
---|
874 | currentNode = currentNode->next; |
---|
875 | delete tmpNode; |
---|
876 | } |
---|
877 | } |
---|
878 | |
---|
879 | /** |
---|
880 | \brief adds a new Node to the bezier Curve |
---|
881 | \param newNode a Vector to the position of the new node |
---|
882 | */ |
---|
883 | void BezierCurve::addNode(const Vector& newNode) |
---|
884 | { |
---|
885 | PathNode* tmpNode; |
---|
886 | if (nodeCount == 0 ) |
---|
887 | tmpNode = firstNode; |
---|
888 | else |
---|
889 | { |
---|
890 | tmpNode = new PathNode; |
---|
891 | currentNode = currentNode->next = tmpNode; |
---|
892 | } |
---|
893 | tmpNode->position = newNode; |
---|
894 | tmpNode->next = 0; // not sure if this really points to NULL!! |
---|
895 | tmpNode->number = (++nodeCount); |
---|
896 | return; |
---|
897 | } |
---|
898 | |
---|
899 | /** |
---|
900 | \brief calculates the Position on the curve |
---|
901 | \param t The position on the Curve (0<=t<=1) |
---|
902 | \return the Position on the Path |
---|
903 | */ |
---|
904 | Vector BezierCurve::calcPos(float t) |
---|
905 | { |
---|
906 | if (nodeCount <=4) |
---|
907 | { |
---|
908 | // if (verbose >= 1) |
---|
909 | // printf ("Please define at least 4 nodes, until now you have only defined %i.\n", nodeCount); |
---|
910 | curvePoint = Vector(0,0,0); |
---|
911 | } |
---|
912 | PathNode* tmpNode = firstNode; |
---|
913 | |
---|
914 | int k,kn,nn,nkn; |
---|
915 | double blend,muk,munk; |
---|
916 | Vector b = Vector(0.0,0.0,0.0); |
---|
917 | |
---|
918 | muk = 1; |
---|
919 | munk = pow(1-t,(double)nodeCount); |
---|
920 | |
---|
921 | for (k=0; k<=nodeCount; k++) { |
---|
922 | nn = nodeCount; |
---|
923 | kn = k; |
---|
924 | nkn = nodeCount - k; |
---|
925 | blend = muk * munk; |
---|
926 | muk *= t; |
---|
927 | munk /= (1-t); |
---|
928 | while (nn >= 1) { |
---|
929 | blend *= nn; |
---|
930 | nn--; |
---|
931 | if (kn > 1) { |
---|
932 | blend /= (double)kn; |
---|
933 | kn--; |
---|
934 | } |
---|
935 | if (nkn > 1) { |
---|
936 | blend /= (double)nkn; |
---|
937 | nkn--; |
---|
938 | } |
---|
939 | } |
---|
940 | b.x += tmpNode->position.x * blend; |
---|
941 | b.y += tmpNode->position.y * blend; |
---|
942 | b.z += tmpNode->position.z * blend; |
---|
943 | |
---|
944 | tmpNode = tmpNode->next; |
---|
945 | } |
---|
946 | return b; |
---|
947 | } |
---|
948 | |
---|
949 | Vector BezierCurve::calcDirection (float t) |
---|
950 | { |
---|
951 | double diff = .00000000001; |
---|
952 | |
---|
953 | Vector diffV = ((calcPos(t+diff) - calcPos(t))/diff); |
---|
954 | diffV.normalize(); |
---|
955 | return diffV; |
---|
956 | } |
---|
957 | |
---|
958 | /** |
---|
959 | \brief returns the Position of the point calculated on the Curve |
---|
960 | \return a Vector to the calculated position |
---|
961 | */ |
---|
962 | Vector BezierCurve::getPos() const |
---|
963 | { |
---|
964 | return curvePoint; |
---|
965 | } |
---|