Visual Servoing Platform version 3.5.0
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 */
53void 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 */
68void 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 */
91void 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 */
110void 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 */
128void 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 */
145void 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 */
160void 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 */
192void 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 */
211void 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 */
229void 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 */
245void 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 */
261void 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 */
279float 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 */
301void 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 */
348float 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 */
370void 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 */
389void 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 */
407void 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 */
429void 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 */
453void 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 */
497void 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 */
524void 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 */
557void 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 */
589void 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 */
618void 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 */
665void 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 */
711void 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 */
725void 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 */
738void 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