Biomechanical Joint Model  Author: Anderson Maciel

---

geom.h

Go to the documentation of this file.

/*@#-------------------------------------------------------------------
 PROJECT            : ESPRIT 6709                              HUMANOID
 MODULE             : HUMAN DATA STRUCTURE

 FILE               : geom.h
 CREATION DATE      : 26.02.93
 CREATION AUTHOR(S) : R. Boulic
 ----------------------------------------------------------------------
 KEYWORD(S)         : 
 PUBLIC TYPE(S)     :
 RELATED TYPE(S)    :

 FUNCTIONALITY      : geometric transformations
 ----------------------------------------------------------------------
 UPDATE    $Revision: 1.7 $
             $Author: abaci $
 ----------------------------------------------------------------------
 LANGAGE            : ANSI-C
 SYSTEM             : UNIX V 4. / SGI
 GRAPHIC LIBRARY    : SGI GL  
 ----------------------------------------------------------------------
 LAB DIRECTOR       : D. THALMANN
 LAB                : EPFL- LIG
 ------------------------------------------------------------------#@*/

# ifndef libgeom_h
# define libgeom_h


/*
** -------------------------------------
** -- Include of Header files ----------
** -------------------------------------
*/

#include        <stdio.h>           /* for FILE type        */


namespace geom {

/*
** -------------------------------------
** -- Public define, typedef, functions
** -------------------------------------
*/
#define M_PI            3.14159265358979323846
#define M_PI_2          1.57079632679489661923
#define LIBGEOM_REAL    double

enum {  ATTRACTION_NULLE     = 0,
        ATTRACTION_COMPLETE  = 1,
        ATTRACTION_PARTIELLE = 2 };

const float EPSIL_MR3D  = 0.00001f;
const float EPSIL_VT3D  = 0.001f;

/* some epsilon value to round off zero */

#define EPSIL_ZERO      0.000001
#define EPSIL_BIG_ZERO  0.00001

/* index for vectors, matrices and quaternions */

enum {  GEOM_X_INDEX = 0,
        GEOM_Y_INDEX = 1,
        GEOM_Z_INDEX = 2,
        GEOM_W_INDEX = 3 };

/* m etant un pointeur sur un espace a une dimension mais utilise ici*/
/* pour deux dimensions ; jmax est la taille d'une ligne (= nb de col.)*/
/* le resultat correspond a :  m[i][j]       (avec int i,j,jmax)*/

#define VIJ(m,i,j,jmax)         (*((m) + (i)*(jmax) + (j)))

typedef LIBGEOM_REAL    MatriX[4][4];   
typedef LIBGEOM_REAL    MR3D[3][3];     
typedef LIBGEOM_REAL    VT3D[3];        
typedef LIBGEOM_REAL    VHOM[4];        
typedef MatriX  MatriX_t;
typedef MR3D    MR3D_t;
typedef VT3D    VT3D_t;
typedef VHOM    VHOM_t;

typedef LIBGEOM_REAL   QUATERNION[4];   
/*----------------------------------------------------------------*/
typedef struct {

  double *data;    
  int    lines;    
} ARRAY1D;

#define AI(array1d, line)       (*((array1d)->data + (line)))

/*----------------------------------------------------------------*/
typedef struct {

  double *data;    
  int    lines,    
         cols;     
} ARRAY2D;

#define AIJ(array2d, line, col) (*((array2d)->data + (col) + (line) * (array2d)->cols))

/*-------------------------------------------------------------------*/
/* Use the following functions instead of acos and asin, if you
   are not sure that your argument is in the valid range (because of
   numerical errors, for example). These functions clamp the argument
   to its valid range.
*/

 double safe_acos (double f);  
 double safe_asin (double f);  
/*-------------------------------------------------------------------*/

        void    matrix_idt_ (MatriX_t matrix);

        void    matrix_copy (const MatriX_t src, 
                             MatriX_t dst);

        void    matrix_compose (const MR3D_t   mr, 
                                const VT3D_t     vt, 
                                MatriX_t matrix);

        void    matrix_decompose (MR3D_t   mr, 
                                  VT3D_t   vt, 
                                  const MatriX_t matrix);

        void    matrix_rotpart   (MatriX_t In, 
                                  MatriX_t Res);

        void    matrix_transpose (const MatriX_t a,
                                  MatriX_t at);

        void    matrix_mult_vhom (const MatriX_t a, 
                                  const VHOM_t   u,
                                  VHOM_t   v);

