Visual Servoing Platform version 3.5.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
152typedef struct {
153 int x, y;
154} Point2i;
155
156typedef struct {
157 short x, y;
158} Point2s;
159
160typedef struct {
161 int x, y, z;
162} Point3i;
163
164typedef struct {
165 float x, y, z, w;
166} Point4f;
167
168typedef 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 */
188typedef 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
212void fprintf_matrix(FILE *fp, Matrix m);
213void ident_matrix(Matrix m);
214void premult_matrix(Matrix a, Matrix b);
215void premult3_matrix(Matrix a, Matrix b);
216void prescale_matrix(Matrix m, Vector *vp);
217void pretrans_matrix(Matrix m, Vector *vp);
218void postleft_matrix(Matrix m, char axis);
219void postmult_matrix(Matrix a, Matrix b);
220void postmult3_matrix(Matrix a, Matrix b);
221void postscale_matrix(Matrix m, Vector *vp);
222void posttrans_matrix(Matrix m, Vector *vp);
223void transpose_matrix(Matrix m);
224
225float cosin_to_angle(float ca, float sa);
226float norm_vector(Vector *vp);
227void point_matrix(Point4f *p4, Point3f *p3, Matrix m);
228void plane_norme(Vector *np, Point3f *ap, Point3f *bp, Point3f *cp);
229void point_3D_3D(Point3f *ip, int size, Matrix m, Point3f *op);
230void point_3D_4D(Point3f *p3, int size, Matrix m, Point4f *p4);
231void rotate_vector(Vector *vp, float a, Vector *axis);
232void upright_vector(Vector *vp, Vector *up);
233
234void Matrix_to_Position(Matrix m, AritPosition *pp);
235void Matrix_to_Rotate(Matrix m, Vector *vp);
236void Position_to_Matrix(AritPosition *pp, Matrix m);
237void Rotate_to_Matrix(Vector *vp, Matrix m);
238void Rotaxis_to_Matrix(float a, Vector *axis, Matrix m);
239void Rotrans_to_Matrix(Vector *rp, Vector *tp, Matrix m);
240void Scale_to_Matrix(Vector *vp, Matrix m);
241void Translate_to_Matrix(Vector *vp, Matrix m);
242
243void fscanf_Point3f(Point3f *pp);
244void fscanf_Vector(Vector *vp);
245
246#endif
247#endif