tf.h

Go to the documentation of this file.

/* -*- mode: C; c-basic-offset: 4 -*- */
/* ex: set shiftwidth=4 tabstop=4 expandtab: */
/*
 * Copyright (c) 2010-2014, Georgia Tech Research Corporation
 * Copyright (c) 2015, Rice University
 * All rights reserved.
 *
 * Author(s): Neil T. Dantam <ntd@gatech.edu>
 * Georgia Tech Humanoid Robotics Lab
 * Under Direction of Prof. Mike Stilman <mstilman@cc.gatech.edu>
 *
 *
 * This file is provided under the following "BSD-style" License:
 *
 *
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *
 *   * Neither the name of copyright holder the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 *
 */

#ifndef AMINO_TF_H
#define AMINO_TF_H

#include <float.h>

#ifdef __cplusplus
extern "C" {
#endif

/**********/
/* Series */
/**********/
#define AA_TF_DEF_SERIES(name, a0, a1, a2)                  \
    static inline double                                    \
    aa_tf_ ## name ##_series2(double theta2)                \
    {                                                       \
        return aa_horner3( theta2, a0, a1, a2 );            \
    }                                                       \
    static inline double                                    \
    aa_tf_ ## name ## _series(double theta)                 \
    {                                                       \
        return aa_tf_ ## name ## _series2(theta*theta);     \
    }                                                       \

AA_TF_DEF_SERIES( sinc, 1., -1./6, 1./120 )
AA_TF_DEF_SERIES( cos, 1., -1./2, 1./24 )
AA_TF_DEF_SERIES( invsinc, 1., 1./6, 7./360 )

static inline void
aa_tf_sinccos2( double theta2, double *sc, double *c )
{
    if( theta2 < sqrt(DBL_EPSILON) ) {
        *sc = aa_tf_sinc_series2(theta2);
        *c = aa_tf_cos_series2(theta2);
    } else {
        double theta = sqrt(theta2);
        double s = sin(theta);
        *c = cos(theta);
        *sc = s / theta;
    }
}

static inline void
aa_tf_sinccos( double theta, double *sc, double *c )
{
    if( theta*theta < sqrt(DBL_EPSILON) ) {
        *sc = aa_tf_sinc_series(theta);
        *c = aa_tf_cos_series(theta);
    } else {
        double s = sin(theta);
        *c = cos(theta);
        *sc = s / theta;
    }
}

static inline double
aa_tf_sinc( double theta )
{
    if( fabs(theta) < sqrt(sqrt(DBL_EPSILON)) ) {
        return aa_tf_sinc_series(theta);
    } else {
        return sin(theta)/theta;
    }
}

/*********/
/* Types */
/*********/

#define AA_TF_X 0
#define AA_TF_Y 1
#define AA_TF_Z 2

typedef struct aa_tf_vec3 {
    union {
        struct {
            double x;   
            double y;   
            double z;   
        };
        double data[3]; 
    };
} aa_tf_vec3_t;

typedef struct aa_tf_rotmat {
    union {
        struct {
            struct aa_tf_vec3 col0;  
            struct aa_tf_vec3 col1;  
            struct aa_tf_vec3 col2;  
        };
        double data[9]; 
    };
} aa_tf_rotmat_t;

typedef struct aa_tf_axang {
    union {
        struct {
            union {
                struct aa_tf_vec3 axis;
                double v[3];
            };
            double angle;
        };
        double data[4];
    };
} aa_tf_axang_t;

#define AA_TF_QUAT_V 0
#define AA_TF_QUAT_XYZ AA_TF_QUAT_V
#define AA_TF_QUAT_X (AA_TF_QUAT_V + AA_TF_X)
#define AA_TF_QUAT_Y (AA_TF_QUAT_V + AA_TF_Y)
#define AA_TF_QUAT_Z (AA_TF_QUAT_V + AA_TF_Z)
#define AA_TF_QUAT_W 3

typedef struct aa_tf_quat {
    union {
        struct {
            double x; 
            double y; 
            double z; 
            double w; 
        };
        struct {
            union {
                struct aa_tf_vec3 vec; 
                double v[3];           
            };
            double scalar; 
        };
        double data[4]; 
    };
} aa_tf_quat_t;

typedef struct aa_tf_eulerzyx {
    union {
        struct {
            double y; 
            double p; 
            double r; 
        };
        double data[3]; 
    };
} aa_tf_eulerzyx_t;


typedef struct aa_tf_tfmat {
    union {
        struct {
            double R[9];      
            aa_tf_vec3_t v;   
        };
        double data[12]; 
    };
} aa_tf_tfmat_t;

#define AA_TF_TFMAT_R 0

#define AA_TF_TFMAT_V 9

#define AA_TF_TFMAT_X 9

#define AA_TF_TFMAT_Y 10

#define AA_TF_TFMAT_Z 11

typedef struct aa_tf_qv {
    union {
        struct {
            aa_tf_quat_t r;
            aa_tf_vec3_t v;
        };
        double data[7]; 
    };
} aa_tf_qv_t;



typedef struct aa_tf_duqu {
    union {
        struct {
            aa_tf_quat_t real;
            aa_tf_quat_t dual;
        };
        double data[8]; 
    };
} aa_tf_duqu_t;

struct aa_tf_dx {
    union {
        struct {
            double dv[3];      
            double omega[3];   
        };
        double data[6]; 
    };
};

#define AA_TF_DX_V 0
#define AA_TF_DX_W 3

struct aa_tf_qv_dx {
    union {
        struct {
            struct aa_tf_qv tf; 
            struct aa_tf_dx dx; 
        };
        double data[13];
    };
};

#define AA_TF_QUTR_Q  0
#define AA_TF_QUTR_QX (AA_TF_QUTR_Q + AA_TF_QUAT_X)
#define AA_TF_QUTR_QY (AA_TF_QUTR_Q + AA_TF_QUAT_Y)
#define AA_TF_QUTR_QZ (AA_TF_QUTR_Q + AA_TF_QUAT_Z)
#define AA_TF_QUTR_QW (AA_TF_QUTR_Q + AA_TF_QUAT_W)

#define AA_TF_QUTR_T  4
#define AA_TF_QUTR_TX (AA_TF_QUTR_T + AA_TF_X)
#define AA_TF_QUTR_TY (AA_TF_QUTR_T + AA_TF_Y)
#define AA_TF_QUTR_TZ (AA_TF_QUTR_T + AA_TF_Z)

#define AA_TF_DUQU_REAL 0
#define AA_TF_DUQU_DUAL 4

#define AA_TF_DUQU_REAL_W    (AA_TF_DUQU_REAL + AA_TF_QUAT_W)
#define AA_TF_DUQU_REAL_XYZ  (AA_TF_DUQU_REAL + AA_TF_QUAT_XYZ)
#define AA_TF_DUQU_REAL_X  (AA_TF_DUQU_REAL + AA_TF_QUAT_X)
#define AA_TF_DUQU_REAL_Y  (AA_TF_DUQU_REAL + AA_TF_QUAT_Y)
#define AA_TF_DUQU_REAL_Z  (AA_TF_DUQU_REAL + AA_TF_QUAT_Z)

#define AA_TF_DUQU_DUAL_W    (AA_TF_DUQU_DUAL + AA_TF_QUAT_W)
#define AA_TF_DUQU_DUAL_XYZ  (AA_TF_DUQU_DUAL + AA_TF_QUAT_XYZ)
#define AA_TF_DUQU_DUAL_X  (AA_TF_DUQU_DUAL + AA_TF_QUAT_X)
#define AA_TF_DUQU_DUAL_Y  (AA_TF_DUQU_DUAL + AA_TF_QUAT_Y)
#define AA_TF_DUQU_DUAL_Z  (AA_TF_DUQU_DUAL + AA_TF_QUAT_Z)

#define AA_TF_EPSILON .0001



#define AA_TF_IDENT_INITIALIZER {1,0,0, 0,1,0, 0,0,1, 0,0,0}

#define AA_TF_ROTMAT_IDENT_INITIALIZER {1,0,0, 0,1,0, 0,0,1}

#define AA_TF_QUAT_IDENT_INITIALIZER {0,0,0,1}

#define AA_TF_DUQU_IDENT_INITIALIZER {0,0,0,1, 0,0,0,0}

#define AA_TF_QUTR_IDENT_INITIALIZER {0,0,0,1, 0,0,0}

#define AA_TF_AXANG_IDENT_INITIALIZER {1,0,0,0}

#define AA_TF_ROTVEC_IDENT_INITIALIZER {0,0,0}

#define AA_TF_VEC_IDENT_INITIALIZER {0,0,0}

#define AA_TF_IDENT ( (double[12]) AA_TF_IDENT_INITIALIZER )
#define AA_TF_ROTMAT_IDENT ( (double[9] AA_TF_ROTMAT_IDENT_INITIALIZER )
#define AA_TF_QUAT_IDENT ( (double[4]) AA_TF_QUAT_IDENT_INITIALIZER )
#define AA_TF_AXANG_IDENT ( (double[4]) AA_TF_AXANG_IDENT_INITIALIZER )
#define AA_TF_ROTVEC_IDENT ( (double[3]) AA_TF_ROTVEC_IDENT_INITIALIZER )

static const double aa_tf_ident[12] = AA_TF_IDENT_INITIALIZER;

static const double aa_tf_tfmat_ident[12] = AA_TF_IDENT_INITIALIZER;

static const double aa_tf_rotmat_ident[9] = AA_TF_ROTMAT_IDENT_INITIALIZER;

static const double aa_tf_quat_ident[4] = AA_TF_QUAT_IDENT_INITIALIZER;

static const double aa_tf_duqu_ident[8] = AA_TF_DUQU_IDENT_INITIALIZER;

static const double aa_tf_qutr_ident[7] = AA_TF_QUTR_IDENT_INITIALIZER;

static const double aa_tf_axang_ident[4] = AA_TF_AXANG_IDENT_INITIALIZER;

static const double aa_tf_rotvec_ident[3] = AA_TF_ROTVEC_IDENT_INITIALIZER;

static const double aa_tf_vec_ident[3] = AA_TF_ROTVEC_IDENT_INITIALIZER;

static const double aa_tf_vec_x[3] = {1, 0, 0};

static const double aa_tf_vec_y[3] = {0, 1, 0};

static const double aa_tf_vec_z[3] = {0, 0, 1};

/**************/
/* Transforms */
/**************/

AA_API void aa_tf_12( const double T[AA_RESTRICT 12],
                      const double p0[AA_RESTRICT 3],
                      double p1[AA_RESTRICT 3] );
AA_API void aa_tf_93( const double R[AA_RESTRICT 9],
                      const double v[AA_RESTRICT 3],
                      const double p0[AA_RESTRICT 3],
                      double p1[AA_RESTRICT 4] );
AA_API void aa_tf_tf_qv( const double quat[AA_RESTRICT 4],
                         const double v[AA_RESTRICT 3],
                         const double p0[AA_RESTRICT 3],
                         double p1[AA_RESTRICT 4] );

AA_API void aa_tf_9( const double R[AA_RESTRICT 9],
                     const double p0[AA_RESTRICT 3],
                     double p1[AA_RESTRICT 4] );

AA_API void aa_tf_12inv( const double T[AA_RESTRICT 12],
                         double Ti[AA_RESTRICT 12] );
AA_API void aa_tf_93inv( const double R[AA_RESTRICT 9],
                         const double v[AA_RESTRICT 3],
                         double Ri[AA_RESTRICT 9], double vi[AA_RESTRICT 3] );
AA_API void aa_tf_qv_conj( const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3],
                           double qc[AA_RESTRICT 4], double vc[AA_RESTRICT 3]  );

