42#include <visp3/core/vpConfig.h>
43#include <visp3/robot/vpWireFrameSimulatorTypes.h>
45#ifndef DOXYGEN_SHOULD_SKIP_THIS
47#define ADD_COORD2(r, a, b) \
49 (r).x = (a).x + (b).x; \
50 (r).y = (a).y + (b).y; \
53#define ADD_COORD3(r, a, b) \
55 (r).x = (a).x + (b).x; \
56 (r).y = (a).y + (b).y; \
57 (r).z = (a).z + (b).z; \
60#define INC_COORD2(r, a) \
66#define INC_COORD3(r, a) \
73#define CROSS_PRODUCT(r, a, b) \
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; \
80#define DIF_COORD2(r, a, b) \
82 (r).x = (a).x - (b).x; \
83 (r).y = (a).y - (b).y; \
86#define DIF_COORD3(r, a, b) \
88 (r).x = (a).x - (b).x; \
89 (r).y = (a).y - (b).y; \
90 (r).z = (a).z - (b).z; \
93#define DOT_PRODUCT(a, b) (((a).x * (b).x) + ((a).y * (b).y) + ((a).z * (b).z))
95#define LENGTH3(a) (sqrt((double)DOT_PRODUCT((a), (a))))
97#define MID_COORD3(r, a, b) \
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; \
104#define MUL_COORD3(r, a, b, c) \
111#define PAR_COORD3(r, t, a, b) \
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; \
118#define SET_COORD2(r, a, b) \
124#define SET_COORD3(r, a, b, c) \
131#define SUB_COORD2(r, a) \
137#define SUB_COORD3(r, a) \
144#define COORD3_COL(x, y, z, m, i) (((x) * (m)[0][i]) + ((y) * (m)[1][i]) + ((z) * (m)[2][i]) + (m)[3][i])
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]))
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))
172#define IDENTITY_MATRIX \
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 } \
194#define IDENTITY_ROTATE \
198#define IDENTITY_SCALE \
202#define IDENTITY_TRANSLATE \
207#define IDENTITY_POSITION \
209 IDENTITY_ROTATE, IDENTITY_SCALE, IDENTITY_TRANSLATE \
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);
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);
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);
243void fscanf_Point3f(Point3f *pp);
244void fscanf_Vector(Vector *
vp);