/*  quatext.c  (C) 1997 Mark A. Livingston */
/* Any non-commercial use of this code is permitted so long as */
/* the above copyright line appears in the file.  Please contact */
/* the author regarding permission for commercial use. */
#include <quat.h>
#include <assert.h>
#include "quatext.h"
#include "svd.h"

static int qx_verbose = 1;
/*===========================================================================*/
void qx_verbose_mode( int m )
{
    qx_verbose = m;
}
/*---------------------------------------------------------------------------*/
void qx_make_trans_matrix( q_matrix_type mat, q_vec_type vec )
     /* Makes a translation matrix with offsets stored in the vector. */
{
  mat[0][0] = mat[1][1] = mat[2][2] = mat[3][3] = 1.0;
  mat[0][1] = mat[0][2] = mat[0][3] = 0.0;
  mat[1][0] = mat[1][2] = mat[1][3] = 0.0;
  mat[2][0] = mat[2][1] = mat[2][3] = 0.0;
  mat[3][0] = vec[0];
  mat[3][1] = vec[1];
  mat[3][2] = vec[2];
}
/*---------------------------------------------------------------------------*/
void qx_orthonorm_matrix( q_matrix_type mat )
{
    q_vec_cross_product( mat[2], mat[0], mat[1] );
    q_vec_cross_product( mat[1], mat[2], mat[0] );
    q_vec_normalize( mat[0], mat[0] );
    q_vec_normalize( mat[1], mat[1] );
    q_vec_normalize( mat[2], mat[2] );
}
/*---------------------------------------------------------------------------*/
/* dst = src * mat, assumes 4th coord is 1 */
void qx_vec_mult_matrix( q_vec_type dest, q_vec_type src, q_matrix_type mat )
{
    q_vec_type copy;

    copy[0] = src[0]*mat[0][0] + src[1]*mat[1][0] + src[2]*mat[2][0]
    +                mat[3][0];
    copy[1] = src[0]*mat[0][1] + src[1]*mat[1][1] + src[2]*mat[2][1]
    +                mat[3][1];
    copy[2] = src[0]*mat[0][2] + src[1]*mat[1][2] + src[2]*mat[2][2]
    +                mat[3][2];

    q_vec_copy( dest, copy );
}
/*---------------------------------------------------------------------------*/
void qx_vec_mult_matrix_rot( q_vec_type dest, q_vec_type src,
			     q_matrix_type mat )
     /* Computes vec*matrix but uses only the rotation part of the matrix;
	ignores the translation part.  Useful for transforming direction
	vectors. */
{
    q_vec_type copy;

    copy[0] = src[0]*mat[0][0] + src[1]*mat[1][0] + src[2]*mat[2][0];
    copy[1] = src[0]*mat[0][1] + src[1]*mat[1][1] + src[2]*mat[2][1];
    copy[2] = src[0]*mat[0][2] + src[1]*mat[1][2] + src[2]*mat[2][2];

    q_vec_copy( dest, copy );
}
/*---------------------------------------------------------------------------*/
void qx_gl_mat_to_matrix( q_matrix_type mat, qgl_matrix_type gl_mat )
{
    int i, j;

    for( i = 0 ; i < 4 ; ++i ){
	for( j = 0 ; j < 4 ; ++j ){
	    mat[i][j] = (double)gl_mat[i][j];
	}
    }
    /*qx_orthonorm_matrix( mat );*/
}
/*---------------------------------------------------------------------------*/
void qx_gl_mat_from_matrix( qgl_matrix_type gl_mat, q_matrix_type mat )
{
    int i, j;

    for( i = 0 ; i < 4 ; ++i ){
	for( j = 0 ; j < 4 ; ++j ){
	    gl_mat[i][j] = (float)mat[i][j];
	}
    }
}
/*---------------------------------------------------------------------------*/
void qx_file_print_vec( FILE *fp, q_vec_type v )
{
    fprintf( fp, "%lf %lf %lf\n", v[Q_X], v[Q_Y], v[Q_Z] );
}
/*---------------------------------------------------------------------------*/
void qx_file_print_matrix( FILE *fp, q_matrix_type m )
{
    fprintf( fp, "%lf %lf %lf %lf\n", m[0][0], m[0][1], m[0][2], m[0][3] );
    fprintf( fp, "%lf %lf %lf %lf\n", m[1][0], m[1][1], m[1][2], m[1][3] );
    fprintf( fp, "%lf %lf %lf %lf\n", m[2][0], m[2][1], m[2][2], m[2][3] );
    fprintf( fp, "%lf %lf %lf %lf\n", m[3][0], m[3][1], m[3][2], m[3][3] );
}
/*---------------------------------------------------------------------------*/
int qx_file_read_vec( FILE *fp, q_vec_type v )
{
    return fscanf( fp, "%lf %lf %lf", &v[Q_X], &v[Q_Y], &v[Q_Z] );
}
/*---------------------------------------------------------------------------*/
int qx_file_read_matrix( FILE *fp, q_matrix_type m )
{
    int ret_val = 0;

    ret_val += fscanf( fp, "%lf %lf %lf %lf"
    , &m[0][0], &m[0][1], &m[0][2], &m[0][3] );
    ret_val += fscanf( fp, "%lf %lf %lf %lf"
    , &m[1][0], &m[1][1], &m[1][2], &m[1][3] );
    ret_val += fscanf( fp, "%lf %lf %lf %lf"
    , &m[2][0], &m[2][1], &m[2][2], &m[2][3] );
    ret_val += fscanf( fp, "%lf %lf %lf %lf"
    , &m[3][0], &m[3][1], &m[3][2], &m[3][3] );

    return ret_val;
}
/*---------------------------------------------------------------------------*/
/*
this code is adapted from quatlib
*/
void qx_invert_matrix( q_matrix_type invertedMatrix, q_matrix_type srcMatrix )
{
    int i, j;
    q_matrix_type copy;  /* temp copy of dest in case src == dest */

    /* invert translation offsets */
    for( i = 0; i < 3; i++ ){
	copy[3][i] = - srcMatrix[i][0] * srcMatrix[3][0]
	-              srcMatrix[i][1] * srcMatrix[3][1]
	-              srcMatrix[i][2] * srcMatrix[3][2];
    }

    /* transpose rotation part */
    for( i = 0; i < 3; i++ ){
	for ( j = 0; j < 3; j++ ){
	    copy[i][j] = srcMatrix[j][i];
	}
    }

    for( i = 0; i < 3; i++ ){
	for ( j = 0; j < 3; j++ ){
	    invertedMatrix[i][j] = copy[i][j];
	}
	invertedMatrix[3][i] = copy[3][i];
    }
    invertedMatrix[0][3] = 0.;
    invertedMatrix[1][3] = 0.;
    invertedMatrix[2][3] = 0.;
    invertedMatrix[3][3] = 1.;
}
/*---------------------------------------------------------------------------*/
void qx_invert_matrixGL( qgl_matrix_type invertedMatrix
                       , qgl_matrix_type srcMatrix )
{
    int i, j;
    qgl_matrix_type copy;  /* temp copy of dest in case src == dest */

    /* invert translation offsets */
    for( i = 0; i < 3; i++ ){
	copy[3][i] = - srcMatrix[i][0] * srcMatrix[3][0]
	-              srcMatrix[i][1] * srcMatrix[3][1]
	-              srcMatrix[i][2] * srcMatrix[3][2];
    }

    /* transpose rotation part */
    for( i = 0; i < 3; i++ ){
	for ( j = 0; j < 3; j++ ){
	    copy[i][j] = srcMatrix[j][i];
	}
    }

    for( i = 0; i < 3; i++ ){
	for ( j = 0; j < 3; j++ ){
	    invertedMatrix[i][j] = copy[i][j];
	}
	invertedMatrix[3][i] = copy[3][i];
    }
    invertedMatrix[0][3] = 0.;
    invertedMatrix[1][3] = 0.;
    invertedMatrix[2][3] = 0.;
    invertedMatrix[3][3] = 1.;
}
/*---------------------------------------------------------------------------*/
/*
Finds 3D rotation axis from 2 rigid body transforms created by rotating
around that axis.  The rotation axis is updated into axisPoint &
axisVect.
Order of xfa and xfb unimportant.  of course, axisVect may point into
one direction or another.
Idea and initial implementation by  Mike Bajura
Cleaned up, reworked and commented by Andrei State
*/

