Visual Servoing Platform  version 3.6.1 under development (2024-11-15)
vpArit.h
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 contient les procedures arithmetiques.
33  *
34  * Authors:
35  * Jean-Luc CORRE
36  *
37 *****************************************************************************/
38 #ifndef vpArit_h
39 #define vpArit_h
40 
41 #include <stdio.h>
42 #include <visp3/core/vpConfig.h>
43 #include <visp3/robot/vpWireFrameSimulatorTypes.h>
44 
45 #ifndef DOXYGEN_SHOULD_SKIP_THIS
46 
47 #define ADD_COORD2(r, a, b) \
48  { \
49  (r).x = (a).x + (b).x; \
50  (r).y = (a).y + (b).y; \
51  }
52 
53 #define ADD_COORD3(r, a, b) \
54  { \
55  (r).x = (a).x + (b).x; \
56  (r).y = (a).y + (b).y; \
57  (r).z = (a).z + (b).z; \
58  }
59 
60 #define INC_COORD2(r, a) \
61  { \
62  (r).x += (a).x; \
63  (r).y += (a).y; \
64  }
65 
66 #define INC_COORD3(r, a) \
67  { \
68  (r).x += (a).x; \
69  (r).y += (a).y; \
70  (r).z += (a).z; \
71  }
72 
73 #define CROSS_PRODUCT(r, a, b) \
74  { \
75  (r).x = (a).y * (b).z - (a).z * (b).y; \
76  (r).y = (a).z * (b).x - (a).x * (b).z; \
77  (r).z = (a).x * (b).y - (a).y * (b).x; \
78  }
79 
80 #define DIF_COORD2(r, a, b) \
81  { \
82  (r).x = (a).x - (b).x; \
83  (r).y = (a).y - (b).y; \
84  }
85 
86 #define DIF_COORD3(r, a, b) \
87  { \
88  (r).x = (a).x - (b).x; \
89  (r).y = (a).y - (b).y; \
90  (r).z = (a).z - (b).z; \
91  }
92 
93 #define DOT_PRODUCT(a, b) (((a).x * (b).x) + ((a).y * (b).y) + ((a).z * (b).z))
94 
95 #define LENGTH3(a) (sqrt((double)DOT_PRODUCT((a), (a))))
96 
97 #define MID_COORD3(r, a, b) \
98  { \
99  (r).x = ((a).x + (b).x) / 2.0; \
100  (r).y = ((a).y + (b).y) / 2.0; \
101  (r).z = ((a).z + (b).z) / 2.0; \
102  }
103 
104 #define MUL_COORD3(r, a, b, c) \
105  { \
106  (r).x *= (a); \
107  (r).y *= (b); \
108  (r).z *= (c); \
109  }
110 
111 #define PAR_COORD3(r, t, a, b) \
112  { \
113  (r).x = ((b).x - (a).x) * (t) + (a).x; \
114  (r).y = ((b).y - (a).y) * (t) + (a).y; \
115  (r).z = ((b).z - (a).z) * (t) + (a).z; \
116  }
117 
118 #define SET_COORD2(r, a, b) \
119  { \
120  (r).x = (a); \
121  (r).y = (b); \
122  }
123 
124 #define SET_COORD3(r, a, b, c) \
125  { \
126  (r).x = (a); \
127  (r).y = (b); \
128  (r).z = (c); \
129  }
130 
131 #define SUB_COORD2(r, a) \
132  { \
133  (r).x -= (a).x; \
134  (r).y -= (a).y; \
135  }
136 
137 #define SUB_COORD3(r, a) \
138  { \
139  (r).x -= (a).x; \
140  (r).y -= (a).y; \
141  (r).z -= (a).z; \
142  }
143 
144 #define COORD3_COL(x, y, z, m, i) (((x) * (m)[0][i]) + ((y) * (m)[1][i]) + ((z) * (m)[2][i]) + (m)[3][i])
145 
146 #define COORD4_COL(x, y, z, w, m, i) (((x) * (m)[0][i]) + ((y) * (m)[1][i]) + ((z) * (m)[2][i]) + ((w) * (m)[3][i]))
147 
148 #define M_POLY1(x, a, b) ((a) * (x) + (b))
149 #define M_POLY2(x, a, b, c) (M_POLY1((x), (a), (b)) * (x) + (c))
150 #define M_POLY3(x, a, b, c, d) (M_POLY2((x), (a), (b), (c)) * (x) + (d))
151 
152 BEGIN_VISP_NAMESPACE
153 typedef struct
154 {
155  int x, y;
156 } Point2i;
157 
158 typedef struct
159 {
160  short x, y;
161 } Point2s;
162 
163 typedef struct
164 {
165  int x, y, z;
166 } Point3i;
167 
168 typedef struct
169 {
170  float x, y, z, w;
171 } Point4f;
172 
173 typedef struct
174 {
175  float x, y, z;
176 } Vector;
177 
178 #define IDENTITY_MATRIX \
179  { \
180  {1.0, 0.0, 0.0, 0.0}, {0.0, 1.0, 0.0, 0.0}, {0.0, 0.0, 1.0, 0.0}, { 0.0, 0.0, 0.0, 1.0 } \
181  }
182 
183 /*
184  * POSITION
185  * ________
186  *
187  * La structure "Position" definit le positionnement d'un objet.
188  * Matrice de positionnement = R.S.T
189  * avec R = Rx.Ry.Rz Matrice de rotation autour des axes (Ox,Oy,Oz),
190  * Les angles sont donnes en degres;
191  * S = Sx.Sy.Sz Matrice d'homothetie sur les axes;
192  * T = Tx.Ty.Tz Matrice de translation sur les axes.
193  */
194 typedef struct
195 {
196  Vector rotate; /* vecteur rotation */
197  Vector scale; /* vecteur homothetie */
198  Vector translate; /* vecteur translation */
199 } AritPosition;
200 
201 #define IDENTITY_ROTATE \
202  { \
203  0.0, 0.0, 0.0 \
204  }
205 #define IDENTITY_SCALE \
206  { \
207  1.0, 1.0, 1.0 \
208  }
209 #define IDENTITY_TRANSLATE \
210  { \
211  0.0, 0.0, 0.0 \
212  }
213 
214 #define IDENTITY_POSITION \
215  { \
216  IDENTITY_ROTATE, IDENTITY_SCALE, IDENTITY_TRANSLATE \
217  }
218 
219 void fprintf_matrix(FILE *fp, Matrix m);
220 void ident_matrix(Matrix m);
221 void premult_matrix(Matrix a, Matrix b);
222 void premult3_matrix(Matrix a, Matrix b);
223 void prescale_matrix(Matrix m, Vector *vp);
224 void pretrans_matrix(Matrix m, Vector *vp);
225 void postleft_matrix(Matrix m, char axis);
226 void postmult_matrix(Matrix a, Matrix b);
227 void postmult3_matrix(Matrix a, Matrix b);
228 void postscale_matrix(Matrix m, Vector *vp);
229 void posttrans_matrix(Matrix m, Vector *vp);
230 void transpose_matrix(Matrix m);
231 
232 float cosin_to_angle(float ca, float sa);
233 float norm_vector(Vector *vp);
234 void point_matrix(Point4f *p4, Point3f *p3, Matrix m);
235 void plane_norme(Vector *np, Point3f *ap, Point3f *bp, Point3f *cp);
236 void point_3D_3D(Point3f *ip, int size, Matrix m, Point3f *op);
237 void point_3D_4D(Point3f *p3, int size, Matrix m, Point4f *p4);
238 void rotate_vector(Vector *vp, float a, Vector *axis);
239 void upright_vector(Vector *vp, Vector *up);
240 
241 void Matrix_to_Position(Matrix m, AritPosition *pp);
242 void Matrix_to_Rotate(Matrix m, Vector *vp);
243 void Position_to_Matrix(AritPosition *pp, Matrix m);
244 void Rotate_to_Matrix(Vector *vp, Matrix m);
245 void Rotaxis_to_Matrix(float a, Vector *axis, Matrix m);
246 void Rotrans_to_Matrix(Vector *rp, Vector *tp, Matrix m);
247 void Scale_to_Matrix(Vector *vp, Matrix m);
248 void Translate_to_Matrix(Vector *vp, Matrix m);
249 
250 void fscanf_Point3f(Point3f *pp);
251 void fscanf_Vector(Vector *vp);
252 END_VISP_NAMESPACE
253 #endif
254 #endif