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