Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpProjection.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Le module "projection.c" contient les procedures de calcul
32  * des matrices de projection perspective et parallele.
33  *
34  * Authors:
35  * Jean-Luc CORRE
36  *
37  *****************************************************************************/
38 
39 
40 #include <visp3/core/vpConfig.h>
41 
42 #ifndef DOXYGEN_SHOULD_SKIP_THIS
43 #include "vpProjection.h"
44 #include <stdio.h>
45 #include <math.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 /*
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; m[0][1] = ry.x; m[0][2] = rz.x; m[0][3] = 0.0;
93  m[1][0] = rx.y; m[1][1] = ry.y; m[1][2] = rz.y; m[1][3] = 0.0;
94  m[2][0] = rx.z; m[2][1] = ry.z; m[2][2] = rz.z; m[2][3] = 0.0;
95  m[3][0] = 0.0 ; m[3][1] = 0.0 ; m[3][2] = 0.0 ; m[3][3] = 1.0;
96 }
97 
98 /*
99  * La procedure "set_parallel" iniatilise la matrice de projection
100  * parallel "wc" par les parametres de visualisation "vp".
101  * Pour plus de renseignements :
102  * "Fundamentals of Interactive Computer Graphics"
103  * J.D. FOLEY, A. VAN DAM, Addison-Wesley. 1982, pp 285-290.
104  * Entree :
105  * vp Parametres de visualisation.
106  * wc Matrice a initialiser.
107  */
108 void set_parallel (View_parameters *vp, Matrix wc)
109 {
110  Matrix m = IDENTITY_MATRIX;
111  Point3f cop;
112  Point4f doprim;
113  Vector dop, v;
114 
115  /*
116  * 1 : Translation du point de reference VRP a l'origine.
117  */
118  SET_COORD3(v,- vp->vrp.x, - vp->vrp.y, - vp->vrp.z);
119  Translate_to_Matrix (&v, wc);
120  /*
121  * 2 : Rotation pour rendre VPN parallele a l'axe des Z negatifs.
122  * 3 : Rotation pour rendre la projection de VUP sur le plan de
123  * projection parallele a l'axe Y.
124  */
125  set_zy (m, &vp->vpn, &vp->vup);
126  /*
127  * 4 : Passer d'un repere droit (absolu) a un repere gauche (vision).
128  */
129  postleft_matrix (m, 'z');
130  postmult_matrix (wc, m);
131  /*
132  * 5 : Alignement de l'axe central du volume de vision sur l'axe Z.
133  * COP = DOP = Direction of Projection.
134  * DOPRIM = DOP * R_TRL
135  * Pas de translation dans la matrice R_TRL pour la transformation
136  * du vecteur DOP.
137  */
138  SET_COORD3(dop,
139  vp->vrp.x - vp->cop.x,
140  vp->vrp.y - vp->cop.y,
141  vp->vrp.z - vp->cop.z);
142  norm_vector (&dop);
143  SET_COORD3(cop,dop.x,dop.y,dop.z);
144  point_matrix (&doprim, &cop, m);
145  ident_matrix (m);
146  m[2][0] = - doprim.x / doprim.z;
147  m[2][1] = - doprim.y / doprim.z;
148  postmult_matrix (wc, m);
149  /*
150  * 6 : Translation et Mise a l'echelle de la pyramide.
151  * Remarque : contrairement a la reference qui donne
152  * 0 < x < 1, 0 < y < 1, 0 < z < 1
153  * je prefere, afin de rester coherent avec la projection perspective,
154  * -1 < x < 1, -1 < y < 1, 0 < z < 1 (w = 1)
155  */
156  SET_COORD3(v,
157  (float)(-(vp->vwd.umax + vp->vwd.umin) / 2.0),
158  (float)(-(vp->vwd.vmax + vp->vwd.vmin) / 2.0),
159  (float)(-vp->depth.front));
160  posttrans_matrix (wc, &v);
161  SET_COORD3(v,
162  (float)(2.0 / (vp->vwd.umax - vp->vwd.umin)),
163  (float)(2.0 / (vp->vwd.vmax - vp->vwd.vmin)),
164  (float)(1.0 / (vp->depth.back - vp->depth.front)));
165  postscale_matrix (wc, &v);
166 }
167 
168 /*
169  * La procedure "set_perspective" iniatilise la matrice de projection
170  * perspective "wc" par les parametres de visualisation "vp".
171  * Pour plus de renseignements :
172  * "Fundamentals of Interactive Computer Graphics"
173  * J.D. FOLEY, A. VAN DAM, Addison-Wesley. 1982, pp 290-302.
174  * Entree :
175  * vp Parametres de visualisation.
176  * wc Matrice a initialiser.
177  */
178 void set_perspective (View_parameters *vp, Matrix wc)
179 {
180  Matrix m = IDENTITY_MATRIX;
181  Point4f vrprim, cw;
182  float zmin;
183  Vector v;
184 
185  /*
186  * 1 : Translation du centre de projection COP a l'origine.
187  */
188  SET_COORD3(v,- vp->cop.x, - vp->cop.y, - vp->cop.z);
189  Translate_to_Matrix (&v, wc);
190  /*
191  * 2 : Rotation pour rendre VPN parallele a l'axe des Z negatifs.
192  * 3 : Rotation pour rendre la projection de VUP sur le plan de
193  * projection parallele a l'axe Y.
194  */
195  set_zy (m, &vp->vpn, &vp->vup);
196  postmult_matrix (wc, m);
197  /*
198  * 4 : Passer d'un repere droit (absolu) a un repere gauche (vision).
199  */
200  postleft_matrix (wc, 'z');
201  /*
202  * 5 : Alignement de l'axe central du volume de vision sur l'axe Z.
203  */
204  point_matrix (&vrprim, &vp->vrp, wc);
205  cw.x = (float)(vrprim.x + (vp->vwd.umin + vp->vwd.umax) / 2.0);
206  cw.y = (float)(vrprim.y + (vp->vwd.vmin + vp->vwd.vmax) / 2.0);
207  cw.z = (float)(vrprim.z);
208  ident_matrix (m);
209  m[2][0] = - cw.x / cw.z;
210  m[2][1] = - cw.y / cw.z;
211  postmult_matrix (wc, m);
212  /*
213  * 6 : Mise a l'echelle de la pyramide.
214  */
215  SET_COORD3(v,
216  (float)((2.0*vrprim.z)/((vp->vwd.umax-vp->vwd.umin)*(vrprim.z+vp->depth.back))),
217  (float)((2.0*vrprim.z)/((vp->vwd.vmax-vp->vwd.vmin)*(vrprim.z+vp->depth.back))),
218  (float)( 1.0/(vrprim.z+vp->depth.back)));
219  postscale_matrix (wc, &v);
220  /*
221  * 7 : Transformation perspective.
222  */
223  zmin = (vrprim.z + vp->depth.front) / (vrprim.z + vp->depth.back);
224  ident_matrix (m);
225  m[2][2] = (float)(1.0 / (1.0 - zmin)); m[2][3] = 1.0;
226  m[3][2] = (float)(- zmin / (1.0 - zmin)); m[3][3] = 0.0;
227  postmult_matrix (wc, m);
228 }
229 
230 #endif