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 248