Biomechanical Joint Model  Author: Anderson Maciel

---

geom.cpp

Go to the documentation of this file.

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

 FILE               : geom.c
 CREATION DATE      : 01.93
 CREATION AUTHOR(S) : R. Boulic
 ----------------------------------------------------------------------
 KEYWORD(S)         : MatriX, MR3D, VT3D, QUATERNION, VHOM
 DEFINED TYPE(S)    : 
 RELATED TYPE(S)    : 

 FUNCTIONALITY      :  low level geometric transformations
        
  ----------------------------------------------------------------------
 LANGAGE            : ANSI-C
 SYSTEM             : UNIX V 4. / SGI
 GRAPHIC LIBRARY    : SGI GL  
 ----------------------------------------------------------------------
 PROJECT DIRECTOR   : D. THALMANN
 LAB                : EPFL- LIG
 ------------------------------------------------------------------#@*/

//#include "geom.h"
#include <libgeom/geom.h>
#include "stdlib.h"

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

#include <stdio.h>
#include <math.h>
#include <assert.h>

//#ifdef _WIN32
/* Useful constants */
#define M_E                     2.7182818284590452354
#define M_LOG2E         1.4426950408889634074
#define M_LOG10E        0.43429448190325182765
#define M_LN2           0.69314718055994530942
#define M_LN10          2.30258509299404568402
#define M_PI            3.14159265358979323846
#define M_PI_2          1.57079632679489661923
#define M_PI_4          0.78539816339744830962
#define M_1_PI          0.31830988618379067154
#define M_2_PI          0.63661977236758134308
#define M_2_SQRTPI      1.12837916709551257390
#define M_SQRT2         1.41421356237309504880
#define M_SQRT1_2       0.70710678118654752440

#define fsin            sin
#define fcos            cos
#define ftan            tan
#define fasin           asin
#define facos           acos
#define fatan           atan
#define fatan2          atan2
#define fsqrt           sqrt
//#endif

#define DEBUG_GEOM      false

#define SEUIL_GREV1     0.01

/*                                                                      */
/*   -> matrices 3x3 correspondant aux 6 transformations (initiales)    */
/*      standards et vecteur "null".                                    */
/*      Attention : les matrices sont repesentees avec des vect-ligne   */
/*                                                                      */
/*      | image de x |                  a cause des procedures          */
/*      | image de y |                  cablees de l'iris4d             */
/*      | image de z |                                                  */




/*
** -------------------------------------
** -- STATIC Functions -----------------
** -------------------------------------
*/

static void nrerror(char *error_text);  
static double **matmxn_convert(double *a,int nrl,int nrh,int ncl,int nch);      
static void Jinv(double *w,int n,double damping);
static void svd(double *a,int m,int n,double *w,double *v); 
static double pythag(double a,double b);


/*@$-------------------------------------------------------------------
    Static  FUNCTION: 
    FUNCTIONALITY   : 
    KEYWORD(S)      : 
    -------------------------------------------------------------------
    FUNCTION TYPE   : void
    here parameter description with : flow / name / functionality  
    the flow can be one of  IN / OUT / IO (for IN and OUT)
    ---------------------------------------------------------------$@*/

static void nrerror(char *error_text)   
{
   fprintf(stderr,"Numerical Recipes run-time error...\n");
   fprintf(stderr,"%s\n",error_text);
   fprintf(stderr,"...now exiting to system...\n");
   exit(1);
}

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

static double **matmxn_convert(double *a,int nrl,int nrh,
                               int ncl,int nch) 
/* allocate a double matrix m[nrl..nrh][ncl..nch] that points to the matrix
declared in the standard C manner as a[nrow][ncol], where nrow=nrh-nrl+1
and ncol=nch-ncl+1. The routine should be called with the address
&a[0][0] as the first argument. */
{
  int i,j,nrow,ncol;
  double **m;
  
  nrow=nrh-nrl+1;
  ncol=nch-ncl+1;               
                                /* allocate pointers to rows */
  if ((m=(double **) malloc((unsigned) (nrow)*sizeof(double*))) == NULL)
    nrerror("allocation failure in matmxn_convert()");
  m -= nrl;
  
                                /* set pointers to rows */
  for(i=0,j=nrl;i<=nrow-1;i++,j++) m[j]=a+ncol*i-ncl;
                                /* return pointer to array of pointers to rows */
  return m;
}

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

static void svd(double *a,int m,int n,double *w,double *v) 
{
  double **aa,*ww,**vv;
  
  aa = matmxn_convert(a,1,m,1,n);
  ww = w-1;
  vv = matmxn_convert(v,1,n,1,n);
  
  geom::matmxn_svdcmp(aa,m,n,ww,vv);
  
                                /* free a matrix allocated by matmxn_convert() */
  free((char*) (aa+1));
  free((char*) (vv+1));
}

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

void Jinv(double *w,int n,double damping) 
     /* damping = 1/maxTeta where maxTeta is maximum allowed norm */
     /* of rotation vector */
{
#define BIG_W            1.0E+19
#define W_MIN_OVER_W_MAX 0.0001    /* 1.0E-4 */
#define W_PREC           1.0E-10   /* absolute minimum limit for sing. val. */

  int i;
  double den;                   /* 'denominateur', auxiliary var. */
  double lambda;                /* f(lambda) = maxi. allowed val. for sing. values */
  double wmin;                  /* smallest singular value */
  double wmax;                  /* largest singular value */
  double weps;                  /* percentage of wmax */
  
  /*
  ** -- Find bigger singular value
  */
  wmax = 0.0;
  for (i=0;i<n;i++) 
   {
     if (w[i] > wmax) 
       wmax = w[i]; 
   } 
  
  /*
  ** -- Clamp to 0 the singular values smaller than `weps'
  */
  weps = wmax*W_MIN_OVER_W_MAX; 
  wmin = wmax;

  if (weps < W_PREC)   /* Absolute limit for weps, to take care      */
    weps = W_PREC;     /* of numerical errors near the 0 (march 98). */

  for (i=0;i<n;i++) 
   {
     if (w[i] < weps) 
       w[i] = 0.0;
     if (w[i] >= weps && w[i]<wmin) 
       wmin = w[i];                        
   }

   /*
   ** -- Find lambda
   */
  lambda = wmin*(damping-wmin);
  if (lambda <= 0.0) 
   {
     lambda = 0.0;              
   } 
  else 
   {
     lambda = sqrt(lambda);
     if (lambda>=wmin) 
       lambda = 0.5 * damping;
   }

  for (i=0;i<n;i++) 
   {
     if (w[i]>BIG_W) 
      {
        w[i] = 1.0/w[i];
      } 
     else 
      {
        den = (w[i]*w[i]+lambda*lambda);
        if (den>0.0) 
          w[i] /= den;
      }
   }
  
# undef BIG_W 
# undef W_MIN_OVER_W_MAX 
}

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

static double pythag(double a,double b) 
     /* computes sqrt(a^2+b^2) N.-R. 2nd edition */
{
  double absa = fabs(a);
  double absb = fabs(b);
  
  if (absa > absb)
    return absa * sqrt(1+(absb*absb)/(absa*absa));
  else
    return (absb == 0.0 ? 0.0 : absb * sqrt(1+(absa*absa)/(absb*absb)));
}


/*
** -------------------------------------
** -- Public Functions -----------------
** -------------------------------------
*/

namespace geom {

 double safe_acos (double f)

 { return f >= 1. ? 0. :
          f <= -1. ? M_PI : acos (f);
 }


 double safe_asin (double f)

 { return f >= 1. ? M_PI_2 :
          f <= -1. ? -M_PI_2 : asin (f);
 }

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

void    matrix_idt_(MatriX matrix)
{
        register LIBGEOM_REAL   *m ;

        m = (LIBGEOM_REAL*)matrix ; 

        *(m++) = 1.     ; *(m++) = 0.     ; *(m++) = 0.     ; *(m++) = 0. ;
        *(m++) = 0.     ; *(m++) = 1.     ; *(m++) = 0.     ; *(m++) = 0. ;
        *(m++) = 0.     ; *(m++) = 0.     ; *(m++) = 1.     ; *(m++) = 0. ;
        *(m++) = 0.     ; *(m++) = 0.     ; *(m++) = 0.     ; *m     = 1. ;
}


/*-------------------------------------------------------------------*/
void    matrix_copy(const MatriX src, MatriX    dst)
{
        register    LIBGEOM_REAL        *ms , *md ;

        ms = (LIBGEOM_REAL*)src ; md = (LIBGEOM_REAL*)dst ; 

        *(md++)=*(ms++); *(md++)=*(ms++); *(md++)=*(ms++); *(md++)=*(ms++); 
        *(md++)=*(ms++); *(md++)=*(ms++); *(md++)=*(ms++); *(md++)=*(ms++); 
        *(md++)=*(ms++); *(md++)=*(ms++); *(md++)=*(ms++); *(md++)=*(ms++); 
        *(md++)=*(ms++); *(md++)=*(ms++); *(md++)=*(ms++); *md    =*ms; 
}


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

void    matrix_compose(const MR3D       mr, 
                       const VT3D       vt, 
                       MatriX   matrix)
{
        matrix[0][0] = mr[0][0] ; matrix[0][1] = mr[0][1] ; 
        matrix[0][2] = mr[0][2] ; matrix[0][3] = 0. ;
        matrix[1][0] = mr[1][0] ; matrix[1][1] = mr[1][1] ; 
        matrix[1][2] = mr[1][2] ; matrix[1][3] = 0. ;
        matrix[2][0] = mr[2][0] ; matrix[2][1] = mr[2][1] ; 
        matrix[2][2] = mr[2][2] ; matrix[2][3] = 0. ;
        matrix[3][0] = vt[0] ; matrix[3][1] = vt[1] ; matrix[3][2] = vt[2]; 
        matrix[3][3] = 1. ;
}


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

void    matrix_decompose(MR3D       mr, 
                         VT3D       vt, 
                         const MatriX       matrix)
{
  mr[0][0] = matrix[0][0]; mr[0][1] = matrix[0][1]; mr[0][2] = matrix[0][2];
  mr[1][0] = matrix[1][0]; mr[1][1] = matrix[1][1]; mr[1][2] = matrix[1][2]; 
  mr[2][0] = matrix[2][0]; mr[2][1] = matrix[2][1]; mr[2][2] = matrix[2][2]; 
  vt[0] = matrix[3][0] ; vt[1] = matrix[3][1] ; vt[2] = matrix[3][2]; 
}

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

void    matrix_transpose(const MatriX   a,
                         MatriX at)
{
        int     i, j;

        for(i=0 ; i< 4 ; i++)
        {
          at [i][i] = a [i][i];

          for(j=i+1 ; j< 4 ; j++)
          {
            LIBGEOM_REAL tmp = a [i][j];

            at [i][j] = a [j][i];
            at [j][i] = tmp;
          }
        }
}

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

void matrix_rotpart(MatriX_t In, MatriX_t Res)
{
  int i, j;
  
  for (i = 0; i < 4; i++)
   for (j = 0; j < 4; j++)
    {
      if (i != 3 && j != 3)
       Res[i][j] = In[i][j];
      else
       Res[i][j] = 0.0;
    }
  Res[3][3] = 1.0;
}

/*-------------------------------------------------------------------*/
/* Multiplication d'un vecteur homogene par une matrice homogene u*a->v */

void    matrix_mult_vhom(const MatriX       a, 
                         const VHOM         u,
                         VHOM       v)
{
        VHOM    w;
        int     jcol,k;

        for(jcol=0 ; jcol<3 ; jcol++) 
        {
          w[jcol] = 0. ;
          for(k=0 ; k<4 ; k++) w[jcol] += u[k]*a[k][jcol] ;
        }
        w[3] = 1. ;

        for(jcol=0 ; jcol<4 ; jcol++) v[jcol] = w[jcol] ;  
}


/*-------------------------------------------------------------------*/
/* Mult. d'un vecteur 3D par une matrice homogene u*a->v  (15.3.93)     */

void    matrix_mult_vt3d(const MatriX       ma, 
                         const VT3D         vu,
                         VT3D       vv)
{
  if(vu != vv)
  {
    vv[0] = vu[0] * ma[0][0] + vu[1] * ma[1][0] + vu[2] * ma[2][0] + ma[3][0];
    vv[1] = vu[0] * ma[0][1] + vu[1] * ma[1][1] + vu[2] * ma[2][1] + ma[3][1];
    vv[2] = vu[0] * ma[0][2] + vu[1] * ma[1][2] + vu[2] * ma[2][2] + ma[3][2];
  }
  else
  {
    static      VT3D    vw;
    vw[0] = vu[0] * ma[0][0] + vu[1] * ma[1][0] + vu[2] * ma[2][0] + ma[3][0];
    vw[1] = vu[0] * ma[0][1] + vu[1] * ma[1][1] + vu[2] * ma[2][1] + ma[3][1];
    vw[2] = vu[0] * ma[0][2] + vu[1] * ma[1][2] + vu[2] * ma[2][2] + ma[3][2];
    
    vv[0] = vw[0];
    vv[1] = vw[1];
    vv[2] = vw[2];
  }
}


/*-------------------------------------------------------------------*/
void    matrix_inverse(const MatriX         mdir,
                       MatriX       minv)
{
  if(minv != mdir)
  {
    minv[0][0] = mdir[0][0]; minv[0][1] = mdir[1][0]; minv[0][2] = mdir[2][0]; 
    minv[1][0] = mdir[0][1]; minv[1][1] = mdir[1][1]; minv[1][2] = mdir[2][1]; 
    minv[2][0] = mdir[0][2]; minv[2][1] = mdir[1][2]; minv[2][2] = mdir[2][2];
    minv[0][3] = minv[1][3] = minv[2][3] = 0.0f; minv[3][3] = 1.0f;
    
    minv[3][0] = -mdir[3][0] * minv[0][0] -mdir[3][1] * minv[1][0] 
      - mdir[3][2] * minv[2][0];
    minv[3][1] = -mdir[3][0] * minv[0][1] -mdir[3][1] * minv[1][1] 
      - mdir[3][2] * minv[2][1];
    minv[3][2] = -mdir[3][0] * minv[0][2] -mdir[3][1] * minv[1][2]
      - mdir[3][2] * minv[2][2];
  }
  else
  {
    register LIBGEOM_REAL mr, mr1, mr2;
    mr = minv[0][1]; minv[0][1] = minv[1][0]; minv[1][0] = mr;
    mr = minv[0][2]; minv[0][2] = minv[2][0]; minv[2][0] = mr;
    mr = minv[2][1]; minv[2][1] = minv[1][2]; minv[1][2] = mr;
    
    mr =  mdir[3][0] * mdir[0][0] +mdir[3][1] * mdir[1][0] 
      + mdir[3][2] * mdir[2][0];
    mr1 = mdir[3][0] * mdir[0][1] +mdir[3][1] * mdir[1][1] 
      + mdir[3][2] * mdir[2][1];
    mr2 = mdir[3][0] * mdir[0][2] +mdir[3][1] * mdir[1][2]
      + mdir[3][2] * mdir[2][2];
    minv[3][0] = -mr;
    minv[3][1] = -mr1;
    minv[3][2] = -mr2;
  }
}

/*--------------------------------------------------------------------*/
/*  Calcul de l'inverse d'une matrice homogene                           */

void matrix_inverse_det(MatriX res, LIBGEOM_REAL determinant, const MatriX source)
{
  MatriX   inv;
  int          i, j;
  LIBGEOM_REAL        scale;

  /*
  ** -- Attention: la matrice est supposee inversible!
  */
  scale = 1.0 / determinant;

  for (i = GEOM_X_INDEX; i <= GEOM_W_INDEX; ++i) {
    for (j = GEOM_X_INDEX; j <= GEOM_W_INDEX; ++j) {
      inv[i][j] = matrix_cofactor(source, j, i) * scale;
    }
  }

  matrix_copy(inv, res);

}

/*--------------------------------------------------------------------*/
/*       Calcul d'un cofacteur d'une matrice homogene                    */

LIBGEOM_REAL matrix_cofactor(const MatriX matrix, int i, int j)
{
  MatriX      mij;
  int             i1, j1, i2, j2;
  static LIBGEOM_REAL    factorsign[] = { 1.0, -1.0, 1.0, -1.0 };
      
  matrix_idt_(mij);

  i1 = GEOM_X_INDEX;
  for (i2 = GEOM_X_INDEX; i2 < GEOM_W_INDEX; ++i2) {
    if (i1 == i) ++i1;

    j1 = GEOM_X_INDEX;
    for (j2 = GEOM_X_INDEX; j2 < GEOM_W_INDEX; ++j2) {
      if (j1 == j) ++j1;

      mij[i2][j2] = matrix[i1][j1];
      ++j1;
    }
    ++i1;   
  }

  return (factorsign[i] * factorsign[j] * matrix_determinant(mij)); 
} 

/*-------------------------------------------------------------------*/
/*    Calcul du determinant d'une matrice homogene                      */

LIBGEOM_REAL matrix_determinant(const MatriX matrix)
{

  LIBGEOM_REAL det;

  det =   
    matrix[GEOM_X_INDEX][GEOM_X_INDEX] * 
    ( matrix[GEOM_Y_INDEX][GEOM_Y_INDEX] * 
      matrix[GEOM_Z_INDEX][GEOM_Z_INDEX] * 
      matrix[GEOM_W_INDEX][GEOM_W_INDEX]
    + matrix[GEOM_Y_INDEX][GEOM_Z_INDEX] * 
      matrix[GEOM_Z_INDEX][GEOM_W_INDEX] * 
      matrix[GEOM_W_INDEX][GEOM_Y_INDEX]
    + matrix[GEOM_Y_INDEX][GEOM_W_INDEX] * 
      matrix[GEOM_Z_INDEX][GEOM_Y_INDEX] * 
      matrix[GEOM_W_INDEX][GEOM_Z_INDEX]
    - matrix[GEOM_W_INDEX][GEOM_Y_INDEX] * 
      matrix[GEOM_Z_INDEX][GEOM_Z_INDEX] * 
      matrix[GEOM_Y_INDEX][GEOM_W_INDEX]
    - matrix[GEOM_W_INDEX][GEOM_Z_INDEX] * 
      matrix[GEOM_Z_INDEX][GEOM_W_INDEX] * 
      matrix[GEOM_Y_INDEX][GEOM_Y_INDEX]
    - matrix[GEOM_W_INDEX][GEOM_W_INDEX] *
      matrix[GEOM_Z_INDEX][GEOM_Y_INDEX] *
      matrix[GEOM_Y_INDEX][GEOM_Z_INDEX] ) -
    matrix[GEOM_X_INDEX][GEOM_Y_INDEX] * 
    ( matrix[GEOM_Y_INDEX][GEOM_X_INDEX] *
      matrix[GEOM_Z_INDEX][GEOM_Z_INDEX] *
      matrix[GEOM_W_INDEX][GEOM_W_INDEX]
    + matrix[GEOM_Y_INDEX][GEOM_Z_INDEX] *
      matrix[GEOM_Z_INDEX][GEOM_W_INDEX] *
      matrix[GEOM_W_INDEX][GEOM_X_INDEX]
    + matrix[GEOM_Y_INDEX][GEOM_W_INDEX] *
      matrix[GEOM_Z_INDEX][GEOM_X_INDEX] *
      matrix[GEOM_W_INDEX][GEOM_Z_INDEX]
    - matrix[GEOM_W_INDEX][GEOM_X_INDEX] *
      matrix[GEOM_Z_INDEX][GEOM_Z_INDEX] *
      matrix[GEOM_Y_INDEX][GEOM_W_INDEX]
    - matrix[GEOM_W_INDEX][GEOM_Z_INDEX] *
      matrix[GEOM_Z_INDEX][GEOM_W_INDEX] *
      matrix[GEOM_Y_INDEX][GEOM_X_INDEX]
    - matrix[GEOM_W_INDEX][GEOM_W_INDEX] *
      matrix[GEOM_Z_INDEX][GEOM_X_INDEX] *
      matrix[GEOM_Y_INDEX][GEOM_Z_INDEX] ) +
    matrix[GEOM_X_INDEX][GEOM_Z_INDEX] * 
    ( matrix[GEOM_Y_INDEX][GEOM_X_INDEX] *
      matrix[GEOM_Z_INDEX][GEOM_Y_INDEX] *
      matrix[GEOM_W_INDEX][GEOM_W_INDEX]
    + matrix[GEOM_Y_INDEX][GEOM_Y_INDEX] *
      matrix[GEOM_Z_INDEX][GEOM_W_INDEX] *
      matrix[GEOM_W_INDEX][GEOM_X_INDEX]
    + matrix[GEOM_Y_INDEX][GEOM_W_INDEX] *
      matrix[GEOM_Z_INDEX][GEOM_X_INDEX] *
      matrix[GEOM_W_INDEX][GEOM_Y_INDEX]
    - matrix[GEOM_W_INDEX][GEOM_X_INDEX] *
      matrix[GEOM_Z_INDEX][GEOM_Y_INDEX] *
      matrix[GEOM_Y_INDEX][GEOM_W_INDEX]
    - matrix[GEOM_W_INDEX][GEOM_Y_INDEX] *
      matrix[GEOM_Z_INDEX][GEOM_W_INDEX] *
      matrix[GEOM_Y_INDEX][GEOM_X_INDEX]
    - matrix[GEOM_W_INDEX][GEOM_W_INDEX] *
      matrix[GEOM_Z_INDEX][GEOM_X_INDEX] *
      matrix[GEOM_Y_INDEX][GEOM_Y_INDEX] ) -
    matrix[GEOM_X_INDEX][GEOM_W_INDEX] * 
    ( matrix[GEOM_Y_INDEX][GEOM_X_INDEX] *
      matrix[GEOM_Z_INDEX][GEOM_Y_INDEX] *
      matrix[GEOM_W_INDEX][GEOM_Z_INDEX]
    + matrix[GEOM_Y_INDEX][GEOM_Y_INDEX] *
      matrix[GEOM_Z_INDEX][GEOM_Z_INDEX] *
      matrix[GEOM_W_INDEX][GEOM_X_INDEX]
    + matrix[GEOM_Y_INDEX][GEOM_Z_INDEX] *
      matrix[GEOM_Z_INDEX][GEOM_X_INDEX] *
      matrix[GEOM_W_INDEX][GEOM_Y_INDEX]
    - matrix[GEOM_W_INDEX][GEOM_X_INDEX] *
      matrix[GEOM_Z_INDEX][GEOM_Y_INDEX] *
      matrix[GEOM_Y_INDEX][GEOM_Z_INDEX]
    - matrix[GEOM_W_INDEX][GEOM_Y_INDEX] * 
      matrix[GEOM_Z_INDEX][GEOM_Z_INDEX] *
      matrix[GEOM_Y_INDEX][GEOM_X_INDEX]
    - matrix[GEOM_W_INDEX][GEOM_Z_INDEX] *
      matrix[GEOM_Z_INDEX][GEOM_X_INDEX] *
      matrix[GEOM_Y_INDEX][GEOM_Y_INDEX] );

  return det;  
}

/*-------------------------------------------------------------------*/
/*      effectue le produit matriciel ma*mb et range le resultat dans mc*/


#define matrix_is_affine(m) ((m [0][3] == 0.) && (m [1][3] == 0.) && \
                             (m [2][3] == 0.) && (m [3][3] == 1.))

