Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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/*
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 */
54void 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 */
81static 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 */
119void 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 */
182void 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