ViSP
vpArit.cpp
1 /****************************************************************************
2  *
3  * $Id: vpArit.cpp 5284 2015-02-09 14:24:10Z fspindle $
4  *
5 * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  * Description:
34  * Le module "arit.c" contient les procedures arithmetiques.
35  *
36  * Authors:
37  * Jean-Luc CORRE
38  *
39  *****************************************************************************/
40 
41 
42 
43 
44 
45 #include <visp/vpMy.h>
46 #include <visp/vpArit.h>
47 #include <stdio.h>
48 #include <math.h>
49 #include <string.h>
50 
51 #if defined(VISP_USE_MSVC)
52 # ifndef M_PI
53 # define M_PI 3.14159265358979323846
54 # endif
55 # ifndef M_PI_2
56 # define M_PI_2 M_PI / 2
57 # endif
58 #endif
59 
60 
61 
62 #ifndef DOXYGEN_SHOULD_SKIP_THIS
63 /*
64  * La procedure "fprintf_matrix" affiche une matrice sur un fichier.
65  * Entree :
66  * fp Fichier en sortie.
67  * m Matrice a ecrire.
68  */
69 void
70 fprintf_matrix (FILE *fp, Matrix m)
71 {
72  int i;
73 
74  fprintf (fp, "(matrix\n");
75  for (i = 0; i < 4; i++)
76  fprintf (fp, "\t%.4f\t%.4f\t%.4f\t%.4f\n",
77  m[i][0], m[i][1], m[i][2], m[i][3]);
78  fprintf (fp, ")\n");
79 }
80 
81 /*
82  * La procedure "ident_matrix" initialise la matrice par la matrice identite.
83  * Entree :
84  * m Matrice a initialiser.
85  */
86 void
87 ident_matrix (Matrix m)
88 {
89  static Matrix identity = IDENTITY_MATRIX;
90 
91  //bcopy ((char *) identity, (char *) m, sizeof (Matrix));
92  memmove ((char *) m, (char *) identity, sizeof (Matrix));
93 /*
94  * Version moins rapide.
95  *
96  * int i, j;
97  *
98  * for (i = 0; i < 4; i++)
99  * for (j = 0; j < 4; j++)
100  * m[i][j] = (i == j) ? 1.0 : 0.0;
101  */
102 }
103 
104 /*
105  * La procedure "premult_matrix" pre multiplie la matrice par la seconde.
106  * Entree :
107  * a Premiere matrice du produit a = b * a.
108  * b Seconde matrice du produit.
109  */
110 void
111 premult_matrix (Matrix a, Matrix b)
112 {
113  Matrix m;
114  int i, j;
115 
116  for (i = 0; i < 4; i++)
117  for (j = 0; j < 4; j++)
118  m[i][j] = b[i][0] * a[0][j] +
119  b[i][1] * a[1][j] +
120  b[i][2] * a[2][j] +
121  b[i][3] * a[3][j];
122  //bcopy ((char *) m, (char *) a, sizeof (Matrix));
123  memmove ((char *) a, (char *) m, sizeof (Matrix));
124 }
125 
126 /*
127  * La procedure "premult3_matrix" premultiplie la matrice par une matrice 3x3.
128  * Note : La procedure "premult3_matrix" optimise "premutl_matrix".
129  * Entree :
130  * a Premiere matrice du produit a = b * a.
131  * b Seconde matrice du produit 3x3.
132  */
133 void
134 premult3_matrix (Matrix a, Matrix b)
135 {
136  Matrix m;
137  int i, j;
138 
139  //bcopy ((char *) a, (char *) m, sizeof (Matrix));
140  memmove ((char *) m, (char *) a, sizeof (Matrix));
141  for (i = 0; i < 3; i++)
142  for (j = 0; j < 4; j++)
143  a[i][j] = b[i][0] * m[0][j] +
144  b[i][1] * m[1][j] +
145  b[i][2] * m[2][j];
146 }
147 
148 /*
149  * La procedure "prescale_matrix" premultiplie la matrice par l'homothetie.
150  * Entree :
151  * m Matrice a multiplier m = vp * m.
152  * vp Vecteur d'homothetie.
153  */
154 void
155 prescale_matrix (Matrix m, Vector *vp)
156 {
157  int i;
158 
159  for (i = 0; i < 4; i++) {
160  m[0][i] *= vp->x;
161  m[1][i] *= vp->y;
162  m[2][i] *= vp->z;
163  }
164 }
165 
166 /*
167  * La procedure "pretrans_matrix" premultiplie la matrice par la translation.
168  * Entree :
169  * m Matrice a multiplier m = vp * m.
170  * vp Vecteur de translation.
171  */
172 void
173 pretrans_matrix (Matrix m, Vector *vp)
174 {
175  int i;
176 
177  for (i = 0; i < 4; i++)
178  m[3][i] += vp->x * m[0][i] + vp->y * m[1][i] + vp->z * m[2][i];
179 }
180 
181 /*
182  * La procedure "postleft_matrix" postmultiplie la matrice
183  * par une matrice gauche sur un des axes.
184  * Entree :
185  * m Matrice a rendre gauche m = m * left.
186  * axis Axe de la matrice gauche 'x', 'y' ou 'z'.
187  */
188 void
189 postleft_matrix (Matrix m, char axis)
190 {
191  static char proc_name[] = "postleft_matrix";
192 
193  int i;
194 
195  switch (axis) {
196  case 'x' :
197  for (i = 0; i < 4; i++) m[i][0] = - m[i][0];
198  break;
199  case 'y' :
200  for (i = 0; i < 4; i++) m[i][1] = - m[i][1];
201  break;
202  case 'z' :
203  for (i = 0; i < 4; i++) m[i][2] = - m[i][2];
204  break;
205  default :
206  fprintf (stderr, "%s: axis unknown\n", proc_name);
207  break;
208  }
209 }
210 
211 /*
212  * La procedure "postmult_matrix" post multiplie la matrice par la seconde.
213  * Entree :
214  * a Premiere matrice du produit a = a * b.
215  * b Seconde matrice du produit.
216  */
217 void
218 postmult_matrix (Matrix a, Matrix b)
219 {
220  Matrix m;
221  int i, j;
222 
223  for (i = 0; i < 4; i++)
224  for (j = 0; j < 4; j++)
225  m[i][j] = a[i][0] * b[0][j] +
226  a[i][1] * b[1][j] +
227  a[i][2] * b[2][j] +
228  a[i][3] * b[3][j];
229  //bcopy ((char *) m, (char *) a, sizeof (Matrix));
230  memmove ((char *) a, (char *) m, sizeof (Matrix));
231 }
232 
233 /*
234  * La procedure "postmult3_matrix" postmultiplie la matrice par une matrice 3x3.
235  * Note : La procedure "postmult3_matrix" optimise "postmutl_matrix".
236  * Entree :
237  * a Premiere matrice du produit a = a * b.
238  * b Seconde matrice du produit 3x3.
239  */
240 void
241 postmult3_matrix (Matrix a, Matrix b)
242 {
243  Matrix m;
244  int i, j;
245 
246  //bcopy ((char *) a, (char *) m, sizeof (Matrix));
247  memmove ((char *) m, (char *) a, sizeof (Matrix));
248  for (i = 0; i < 4; i++)
249  for (j = 0; j < 3; j++)
250  a[i][j] = m[i][0] * b[0][j] +
251  m[i][1] * b[1][j] +
252  m[i][2] * b[2][j];
253 }
254 
255 /*
256  * La procedure "postscale_matrix" post multiplie la matrice par l'homothetie.
257  * Entree :
258  * m Matrice a multiplier m = m * vp.
259  * vp Vecteur d'homothetie.
260  */
261 void
262 postscale_matrix (Matrix m, Vector *vp)
263 {
264  int i;
265 
266  for (i = 0; i < 4; i++) {
267  m[i][0] *= vp->x;
268  m[i][1] *= vp->y;
269  m[i][2] *= vp->z;
270  }
271 }
272 
273 /*
274  * La procedure "posttrans_matrix" post mutiplie la matrice par la translation.
275  * Entree :
276  * m Matrice a multiplier m = m * vp.
277  * vp Vecteur de translation.
278  */
279 void
280 posttrans_matrix (Matrix m, Vector *vp)
281 {
282  int i;
283 
284  for (i = 0; i < 4; i++) {
285  m[i][0] += m[i][3] * vp->x;
286  m[i][1] += m[i][3] * vp->y;
287  m[i][2] += m[i][3] * vp->z;
288  }
289 }
290 
291 /*
292  * La procedure "transpose_matrix" transpose la matrice.
293  * Entree :
294  * m Matrice a transposer.
295  */
296 void
297 transpose_matrix (Matrix m)
298 {
299  unsigned int i, j;
300  float t;
301 
302  for (i = 0; i < 4; i++)
303  for (j = 0; j < i; j++)
304  SWAP(m[i][j],m[j][i],t);
305 }
306 
307 /*
308  * La procedure "cosin_to_angle" calcule un angle a partir d'un cosinus
309  * et d'un sinus.
310  * Entree :
311  * ca, sa Cosinus et Sinus de l'angle.
312  * Sortie :
313  * Angle en radians.
314  */
315 float
316 cosin_to_angle (float ca, float sa)
317 {
318  float a; /* angle a calculer */
319 
320  if (FABS(ca) < M_EPSILON) {
321  a = (sa > (float)0.0) ? (float)M_PI_2 : (float)(- M_PI_2);
322  }
323  else {
324  a = (float) atan((double) (sa / ca));
325  if (ca < (float)0.0) a += (sa > (float)0.0) ? (float)M_PI : (float)(- M_PI);
326  }
327  return (a);
328 }
329 
330 /*
331  * La procedure "cosin_to_lut" precalcule les tables des "cosinus" et "sinus".
332  * Les tables possedent "2 ** level" entrees pour M_PI_2 radians.
333  * Entree :
334  * level Niveau de decomposition.
335  * coslut Table pour la fonction "cosinus".
336  * sinlut Table pour la fonction "sinus".
337  */
338 void
339 cosin_to_lut (Index level, float *coslut, float *sinlut)
340 {
341  int i;
342  int i_pi_2 = TWO_POWER(level);
343  int quad; /* quadrant courant */
344  float ca; /* cosinus courant */
345  double a; /* angle courant */
346  double step = M_PI_2 / (double) i_pi_2;
347 
348  quad = 0;
349  coslut[quad] = 1.0; sinlut[quad] = 0.0; /* 0 */
350  quad += i_pi_2;
351  coslut[quad] = 0.0; sinlut[quad] = 1.0; /* PI/2 */
352  quad += i_pi_2;
353  coslut[quad] = -1.0; sinlut[quad] = 0.0; /* PI */
354  quad += i_pi_2;
355  coslut[quad] = 0.0; sinlut[quad] = -1.0; /* 3PI/2*/
356 
357  for (i = 1, a = step; i < i_pi_2; i++, a += step) {
358  ca = (float) cos (a);
359  quad = 0;
360  coslut[quad + i] = ca; /* cos(a) */
361  quad += i_pi_2;
362  sinlut[quad - i] = ca; /* sin(PI/2-a) */
363  sinlut[quad + i] = ca; /* sin(PI/2+a) */
364  quad += i_pi_2;
365  coslut[quad - i] = - ca; /* cos(PI-a) */
366  coslut[quad + i] = - ca; /* cos(PI+a) */
367  quad += i_pi_2;
368  sinlut[quad - i] = - ca; /* sin(3PI/2-a) */
369  sinlut[quad + i] = - ca; /* sin(3PI/2+a) */
370  quad += i_pi_2;
371  coslut[quad - i] = ca; /* cos(2PI-a) */
372  }
373 }
374 
375 /*
376  * La procedure "norm_vector" normalise le vecteur.
377  * Si la norme est nulle la normalisation n'est pas effectuee.
378  * Entree :
379  * vp Le vecteur a norme.
380  * Sortie :
381  * La norme du vecteur.
382  */
383 float
384 norm_vector (Vector *vp)
385 {
386  static char proc_name[] = "norm_vector";
387 
388  float norm; /* norme du vecteur */
389 
390  if ((norm = (float) sqrt ((double) DOT_PRODUCT(*vp,*vp))) > M_EPSILON) {
391  vp->x /= norm;
392  vp->y /= norm;
393  vp->z /= norm;
394  }
395  else fprintf (stderr, "%s: nul vector\n", proc_name);
396  return (norm);
397 }
398 
399 /*
400  * La procedure "plane_norme" calcule le vecteur norme orthogonal au plan
401  * defini par les 3 points.
402  * Entree :
403  * np Le vecteur norme orthogonal au plan.
404  * ap, bp, cp Points formant un repere du plan.
405  */
406 void
407 plane_norme (Vector *np, Point3f *ap, Point3f *bp, Point3f *cp)
408 {
409  Vector u, v;
410 
411  DIF_COORD3(u,*bp,*ap); /* base orthonorme (ap, u, v) */
412  DIF_COORD3(v,*cp,*ap);
413  norm_vector (&u);
414  norm_vector (&v);
415  CROSS_PRODUCT (*np,u,v);
416 }
417 
418 /*
419  * La procedure "point_matrix" deplace un point 3D dans un espace 4D.
420  * Une matrice homogene 4x4 effectue le changement de repere.
421  * Entree :
422  * p4 Point homogene resultat = p3 x m.
423  * p3 Point a deplacer.
424  * m Matrice de changement de repere.
425  */
426 void
427 point_matrix (Point4f *p4, Point3f *p3, Matrix m)
428 {
429  float x = p3->x, y = p3->y, z = p3->z;
430 
431  p4->x = COORD3_COL(x,y,z,m,0);
432  p4->y = COORD3_COL(x,y,z,m,1);
433  p4->z = COORD3_COL(x,y,z,m,2);
434  p4->w = COORD3_COL(x,y,z,m,3);
435 }
436 
437 /*
438  * La procedure "point_3D_3D" deplace un tableau de points 3D dans un espace 3D.
439  * Une matrice 4x3 effectue le changement de repere.
440  * La quatrieme colonne de la matrice vaut [0, 0, 0, 1] et n'est pas utilisee.
441  * Entree :
442  * ip Tableau de points 3D a deplacer.
443  * size Taille du tableau "ip".
444  * m Matrice de changement de repere.
445  * Entree/Sortie :
446  * op Tableau de points 3D resultat.
447  */
448 void
449 point_3D_3D (Point3f *ip, int size, Matrix m, Point3f *op)
450 {
451  Point3f *pend = ip + size; /* borne de ip */
452  float x, y, z; /* [xyz] = *ip */
453 
454  for (; ip < pend; ip++, op++) {
455  x = ip->x;
456  y = ip->y;
457  z = ip->z;
458 
459  op->x = COORD3_COL(x,y,z,m,0);
460  op->y = COORD3_COL(x,y,z,m,1);
461  op->z = COORD3_COL(x,y,z,m,2);
462  }
463 }
464 
465 /*
466  * La procedure "point_3D_4D" deplace un tableau de points 3D dans un espace 4D.
467  * Une matrice homogene 4x4 effectue le changement de repere.
468  * Entree :
469  * p3 Tableau de points 3D a deplacer.
470  * size Taille du tableau "p3".
471  * m Matrice de changement de repere.
472  * Entree/Sortie :
473  * p4 Tableau de points 4D resultat.
474  */
475 void
476 point_3D_4D (Point3f *p3, int size, Matrix m, Point4f *p4)
477 {
478  Point3f *pend = p3 + size; /* borne de p3 */
479  float x, y, z; /* [xyz] = *p3 */
480 
481  for (; p3 < pend; p3++, p4++) {
482  x = p3->x;
483  y = p3->y;
484  z = p3->z;
485 
486  p4->x = COORD3_COL(x,y,z,m,0);
487  p4->y = COORD3_COL(x,y,z,m,1);
488  p4->z = COORD3_COL(x,y,z,m,2);
489  p4->w = COORD3_COL(x,y,z,m,3);
490  }
491 }
492 
493 /*
494  * La procedure "rotate_vector" transforme le vecteur
495  * par la rotation de sens trigonometrique d'angle et d'axe donnes.
496  * Entree :
497  * vp Vecteur a transformer.
498  * a Angle de rotation en degres.
499  * axis Vecteur directeur de l'axe de rotation.
500  */
501 void
502 rotate_vector (Vector *vp, float a, Vector *axis)
503 {
504  Vector n, u, v, cross;
505  float f;
506 
507  a *= (float)M_PI / (float)180.0; /* passage en radians */
508 
509  n = *axis; /* norme le vecteur directeur */
510  norm_vector (&n);
511 
512  /*
513  * Avant rotation, vp vaut :
514  * u + v
515  * Apres rotation, vp vaut :
516  * u + cos(a) * v + sin(a) * (n^vp)
517  * = u + cos(a) * v + sin(a) * (n^v)
518  * avec u = (vp.n) * n, v = vp-u;
519  * ou "u" est la projection de "vp" sur l'axe "axis",
520  * et "v" est la composante de "vp" perpendiculaire a "axis".
521  */
522  f = DOT_PRODUCT(*vp, n);
523  u = n;
524  MUL_COORD3(u,f,f,f); /* (vp.n) * n */
525 
526  DIF_COORD3(v,*vp,u); /* calcule "v" */
527 
528  f = (float) cos ((double) a);
529  MUL_COORD3(v,f,f,f); /* v * cos(a) */
530 
531  CROSS_PRODUCT(cross,n,*vp);
532  f = (float) sin ((double) a);
533  MUL_COORD3(cross,f,f,f); /* (n^v) * sin(a) */
534 
535  SET_COORD3(*vp,
536  u.x + v.x + cross.x,
537  u.y + v.y + cross.y,
538  u.z + v.z + cross.z);
539 }
540 
541 /*
542  * La procedure "upright_vector" calcule un vecteur perpendiculaire.
543  * Les vecteurs ont un produit scalaire nul.
544  * Entree :
545  * vp Vecteur origine.
546  * Entree/Sortie :
547  * up Vecteur perpendiculaire a vp.
548  */
549 void
550 upright_vector (Vector *vp, Vector *up)
551 {
552  static char proc_name[] = "upright_vector";
553 
554  if (FABS(vp->z) > M_EPSILON) { /* x et y sont fixes */
555  up->z = - (vp->x + vp->y) / vp->z;
556  up->x = up->y = 1.0;
557  }
558  else if (FABS(vp->y) > M_EPSILON) { /* x et z sont fixes */
559  up->y = - (vp->x + vp->z) / vp->y;
560  up->x = up->z = 1.0;
561  }
562  else if (FABS(vp->x) > M_EPSILON) { /* y et z sont fixes */
563  up->x = - (vp->y + vp->z) / vp->x;
564  up->y = up->z = 1.0;
565  }
566  else {
567  up->x = up->y = up->z = 0.0;
568  fprintf (stderr, "%s: nul vector\n", proc_name);
569  return;
570  }
571 }
572 
573 /*
574  * La procedure "Matrix_to_Position" initialise la position par la matrice.
575  * Si M est la matrice, et P la position : M = R.Sid.T, P = (R,Sid,T).
576  * On suppose que la matrice de rotation 3x3 de M est unitaire.
577  * Entree :
578  * m Matrice de rotation et de translation.
579  * pp Position a initialiser.
580  */
581 void
582 Matrix_to_Position (Matrix m, AritPosition *pp)
583 {
584  Matrix_to_Rotate (m, &pp->rotate);
585  SET_COORD3(pp->scale,1.0,1.0,1.0);
586  SET_COORD3(pp->translate,m[3][0],m[3][1],m[3][2]);
587 }
588 
589 /*
590  * La procedure "Matrix_to_Rotate" initialise la rotation par la matrice.
591  * Si M est la matrice, si R est la matrice de rotation :
592  *
593  * | m00 m01 m02 0 |
594  * M = Rx.Ry.Rz = | m10 m11 m12 0 |
595  * | m20 m21 m22 0 |
596  * | 0 0 0 1 |
597  *
598  * et m00 = cy.cz m01 = cy.sz m02 = -sy
599  * m10 = sx.sy.cz-cx.sz m11 = sx.sy.sz+cx.cz m12 = sx.cy
600  * m20 = cx.sy.cz+sx.sz m21 = cx.sy.sz-sx.cz m22 = cx.cy
601  * avec ci = cos Oi et si = sin Oi.
602  *
603  * R = Rx.Ry.Rz
604  * Rx rotation autour de Ox d'angle O1
605  * Ry rotation autour de Oy d'angle O2
606  * Rz rotation autour de Oz d'angle O3
607  *
608  * Singularite : si |ry| == 90 degres alors rz = 0,
609  * soit une rotation d'axe 0z et d'angle "rx + rz".
610  *
611  * Entree :
612  * m Matrice contenant la composition des rotations.
613  * vp Rotations par rapport aux axes d'un repere droit en degres.
614  */
615 void
616 Matrix_to_Rotate (Matrix m, Vector *vp)
617 {
618  float cx, sx; /* cosinus et sinus de la rotation sur l'axe X */
619  float cy, sy; /* cosinus et sinus de la rotation sur l'axe Y */
620  float cz, sz; /* cosinus et sinus de la rotation sur l'axe Z */
621 
622  sy = - m[0][2];
623  cy = (float) sqrt (1.0 - (double) (sy * sy));
624 
625  if (FABS(cy) > M_EPSILON) {
626  sz = m[0][1] / cy;
627  cz = m[0][0] / cy;
628 
629  sx = m[1][2] / cy;
630  cx = m[2][2] / cy;
631 
632  SET_COORD3(*vp,
633  cosin_to_angle (cx, sx),
634  cosin_to_angle (cy, sy),
635  cosin_to_angle (cz, sz));
636  }
637  else { /* RZ = 0 => Ry = +/- 90 degres */
638  sx = m[1][1];
639  cx = - m[2][1];
640 
641  SET_COORD3(*vp,
642  cosin_to_angle (cx, sx),
643  (sy > (float)0.0) ? (float)M_PI_2 : (float)(- M_PI_2),
644  (float)0.0);
645  }
646  vp->x *= (float)180.0 / (float)M_PI; /* passage en degres */
647  vp->y *= (float)180.0 / (float)M_PI;
648  vp->z *= (float)180.0 / (float)M_PI;
649 }
650 
651 /*
652  * La procedure "Position_to_Matrix" initialise la matrice par la position.
653  * Matrice resultat : M = Sx.Sy.Sz.Rx.Ry.Rz.Tx.Ty.Tz
654  * Entree :
655  * pp Position de reference.
656  * m Matrice a initialiser.
657  */
658 void
659 Position_to_Matrix (AritPosition *pp, Matrix m)
660 {
661  Rotate_to_Matrix (&pp->rotate, m); /* rotation */
662  prescale_matrix (m, &pp->scale); /* homothetie */
663  m[3][0] = pp->translate.x; /* translation */
664  m[3][1] = pp->translate.y;
665  m[3][2] = pp->translate.z;
666 }
667 
668 /*
669  * La procedure "Rotate_to_Matrix" initialise la matrice par la rotation.
670  *
671  * | m00 m01 m02 0 |
672  * M = Rx.Ry.Rz = | m10 m11 m12 0 |
673  * | m20 m21 m22 0 |
674  * | 0 0 0 1 |
675  *
676  * Rx rotation autour de Ox d'angle O1
677  * Ry rotation autour de Oy d'angle O2
678  * Rz rotation autour de Oz d'angle O3
679  * et m00 = cy.cz m01 = cy.sz m02 = -sy
680  * m10 = sx.sy.cz-cx.sz m11 = sx.sy.sz+cx.cz m12 = sx.cy
681  * m20 = cx.sy.cz+sx.sz m21 = cx.sy.sz-sx.cz m22 = cx.cy
682  * avec ci = cos Oi et si = sin Oi.
683  *
684  * Entree :
685  * vp Rotations par rapport aux axes d'un repere droit en degres.
686  * m Matrice a initialiser.
687  */
688 void
689 Rotate_to_Matrix (Vector *vp, Matrix m)
690 {
691  float rx = vp->x * (float)M_PI / (float)180.0, /* passage en radians */
692  ry = vp->y * (float)M_PI / (float)180.0,
693  rz = vp->z * (float)M_PI / (float)180.0;
694  float cx = (float) cos ((double) rx),
695  sx = (float) sin ((double) rx),
696  cy = (float) cos ((double) ry),
697  sy = (float) sin ((double) ry),
698  cz = (float) cos ((double) rz),
699  sz = (float) sin ((double) rz);
700 
701  m[0][0] = cy * cz;
702  m[1][0] = (sx * sy * cz) - (cx * sz);
703  m[2][0] = (cx * sy * cz) + (sx * sz);
704 
705  m[0][1] = cy * sz;
706  m[1][1] = (sx * sy * sz) + (cx * cz);
707  m[2][1] = (cx * sy * sz) - (sx * cz);
708 
709  m[0][2] = - sy;
710  m[1][2] = sx * cy;
711  m[2][2] = cx * cy;
712 
713  m[0][3] = m[1][3] = m[2][3] = 0.0;
714  m[3][0] = m[3][1] = m[3][2] = 0.0;
715  m[3][3] = 1.0;
716 }
717 
718 /*
719  * La procedure "Rotaxis_to_Matrix" initialise la matrice par la rotation
720  * d'angle et d'axe donnes.
721  * Si M est la matrice, O l'angle et N le vecteur directeur de l'axe :
722  *
723  * M = cos(O) Id3 + (1 - cosO) Nt N + sinO N~
724  *
725  * | NxNxverO+ cosO NxNyverO+NzsinO NxNzverO-NxsinO 0 |
726  * M = | NxNyverO-NzsinO NyNyverO+ cosO NyNzverO+NxsinO 0 |
727  * | NxNzverO+NysinO NyNzverO-NxsinO NzNzverO+ cosO 0 |
728  * | 0 0 0 1 |
729  *
730  * O angle de rotation.
731  * N Vecteur directeur norme de l'axe de rotation.
732  * Nt Vecteur transpose.
733  * N~ | 0 Nz -Ny|
734  * |-Nz 0 Nx|
735  * | Ny -Nx 0 |
736  * Entree :
737  * a Angle de rotation en degres.
738  * axis Vecteur directeur de l'axe de la rotation.
739  * m Matrice a initialiser.
740  */
741 void
742 Rotaxis_to_Matrix (float a, Vector *axis, Matrix m)
743 {
744  float cosa;
745  float sina;
746  float vera; /* 1 - cosa */
747  Vector n; /* vecteur norme*/
748  Vector conv; /* verO n */
749  Vector tilde; /* sinO n */
750 
751  a *= (float)M_PI / (float)180.0; /* passage en radians */
752 
753  cosa = (float) cos ((double) a);
754  sina = (float) sin ((double) a);
755  vera = (float)1.0 - cosa;
756 
757  n = *axis; /* norme le vecteur directeur */
758  norm_vector (&n);
759  tilde = conv = n;
760  MUL_COORD3(conv,vera,vera,vera);
761  MUL_COORD3(tilde,sina,sina,sina);
762 
763  m[0][0] = conv.x * n.x + cosa;
764  m[0][1] = conv.x * n.y + tilde.z;
765  m[0][2] = conv.x * n.z - tilde.y;
766 
767  m[1][0] = conv.y * n.x - tilde.z;
768  m[1][1] = conv.y * n.y + cosa;
769  m[1][2] = conv.y * n.z + tilde.x;
770 
771  m[2][0] = conv.z * n.x + tilde.y;
772  m[2][1] = conv.z * n.y - tilde.x;
773  m[2][2] = conv.z * n.z + cosa;
774 
775  m[0][3] = m[2][3] = m[1][3] = 0.0;
776  m[3][0] = m[3][1] = m[3][2] = 0.0;
777  m[3][3] = 1.0;
778 }
779 
780 /*
781  * La procedure "Rotrans_to_Matrix" initialise la matrice par la rotation
782  * et de la translation.
783  * Entree :
784  * rp Vecteur des angles de rotation en degres.
785  * tp Vecteur des coordonnees de translation.
786  * m Matrice a initialiser.
787  */
788 void
789 Rotrans_to_Matrix (Vector *rp, Vector *tp, Matrix m)
790 {
791  Rotate_to_Matrix (rp, m); /* matrice de rotation */
792  m[3][0] = tp->x; /* matrice de translation */
793  m[3][1] = tp->y;
794  m[3][2] = tp->z;
795 }
796 
797 /*
798  * La procedure "Scale_to_Matrix" initialise la matrice par l'homothetie.
799  * Entree :
800  * vp Vecteur des coordonnees d'homothetie.
801  * m Matrice a initialiser.
802  */
803 void
804 Scale_to_Matrix (Vector *vp, Matrix m)
805 {
806  ident_matrix (m);
807  m[0][0] = vp->x;
808  m[1][1] = vp->y;
809  m[2][2] = vp->z;
810 }
811 
812 /*
813  * La procedure "Translate_to_Matrix" initialise la matrice par la translation.
814  * Entree :
815  * vp Vecteur des coordonnees de translation.
816  * m Matrice a initialiser.
817  */
818 void
819 Translate_to_Matrix (Vector *vp, Matrix m)
820 {
821  ident_matrix (m);
822  m[3][0] = vp->x;
823  m[3][1] = vp->y;
824  m[3][2] = vp->z;
825 }
826 
827 #endif