void matrix_mult (const MatriX ma,
                  const MatriX mb,
                  MatriX mc)

{
  if (matrix_is_affine (ma) && matrix_is_affine (mb))
  {
    register LIBGEOM_REAL *a , *b , *c;

    LIBGEOM_REAL a00 , a01 , a02 ,
        a10 , a11 , a12 ,
        a20 , a21 , a22 ,
        a30 , a31 , a32 ,

        b00 , b01 , b02 ,
        b10 , b11 , b12 ,
        b20 , b21 , b22 ,
        b30 , b31 , b32 ;

        a = (LIBGEOM_REAL*) ma;
        b = (LIBGEOM_REAL*) mb;
        c = (LIBGEOM_REAL*) mc; 

        a00 = *(a++) ; a01 = *(a++) ; a02 = *(a++) ;  a++ ;
        a10 = *(a++) ; a11 = *(a++) ; a12 = *(a++) ;  a++ ;
        a20 = *(a++) ; a21 = *(a++) ; a22 = *(a++) ;  a++ ;
        a30 = *(a++) ; a31 = *(a++) ; a32 = *a ;

        b00 = *(b++) ; b01 = *(b++) ; b02 = *(b++) ; b++ ;
        b10 = *(b++) ; b11 = *(b++) ; b12 = *(b++) ; b++ ;
        b20 = *(b++) ; b21 = *(b++) ; b22 = *(b++) ; b++ ;
        b30 = *(b++) ; b31 = *(b++) ; b32 = *b ;

        *(c++) = a00 * b00  +  a01 * b10  +  a02 * b20  ;
        *(c++) = a00 * b01  +  a01 * b11  +  a02 * b21  ;
        *(c++) = a00 * b02  +  a01 * b12  +  a02 * b22  ;
        *(c++) = 0. ;

        *(c++) = a10 * b00  +  a11 * b10  +  a12 * b20  ;
        *(c++) = a10 * b01  +  a11 * b11  +  a12 * b21  ;
        *(c++) = a10 * b02  +  a11 * b12  +  a12 * b22  ;
        *(c++) = 0. ;

        *(c++) = a20 * b00  +  a21 * b10  +  a22 * b20  ;
        *(c++) = a20 * b01  +  a21 * b11  +  a22 * b21  ;
        *(c++) = a20 * b02  +  a21 * b12  +  a22 * b22  ;
        *(c++) = 0. ;

        *(c++) = a30 * b00  +  a31 * b10  +  a32 * b20 + b30 ;
        *(c++) = a30 * b01  +  a31 * b11  +  a32 * b21 + b31 ;
        *(c++) = a30 * b02  +  a31 * b12  +  a32 * b22 + b32 ;
        *(c)   = 1. ;
  }
  else /* version for general case, seldom used -> no optimisation */
  {
    int    i, j;
    MatriX mt;

    for (i=0; i < 4; i++)
      for (j=0; j < 4; j++)
        mt [i][j] = ma [i][0] * mb [0][j] +
                    ma [i][1] * mb [1][j] +
                    ma [i][2] * mb [2][j] +
                    ma [i][3] * mb [3][j];

    matrix_copy (mt, mc);
  }
}
/*-------------------------------------------------------------------*/
/*  pour des matrices affines seulement : usage reserve              */
/*  effectue le produit matriciel ma*mb et range le resultat dans mc*/


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

{  
  register LIBGEOM_REAL *a , *b , *c;

  LIBGEOM_REAL a00 , a01 , a02 ,
        a10 , a11 , a12 ,
        a20 , a21 , a22 ,
        a30 , a31 , a32 ,

        b00 , b01 , b02 ,
        b10 , b11 , b12 ,
        b20 , b21 , b22 ,
        b30 , b31 , b32 ;

        a = (LIBGEOM_REAL*) ma;
        b = (LIBGEOM_REAL*) mb;
        c = (LIBGEOM_REAL*) mc; 

        a00 = *(a++) ; a01 = *(a++) ; a02 = *(a++) ;  a++ ;
        a10 = *(a++) ; a11 = *(a++) ; a12 = *(a++) ;  a++ ;
        a20 = *(a++) ; a21 = *(a++) ; a22 = *(a++) ;  a++ ;
        a30 = *(a++) ; a31 = *(a++) ; a32 = *a ;

        b00 = *(b++) ; b01 = *(b++) ; b02 = *(b++) ; b++ ;
        b10 = *(b++) ; b11 = *(b++) ; b12 = *(b++) ; b++ ;
        b20 = *(b++) ; b21 = *(b++) ; b22 = *(b++) ; b++ ;
        b30 = *(b++) ; b31 = *(b++) ; b32 = *b ;

        *(c++) = a00 * b00  +  a01 * b10  +  a02 * b20  ;
        *(c++) = a00 * b01  +  a01 * b11  +  a02 * b21  ;
        *(c++) = a00 * b02  +  a01 * b12  +  a02 * b22  ;
        *(c++) = 0. ;

        *(c++) = a10 * b00  +  a11 * b10  +  a12 * b20  ;
        *(c++) = a10 * b01  +  a11 * b11  +  a12 * b21  ;
        *(c++) = a10 * b02  +  a11 * b12  +  a12 * b22  ;
        *(c++) = 0. ;

        *(c++) = a20 * b00  +  a21 * b10  +  a22 * b20  ;
        *(c++) = a20 * b01  +  a21 * b11  +  a22 * b21  ;
        *(c++) = a20 * b02  +  a21 * b12  +  a22 * b22  ;
        *(c++) = 0. ;

        *(c++) = a30 * b00  +  a31 * b10  +  a32 * b20 + b30 ;
        *(c++) = a30 * b01  +  a31 * b11  +  a32 * b21 + b31 ;
        *(c++) = a30 * b02  +  a31 * b12  +  a32 * b22 + b32 ;
        *(c)   = 1. ;
}

/*-------------------------------------------------------------------*/
bool    matrix_egalite(const MatriX     a,
                       const MatriX     b)
{
        MR3D    mra,mrb;
        VT3D    vta,vtb;

        matrix_decompose(mra,vta,a);
        matrix_decompose(mrb,vtb,b);
        if(mr3d_egalite(mra,mrb,EPSIL_MR3D)&& vt3d_egalite(vta,vtb,EPSIL_VT3D))
          return(true);
        else return(false);
}

/*-------------------------------------------------------------------*/
/* cette procedure cherche a attirer une transformation homogene    */
/* vers l'identite.                                                 */

int     matrix_attraction_idt(MatriX    a)
{
        MR3D    mr ;
        VT3D    vt ;
        int     result1,result2 ;

        a[0][3] = a[1][3] = a[2][3] = 0. ;
        a[3][3] = 1. ;

        matrix_decompose(mr,vt,a);

        result1 = mr3d_attraction_idt(mr,EPSIL_MR3D);
        result2 = vt3d_attraction_nul(vt,EPSIL_VT3D);

        if((result1  == ATTRACTION_COMPLETE)&&(result2 == ATTRACTION_COMPLETE))
        {
          matrix_compose(mr,vt,a);
          return(ATTRACTION_COMPLETE);
        }

        if((result1  == ATTRACTION_NULLE) && (result2 == ATTRACTION_NULLE))
          return(ATTRACTION_NULLE);

        matrix_compose(mr,vt,a);
        return(ATTRACTION_PARTIELLE);
}

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

int     matrix_proximite(const MatriX       a,
                         const MatriX       b, 
                         LIBGEOM_REAL       seuil_mr,
                         LIBGEOM_REAL       seuil_vt)
{
        MR3D    mra,mrb;
        VT3D    vta,vtb;
        int     proximite;
        bool    eqmr,eqvt;

        matrix_decompose(mra,vta,a);
        matrix_decompose(mrb,vtb,b);
        eqmr = mr3d_egalite(mra,mrb,seuil_mr);
        eqvt = vt3d_egalite(vta,vtb,seuil_vt);
        if(eqmr && eqvt)        proximite = 0;
        else if(eqmr && !eqvt)  proximite = 1;
        else if(!eqmr && eqvt)  proximite = 2;
        else                    proximite = 3;
        return(proximite);
}

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

void matrix_lerp (const MatriX_t a, const MatriX_t b, LIBGEOM_REAL u, MatriX result)
     /* linear interpolation */
     /* result = a to b, when u = 0 to 1 */
{
  QUATERNION prevQt;            /* previous quaternion */
  QUATERNION crtQt;             /* current quaternion */
  QUATERNION newQt;             /* new quaternion */
  register LIBGEOM_REAL v;
                                /* interpolate rotation part */
  quat_from_matrix(a,prevQt);
  quat_from_matrix(b,crtQt);
  quat_slerp(prevQt,crtQt,u,newQt);
  quat_to_matrix(newQt, result);

                                /* interpolate translation part */
  v = 1-u;                      /* speed up arithmetic */
  result[3][0] = v * a[3][0] + u * b[3][0];
  result[3][1] = v * a[3][1] + u * b[3][1];
  result[3][2] = v * a[3][2] + u * b[3][2];
  result[0][3] = 0;
  result[1][3] = 0;
  result[2][3] = 0;
  result[3][3] = 1; 
}

/*@#-------------------------------------------------------------------
    Public FUNCTION : 
    FUNCTIONALITY   : write the matrix m in the file f.
    KEYWORD(S)      : 
    -------------------------------------------------------------------
    FUNCTION TYPE   : bool
    here parameter description with : flow / name / functionality  
    the flow can be one of  IN / OUT / IO (for IN and OUT)
    ---------------------------------------------------------------#@*/
bool    matrix_write(const MatriX m, FILE *f)
{
  char *format_str;

  if (sizeof(LIBGEOM_REAL) == sizeof(float))
    format_str = "%f %f %f %f";
  else 
    format_str = "%lf %lf %lf %lf";

    if (f)
    {
        fprintf(f, "\n"); 
        fprintf(f, format_str, 
                m[GEOM_X_INDEX][GEOM_X_INDEX], 
                m[GEOM_X_INDEX][GEOM_Y_INDEX], 
                m[GEOM_X_INDEX][GEOM_Z_INDEX], 
                m[GEOM_X_INDEX][GEOM_W_INDEX]);
        fprintf(f, "\n"); 
        fprintf(f, format_str, 
                m[GEOM_Y_INDEX][GEOM_X_INDEX], 
                m[GEOM_Y_INDEX][GEOM_Y_INDEX], 
                m[GEOM_Y_INDEX][GEOM_Z_INDEX], 
                m[GEOM_Y_INDEX][GEOM_W_INDEX]);
        fprintf(f, "\n"); 
        fprintf(f, format_str, 
                m[GEOM_Z_INDEX][GEOM_X_INDEX], 
                m[GEOM_Z_INDEX][GEOM_Y_INDEX], 
                m[GEOM_Z_INDEX][GEOM_Z_INDEX], 
                m[GEOM_Z_INDEX][GEOM_W_INDEX]);
        fprintf(f, "\n"); 
        fprintf(f, format_str, 
                m[GEOM_W_INDEX][GEOM_X_INDEX], 
                m[GEOM_W_INDEX][GEOM_Y_INDEX], 
                m[GEOM_W_INDEX][GEOM_Z_INDEX], 
                m[GEOM_W_INDEX][GEOM_W_INDEX]); 
        fprintf(f, "\n"); 
        return(true);
    }
    return(false);
}


/*@#-------------------------------------------------------------------
    Public FUNCTION : 
    FUNCTIONALITY   : read the matrix m from the file f.
    KEYWORD(S)      : 
    -------------------------------------------------------------------
    FUNCTION TYPE   : bool
    here parameter description with : flow / name / functionality  
    the flow can be one of  IN / OUT / IO (for IN and OUT)
    ---------------------------------------------------------------#@*/
bool    matrix_read(MatriX m, FILE *f)
{
  char *format_str;

  if (sizeof(LIBGEOM_REAL) == sizeof(float))
    format_str = "%f %f %f %f";
  else 
    format_str = "%lf %lf %lf %lf";

    if (f)
    {
         
        fscanf(f, format_str, &m[GEOM_X_INDEX][GEOM_X_INDEX], 
        &m[GEOM_X_INDEX][GEOM_Y_INDEX],&m[GEOM_X_INDEX][GEOM_Z_INDEX],
        &m[GEOM_X_INDEX][GEOM_W_INDEX]);
         
        fscanf(f, format_str, &m[GEOM_Y_INDEX][GEOM_X_INDEX], 
        &m[GEOM_Y_INDEX][GEOM_Y_INDEX], &m[GEOM_Y_INDEX][GEOM_Z_INDEX], 
        &m[GEOM_Y_INDEX][GEOM_W_INDEX]);
         
        fscanf(f, format_str, &m[GEOM_Z_INDEX][GEOM_X_INDEX], 
        &m[GEOM_Z_INDEX][GEOM_Y_INDEX], &m[GEOM_Z_INDEX][GEOM_Z_INDEX], 
        &m[GEOM_Z_INDEX][GEOM_W_INDEX]);
         
        fscanf(f, format_str, &m[GEOM_W_INDEX][GEOM_X_INDEX], 
        &m[GEOM_W_INDEX][GEOM_Y_INDEX], &m[GEOM_W_INDEX][GEOM_Z_INDEX], 
        &m[GEOM_W_INDEX][GEOM_W_INDEX]);        
         
        return(true);
    }
    return(false);
}


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

void    mr3d_idt_(MR3D  mr)
{
        register LIBGEOM_REAL   *m ;

        m = (LIBGEOM_REAL*)mr ; 

        *(m++) = 1.     ; *(m++) = 0.     ; *(m++) = 0.     ; 
        *(m++) = 0.     ; *(m++) = 1.     ; *(m++) = 0.     ; 
        *(m++) = 0.     ; *(m++) = 0.     ; *m     = 1.     ; 
}

