Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpTmstack.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 "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
53static Matrix stack[STACKSIZE] /* = IDENTITY_MATRIX*/; /* pile */
54static 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 */
62Matrix *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 */
70void 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 */
80void 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 */
94void 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 */
110void 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 */
129void 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 */
137void 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 */
151void 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 */
159void 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 */
167void 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 */
175void 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 */
189void 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 */
197void pretranslate_tmstack(Vector *vp) { pretrans_matrix(*sp, vp); }
198
199#endif