BRL-CAD
anim.c
Go to the documentation of this file.
1 /* A N I M . C
2  * BRL-CAD
3  *
4  * Copyright (c) 1993-2014 United States Government as represented by
5  * the U.S. Army Research Laboratory.
6  *
7  * This library is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU Lesser General Public License
9  * version 2.1 as published by the Free Software Foundation.
10  *
11  * This library is distributed in the hope that it will be useful, but
12  * WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14  * Lesser General Public License for more details.
15  *
16  * You should have received a copy of the GNU Lesser General Public
17  * License along with this file; see the file named COPYING for more
18  * information.
19  */
20 
21 #include "common.h"
22 
23 #include <stdio.h>
24 #include <math.h>
25 
26 #include "bu/log.h"
27 #include "vmath.h"
28 #include "bn/anim.h"
29 #include "anim.h"
30 
31 
32 #define NORMAL 0
33 #define ERROR1 1
34 #define ERROR2 2
35 
36 
37 void
39 {
40  int i;
41  fastf_t store;
42 
43  for (i=0; i<9; i+=4) {
44  store = m[i];
45  m[i] = -m[i+1];
46  m[i+1] = m[i+2];
47  m[i+2] = -store;
48  }
49 }
50 
51 
52 void
54 {
55  int i;
56  fastf_t store;
57 
58  for (i=0; i<9; i+=4) {
59  store = m[i+2];
60  m[i+2] = m[i+1];
61  m[i+1] = -m[i];
62  m[i] = -store;
63  }
64 }
65 
66 
67 void
68 anim_tran(mat_t m)
69 {
70  int i;
71  fastf_t store;
72 
73  int src[6] = { 1, 2, 3, 6, 7, 11 };
74  int dst[6] = { 4, 8, 12, 9, 13, 14};
75 
76  for (i=0; i<6; i++) {
77  store = m[dst[i]];
78  m[dst[i]] = m[src[i]];
79  m[src[i]] = store;
80  }
81 }
82 
83 
84 /***************************************
85  *ANIM_MAT2* - Conversions from matrices
86  ***************************************/
87 
88 int
89 anim_mat2zyx(const mat_t viewrot, vect_t angle)
90 {
91  int i, return_value, id_x, id_z;
92  fastf_t sin_x, sin_z, cos_x, cos_z, big_x, big_z;
93  static fastf_t previous[3];
94 
95  if (ZERO(viewrot[1]) && ZERO(viewrot[0])) {
96  return_value = ERROR1;
97  angle[0] = 0.0;
98  angle[2] = atan2(viewrot[4], viewrot[5]);
99  } else {
100  return_value = NORMAL;
101  angle[2] = atan2(-viewrot[1], viewrot[0]);
102  angle[0] = atan2(-viewrot[6], viewrot[10]);
103  }
104 
105  sin_x = sin(angle[0]);
106  sin_z = sin(angle[2]);
107  cos_x = cos(angle[0]);
108  cos_z = cos(angle[2]);
109 
110  /* in principle, we can use the sin_x or cos_x with sin_z or cos_z
111  * to figure out angle[1], as long as they are non-zero. To avoid
112  * ill-conditioning effects, we choose the two that are greatest
113  * in absolute value
114  */
115 
116  id_z = (fabs(sin_z) > fabs(cos_z)) ? 1 : 0;
117  big_z = id_z ? sin_z : cos_z;
118  id_x = (fabs(sin_x) > fabs(cos_x)) ? 1 : 0;
119  big_x = id_x ? sin_x : cos_x;
120 
121  if (fabs(big_x*big_z) < VDIVIDE_TOL) {
122  /* this should be impossible*/
123  /* unable to calculate pitch*/
124  return ERROR2;
125  } else if (id_x && (!id_z))
126  angle[1]=atan2((viewrot[4] - cos_x*sin_z)/(sin_x*cos_z), -viewrot[6]/sin_x);
127  else if ((!id_x) && (!id_z))
128  angle[1]=atan2((-viewrot[8] + sin_x*sin_z)/(cos_x*cos_z), viewrot[0]/cos_z);
129  else if (id_x && id_z)
130  angle[1]=atan2((-viewrot[5] + cos_x*cos_z)/(sin_x*sin_z), -viewrot[1]/sin_z);
131  else if ((!id_x) && id_z)
132  angle[1]=atan2((viewrot[9] - sin_x*cos_z)/(cos_x*sin_z), viewrot[10]/cos_x);
133 
134 
135  /* assume the smallest possible arc-length from frame to frame */
136  for (i=0; i<3; i++) {
137  while ((angle[i] - previous[i]) > M_PI)
138  angle[i] -= M_2PI;
139  while ((previous[i] - angle[i]) > M_PI)
140  angle[i] += M_2PI;
141  previous[i] = angle[i];
142  }
143 
144  return return_value;
145 }
146 
147 
148 int
149 anim_mat2ypr(mat_t viewrot, vect_t angle)
150 {
151  int i, return_value, id_y, id_r;
152  fastf_t sin_y, sin_r, cos_y, cos_r, big_y, big_r;
153  static fastf_t prev_angle[3];
154 
155  if (ZERO(viewrot[9]) && ZERO(viewrot[10])) {
156  return_value = ERROR1;
157  angle[2] = 0.0;
158  angle[0] = atan2(-viewrot[1], viewrot[5]);
159  } else {
160  return_value = NORMAL;
161  angle[0] = atan2(viewrot[4], viewrot[0]);
162  angle[2] = atan2(viewrot[9], viewrot[10]);
163  }
164 
165  sin_y = sin(angle[0]);
166  sin_r = sin(angle[2]);
167  cos_y = cos(angle[0]);
168  cos_r = cos(angle[2]);
169 
170  /* in principle, we can use sin_y or cos_y with sin_r or cos_r to
171  * figure out angle[1], as long as they are non-zero. To avoid
172  * ill-conditioning effects, we choose the two that are greatest
173  * in absolute value
174  */
175 
176  id_y = (fabs(sin_y) > fabs(cos_y)) ? 1 : 0;
177  big_y = id_y ? sin_y : cos_y;
178  id_r = (fabs(sin_r) > fabs(cos_r)) ? 1 : 0;
179  big_r = id_r ? sin_r : cos_r;
180 
181  if (fabs(big_y*big_r) < VDIVIDE_TOL) {
182  /* this should not happen */
183  /* unable to calculate pitch*/
184  return ERROR2;
185  } else if ((!id_y) && id_r)
186  angle[1] = atan2(-(viewrot[1]+sin_y*cos_r)/(cos_y*sin_r), viewrot[9]/sin_r);
187  else if (id_y && (!id_r))
188  angle[1] = atan2(-(viewrot[6]+cos_y*sin_r)/(sin_y*cos_r), viewrot[10]/cos_r);
189  else if (id_y && id_r)
190  angle[1] = atan2(-(viewrot[5]-cos_y*cos_r)/(sin_y*sin_r), viewrot[4]/sin_y);
191  else if ((!id_y) && (!id_r))
192  angle[1] = atan2(-(viewrot[2]-sin_y*sin_r)/(cos_y*cos_r), viewrot[0]/cos_y);
193 
194  /* assume the smallest possible arc-length from frame to frame */
195  for (i=0; i<3; i++) {
196  while ((angle[i] - prev_angle[i]) > M_PI)
197  angle[i] -= M_2PI;
198  while ((prev_angle[i] - angle[i]) > M_PI)
199  angle[i] += M_2PI;
200  prev_angle[i] = angle[i];
201  }
202 
203  return return_value;
204 }
205 
206 
207 int
208 anim_mat2quat(quat_t quat, const mat_t viewrot)
209 {
210  int i;
211  fastf_t qdiff[4], square, mag1, mag2;
212  static fastf_t prev_quat[4];
213 
214  square = 0.25 * (1 + viewrot[0] + viewrot[5] + viewrot[10]);
215  if (!ZERO(square)) {
216  quat[W] = sqrt(square);
217  quat[X] = 0.25 * (viewrot[9] - viewrot[6])/ quat[W];
218  quat[Y] = 0.25 * (viewrot[2] - viewrot[8])/ quat[W];
219  quat[Z] = 0.25 * (viewrot[4] - viewrot[1])/ quat[W];
220  } else {
221  quat[W] = 0.0;
222  square = -0.5 * (viewrot[5] + viewrot[10]);
223  if (!ZERO(square)) {
224  quat[X] = sqrt(square);
225  quat[Y] = 0.5 * viewrot[4] / quat[X];
226  quat[Z] = 0.5 * viewrot[8] / quat[X];
227  } else {
228  quat[X] = 0.0;
229  square = 0.5 * (1 - viewrot[10]);
230  if (!ZERO(square)) {
231  quat[Y] = sqrt(square);
232  quat[Z] = 0.5 * viewrot[9]/ quat[Y];
233  } else {
234  quat[Y] = 0.0;
235  quat[Z] = 1.0;
236  }
237  }
238  }
239 
240  /* quaternions on opposite sides of a four-dimensional sphere are
241  equivalent. Take the quaternion closest to the previous one */
242 
243  for (i=0; i<4; i++)
244  qdiff[i] = prev_quat[i] - quat[i];
245  mag1 = QMAGSQ(qdiff);
246  for (i=0; i<4; i++)
247  qdiff[i] = prev_quat[i] + quat[i];
248  mag2 = QMAGSQ(qdiff);
249 
250  for (i=0; i<4; i++) {
251  if (mag1 > mag2) /* inverse of quat would be closer */
252  quat[i] = -quat[i];
253  prev_quat[i] = quat[i];
254  }
255 
256  return 1;
257 }
258 
259 
260 /***************************************
261  *ANIM_*2MAT - Conversions to matrices
262  ***************************************/
263 
264 void
265 anim_ypr2mat(mat_t m, const vect_t a)
266 {
267  fastf_t cos_y, cos_p, cos_r, sin_y, sin_p, sin_r;
268 
269  cos_y = cos(a[0]);
270  cos_p = cos(a[1]);
271  cos_r = cos(a[2]);
272  sin_y = sin(a[0]);
273  sin_p = sin(a[1]);
274  sin_r = sin(a[2]);
275 
276  m[0] = cos_y*cos_p;
277  m[1] = -cos_y*sin_p*sin_r-sin_y*cos_r;
278  m[2] = -cos_y*sin_p*cos_r+sin_y*sin_r;
279  m[3] = 0;
280  m[4] = sin_y*cos_p;
281  m[5] = -sin_y*sin_p*sin_r+cos_y*cos_r;
282  m[6] = -sin_y*sin_p*cos_r-cos_y*sin_r;
283  m[7] = 0;
284  m[8]= sin_p;
285  m[9] = cos_p*sin_r;
286  m[10] = cos_p*cos_r;
287  m[11] = 0.0;
288  m[12] = 0.0;
289  m[13] = 0.0;
290  m[14] = 0.0;
291  m[15] = 1.0;
292 }
293 
294 
295 void
296 anim_ypr2vmat(mat_t m, const vect_t a)
297 {
298  fastf_t cos_y, cos_p, cos_r, sin_y, sin_p, sin_r;
299 
300  cos_y = cos(a[0]);
301  cos_p = cos(a[1]);
302  cos_r = cos(a[2]);
303  sin_y = sin(a[0]);
304  sin_p = sin(a[1]);
305  sin_r = sin(a[2]);
306 
307  m[0] = -cos_y*sin_p*sin_r-sin_y*cos_r;
308  m[1] = -sin_y*sin_p*sin_r+cos_y*cos_r;
309  m[2] = cos_p*sin_r;
310  m[3] = 0;
311  m[4] = -cos_y*sin_p*cos_r+sin_y*sin_r;
312  m[5] = -sin_y*sin_p*cos_r-cos_y*sin_r;
313  m[6] = cos_p*cos_r;
314  m[7] = 0;
315  m[8] = cos_y*cos_p;
316  m[9] = sin_y*cos_p;
317  m[10] = sin_p;
318  m[11] = 0.0;
319  m[12] = 0.0;
320  m[13] = 0.0;
321  m[14] = 0.0;
322  m[15] = 1.0;
323 }
324 
325 
326 void
327 anim_y_p_r2mat(mat_t m, double y, double p, double r)
328 {
329  fastf_t cos_y = cos(y);
330  fastf_t sin_y = sin(y);
331  fastf_t cos_p = cos(p);
332  fastf_t sin_p = sin(p);
333  fastf_t cos_r = cos(r);
334  fastf_t sin_r = sin(r);
335 
336  m[0] = cos_y*cos_p;
337  m[1] = -cos_y*sin_p*sin_r-sin_y*cos_r;
338  m[2] = -cos_y*sin_p*cos_r+sin_y*sin_r;
339  m[4] = sin_y*cos_p;
340  m[5] = -sin_y*sin_p*sin_r+cos_y*cos_r;
341  m[6] = -sin_y*sin_p*cos_r-cos_y*sin_r;
342  m[8]= sin_p;
343  m[9] = cos_p*sin_r;
344  m[10] = cos_p*cos_r;
345  m[3]=m[7]=m[11]=m[12]=m[13]=m[14]=0;
346  m[15]=1;
347 }
348 
349 
350 void
351 anim_dy_p_r2mat(mat_t m, double y, double p, double r)
352 {
353  fastf_t radian_yaw = y*DEG2RAD;
354  fastf_t radian_pitch = p*DEG2RAD;
355  fastf_t radian_roll = r*DEG2RAD;
356 
357  fastf_t cos_y = cos(radian_yaw);
358  fastf_t sin_y = sin(radian_yaw);
359  fastf_t cos_p = cos(radian_pitch);
360  fastf_t sin_p = sin(radian_pitch);
361  fastf_t cos_r = cos(radian_roll);
362  fastf_t sin_r = sin(radian_roll);
363 
364  m[0] = cos_y*cos_p;
365  m[1] = -cos_y*sin_p*sin_r-sin_y*cos_r;
366  m[2] = -cos_y*sin_p*cos_r+sin_y*sin_r;
367  m[4] = sin_y*cos_p;
368  m[5] = -sin_y*sin_p*sin_r+cos_y*cos_r;
369  m[6] = -sin_y*sin_p*cos_r-cos_y*sin_r;
370  m[8]= sin_p;
371  m[9] = cos_p*sin_r;
372  m[10] = cos_p*cos_r;
373  m[3]=m[7]=m[11]=m[12]=m[13]=m[14]=0;
374  m[15]=1;
375 }
376 
377 
378 void
379 anim_dy_p_r2vmat(mat_t m, double yaw, double pch, double rll)
380 {
381 
382  float ryaw = yaw*DEG2RAD;
383  float rpch = pch*DEG2RAD;
384  float rrll = rll*DEG2RAD;
385 
386  float cos_y = cos(ryaw);
387  float sin_y = sin(ryaw);
388  float cos_p = cos(rpch);
389  float sin_p = sin(rpch);
390  float cos_r = cos(rrll);
391  float sin_r = sin(rrll);
392 
393  m[0] = -cos_y*sin_p*sin_r-sin_y*cos_r;
394  m[1] = -sin_y*sin_p*sin_r+cos_y*cos_r;
395  m[2] = cos_p*sin_r;
396  m[4] = -cos_y*sin_p*cos_r+sin_y*sin_r;
397  m[5] = -sin_y*sin_p*cos_r-cos_y*sin_r;
398  m[6] = cos_p*cos_r;
399  m[8] = cos_y*cos_p;
400  m[9] = sin_y*cos_p;
401  m[10]= sin_p;
402  m[3]=m[7]=m[11]=0;
403  m[12]=m[13]=m[14]=0;
404  m[15]=1;
405 
406 }
407 
408 
409 void
410 anim_x_y_z2mat(mat_t m, double x, double y, double z)
411 {
412  fastf_t cosx = cos(x);
413  fastf_t sinx = sin(x);
414  fastf_t cosy = cos(y);
415  fastf_t siny = sin(y);
416  fastf_t cosz = cos(z);
417  fastf_t sinz = sin(z);
418 
419  m[0] = cosz*cosy;
420  m[1] = cosz*siny*sinx-sinz*cosx;
421  m[2] = cosz*siny*cosx+sinz*sinx;
422  m[4] = sinz*cosy;
423  m[5] = sinz*siny*sinx+cosz*cosx;
424  m[6] = sinz*siny*cosx-cosz*sinx;
425  m[8] = -siny;
426  m[9] = cosy*sinx;
427  m[10] = cosy*cosx;
428  m[3]=m[7]=m[11]=m[12]=m[13]=m[14]=0;
429  m[15]=1;
430 }
431 
432 
433 void
434 anim_dx_y_z2mat(mat_t m, double x, double y, double z)
435 {
436  fastf_t cosx, cosy, cosz, sinx, siny, sinz;
437 
438  x *= DEG2RAD;
439  y *= DEG2RAD;
440  z *= DEG2RAD;
441 
442  cosx = cos(x);
443  sinx = sin(x);
444  cosy = cos(y);
445  siny = sin(y);
446  cosz = cos(z);
447  sinz = sin(z);
448 
449  m[0] = cosz*cosy;
450  m[1] = cosz*siny*sinx-sinz*cosx;
451  m[2] = cosz*siny*cosx+sinz*sinx;
452  m[4] = sinz*cosy;
453  m[5] = sinz*siny*sinx+cosz*cosx;
454  m[6] = sinz*siny*cosx-cosz*sinx;
455  m[8] = -siny;
456  m[9] = cosy*sinx;
457  m[10] = cosy*cosx;
458  m[3]=m[7]=m[11]=m[12]=m[13]=m[14]=0.0;
459  m[15]=1.0;
460 }
461 
462 
463 void
464 anim_zyx2mat(mat_t m, const vect_t a)
465 {
466  fastf_t cosX, cosY, cosZ, sinX, sinY, sinZ;
467 
468  cosX = cos(a[0]);
469  cosY = cos(a[1]);
470  cosZ = cos(a[2]);
471  sinX = sin(a[0]);
472  sinY = sin(a[1]);
473  sinZ = sin(a[2]);
474 
475  m[0] = cosY*cosZ;
476  m[1] = -cosY*sinZ;
477  m[2] = sinY;
478  m[3] = 0;
479  m[4] = cosX*sinZ + sinX*sinY*cosZ;
480  m[5] = cosX*cosZ - sinX*sinY*sinZ;
481  m[6] = -sinX*cosY;
482  m[7] = 0;
483  m[8] = sinX*sinZ - cosX*sinY*cosZ;
484  m[9] = sinX*cosZ + cosX*sinY*sinZ;
485  m[10] = cosX*cosY;
486  m[11] = 0.0;
487  m[12] = 0.0;
488  m[13] = 0.0;
489  m[14] = 0.0;
490  m[15] = 1.0;
491 
492 }
493 
494 
495 void
496 anim_z_y_x2mat(mat_t m, double x, double y, double z)
497 {
498  fastf_t cosx = cos(x);
499  fastf_t sinx = sin(x);
500  fastf_t cosy = cos(y);
501  fastf_t siny = sin(y);
502  fastf_t cosz = cos(z);
503  fastf_t sinz = sin(z);
504 
505  m[0] = cosy*cosz;
506  m[1] = -cosy*sinz;
507  m[2] = siny;
508  m[4] = cosx*sinz + sinx*siny*cosz;
509  m[5] = cosx*cosz - sinx*siny*sinz;
510  m[6] = -sinx*cosy;
511  m[8] = sinx*sinz - cosx*siny*cosz;
512  m[9] = sinx*cosz + cosx*siny*sinz;
513  m[10]= cosx*cosy;
514  m[3]=m[7]=m[11]=m[12]=m[13]=m[14]=0.0;
515  m[15]=1.0;
516 }
517 
518 
519 void
520 anim_dz_y_x2mat(mat_t m, double x, double y, double z)
521 {
522  fastf_t cosx, cosy, cosz, sinx, siny, sinz;
523 
524  x *= DEG2RAD;
525  y *= DEG2RAD;
526  z *= DEG2RAD;
527 
528  cosx = cos(x);
529  sinx = sin(x);
530  cosy = cos(y);
531  siny = sin(y);
532  cosz = cos(z);
533  sinz = sin(z);
534 
535  m[0] = cosy*cosz;
536  m[1] = -cosy*sinz;
537  m[2] = siny;
538  m[4] = cosx*sinz + sinx*siny*cosz;
539  m[5] = cosx*cosz - sinx*siny*sinz;
540  m[6] = -sinx*cosy;
541  m[8] = sinx*sinz - cosx*siny*cosz;
542  m[9] = sinx*cosz + cosx*siny*sinz;
543  m[10]= cosx*cosy;
544  m[3]=m[7]=m[11]=m[12]=m[13]=m[14]=0;
545  m[15]=1;
546 }
547 
548 
549 void
550 anim_quat2mat(mat_t m, const quat_t qq)
551 {
552  fastf_t two_q[4];
553  quat_t q;
554 
555  QMOVE(q, qq);
556  QUNITIZE(q);
557 
558  VADD2N(two_q, q, q, 4);
559 
560  m[0] = 1.0 - two_q[Y]*q[Y] - two_q[Z]*q[Z];
561  m[1] = two_q[X]*q[Y] - two_q[W]*q[Z];
562  m[2] = two_q[X]*q[Z] + two_q[W]*q[Y];
563  m[3] = 0.0;
564  m[4] = two_q[X]*q[Y] + two_q[W]*q[Z];
565  m[5] = 1.0 - two_q[X]*q[X] - two_q[Z]*q[Z];
566  m[6] = two_q[Y]*q[Z] - two_q[W]*q[X];
567  m[7] = 0.0;
568  m[8] = two_q[X]*q[Z] - two_q[W]*q[Y];
569  m[9] = two_q[Y]*q[Z] + two_q[W]*q[X];
570  m[10] = 1.0 - two_q[X]*q[X] - two_q[Y]*q[Y];
571  m[11] = 0.0;
572  m[12] = 0.0;
573  m[13] = 0.0;
574  m[14] = 0.0;
575  m[15] = 1.0;
576 }
577 
578 
579 void
580 anim_dir2mat(mat_t m, const vect_t d, const vect_t d2b)
581 {
582  fastf_t hypotenuse, sign;
583  vect_t d2;
584 
585  VMOVE(d2, d2b);
586  sign = 1.0;
587  hypotenuse = sqrt(d[0]*d[0]+d[1]*d[1]);
588  if (hypotenuse < VDIVIDE_TOL) {
589  /* vertical direction - use d2 to determine roll */
590  hypotenuse = sqrt(d2[0]*d2[0]+d2[1]*d2[1]);
591  if (hypotenuse < VDIVIDE_TOL) {
592  /* use x-axis as default*/
593  VSET(d2, 1, 0, 0);
594  hypotenuse = 1;
595  }
596  if (d[2] < 0)
597  sign = -1.0;
598  m[1] = -d2[1]/hypotenuse;
599  m[5] = d2[0]/hypotenuse;
600  m[2] = -sign * d2[0]/hypotenuse;
601  m[6] = -sign * d2[1]/hypotenuse;
602  m[8] = sign;
603  m[0]=m[4]=m[9]=m[10]=0.0;
604  } else {
605  /* normal - no roll*/
606  m[0] = d[0];
607  m[1] = -d[1]/hypotenuse;
608  m[2] = -d[0]*d[2]/hypotenuse;
609  m[4] = d[1];
610  m[5] = d[0]/hypotenuse;
611  m[6] = -d[1]*d[2]/hypotenuse;
612  m[8] = d[2];
613  m[9] = 0.0;
614  m[10] = hypotenuse;
615  }
616  m[3]=m[7]=m[11]=0.0;
617  m[12]=m[13]=m[14]=0.0;
618  m[15]=1.0;
619 
620 }
621 
622 
623 void
624 anim_dirn2mat(mat_t m, const vect_t dx2, const vect_t dn)
625 {
626  vect_t temp;
627  fastf_t hyp, sign, inv, mag;
628  vect_t dx;
629 
630  VMOVE(dx, dx2);
631  mag = MAGNITUDE(dx);
632  if (mag < VDIVIDE_TOL) {
633  bu_log("anim_dirn2mat: Need non-zero vector");
634  return;
635  }
636  inv = 1.0/mag;
637  dx[0] *= inv;
638  dx[1] *= inv;
639  dx[2] *= inv;
640  hyp = sqrt(dx[0]*dx[0]+dx[1]*dx[1]);
641  if (hyp < VDIVIDE_TOL) {
642  /* vertical - special handling */
643  sign = (dx[2] < 0) ? -1.0 : 1.0;
644  VSET(temp, dn[0], dn[1], 0.0);
645  mag = MAGNITUDE(temp);
646  if (mag < VDIVIDE_TOL) {
647  /* use default */
648  VSET(temp, 0.0, 1.0, 0.0);
649  } else {
650  inv = 1.0/mag;
651  temp[0] *= inv;
652  temp[1] *= inv;
653  }
654  m[0] = 0.0;
655  m[4] = 0.0;
656  m[8] = sign;
657  m[1] = temp[0];
658  m[5] = temp[1];
659  m[9] = 0.0;
660  m[2] = -sign*temp[1];
661  m[6] = sign*temp[0];
662  m[10] = 0.0;
663  m[3]=m[7]=m[11]=0.0;
664  m[12]=m[13]=m[14]=0.0;
665  m[15]=1.0;
666  return;
667  }
668 
669  /*else normal*/
670  m[0] = dx[0];
671  m[1] = -dx[1]/hyp;
672  m[2] = -dx[0]*dx[2]/hyp;
673  m[4] = dx[1];
674  m[5] = dx[0]/hyp;
675  m[6] = -dx[1]*dx[2]/hyp;
676  m[8] = dx[2];
677  m[9] = 0.0;
678  m[10] = hyp;
679  m[3]=m[7]=m[11]=0.0;
680  m[12]=m[13]=m[14]=0.0;
681  m[15]=1.0;
682 
683 }
684 
685 
686 #define ASM_EMPTY 0
687 #define ASM_FIRST 1
688 #define ASM_FULL 2
689 
690 int
691 anim_steer_mat(mat_t mat, vect_t point, int end)
692 {
693  static vect_t p1, p2, p3;
694  vect_t dir = VINIT_ZERO;
695  static vect_t norm;
696  static int state = ASM_EMPTY;
697 
698  VMOVE(p1, p2);
699  VMOVE(p2, p3);
700  VMOVE(p3, point);
701 
702  switch (state) {
703  case ASM_EMPTY:
704  if (end) {
705  state = ASM_EMPTY;
706  } else {
707  state = ASM_FIRST;
708  /* "don't print yet */
709  }
710  return 0;
711  case ASM_FIRST:
712  if (end) {
713  /* only one point specified, use default direction */
714  VSET(dir, 1.0, 0.0, 0.0);
715  VSET(norm, 0.0, 1.0, 0.0);
716  state = ASM_EMPTY;
717  } else {
718  VSUBUNIT(dir, p3, p2);
719  VSET(norm, 0.0, 1.0, 0.0);
720  state = ASM_FULL;
721  }
722  break;
723  case ASM_FULL:
724  if (end) {
725  VSUBUNIT(dir, p2, p1);
726  state = ASM_EMPTY;
727  } else {
728  VSUBUNIT(dir, p3, p1);
729  state = ASM_FULL;
730  }
731  }
732 
733  /* go for it */
734  anim_dirn2mat(mat, dir, norm); /* create basic rotation matrix */
735  VSET(norm, mat[1], mat[5], 0.0); /* save for next time */
736  VMOVE(point, p2); /* for main's purposes, the current point is p2 */
737  return 1; /* return signal go ahead and print */
738 
739 }
740 
741 
742 /***************************************
743  * Other animation routines
744  ***************************************/
745 
746 
747 void
748 anim_add_trans(mat_t m, const vect_t post, const vect_t pre)
749 {
750  int i;
751  for (i=0; i<3; i++)
752  m[3+i*4] += m[i*4]*pre[0] + m[1+i*4]*pre[1]+m[2+i*4]*pre[2] + post[i];
753 
754 }
755 
756 
757 void
758 anim_rotatez(fastf_t a, vect_t d)
759 {
760  fastf_t temp[3];
761  fastf_t cos_y = cos(a);
762  fastf_t sin_y = sin(a);
763  temp[0] = d[0]*cos_y - d[1]*sin_y;
764  temp[1] = d[0]*sin_y + d[1]*cos_y;
765  d[0]=temp[0];
766  d[1]=temp[1];
767 }
768 
769 
770 void
771 anim_mat_print(FILE *fp, const mat_t m, int s_colon)
772 {
773  bu_flog(fp, "%.10g %.10g %.10g %.10g\n", m[0], m[1], m[2], m[3]);
774  bu_flog(fp, "%.10g %.10g %.10g %.10g\n", m[4], m[5], m[6], m[7]);
775  bu_flog(fp, "%.10g %.10g %.10g %.10g\n", m[8], m[9], m[10], m[11]);
776  bu_flog(fp, "%.10g %.10g %.10g %.10g", m[12], m[13], m[14], m[15]);
777  if (s_colon)
778  bu_flog(fp, ";");
779  bu_flog(fp, "\n");
780 }
781 
782 
783 void
785  FILE *fp,
786  const mat_t m,
787  const char *formstr,
788  const char *linestr,
789  const char *endstr)
790 {
791  char mystr[80];
792  snprintf(mystr, 80, "%s%s%s%s%%s", formstr, formstr, formstr, formstr);
793  bu_flog(fp, mystr, m[0], m[1], m[2], m[3], linestr);
794  bu_flog(fp, mystr, m[4], m[5], m[6], m[7], linestr);
795  bu_flog(fp, mystr, m[8], m[9], m[10], m[11], linestr);
796  bu_flog(fp, mystr, m[12], m[13], m[14], m[15], endstr);
797 }
798 
799 
800 void
802 {
803  m[0] = -m[0];
804  m[1] = -m[1];
805  m[4] = -m[4];
806  m[5] = -m[5];
807  m[8] = -m[8];
808  m[9] = -m[9];
809 }
810 
811 
812 /*
813  * Local Variables:
814  * mode: C
815  * tab-width: 8
816  * indent-tabs-mode: t
817  * c-file-style: "stroustrup"
818  * End:
819  * ex: shiftwidth=4 tabstop=8
820  */
HIDDEN int sign(double val)
Definition: brep.cpp:1145
void bu_log(const char *,...) _BU_ATTR_PRINTF12
Definition: log.c:176
void anim_view_rev(mat_t m)
Reverse the direction of a view matrix, keeping it right-side up.
Definition: anim.c:801
void anim_dx_y_z2mat(mat_t m, double x, double y, double z)
Make a rotation matrix corresponding to a rotation of "x" degrees about the x-axis, "y" degrees about the y-axis, and then "z" degrees about the z-axis.
Definition: anim.c:434
void anim_add_trans(mat_t m, const vect_t post, const vect_t pre)
Add pre- and post- translation to a rotation matrix. The resulting matrix has the effect of performin...
Definition: anim.c:748
void anim_y_p_r2mat(mat_t m, double y, double p, double r)
Make matrix to rotate an object to the given yaw, pitch, and roll. (Specified in radians.)
Definition: anim.c:327
#define VSET(a, b, c, d)
Definition: color.c:53
void anim_rotatez(fastf_t a, vect_t d)
Rotate the vector "d" through "a" radians about the z-axis.
Definition: anim.c:758
void anim_z_y_x2mat(mat_t m, double x, double y, double z)
Make a rotation matrix corresponding to a rotation of "z" radians about the z-axis, "y" radians about the y-axis, and then "x" radians about the x-axis.
Definition: anim.c:496
#define M_PI
Definition: fft.h:35
#define ERROR2
Definition: anim.c:34
void anim_dirn2mat(mat_t m, const vect_t dx2, const vect_t dn)
make a matrix which turns a vehicle from the x-axis to point in the desired direction, staying "right-side up". In cases where the direction is vertical, the second vector is consulted. The second vector defines a normal to the vertical plane into which the vehicle's x and z axes should be put. A good choice to put here is the direction of the vehicle's y-axis in the previous frame.
Definition: anim.c:624
void anim_dir2mat(mat_t m, const vect_t d, const vect_t d2b)
make a matrix which turns a vehicle from the x-axis to point in the desired direction, staying "right-side up" (i.e. the y-axis never has a z-component). A second direction vector is consulted when the given direction is vertical. This is intended to represent the direction from a previous frame.
Definition: anim.c:580
Header file for the BRL-CAD common definitions.
void anim_dz_y_x2mat(mat_t m, double x, double y, double z)
Make a rotation matrix corresponding to a rotation of "z" degrees about the z-axis, "y" degrees about the y-axis, and then "x" degrees about the x-axis.
Definition: anim.c:520
void anim_zyx2mat(mat_t m, const vect_t a)
Make a rotation matrix corresponding to a rotation of "z" radians about the z-axis, "y" radians about the y-axis, and then "x" radians about the x-axis.
Definition: anim.c:464
void anim_v_permute(mat_t m)
Routines useful in animation programs.
Definition: anim.c:38
void anim_mat_printf(FILE *fp, const mat_t m, const char *formstr, const char *linestr, const char *endstr)
print out 4X4 matrix. formstr must be less than twenty chars
Definition: anim.c:784
Definition: color.c:49
int anim_steer_mat(mat_t mat, vect_t point, int end)
given the next frame's position, remember the value of the previous frame's position and calculate a ...
Definition: anim.c:691
void anim_ypr2mat(mat_t m, const vect_t a)
Create a premultiplication rotation matrix to turn the front of an object (its x-axis) to the given y...
Definition: anim.c:265
#define ASM_FIRST
Definition: anim.c:687
void anim_v_unpermute(mat_t m)
Undo the mapping done by anim_v_permute().
Definition: anim.c:53
#define ERROR1
Definition: anim.c:33
Coord * point
Definition: chull3d.cpp:52
void anim_x_y_z2mat(mat_t m, double x, double y, double z)
Make a rotation matrix corresponding to a rotation of "x" radians about the x-axis, "y" radians about the y-axis, and then "z" radians about the z-axis.
Definition: anim.c:410
#define NORMAL
Definition: anim.c:32
#define ASM_FULL
Definition: anim.c:688
void anim_dy_p_r2mat(mat_t m, double y, double p, double r)
Make matrix to rotate an object to the given yaw, pitch, and roll. (Specified in degrees.)
Definition: anim.c:351
void anim_tran(mat_t m)
Transpose matrix in place.
Definition: anim.c:68
#define ZERO(val)
Definition: units.c:38
void anim_quat2mat(mat_t m, const quat_t qq)
Make 4x4 matrix from the given quaternion Note: these quaternions are the conjugates of the quaternio...
Definition: anim.c:550
void anim_ypr2vmat(mat_t m, const vect_t a)
Create a post-multiplication rotation matrix, which could be used to move the virtual camera to the g...
Definition: anim.c:296
void anim_dy_p_r2vmat(mat_t m, double yaw, double pch, double rll)
Make a view rotation matrix, given desired yaw, pitch and roll. (Note that the matrix is a permutatio...
Definition: anim.c:379
int anim_mat2zyx(const mat_t viewrot, vect_t angle)
Convert the rotational part of a 4x4 transformation matrix to zyx form, that is to say...
Definition: anim.c:89
Definition: color.c:51
#define ASM_EMPTY
Definition: anim.c:686
int anim_mat2quat(quat_t quat, const mat_t viewrot)
This interprets the rotational part of a 4x4 transformation matrix in terms of unit quaternions...
Definition: anim.c:208
double fastf_t
Definition: defines.h:300
int anim_mat2ypr(mat_t viewrot, vect_t angle)
Convert the rotational part of a 4x4 transformation matrix to yaw-pitch-roll form, that is to say, +roll degrees about the x-axis, -pitch degrees about the y-axis, and +yaw degrees about the z-axis. The angles are stored in radians as a vector in the order y, p, r. A return of ERROR1 means that arbitrary assumptions were necessary. ERROR2 means that the conversion failed.
Definition: anim.c:149
void bu_flog(FILE *, const char *,...) _BU_ATTR_PRINTF23
Definition: log.c:246
Definition: color.c:50
void anim_mat_print(FILE *fp, const mat_t m, int s_colon)
print out 4X4 matrix, with optional colon
Definition: anim.c:771