AA_API void aa_tf_12chain( const double T1[AA_RESTRICT 12],
                           const double T2[AA_RESTRICT 12],
                           double T[AA_RESTRICT 12] );

AA_API void aa_tf_v12chain( double T[AA_RESTRICT 12] ,
                            const double T1[AA_RESTRICT 12],
                            const double T2[AA_RESTRICT 12],
                            ... );

AA_API void aa_tf_93chain( const double R0[AA_RESTRICT 9],
                           const double v0[AA_RESTRICT 3],
                           const double R1[AA_RESTRICT 9],
                           const double v1[AA_RESTRICT 3],
                           double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );
AA_API void aa_tf_qv_chain( const double q0[AA_RESTRICT 4],
                            const double v0[AA_RESTRICT 3],
                            const double q1[AA_RESTRICT 4],
                            const double v1[AA_RESTRICT 3],
                            double q[AA_RESTRICT 4],
                            double v[AA_RESTRICT 3] );

AA_API void aa_tf_93rel( const double R1[AA_RESTRICT 9],
                         const double v1[AA_RESTRICT 3],
                         const double R2[AA_RESTRICT 9],
                         const double v2[AA_RESTRICT 3],
                         double Rrel[AA_RESTRICT 9],
                         double vrel[AA_RESTRICT 3] );