int qx_find_axis( q_xyz_quat_type xfaForg
,                 q_xyz_quat_type xfbForg
,                 q_vec_type axisPoint, q_vec_type axisVect )
{
    q_vec_type p0, p1, pMid, v01, vMidToOrig;
    q_type orgFxfa, xfbFxfa;
    double angle, dist01, distMidToOrig;

    q_invert(                      orgFxfa, xfaForg.quat );
    q_mult( xfbFxfa, xfbForg.quat, orgFxfa );

    /* now xfbFxfa represents rotation 0->1 */
    /*if( xfbFxfa[Q_W] < (double)0. ){*/
	/* angle bigger than 180 degrees */
	/*q_conjugate( xfbFxfa, xfbFxfa );*/
	/*xfbFxfa[Q_W] xfbxfbFxfa[Q_W];*/
    /*}*/

    if( qx_verbose ) {
      fprintf( stdout, "qx_find_axis(): cos(half_angle) = %lf\n", xfbFxfa[Q_W]);
      angle = 2. * acos( xfbFxfa[Q_W] );
      fprintf( stdout, "qx_find_axis(): angle = %lf degrees\n"
      ,                                         Q_RAD_TO_DEG( angle ) );
    }

    q_to_vec( axisVect, xfbFxfa ); q_vec_normalize( axisVect, axisVect );
    /* now axisVect is normalized rotation axis for positive rotation */
    /* from 0 into 1 (right-hand rule) */

    q_vec_copy( p0, xfaForg.xyz );
    q_vec_copy( p1, xfbForg.xyz );

    q_vec_subtract( v01, p1, p0 );
    dist01 = q_vec_magnitude( v01 );
    q_vec_normalize( v01, v01 );
    q_vec_cross_product( vMidToOrig, axisVect, v01 );

    if( qx_verbose ) {
      fprintf(stdout,"qx_find_axis(): length of vMidToOrig = %lf\n"
      , q_vec_magnitude( vMidToOrig ) );
    }

    /*q_vec_normalize( vMidToOrig, vMidToOrig );*/

    /* midpoint between p0 and p1: */
    q_vec_add( pMid, p1, p0 ); q_vec_scale( pMid, (double).5, pMid );

    distMidToOrig = .5 * dist01 * ( 1. / tan( .5 * angle ) );

    q_vec_scale( vMidToOrig, distMidToOrig, vMidToOrig );
    q_vec_add( axisPoint, pMid, vMidToOrig );

    if( qx_verbose ) {
      fprintf(stdout,"qx_find_axis(): axis point:  " ); q_vec_print( axisPoint );
      fprintf(stdout,"qx_find_axis(): axis vector: " ); q_vec_print( axisVect  );
      fflush( stdout );
    }
}
/*---------------------------------------------------------------------------*/
void qx_diff( q_type xfbFxfa, q_type xfaForg, q_type xfbForg )
{
    q_type  xfaForg_l, orgFxfa;
    double  cosAngle;

    q_copy(xfaForg_l, xfaForg);

    cosAngle = xfaForg_l[Q_X]*xfbForg[Q_X] + xfaForg_l[Q_Y]*xfbForg[Q_Y]
    +          xfaForg_l[Q_Z]*xfbForg[Q_Z] + xfaForg_l[Q_W]*xfbForg[Q_W];

    /* If the above dot product is negative, it's better to
     *  go between the negative of the initial and the final, so that
     *  we use the shorter path.
     */
    if( cosAngle < 0. ){
	q_conjugate( xfaForg_l, xfaForg_l );
	xfaForg_l[Q_W] = -xfaForg_l[Q_W];
    }
    q_invert(                 orgFxfa, xfaForg_l );
    q_mult( xfbFxfa, xfbForg, orgFxfa );
    q_normalize( xfbFxfa, xfbFxfa );
}
/*---------------------------------------------------------------------------*/
void qx_matrix_diff( double *angl, double *dist, q_matrix_type m1
,                                                q_matrix_type m2 )
{
    q_matrix_type m1i, m2i;
    q_xyz_quat_type qx1, qx2;
    q_vec_type v;
    q_type q;

    qx_invert_matrix( m1i, m1 );
    qx_invert_matrix( m2i, m2 );
    q_row_matrix_to_xyz_quat( &qx1, m1i );
    q_row_matrix_to_xyz_quat( &qx2, m2i );
    q_vec_subtract( v, qx1.xyz , qx2.xyz  );
    qx_diff(        q, qx1.quat, qx2.quat );
    *dist = q_vec_magnitude( v );
    *angl = Q_RAD_TO_DEG( 2. * acos( q[Q_W] ) );
}
/*---------------------------------------------------------------------------*/
/*
Spherical linear interpolation and extrapolation of unit quaternions.
t==0.: xfcForg==xfaForg, possibly negated
t==1.: xfcForg==xfbForg, possibly negated
t==.5: xfcForg halfway between xfaForg & xfbForg, on the shorter path.
t<0. and t>1. acceptable for extrapolation.
Should be more general than q_slerp(), but in fact it turns out that
q_slerp handles arguments outside the [0,1] range as well.

As t goes from 0 to 1, destQuat goes from xfaForg to xfbForg.
This routine should always return a point along the shorter
of the two paths connecting xfaForg & xfaForg.  That is why
xfcForg may be negated w/ respect to xfaForg.

src == dest should be ok, although that doesn't seem to make much
sense here.

qx_slerpx() and q_slerp() were compared using thousands of random
quaternions and values for t between -3000. and 3000., with no
difference in results.

Written by Andrei State
uses ideas by Gary Bishop and Mike Bajura and some code from quatlib
*/

