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