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