45 #include <visp/vpMy.h>
46 #include <visp/vpArit.h>
51 #if defined(VISP_USE_MSVC)
53 # define M_PI 3.14159265358979323846
56 # define M_PI_2 M_PI / 2
62 #ifndef DOXYGEN_SHOULD_SKIP_THIS
70 fprintf_matrix (FILE *fp, Matrix m)
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]);
87 ident_matrix (Matrix m)
89 static Matrix identity = IDENTITY_MATRIX;
92 memmove ((
char *) m, (
char *) identity,
sizeof (Matrix));
111 premult_matrix (Matrix a, Matrix b)
116 for (i = 0; i < 4; i++)
117 for (j = 0; j < 4; j++)
118 m[i][j] = b[i][0] * a[0][j] +
123 memmove ((
char *) a, (
char *) m,
sizeof (Matrix));
134 premult3_matrix (Matrix a, Matrix b)
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] +
155 prescale_matrix (Matrix m, Vector *vp)
159 for (i = 0; i < 4; i++) {
173 pretrans_matrix (Matrix m, Vector *vp)
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];
189 postleft_matrix (Matrix m,
char axis)
191 static char proc_name[] =
"postleft_matrix";
197 for (i = 0; i < 4; i++) m[i][0] = - m[i][0];
200 for (i = 0; i < 4; i++) m[i][1] = - m[i][1];
203 for (i = 0; i < 4; i++) m[i][2] = - m[i][2];
206 fprintf (stderr,
"%s: axis unknown\n", proc_name);
218 postmult_matrix (Matrix a, Matrix b)
223 for (i = 0; i < 4; i++)
224 for (j = 0; j < 4; j++)
225 m[i][j] = a[i][0] * b[0][j] +
230 memmove ((
char *) a, (
char *) m,
sizeof (Matrix));
241 postmult3_matrix (Matrix a, Matrix b)
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] +
262 postscale_matrix (Matrix m, Vector *vp)
266 for (i = 0; i < 4; i++) {
280 posttrans_matrix (Matrix m, Vector *vp)
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;
297 transpose_matrix (Matrix m)
302 for (i = 0; i < 4; i++)
303 for (j = 0; j < i; j++)
304 SWAP(m[i][j],m[j][i],t);
316 cosin_to_angle (
float ca,
float sa)
320 if (FABS(ca) < M_EPSILON) {
321 a = (sa > (float)0.0) ? (float)M_PI_2 : (
float)(- M_PI_2);
324 a = (float) atan((
double) (sa / ca));
325 if (ca < (
float)0.0) a += (sa > (float)0.0) ? (float)M_PI : (
float)(- M_PI);
339 cosin_to_lut (Index level,
float *coslut,
float *sinlut)
342 int i_pi_2 = TWO_POWER(level);
346 double step = M_PI_2 / (double) i_pi_2;
349 coslut[quad] = 1.0; sinlut[quad] = 0.0;
351 coslut[quad] = 0.0; sinlut[quad] = 1.0;
353 coslut[quad] = -1.0; sinlut[quad] = 0.0;
355 coslut[quad] = 0.0; sinlut[quad] = -1.0;
357 for (i = 1, a = step; i < i_pi_2; i++, a += step) {
358 ca = (float) cos (a);
360 coslut[quad + i] = ca;
362 sinlut[quad - i] = ca;
363 sinlut[quad + i] = ca;
365 coslut[quad - i] = - ca;
366 coslut[quad + i] = - ca;
368 sinlut[quad - i] = - ca;
369 sinlut[quad + i] = - ca;
371 coslut[quad - i] = ca;
384 norm_vector (Vector *vp)
386 static char proc_name[] =
"norm_vector";
390 if ((norm = (
float) sqrt ((
double) DOT_PRODUCT(*vp,*vp))) > M_EPSILON) {
395 else fprintf (stderr,
"%s: nul vector\n", proc_name);
407 plane_norme (Vector *np, Point3f *ap, Point3f *bp, Point3f *cp)
411 DIF_COORD3(u,*bp,*ap);
412 DIF_COORD3(v,*cp,*ap);
415 CROSS_PRODUCT (*np,u,v);
427 point_matrix (Point4f *p4, Point3f *p3, Matrix m)
429 float x = p3->x, y = p3->y, z = p3->z;
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);
449 point_3D_3D (Point3f *ip,
int size, Matrix m, Point3f *op)
451 Point3f *pend = ip + size;
454 for (; ip < pend; ip++, op++) {
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);
476 point_3D_4D (Point3f *p3,
int size, Matrix m, Point4f *p4)
478 Point3f *pend = p3 + size;
481 for (; p3 < pend; p3++, p4++) {
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);
502 rotate_vector (Vector *vp,
float a, Vector *axis)
504 Vector n, u, v, cross;
507 a *= (float)M_PI / (
float)180.0;
522 f = DOT_PRODUCT(*vp, n);
528 f = (float) cos ((
double) a);
531 CROSS_PRODUCT(cross,n,*vp);
532 f = (float) sin ((
double) a);
533 MUL_COORD3(cross,f,f,f);
538 u.z + v.z + cross.z);
550 upright_vector (Vector *vp, Vector *up)
552 static char proc_name[] =
"upright_vector";
554 if (FABS(vp->z) > M_EPSILON) {
555 up->z = - (vp->x + vp->y) / vp->z;
558 else if (FABS(vp->y) > M_EPSILON) {
559 up->y = - (vp->x + vp->z) / vp->y;
562 else if (FABS(vp->x) > M_EPSILON) {
563 up->x = - (vp->y + vp->z) / vp->x;
567 up->x = up->y = up->z = 0.0;
568 fprintf (stderr,
"%s: nul vector\n", proc_name);
582 Matrix_to_Position (Matrix m, AritPosition *pp)
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]);
616 Matrix_to_Rotate (Matrix m, Vector *vp)
623 cy = (float) sqrt (1.0 - (
double) (sy * sy));
625 if (FABS(cy) > M_EPSILON) {
633 cosin_to_angle (cx, sx),
634 cosin_to_angle (cy, sy),
635 cosin_to_angle (cz, sz));
642 cosin_to_angle (cx, sx),
643 (sy > (
float)0.0) ? (
float)M_PI_2 : (
float)(- M_PI_2),
646 vp->x *= (float)180.0 / (
float)M_PI;
647 vp->y *= (float)180.0 / (
float)M_PI;
648 vp->z *= (float)180.0 / (
float)M_PI;
659 Position_to_Matrix (AritPosition *pp, Matrix m)
661 Rotate_to_Matrix (&pp->rotate, m);
662 prescale_matrix (m, &pp->scale);
663 m[3][0] = pp->translate.x;
664 m[3][1] = pp->translate.y;
665 m[3][2] = pp->translate.z;
689 Rotate_to_Matrix (Vector *vp, Matrix m)
691 float rx = vp->x * (float)M_PI / (
float)180.0,
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);
702 m[1][0] = (sx * sy * cz) - (cx * sz);
703 m[2][0] = (cx * sy * cz) + (sx * sz);
706 m[1][1] = (sx * sy * sz) + (cx * cz);
707 m[2][1] = (cx * sy * sz) - (sx * cz);
713 m[0][3] = m[1][3] = m[2][3] = 0.0;
714 m[3][0] = m[3][1] = m[3][2] = 0.0;
742 Rotaxis_to_Matrix (
float a, Vector *axis, Matrix m)
751 a *= (float)M_PI / (
float)180.0;
753 cosa = (float) cos ((
double) a);
754 sina = (float) sin ((
double) a);
755 vera = (float)1.0 - cosa;
760 MUL_COORD3(conv,vera,vera,vera);
761 MUL_COORD3(tilde,sina,sina,sina);
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;
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;
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;
775 m[0][3] = m[2][3] = m[1][3] = 0.0;
776 m[3][0] = m[3][1] = m[3][2] = 0.0;
789 Rotrans_to_Matrix (Vector *rp, Vector *tp, Matrix m)
791 Rotate_to_Matrix (rp, m);
804 Scale_to_Matrix (Vector *vp, Matrix m)
819 Translate_to_Matrix (Vector *vp, Matrix m)