        void    matrix_mult_vt3d (const MatriX_t a, 
                                  const VT3D_t   u,
                                  VT3D_t   v);

        void    matrix_inverse (const MatriX_t matrix,
                                MatriX_t matrix_inv);

        void    matrix_inverse_det (MatriX_t res,
                                                                LIBGEOM_REAL    determinant,
                                                                const MatriX_t source);

        LIBGEOM_REAL    matrix_cofactor (const MatriX_t matrix,
                                 int      i,
                                 int      j);

        LIBGEOM_REAL    matrix_determinant (const MatriX_t matrix);

        void    matrix_mult (const MatriX_t     ma,
                             const MatriX_t     mb,
                             MatriX_t   mc);    

        void    matrix_mult_opt (const MatriX ma, const MatriX mb, MatriX mc);

        bool matrix_egalite (const MatriX_t a,
                                  const MatriX_t b);

        int   matrix_attraction_idt (MatriX_t   a);

        int     matrix_proximite (const MatriX_t  a,
                                  const MatriX_t  b, 
                                  LIBGEOM_REAL      seuil_mr,
                                  LIBGEOM_REAL      seuil_vt);

        void   matrix_lerp (const MatriX_t a,
                            const MatriX_t b,
                            LIBGEOM_REAL    u,
                            MatriX   result);

        bool  matrix_write (const MatriX_t m, 
                                 FILE * f);

        bool  matrix_read  (MatriX_t m, 
                                 FILE * f);

/*-------------------------------------------------------------------*/

        void    mr3d_idt_ (MR3D_t    mr);

        void    mr3d_get  (const MatriX_t  matrix, 
                           MR3D_t a);

        void    mr3d_copy (const MR3D_t src, 
                           MR3D_t dst);

        void    mr3d_load (MatriX_t matrix, 
                           const MR3D_t a);                      

        void    mr3d_transpose (const MR3D_t a,
                                MR3D_t at);

        void    mr3d_self_transpose (MR3D_t ma);

        void    mr3d_perm_circ (MR3D_t a, 
                                MR3D_t b);

        void    mr3d_opposite (MR3D_t a, 
                               MR3D_t b);

        void    mr3d_symtxy (MR3D_t a, 
                             MR3D_t b);

        void    mr3d_scale (const MR3D_t self, 
                            LIBGEOM_REAL scalar, 
                            MR3D_t res);

        void    mr3d_sub  (const MR3D_t left, 
                           const MR3D_t right, 
                           MR3D_t res);

        void    mr3d_add  (const MR3D_t left, 
                           const MR3D_t right, 
                           MR3D_t res);

        void    mr3d_mult (const MR3D_t a, 
                           const MR3D_t b, 
                           MR3D_t c);

        void    mr3d_mult_vt3d (const MR3D_t ma,
                                const VT3D_t vu,
                                VT3D_t  vv);

        void    mr3d_from_axis_angle (MR3D_t mr, 
                                      const VT3D_t axis);

        void    mr3d_to_axis_angle   (VT3D_t axis, 
                                      const MR3D_t mr);

        double  mr3d_axis_angle      (const MR3D_t mr);

        bool mr3d_egalite (const MR3D_t a,
                           const MR3D_t b,
                           LIBGEOM_REAL  seuil_mr = EPSIL_MR3D);

        int     mr3d_attraction_idt (MR3D_t a, LIBGEOM_REAL  seuil_mr);

        int     mr3d_attraction_ini (MR3D_t a, LIBGEOM_REAL  seuil_mr);

        void    mr3d_conv_xyz_mob (const MR3D_t m, 
                                   LIBGEOM_REAL *ang1,
                                   LIBGEOM_REAL *ang2,
                                   LIBGEOM_REAL *ang3);

        void    mr3d_conv_yxz_mob (const MR3D_t m, 
                                   LIBGEOM_REAL *ang1,
                                   LIBGEOM_REAL *ang2,
                                   LIBGEOM_REAL *ang3);

        void    mr3d_from_angle_seq_xyz (MR3D mx,
                                         LIBGEOM_REAL angX, 
                                         LIBGEOM_REAL angY, 
                                         LIBGEOM_REAL angZ);

        void    mr3d_from_angle_seq_yxz (MR3D mx, 
                                         LIBGEOM_REAL angY, 
                                         LIBGEOM_REAL angX, 
                                         LIBGEOM_REAL angZ);

        void    mr3d_from_two_vectors(const VT3D_t a, const VT3D_t b, MR3D R);

