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