AA_API void aa_tf_12rel( const double T1[AA_RESTRICT 12],
                         const double T2[AA_RESTRICT 12],
                         double Trel[AA_RESTRICT 12] );

/************/
/* Matrices */
/************/

AA_API void aa_tf_skewsym_scal2( double a, double b, const double u[3], double R[9] );

AA_API void
aa_tf_skewsym_scal_c( const double u[AA_RESTRICT 3],
                      const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3],
                      double R[9] );


/* Multiply */
AA_API void aa_tf_rotmat_mul( const double R0[AA_RESTRICT 9],
                              const double R1[AA_RESTRICT 9],
                              double R[AA_RESTRICT 9] );

AA_API void aa_tf_tfmat_mul( const double T0[AA_RESTRICT 12],
                             const double T1[AA_RESTRICT 12],
                             double T[AA_RESTRICT 12] );

AA_API void aa_tf_tfmat2_mul( const double R0[AA_RESTRICT 9],
                              const double v0[AA_RESTRICT 3],
                              const double R1[AA_RESTRICT 9],
                              const double v1[AA_RESTRICT 3],
                              double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );


/* Inverting Multiply */

AA_API void aa_tf_rotmat_imul( const double R0[AA_RESTRICT 9],
                               const double R1[AA_RESTRICT 9],
                               double R[AA_RESTRICT 9] );

AA_API void aa_tf_rotmat_muli( const double R0[AA_RESTRICT 9],
                               const double R1[AA_RESTRICT 9],
                               double R[AA_RESTRICT 9] );

AA_API void aa_tf_tfmat_imul( const double T0[AA_RESTRICT 12],
                              const double T1[AA_RESTRICT 12],
                              double T[AA_RESTRICT 12] );

AA_API void aa_tf_tfmat_muli( const double T0[AA_RESTRICT 12],
                              const double T1[AA_RESTRICT 12],
                              double T[AA_RESTRICT 12] );

AA_API void aa_tf_tfmat2_imul( const double R0[AA_RESTRICT 9],
                               const double v0[AA_RESTRICT 3],
                               const double R1[AA_RESTRICT 9],
                               const double v1[AA_RESTRICT 3],
                               double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );

AA_API void aa_tf_tfmat2_muli( const double R0[AA_RESTRICT 9],
                               const double v0[AA_RESTRICT 3],
                               const double R1[AA_RESTRICT 9],
                               const double v1[AA_RESTRICT 3],
                               double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );

/* Transform */

AA_API void aa_tf_rotmat_rot( const double R[AA_RESTRICT 9],
                              const double p0[AA_RESTRICT 3],
                              double p1[AA_RESTRICT 3] );

AA_API void aa_tf_tfmat_tf( const double T[AA_RESTRICT 12],
                      const double p0[AA_RESTRICT 3],
                      double p1[AA_RESTRICT 3] );

AA_API void aa_tf_tfmat2_tf( const double R[AA_RESTRICT 9],
                             const double v[AA_RESTRICT 3],
                             const double p0[AA_RESTRICT 3],
                             double p1[AA_RESTRICT 4] );