        void    mr3d_reorthogonalize (MR3D_t R, int limit);

        bool  mr3d_write (const MR3D_t self, FILE * f);

        bool  mr3d_read  (MR3D_t self, FILE * f);

/*-------------------------------------------------------------------*/

        void    vt3d_get (const MatriX_t      matrix,
                          VT3D_t        v);

        void    vt3d_load (MatriX_t     matrix,
                           const VT3D_t v);

        void    vt3d_copy (const VT3D_t src,
                           VT3D_t dst);

        void    vt3d_set_null (VT3D_t   v);

        bool vt3d_normalize (const VT3D_t u,
                                  VT3D_t v);

        void    vt3d_cross   (const VT3D_t      u,
                              const VT3D_t      v,
                              VT3D_t    w);

        void    vt3d_scale   (const VT3D_t  scale,
                              const VT3D_t      u,
                              VT3D_t    v);

        LIBGEOM_REAL    vt3d_get_dot (const VT3D_t      u,
                              const VT3D_t      v);

        void    vt3d_vhom (const VT3D_t vt3d, 
                           VHOM_t vhom);

        void    vhom_vt3d (const VHOM_t vhom, 
                           VT3D_t vt3d);

        void    vt3d_let (LIBGEOM_REAL x, LIBGEOM_REAL y, LIBGEOM_REAL z, VT3D_t w);

        void    vt3d_add (const VT3D_t u, const VT3D_t v, VT3D_t w);

        void    vt3d_sub (const VT3D_t u, const VT3D_t v, VT3D_t w);

        void    vt3d_mult (const VT3D_t u, const VT3D_t v, VT3D_t w);

        void    vt3d_div (const VT3D_t u, const VT3D_t v, VT3D_t w);

        void    vt3d_mults (const VT3D_t v, LIBGEOM_REAL s, VT3D_t w);

        void    vt3d_divs (const VT3D_t v, LIBGEOM_REAL s, VT3D_t w);

        void  vt3d_neg (const VT3D_t src, VT3D_t dest);

        LIBGEOM_REAL    vt3d_get_norm (const VT3D_t v);

        LIBGEOM_REAL    vt3d_get_norm2 (const VT3D_t v);

        LIBGEOM_REAL    vt3d_get_dist (const VT3D_t u, const VT3D_t v);

        LIBGEOM_REAL    vt3d_get_distptline (const VT3D_t p, const VT3D_t q, const VT3D_t n);

        LIBGEOM_REAL    vt3d_get_distptplane (const VT3D_t p, const VT3D_t q, const VT3D_t n);

        void    vt3d_rotv (const VT3D_t v, LIBGEOM_REAL sa, LIBGEOM_REAL ca, char k, VT3D_t w);

        void    vt3d_rotvect (const VT3D_t v, const VT3D_t c, VT3D_t k, 
                              LIBGEOM_REAL sa, LIBGEOM_REAL ca, VT3D_t w);

        bool vt3d_egalite (const VT3D_t a,
                                const VT3D_t b, 
                                LIBGEOM_REAL    seuil_vt);

        int     vt3d_attraction_nul (VT3D_t a, 
                                     LIBGEOM_REAL  seuil_vt);

        bool vt3d_inside_polygon (VT3D point, VT3D *p, int N);

        void vt3d_lerp (const VT3D p1, const VT3D p2, LIBGEOM_REAL u, VT3D result);


        void vt3d_slerp (const VT3D p1, const VT3D p2, LIBGEOM_REAL u, VT3D result);

        bool vt3d_write (const VT3D_t self, FILE * f);

        bool vt3d_read  (VT3D_t self, FILE *f);

        void      vt3d_rotate_around_axis_angle (const VT3D  axis_angle,
                                                 const VT3D  v_in,
                                                 VT3D  v_out);

/*------------------------------------------------------------------*/

  double        *vtn_creation(int       n);
  void  vtn_destr(double        *p);
  void  vtn_nul(double          *p, 
                            int         n);
  double        vtn_norm(const double           *p, 
                            int         n);
  double        vtn_anorm(const double  *p, 
                            int         n);
  void  vtn_copy(const double   *psrc,
                            double      *pdst, 
                            int         n);
  void  vtn_add(const double            *p1,
                const double    *p2,
                            double      *pdst, 
                            int         n);
  void  vtn_sub(const double            *p1,
                const double    *p2,
                            double      *pdst, 
                            int         n);
  void    vtn_mults (const double  *psrc, 
                           double  *pdst, 
                           int n, 
                           double  alpha);
  double        vtn_dot(const double            *p1,
                        const double    *p2,
                            int         n);
  double        vtn_inv(const double            *p,
                            double      *p_inv,
                            int         n);

/*------------------------------------------------------------------*/