/*-------------------------------------------------------------------*/
void    mr3d_get(const MatriX       matrix, 
                 MR3D       a)
{
  int   i,j;
  
  for(i=0 ; i<3 ; i++)
   for(j=0 ; j<3 ; j++)
    a[i][j] = matrix[i][j] ;
}

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

void    mr3d_copy(const MR3D src, MR3D dst)
{
  dst[0][0] = src[0][0] ; dst[0][1] = src[0][1]   ; dst[0][2] = src[0][2] ; 
  dst[1][0] = src[1][0] ; dst[1][1] = src[1][1]   ; dst[1][2] = src[1][2] ; 
  dst[2][0] = src[2][0] ; dst[2][1] = src[2][1]   ; dst[2][2] = src[2][2] ;
}

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

void    mr3d_load(MatriX    matrix, 
                  const MR3D        a)           
{
        int     i,j;

        for(i=0 ; i<3 ; i++)
          for(j=0 ; j<3 ; j++)
              matrix[i][j] = a[i][j] ;
} 

/*-------------------------------------------------------------------*/
void    mr3d_transpose(const MR3D       ma,
                       MR3D     mat)
{
  if(ma != mat)
  {
    mat[0][0] = ma[0][0]; mat[0][1] = ma[1][0]; mat[0][2] = ma[2][0]; 
    mat[1][0] = ma[0][1]; mat[1][1] = ma[1][1]; mat[1][2] = ma[2][1]; 
    mat[2][0] = ma[0][2]; mat[2][1] = ma[1][2]; mat[2][2] = ma[2][2];
  }
  else 
  { /* self transpose case */    
        register    LIBGEOM_REAL        w ;
        register    LIBGEOM_REAL        *a ;

        a = (LIBGEOM_REAL*)ma ;

        w = *(a+1) ; *(a+1) = *(a+3) ; *(a+3) = w ;
        w = *(a+2) ; *(a+2) = *(a+6) ; *(a+6) = w ;
        w = *(a+5) ; *(a+5) = *(a+7) ; *(a+7) = w ;
  }
}

/*-------------------------------------------------------------------*/
void    mr3d_self_transpose(MR3D ma)
{
        register    LIBGEOM_REAL        w ;
        register    LIBGEOM_REAL        *a ;

        a = (LIBGEOM_REAL*)ma ;

        w = *(a+1) ; *(a+1) = *(a+3) ; *(a+3) = w ;
        w = *(a+2) ; *(a+2) = *(a+6) ; *(a+6) = w ;
        w = *(a+5) ; *(a+5) = *(a+7) ; *(a+7) = w ;
}

/*-------------------------------------------------------------------*/
void    mr3d_perm_circ(MR3D     a, 
                       MR3D     b)
{
        VT3D    v;
        int     jcol;
        
        for(jcol=0 ; jcol<3 ; jcol++)
        {
          v[jcol]    = a[0][jcol] ;
          b[0][jcol] = a[1][jcol] ;
          b[1][jcol] = a[2][jcol] ;
          b[2][jcol] = v[jcol] ;
        }
}

/*-------------------------------------------------------------------*/
/* "l'oppose" calcule ici n'est pas obtenu en prenant l'oppose de       */
/* chacun des termes de la matrice car on n'obtiendrait pas une base    */
/* droite. On choisit donc de prendre l'oppose du seul vecteur z        */
/* car ce vecteur est privilegie pour la representation du mouvement et */
/* on permutte les vecteurs x et y obtenant ainsi une base droite.      */

void    mr3d_opposite(MR3D      a, 
                      MR3D      b)
{
        VT3D    v;
        int     jcol;

        for(jcol=0 ; jcol<3 ; jcol++)
        {
          b[2][jcol] = -a[2][jcol] ;
          v[jcol]    =  a[0][jcol] ;
          b[0][jcol] =  a[1][jcol] ;
          b[1][jcol] =  v[jcol] ;
        }
}

/*-------------------------------------------------------------------*/
/* la "symetrie" calculee ici conserve la coordonnee z et oppose les    */
/* coordonnes x et y . Ce module est complementaire des deux precedents */
/* et permet d'obtenir n'importe qu'elle orientation orthogonale d'une  */
/* base par rapport a une autre en combinant quelques appels .          */

void    mr3d_symtxy(MR3D        a, 
                    MR3D        b)
{
        int     jcol;

        for(jcol=0 ; jcol<3 ; jcol++)
        {
          b[0][jcol] = -a[0][jcol] ;
          b[1][jcol] = -a[1][jcol] ;
          b[2][jcol] = a[2][jcol] ;
        }
}

/*@#-------------------------------------------------------------------
    Public FUNCTION : 
    FUNCTIONALITY   : matrix multiplication by scalar
    KEYWORD(S)      : 
    -------------------------------------------------------------------
    FUNCTION TYPE   : void
    here parameter description with : flow / name / functionality  
    the flow can be one of  IN / OUT / IO (for IN and OUT)
    ---------------------------------------------------------------#@*/
void     mr3d_scale (const MR3D self, LIBGEOM_REAL scalar, MR3D res)
{
    register int    i, j;
    
    for (i = GEOM_X_INDEX; i <= GEOM_Z_INDEX; ++i)
    {
        for (j = GEOM_X_INDEX; j <= GEOM_Z_INDEX; ++j)
        {
           res[i][j] = scalar * self[i][j];
        }
    }
}

/*@#-------------------------------------------------------------------
    Public FUNCTION : 
    FUNCTIONALITY   : matrix substraction
    KEYWORD(S)      : 
    -------------------------------------------------------------------
    FUNCTION TYPE   : void
    here parameter description with : flow / name / functionality  
    the flow can be one of  IN / OUT / IO (for IN and OUT)
    ---------------------------------------------------------------#@*/
void     mr3d_sub (const MR3D left, const MR3D right, MR3D res)
{
    register int    i, j;
    
    for (i = GEOM_X_INDEX; i <= GEOM_Z_INDEX; ++i)
    {
        for (j = GEOM_X_INDEX; j <= GEOM_Z_INDEX; ++j)
        {
           res[i][j] = left[i][j] - right[i][j];
        }
    }
}

/*@#-------------------------------------------------------------------
    Public FUNCTION : 
    FUNCTIONALITY   : matrix addition
    KEYWORD(S)      : 
    -------------------------------------------------------------------
    FUNCTION TYPE   : void
    here parameter description with : flow / name / functionality  
    the flow can be one of  IN / OUT / IO (for IN and OUT)
    ---------------------------------------------------------------#@*/
void     mr3d_add (const MR3D left, const MR3D right, MR3D res)
{
    register int    i, j;
    
    for (i = GEOM_X_INDEX; i <= GEOM_Z_INDEX; ++i)
    {
        for (j = GEOM_X_INDEX; j <= GEOM_Z_INDEX; ++j)
        {
           res[i][j] = left[i][j] + right[i][j];
        }
    }
}


/*-------------------------------------------------------------------*/
/* effectue la multiplication des matrices 3x3   :    a*b -> c          */

void    mr3d_mult(const MR3D  a, const MR3D  b, MR3D  c)
{
        MR3D    w;
        register int    i,j ;

        for(i=0 ; i<3 ; i++)
        {
          for(j=0 ; j<3 ; j++)
          {
            w[i][j] = a [i][0] * b [0][j] + a [i][1] * b [1][j] + a [i][2] * b [2][j];
          }
        }

        for(i=0 ; i<3 ; i++)
          for(j=0 ; j<3 ; j++)
            c[i][j] = w[i][j] ;    
}

/*-------------------------------------------------------------------*/
/* multiplication dans l'espace 3d  u*a  donnant le vecteur v           */
/* version optimisee (ancienne version en commentaire)                  */

void    mr3d_mult_vt3d(const MR3D           ma,
                       const VT3D           vu,
                       VT3D         vv)
{
  if(vu != vv)
  { 
    vv[0] = vu[0] * ma[0][0]  +  vu[1] * ma[1][0]   +  vu[2] * ma[2][0];
    vv[1] = vu[0] * ma[0][1]  +  vu[1] * ma[1][1]   +  vu[2] * ma[2][1];
    vv[2] = vu[0] * ma[0][2]  +  vu[1] * ma[1][2]   +  vu[2] * ma[2][2];
  }
  else
  { 
    static      VT3D    vw;
 
    vw[0] = vu[0] * ma[0][0]  +  vu[1] * ma[1][0]   +  vu[2] * ma[2][0];
    vw[1] = vu[0] * ma[0][1]  +  vu[1] * ma[1][1]   +  vu[2] * ma[2][1];
    vw[2] = vu[0] * ma[0][2]  +  vu[1] * ma[1][2]   +  vu[2] * ma[2][2];
    
    vv[0] = vw[0];
    vv[1] = vw[1];
    vv[2] = vw[2];
  }
}

/*-------------------------------------------------------------------*/
#define EPSILON 1e-4

/*-------------------------------------------------------------------*/
/* conversion d'un vecteur de rotation en matrice de rotation           */
/* en passant par un quaternion (source toolkit matrix_h.c)             */

void    mr3d_from_axis_angle(MR3D mr, const VT3D faxis)
{
   double    k[3];
   double    axis[3];
   double    ang;               /* scalar angle of rotation */
   double    c, s, v;           /* cosine, sine, versine of ang */

    /* these equations are set up for cw rot. */
    axis[0]= (double) faxis[0];
    axis[1]= (double) faxis[1];
    axis[2]= (double) faxis[2];
    
   ang = sqrt((axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]));

   if (ang < EPSILON)
   {
     mr3d_idt_ (mr);

     return;
   }

   k[0] = axis[0]/ang ;       /* normalization */
   k[1] = axis[1]/ang ;
   k[2] = axis[2]/ang ;

   c = cos( ang );             /* in right-handed, ccw rot in left-handed */
   s = sin( ang );
   v = 1.0 - c;                /* versine(x) = 1 - cos(x) */

   mr[0][0] = (LIBGEOM_REAL) (k[0] * k[0] * v + c);
   mr[0][1] = (LIBGEOM_REAL) (k[0] * k[1] * v + k[2] * s);
   mr[0][2] = (LIBGEOM_REAL) (k[0] * k[2] * v - k[1] * s);

   mr[1][0] = (LIBGEOM_REAL) (k[1] * k[0] * v - k[2] * s);
   mr[1][1] = (LIBGEOM_REAL) (k[1] * k[1] * v + c);
   mr[1][2] = (LIBGEOM_REAL) (k[1] * k[2] * v + k[0] * s);

   mr[2][0] = (LIBGEOM_REAL) (k[2] * k[0] * v + k[1] * s);
   mr[2][1] = (LIBGEOM_REAL) (k[2] * k[1] * v - k[0] * s);
   mr[2][2] = (LIBGEOM_REAL) (k[2] * k[2] * v + c);
}

/*-------------------------------------------------------------------*/
/* conversion d'une matrice de rotation en vecteur de rotation          */
/* en passant par un quaternion (source toolkit matrix_h.c)             */

void    mr3d_to_axis_angle(VT3D axis, const MR3D mr)

{ MatriX     matrix;
  VT3D       vt_zero = { 0., 0., 0. };
  QUATERNION quat;
  double     angle, scalar, norm;

  matrix_compose (mr, vt_zero, matrix);

  quat_from_matrix (matrix, quat);

   /* calculate angle */
   angle = 2.0 * safe_acos(quat[3]);

   norm = sqrt(quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2]) ;
   if (norm > EPSILON)
      {
        scalar  = angle/norm ;
        axis[0] = (LIBGEOM_REAL) (scalar * quat[0]);
        axis[1] = (LIBGEOM_REAL) (scalar * quat[1]);
        axis[2] = (LIBGEOM_REAL) (scalar * quat[2]);
      }
   else
      {
        axis[0] = (LIBGEOM_REAL) angle ;
        axis[1] = 0. ;
        axis[2] = 0. ;
      }
}

/*-------------------------------------------------------------------*/
/* renvoie seulement la valeur de l'axis-angle de la matrice  mr     */
/* cette valeur est toujours positive comprise dans [0,pi]           */
/* d'apres Graphic Gems I, p466 */

double  mr3d_axis_angle(const MR3D mr)
{
   double ww;

   ww = (mr[0][0] + mr[1][1] + mr[2][2] -1.)/2. ;
   return safe_acos(ww);
}

/*-------------------------------------------------------------------*/
bool mr3d_egalite(const MR3D    a,
                  const MR3D    b, 
                  LIBGEOM_REAL  seuil_mr)
{
        LIBGEOM_REAL    delta;
        int     i,j,egalite;

        for(egalite=1,i=0 ; i<3 ; i++)
        {
          for(j=0 ; j<3 ; j++)
          {
            delta = fabs(a[i][j]-b[i][j]) ;
            if(delta > seuil_mr)
            {
              if(DEBUG_GEOM)
              {
                printf(" warning : 3x3 matrice difference of %f \n", delta);
                printf("           for element [%d][%d]\n", i, j);
              }
              egalite = 0;
            }
          }
        }
        return(egalite);
}

/*-------------------------------------------------------------------*/
/* Cette procedure cherche a attirer une matrice vers l'identite.   */
/* si l'ecart entre l'un de ses elements et la valeur correspondante*/
/* de l'identite est inferieur a un seuil alors l'element la prend. */


int     mr3d_attraction_idt(MR3D        a, 
                            LIBGEOM_REAL        seuil_mr)
{
        int     i,j,k ;

        k = 0 ;

        for(i=0 ; i<3 ; i++)
        {
          for(j=0 ; j<3 ; j++)
          {
            if(i!=j)
              if(fabs((double)a[i][j]) < seuil_mr) 
                {a[i][j] = 0. ; k++ ; }
            else
              if(fabs((double)(a[i][j] - 1.)) < seuil_mr)
                {a[i][j] = 1. ; k++ ; }
          }
        }

        if(k == 0)         return(ATTRACTION_NULLE);
        else if(k == 9)    return(ATTRACTION_COMPLETE);
        else               return(ATTRACTION_PARTIELLE);
}

/*-------------------------------------------------------------------*/
/* Cette procedure cherche a attirer une matrice vers une combinaison   */
/* de vecteurs formes de 0. et de 1. si la difference est inferieure a  */
/* la valeur de seuil donnee en parametre. Aucune verification de       */
/* normalisation du resultat n'est effectuee.                           */

int     mr3d_attraction_ini(MR3D        a, 
                            LIBGEOM_REAL        seuil_mr)
{
        int     i,j,k ;

        k = 0 ;

        for(i=0 ; i<3 ; i++)
        {
          for(j=0 ; j<3 ; j++)
          {
            if(fabs((double)a[i][j]) < seuil_mr) 
              {a[i][j] = 0. ; k++ ; }
            else if(fabs((double)(a[i][j] - 1.)) < seuil_mr)
              {a[i][j] = 1. ; k++ ; }
          }
        }

        if(k == 0)         return(ATTRACTION_NULLE);
        else if(k == 9)    return(ATTRACTION_COMPLETE);
        else               return(ATTRACTION_PARTIELLE);
}

/*-------------------------------------------------------------------*/
/*-------------------------------------------------------------------*/
/*-------------------------------------------------------------------*/
/* conversion de la matrice ligne m en trois angles XYZ mobile          */
/* ang1 et ang3 sont normalises entre + ou - PI, ang2 entre + ou - PI/2 */

void mr3d_conv_xyz_mob (const MR3D m, LIBGEOM_REAL *ang1,LIBGEOM_REAL *ang2,LIBGEOM_REAL *ang3)
{
   LIBGEOM_REAL  a1,a2,a3;
   double Epsilon = 0.001;

   /* This is based on the equations found in "Introduction
      to robotics", by J.Craig, p. 47

      alpha = a3
      beta  = a2
      gamma = a1

    */

   a2 = atan2 (-m[2][0], sqrt (m[0][0]*m[0][0] + m[1][0]*m[1][0]));

   if (fabs(a2 - M_PI_2) < Epsilon) {  
      a1 = atan2 (m[0][1], m[1][1]);
      a3 = 0.;
   } else
   if (fabs(a2 + M_PI_2) < Epsilon) { 
      a1 = -atan2 (m[0][1], m[1][1]);
      a3 = 0.;
   } else {
      a1 = atan2 (m[2][1], m[2][2]);
      a3 = atan2 (m[1][0], m[0][0]);
   }

   /* A sign inversion to be compatible with the inverse function... */

   *ang1 = -a1;
   *ang2 = -a2;
   *ang3 = -a3;
}

/*------------------------------------------------------------------*/
/* conversion de la matrice ligne m en trois angles YXZ mobile      */
/* ang1 et ang3 sont normalises entre + ou - PI, ang2 entre +- PI/2 */
/* correspond a la sequence pour l'analyse des mvts en orthopedie   */

void  mr3d_conv_yxz_mob (const MR3D m, LIBGEOM_REAL *ang1, LIBGEOM_REAL *ang2, LIBGEOM_REAL *ang3)
{
  LIBGEOM_REAL  a1,a2,a3;
  double Epsilon = 0.001;

  /* Derived like the _xyz version (see Craig). */

  a2 = atan2 (m [2][1], sqrt (m [1][1] * m [1][1] + m [0][1] * m [0][1]));

  if (fabs (a2 - M_PI_2) < Epsilon)
  { a1 = atan2 (m [1][0], m [0][0]);
    a3 = 0.;
  }
  else
  if (fabs (a2 + M_PI_2) < Epsilon)
  { a1 = -atan2 (m [1][0], m [0][0]);
    a3 = 0.;
  }
  else
  { a1 = -atan2 (m [2][0], m [2][2]);
    a3 = -atan2 (m [0][1], m [1][1]);
  }

  /* A sign conversion to be compatible with the inverse function... */

  *ang1 = -a1;
  *ang2 = -a2;
  *ang3 = -a3;
}

/*@$-------------------------------------------------------------------
    Public  FUNCTION: 
    FUNCTIONALITY   : convert the matrix (line-matrix) in 3 angles ZXY.
    ang1 and ang3 normalised between +- PI, ang2 between +- PI/2.
    Correspond a la sequence pour l'analyse des mvts en homophobie 
    (Walter'93).

    Zinv(ang1) * Z_ON_Y * Zinv(ang2) * Z_ON_Y * Zinv(ang3)

    Remember that Z_ON_Y = inverse(Z_ON_X) !!!!!!

    KEYWORD(S)      : Sequence d'Euler-Boulic, dites de l'Euler-tronquee
    mieux connue sous le nom d'Euler-ouimaisnon.
    -------------------------------------------------------------------
    FUNCTION TYPE   : bool
    here parameter description with : flow / name / functionality  
    the flow can be one of  IN / OUT / IO (for IN and OUT)
    ---------------------------------------------------------------$@*/