AA_API void aa_tf_rotmat_inv1( double R[AA_RESTRICT 9] );

AA_API void aa_tf_rotmat_inv2( const double R[AA_RESTRICT 9],
                               double Ri[AA_RESTRICT 9] );

AA_API void aa_tf_tfmat_inv1( double T[AA_RESTRICT 12] );

AA_API void aa_tf_tfmat_inv2( const double T[AA_RESTRICT 12],
                              double Ti[AA_RESTRICT 12] );

AA_API int aa_tf_isrotmat( const double R[AA_RESTRICT 9] );


AA_API void aa_tf_9mul( const double R0[AA_RESTRICT 9],
                        const double R1[AA_RESTRICT 9],
                        double R[AA_RESTRICT 9] );
AA_API void aa_tf_9rot( const double R[AA_RESTRICT 9],
                        const double p0[AA_RESTRICT 3],
                        double p1[AA_RESTRICT 3] );

AA_API void aa_tf_9rel( const double R1[AA_RESTRICT 9], const double R2[AA_RESTRICT 9],
                        double Ri[AA_RESTRICT 9] );

AA_API void aa_tf_rotmat_exp_aa( const double aa[AA_RESTRICT 4], double R[AA_RESTRICT 9] );

AA_API void aa_tf_rotmat_expv( const double rv[AA_RESTRICT 3], double R[AA_RESTRICT 9] );

AA_API void aa_tf_rotmat_lnv( const double R[AA_RESTRICT 9], double v[AA_RESTRICT 3] );

AA_API void aa_tf_tfmat_expv( const double v[AA_RESTRICT 6], double T[AA_RESTRICT 12] );

AA_API void aa_tf_tfmat_lnv( const double T[AA_RESTRICT 12], double v[AA_RESTRICT 6] );

AA_API void aa_tf_rotmat_vel2diff( const double R[AA_RESTRICT 9],
                                   const double w[AA_RESTRICT 3], double dR[AA_RESTRICT 9] );

AA_API void aa_tf_rotmat_diff2vel( const double R[AA_RESTRICT 9],
                                   const double dR[AA_RESTRICT 9], double w[AA_RESTRICT 3] );

AA_API void aa_tf_rotmat_svel( const double R0[AA_RESTRICT 9],
                               const double w[AA_RESTRICT 3], double dt,
                               double R1[AA_RESTRICT 9] );



AA_API void aa_tf_tfmat_svel( const double T0[AA_RESTRICT 12],
                              const double w[AA_RESTRICT 3], double dt,
                              double T1[AA_RESTRICT 12] );

AA_API void aa_tf_v9mul( double R[AA_RESTRICT 9],
                         const double R1[AA_RESTRICT 9], const double R2[AA_RESTRICT 9], ... );


AA_API void
aa_tf_rotmat_xy( const double x_axis[AA_RESTRICT 3],
                 const double y_axis[AA_RESTRICT 3],
                 double R[AA_RESTRICT 9] );
AA_API void
aa_tf_rotmat_yz( const double y_axis[AA_RESTRICT 3],
                 const double z_axis[AA_RESTRICT 3],
                 double R[AA_RESTRICT 9] );

AA_API void
aa_tf_rotmat_zx( const double z_axis[AA_RESTRICT 3],
                 const double x_axis[AA_RESTRICT 3],
                 double R[AA_RESTRICT 9] );


/***********/
/* Vectors */
/***********/

#define AA_TF_DOTX(a,b) \
    ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2])

static inline double
AA_TF_VDOT( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3] )
{
    return AA_TF_DOTX(a,b);
}

static inline float
AA_TF_VDOTF( const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3] )
{
    return AA_TF_DOTX(a,b);
}


AA_API double
aa_tf_vdot( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3] );

AA_API float
aa_tf_vdotf( const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3] );

#define AA_TF_CROSSX( a, b, c ) \
    (c)[0] =  (a)[1]*(b)[2] - (a)[2]*(b)[1]; \
    (c)[1] =  (a)[2]*(b)[0] - (a)[0]*(b)[2]; \
    (c)[2] =  (a)[0]*(b)[1] - (a)[1]*(b)[0]; \


static inline void
AA_TF_CROSS( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3], double c[AA_RESTRICT 3] )
{
    AA_TF_CROSSX(a,b,c);
}

static inline void
AA_TF_CROSSF( const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3], float c[AA_RESTRICT 3] )
{
    AA_TF_CROSSX(a,b,c);
}

AA_API void aa_tf_cross( const double a[AA_RESTRICT 3], const double b[AA_RESTRICT 3],
                         double c[AA_RESTRICT 3] ) ;


AA_API void aa_tf_crossf( const float a[AA_RESTRICT 3], const float b[AA_RESTRICT 3],
                          float c[AA_RESTRICT 3] ) ;

AA_API void aa_tf_vnormalize( double v[AA_RESTRICT 3] );


AA_API void aa_tf_vnormalizef( float v[AA_RESTRICT 3] );


/***************/
/* Quaternions */
/***************/

static inline double
AA_TF_QDOT( const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4] )
{
    return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3];
}

double
aa_tf_qdot( const double a[AA_RESTRICT 4], const double b[AA_RESTRICT 4] );

AA_API void aa_tf_qnormalize( double q[AA_RESTRICT 4] );


AA_API double aa_tf_qnorm( const double q[AA_RESTRICT 4] );

