Visual Servoing Platform  version 3.5.1 under development (2022-09-25)
vpTmstack.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 "tmstack.c" contient les procedures de gestion
33  * de la pile de matrices de transformation (Transformation
34  * Matrix STACK).
35  *
36  * Authors:
37  * Jean-Luc CORRE
38  *
39  *****************************************************************************/
40 
41 #include "vpTmstack.h"
42 #include "vpArit.h"
43 #include "vpMy.h"
44 #include <math.h>
45 #include <stdio.h>
46 #include <string.h>
47 #include <visp3/core/vpConfig.h>
48 
49 #ifndef DOXYGEN_SHOULD_SKIP_THIS
50 
51 #define STACKSIZE 32
52 
53 static Matrix stack[STACKSIZE] /* = IDENTITY_MATRIX*/; /* pile */
54 static Matrix *sp = stack; /* sommet */
55 
56 /*
57  * La procedure "get_tmstack" retourne la matrice au sommet
58  * de la pile des matrices de transformation.
59  * Sortie :
60  * Pointeur de la matrice au sommet de la pile.
61  */
62 Matrix *get_tmstack(void) { return (sp); }
63 
64 /*
65  * La procedure "load_tmstack" charge une matrice au sommet
66  * de la pile des matrices de transformation.
67  * Entree :
68  * m Matrice a charger.
69  */
70 void load_tmstack(Matrix m)
71 {
72  // bcopy ((char *) m, (char *) *sp, sizeof (Matrix));
73  memmove((char *)*sp, (char *)m, sizeof(Matrix));
74 }
75 
76 /*
77  * La procedure "pop_tmstack" depile la matrice au sommet
78  * de la pile des matrices de transformation.
79  */
80 void pop_tmstack(void)
81 {
82  if (sp == stack) {
83  static char proc_name[] = "pop_tmstack";
84  fprintf(stderr, "%s: stack underflow\n", proc_name);
85  return;
86  } else
87  sp--;
88 }
89 
90 /*
91  * La procedure "push_tmstack" empile et duplique le sommet
92  * de la pile des matrices de transformation.
93  */
94 void push_tmstack(void)
95 {
96  if (sp == stack + STACKSIZE - 1) {
97  static char proc_name[] = "push_tmstack";
98  fprintf(stderr, "%s: stack overflow\n", proc_name);
99  return;
100  }
101  sp++;
102  // bcopy ((char *) (sp - 1), (char *) sp, sizeof (Matrix));
103  memmove((char *)sp, (char *)(sp - 1), sizeof(Matrix));
104 }
105 
106 /*
107  * La procedure "swap_tmstack" echange les deux premieres matrices
108  * de la pile des matrices de transformation.
109  */
110 void swap_tmstack(void)
111 {
112  Matrix *mp, tmp;
113 
114  mp = (sp == stack) ? sp + 1 : sp - 1;
115  // bcopy ((char *) *sp, (char *) tmp, sizeof (Matrix));
116  // bcopy ((char *) *mp, (char *) *sp, sizeof (Matrix));
117  // bcopy ((char *) tmp, (char *) *mp, sizeof (Matrix));
118  memmove((char *)tmp, (char *)*sp, sizeof(Matrix));
119  memmove((char *)*sp, (char *)*mp, sizeof(Matrix));
120  memmove((char *)*mp, (char *)tmp, sizeof(Matrix));
121 }
122 
123 /*
124  * La procedure "postmult_tmstack" postmultiplie la matrice au sommet
125  * de la pile des matrices de transformation.
126  * Entree :
127  * m Matrice multiplicative.
128  */
129 void postmult_tmstack(Matrix m) { postmult_matrix(*sp, m); }
130 
131 /*
132  * La procedure "postrotate_tmstack" postmultiplie la matrice au sommet
133  * de la pile des matrices de transformation par une rotation.
134  * Entree :
135  * vp Vecteur de rotation.
136  */
137 void postrotate_tmstack(Vector *vp)
138 {
139  Matrix m;
140 
141  Rotate_to_Matrix(vp, m);
142  postmult3_matrix(*sp, m);
143 }
144 
145 /*
146  * La procedure "postscale_tmstack" postmultiplie la matrice au sommet
147  * de la pile des matrices de transformation par une homothetie.
148  * Entree :
149  * vp Vecteur d'homothetie.
150  */
151 void postscale_tmstack(Vector *vp) { postscale_matrix(*sp, vp); }
152 
153 /*
154  * La procedure "posttranslate_tmstack" postmultiplie la matrice au sommet
155  * de la pile des matrices de transformation par une translation.
156  * Entree :
157  * vp Vecteur de translation.
158  */
159 void posttranslate_tmstack(Vector *vp) { posttrans_matrix(*sp, vp); }
160 
161 /*
162  * La procedure "premult_tmstack" premultiplie la matrice au sommet
163  * de la pile des matrices de transformation.
164  * Entree :
165  * m Matrice multiplicative.
166  */
167 void premult_tmstack(Matrix m) { premult_matrix(*sp, m); }
168 
169 /*
170  * La procedure "prerotate_tmstack" premultiplie la matrice au sommet
171  * de la pile des matrices de transformation par une rotation.
172  * Entree :
173  * vp Vecteur de rotation.
174  */
175 void prerotate_tmstack(Vector *vp)
176 {
177  Matrix m;
178 
179  Rotate_to_Matrix(vp, m);
180  premult3_matrix(*sp, m);
181 }
182 
183 /*
184  * La procedure "prescale_tmstack" premultiplie la matrice au sommet
185  * de la pile des matrices de transformation par une homothetie.
186  * Entree :
187  * vp Vecteur d'homothetie.
188  */
189 void prescale_tmstack(Vector *vp) { prescale_matrix(*sp, vp); }
190 
191 /*
192  * La procedure "pretranslate_tmstack" premultiplie la matrice au sommet
193  * de la pile des matrices de transformation par une translation.
194  * Entree :
195  * vp Vecteur de translation.
196  */
197 void pretranslate_tmstack(Vector *vp) { pretrans_matrix(*sp, vp); }
198 
199 #endif