void mr3d_conv_EulerBoulic_zxy(MR3D_t m, LIBGEOM_REAL *ang1, LIBGEOM_REAL *ang2, LIBGEOM_REAL *ang3,
                               bool verbose)
{
    double a1,a2,a3, s2;
    double Epsilon = 0.001;

    s2 = (double)  m[2][2] ;  
    a2 = asin(s2);

      if (fabs(a2 - M_PI_2) < Epsilon)  /* singularite  a2 == pi/2  */
      {
          if(verbose) 
            fprintf(stderr,"Warning: Euler-Boulic Singularity (PI/2) in ZXY\n");
          a1 = atan2((double) (m[1][1]), (double)(m[0][1]));
          a2 = M_PI_2;
          a3 = 0.0;
      }
      else if (fabs(a2 + M_PI_2) < Epsilon) /* singularite a2==-pi/2 */
      { 
          if(verbose) 
            fprintf(stderr,"Warning: Euler-Boulic Singularity (-PI/2) in ZXY\n");
          a1 = atan2((double) (m[1][1]), (double)(m[0][1]));
          a2 = -M_PI_2;
          a3 = 0.0;
      }
      else
      {
        a1 = atan2((double)(-m[0][2]), (double)(m[1][2]));        
        a3 = atan2((double)(-m[2][1]), (double)(m[2][0]));
      }

   *ang1 = (LIBGEOM_REAL)a1;
   *ang2 = (LIBGEOM_REAL)a2;
   *ang3 = (LIBGEOM_REAL)a3;
}

/*@$-------------------------------------------------------------------
    Public  FUNCTION: 
    FUNCTIONALITY   : convert the matrix (line-matrix) in 3 angles ZYX.
    ang1 and ang3 normalised between +- PI, ang2 between +- PI/2.
    Correspond a la sequence pour l'analyse des mvts en homophobie 
    (Walter'93).

    Zinv(ang1) * Z_ON_X * Zinv(ang2) * Z_ON_X * Zinv(ang3)

    Remember that Z_ON_X = inverse(Z_ON_Y) !!!!!!

    KEYWORD(S)      : Sequence d'Euler-Boulic, dites de l'Euler-tronquee
    mieux connue sous le nom d'Euler-ouimaisnon.
    -------------------------------------------------------------------
    FUNCTION TYPE   : bool
    here parameter description with : flow / name / functionality  
    the flow can be one of  IN / OUT / IO (for IN and OUT)
    ---------------------------------------------------------------$@*/
void  mr3d_conv_EulerBoulic_zyx(MR3D_t m, LIBGEOM_REAL *ang1, LIBGEOM_REAL *ang2, 
                                LIBGEOM_REAL *ang3, bool verbose)
{
    double a1,a2,a3, s2;
    double Epsilon = 0.001;

    s2 = (double) -m[2][2] ;  
    a2 = asin(s2);

    if (fabs(a2 - M_PI_2) < Epsilon)  /* singularite  a2 == pi/2  */
    {
        if(verbose) 
            fprintf(stderr,"Warning: Euler-Boulic Singularity (PI/2) in ZYX\n");
        a1 = atan2((double)(-m[0][0]), (double)(m[1][0]));
        a2 = M_PI_2;
        a3 = 0.0;
    }
    else if (fabs(a2 + M_PI_2) < Epsilon) /* singularite a2==-pi/2 */
    { 
        if(verbose) 
            fprintf(stderr,"Warning: Euler-Boulic Singularity (-PI/2) in ZYX\n");
        a1 = atan2((double)(-m[0][0]), (double)(m[1][0]));
        a2 = -M_PI_2;
        a3 = 0.0;
    }
    else
    {
        a1 = atan2((double)(m[1][2]), (double)(m[0][2]));
        a3 = atan2((double)(m[2][0]), (double)(m[2][1]));
    }

    *ang1 = (LIBGEOM_REAL)a1;
    *ang2 = (LIBGEOM_REAL)a2;
    *ang3 = (LIBGEOM_REAL)a3;
}

/*@$-------------------------------------------------------------------
    Public  FUNCTION: 
    FUNCTIONALITY   : convert the matrix (line-matrix) in 2 angles ZX.
    ang1 and ang2 normalised between +- PI.
    Correspond a la sequence pour l'analyse des mvts en homophobie 
    (Walter'93).
    KEYWORD(S)      : Sequence d'Euler-Boulic-2D, dites de 
    l'Euler-doublement-tronquee mieux connue sous le nom 
    d'Euler-ouimaisnonmaisnon.
    -------------------------------------------------------------------
    FUNCTION TYPE   : bool
    here parameter description with : flow / name / functionality  
    the flow can be one of  IN / OUT / IO (for IN and OUT)
    ---------------------------------------------------------------$@*/
void  mr3d_conv_EulerBoulic_zx(MR3D_t m, LIBGEOM_REAL *ang1, LIBGEOM_REAL *ang2, 
                       bool verbose)
{
   *ang1 = (LIBGEOM_REAL) atan2((double)( m[1][2]), (double)(m[0][2]));
   *ang2 = (LIBGEOM_REAL) atan2((double)( m[2][0]), (double)(m[2][1]));
}

/*@$-------------------------------------------------------------------
    Public  FUNCTION: 
    FUNCTIONALITY   : convert the matrix (line-matrix) in 2 angles ZY.
    ang1 and ang2 normalised between +- PI.
    Correspond a la sequence pour l'analyse des mvts en homophobie 
    (Walter'93).
    KEYWORD(S)      : Sequence d'Euler-Boulic-2D, dites de 
    l'Euler-doublement-tronquee mieux connue sous le nom 
    d'Euler-ouimaisnonmaisnon.
    -------------------------------------------------------------------
    FUNCTION TYPE   : bool
    here parameter description with : flow / name / functionality  
    the flow can be one of  IN / OUT / IO (for IN and OUT)
    ---------------------------------------------------------------$@*/
void  mr3d_conv_EulerBoulic_zy(MR3D_t m, LIBGEOM_REAL *ang1, LIBGEOM_REAL *ang2,
                              bool verbose)
{
   *ang1 = (LIBGEOM_REAL) atan2((double)(-m[0][2]), (double)(m[1][2]));
   *ang2 = (LIBGEOM_REAL) atan2((double)(-m[2][1]), (double)(m[2][0]));
}

/*@$-------------------------------------------------------------------
    Public  FUNCTION: 
    FUNCTIONALITY   : convert the matrix (line-matrix) in 1 angles Z.
    ang1 normalised between +- PI.
    Correspond a la sequence pour l'analyse des mvts en homophobie 
    (Walter'93).
    KEYWORD(S)      : Sequence d'Euler-Boulic-1D, dites de 
    l'Euler-triplement-tronquee mieux connue sous le nom 
    d'Euler-ouimaisnonmaisnonmaisnon.
    -------------------------------------------------------------------
    FUNCTION TYPE   : bool
    here parameter description with : flow / name / functionality  
    the flow can be one of  IN / OUT / IO (for IN and OUT)
    ---------------------------------------------------------------$@*/
void  mr3d_conv_EulerBoulic_z(MR3D_t m, LIBGEOM_REAL *ang1, bool verbose)
{
   *ang1 = (LIBGEOM_REAL) atan2((double)( m[1][0]), (double)(m[0][0]));
}

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

void mr3d_from_angle_seq_xyz(MR3D mx,LIBGEOM_REAL angX, LIBGEOM_REAL angY, LIBGEOM_REAL angZ)
     /* From a rotation angle sequence to a rotation matrix.
        Taken from Graphics Gems IV p. 222, but adpated to 
        produce a row(!) matrix. It replaces the GL code:
          irisGL_loadmatrix(idt);
          irisGL_rot(angleX, 'x');
          irisGL_rot(angleY, 'y');
          irisGL_rot(angleZ, 'z');
          irisGL_getmatrix(newMx); 
        angle order: XYZr = [Z,Odd,Diff,R-frame] */
{
    LIBGEOM_REAL ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
   
    ci = cos(-angZ);
    cj = cos(-angY); 
    ch = cos(-angX);
    si = sin(-angZ);
    sj = sin(-angY); 
    sh = sin(-angX);
    cc = ci*ch; 
    cs = ci*sh; 
    sc = si*ch; 
    ss = si*sh;
    
    mx[0][0] = cj*ci;    mx[1][0] = cj*si;    mx[2][0] = -sj;      
    mx[0][1] = sj*cs-sc; mx[1][1] = sj*ss+cc; mx[2][1] = cj*sh; 
    mx[0][2] = sj*cc+ss; mx[1][2] = sj*sc-cs; mx[2][2] = cj*ch;
}

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

void mr3d_from_angle_seq_yxz(MR3D mx, LIBGEOM_REAL angY, LIBGEOM_REAL angX, LIBGEOM_REAL angZ)
     /* From a rotation angle sequence to a rotation matrix.
        Taken from Graphics Gems IV p. 222, but adpated to 
        produce a row(!) matrix. It replaces the GL code:
          irisGL_loadmatrix(idt);
          irisGL_rot(angleX, 'y');
          irisGL_rot(angleY, 'x');
          irisGL_rot(angleZ, 'z');
          irisGL_getmatrix(newMx); 
        angle order: YXZr = [Z,Even,Diff,R-frame] */
{
  LIBGEOM_REAL ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
  
  ci = cos(angZ);
  cj = cos(angX); 
  ch = cos(angY);
  si = sin(angZ);
  sj = sin(angX); 
  sh = sin(angY);
  cc = ci*ch;   
  cs = ci*sh;   
  sc = si*ch;  
  ss = si*sh;
     
  mx[0][0] = sj*ss+cc; mx[1][0] = sj*cs-sc; mx[2][0] = cj*sh; 
  mx[0][1] = cj*si;    mx[1][1] = cj*ci;    mx[2][1] = -sj;   
  mx[0][2] = sj*sc-cs; mx[1][2] = sj*cc+ss; mx[2][2] = cj*ch; 
}


void mr3d_from_two_vectors(const VT3D_t a, const VT3D_t b, MR3D R)
{
   static VT3D v0, v1, absv0, leastCoordAxis, axis;
   LIBGEOM_REAL cosTheta, sinTheta, x, y, z, t;

   vt3d_normalize(a, v0);
   vt3d_normalize(b, v1);

   cosTheta = vt3d_get_dot(v0, v1);
   vt3d_cross(v0, v1, axis);
   sinTheta = vt3d_get_norm(axis);
    
   if (fabs(sinTheta) > EPSIL_BIG_ZERO)
   {
      /* normalize axis */
      axis[0] /= sinTheta;
      axis[1] /= sinTheta;
      axis[2] /= sinTheta;
   }
   else if (cosTheta < 0.0)  /* singular case, vectors are colinear but opposite */
   {
      absv0[0] = fabs(v0[0]); absv0[1] = fabs(v0[1]); absv0[2] = fabs(v0[2]);
      leastCoordAxis[0] = leastCoordAxis[1] = leastCoordAxis[2] = 0;
      leastCoordAxis[absv0[0]<=absv0[1] ? absv0[0]<=absv0[2] ? 0 : 2
                    : absv0[1]<=absv0[2] ? 1 : 2] = 1;
      vt3d_cross(v0, leastCoordAxis, axis);
      vt3d_normalize(axis, axis);
   }
   else
   {
      mr3d_idt_(R);
      return;
   }
    
   x = axis[0];
   y = axis[1];
   z = axis[2];
    
   t = 1.0f - cosTheta;
    
   R[0][0] = t * x * x + cosTheta;
   R[0][1] = t * x * y + sinTheta * z;
   R[0][2] = t * x * z - sinTheta * y;
   R[1][0] = t * y * x - sinTheta * z;
   R[1][1] = t * y * y + cosTheta;
   R[1][2] = t * y * z + sinTheta * x;
   R[2][0] = t * z * x + sinTheta * y;
   R[2][1] = t * z * y - sinTheta * x;
   R[2][2] = t * z * z + cosTheta;
} 

/*@#-------------------------------------------------------------------
    Public FUNCTION : 
    FUNCTIONALITY   : re-orthogonalization of matrix
    KEYWORD(S)      : 
    -------------------------------------------------------------------
    FUNCTION TYPE   : void
    here parameter description with : flow / name / functionality  
    the flow can be one of  IN / OUT / IO (for IN and OUT)
    ---------------------------------------------------------------#@*/
/* 
   MatriX Orthogonalization
   Eric Raible
   from "Graphics Gems", Academic Press, 1990

   Modified 09/11/93 by Tom Molet for MR3D compatibility,
   and ANSI-C conversion.
*/

/* find maximum of a and b */
#define MAX(a,b)        (((a)>(b))?(a):(b))     

/*
 * Reorthogonalize matrix R - that is find an orthogonal matrix that is
 * "close" to R by computing an approximation to the orthogonal matrix
 *
 *           T  -1/2
 *   RC = R(R R)
 *                              T      -1
 * [RC is orthogonal because (RC) = (RC) ]
 *                                                                -1/2
 * To compute C, we evaluate the Taylor expansion of F(x) = (I + x)
 * (where x = C - I) about x=0.
 * This gives C = I - (1/2)x + (3/8)x^2 - (5/16)x^3 + ...
 */

static LIBGEOM_REAL coef[10] =                  /* From mathematica */
  { 1., -1./2., 3./8., -5./16., 35./128., -63./256.,
    231./1024., -429./2048., 6435./32768., -12155./65536. };

void    mr3d_reorthogonalize (MR3D R, int limit)
{
  MR3D I, Temp, X, X_power, Sum;
  int power;

  limit = MAX(limit, 10);

  mr3d_transpose (R, Temp);             /* Rt */
  mr3d_mult (Temp, R, Temp);    /* RtR */
  mr3d_idt_ (I);
  mr3d_sub (Temp, I, X);        /* RtR - I */
  mr3d_idt_ (X_power);          /* X^0 */
  mr3d_idt_ (Sum);                      /* coef[0] * X^0 */

  for (power = 1; power < limit; ++power)
    {
      mr3d_mult (X_power, X, X_power);
      mr3d_scale (X_power, coef[power], Temp);
      mr3d_add (Sum, Temp, Sum);
    }

  mr3d_mult (R, Sum, R);
}


/*@#-------------------------------------------------------------------
    Public FUNCTION : 
    FUNCTIONALITY   : write the rotation-matrix self in the file f.
    KEYWORD(S)      : 
    -------------------------------------------------------------------
    FUNCTION TYPE   : bool
    here parameter description with : flow / name / functionality  
    the flow can be one of  IN / OUT / IO (for IN and OUT)
    ---------------------------------------------------------------#@*/
bool    mr3d_write(const MR3D self, FILE * f)
{
  char *format_str;

  if (sizeof(LIBGEOM_REAL) == sizeof(float))
    format_str = "%f %f %f\n";
  else 
    format_str = "%lf %lf %lf\n";

    if(f)
    {
        fprintf(f, format_str,self[GEOM_X_INDEX][GEOM_X_INDEX], 
                                self[GEOM_X_INDEX][GEOM_Y_INDEX], 
                                self[GEOM_X_INDEX][GEOM_Z_INDEX]);
        
        fprintf(f, format_str,self[GEOM_Y_INDEX][GEOM_X_INDEX], 
                                self[GEOM_Y_INDEX][GEOM_Y_INDEX], 
                                self[GEOM_Y_INDEX][GEOM_Z_INDEX]);
        
        fprintf(f, format_str,self[GEOM_Z_INDEX][GEOM_X_INDEX], 
                                self[GEOM_Z_INDEX][GEOM_Y_INDEX], 
                                self[GEOM_Z_INDEX][GEOM_Z_INDEX]);
        
        return(true);
    }
    return(false);
}


/*@#-------------------------------------------------------------------
    Public FUNCTION : 
    FUNCTIONALITY   : read the rotation-matrix self from the file f.
    KEYWORD(S)      : 
    -------------------------------------------------------------------
    FUNCTION TYPE   : bool
    here parameter description with : flow / name / functionality  
    the flow can be one of  IN / OUT / IO (for IN and OUT)
    ---------------------------------------------------------------#@*/
bool    mr3d_read(MR3D self, FILE * f)
{
  char *format_str;

  if (sizeof(LIBGEOM_REAL) == sizeof(float))
    format_str = "%f %f %f";
  else 
    format_str = "%lf %lf %lf";

    if(f)
    {
        fscanf(f, format_str,&self[GEOM_X_INDEX][GEOM_X_INDEX], 
                                &self[GEOM_X_INDEX][GEOM_Y_INDEX], 
                                &self[GEOM_X_INDEX][GEOM_Z_INDEX]);
        
         
        fscanf(f, format_str,&self[GEOM_Y_INDEX][GEOM_X_INDEX], 
                                &self[GEOM_Y_INDEX][GEOM_Y_INDEX], 
                                &self[GEOM_Y_INDEX][GEOM_Z_INDEX]);
        
         
        fscanf(f, format_str,&self[GEOM_Z_INDEX][GEOM_X_INDEX], 
                                &self[GEOM_Z_INDEX][GEOM_Y_INDEX], 
                                &self[GEOM_Z_INDEX][GEOM_Z_INDEX]);
        
         
        return(true);
    }
    return(false);
}



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

void    vt3d_get(const MatriX       matrix,
                 VT3D       v)
{
        v[0] = matrix[3][0] ;
        v[1] = matrix[3][1] ;
        v[2] = matrix[3][2] ;
}

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

void    vt3d_load(MatriX    matrix,
                  const VT3D        v)
{
        matrix[3][0] = v[0] ;
        matrix[3][1] = v[1] ;
        matrix[3][2] = v[2] ;
}

/*-------------------------------------------------------------------*/
/* Transcription d'un vecteur 3d sous forme d'un vecteur 4d pour de     */
/* futures manipulations par des transformations homogenes (sans zoom)  */
 
void    vt3d_vhom(const VT3D        vt3d, 
                  VHOM      vhom)
{
        vhom[0] = vt3d[0];
        vhom[1] = vt3d[1];
        vhom[2] = vt3d[2];
        vhom[3] = 1. ;
}

/*-------------------------------------------------------------------*/
/* Transcription d'un vecteur 4d homogene en vecteur 3d (hyp : zoom=1)  */

void    vhom_vt3d(const VHOM        vhom, 
                  VT3D      vt3d)
{
        vt3d[0] = vhom[0];
        vt3d[1] = vhom[1];
        vt3d[2] = vhom[2];
}

/*-------------------------------------------------------------------*/
/* LET: affectation d'un vecteur.       (assignment)                    */