  double        *matmxn_creation (int   lmax,
                                  int   cmax);
  void  matmxn_destr (double    *p);
  void  matmxn_copy  (const double * a, double * b, int lmax, int cmax);

  void  matmxn_nul (double      *p, 
                            int         lmax,
                            int         cmax);
  void  matmxn_idt (double      *p, 
                            int         lmax,
                            int         cmax);
  void  matmxn_mult (const double       *a,
                     const double       *b,
                             double     *c, 
                             int        imax,
                             int        jmax,
                             int        kmax);
  void  matmxn_mult_vtn (const double   *a,
                         const double   *b,
                                 double *c, 
                                 int    lmax ,
                                 int    cmax);
  void    matmxn_write (double *d,
                              int    height,
                              int    width,
                              FILE   *f);
  void    matmxn_transpose (const double *src,
                                  double *dest,
                                  int    src_width,
                                  int    src_height);
  bool  matmxn_pseudo_inverse (double  *m,
                                          int   lmax,
                                          int   cmax, 
                                          double *mp);
  void matmxn_damped_least_square_inverse (double *j_inv, 
                                                 double *j_mat, 
                                                 int j_width, 
                                                 int j_height, 
                                                 double damping);
  void matmxn_svdcmp(double **a,int m,int n,double *w,double **v);      
                   /* Given a matrix a[1..m][1..n], this routine computes 
                      its singular value decomposition. 
                      A = U * W * Vt. The matrix U replaces a on output. The
                      diagonal matrix of singular values W is output as
                      vector w[1..n]. The matrix V (not the transpose Vt)
                      is output as v[1..n][1..n]. 
                      "Numerical Recipes in C", 2nd edition ! 
                      */

/*------------------------------------------------------------------*/
/*
 * QUATERNIONS
 *
 * References:    SIGGRAPH 93, course 60 (Ken Shoemake)
 *                Graphic Gems, Vol 1,2,3,4
 *
 */

        void  quat_set  (QUATERNION self,
                             LIBGEOM_REAL w, 
                             LIBGEOM_REAL x, 
                             LIBGEOM_REAL y, 
                             LIBGEOM_REAL z);  

        void  quat_set_null  (QUATERNION self); 

        void  quat_set_unit  (QUATERNION self); 

        void  quat_assign    (QUATERNION self, 
                              QUATERNION newQuat);      

        void quat_copy (const QUATERNION src,
                        QUATERNION dest);

        void  quat_get_inv      (const QUATERNION self, 
                                 QUATERNION inv); 

        void  quat_get_conj   (const QUATERNION self, 
                               QUATERNION conj); 

        LIBGEOM_REAL quat_get_norm  (const QUATERNION self);    

        LIBGEOM_REAL quat_get_distance (const QUATERNION left,
                                 const QUATERNION right); 

        LIBGEOM_REAL quat_get_sdistance (const QUATERNION left,
                                  const QUATERNION right);

        void  quat_neg       (QUATERNION self); 

        void  quat_normalize (QUATERNION self); 

        LIBGEOM_REAL quat_dot       (const QUATERNION left,
                              const QUATERNION right);

        void  quat_pow       (const QUATERNION self, 
                              LIBGEOM_REAL alpha, 
                              QUATERNION result); 

        void  quat_exp       (const QUATERNION self, 
                              QUATERNION result); 

        void  quat_ln        (const QUATERNION self, 
                              QUATERNION result); 

        void  quat_mults      (const QUATERNION self, 
                               LIBGEOM_REAL s, 
                               QUATERNION result); 

        void  quat_mult         (const QUATERNION left, 
                                 const QUATERNION right, 
                                 QUATERNION result);

        void  quat_sub        (const QUATERNION left, 
                               const QUATERNION right, 
                               QUATERNION result);

        void  quat_add        (const QUATERNION left, 
                               const QUATERNION right, 
                               QUATERNION result);

        void  quat_slerp      (const QUATERNION left,
                               const QUATERNION right,
                               LIBGEOM_REAL u, 
                               QUATERNION result);                              

        void  quat_dump      (const QUATERNION self);   

        void  quat_from_axis  (QUATERNION self,
                               const VT3D axis,
                               LIBGEOM_REAL angle);

