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