void    vt3d_let(LIBGEOM_REAL x, LIBGEOM_REAL y, LIBGEOM_REAL z, VT3D w)
{
   w[0] = x;
   w[1] = y;
   w[2] = z;
}

/*@#-------------------------------------------------------------------
    Public FUNCTION : 
    FUNCTIONALITY   : 
    KEYWORD(S)      : 
    -------------------------------------------------------------------
    FUNCTION TYPE   : void
    here parameter description with : flow / name / functionality  
    the flow can be one of  IN / OUT / IO (for IN and OUT)
    ---------------------------------------------------------------#@*/
void    vt3d_copy(const VT3D src, VT3D dst)
{
    dst[0] = src[0];
    dst[1] = src[1];
    dst[2] = src[2];
}

/*@#-------------------------------------------------------------------
    Public FUNCTION : 
    FUNCTIONALITY   : 
    KEYWORD(S)      : 
    -------------------------------------------------------------------
    FUNCTION TYPE   : void
    here parameter description with : flow / name / functionality  
    the flow can be one of  IN / OUT / IO (for IN and OUT)
    ---------------------------------------------------------------#@*/
void    vt3d_set_null(VT3D dst)
{
    dst[0] = 0.0;
    dst[1] = 0.0;
    dst[2] = 0.0;
}

/*-------------------------------------------------------------------*/
/* ADD: u + v.                                                          */

void    vt3d_add(const VT3D u, const VT3D v,VT3D w)
{
   w[0] = u[0] + v[0];
   w[1] = u[1] + v[1];
   w[2] = u[2] + v[2];
}

/*-------------------------------------------------------------------*/
/* SOUST: u - v.                                                        */

void    vt3d_sub(const VT3D u, const VT3D v,VT3D w)
{
   w[0] = u[0] - v[0];
   w[1] = u[1] - v[1];
   w[2] = u[2] - v[2];
}

/*-------------------------------------------------------------------*/
/* MULT: retourne le resultat de la multiplication de u avec v.         */
/* same as scale */

void    vt3d_mult(const VT3D u, const VT3D v,VT3D w)
{
   w[0] = u[0] * v[0];
   w[1] = u[1] * v[1];
   w[2] = u[2] * v[2];
}

/*-------------------------------------------------------------------*/
/* DIV: retourne le resultat de la division de u avec v.                */

void    vt3d_div(const VT3D u, const VT3D v,VT3D w)
{
   w[0] = u[0] / v[0];
   w[1] = u[1] / v[1];
   w[2] = u[2] / v[2];
}

/*-------------------------------------------------------------------*/
/* MULTV: multiplication d'un vecteur avec un scalaire.                 */

void    vt3d_mults(const VT3D v,LIBGEOM_REAL s, VT3D w)
{
   w[0] = v[0] * s;
   w[1] = v[1] * s;
   w[2] = v[2] * s;
}

/*-------------------------------------------------------------------*/
/* DIVS: division d'un vecteur avec un scalaire.                        */

void    vt3d_divs(const VT3D v,LIBGEOM_REAL s, VT3D w)
{
   if (s != 0.0) {
      w[0] = v[0] / s;
      w[1] = v[1] / s;
      w[2] = v[2] / s;
   }
}

/*-------------------------------------------------------------------*/
/* negate a vt3d vector (change sign).                               */

void vt3d_neg (const VT3D src, VT3D dest)

{ dest [0] = - src [0];
  dest [1] = - src [1];
  dest [2] = - src [2];
}

/*-------------------------------------------------------------------*/
/* NORME: retourne la norme du vecteur v. (norm)                        */

LIBGEOM_REAL    vt3d_get_norm(const VT3D v)
{
   return( (LIBGEOM_REAL)sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]) );
}

/*-------------------------------------------------------------------*/
/* NORME2: retourne la norme au carre du vecteur v.                     */

LIBGEOM_REAL    vt3d_get_norm2(const VT3D v)
{
   return( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] );
}

/*-------------------------------------------------------------------*/
/* DIST: distance entre deux points.    (norm of u-v)                   */

LIBGEOM_REAL    vt3d_get_dist(const VT3D u, const VT3D v)
{
    VT3D    w;

    vt3d_sub(u,v,w);
    return(vt3d_get_norm(w));
}

/*-------------------------------------------------------------------*/
/* DISTPTDROITE: distance entre un point P et la droite definie par un  */
/*                 point Q et une direction unitaire N.                 */
/*              (shortest dist between point and a line)                */

LIBGEOM_REAL    vt3d_get_distptline(const VT3D p, const VT3D q, const VT3D n)
{
    VT3D    w ;

    vt3d_sub(p, q, w);
    vt3d_cross(n,w, w);
    return(vt3d_get_norm(w));
}

/*-------------------------------------------------------------------*/
/* DISTPTPLAN: distance entre un point P et le plan defini par un point */
/*               Q et une normale unitaire N.                           */
/*              (shortest dist between point and a plane)               */

LIBGEOM_REAL    vt3d_get_distptplane(const VT3D p, const VT3D q, const VT3D n)
{
    VT3D    w ;

    vt3d_sub(p, q, w);
    return((LIBGEOM_REAL)fabs((double)vt3d_get_dot(n,w)));
}

/*-------------------------------------------------------------------*/
/* ROTV: rotation du vecteur V autour de l'axe K (x,y ou z) de A radian.*/
/*         SA et CA sont les sinus et cosinus de A.                     */
/*      (rotate v around x or y or z by angle  A rd)                    */

void    vt3d_rotv(const VT3D v,LIBGEOM_REAL sa, LIBGEOM_REAL ca, char k, VT3D w)
{
   switch(k) {
      case 'x':
         vt3d_let(v[0], v[1]*ca-v[2]*sa, v[1]*sa+v[2]*ca, w);
         break;
      case 'y':
         vt3d_let(v[0]*ca+v[2]*sa, v[1], -v[0]*sa+v[2]*ca, w);
         break;
      case 'z':
         vt3d_let(v[0]*ca-v[1]*sa, v[0]*sa+v[1]*ca, v[2], w);
         break;
   }
}

/*-------------------------------------------------------------------*/
/* ROTVECT: rotation du vecteur V autour de l'axe de centre C et de     */
/*            direction K (unitaire) d'un angle A (radian).             */
/*            SA et CA sont les sinus et cosinus de A.                  */
/*      (rotate v around k passing by point c by angle  A rd)           */

void    vt3d_rotvect(const VT3D v, const VT3D c, VT3D k, LIBGEOM_REAL sa, LIBGEOM_REAL ca, VT3D w)
{
   LIBGEOM_REAL va,kxy,kxz,kyz;
   VT3D r1,r2,r3;

   va = 1.0 - ca;
   vt3d_sub(v,c,w);

   kxy = k[0] * k[1];
   kxz = k[0] * k[2];
   kyz = k[1] * k[2];

   vt3d_let( k[0]*k[0]*va+ca, kxy*va-k[2]*sa, kxz*va+k[1]*sa, r1 );
   vt3d_let( kxy*va+k[2]*sa, k[1]*k[1]*va+ca, kyz*va-k[0]*sa, r2 );
   vt3d_let( kxz*va-k[1]*sa, kyz*va+k[0]*sa, k[2]*k[2]*va+ca, r3 );

   vt3d_let(vt3d_get_dot(r1,w), vt3d_get_dot(r2,w), vt3d_get_dot(r3,w), w);

   vt3d_add(w,c,w);
}

/*-------------------------------------------------------------------*/
/* construit dans v la normalisation du vecteur u                       */

bool    vt3d_normalize(const VT3D       u,
                       VT3D     v)
{
        double  norm ;
        int     i ;

        norm = sqrt((double)(u[0]*u[0] + u[1]*u[1] + u[2]*u[2])) ;

        if(norm > EPSIL_ZERO)
          {for(i=0 ; i<3 ; i++) v[i] = u[i]/norm ; return(true) ;}
        else
          {for(i=0 ; i<3 ; i++) v[i] = 0. ; return(false) ;}
}

/*-------------------------------------------------------------------*/
/* calcule  le produit vectoriel u * v -> w (cross product)             */

void    vt3d_cross(const VT3D   u,
                   const VT3D   v,
                   VT3D w)
{
        VT3D    x;

        x[0] =   u[1]*v[2] - u[2]*v[1] ;
        x[1] = - u[0]*v[2] + u[2]*v[0] ;
        x[2] =   u[0]*v[1] - u[1]*v[0] ;

        w[0] = x[0] ; w[1] = x[1] ; w[2] = x[2] ;
}

/*-------------------------------------------------------------------*/
/* applique un facteur d'echelle independant pour chaque coordonnee     */

void    vt3d_scale(const VT3D       scale,
                   const VT3D       u,
                   VT3D     v)
{
        v[0] =   u[0]*scale[0] ;
        v[1] =   u[1]*scale[1] ;
        v[2] =   u[2]*scale[2] ;
}

/*-------------------------------------------------------------------*/
/* renvoie le produit scalaire u*v      (dot product)                   */

LIBGEOM_REAL    vt3d_get_dot(const VT3D     u,
                     const VT3D     v)
{
        return(u[0]*v[0] + u[1]*v[1] + u[2]*v[2]);
}


/*-------------------------------------------------------------------*/
bool vt3d_egalite(const VT3D    a,
                  const VT3D    b, 
                  LIBGEOM_REAL  seuil_vt)
{
        LIBGEOM_REAL    delta;
        int     i,egalite;

        for(egalite=1,i=0 ; i<3 ; i++)
        {
          delta = fabs(a[i]-b[i]) ;
          if(delta > seuil_vt)
          {
            if(DEBUG_GEOM)
            {
              printf(" warning : 3D vector difference of %f \n", delta);
              printf("           for element [%d]\n", i);
            }
            egalite = 0;
          }
        }
        return(egalite);
}

/*-------------------------------------------------------------------*/
/* cette procedure attire un vecteur vers le vecteur nul. si un     */
/* element est inferieur (en val abs) a un seuil il prend la valeur0*/

int     vt3d_attraction_nul(VT3D        a, 
                            LIBGEOM_REAL        seuil_vt)
{
        int     i,k;

        k = 0 ;

        for(i=0 ; i<3 ; i++)
          if(fabs((double)a[i]) < seuil_vt)
            {a[i] = 0. ; k++ ;}

        if(k == 0)         return(ATTRACTION_NULLE);
        else if(k == 3)    return(ATTRACTION_COMPLETE);
        else               return(ATTRACTION_PARTIELLE);
}

/*@#-------------------------------------------------------------------
    Public FUNCTION : 
    FUNCTIONALITY   : Tells if a point is inside a 2D polygon or not.
                      The polygon may be concave or convex, and is
                      made of N vertices pointed by p. Only the first
                      two coordinates are considered (x and y).
    KEYWORD(S)      : 
    -------------------------------------------------------------------
    FUNCTION TYPE   : bool
    here parameter description with : flow / name / functionality  
    the flow can be one of  IN / OUT / IO (for IN and OUT)
    ---------------------------------------------------------------#@*/

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

{
  int     i, j;
  bool c = false;
  LIBGEOM_REAL   xt = point[0], yt = point[1];

  for (i = 0, j = N-1; i < N; j = i++)
  {
    if ((p[i][1]<=yt && yt<p[j][1] && (p[j][1]-p[i][1]) * (xt-p[i][0])<(p[j][0]-p[i][0])*(yt-p[i][1]))
        ||  (p[j][1]<=yt && yt<p[i][1] && (p[j][1]-p[i][1]) * (xt-p[i][0])>(p[j][0]-p[i][0])*(yt-p[i][1])))
    {
      c = !c;
    }
  }

  return c;
}

/*@#-------------------------------------------------------------------
    Public FUNCTION : 
    FUNCTIONALITY   : Rotates a vector about a given axis angle
                      (using Rodriguez formula).
    KEYWORD(S)      : 
    -------------------------------------------------------------------
    FUNCTION TYPE   : 
    here parameter description with : flow / name / functionality  
    the flow can be one of  IN / OUT / IO (for IN and OUT)
    ---------------------------------------------------------------#@*/

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

{ LIBGEOM_REAL alpha = vt3d_get_norm (axis_angle);

  if (alpha > EPSILON)
  {
    LIBGEOM_REAL sin_alpha = sin (alpha),
          cos_alpha = cos (alpha);
    VT3D  v2, omega;

    vt3d_mults (axis_angle, 1./alpha, omega);

    vt3d_mults (v_in, cos_alpha, v_out);
    vt3d_cross (omega, v_in,     v2);
    vt3d_mults (v2, sin_alpha,   v2);
    vt3d_add   (v_out, v2, v_out);

    vt3d_mults (omega, vt3d_get_dot (omega, v_in) * (1. - cos_alpha), v2);
    vt3d_add   (v_out, v2, v_out);
  }
  else
  {
    vt3d_copy (v_in, v_out);
  }
}

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

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

{ 
  static VT3D tmp;

  vt3d_mults (p1, 1-u, tmp);
  vt3d_mults (p2, u, result);
  vt3d_add (tmp, result, result);
}

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

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

{ LIBGEOM_REAL dot = vt3d_get_dot (p1, p2);
  LIBGEOM_REAL omega = safe_acos (dot);
  VT3D tmp;

  if (omega < EPSIL_BIG_ZERO)
  {
    /* Linear interpolation */

    vt3d_mults (p1, 1.-u, tmp);
    vt3d_mults (p2, u, result);
  }
  else
  {
    LIBGEOM_REAL under = sin (omega),
             f1 = sin ((1.-u)*omega),
             f2 = sin (u*omega);

    vt3d_mults (p1, f1/under, tmp);
    vt3d_mults (p2, f2/under, result);
  }

  vt3d_add (tmp, result, result);
}

/*@#-------------------------------------------------------------------
    Public FUNCTION : 
    FUNCTIONALITY   : write the vector self in the file f.
    KEYWORD(S)      : 
    -------------------------------------------------------------------
    FUNCTION TYPE   : bool
    here parameter description with : flow / name / functionality  
    the flow can be one of  IN / OUT / IO (for IN and OUT)
    ---------------------------------------------------------------#@*/
bool    vt3d_write(const VT3D self, FILE * f)
{
  char *format_str;

  if (sizeof(LIBGEOM_REAL) == sizeof(float))
    format_str = "%f %f %f";
  else 
    format_str = "%lf %lf %lf";

    if(f)
    {
        fprintf(f, format_str, self[0], self[1], self[2]);
        
        return(true);
    }
    return(false);
}

/*@#-------------------------------------------------------------------
    Public FUNCTION : 
    FUNCTIONALITY   : read the vector self from the file f.
    KEYWORD(S)      : 
    -------------------------------------------------------------------
    FUNCTION TYPE   : bool
    here parameter description with : flow / name / functionality  
    the flow can be one of  IN / OUT / IO (for IN and OUT)
    ---------------------------------------------------------------#@*/
bool    vt3d_read(VT3D self, FILE * f)
{
  char *format_str;

  if (sizeof(LIBGEOM_REAL) == sizeof(float))
    format_str = "%f %f %f";
  else 
    format_str = "%lf %lf %lf";

    if(f)
    {
        fscanf(f, format_str, &self[0], &self[1], &self[2]);
        
        return(true);
    }
    return(false);
}


/*-------------------------------------------------------------------*/
/*-------------------------------------------------------------------*/
/* operations sur des vecteurs de dimension n Attention : l'utilisateur */
/* est entierement responsable de la coherence de la dimension envoyee  */
/* en parametre avec l'espace reserve par le pointeur.                  */
/*-------------------------------------------------------------------*/

double  *vtn_creation(int   n)
{
        double  *p;

        if(n>0)
        {
          p = (double*) malloc((unsigned) n*sizeof(double));
          if(!p) {printf("erreur allocation memoire \n"); exit(0);}

          vtn_nul(p,n);
          return(p);
        }
        else return(NULL);
}

/*-------------------------------------------------------------------*/
void    vtn_destr(double    *p)
{
        if(p) free(p);
}

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

void    vtn_nul(double      *p, 
                int         n)
{
        int     i;

        if(p) for(i=0 ; i<n ; i++) p[i] = 0. ;
}

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

double  vtn_norm(const double       *p, 
                 int        n)
{
        double  pnorm ;
        int     i;

        if(p)
        {
          for(i=0 , pnorm = 0. ; i<n ; i++) pnorm += p[i] * p[i] ;
          return(sqrt(pnorm)) ;
        }
        return(0.);
}

/*-------------------------------------------------------------------*/
/* norme au premier degre minimisant les erreurs d'arrondi              */

double  vtn_anorm(const double  *p, 
                  int           n)
{
        double  anorm ;
        int     i ;

        if(p)
        {
          for(i=0 , anorm = 0. ; i<n ; i++) anorm += fabs(p[i]) ;
          return(anorm) ;
        }
        return(0.);
}

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

void    vtn_copy(const double   *psrc,
                 double *pdst, 
                 int            n)
{
        int     i;

        if(psrc && pdst) for(i=0 ; i<n ; i++) pdst[i] = psrc[i] ;
}


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

void    vtn_add(const double        *p1,
                const double        *p2,
                double      *pdst, 
                int         n)
{
        int     i;

        if(p1 && p2 && pdst) for(i=0 ; i<n ; i++) pdst[i] = p1[i] + p2[i] ;
}

/*-------------------------------------------------------------------*/
/* calcule   pdst = p1 - p2                                             */

void    vtn_sub(const double        *p1,
                const double        *p2,
                double      *pdst, 
                int         n)
{
  int   i;
  
  if(p1 && p2 && pdst) 
    for(i=0 ; i<n ; i++) 
      pdst[i] = p1[i] - p2[i] ;
}

/*-------------------------------------------------------------------*/
/* calcule   pdst = psrc * alpha                                */

void vtn_mults (const double  *psrc, 
                double  *pdst, 
                int n, 
                double  alpha)
{
  int i;
  for (i=0 ; i<n ; i++) 
    pdst[i] = alpha * psrc[i];
}


/*-------------------------------------------------------------------*/
/* calcule   pscal = produit scalaire (p1.p2)                           */

double  vtn_dot(const double    *p1,
                const double    *p2,
                int         n)
{
  int   i;
  double  pscal;
  
  if(p1 && p2)
   {
     pscal = 0. ;
     for(i=0 ; i<n ; i++) pscal += p1[i] * p2[i] ;
     return(pscal);
   }
  return(0.);
}

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