        void  quat_from_axis_angle (QUATERNION q,
                                    const VT3D       axis_angle);

        void  quat_to_axis_angle (const QUATERNION q,
                                  VT3D       axis_angle);

        void  quat_from_matrix (const MatriX m, 
                                QUATERNION result);

        void  quat_to_matrix   (const QUATERNION self, 
                                MatriX result); 

        void  quat_from_mr3d   (const MR3D   m, 
                                QUATERNION result);

        void  quat_to_mr3d     (const QUATERNION self, 
                                MR3D   result); 

                
        void quat_from_two_vectors (const VT3D       v1,
                                    const VT3D       v2,
                                    QUATERNION q);

        void quat_from_swing (QUATERNION q,
                              const LIBGEOM_REAL      swing [2]);


        void quat_from_swing_and_twist (QUATERNION q,
                                        const LIBGEOM_REAL      swing [2],
                                        LIBGEOM_REAL      twist);

        void quat_to_swing_and_twist (const QUATERNION q,
                                      LIBGEOM_REAL      swing [2],
                                      LIBGEOM_REAL      *twist);

/*--------------------------------------------------------------------*/
/* --------------------- array1d functions -------------------------- */

        ARRAY1D *array1d_creation (int lines);

        void array1d_destroy (ARRAY1D *array1d);

        void array1d_nul (ARRAY1D *array1d);

        double array1d_norm (ARRAY1D *array1d);

        double array1d_anorm (ARRAY1D *array1d);

        void array1d_copy (ARRAY1D *src,
                           ARRAY1D *dest);

        void array1d_add (ARRAY1D *src_left,
                          ARRAY1D *src_right,
                          ARRAY1D *dest);

        void array1d_sub (ARRAY1D *src_left,
                          ARRAY1D *src_right,
                          ARRAY1D *dest);

        void array1d_mults (ARRAY1D *src,
                            ARRAY1D *dest,
                            double  factor);

        double array1d_dot (ARRAY1D *src1,
                            ARRAY1D *src2);

        double array1d_inv (ARRAY1D *src,
                            ARRAY1D *dest);

        void array1d_write (ARRAY1D *a,
                            FILE    *file);

/*--------------------------------------------------------------------*/
/* ----------------------- array2d functions ------------------------ */

        ARRAY2D *array2d_creation (int lines,
                                   int cols);

        void array2d_destroy (ARRAY2D *array2d);

        int  array2d_same_size (ARRAY2D *a,
                                ARRAY2D *b);

        void array2d_nul (ARRAY2D *array2d);

        void array2d_idt (ARRAY2D *array2d);

        void array2d_copy (ARRAY2D *src,
                           ARRAY2D *dest);

        void array2d_mult (ARRAY2D *a,
                           ARRAY2D *b,
                           ARRAY2D *dest);

        void array2d_add (ARRAY2D *a,
                          ARRAY2D *b,
                          ARRAY2D *dest);

        void array2d_sub (ARRAY2D *a,
                          ARRAY2D *b,
                          ARRAY2D *dest);

        void array2d_mult_array1d (ARRAY2D *array2d,
                                   ARRAY1D *array1d,
                                   ARRAY1D *dest);

        void array2d_scale (ARRAY2D *src,
                            float   scalar,
                            ARRAY2D *dest);


        void array2d_pseudo_inverse (ARRAY2D *src,
                                     ARRAY2D *dest);

        void array2d_damped_least_square_inverse (ARRAY2D *j_inv,    
                                                  ARRAY2D *j_mat,    
                                                  double  damping);

void array2d_compute_LS_and_DLS_inverse (ARRAY2D *LS_inv,   /* output LS-inverse  */
                                         ARRAY2D *DLS_inv,  /* output DLS-inverse */
                                         ARRAY2D *j_mat,    /* input matrix */
                                         ARRAY1D *task,
                                         double  damping);

        void array2d_write (ARRAY2D *a,
                            FILE    *file);

        void array2d_set_column (ARRAY2D *array2d,
                                 int     column,
                                 double  value);

        void array2d_set_line (ARRAY2D *array2d,
                               int     line,
                               double  value);

        void array2d_transpose (ARRAY2D *src,
                                ARRAY2D *dest);

/*--------------------------------------------------------------------*/
/* ----------------------- polynomial functions --------------------- */

        int polynomial_solve_quadric (double c[3], double s[2]);

        int polynomial_solve_cubic   (double c[4], double s[3]);

        int polynomial_solve_quartic (double c[5], double s[4]);

}
#endif

---