AA_API void aa_tf_qminimize( double q[AA_RESTRICT 4] );

AA_API void aa_tf_qminimize2( const double q[AA_RESTRICT 4], double qmin[AA_RESTRICT 4] );

AA_API void aa_tf_qnormalize2( const double q[AA_RESTRICT 4], double qnorm[AA_RESTRICT 4] );

AA_API void aa_tf_qconj( const double q[AA_RESTRICT 4],
                         double r[AA_RESTRICT 4] );


AA_API void aa_tf_qexp( const double q[AA_RESTRICT 4],
                        double r[AA_RESTRICT 4] );


AA_API void aa_tf_qln( const double q[AA_RESTRICT 4],
                       double r[AA_RESTRICT 4] );

AA_API void aa_tf_qduln( const double q[AA_RESTRICT 4],
                         const double dq[AA_RESTRICT 4],
                         double dln[AA_RESTRICT 3] );

AA_API void aa_tf_qdulnj( const double q[AA_RESTRICT 4],
                         const double dq[AA_RESTRICT 4],
                         double dln[AA_RESTRICT 3] );

AA_API void aa_tf_qdpexp( const double e[AA_RESTRICT 3],
                          const double de[AA_RESTRICT 3],
                          double dq[AA_RESTRICT 4] );

AA_API void aa_tf_qdpexpj( const double e[AA_RESTRICT 3],
                          const double de[AA_RESTRICT 3],
                          double dq[AA_RESTRICT 4] );

double aa_tf_qangle( const double q[AA_RESTRICT 4] );

/* Relative quaternion angles
 */
double aa_tf_qangle_rel( const double *q, const double *p );

double aa_tf_quhypangle2
( const double q[AA_RESTRICT 4], const double p[AA_RESTRICT 4] );

AA_API void aa_tf_qinv( const double q[AA_RESTRICT 4],
                        double r[AA_RESTRICT 4] );

AA_API void aa_tf_qadd( const double a[AA_RESTRICT 4],
                        const double b[AA_RESTRICT 4],
                        double c[AA_RESTRICT 4] );

AA_API void aa_tf_qsub( const double a[AA_RESTRICT 4],
                        const double b[AA_RESTRICT 4],
                        double c[AA_RESTRICT 4] );

AA_API void aa_tf_qmul( const double a[AA_RESTRICT 4],
                        const double b[AA_RESTRICT 4],
                        double c[AA_RESTRICT 4] );

AA_API void aa_tf_qmulnorm( const double a[AA_RESTRICT 4],
                            const double b[AA_RESTRICT 4],
                            double c[AA_RESTRICT 4] );

AA_API void aa_tf_qmul_qv( const double q[AA_RESTRICT 4],
                           const double v[AA_RESTRICT 3],
                           double c[AA_RESTRICT 4] );

AA_API void aa_tf_qmul_vq( const double v[AA_RESTRICT 3],
                           const double q[AA_RESTRICT 4],
                           double c[AA_RESTRICT 4] );

AA_API void aa_tf_qcmul( const double a[AA_RESTRICT 4],
                         const double b[AA_RESTRICT 4],
                         double c[AA_RESTRICT 4] );

AA_API void aa_tf_qmulc( const double a[AA_RESTRICT 4],
                         const double b[AA_RESTRICT 4],
                         double c[AA_RESTRICT 4] );


AA_API void
aa_tf_qrot1( const double q[AA_RESTRICT 4], double v[AA_RESTRICT 3] );

AA_API void aa_tf_qrot( const double q[AA_RESTRICT 4],
                        const double v[AA_RESTRICT 3],
                        double p[AA_RESTRICT 3] );

AA_API void aa_tf_qrel(const double q1[AA_RESTRICT 4],
                       const double q2[AA_RESTRICT 4],
                       double q_rel[AA_RESTRICT 4]);

AA_API void aa_tf_qslerp( double tau, const double a[AA_RESTRICT 4],
                          const double b[AA_RESTRICT 4],
                          double c[AA_RESTRICT 4] );


AA_API void aa_tf_qslerpalg( double tau, const double a[AA_RESTRICT 4],
                             const double b[AA_RESTRICT 4],
                             double c[AA_RESTRICT 4] );

AA_API void aa_tf_qslerpdiff( double tau, const double a[AA_RESTRICT 4],
                              const double b[AA_RESTRICT 4],
                              double c[AA_RESTRICT 4] );


AA_API void aa_tf_qslerpdiffalg( double tau, const double a[AA_RESTRICT 4],
                                 const double b[AA_RESTRICT 4],
                                 double c[AA_RESTRICT 4] );

AA_API void aa_tf_qslerpchaindiff( double u, double du,
                                   const double q1[AA_RESTRICT 4], const double dq1[AA_RESTRICT 4],
                                   const double q2[AA_RESTRICT 4], const double dq2[AA_RESTRICT 4],
                                   double q[AA_RESTRICT 4], double dq[AA_RESTRICT 4] );

AA_API void aa_tf_qslerp3diff( double u12, double du12,
                               const double q1[AA_RESTRICT 4], const double q2[AA_RESTRICT 4],
                               double u34, double du34,
                               const double q3[AA_RESTRICT 4], const double q4[AA_RESTRICT 4],
                               double u, double du,
                               double q[AA_RESTRICT 4], double dq[AA_RESTRICT 4] );