double  vtn_inv(const double        *p,
                double      *p_inv, 
                int         n)
{
        double  pnorm2 ;
        int     i;

        if(p != NULL && p_inv != NULL)
        {
          for(i=0, pnorm2 = 0. ; i<n ; i++) pnorm2 += p[i] * p[i] ;
          if(pnorm2 > EPSIL_ZERO)
          { 
            for(i=0 ; i<n ; i++) p_inv[i] =  p[i] / pnorm2 ;
            return(pnorm2);
          }
          else
          {
            for(i=0 ; i<n ; i++) p_inv[i] = 0. ;
            printf("warning : inversion de vecteur nul  \n");
            return(0.);
          }
        }
        fprintf(stderr, "warning : vtn_inv() NULL pointer argument\n");
        return(0.);
}





/*-------------------------------------------------------------------*/
/* operations sur des matrices mxn  ; Attention : l'utilisateur est     */
/* entierement responsable de la consistance des dimensions envoyees    */
/* en parametre avec l'espace reserve par les pointeurs.                */

double  *matmxn_creation(int    lmax,
                         int    cmax)
{
        double  *p;

        if((lmax>0)&&(cmax>0))
        {
          p = (double*) malloc((unsigned) lmax*cmax*sizeof(double));
          if(!p) {printf("erreur allocation memoire \n"); exit(0);}

          matmxn_nul(p,lmax,cmax);
          return(p);
        }
        else return(NULL);
}

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

void    matmxn_destr(double     *p)
{
        if(p) free(p);
}

/*-------------------------------------------------------------------*/
void    matmxn_nul(double       *p, 
                   int          lmax,
                   int          cmax)
{
        int     i,
                size = lmax * cmax;

        if(p)
          for(i=0 ; i<size ; i++)
            {
              *p = 0.;
              p++;
            }
}

/*-------------------------------------------------------------------*/
void    matmxn_idt(double       *p, 
                   int          lmax,
                   int          cmax)
{
        int i, min;

        if(p)
        {
          matmxn_nul (p, lmax, cmax);

          min = cmax<lmax ? cmax : lmax;

          for (i=0 ; i<min ; i++)
            VIJ (p,i,i,cmax)= 1. ;
        }
}


/*-------------------------------------------------------------------*/
/*  copie le contenue de a vers b (pour i de 0 a lmax*cmax).            */

void    matmxn_copy(const double * a, double * b, int lmax, int cmax)
{
        int dmax, i ;

        dmax = lmax*cmax ;

        for(i=0 ; i<dmax ; i++, a++, b++) *b = *a ;
}

/*-------------------------------------------------------------------*/
/*  calcule :   a(imax x jmax) * b(jamx x kmax) -> c(imax x kmax)       */

void    matmxn_mult(const double        *a ,
                    const double        *b ,
                    double      *c , 
                    int         imax ,
                    int         jmax ,
                    int         kmax)
{ int i,j,k;

  if ((c != a) && (c != b))
    {
      for (i=0; i < imax; i++)
        for (k=0; k < kmax; k++)
          {
            double v = 0.;

            for (j=0; j < jmax; j++)
              v += VIJ (a,i,j,jmax) * VIJ (b,j,k,kmax);

            VIJ (c, i, k, kmax) = v;
          }
    }
  else
    {
        double  *ww;

        ww = matmxn_creation(imax,kmax);

        for(i=0 ; i<imax ; i++)
        {
          for(k=0 ; k<kmax ; k++)
          {
            double v = 0.;

            for(j=0 ; j<jmax ; j++)
              v += VIJ(a,i,j,jmax)*VIJ(b,j,k,kmax) ;

            VIJ(ww,i,k,kmax) = v;
          }
        }

        matmxn_copy (ww, c, imax, kmax);

        free(ww);
    }
}

/*-------------------------------------------------------------------*/
/* calcule :  a(lmax x cmax) * b(cmax) -> c(lmax)                       */

void    matmxn_mult_vtn(const double        *a ,
                        const double        *b ,
                        double      *c , 
                        int         lmax ,
                        int         cmax)
{
        double  *w;
        int     i,j;

        w = vtn_creation(lmax);

        for(i=0 ; i<lmax ; i++)
        {
          w[i] = 0. ;
          for(j=0 ; j<cmax ; j++)
              w[i] += VIJ(a,i,j,cmax)*b[j] ;
        }

        for(i=0 ; i<lmax ; i++)
          c[i] = w[i] ;

        free(w);
}

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

void matmxn_write (double *d, int width, int height, FILE *f)

{ int i, j;

  for (i=0; i < height; i++)
  {
    for (j=0; j < width; j++)
      fprintf (f, "%f ", VIJ (d, i, j, width));

    fprintf (f, "\n");
  }
}

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

void matmxn_transpose (const double *src,
                       double *dest,
                       int    src_width,
                       int    src_height)

{ int i, j;

  if (src != dest)
    for (i=0; i < src_height; i++)
      for (j=0; j < src_width; j++)
        VIJ (dest, j, i, src_height) = VIJ (src, i, j, src_width);
  else
  {
    double *tmp = matmxn_creation (src_width, src_height);

    for (i=0; i < src_height; i++)
      for (j=0; j < src_width; j++)
        VIJ (tmp, j, i, src_height) = VIJ (src, i, j, src_width);

    matmxn_copy (tmp, dest, src_height, src_width);

    matmxn_destr (tmp);
  }
}

/*-------------------------------------------------------------------*/
/* calcul de la pseudo inverse d'une matrice de lmax lignes sur cmax    */
/* colonnes. Normallement lmax < cmax (redondance classique).           */


bool    matmxn_pseudo_inverse(double    *m ,
                              int       lmax ,
                              int       cmax , 
                              double    *mp)
{
        double  *a, *b, *c, *v;     /* vecteur colonne  de longueur lmax*/
        double  *d;                 /* vecteur ligne    de longueur cmax*/

        int     i,j,k;
        double  coef , anorm_a , anorm_c , seuil ;

        /*-----------------------------------------------------------*/
        /* allocation des vecteurs en fonction des dimensions du systeme*/

        a = vtn_creation(lmax);
        b = vtn_creation(lmax);
        c = vtn_creation(lmax);
        v = vtn_creation(lmax);
        d = vtn_creation(cmax);

        /*-----------------------------------------------------------*/
        /* traitement de la premiere colonne de la matrice m            */

        for(i=0 ; i<lmax ; i++) a[i] = VIJ(m,i,0,cmax);
        anorm_a = vtn_anorm(a,lmax);

        if(anorm_a < EPSIL_ZERO) for(i=0 ; i<lmax ; i++) VIJ(mp,0,i,lmax) = 0. ; 
        else
        {
          vtn_inv(a,v,lmax);
          for(i=0 ; i<lmax ; i++) VIJ(mp,0,i,lmax)  = v[i] ; 
        }

        /*-----------------------------------------------------------*/
        /* boucle de traitement de la colonne d'indice k                */

        for(k=1 ; k<cmax ; k++)
        {
          for(i=0 ; i<lmax ; i++)                   a[i] = VIJ(m,i,k,cmax) ;
          for(i=0 ; i<k    ; i++)           
            for(d[i] = 0. , j=0 ; j<lmax ; j++)     d[i] += VIJ(mp,i,j,lmax)*a[j] ;

          for(i=0 ; i<lmax ; i++)
            for(v[i] = 0. , j=0 ; j<k ; j++)        v[i] += VIJ(m,i,j,cmax)*d[j] ;

          for(i=0 ; i<lmax ; i++)                   c[i] = a[i] - v[i] ;

          anorm_a = vtn_anorm(a,lmax);
          anorm_c = vtn_anorm(c,lmax);

          seuil = SEUIL_GREV1*anorm_a ;
          if(anorm_c < seuil)
          {
            coef = 0. ;
            for(i=0 ; i<lmax   ; i++)         
              for(b[i] = 0. , j=0 ; j<k ; j++)      b[i] += d[j]*VIJ(mp,j,i,lmax);

            for(i=0 ; i<k    ; i++)                 coef += d[i]*d[i] ;
            coef = coef + 1. ;
            for(i=0 ; i<lmax ; i++)                 b[i] /= coef ;     
          }
          else
            vtn_inv(c,b,lmax);
          
          for(i=0 ; i<k    ; i++)
            for(j=0 ; j<lmax ; j++)
              VIJ(mp,i,j,lmax) -= d[i]*b[j];

          for(i=0 ; i<lmax ; i++)
            VIJ(mp,k,i,lmax) = b[i] ;
        }

        /*-----------------------------------------------------------*/
        /* liberation des vecteurs alloues                              */

        free(a) ; free(b) ; free(c) ; free(v) ; free(d) ;

        return(true);
}

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

void matmxn_damped_least_square_inverse(double *j_inv, 
                                        double *j_mat, 
                                        int j_width, 
                                        int j_height, 
                                        double damping) 
{
  double *J, *v, *w;
  int i, j, k;
  int height, width;
  
  if (j_width >=j_height) 
   {
      height = j_width;
      width  = j_width;
   } 
  else 
   {
     height = j_height;
     width  = j_width;
   }
  J = matmxn_creation(height,width);
  w = vtn_creation(width);
  v = matmxn_creation(width,width);
  
  for (i=0;i<j_height;i++)
    for (j=0;j<j_width;j++)
     {
       VIJ(J,i,j,width) = VIJ(j_mat,i,j,j_width);
     }
  
  svd(J,height,width,w,v);      
  
  Jinv(w,width,damping);
  
  for (i=0;i<width;i++)
    for (j=0;j<width;j++)
      VIJ(v,i,j,width) *= w[j];

  /* Compute : j_inv = v * transpose (J) */

  for (i=0;i<j_width;i++) 
    for (j=0;j<j_height;j++) 
     {
       double tmp = 0.;
       double *vline = v + i * width,
              *Jline = J + j * width;

       for (k=0;k<j_width;k++) 
         tmp += *(vline + k) * *(Jline + k);

       /* equivalent a :

         tmp += VIJ(v,i,k,width) * VIJ(J,j,k,width);
       */

       VIJ (j_inv,i,j,j_height) = tmp;
     }
  
  matmxn_destr(J);
  matmxn_destr(v);
  vtn_destr(w);
}

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

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 ! */
{
#define GEOM_SIGN(a,b)       ((b) >= 0.0 ? fabs(a) : -fabs(a))
#define GEOM_MIN(a,b)        ((a) < (b) ? (a) : (b))

 /* anorm HAS to be float to avoid the svd to loop more than 30 times and then aborting... */
 int flag,i,its,j,jj,k,l,nm;
 double c,f,h,s,x,y,z;
 double g=0.0,scale=0.0;
 float anorm = 0.0;
 double *rv1;

  /* This test appears to be useless with 2nd edition of NR (c'est probabl.
     un reste de la 1ere edition de NR.

  if (m < n) 
    nrerror("matmxn_svdcmp: You must augment A with extra zero rows");

  */

                                /* allocate a double vector with subscript */
                                /* range v[nl..nh];  rv1=vector(1,n); */
  rv1=(double *)malloc((unsigned) (n)*sizeof(double))-1;
  if (!rv1) 
    nrerror("allocation failure in vector()");

  for (i=1;i<=n;i++) {
    l=i+1;
    rv1[i]=scale*g;
    g=s=scale=0.0;
    if (i <= m) {
      for (k=i;k<=m;k++) scale += fabs(a[k][i]);
      if (scale) {
        for (k=i;k<=m;k++) {
          a[k][i] /= scale;
          s += a[k][i]*a[k][i];
        }
        f=a[i][i];
        g = -GEOM_SIGN(sqrt(s),f);
        h=f*g-s;
        a[i][i]=f-g;
          for (j=l;j<=n;j++) {
            for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j];
            f=s/h;
            for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
          }
        for (k=i;k<=m;k++) a[k][i] *= scale;
      }
    }
    w[i]=scale*g;
    g=s=scale=0.0;
    if (i <= m && i != n) {
      for (k=l;k<=n;k++) scale += fabs(a[i][k]);
      if (scale) {
        for (k=l;k<=n;k++) {
          a[i][k] /= scale;
          s += a[i][k]*a[i][k];
        }
        f=a[i][l];
        g = -GEOM_SIGN(sqrt(s),f);
        h=f*g-s;
        a[i][l]=f-g;
        for (k=l;k<=n;k++) rv1[k]=a[i][k]/h;
          for (j=l;j<=m;j++) {
            for (s=0.0,k=l;k<=n;k++) s += a[j][k]*a[i][k];
            for (k=l;k<=n;k++) a[j][k] += s*rv1[k];
          }
        for (k=l;k<=n;k++) a[i][k] *= scale;
      }
    }
    anorm=MAX(anorm,(fabs(w[i])+fabs(rv1[i])));
  }
  for (i=n;i>=1;i--) {
    if (i < n) {
      if (g) {
        for (j=l;j<=n;j++)
          v[j][i]=(a[i][j]/a[i][l])/g;
        for (j=l;j<=n;j++) {
          for (s=0.0,k=l;k<=n;k++) s += a[i][k]*v[k][j];
          for (k=l;k<=n;k++) v[k][j] += s*v[k][i];
        }
      }
      for (j=l;j<=n;j++) v[i][j]=v[j][i]=0.0;
    }
    v[i][i]=1.0;
    g=rv1[i];
    l=i;
  }
  for (i=GEOM_MIN(m,n);i>=1;i--) {              /* Accumulation of left-hand transformations */
    l=i+1;
    g=w[i];
    for (j=l;j<=n;j++) a[i][j]=0.0;
    if (g) {
      g=1.0/g;
        for (j=l;j<=n;j++) {
          for (s=0.0,k=l;k<=m;k++) s += a[k][i]*a[k][j];
          f=(s/a[i][i])*g;
          for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
      }
      for (j=i;j<=m;j++) a[j][i] *= g;
    } else {
      for (j=i;j<=m;j++) a[j][i]=0.0;
    }
    ++a[i][i];
  }
  for (k=n;k>=1;k--) {          /* diagonalization... */
    for (its=1;its<=30;its++) {
      flag=1;
      for (l=k;l>=1;l--) {
        nm=l-1;
    /* Keep the casts into float as they are! ==>> see comment above */
   if ((float)(fabs((float)rv1[l])+anorm) == anorm) {
     flag=0;
     break;
   }
   if ((float)(fabs((float)w[nm])+anorm) == anorm) break;
     }
     if (flag) {
   c=0.0;
   s=1.0;
   for (i=l;i<=k;i++) {
     f=s*rv1[i];
     rv1[i]=c*rv1[i];
     if ((float)(fabs((float)f)+anorm) == anorm) break; 
          g=w[i];
          h=pythag(f,g);
          w[i]=h;
          h=1.0/h;
          c=g*h;
          s= -f*h;
          for (j=1;j<=m;j++) {
            y=a[j][nm];
            z=a[j][i];
            a[j][nm]=y*c+z*s;
            a[j][i]=z*c-y*s;
          }
        }
      }
      z=w[k];
      if (l == k) {
        if (z < 0.0) {
          w[k] = -z;
          for (j=1;j<=n;j++) v[j][k] = -v[j][k];
        }
        break;
      }
      if (its == 30) 
        nrerror("No convergence in 30 matmxn_svdcmp iterations");
      x=w[l];
      nm=k-1;
      y=w[nm];
      g=rv1[nm];
      h=rv1[k];
      f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
      g=pythag(f,1.0);
      f=((x-z)*(x+z)+h*((y/(f+GEOM_SIGN(g,f)))-h))/x;
      c=s=1.0;                  /* Next QR transformation */
      for (j=l;j<=nm;j++) {
        i=j+1;
        g=rv1[i];
        y=w[i];
        h=s*g;
        g=c*g;
        z=pythag(f,h);
        rv1[j]=z;
        c=f/z;
        s=h/z;
        f=x*c+g*s;
        g=g*c-x*s;
        h=y*s;
        y=y*c;
        for (jj=1;jj<=n;jj++) {
          x=v[jj][j];
          z=v[jj][i];
          v[jj][j]=x*c+z*s;
          v[jj][i]=z*c-x*s;
        }
        z=pythag(f,h);
        w[j]=z;
        if (z) {
          z=1.0/z;
          c=f*z;
          s=h*z;
        }
        f=(c*g)+(s*y);
        x=(c*y)-(s*g);
        for (jj=1;jj<=m;jj++) {
          y=a[jj][j];
          z=a[jj][i];
          a[jj][j]=y*c+z*s;
          a[jj][i]=z*c-y*s;
        }
      }
      rv1[l]=0.0;
      rv1[k]=f;
      w[k]=x;
    }
  }
                                /* free a double vector allocated with vector() */
                                /* free_vector(rv1,1);*/
  free((char*) (rv1+1));
#undef GEOM_MIN
#undef GEOM_SIGN
}
/*------------------------------------------------------*/

static
void svd_invert
            (double  *target,
             double  *v,        /* will be exploded */
             double  *w,
             double  *J,
             int     width,
             int     height,
             bool transpose) /* Transpose target */

{ int i, j, k;

  for (i=0;i<width;i++)
    for (j=0;j<width;j++)
      VIJ(v,i,j,width) *= w[j];

  /* Compute : target = v * transpose (J) */

  for (i=0;i<width;i++) 
    for (j=0;j<height;j++) 
     {
       double tmp = 0.;
       double *vline = v + i * width,
              *Jline = J + j * width;

       for (k=0;k<width;k++) 
         tmp += *(vline + k) * *(Jline + k);

       /* equivalent a :

         tmp += VIJ(v,i,k,width) * VIJ(J,j,k,width);
       */

       if (transpose)
         VIJ (target, j, i, width) = tmp;
       else
         VIJ (target, i, j, height) = tmp;
     }
}

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

/* The following function computes the damped least square (DLS) inverse
   of matrix [height x width]; the damping is the maximum allowed norm
   for the solution of (matrix+ * task), where task is a vector or
   size height. The least square (LS) inverse (pseudoinverse) can also be
   computed at the same time (for about the same CPU time) if LS_inv
   is not NULL. Also, if task == NULL, it is assumed that task is a unit
   vector.
*/

void matmxn_compute_LS_and_DLS_inverse (double *LS_inv,
                                        double *DLS_inv, 
                                        double *matrix, 
                                        int    width, 
                                        int    height, 
                                        double *task,
                                        double damping)

