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