Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
vpProjection.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 "projection.c" contient les procedures de calcul
33  * des matrices de projection perspective et parallele.
34  *
35  * Authors:
36  * Jean-Luc CORRE
37  *
38  *****************************************************************************/
39 
40 #include <visp3/core/vpConfig.h>
41 
42 #ifndef DOXYGEN_SHOULD_SKIP_THIS
43 #include "vpProjection.h"
44 #include <math.h>
45 #include <stdio.h>
46 
47 /*
48  * La procedure "View_to_Matrix" constuit la matrice homogene de projection
49  * a partir des parametres de la prise de vue.
50  * Entree :
51  * vp Parametres de la prise de vue.
52  * m Matrice homogene a construire.
53  */
54 void View_to_Matrix(View_parameters *vp, Matrix m)
55 {
56  static char proc_name[] = "View_to_Matrix";
57 
58  switch (vp->type) {
59  case PARALLEL:
60  set_parallel(vp, m);
61  break;
62  case PERSPECTIVE:
63  set_perspective(vp, m);
64  break;
65  default:
66  fprintf(stderr, "%s: bad view type\n", proc_name);
67  set_perspective(vp, m);
68  break;
69  }
70 }
71 
72 /*
73  * La procedure "set_zy" initialise la matrice par une composition :
74  * 1 - aligne le premier vecteur sur l'axe Z dans le sens negatif.
75  * 2 - aligne la projection du second vecteur sur l'axe Y.
76  * Entree :
77  * m Matrice a initialiser.
78  * v0 Premier vecteur.
79  * v1 Second vecteur.
80  */
81 static void set_zy(Matrix m, Vector *v0, Vector *v1)
82 {
83  Vector rx, ry, rz;
84 
85  SET_COORD3(rz, -v0->x, -v0->y, -v0->z);
86  CROSS_PRODUCT(rx, *v0, *v1);
87  norm_vector(&rx);
88  norm_vector(&rz);
89  CROSS_PRODUCT(ry, rz, rx); /* ry est norme */
90 
91  m[0][0] = rx.x;
92  m[0][1] = ry.x;
93  m[0][2] = rz.x;
94  m[0][3] = 0.0;
95  m[1][0] = rx.y;
96  m[1][1] = ry.y;
97  m[1][2] = rz.y;
98  m[1][3] = 0.0;
99  m[2][0] = rx.z;
100  m[2][1] = ry.z;
101  m[2][2] = rz.z;
102  m[2][3] = 0.0;
103  m[3][0] = 0.0;
104  m[3][1] = 0.0;
105  m[3][2] = 0.0;
106  m[3][3] = 1.0;
107 }
108 
109 /*
110  * La procedure "set_parallel" iniatilise la matrice de projection
111  * parallel "wc" par les parametres de visualisation "vp".
112  * Pour plus de renseignements :
113  * "Fundamentals of Interactive Computer Graphics"
114  * J.D. FOLEY, A. VAN DAM, Addison-Wesley. 1982, pp 285-290.
115  * Entree :
116  * vp Parametres de visualisation.
117  * wc Matrice a initialiser.
118  */
119 void set_parallel(View_parameters *vp, Matrix wc)
120 {
121  Matrix m = IDENTITY_MATRIX;
122  Point3f cop;
123  Point4f doprim;
124  Vector dop, v;
125 
126  /*
127  * 1 : Translation du point de reference VRP a l'origine.
128  */
129  SET_COORD3(v, -vp->vrp.x, -vp->vrp.y, -vp->vrp.z);
130  Translate_to_Matrix(&v, wc);
131  /*
132  * 2 : Rotation pour rendre VPN parallele a l'axe des Z negatifs.
133  * 3 : Rotation pour rendre la projection de VUP sur le plan de
134  * projection parallele a l'axe Y.
135  */
136  set_zy(m, &vp->vpn, &vp->vup);
137  /*
138  * 4 : Passer d'un repere droit (absolu) a un repere gauche (vision).
139  */
140  postleft_matrix(m, 'z');
141  postmult_matrix(wc, m);
142  /*
143  * 5 : Alignement de l'axe central du volume de vision sur l'axe Z.
144  * COP = DOP = Direction of Projection.
145  * DOPRIM = DOP * R_TRL
146  * Pas de translation dans la matrice R_TRL pour la transformation
147  * du vecteur DOP.
148  */
149  SET_COORD3(dop, vp->vrp.x - vp->cop.x, vp->vrp.y - vp->cop.y, vp->vrp.z - vp->cop.z);
150  norm_vector(&dop);
151  SET_COORD3(cop, dop.x, dop.y, dop.z);
152  point_matrix(&doprim, &cop, m);
153  ident_matrix(m);
154  m[2][0] = -doprim.x / doprim.z;
155  m[2][1] = -doprim.y / doprim.z;
156  postmult_matrix(wc, m);
157  /*
158  * 6 : Translation et Mise a l'echelle de la pyramide.
159  * Remarque : contrairement a la reference qui donne
160  * 0 < x < 1, 0 < y < 1, 0 < z < 1
161  * je prefere, afin de rester coherent avec la projection perspective,
162  * -1 < x < 1, -1 < y < 1, 0 < z < 1 (w = 1)
163  */
164  SET_COORD3(v, (float)(-(vp->vwd.umax + vp->vwd.umin) / 2.0), (float)(-(vp->vwd.vmax + vp->vwd.vmin) / 2.0),
165  (float)(-vp->depth.front));
166  posttrans_matrix(wc, &v);
167  SET_COORD3(v, (float)(2.0 / (vp->vwd.umax - vp->vwd.umin)), (float)(2.0 / (vp->vwd.vmax - vp->vwd.vmin)),
168  (float)(1.0 / (vp->depth.back - vp->depth.front)));
169  postscale_matrix(wc, &v);
170 }
171 
172 /*
173  * La procedure "set_perspective" iniatilise la matrice de projection
174  * perspective "wc" par les parametres de visualisation "vp".
175  * Pour plus de renseignements :
176  * "Fundamentals of Interactive Computer Graphics"
177  * J.D. FOLEY, A. VAN DAM, Addison-Wesley. 1982, pp 290-302.
178  * Entree :
179  * vp Parametres de visualisation.
180  * wc Matrice a initialiser.
181  */
182 void set_perspective(View_parameters *vp, Matrix wc)
183 {
184  Matrix m = IDENTITY_MATRIX;
185  Point4f vrprim, cw;
186  float zmin;
187  Vector v;
188 
189  /*
190  * 1 : Translation du centre de projection COP a l'origine.
191  */
192  SET_COORD3(v, -vp->cop.x, -vp->cop.y, -vp->cop.z);
193  Translate_to_Matrix(&v, wc);
194  /*
195  * 2 : Rotation pour rendre VPN parallele a l'axe des Z negatifs.
196  * 3 : Rotation pour rendre la projection de VUP sur le plan de
197  * projection parallele a l'axe Y.
198  */
199  set_zy(m, &vp->vpn, &vp->vup);
200  postmult_matrix(wc, m);
201  /*
202  * 4 : Passer d'un repere droit (absolu) a un repere gauche (vision).
203  */
204  postleft_matrix(wc, 'z');
205  /*
206  * 5 : Alignement de l'axe central du volume de vision sur l'axe Z.
207  */
208  point_matrix(&vrprim, &vp->vrp, wc);
209  cw.x = (float)(vrprim.x + (vp->vwd.umin + vp->vwd.umax) / 2.0);
210  cw.y = (float)(vrprim.y + (vp->vwd.vmin + vp->vwd.vmax) / 2.0);
211  cw.z = (float)(vrprim.z);
212  ident_matrix(m);
213  m[2][0] = -cw.x / cw.z;
214  m[2][1] = -cw.y / cw.z;
215  postmult_matrix(wc, m);
216  /*
217  * 6 : Mise a l'echelle de la pyramide.
218  */
219  SET_COORD3(v, (float)((2.0 * vrprim.z) / ((vp->vwd.umax - vp->vwd.umin) * (vrprim.z + vp->depth.back))),
220  (float)((2.0 * vrprim.z) / ((vp->vwd.vmax - vp->vwd.vmin) * (vrprim.z + vp->depth.back))),
221  (float)(1.0 / (vrprim.z + vp->depth.back)));
222  postscale_matrix(wc, &v);
223  /*
224  * 7 : Transformation perspective.
225  */
226  zmin = (vrprim.z + vp->depth.front) / (vrprim.z + vp->depth.back);
227  ident_matrix(m);
228  m[2][2] = (float)(1.0 / (1.0 - zmin));
229  m[2][3] = 1.0;
230  m[3][2] = (float)(-zmin / (1.0 - zmin));
231  m[3][3] = 0.0;
232  postmult_matrix(wc, m);
233 }
234 
235 #endif