{ double  *u, *v, *w, *v2, *w2;
  int     i;
  bool is_horizontal = width > height;

  if (task != NULL)
    damping *= vtn_norm (task, height);

  /* For horizontal matrices, we compute the SVD of the transpose,
     because this is much faster (the order of complexity of the SVD
     is ~ width * width * height).
  */

  if (is_horizontal)
  {
    u = matmxn_creation (width, height);

    matmxn_transpose (matrix, u, width, height);

    /* Swap width and height (transpose) */

    i = width; width = height; height = i;
  }
  else
  {
    u = matmxn_creation (height, width);

    matmxn_copy (matrix, u, width, height);
  }

  v = matmxn_creation (width, width);
  v2= matmxn_creation (width, width);
  w = vtn_creation (width);
  w2= vtn_creation (width);

  svd (u,height,width,w,v);

  /* Compute LS-inverse if required */

  if (LS_inv != NULL)
  {
    matmxn_copy (v, v2, width, width);
    vtn_copy (w, w2, width);

    Jinv (w2, width, 0.);

    svd_invert (LS_inv, v2, w2, u, width, height, is_horizontal);
  }

  /* Compute DLS-inverse */

  Jinv (w, width, damping);

  svd_invert (DLS_inv, v, w, u, width, height, is_horizontal);

  matmxn_destr(u);
  matmxn_destr(v);
  matmxn_destr(v2);
  vtn_destr(w);
  vtn_destr(w2);
}

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


/* ================== QUATERNION FUNCTIONS =================== */

/*
 * This library of functions performs operations on quaternions.
 *
 * added by L. Emering, Novembre 10, 1995
 *
 */

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

void quat_set(QUATERNION self, LIBGEOM_REAL w, LIBGEOM_REAL x, LIBGEOM_REAL y, LIBGEOM_REAL z)  
{
  self[GEOM_W_INDEX] = w;
  self[GEOM_X_INDEX] = x;
  self[GEOM_Y_INDEX] = y;
  self[GEOM_Z_INDEX] = z;
}

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

void quat_set_null (QUATERNION self)    
{
  self[GEOM_W_INDEX] = 0.0;
  self[GEOM_X_INDEX] = 0.0;
  self[GEOM_Y_INDEX] = 0.0;
  self[GEOM_Z_INDEX] = 0.0;
}

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

void quat_set_unit(QUATERNION self)             
{
  self[GEOM_W_INDEX] = 1.0;
  self[GEOM_X_INDEX] = 0.0;
  self[GEOM_Y_INDEX] = 0.0;
  self[GEOM_Z_INDEX] = 0.0;
}

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

void quat_assign (QUATERNION self, const QUATERNION newQuat)
{
  self[GEOM_W_INDEX] = newQuat[GEOM_W_INDEX];
  self[GEOM_X_INDEX] = newQuat[GEOM_X_INDEX];
  self[GEOM_Y_INDEX] = newQuat[GEOM_Y_INDEX];
  self[GEOM_Z_INDEX] = newQuat[GEOM_Z_INDEX];
}

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

void quat_copy (const QUATERNION src ,QUATERNION dest)
{
  dest[GEOM_W_INDEX] = src [GEOM_W_INDEX];
  dest[GEOM_X_INDEX] = src [GEOM_X_INDEX];
  dest[GEOM_Y_INDEX] = src [GEOM_Y_INDEX];
  dest[GEOM_Z_INDEX] = src [GEOM_Z_INDEX];
}

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

void quat_get_inv(const QUATERNION self, QUATERNION inv)        
{
  LIBGEOM_REAL qn;
  
  qn = quat_dot(self, self);
  inv[GEOM_W_INDEX] =  self[GEOM_W_INDEX]/qn;
  inv[GEOM_X_INDEX] = -self[GEOM_X_INDEX]/qn;
  inv[GEOM_Y_INDEX] = -self[GEOM_Y_INDEX]/qn;
  inv[GEOM_Z_INDEX] = -self[GEOM_Z_INDEX]/qn;
}

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

void quat_get_conj(const QUATERNION self, QUATERNION conj)      
{
  conj[GEOM_W_INDEX] =  self[GEOM_W_INDEX];
  conj[GEOM_X_INDEX] = -self[GEOM_X_INDEX];
  conj[GEOM_Y_INDEX] = -self[GEOM_Y_INDEX];
  conj[GEOM_Z_INDEX] = -self[GEOM_Z_INDEX];
}

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

LIBGEOM_REAL quat_get_norm(const QUATERNION self)       
{
  return(sqrt((self[GEOM_W_INDEX]*self[GEOM_W_INDEX]) + 
              (self[GEOM_X_INDEX]*self[GEOM_X_INDEX]) + 
              (self[GEOM_Y_INDEX]*self[GEOM_Y_INDEX]) + 
              (self[GEOM_Z_INDEX]*self[GEOM_Z_INDEX])));
}

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

LIBGEOM_REAL quat_get_distance (const QUATERNION left, const QUATERNION right)
{
  QUATERNION qt;
  
  quat_sub(left, right, qt);
  return sqrt(quat_dot(qt,qt));
}

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

LIBGEOM_REAL quat_get_sdistance (const QUATERNION left, const QUATERNION right)
{
  QUATERNION q2n, qt;
  LIBGEOM_REAL dp, dn;
  
  quat_assign(q2n,right);
  quat_neg(q2n);
                                /* calculate length of (Q1, +Q2) arc */
  quat_sub (left, right, qt);
  dp = quat_dot(qt, qt);
  
                                /* calculate length of (Q1, -Q2) arc */
  quat_sub(left, q2n, qt);
  dn = quat_dot(qt, qt);
  return (dn < dp ? sqrt(dn) : sqrt(dp));
}

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

void quat_neg(QUATERNION self)  
{
  self[GEOM_W_INDEX] = -self[GEOM_W_INDEX];
  self[GEOM_X_INDEX] = -self[GEOM_X_INDEX];
  self[GEOM_Y_INDEX] = -self[GEOM_Y_INDEX];
  self[GEOM_Z_INDEX] = -self[GEOM_Z_INDEX];
}

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

void quat_normalize (QUATERNION self)   
     /* normalizes a quaternion */
{
  LIBGEOM_REAL qn = quat_get_norm(self);
  
  if (qn < EPSIL_BIG_ZERO)
   return;
  self[GEOM_W_INDEX] = self[GEOM_W_INDEX]/qn;
  self[GEOM_X_INDEX] = self[GEOM_X_INDEX]/qn;
  self[GEOM_Y_INDEX] = self[GEOM_Y_INDEX]/qn;
  self[GEOM_Z_INDEX] = self[GEOM_Z_INDEX]/qn;
}

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

LIBGEOM_REAL quat_dot(const QUATERNION left, const QUATERNION right)
{
  return ((left[GEOM_W_INDEX] * right[GEOM_W_INDEX]) + 
          (left[GEOM_X_INDEX] * right[GEOM_X_INDEX]) + 
          (left[GEOM_Y_INDEX] * right[GEOM_Y_INDEX]) + 
          (left[GEOM_Z_INDEX] * right[GEOM_Z_INDEX]));
}

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

void quat_pow(const QUATERNION self, LIBGEOM_REAL alpha, QUATERNION result)
     /* raise a normalized quaternion to the power alpha */
{
  LIBGEOM_REAL alphatheta, scale;
  
  scale = fsqrt((self[GEOM_X_INDEX] * self[GEOM_X_INDEX]) + 
                (self[GEOM_Y_INDEX] * self[GEOM_Y_INDEX]) + 
                (self[GEOM_Z_INDEX] * self[GEOM_Z_INDEX]));
  alphatheta = alpha * fatan2(scale, self[GEOM_W_INDEX]);
  if (scale > EPSIL_BIG_ZERO)
   scale = fsin(alphatheta)/scale;
  result[GEOM_W_INDEX] = fcos(alphatheta);
  result[GEOM_X_INDEX] = scale * self[GEOM_X_INDEX];
  result[GEOM_Y_INDEX] = scale * self[GEOM_Y_INDEX];
  result[GEOM_Z_INDEX] = scale * self[GEOM_Z_INDEX];
}

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

void quat_exp(const QUATERNION self, QUATERNION result) 
     /* exponentiate quaternion, assuming LIBGEOM_REAL part 0 */
{
  LIBGEOM_REAL theta, scale;
  
  theta = fsqrt((self[GEOM_X_INDEX]*self[GEOM_X_INDEX]) + 
                (self[GEOM_Y_INDEX]*self[GEOM_Y_INDEX]) + 
                (self[GEOM_Z_INDEX]*self[GEOM_Z_INDEX]));
  scale = 1.0;
  if (theta > EPSIL_BIG_ZERO)
   scale = fsin(theta)/theta;
  result[GEOM_W_INDEX] = fcos(theta);
  result[GEOM_X_INDEX] = scale * self[GEOM_X_INDEX];
  result[GEOM_Y_INDEX] = scale * self[GEOM_Y_INDEX];
  result[GEOM_Z_INDEX] = scale * self[GEOM_Z_INDEX];
}

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

void quat_ln (const QUATERNION self, QUATERNION result) 
     /* take the natural logarithm of a normalized quaternion */
{
  LIBGEOM_REAL theta, scale;
  
  scale = fsqrt((self[GEOM_X_INDEX]*self[GEOM_X_INDEX]) + 
                (self[GEOM_Y_INDEX]*self[GEOM_Y_INDEX]) + 
                (self[GEOM_Z_INDEX]*self[GEOM_Z_INDEX]));
  theta = fatan2(scale, self[GEOM_W_INDEX]);
  if (scale > EPSIL_BIG_ZERO)
   scale = theta/scale;
  result[GEOM_W_INDEX] = 0.0;
  result[GEOM_X_INDEX] = scale*self[GEOM_X_INDEX];
  result[GEOM_Y_INDEX] = scale*self[GEOM_Y_INDEX];
  result[GEOM_Z_INDEX] = scale*self[GEOM_Z_INDEX];
}

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

void quat_mults (const QUATERNION self, LIBGEOM_REAL s, QUATERNION result)      
{
  result[GEOM_W_INDEX] = self[GEOM_W_INDEX] * s;
  result[GEOM_X_INDEX] = self[GEOM_X_INDEX] * s;
  result[GEOM_Y_INDEX] = self[GEOM_Y_INDEX] * s;
  result[GEOM_Z_INDEX] = self[GEOM_Z_INDEX] * s;
}

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

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

  temp[GEOM_W_INDEX]   = left[GEOM_W_INDEX]*right[GEOM_W_INDEX] - 
                          left[GEOM_X_INDEX]*right[GEOM_X_INDEX] - 
                           left[GEOM_Y_INDEX]*right[GEOM_Y_INDEX] - 
                            left[GEOM_Z_INDEX]*right[GEOM_Z_INDEX];
  temp[GEOM_X_INDEX]   = left[GEOM_W_INDEX]*right[GEOM_X_INDEX] + 
                          left[GEOM_X_INDEX]*right[GEOM_W_INDEX] + 
                           left[GEOM_Y_INDEX]*right[GEOM_Z_INDEX] - 
                            left[GEOM_Z_INDEX]*right[GEOM_Y_INDEX];
  temp[GEOM_Y_INDEX]   = left[GEOM_W_INDEX]*right[GEOM_Y_INDEX] + 
                          left[GEOM_Y_INDEX]*right[GEOM_W_INDEX] + 
                           left[GEOM_Z_INDEX]*right[GEOM_X_INDEX] - 
                            left[GEOM_X_INDEX]*right[GEOM_Z_INDEX];
  result[GEOM_Z_INDEX] = left[GEOM_W_INDEX]*right[GEOM_Z_INDEX] + 
                          left[GEOM_Z_INDEX]*right[GEOM_W_INDEX] + 
                           left[GEOM_X_INDEX]*right[GEOM_Y_INDEX] - 
                            left[GEOM_Y_INDEX]*right[GEOM_X_INDEX];

  result [GEOM_W_INDEX] = temp [GEOM_W_INDEX];
  result [GEOM_X_INDEX] = temp [GEOM_X_INDEX];
  result [GEOM_Y_INDEX] = temp [GEOM_Y_INDEX];
}

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

void quat_sub (const QUATERNION left, const QUATERNION right, QUATERNION result)
{
  result[GEOM_W_INDEX] = left[GEOM_W_INDEX]-right[GEOM_W_INDEX];
  result[GEOM_X_INDEX] = left[GEOM_X_INDEX]-right[GEOM_X_INDEX];
  result[GEOM_Y_INDEX] = left[GEOM_Y_INDEX]-right[GEOM_Y_INDEX];
  result[GEOM_Z_INDEX] = left[GEOM_Z_INDEX]-right[GEOM_Z_INDEX];
}

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

void quat_add (const QUATERNION left, const QUATERNION right, QUATERNION result)
{
  result[GEOM_W_INDEX] = left[GEOM_W_INDEX]+right[GEOM_W_INDEX];
  result[GEOM_X_INDEX] = left[GEOM_X_INDEX]+right[GEOM_X_INDEX];
  result[GEOM_Y_INDEX] = left[GEOM_Y_INDEX]+right[GEOM_Y_INDEX];
  result[GEOM_Z_INDEX] = left[GEOM_Z_INDEX]+right[GEOM_Z_INDEX];
}

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

void quat_slerp (const QUATERNION left, const QUATERNION right,
                 LIBGEOM_REAL u, QUATERNION result)
{
   QUATERNION qt1, qt2;
   LIBGEOM_REAL cs, sx, theta, scale1, scale2;

   cs = quat_dot(left, right);

   /* if quaternions are opposed, negate the second one */
   /* i.e. force quats to lie in the same hemisphere (this is due to the two-to-one mapping
      from unit quaternions to rotation matrices: R(q) = R(-q)) */
   if (cs < 0)
   {
      cs = -cs;
      quat_set(qt2, -right[GEOM_W_INDEX], -right[GEOM_X_INDEX], 
               -right[GEOM_Y_INDEX], -right[GEOM_Z_INDEX]);
   }
   else quat_copy (right, qt2);

   /* calculate coefficients */

   if (1.0-cs > EPSIL_BIG_ZERO)
   {
      theta = facos(cs);
      sx = fsin(theta);
      scale1 = fsin((1.0-u) * theta) / sx;
      scale2 = fsin(u * theta) / sx;
   }
   else
   {
      scale1 = 1.0 - u;
      scale2 = u;
   }
   
   quat_mults(left, scale1, qt1);
   quat_mults(qt2, scale2, qt2);
   quat_add (qt1, qt2, result);
}

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

void quat_dump(const QUATERNION self)
{
  printf("\nw:%f  x:%f  y:%f  z:%f", 
         self[GEOM_W_INDEX], 
         self[GEOM_X_INDEX], 
         self[GEOM_Y_INDEX], 
         self[GEOM_Z_INDEX]);
}

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

void quat_from_axis (QUATERNION self, const VT3D axis, LIBGEOM_REAL angle)
{
  LIBGEOM_REAL alpha = angle * 0.5;
  LIBGEOM_REAL sinAlpha = sin(alpha);
  
  self[GEOM_W_INDEX] = cos(alpha);
  self[GEOM_X_INDEX] = sinAlpha * axis[GEOM_X_INDEX];
  self[GEOM_Y_INDEX] = sinAlpha * axis[GEOM_Y_INDEX];
  self[GEOM_Z_INDEX] = sinAlpha * axis[GEOM_Z_INDEX];
}

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

void quat_from_axis_angle (QUATERNION q, const VT3D axis_angle)

{
  VT3D  axis;
  LIBGEOM_REAL norm = vt3d_get_norm (axis_angle);
  
  if (norm > EPSIL_BIG_ZERO)
    {
      vt3d_mults (axis_angle, 1./norm, axis);
      quat_from_axis (q, axis, norm);
    }
  else
    quat_set (q, 1., 0., 0., 0.);
}

/*------------------------------------------------------------------*/
/* Converts a quaternion to an axis-angle whose norm is between 0 and PI. */

void quat_to_axis_angle (const QUATERNION quat, VT3D axis)

{ double angle, norm, scalar;

  norm = sqrt(quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2]) ;

  angle = 2. * safe_acos (quat [3]);

  if (norm > EPSILON)
  {
    if (angle > M_PI)
      scalar = (angle - 2.*M_PI) / norm;
    else
      scalar = angle / norm;

    axis[0] = (LIBGEOM_REAL) (scalar * quat[0]);
    axis[1] = (LIBGEOM_REAL) (scalar * quat[1]);
    axis[2] = (LIBGEOM_REAL) (scalar * quat[2]);
  }
  else
  {
     axis[0] = 0.;
     axis[1] = 0.;
     axis[2] = 0.;
  }
}

/*------------------------------------------------------------------*/
/* Code taken from Gamasutra web site, article of July 1998.
   Replaces an old, buggy version.
*/

void quat_from_matrix (const MatriX m, QUATERNION quat)

{ LIBGEOM_REAL  tr, s;

  tr = m[0][0] + m[1][1] + m[2][2];

  /* check the diagonal */

  if (tr > 0.0) {
    s = sqrt (tr + 1.0);
    quat[3] = s / 2.0;
    s = 0.5 / s;
    quat[0] = (m[1][2] - m[2][1]) * s;
    quat[1] = (m[2][0] - m[0][2]) * s;
    quat[2] = (m[0][1] - m[1][0]) * s;
  }
  else
  {
    int i, j, k;
    int nxt[3] = {1, 2, 0};

    /* diagonal is negative */

     if (m[1][1] > m[0][0])
       i = 1;
     else
       i = 0;

     if (m[2][2] > m[i][i])
       i = 2;

      j = nxt[i];
      k = nxt[j];

      s = sqrt ((m[i][i] - (m[j][j] + m[k][k])) + 1.0);

      quat[i] = s * 0.5;

      if (s != 0.0)   /* s should be never equal to 0 if matrix is orthogonal */
          s = 0.5 / s;

      quat[3] = (m[j][k] - m[k][j]) * s;
      quat[j] = (m[i][j] + m[j][i]) * s;
      quat[k] = (m[i][k] + m[k][i]) * s;
  }
}

/*------------------------------------------------------------------*/
/* Code taken from Gamasutra web site, article of July 1998.
   Replaces an old, buggy version.
*/

void quat_from_mr3d (const MR3D m, QUATERNION quat)

{ LIBGEOM_REAL  tr, s;

  tr = m[0][0] + m[1][1] + m[2][2];

  /* check the diagonal */

  if (tr > 0.0) {
    s = sqrt (tr + 1.0);
    quat[3] = s / 2.0;
    s = 0.5 / s;
    quat[0] = (m[1][2] - m[2][1]) * s;
    quat[1] = (m[2][0] - m[0][2]) * s;
    quat[2] = (m[0][1] - m[1][0]) * s;
  }
  else
  {
    int i, j, k;
    int nxt[3] = {1, 2, 0};

    /* diagonal is negative */

     if (m[1][1] > m[0][0])
       i = 1;
     else
       i = 0;

     if (m[2][2] > m[i][i])
       i = 2;

      j = nxt[i];
      k = nxt[j];

      s = sqrt ((m[i][i] - (m[j][j] + m[k][k])) + 1.0);

      quat[i] = s * 0.5;

      if (s != 0.0)   /* s should be never equal to 0 if matrix is orthogonal */
          s = 0.5 / s;

      quat[3] = (m[j][k] - m[k][j]) * s;
      quat[j] = (m[i][j] + m[j][i]) * s;
      quat[k] = (m[i][k] + m[k][i]) * s;
  }
}

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

