Visual Servoing Platform  version 3.6.1 under development (2024-12-03)
vpVwstack.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 "vwstack.c" contient les procedures de gestion
33  * de la pile des points de vue (VieW STACK).
34  *
35  * Authors:
36  * Jean-Luc CORRE
37  *
38 *****************************************************************************/
39 
40 #include <visp3/core/vpConfig.h>
41 
42 #ifndef DOXYGEN_SHOULD_SKIP_THIS
43 
44 #include <cmath> // std::fabs()
45 #include <limits>
46 #include <stdarg.h>
47 #include <stdio.h>
48 #include <string.h>
49 
50 #include "vpArit.h"
51 #include "vpMy.h"
52 #include "vpView.h"
53 #include "vpVwstack.h"
54 
55 #define STACKSIZE 4
56 
57 BEGIN_VISP_NAMESPACE
58 static View_parameters stack[STACKSIZE] = { vpDEFAULT_VIEW };
59 static View_parameters *sp = stack;
60 
61 /*
62  * La procedure "fprintf_vwstack" affiche un parametre du sommet
63  * de la pile des prises de vue.
64  * Entree :
65  * fp Fichier de sortie.
66  * argv Argument a afficher.
67  * Si argv est nul, tous les parametres sont affiches.
68  */
69 void fprintf_vwstack(FILE *fp, char *argv)
70 {
71  if (argv == NULL || strcmp(argv, "type") == 0) {
72  const char *typetoa;
73 
74  switch (sp->type) {
75  case PARALLEL:
76  typetoa = "parallel";
77  break;
78  case PERSPECTIVE:
79  typetoa = "perspective";
80  break;
81  default:
82  typetoa = "unknown";
83  break;
84  }
85  fprintf(fp, "(type\t%s)\n", typetoa);
86  if (argv != NULL)
87  return;
88  }
89  if (argv == NULL || strcmp(argv, "cop") == 0) {
90  fprintf(fp, "(cop\t%.3f\t%.3f\t%.3f)\n", sp->cop.x, sp->cop.y, sp->cop.z);
91  if (argv != NULL)
92  return;
93  }
94  if (argv == NULL || strcmp(argv, "vrp") == 0) {
95  fprintf(fp, "(vrp\t%.3f\t%.3f\t%.3f)\n", sp->vrp.x, sp->vrp.y, sp->vrp.z);
96  if (argv != NULL)
97  return;
98  }
99  if (argv == NULL || strcmp(argv, "vpn") == 0) {
100  fprintf(fp, "(vpn\t%.3f\t%.3f\t%.3f)\n", sp->vpn.x, sp->vpn.y, sp->vpn.z);
101  if (argv != NULL)
102  return;
103  }
104  if (argv == NULL || strcmp(argv, "vup") == 0) {
105  fprintf(fp, "(vup\t%.3f\t%.3f\t%.3f)\n", sp->vup.x, sp->vup.y, sp->vup.z);
106  if (argv != NULL)
107  return;
108  }
109  if (argv == NULL || strcmp(argv, "window") == 0) {
110  fprintf(fp, "(window\t%.3f\t%.3f\t%.3f\t%.3f)\n", sp->vwd.umin, sp->vwd.umax, sp->vwd.vmin, sp->vwd.vmax);
111  if (argv != NULL)
112  return;
113  }
114  if (argv == NULL || strcmp(argv, "depth") == 0) {
115  fprintf(fp, "(depth\t%.3f\t%.3f)\n", sp->depth.front, sp->depth.back);
116  if (argv != NULL)
117  return;
118  }
119  if (argv != NULL) {
120  static char proc_name[] = "fprintf_vwstack";
121  fprintf(stderr, "%s: argument unknown\n", proc_name);
122  }
123 }
124 
125 /*
126  * La procedure "get_vwstack" retourne le point de vue au sommet
127  * de la pile des points de vue.
128  * Sortie :
129  * Pointeur sur le point de vue du sommet de la pile.
130  */
131 View_parameters *get_vwstack(void) { return (sp); }
132 
133 /*
134  * La procedure "load_vwstack" charge un point de vue au sommet
135  * de la pile des points de vue.
136  * Entree :
137  * vp Point de vue a charger.
138  */
139 void load_vwstack(View_parameters *vp) { *sp = *vp; }
140 
141 /*
142  * La procedure "pop_vwstack" depile le point de vue au sommet
143  * de la pile des points de vue.
144  */
145 void pop_vwstack(void)
146 {
147  if (sp == stack) {
148  static char proc_name[] = "pop_vwstack";
149  fprintf(stderr, "%s: stack underflow\n", proc_name);
150  return;
151  }
152  else
153  sp--;
154 }
155 
156 /*
157  * La procedure "push_vwstack" empile et duplique le point de vue au sommet
158  * de la pile des points de vue.
159  */
160 void push_vwstack(void)
161 {
162  if (sp == stack + STACKSIZE - 1) {
163  static char proc_name[] = "push_vwstack";
164  fprintf(stderr, "%s: stack overflow\n", proc_name);
165  return;
166  }
167  sp++;
168  *sp = *(sp - 1);
169 }
170 
171 /*
172  * La procedure "swap_vwstack" echange les deux premiers elements
173  * de la pile des points de vue.
174  */
175 void swap_vwstack(void)
176 {
177  View_parameters *vp, tmp;
178 
179  vp = (sp == stack) ? sp + 1 : sp - 1;
180  SWAP(*sp, *vp, tmp);
181 }
182 
183 /*
184  * La procedure "add_vwstack" modifie un agrument du point de vue au sommet
185  * de la pile des points de vue.
186  * Entree :
187  * va_alist Nom de l'argument a modifier suivi de ses parametres.
188  */
189 
190 void add_vwstack(const char *path, ...)
191 // add_vwstack (va_alist)
192 // va_dcl
193 {
194  va_list ap;
195  char *argv;
196 
197  va_start(ap, path);
198  argv = va_arg(ap, char *);
199  if (strcmp(argv, "cop") == 0) {
200  /* initialise le centre de projection */
201  SET_COORD3(sp->cop, (float)va_arg(ap, double), (float)va_arg(ap, double), (float)va_arg(ap, double));
202  }
203  else if (strcmp(argv, "depth") == 0) {
204  /* initialise les distances des plans de decoupage */
205  sp->depth.front = (float)va_arg(ap, double);
206  sp->depth.back = (float)va_arg(ap, double);
207  }
208  else if (strcmp(argv, "type") == 0) {
209  /* initialise le type de projection */
210  sp->type = (Type)va_arg(ap, int);
211  }
212  else if (strcmp(argv, "vpn") == 0) {
213  /* initialise le vecteur normal au plan */
214  float x = (float)va_arg(ap, double);
215  float y = (float)va_arg(ap, double);
216  float z = (float)va_arg(ap, double);
217 
218  // if (x == 0 && y == 0 && z == 0)
219  if (std::fabs(x) <= std::numeric_limits<double>::epsilon() &&
220  std::fabs(y) <= std::numeric_limits<double>::epsilon() &&
221  std::fabs(z) <= std::numeric_limits<double>::epsilon()) {
222  static char proc_name[] = "add_vwstack";
223  fprintf(stderr, "%s: bad vpn\n", proc_name);
224  }
225  else {
226  SET_COORD3(sp->vpn, x, y, z);
227  }
228  }
229  else if (strcmp(argv, "vrp") == 0) {
230  /* initialise le vecteur de reference */
231  SET_COORD3(sp->vrp, (float)va_arg(ap, double), (float)va_arg(ap, double), (float)va_arg(ap, double));
232  }
233  else if (strcmp(argv, "vup") == 0) {
234  /* initialise le vecteur haut du plan */
235  float x = (float)va_arg(ap, double);
236  float y = (float)va_arg(ap, double);
237  float z = (float)va_arg(ap, double);
238 
239  // if (x == 0 && y == 0 && z == 0)
240  if (std::fabs(x) <= std::numeric_limits<double>::epsilon() &&
241  std::fabs(y) <= std::numeric_limits<double>::epsilon() &&
242  std::fabs(z) <= std::numeric_limits<double>::epsilon()) {
243  static char proc_name[] = "add_vwstack";
244  fprintf(stderr, "%s: bad vup\n", proc_name);
245  }
246  else {
247  SET_COORD3(sp->vup, x, y, z);
248  }
249  }
250  else if (strcmp(argv, "window") == 0) {
251  /* initialise la fenetre de projection */
252  sp->vwd.umin = (float)va_arg(ap, double);
253  sp->vwd.umax = (float)va_arg(ap, double);
254  sp->vwd.vmin = (float)va_arg(ap, double);
255  sp->vwd.vmax = (float)va_arg(ap, double);
256  }
257  else {
258  static char proc_name[] = "add_vwstack";
259  fprintf(stderr, "%s: bad argument\n", proc_name);
260  }
261  va_end(ap);
262 }
263 END_VISP_NAMESPACE
264 #endif