void qx_slerpx( q_type xfcForg, q_type xfaForg, q_type xfbForg, double t )
{
    q_type  xfaForg_l, xfcFxfa, orgFxfa;
    double  angle, cosAngle;

    q_copy(xfaForg_l, xfaForg);

    cosAngle = xfaForg_l[Q_X]*xfbForg[Q_X] + xfaForg_l[Q_Y]*xfbForg[Q_Y]
    +          xfaForg_l[Q_Z]*xfbForg[Q_Z] + xfaForg_l[Q_W]*xfbForg[Q_W];

    /* If the above dot product is negative, it's better to
     *  go between the negative of the initial and the final, so that
     *  we use the shorter path.
     */
    if( cosAngle < 0. ){
	q_conjugate( xfaForg_l, xfaForg_l );
	xfaForg_l[Q_W] = -xfaForg_l[Q_W];
    }
    q_invert(                 orgFxfa, xfaForg_l );
    q_mult( xfcFxfa, xfbForg, orgFxfa );
    angle = 2. * acos( xfcFxfa[Q_W] );

 assert( angle >= -Q_PI && angle <= Q_PI );
 assert( angle >= 0. );

    angle *= t;
    q_make( xfcFxfa, xfcFxfa[Q_X], xfcFxfa[Q_Y], xfcFxfa[Q_Z], angle );
    q_mult( xfcForg, xfcFxfa, xfaForg_l );
}
/*---------------------------------------------------------------------------*/
void qx_xyz_quat_scale( q_xyz_quat_type *dst
,                       q_xyz_quat_type *src, double t )
{
    q_type q_id = Q_ID_QUAT;

    q_slerp(     dst->quat, q_id, src->quat, t );
    q_vec_scale( dst->xyz , t   , src->xyz );
}
/*---------------------------------------------------------------------------*/
/*
Compute determinant of 3x3 matrix defined by 3 column or row vectors.
*/
double qx_det_3_vec( q_vec_type vec1, q_vec_type vec2, q_vec_type vec3 )
{
    q_vec_type crossProd;

    q_vec_cross_product( crossProd, vec2, vec3 );
    return( q_vec_dot_product( vec1, crossProd ) );
}
/*---------------------------------------------------------------------------*/
/*
Attempt to compute intersection of 2 3d lines represented by
    pnt0 + parameter * vec0
    pnt1 + parameter * vec1
vec0 and vec1 are assumed to be normalized.
If the lines are (almost) parallel, returns 0, otherwise 1.
par0 and par1 are parameters for the intersection point or the nearest
points (if the lines don't intersect).
*/
int qx_intersect_2_3d_lines( q_vec_type pnt0, q_vec_type vec0
,                            q_vec_type pnt1, q_vec_type vec1
,                            double *par0, double *par1 )
{
    q_vec_type v01, v0xv1;
    double denom;

    q_vec_cross_product( v0xv1, vec0, vec1 );
    denom = q_vec_dot_product( v0xv1, v0xv1 );
    if( denom < Q_EPSILON ) return 0;

    q_vec_subtract( v01, pnt1, pnt0 );
    denom = (double)1. / denom;
    *par0 = qx_det_3_vec( v01, vec1, v0xv1 ) * denom;
    *par1 = qx_det_3_vec( v01, vec0, v0xv1 ) * denom;
    return 1;
}
/*---------------------------------------------------------------------------*/
/*
Attempt to compute intersection of 2 3d lines represented by
    pnt0 + parameter * vec0
    pnt1 + parameter * vec1
vec0 and vec1 are assumed to be normalized.
If the lines are (almost) parallel, returns 0, otherwise 1.
inters is the intersection point or a point halfway between the
nearest points if the lines don't intersect
*/
int qx_touch_2_3d_lines( q_vec_type pnt0, q_vec_type vec0
,                        q_vec_type pnt1, q_vec_type vec1
,                        q_vec_type inters, int distprt )
{
    double p0, p1;
    q_vec_type nearest_p0, nearest_p1, vec;
    int ret_val = qx_intersect_2_3d_lines( pnt0, vec0, pnt1, vec1, &p0, &p1 );

    if( !ret_val ){
	/*fprintf( stderr*/
	/*, "qx_touch_2_3d_lines(): the 2 lines are nearly parallel\n");*/
	/*fflush( stderr );*/
    }else{
	/* compute nearest points */
	q_vec_scale(                 nearest_p0, p0, vec0 );
	q_vec_add( nearest_p0, pnt0, nearest_p0 );
	q_vec_scale(                 nearest_p1, p1, vec1 );
	q_vec_add( nearest_p1, pnt1, nearest_p1 );

	q_vec_subtract( vec, nearest_p1, nearest_p0 );

	/* tell user how far apart the 2 nearest points are: */
	if( distprt && qx_verbose ){
	    fprintf( stderr
	    , "qx_touch_2_3d_lines(): distance between 2 nearest points: %lf\n"
	    ,                                      q_vec_magnitude( vec ) );
	    fflush( stderr );
	}

	/* compute point halfway between the nearest points: */
	q_vec_scale(       vec, (double).5, vec );
	q_vec_add( inters, vec, nearest_p0 );
    }

    return ret_val;
}
/*---------------------------------------------------------------------------*/
/*
Attempts to compute 4 3D coordinates for 3D calibration wireframe from
the x and y axes indicated by xAxPnt, xAxVec, yAxPnt, yAxVec.
The calibration wireframe will have 4 vertices:
    wf[0]: origin
    wf[1]: point on x axis
    wf[2]: point on y axis
    wf[3]: point on z axis
Points 1 through 3 are at coordinate size on each axis.
The angle 012 will not be a right angle if xAxVec & yAxVec
were not perpendicular to begin with.
Returns 1 if successful, 0 otherwise.
*/