void quat_to_matrix (const QUATERNION self, MatriX result)
{
  LIBGEOM_REAL xx, yy, zz, xy, xz, yz, wx, wy, wz;
  LIBGEOM_REAL qnorm2;

  qnorm2 = 2.0f / (self[GEOM_X_INDEX]*self[GEOM_X_INDEX] + 
                   self[GEOM_Y_INDEX]*self[GEOM_Y_INDEX] + 
                   self[GEOM_Z_INDEX]*self[GEOM_Z_INDEX] + 
                   self[GEOM_W_INDEX]*self[GEOM_W_INDEX]);
  xx = qnorm2 * self[GEOM_X_INDEX];
  yy = qnorm2 * self[GEOM_Y_INDEX];
  zz = qnorm2 * self[GEOM_Z_INDEX] ;
  xy = xx * self[GEOM_Y_INDEX] ;
  xz = xx * self[GEOM_Z_INDEX] ;
  wx = xx * self[GEOM_W_INDEX] ;
  yz = yy * self[GEOM_Z_INDEX] ;
  wy = yy * self[GEOM_W_INDEX] ;
  wz = zz * self[GEOM_W_INDEX] ;
  xx *= self[GEOM_X_INDEX] ;
  yy *= self[GEOM_Y_INDEX] ;
  zz *= self[GEOM_Z_INDEX] ;
  
  result[0][0]= 1.0-yy-zz; result[0][1]= xy+wz;     
  result[0][2]= xz-wy;     result[0][3]= 0.0;
  result[1][0]= xy-wz;     result[1][1]= 1.0-xx-zz; 
  result[1][2]= yz+wx;     result[1][3]= 0.0;
  result[2][0]= xz+wy;     result[2][1]= yz-wx;     
  result[2][2]= 1.0-xx-yy; result[2][3]= 0.0;
  result[3][0]= 0.0;       result[3][1]= 0.0;       
  result[3][2]= 0.0;       result[3][3]= 1.0;
}

void quat_to_mr3d (const QUATERNION self, MR3D result)
{
  LIBGEOM_REAL xx, yy, zz, xy, xz, yz, wx, wy, wz;
  LIBGEOM_REAL qnorm2;

  qnorm2 = 2.0f / (self[GEOM_X_INDEX]*self[GEOM_X_INDEX] + 
                   self[GEOM_Y_INDEX]*self[GEOM_Y_INDEX] + 
                   self[GEOM_Z_INDEX]*self[GEOM_Z_INDEX] + 
                   self[GEOM_W_INDEX]*self[GEOM_W_INDEX]);
  xx = qnorm2 * self[GEOM_X_INDEX];
  yy = qnorm2 * self[GEOM_Y_INDEX];
  zz = qnorm2 * self[GEOM_Z_INDEX] ;
  xy = xx * self[GEOM_Y_INDEX] ;
  xz = xx * self[GEOM_Z_INDEX] ;
  wx = xx * self[GEOM_W_INDEX] ;
  yz = yy * self[GEOM_Z_INDEX] ;
  wy = yy * self[GEOM_W_INDEX] ;
  wz = zz * self[GEOM_W_INDEX] ;
  xx *= self[GEOM_X_INDEX] ;
  yy *= self[GEOM_Y_INDEX] ;
  zz *= self[GEOM_Z_INDEX] ;
  
  result[0][0]= 1.0-yy-zz; result[0][1]= xy+wz;     
  result[0][2]= xz-wy;     
  result[1][0]= xy-wz;     result[1][1]= 1.0-xx-zz; 
  result[1][2]= yz+wx;     
  result[2][0]= xz+wy;     result[2][1]= yz-wx;     
  result[2][2]= 1.0-xx-yy; 
}

/*------------------------------------------------------------------*/
/* Compute quaternion that performs a rotation that directly
   rotates v1 to v2.
*/

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

{ double dotp = vt3d_get_dot (v1, v2) + 1.;

  if (dotp > EPSIL_ZERO)
  {
    double c = sqrt (dotp * 0.5),
           d = 0.5 / c;
    VT3D   n;
  
    vt3d_cross (v1, v2, n);

    quat_set (q, c, n [0] * d, n [1] * d, n [2] * d);
  }
  else /* singularity : choose a solution among an infinity. */
  {
    quat_set (q, 0., 1., 0., 0.);
  }
}

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

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

{ 
  VT3D aa_swing;

  vt3d_let (swing [0], swing [1], 0., aa_swing);
  quat_from_axis_angle (q, aa_swing);
}

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

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

{ QUATERNION qswing, qtwist;
  VT3D aa_swing, aa_twist;

  vt3d_let (swing [0], swing [1], 0.,    aa_swing);
  vt3d_let (0.,        0.,        twist, aa_twist);

  quat_from_axis_angle (qswing, aa_swing);
  quat_from_axis_angle (qtwist, aa_twist);

  quat_mult (qswing, qtwist, q);
}

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

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

{ LIBGEOM_REAL beta, s1, s2, gamma, sin_gamma, cos_gamma, fact;

  s1 = q [3] * q [3] + q [2] * q [2];
  s2 = q [0] * q [0] + q [1] * q [1];

  beta = atan2 (sqrt (s1*s2), s1);

  gamma = atan2 (q [2], q [3]);

  *twist = 2. * gamma;

  sin_gamma = sin (gamma);
  cos_gamma = cos (gamma);

  if (fabs (beta) < 0.001)
    fact = 1.;
  else
    fact = beta / sin (beta);

  fact *= 2.;

  swing [0] = fact * (cos_gamma * q [0] - sin_gamma * q [1]);
  swing [1] = fact * (sin_gamma * q [0] + cos_gamma * q [1]);
}

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

/*-------------------------------------------------------------------*/
/* ARRAY 1D                                                          */
/*-------------------------------------------------------------------*/

ARRAY1D *array1d_creation (int lines)

{ ARRAY1D *array1d = (ARRAY1D *) malloc (sizeof (ARRAY1D));

  if (array1d)
  {
    array1d->data = vtn_creation (lines);
    array1d->lines = lines;
  }

  return array1d;
}

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

void array1d_destroy (ARRAY1D *array1d)

{ if (array1d)
  {
    vtn_destr (array1d->data);
    free (array1d);
  }
}

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

void array1d_nul (ARRAY1D *array1d)

{ vtn_nul (array1d->data, array1d->lines); }

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

double array1d_norm (ARRAY1D *array1d)

{ return vtn_norm (array1d->data, array1d->lines); }

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

double array1d_anorm (ARRAY1D *array1d)

{ return vtn_anorm (array1d->data, array1d->lines); }

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

void array1d_copy (ARRAY1D *src,
                   ARRAY1D *dest)

{ assert (src->lines == dest->lines);

  vtn_copy (src->data, dest->data, src->lines);
}

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

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

{ assert (src_left->lines == src_right->lines);
  assert (src_left->lines == dest->lines);

  vtn_add (src_left->data, src_right->data, dest->data, dest->lines);
}

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

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

{ assert (src_left->lines == src_right->lines);
  assert (src_left->lines == dest->lines);

  vtn_sub (src_left->data, src_right->data, dest->data, dest->lines);
}

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

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

{ assert (src->lines == dest->lines);

  vtn_mults (src->data, dest->data, src->lines, factor);
}

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

double array1d_dot (ARRAY1D *src1,
                    ARRAY1D *src2)

{ assert (src1->lines == src2->lines);

  return vtn_dot (src1->data, src2->data, src1->lines);
}

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

double array1d_inv (ARRAY1D *src,
                    ARRAY1D *dest)

{ assert (src->lines == dest->lines);

  return vtn_inv (src->data, dest->data, src->lines);
}

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

void array1d_write (ARRAY1D *a,
                    FILE    *file)

{ int i;

  for (i=0; i < a->lines; i++)
    fprintf (file, "%f\n", AI (a, i));
}

/*-------------------------------------------------------------------*/
/* ARRAY 2D                                                          */
/*-------------------------------------------------------------------*/

ARRAY2D *array2d_creation (int lines,
                           int cols)

{ ARRAY2D *array2d = (ARRAY2D *) malloc (sizeof (ARRAY2D));

  if (array2d)
  {
    array2d->data  = matmxn_creation (lines, cols);
    array2d->lines = lines;
    array2d->cols  = cols;
  }

  return array2d;
}

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

void array2d_destroy (ARRAY2D *array2d)

{ if (array2d)
  {
    matmxn_destr (array2d->data);
    free (array2d);
  }
}

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

int  array2d_same_size (ARRAY2D *a,
                        ARRAY2D *b)

{ return (a->lines == b->lines) && (a->cols == b->cols); }

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

void array2d_nul (ARRAY2D *array2d)

{ matmxn_nul (array2d->data, array2d->lines, array2d->cols); }

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

void array2d_idt (ARRAY2D *array2d)

{ matmxn_idt (array2d->data, array2d->lines, array2d->cols); }

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

void array2d_copy (ARRAY2D *src,
                   ARRAY2D *dest)

{ assert (src->lines == dest->lines);
  assert (src->cols  == dest->cols);

  matmxn_copy (src->data, dest->data, src->lines, src->cols);
}

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

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

{ assert (a->cols  == b->lines);
  assert (a->lines == dest->lines);
  assert (b->cols  == dest->cols);

  matmxn_mult (a->data, b->data, dest->data, a->lines, a->cols, b->cols);
}

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

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

{ int size;
  double *ap    = a->data,
         *bp    = b->data,
         *destp = dest->data;

  assert (array2d_same_size (a, b) && array2d_same_size (a, dest));

  size = a->lines * a->cols;

  while (size-- > 0)
  {
    *destp = *ap + *bp;

    ap++;
    bp++;
    destp++;
  }
}

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

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

{ int size;
  double *ap    = a->data,
         *bp    = b->data,
         *destp = dest->data;

  assert (array2d_same_size (a, b) && array2d_same_size (a, dest));

  size = a->lines * a->cols;

  while (size-- > 0)
  {
    *destp = *ap - *bp;

    ap++;
    bp++;
    destp++;
  }
}

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

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

{ assert (array2d->cols  == array1d->lines);
  assert (array2d->lines == dest->lines);

  matmxn_mult_vtn (array2d->data, array1d->data, dest->data,
                   array2d->lines, array2d->cols);
}

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

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

{ int i, j;

  assert (src->lines == dest->lines);
  assert (src->cols  == dest->cols);

  for (i=0; i < src->lines; i++)
    for (j=0; j < src->cols; j++)
      AIJ (dest, i, j) = AIJ (src, i, j) * scalar;
}

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

void array2d_pseudo_inverse (ARRAY2D *src,
                             ARRAY2D *dest)

{ assert (src->lines == dest->cols);
  assert (src->cols == dest->lines);

  matmxn_pseudo_inverse (src->data, src->lines, src->cols, dest->data);
}

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

void array2d_damped_least_square_inverse (ARRAY2D *j_inv,    /* output */
                                          ARRAY2D *j_mat,    /* input  */
                                          double  damping)

{ matmxn_damped_least_square_inverse (j_inv->data, j_mat->data, j_mat->cols, j_mat->lines, 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,     /* (see relative matmxn_ function) */
                                         double  damping)   /* for DLS-inverse    */

{ matmxn_compute_LS_and_DLS_inverse (LS_inv == NULL ? NULL : LS_inv->data,
                                     DLS_inv->data, j_mat->data,
                                     j_mat->cols, j_mat->lines, task->data, damping);
}

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

void array2d_write (ARRAY2D *a,
                    FILE    *file)

{ int i, j;

  for (i=0; i < a->lines; i++)
  {
    for (j=0; j < a->cols; j++)
      fprintf (file, "%f ", AIJ (a, i, j));

    fprintf (file, "\n");
  }
}

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

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

{ int    lines;
  double *data;

  assert ((array2d != NULL) && (column < array2d->cols) && (column >= 0));

  lines = array2d->lines;
  data  = array2d->data + column;

  while (lines-- > 0)
  {
    *data = value;
    data += array2d->cols;
  }
}

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

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

{ int    cols;
  double *data;

  assert ((array2d != NULL) && (line < array2d->lines) && (line >= 0));

  cols = array2d->cols;
  data = array2d->data + line * cols;

  while (cols-- > 0)
  {
    *data = value;
    data++;
  }
}

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

void array2d_transpose (ARRAY2D *src,
                        ARRAY2D *dest)

{ assert (dest->lines == src->cols);
  assert (dest->cols == src->lines);

  matmxn_transpose (src->data, dest->data, src->cols, src->lines);
}

/*-------------------------------------------------------------------*/
/* Functions to solve polynomials of 2nd, 3rt and 4th degree.        */
/* Source: graphics gems                                             */
/*-------------------------------------------------------------------*/

/* epsilon surrounding for near zero values */

#define     EQN_EPS     1e-9
#define     IsZero(x)   ((x) > -EQN_EPS && (x) < EQN_EPS)

#define     CBRT(x)     ((x) > 0.0 ? pow((double)(x), 1.0/3.0) : \
                          ((x) < 0.0 ? -pow((double)-(x), 1.0/3.0) : 0.0))

int polynomial_solve_quadric (double c[3], double s[2])
{
    double p, q, D;

    /* normal form: x^2 + px + q = 0 */

    p = c[ 1 ] / (2 * c[ 2 ]);
    q = c[ 0 ] / c[ 2 ];

    D = p * p - q;

    if (IsZero(D))
    {
        s[ 0 ] = - p;
        return 1;
    }
    else if (D < 0)
    {
        return 0;
    }
    else /* if (D > 0) */
    {
        double sqrt_D = sqrt(D);

        s[ 0 ] =   sqrt_D - p;
        s[ 1 ] = - sqrt_D - p;
        return 2;
    }
}


int polynomial_solve_cubic (double c[4], double s[3])
{
    int     i, num;
    double  sub;
    double  A, B, C;
    double  sq_A, p, q;
    double  cb_p, D;

    /* normal form: x^3 + Ax^2 + Bx + C = 0 */

    A = c[ 2 ] / c[ 3 ];
    B = c[ 1 ] / c[ 3 ];
    C = c[ 0 ] / c[ 3 ];

    /*  substitute x = y - A/3 to eliminate quadric term:
        x^3 +px + q = 0 */

    sq_A = A * A;
    p = 1.0/3 * (- 1.0/3 * sq_A + B);
    q = 1.0/2 * (A * (2.0/27 * sq_A - 1.0/3 * B) + C);

    /* use Cardano's formula */

    cb_p = p * p * p;
    D = q * q + cb_p;

    if (IsZero(D))
    {
        if (IsZero(q)) /* one triple solution */
        {
            s[ 0 ] = 0;
            num = 1;
        }
        else /* one single and one double solution */
        {
            double u = CBRT(-q);
            s[ 0 ] = 2 * u;
            s[ 1 ] = - u;
            num = 2;
        }
    }
    else if (D < 0) /* Casus irreducibilis: three real solutions */
    {
        double phi = 1.0/3 * acos(-q / sqrt(-cb_p));
        double t = 2 * sqrt(-p);

        s[ 0 ] =   t * cos(phi);
        s[ 1 ] = - t * cos(phi + M_PI / 3);
        s[ 2 ] = - t * cos(phi - M_PI / 3);
        num = 3;
    }
    else /* one real solution */
    {
        double sqrt_D = sqrt(D);
        double u = CBRT(sqrt_D - q);
        double v = - CBRT(sqrt_D + q);

        s[ 0 ] = u + v;
        num = 1;
    }

    /* resubstitute */

    sub = 1.0/3 * A;

    for (i = 0; i < num; ++i)
        s[ i ] -= sub;

    return num;
}


int polynomial_solve_quartic(double c[5], double s[4])
{
    double  coeffs[ 4 ];
    double  z, u, v, sub;
    double  A, B, C, D;
    double  sq_A, p, q, r;
    int     i, num;

    /* normal form: x^4 + Ax^3 + Bx^2 + Cx + D = 0 */

    A = c[ 3 ] / c[ 4 ];
    B = c[ 2 ] / c[ 4 ];
    C = c[ 1 ] / c[ 4 ];
    D = c[ 0 ] / c[ 4 ];

    /*  substitute x = y - A/4 to eliminate cubic term:
        x^4 + px^2 + qx + r = 0 */

    sq_A = A * A;
    p = - 3.0/8 * sq_A + B;
    q = A * (1.0/8 * sq_A - 1.0/2 * B) + C;
    r = sq_A * (- 3.0/256*sq_A + 1.0/16*B) - 1.0/4*A*C + D;

    if (IsZero(r))
    {
        /* no absolute term: y(y^3 + py + q) = 0 */

        coeffs[ 0 ] = q;
        coeffs[ 1 ] = p;
        coeffs[ 2 ] = 0;
        coeffs[ 3 ] = 1;

        num = polynomial_solve_cubic(coeffs, s);

        s[ num++ ] = 0;
    }
    else
    {
        /* solve the resolvent cubic ... */

        coeffs[ 0 ] = 1.0/2 * r * p - 1.0/8 * q * q;
        coeffs[ 1 ] = - r;
        coeffs[ 2 ] = - 1.0/2 * p;
        coeffs[ 3 ] = 1;

        (void) polynomial_solve_cubic(coeffs, s);

        /* ... and take the one real solution ... */

        z = s[ 0 ];

        /* ... to build two quadric equations */

        u = z * z - r;
        v = 2 * z - p;

        if (IsZero(u))
            u = 0;
        else if (u > 0)
            u = sqrt(u);
        else
            return 0;

        if (IsZero(v))
            v = 0;
        else if (v > 0)
            v = sqrt(v);
        else
            return 0;

        coeffs[ 0 ] = z - u;
        coeffs[ 1 ] = q < 0 ? -v : v;
        coeffs[ 2 ] = 1;

        num = polynomial_solve_quadric(coeffs, s);

        coeffs[ 0 ]= z + u;
        coeffs[ 1 ] = q < 0 ? v : -v;
        coeffs[ 2 ] = 1;

        num += polynomial_solve_quadric(coeffs, s + num);
    }

    /* resubstitute */

    sub = 1.0/4 * A;

    for (i = 0; i < num; ++i)
        s[ i ] -= sub;

    return num;
}

}

---