/* Quaternaion time derivate to angular velocity */
AA_API void aa_tf_qdiff2vel( const double q[AA_RESTRICT 4],
                             const double dq_dt[AA_RESTRICT 4],
                             double v[AA_RESTRICT 3] );

/* Angular velocity to quaternion time derivative */
AA_API void aa_tf_qvel2diff( const double q[AA_RESTRICT 4],
                             const double v[AA_RESTRICT 3],
                             double dq_dt[AA_RESTRICT 4] );


/* Convert rotation vector derivative to rotational velocity */
AA_API void aa_tf_rotvec_diff2vel( const double v[3], const double dv[3],
                                   double w[3] );

AA_API void aa_tf_qrk1( const double q0[AA_RESTRICT 4],
                           const double dq[AA_RESTRICT 4],
                           double dt,
                           double q1[AA_RESTRICT 4] );

AA_API void aa_tf_qvelrk1( const double q0[AA_RESTRICT 4],
                           const double v[AA_RESTRICT 3],
                           double dt,
                           double q1[AA_RESTRICT 4] );


AA_API void aa_tf_qvelrk4( const double q0[AA_RESTRICT 4],
                           const double v[AA_RESTRICT 3],
                           double dt,
                           double q1[AA_RESTRICT 4] );


AA_API void aa_tf_qsvel( const double q0[AA_RESTRICT 4],
                         const double v[AA_RESTRICT 3],
                         double dt,
                         double q1[AA_RESTRICT 4] );

AA_API void aa_tf_qsdiff( const double q0[AA_RESTRICT 4],
                          const double dq[AA_RESTRICT 4],
                          double dt,
                          double q1[AA_RESTRICT 4] );


AA_API void aa_tf_xangle2quat( double theta_x, double q[AA_RESTRICT 4] );
AA_API void aa_tf_yangle2quat( double theta_y, double q[AA_RESTRICT 4] );
AA_API void aa_tf_zangle2quat( double theta_z, double q[AA_RESTRICT 4] );

/* Construct matrix for Davenport's q-method */
AA_API void aa_tf_quat_davenport_matrix
( size_t n, const double *w, const double *q, size_t ldqq, double *M );

/* Weighted average quaternion using Davenport's q-method
 *
 * @param n number of quaternions
 * @param w weights
 * @param Q array of quaternions
 * @param ldq leading dimension of Q
 * @param y average quaternion
 */
AA_API void aa_tf_quat_davenport
( size_t n, const double *w, const double *Q, size_t ldq, double *y );



AA_API void aa_tf_qmatrix_l( const double *q, double *M, size_t ldm );

AA_API void aa_tf_qmatrix_r( const double *q, double *M, size_t ldm );


void aa_tf_qurand( double q[4] );

/*********/
/* Axang */
/*********/

AA_API void aa_tf_axang_make( double x, double y, double z, double theta,
                              double axang[AA_RESTRICT 4] );

AA_API void aa_tf_axang_permute( const double rv[AA_RESTRICT 4], int k,
                                 double rv_p[AA_RESTRICT 4] );

AA_API void aa_tf_rotvec_permute( const double rv[AA_RESTRICT 3], int k,
                                  double rv_p[AA_RESTRICT 3] );

AA_API void aa_tf_rotvec_near( const double rv[AA_RESTRICT 3],
                               const double rv_near[AA_RESTRICT 3],
                               double rv_p[AA_RESTRICT 3] );

/**************/
/* Quat-Trans */
/**************/

void aa_tf_qv_conj( const double q[4], const double v[3],
                    double qc[4], double vc[3] );


#define AA_TF_QUTR_Q 0
#define AA_TF_QUTR_V 4

void aa_tf_qutr2duqu( const double e[7], double s[8] );

void aa_tf_qutr2tfmat( const double e[7], double T[12] );

void aa_tf_tfmat2qutr( const double T[12], double e[7] );

void aa_tf_duqu2qutr( const double s[8], double e[7] );

void aa_tf_qutr_mul( const double a[7], const double b[7], double c[7] ) ;

AA_API void
aa_tf_qutr_tf( const double E[AA_RESTRICT 7], const double p0[AA_RESTRICT 3],
               double p1[AA_RESTRICT 3] );

void aa_tf_qutr_mulnorm( const double a[7], const double b[7], double c[7] ) ;

void aa_tf_qutr_conj( const double a[7], double c[7] ) ;

void aa_tf_qutr_mulc( const double a[7], const double b[7], double c[7] ) ;

void aa_tf_qutr_cmul( const double a[7], const double b[7], double c[7] ) ;

void aa_tf_qutr_diff2vel
( const double e[7], const double de[7], double dx[6] );

void aa_tf_qutr_vel2diff
( const double e[7], const double dx[6], double de[7] );

void aa_tf_qutr_svel
( const double e0[7], const double dx[6], double dt, double e1[7] );

void aa_tf_qutr_sdiff
( const double e0[7], const double de[7], double dt, double e1[7] );

void aa_tf_qutr_wavg
( size_t n, const double *w, const double *EE, size_t ldee, double *a );

void aa_tf_qutr_rand( double E[7] );

/***************/
/* Conversions */
/***************/

AA_API void aa_tf_quat2axang( const double q[AA_RESTRICT 4],
                              double axang[AA_RESTRICT 4] );


AA_API void aa_tf_axang2quat( const double axang[AA_RESTRICT 4],
                              double q[AA_RESTRICT 4] );