int qx_make_3d_cal_wf( double size, q_vec_type xAxPnt, q_vec_type xAxVec
,                                   q_vec_type yAxPnt, q_vec_type yAxVec
,                      q_vec_type wf[] )
{
    q_vec_type xNearestPt, yNearestPt, vxy, zAxVec;
    double xPar, yPar;

    /* tell user what the angle between the 2 input axis vectors is; */
    /* it should be 90 degrees: */
    if( qx_verbose ) {
      fprintf( stderr
      , "qx_make_3d_cal_wf(): angle between the 2 axis vectors: %f degrees\n"
      , Q_RAD_TO_DEG( acos( q_vec_dot_product( xAxVec, yAxVec ) ) ) );
    }

    /*
    The 2 axes may not intersect, so we need to compute a point in the
    center of the line segment connecting the closest points between
    those axes.  That point will be our origin.
    */
    /* first get the 2 nearest points, which may be identical: */
    if( !qx_intersect_2_3d_lines( xAxPnt, xAxVec
    ,                             yAxPnt, yAxVec, &xPar, &yPar ) ){
      if( qx_verbose ) {
	fprintf( stderr
	, "qx_make_3d_cal_wf(): the 2 axes are nearly parallel");
      }
      return 0;
    }
    q_vec_scale(                   xNearestPt, xPar, xAxVec );
    q_vec_add( xNearestPt, xAxPnt, xNearestPt );
    q_vec_scale(                   yNearestPt, yPar, yAxVec );
    q_vec_add( yNearestPt, yAxPnt, yNearestPt );

    q_vec_subtract( vxy, yNearestPt, xNearestPt );

    /* tell user how far apart the 2 nearest points are: */
    if( qx_verbose ) {
      fprintf( stderr
      , "qx_make_3d_cal_wf(): distance between the 2 nearest axis points: %f\n"
      ,                                      q_vec_magnitude( vxy ) );
    }

    /* make a vector for z axis: */
    q_vec_cross_product(     zAxVec, xAxVec, yAxVec );
    q_vec_normalize( zAxVec, zAxVec );

    /* put origin halfway between the nearest points: */
    q_vec_scale(      vxy, (double).5, vxy );
    q_vec_add( wf[0], vxy, xNearestPt );

    q_vec_scale(      vxy, size, xAxVec );
    q_vec_add( wf[1], vxy, wf[0] );         /* x axis point */

    q_vec_scale(      vxy, size, yAxVec );
    q_vec_add( wf[2], vxy, wf[0] );         /* y axis point */

    q_vec_scale(      vxy, size, zAxVec );
    q_vec_add( wf[3], vxy, wf[0] );         /* z axis point */
}
/*---------------------------------------------------------------------------*/
/*
user friendly quaternion print.
" R [ A1 A2 ] " means rotate "R" deg, w.r.t an axis, which points "A1" deg
  in x-z coordinateand "A2" deg from y axis
*/
void qx_uprint( q_type q )
{
    double len, l2, w, angle, a1, a2;

    len   = sqrt( q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3] ) ;
    w     = q[3] / len;
    angle = 2 * acos( w ) * 180 / M_PI ;
    a1    = atan2( q[0] , q[2] ) * 180 / M_PI ;
    l2    = sqrt( q[0] * q[0] + q[2] * q[2] ) ;
    a2    = atan2( q[1] , l2 );

    printf("%f [ %f %f ]\n", angle, a1, a2 );
}
/*---------------------------------------------------------------------------*/
/*
XYZ/Quaternion print
*/
void qx_file_xyz_quat_print( FILE *fp, q_xyz_quat_type *xyzq )
{
    fprintf( fp, "{ %lf, %lf, %lf }, ",
	     xyzq->xyz[0], xyzq->xyz[1], xyzq->xyz[2] );
    fprintf( fp, "{ %lf, %lf, %lf, %lf }\n",
	     xyzq->quat[0], xyzq->quat[1], xyzq->quat[2], xyzq->quat[3] );
    return;
}
/*---------------------------------------------------------------------------*/
/*
routine for averaging position/quaternion readings in a consistent manner
*/
void qx_xyz_quat_average( q_xyz_quat_type *destQ, q_xyz_quat_type *q,
			  double *w, int n )
{
    int i;
    double totW = 0.0;
    q_xyz_quat_type sum;

    sum.xyz[Q_X] = sum.xyz[Q_Y] = sum.xyz[Q_Z] = 0.0;
    sum.quat[Q_X] = sum.quat[Q_Y] = sum.quat[Q_Z] = sum.quat[Q_W] = 0.0;

    for( i = 0; i < n; i++ ) {
      sum.xyz[Q_X] += w[i] * q[i].xyz[Q_X];
      sum.xyz[Q_Y] += w[i] * q[i].xyz[Q_Y];
      sum.xyz[Q_Z] += w[i] * q[i].xyz[Q_Z];

      sum.quat[Q_X] += w[i] * q[i].quat[Q_X];
      sum.quat[Q_Y] += w[i] * q[i].quat[Q_Y];
      sum.quat[Q_Z] += w[i] * q[i].quat[Q_Z];
      sum.quat[Q_W] += w[i] * q[i].quat[Q_W];

      totW += w[i];
    }

    destQ->xyz[Q_X] = sum.xyz[Q_X] / totW;
    destQ->xyz[Q_Y] = sum.xyz[Q_Y] / totW;
    destQ->xyz[Q_Z] = sum.xyz[Q_Z] / totW;

    destQ->quat[Q_X] = sum.quat[Q_X] / totW;
    destQ->quat[Q_Y] = sum.quat[Q_Y] / totW;
    destQ->quat[Q_Z] = sum.quat[Q_Z] / totW;
    destQ->quat[Q_W] = sum.quat[Q_W] / totW;
    q_normalize( destQ->quat, destQ->quat );

    return;
}
/*---------------------------------------------------------------------------*/
/*
some mathematical operations on matrices that quatlib doesn't (shouldn't?) have
*/
void qx_matrix_scale( q_matrix_type d, double f, q_matrix_type s )
{
    int i, j;

    for( i = 0; i < 4; i++ )
      for( j = 0; j < 4; j++ )
	d[i][j] = f * s[i][j];

    return;
}
void qx_matrix_add( q_matrix_type s, q_matrix_type m1, q_matrix_type m2 )
{
    int i, j;

    for( i = 0; i < 4; i++ )
      for( j = 0; j < 4; j++ )
	s[i][j] = m1[i][j] + m2[i][j];

    return;
}
/* This one might well belong in quatlib */
void qx_matrix_transpose( q_matrix_type t, q_matrix_type a )
{
    int i, j;

    for( i = 0; i < 4; i++ )
      for( j = 0; j < 4; j++ )
	t[i][j] = a[j][i];

    return;
}
/*---------------------------------------------------------------------------*/
/*
routine for averaging position/quaternion readings in a consistent manner
*/
void qx_xyz_quat_wgt_avg( q_xyz_quat_type *d, q_xyz_quat_type *q, double *w,
			  int n )
{
    int              i;
    double           totW;
    q_matrix_type    a, u, v, vt; /* SVD matrices */
    q_type           sigma; /* not really a quat, just a 4-vector */
    q_matrix_type    tmpM;
    q_vec_type       tmpV;

    /* first do the positions (vectors in Euclidean space) in the usual way */
    q_vec_scale( tmpV, w[0], q[0].xyz );
    q_vec_copy( d->xyz, tmpV );
    totW = w[0];
    for( i = 1; i < n; i++ ) {
      q_vec_scale( tmpV, w[i], q[i].xyz );
      q_vec_add( d->xyz, d->xyz, tmpV );
      totW += w[i];
    }
    q_vec_scale( d->xyz, 1.0 / totW, d->xyz );

    /* now do the quats (4-vectors in non-Euclidean space) with SVD method */
    /* convert the quats to rotation matrices, scale, and sum them */
    q_to_row_matrix( tmpM, q[0].quat );
    qx_matrix_scale( a, w[0], tmpM );
    for( i = 1; i < n; i++ ) {
      q_to_row_matrix( tmpM, q[i].quat );
      qx_matrix_scale( tmpM, w[i], tmpM );
      qx_matrix_add( a, a, tmpM );
    }
    /* compute the SVD of the summed matrix */
    call_svd( a, 4, 4, u, sigma, v );
    /* this SVD returns V, not V^t, which is the SVD factor */
    qx_matrix_transpose( vt, v );
    /* compute the average rotation matrix = U V */
    q_matrix_mult( tmpM, u, vt );
    /* convert the rotation to a quat */
    q_from_row_matrix( d->quat, tmpM );
    q_normalize( d->quat, d->quat );

    return;
}
/*---------------------------------------------------------------------------*/
/* plane equation from 3 points */
void qx_plane_from_3_points( plane p, q_vec_type *pt )
{
    q_vec_type v0, v1, normal;

    /* Step 1: find a normal to the plane */
    q_vec_subtract( v0, pt[1], pt[0] );
    q_vec_subtract( v1, pt[2], pt[0] );
    q_vec_cross_product( normal, v0, v1 );
    q_vec_normalize( normal, normal );

    /* Copy it into the right place */
    p[0] = normal[0];
    p[1] = normal[1];
    p[2] = normal[2];

    /* Step 2: find the offset of the plane from the origin */
    p[3] = q_vec_dot_product( normal, pt[0] );

    return;
}
/*---------------------------------------------------------------------------*/
/* angle between two planes */
double qx_angle_btw_planes( plane p0, plane p1 )
{
    double c;
    q_vec_type n0, n1;

    /* This is just the angle between the two normals */
    q_vec_set( n0, p0[0], p0[1], p0[2] );
    q_vec_set( n1, p1[0], p1[1], p1[2] );
    c = q_vec_dot_product( n0, n1 ) /
          ( q_vec_magnitude( n0 ) * q_vec_magnitude( n1 ) );
    return acos( c );
}
/*---------------------------------------------------------------------------*/
/* point to plane distance */
double qx_point_plane_dist( q_vec_type pt, plane p )
{
    q_vec_type q, q_pt, n;

    /* Find a point q in plane */
    q_vec_set( q, 0.0, p[3] / p[1], 0.0 );

    /* Project the vector (pt -> q) onto plane normal, return length */
    q_vec_subtract( q_pt, q, pt );
    q_vec_set( n, p[0], p[1], p[2] );
    return q_vec_dot_product( q_pt, n ) / q_vec_magnitude( n );
}
/*---------------------------------------------------------------------------*/
/* project point onto plane */
void qx_project_point_to_plane( q_vec_type newp, q_vec_type p, plane r )
{
    q_vec_type q, q_p, n;
    double dot, dist;

    /*
    ** First task: find if we want to travel in the direction of the normal
    ** or in the opposite direction.  Do this by finding a point in the plane
    ** to form a vector from the plane to the current point, then dot this
    ** vector with the normal.  The sign of the dot product is all we need.
    */
    q_vec_set( q, 0.0, r[3] / r[1], 0.0 );
    q_vec_subtract( q_p, p, q );
    q_vec_set( n, r[0], r[1], r[2] );
    dot = q_vec_dot_product( q_p, n );

    /* Find how far to move, then go in the correct direction */
    dist = qx_point_plane_dist( p, r );
    q_vec_scale( n, dist, n ); /* +/- movement vector */
    if( dot < 0.0 ) { /* normal points away from us, move in that direction */
      q_vec_add( newp, p, n );
    } else { /* normal points toward us, move in the other direction */
      q_vec_subtract( newp, p, n );
    }

    return;
}
/*===========================================================================*/