/*****************************************************************************
 *
    xyzquat.c - source for all operations related to the xyzquat data type
    
    
    Overview:
    
    
    Notes:


    Revision History:

    Author	    	Date	  Comments
    ------	    	--------  ----------------------------
    Erik Erikson        06/26/92  Added q_xyz_quat_compose
    Stefan Gottschalk
    Russ Taylor
    Rich Holloway	01/25/91  Initial version


   Developed at the University of North Carolina at Chapel Hill, supported
   by the following grants/contracts:
   
     DARPA #DAEA18-90-C-0044
     ONR #N00014-86-K-0680
     NIH #5-R24-RR-02170
   
 *
 *****************************************************************************/

#include "quat.h"
#include "pdefs.h"

/*****************************************************************************
 *
   q_xyz_quat_invert - invert a vector/quaternion transformation pair 
 
    input:
    	- dest and source pointers
    
    output:
    	- src is inverted and put into dest
    
    notes:
    	- src and dest may be same
 *
 *****************************************************************************/

void
q_xyz_quat_invert(destPtr, srcPtr)
    
     q_xyz_quat_type 		*destPtr;
     q_xyz_quat_type 	*srcPtr;

{

/* invert rotation first  */
q_invert(destPtr->quat, srcPtr->quat);

/* vec = -vec	*/
q_vec_invert(destPtr->xyz, srcPtr->xyz);

/* rotate translation offsets into inverted system	*/
q_xform(destPtr->xyz, destPtr->quat, destPtr->xyz);


}	/* q_xyz_quat_invert */



/*****************************************************************************
 *
   q_row_matrix_to_xyz_quat - converts a row matrix to an xyz_quat
 
    input:  
    	- pointer to each
    
    output:
    	- new xyz_quat
    
    notes:
    	- each call to this function takes about .1 milliseconds on pxpl4
	    a VAX 3200 GPX, as of 4/11/91.
 *
 *****************************************************************************/

void
q_row_matrix_to_xyz_quat(xyzQuatPtr, rowMatrix)
    
    q_xyz_quat_type 	*xyzQuatPtr;
    q_matrix_type  rowMatrix;
{
    int	    i;

q_from_row_matrix(xyzQuatPtr->quat, rowMatrix);

for ( i = 0; i < 3; i++ )
    xyzQuatPtr->xyz[i] = rowMatrix[3][i];

}	/* q_row_matrix_to_xyz_quat */


/*****************************************************************************
 *
   q_xyz_quat_to_row_matrix - convert an xyz_quat to a row matrix
 
    input:
    	- pointer to each
    
    output:
    	- new row matrix
    
    notes:
    	- each call to this function takes about .14 milliseconds on pxpl4
	    a VAX 3200 GPX, as of 4/11/91.
 *
 *****************************************************************************/

void
q_xyz_quat_to_row_matrix(rowMatrix, xyzQuatPtr)
    
    q_matrix_type   		rowMatrix;
    q_xyz_quat_type 	*xyzQuatPtr;

{

    int	    i;


q_to_row_matrix(rowMatrix, xyzQuatPtr->quat);

for ( i = 0; i < 3; i++ )
    rowMatrix[3][i] = xyzQuatPtr->xyz[i];

}	/* q_xyz_quat_to_row_matrix */



/*****************************************************************************
 *
   qp_pmatrix_to_xyz_quat - converts a PPHIGS matrix to an xyz_quat
 
    input:  
    	- pointer to each
    
    output:
    	- new xyz_quat
 *
 *****************************************************************************/

void
qp_pmatrix_to_xyz_quat(xyzQuatPtr, pMatrix)
    
    q_xyz_quat_type 	*xyzQuatPtr;
    Q_MatrixType   	pMatrix;
{
    int	    i;

qp_from_matrix(xyzQuatPtr->quat, pMatrix);

for ( i = 0; i < 3; i++ )
    xyzQuatPtr->xyz[i] = pMatrix[i][3];

}	/* qp_pmatrix_to_xyz_quat */


/*****************************************************************************
 *
   qp_xyz_quat_to_pmatrix - convert an xyz_quat to a row matrix
 
    input:
    	- pointer to each
    
    output:
    	- new row matrix
 *
 *****************************************************************************/

void
qp_xyz_quat_to_pmatrix(pMatrix, xyzQuatPtr)
    
    Q_MatrixType   		pMatrix;
    q_xyz_quat_type 	*xyzQuatPtr;
{
    int	    i;

qp_to_matrix(pMatrix, xyzQuatPtr->quat);

for ( i = 0; i < 3; i++ )
    pMatrix[i][3] = xyzQuatPtr->xyz[i];

}	/* qp_xyz_quat_to_pmatrix */



/*****************************************************************************
 *
   q_xyz_quat_compose - compose q_xyz_quat_types CfromB and BfromA to 
                        get CfromA

    input:
        - pointers to q_xyz_quat_types CfromA, CfromB and BfromA

    output:
        - CfromA

    overview:
        - treat the BA q_xyz_quat_type as local, since it's closer to 
	    the point:

            newp = CB*BA*p

        - first xform the xlate part of BA into the CB system by rotating
            and scaling it

        - add the xformed local xlate to the unchanged global xlate

        - then compose the rotation parts by multiplying global*local
 *
 *****************************************************************************/

void
q_xyz_quat_compose(C_from_A_ptr, C_from_B_ptr, B_from_A_ptr)
q_xyz_quat_type    *C_from_A_ptr, *C_from_B_ptr, *B_from_A_ptr;
{
    q_vec_type rotated_BA_vec;

    /* rotate local xlate into global   */
    q_xform(rotated_BA_vec, C_from_B_ptr->quat, B_from_A_ptr->xyz);

    /* now add the xformed local vec to the unchanged global vec    */
    q_vec_add(C_from_A_ptr->xyz, C_from_B_ptr->xyz, rotated_BA_vec);

    /* compose the rotations    */
    /* CA_rotate = CB_rotate . BA_rotate */
    q_mult(C_from_A_ptr->quat, C_from_B_ptr->quat, B_from_A_ptr->quat);

    q_normalize(C_from_A_ptr->quat, C_from_A_ptr->quat);
} 	/* qp_xyz_quat_compose */