Visual Servoing Platform  version 3.4.0
vpArit.h
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 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 typedef struct {
153  int x, y;
154 } Point2i;
155 
156 typedef struct {
157  short x, y;
158 } Point2s;
159 
160 typedef struct {
161  int x, y, z;
162 } Point3i;
163 
164 typedef struct {
165  float x, y, z, w;
166 } Point4f;
167 
168 typedef struct {
169  float x, y, z;
170 } Vector;
171 
172 #define IDENTITY_MATRIX \
173  { \
174  {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 } \
175  }
176 
177 /*
178  * POSITION
179  * ________
180  *
181  * La structure "Position" definit le positionnement d'un objet.
182  * Matrice de positionnement = R.S.T
183  * avec R = Rx.Ry.Rz Matrice de rotation autour des axes (Ox,Oy,Oz),
184  * Les angles sont donnes en degres;
185  * S = Sx.Sy.Sz Matrice d'homothetie sur les axes;
186  * T = Tx.Ty.Tz Matrice de translation sur les axes.
187  */
188 typedef struct {
189  Vector rotate; /* vecteur rotation */
190  Vector scale; /* vecteur homothetie */
191  Vector translate; /* vecteur translation */
192 } AritPosition;
193 
194 #define IDENTITY_ROTATE \
195  { \
196  0.0, 0.0, 0.0 \
197  }
198 #define IDENTITY_SCALE \
199  { \
200  1.0, 1.0, 1.0 \
201  }
202 #define IDENTITY_TRANSLATE \
203  { \
204  0.0, 0.0, 0.0 \
205  }
206 
207 #define IDENTITY_POSITION \
208  { \
209  IDENTITY_ROTATE, IDENTITY_SCALE, IDENTITY_TRANSLATE \
210  }
211 
212 void fprintf_matrix(FILE *fp, Matrix m);
213 void ident_matrix(Matrix m);
214 void premult_matrix(Matrix a, Matrix b);
215 void premult3_matrix(Matrix a, Matrix b);
216 void prescale_matrix(Matrix m, Vector *vp);
217 void pretrans_matrix(Matrix m, Vector *vp);
218 void postleft_matrix(Matrix m, char axis);
219 void postmult_matrix(Matrix a, Matrix b);
220 void postmult3_matrix(Matrix a, Matrix b);
221 void postscale_matrix(Matrix m, Vector *vp);
222 void posttrans_matrix(Matrix m, Vector *vp);
223 void transpose_matrix(Matrix m);
224 
225 float cosin_to_angle(float ca, float sa);
226 float norm_vector(Vector *vp);
227 void point_matrix(Point4f *p4, Point3f *p3, Matrix m);
228 void plane_norme(Vector *np, Point3f *ap, Point3f *bp, Point3f *cp);
229 void point_3D_3D(Point3f *ip, int size, Matrix m, Point3f *op);
230 void point_3D_4D(Point3f *p3, int size, Matrix m, Point4f *p4);
231 void rotate_vector(Vector *vp, float a, Vector *axis);
232 void upright_vector(Vector *vp, Vector *up);
233 
234 void Matrix_to_Position(Matrix m, AritPosition *pp);
235 void Matrix_to_Rotate(Matrix m, Vector *vp);
236 void Position_to_Matrix(AritPosition *pp, Matrix m);
237 void Rotate_to_Matrix(Vector *vp, Matrix m);
238 void Rotaxis_to_Matrix(float a, Vector *axis, Matrix m);
239 void Rotrans_to_Matrix(Vector *rp, Vector *tp, Matrix m);
240 void Scale_to_Matrix(Vector *vp, Matrix m);
241 void Translate_to_Matrix(Vector *vp, Matrix m);
242 
243 void fscanf_Point3f(Point3f *pp);
244 void fscanf_Vector(Vector *vp);
245 
246 #endif
247 #endif