AA_API void aa_tf_axang2quat2( const double axis[AA_RESTRICT 3],
                               double angle,
                               double q[AA_RESTRICT 4] );


AA_API void
aa_tf_vecs2quat( const double u[AA_RESTRICT 3],
                 const double v[AA_RESTRICT 3],
                 double q[AA_RESTRICT 4] );


AA_API void aa_tf_axang2rotvec( const double axang[AA_RESTRICT 4],
                                double rotvec[AA_RESTRICT 3] );

AA_API void aa_tf_rotvec2axang( const double rotvec[AA_RESTRICT 3],
                                double axang[AA_RESTRICT 4] );

AA_API void aa_tf_rotvec2quat( const double rotvec[AA_RESTRICT 3],
                               double q[AA_RESTRICT 4] );
AA_API void aa_tf_quat2rotvec( const double q[AA_RESTRICT 4],
                               double rotvec[AA_RESTRICT 3] );


AA_API void aa_tf_quat2rotvec_near( const double q[AA_RESTRICT 4],
                                    const double rv_near[AA_RESTRICT 3],
                                    double rotvec[AA_RESTRICT 3] );

AA_API void aa_tf_quat2rotmat( const double quat[AA_RESTRICT 4],
                               double rotmat[AA_RESTRICT 9] );
AA_API void aa_tf_rotmat2quat( const double rotmat[AA_RESTRICT 9],
                               double quat[AA_RESTRICT 4] );

AA_API void aa_tf_rotmat2axang( const double R[AA_RESTRICT 9],
                                double ra[AA_RESTRICT 4] );
AA_API void aa_tf_rotmat2rotvec( const double R[AA_RESTRICT 9],
                                 double rv[AA_RESTRICT 3] );

AA_API void aa_tf_axang2rotmat( const double ra[AA_RESTRICT 4],
                                double R[AA_RESTRICT 9] );
AA_API void aa_tf_rotvec2rotmat( const double rv[AA_RESTRICT 3],
                                 double R[AA_RESTRICT 9] );


AA_API void aa_tf_qv2tfmat( const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3],
                           double T[AA_RESTRICT 12] ) ;

/* AA_API void aa_tf_tfv2tfq( const double vrv[AA_RESTRICT 6],  */
/*                            double x[AA_RESTRICT 3], double quat[AA_RESTRICT 4] ); */
/* AA_API void aa_tf_tfq2tfv( const double x[AA_RESTRICT 3], const double quat[AA_RESTRICT 4],  */
/*                            double vrv[AA_RESTRICT 6] ); */

AA_API void aa_tf_rotmat2eulerzyx( const double R[AA_RESTRICT 9],
                                   double e[AA_RESTRICT 3] );


AA_API void aa_tf_quat2eulerzyx( const double q[AA_RESTRICT 4],
                                 double e[AA_RESTRICT 3] );


#define AA_TF_DEF_EULER(letters)                                        \
    AA_API void                                                         \
    aa_tf_euler ## letters ## 2rotmat( double e1, double e2, double e3, \
                                       double R[AA_RESTRICT 9] );       \
    AA_API void                                                         \
    aa_tf_euler ## letters ## 2quat( double e1, double e2, double e3,   \
                                    double q[AA_RESTRICT 4] );

AA_TF_DEF_EULER( xyz )
AA_TF_DEF_EULER( xzy )

AA_TF_DEF_EULER( yxz )
AA_TF_DEF_EULER( yzx )

AA_TF_DEF_EULER( zyx )
AA_TF_DEF_EULER( zxy )

AA_TF_DEF_EULER( xyx )
AA_TF_DEF_EULER( xzx )

AA_TF_DEF_EULER( yxy )
AA_TF_DEF_EULER( yzy )

AA_TF_DEF_EULER( zxz )
AA_TF_DEF_EULER( zyz )

AA_API void aa_tf_xangle2rotmat( double theta_x, double R[AA_RESTRICT 9] );
AA_API void aa_tf_yangle2rotmat( double theta_y, double R[AA_RESTRICT 9] );
AA_API void aa_tf_zangle2rotmat( double theta_z, double R[AA_RESTRICT 9] );

AA_API void
aa_tf_rotmat_mzlook( const double eye[AA_RESTRICT 3],
                     const double target[AA_RESTRICT 3],
                     const double up[AA_RESTRICT 3],
                     double R[AA_RESTRICT 9] );


AA_API void
aa_tf_tfmat_mzlook( const double eye[AA_RESTRICT 3],
                    const double target[AA_RESTRICT 3],
                    const double up[AA_RESTRICT 3],
                    double T[AA_RESTRICT 12] );

AA_API void
aa_tf_qmzlook( const double eye[AA_RESTRICT 3],
               const double target[AA_RESTRICT 3],
               const double up[AA_RESTRICT 3],
               double q[AA_RESTRICT 4] );

AA_API void
aa_tf_qv_mzlook( const double eye[AA_RESTRICT 3],
                 const double target[AA_RESTRICT 3],
                 const double up[AA_RESTRICT 3],
                 double q[AA_RESTRICT 4],
                 double v[AA_RESTRICT 3] );

AA_API void
aa_tf_qutr_mzlook( const double eye[AA_RESTRICT 3],
                   const double target[AA_RESTRICT 3],
                   const double up[AA_RESTRICT 3],
                   double T[AA_RESTRICT 12] );


/* Dual Quaternions */

AA_API void aa_tf_duqu_add( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
                            double d3[AA_RESTRICT 8] );

AA_API void aa_tf_duqu_sub( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
                            double d3[AA_RESTRICT 8] );

AA_API void aa_tf_duqu_smul( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
                             double d3[AA_RESTRICT 8] );

AA_API void aa_tf_duqu_mul( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
                            double d3[AA_RESTRICT 8] );

AA_API void aa_tf_duqu_matrix_l( const double *q, double *M, size_t ldm );

AA_API void aa_tf_duqu_matrix_r( const double *q, double *M, size_t ldm );

AA_API void aa_tf_duqu_cmul( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
                             double d3[AA_RESTRICT 8] );

AA_API void aa_tf_duqu_mulc( const double d1[AA_RESTRICT 8], const double d2[AA_RESTRICT 8],
                             double d3[AA_RESTRICT 8] );

AA_API void aa_tf_duqu_conj( const double d[AA_RESTRICT 8], double dconj[AA_RESTRICT 8] );

AA_API void aa_tf_duqu_exp( const double d[AA_RESTRICT 8], double e[AA_RESTRICT 8] );

AA_API void aa_tf_duqu_ln( const double d[AA_RESTRICT 8], double e[AA_RESTRICT 8] );

AA_API void aa_tf_duqu_norm( const double d[AA_RESTRICT 8], double *nreal, double *ndual );

AA_API void aa_tf_duqu_normalize( double d[AA_RESTRICT 8] );

AA_API void aa_tf_duqu_minimize( double d[AA_RESTRICT 8] );

AA_API void aa_tf_tf_duqu( const double d[AA_RESTRICT 8], const double p0[AA_RESTRICT 3],
                        double p1[AA_RESTRICT 3]  );

AA_API void aa_tf_duqu_trans( const double d[AA_RESTRICT 8], double v[AA_RESTRICT 3] );

AA_API void aa_tf_duqu2tfmat( const double d[AA_RESTRICT 8], double T[AA_RESTRICT 12] );

AA_API void aa_tf_tfmat2duqu( const double T[AA_RESTRICT 12], double d[AA_RESTRICT 8] ) ;

AA_API void aa_tf_qv2duqu( const double q[AA_RESTRICT 4], const double v[AA_RESTRICT 3],
                           double d[AA_RESTRICT 8] ) ;

AA_API void aa_tf_xyz2duqu( double x, double y, double z,
                            double d[AA_RESTRICT 8] ) ;

AA_API void aa_tf_xxyz2duqu( double theta, double x, double y, double z,
                             double d[AA_RESTRICT 8] );

AA_API void aa_tf_yxyz2duqu( double theta, double x, double y, double z,
                             double d[AA_RESTRICT 8] );

AA_API void aa_tf_zxyz2duqu( double theta, double x, double y, double z,
                             double d[AA_RESTRICT 8] );

AA_API void aa_tf_duqu2qv( const double d[AA_RESTRICT 8],
                           double q[AA_RESTRICT 4], double v[AA_RESTRICT 3] ) ;

void aa_tf_tfmat2av( const double T[12], double q[AA_RESTRICT 4], double v[AA_RESTRICT 3] ) ;

AA_API void aa_tf_duqu_vel2twist( const double d[AA_RESTRICT 8], const double dx[AA_RESTRICT 6],
                                  double t[AA_RESTRICT 8] ) ;

AA_API void aa_tf_duqu_twist2vel( const double d[AA_RESTRICT 8], const double t[AA_RESTRICT 8],
                                  double dx[AA_RESTRICT 6] ) ;

AA_API void aa_tf_duqu_twist2diff( const double d[AA_RESTRICT 8], const double t[AA_RESTRICT 8],
                                  double dd[AA_RESTRICT 8] ) ;

AA_API void aa_tf_duqu_vel2diff( const double d[AA_RESTRICT 8], const double dx[AA_RESTRICT 6],
                                 double dd[AA_RESTRICT 8] ) ;

AA_API void aa_tf_duqu_diff2vel( const double d[AA_RESTRICT 8], const double dd[AA_RESTRICT 8],
                                 double dx[AA_RESTRICT 6] ) ;

AA_API void aa_tf_duqu_diff2twist( const double d[AA_RESTRICT 8], const double dd[AA_RESTRICT 8],
                                   double twist[AA_RESTRICT 8] ) ;

AA_API void aa_tf_duqu_stwist( const double d0[AA_RESTRICT 8], const double twist[AA_RESTRICT 8],
                               double dt, double d1[AA_RESTRICT 6] ) ;

AA_API void aa_tf_duqu_svel( const double d0[AA_RESTRICT 8], const double dx[AA_RESTRICT 6],
                             double dt, double d1[AA_RESTRICT 6] ) ;

AA_API void aa_tf_duqu_sdiff( const double d0[AA_RESTRICT 8], const double dd[AA_RESTRICT 8],
                              double dt, double d1[AA_RESTRICT 6] ) ;




/* Misc */

void aa_tf_relx_mean( size_t n, const double *R,
                      const double *X, size_t ldx,
                      const double *Y, size_t ldy,
                      double rel[3]);

void aa_tf_relx_median( size_t n, const double *R,
                        const double *X, size_t ldx,
                        const double *Y, size_t ldy,
                        double rel[3]);

#ifdef __cplusplus
}
#endif

#endif //AMINO_TF_H