anim.c

Go to the documentation of this file.

/*                          A N I M . C
 * BRL-CAD
 *
 * Copyright (c) 1993-2006 United States Government as represented by
 * the U.S. Army Research Laboratory.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation; either version 2 of
 * the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Library General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this file; see the file named COPYING for more
 * information.
 */
/** \addtogroup anim */
/*@{*/
/** @file anim.c
 * @brief
 *      Routines useful in animation programs.
 *
 *  @author
 *      Carl J. Nuzman
 *
 *  @par Source -
 *      The U. S. Army Research Laboratory
 *@n      Aberdeen Proving Ground, Maryland  21005-5068  USA
 *
 *
 *
 *
 ***********************************************************************
 *
 * @par This file includes the following routines:
 * @li anim_v_permute() apply camera animation permutation
 * @li anim_v_unpermute()       undo camera animation permutation
 * @li anim_tran()              transpose matrix in place
 * @li anim_mat2zyx()           extract angles from rotation matrix
 * @li anim_mat2ypr()           extract yaw,pitch,roll from rotation matrix
 * @li anim_mat2quat()          extract quaternion from rotation matrix
 * @li anim_ypr2mat()           create rotation matrix from ypr, etc.
 * @li anim_ypr2vmat()
 * @li anim_y_p_r2mat()
 * @li anim_dy_p_r2mat()
 * @li anim_dy_p_r2vmat()
 * @li anim_x_y_z2mat()
 * @li anim_dx_y_z2mat()
 * @li anim_z_y_x2mat()
 * @li anim_dz_y_x2mat()
 * @li anim_quat2mat()
 * @li anim_dir2mat()           create rotation matrix from direction
 * @li anim_dirn2mat()          create rot matrix from dir and normal
 * @li anim_steer_mat() automatic steering
 * @li anim_add_trans() add pre- and post- translation to matrix
 * @li anim_rotatez()           rotate vector about z-axis
 * @li anim_mat_print() print matrix with optional semi-colon
 * @li anim_view_rev()          reverse view matrix
 */


#include "common.h"

#include <stdio.h>
#include <math.h>
#include "machine.h"
#include "bu.h"
#include "vmath.h"
#include "bn.h"
#include "anim.h"

#ifndef M_PI
#define M_PI    3.14159265358979323846
#endif

#define NORMAL          0
#define ERROR1          1
#define ERROR2          2


/**
 *  Orientation conventions:
 * The default object orientation is facing the positive x-axis, with
 * the positive y-axis to the object's left and the positive z-axis above
 * the object.
 * The default view orientation for rt and mged is facing the negative z-axis,
 * with the negative x-axis leading to the left and the positive y-axis
 * going upwards.
 */

/**
 *  ANIM_V_PERMUTE 
 * @brief Pre-multiply a rotation matrix by a matrix
 * which maps the z-axis to the negative x-axis, the y-axis to the
 * z-axis and the x-axis to the negative y-axis.
 *
 * This has the effect of twisting an object in the default view orientation
 * into the default object orientation before applying the matrix.
 * Given a matrix designed to operate on an object, yield a matrix which
 * operates on the view.
 */
void anim_v_permute(mat_t m)
{
        int i;
        fastf_t store;

        for (i=0; i<9; i+=4){
                store = m[i];
                m[i] = -m[i+1];
                m[i+1] = m[i+2];
                m[i+2] = -store;
        }
}

/**
 *  ANIM_V_UNPERMUTE 
 * @brief Undo the mapping done by anim_v_permute().
 *
 * This has the effect of twisting an object in the default object
 * orientation into the default view orientation before applying the
 * matrix.
 * Given a matrix designed to operate on the view, yield a matrix which
 * operates on an object.
 */
void anim_v_unpermute(mat_t m)
{
        int i;
        fastf_t store;

        for (i=0; i<9; i+=4){
                store = m[i+2];
                m[i+2] = m[i+1];
                m[i+1] = -m[i];
                m[i] = -store;
        }
}

/** @brief Transpose matrix in place */
void anim_tran(mat_t m)
{
        int i;
        fastf_t store;
#if 1
        /* The sun4's complain about no automatic aggregate initialization,
         * so we'll do it another way. :-(
         */
        int src[6];
        int dst[6];

        src[0] = 1;
        src[1] = 2;
        src[2] = 3;
        src[3] = 6;
        src[4] = 7;
        src[5] = 11;

        dst[0] = 4;
        dst[1] = 8;
        dst[2] = 12;
        dst[3] = 9;
        dst[4] = 13;
        dst[5] = 14;
#else
        int src[] = { 1, 2, 3, 6, 7, 11 };
        int dst[] = { 4, 8, 12, 9, 13, 14};
#endif

        for (i=0; i<6; i++) {
                store = m[dst[i]];
                m[dst[i]] = m[src[i]];
                m[src[i]] = store;
        }
}

/***************************************
 *ANIM_MAT2* - Conversions from matrices
 ***************************************/

/** ANIM_MAT2ZYX 
 * @brief
 *  Convert the rotational part of a 4x4 transformation matrix
 * to zyx form, that is to say, rotations carried out in the order z, y,
 * and then x. The angles are stored in radians as a vector in the order
 * x,y,z. A return value of ERROR1 means that arbitrary assumptions were
 * necessary. ERROR2 means that the conversion failed.
 */
int anim_mat2zyx(const mat_t viewrot, vect_t angle)
{
        int i, return_value, id_x, id_z;
        fastf_t sin_x, sin_z, cos_x, cos_z, big_x, big_z;
        static fastf_t previous[3];

        if ((viewrot[1]==0.0) && (viewrot[0]==0.0)){
                return_value = ERROR1;
                angle[0] = 0.0;
                angle[2] = atan2(viewrot[4],viewrot[5]);
                /*bu_log("Warning: x arbitrarily set to 0.0; z set to %f.\n",angle[2]);*/
        }
        else {
                return_value = NORMAL;
                angle[2] = atan2(-viewrot[1],viewrot[0]);
                angle[0] = atan2(-viewrot[6],viewrot[10]);
        }

        sin_x = sin(angle[0]);
        sin_z = sin(angle[2]);
        cos_x = cos(angle[0]);
        cos_z = cos(angle[2]);

        /* in principle, we can use the sin_x or cos_x with sin_z or cos_z to
         * figure out angle[1], as long as they are non-zero. To avoid
         * ill-conditioning effects, we choose the two that are greatest in
         * absolute value
         */

        id_z  = (fabs(sin_z) > fabs(cos_z)) ? 1 : 0;
        big_z = id_z ? sin_z : cos_z;
        id_x  = (fabs(sin_x) > fabs(cos_x)) ? 1 : 0;
        big_x = id_x ? sin_x : cos_x;

        if (fabs(big_x*big_z) < VDIVIDE_TOL){ /* this should be impossible*/
                /* unable to calculate pitch*/
                return(ERROR2);
        }
        else if ( id_x && (!id_z) )
                angle[1]=atan2( (viewrot[4] - cos_x*sin_z)/(sin_x*cos_z), -viewrot[6]/sin_x);
        else if ( (!id_x) && (!id_z) )
                angle[1]=atan2( (-viewrot[8] + sin_x*sin_z)/(cos_x*cos_z), viewrot[0]/cos_z);
        else if ( id_x && id_z )
                angle[1]=atan2( (-viewrot[5] + cos_x*cos_z)/(sin_x*sin_z), -viewrot[1]/sin_z);
        else if ( (!id_x) && id_z )
                angle[1]=atan2( (viewrot[9] - sin_x*cos_z)/(cos_x*sin_z), viewrot[10]/cos_x);


        /* assume the smallest possible arc-length from frame to frame */
        for (i=0; i<3; i++) {
                while ((angle[i] - previous[i]) > M_PI)
                        angle[i] -= (2.0*M_PI);
                while ((previous[i] - angle[i]) > M_PI)
                        angle[i] += (2.0*M_PI);
                previous[i] = angle[i];
        }

        return(return_value);
}

/** ANIM_MAT2YPR 
 *@brief
 * Convert the rotational part of a 4x4 transformation matrix
 * to yaw-pitch-roll form, that is to say, +roll degrees about the x-axis,
 * -pitch degrees about the y-axis, and +yaw degrees about the
 * z-axis. The angles are stored in radians as a vector in the order y,p,r.
 * A return of ERROR1 means that arbitrary assumptions were necessary.
 * ERROR2 means that the conversion failed.
 */
int anim_mat2ypr(mat_t viewrot, vect_t angle)
{
        int i, return_value, id_y, id_r;
        fastf_t sin_y, sin_r, cos_y, cos_r, big_y, big_r;
        static fastf_t prev_angle[3];

        if ((viewrot[9]==0.0) && (viewrot[10]==0.0)){
                return_value = ERROR1;
                angle[2] = 0.0;
                angle[0] = atan2(-viewrot[1],viewrot[5]);
                /*bu_log("Warning: roll arbitrarily set to 0.0; yaw set to %f radians.\n",angle[0]);*/
        }
        else {
                return_value = NORMAL;
                angle[0] = atan2(viewrot[4],viewrot[0]);
                angle[2] = atan2(viewrot[9],viewrot[10]);
        }

        sin_y = sin(angle[0]);
        sin_r = sin(angle[2]);
        cos_y = cos(angle[0]);
        cos_r = cos(angle[2]);

        /* in principle, we can use sin_y or cos_y with sin_r or cos_r to
         * figure out angle[1], as long as they are non-zero. To avoid
         * ill-conditioning effects, we choose the two that are greatest in
         * absolute value
         */

        id_y  = (fabs(sin_y) > fabs(cos_y)) ? 1 : 0;
        big_y = id_y ? sin_y : cos_y;
        id_r  = (fabs(sin_r) > fabs(cos_r)) ? 1 : 0;
        big_r = id_r ? sin_r : cos_r;

        if (fabs(big_y*big_r) < VDIVIDE_TOL){ /* this should not happen */
                /* unable to calculate pitch*/
                return(ERROR2);
        }
        else if ( (!id_y) && id_r )
                angle[1] = atan2( -(viewrot[1]+sin_y*cos_r)/(cos_y*sin_r),viewrot[9]/sin_r);
        else if ( id_y && (!id_r) )
                angle[1] = atan2( -(viewrot[6]+cos_y*sin_r)/(sin_y*cos_r),viewrot[10]/cos_r);
        else if ( id_y && id_r )
                angle[1] = atan2( -(viewrot[5]-cos_y*cos_r)/(sin_y*sin_r),viewrot[4]/sin_y);
        else if ( (!id_y) && (!id_r) )
                angle[1] = atan2( -(viewrot[2]-sin_y*sin_r)/(cos_y*cos_r),viewrot[0]/cos_y);


        /* assume the smallest possible arc-length from frame to frame */
        for (i=0; i<3; i++) {
                while ((angle[i] - prev_angle[i]) > M_PI)
                        angle[i] -= (2.0*M_PI);
                while ((prev_angle[i] - angle[i]) > M_PI)
                        angle[i] += (2.0*M_PI);
                prev_angle[i] = angle[i];
        }

        return(return_value);
}

/** ANIM_MAT2QUAT 
 * @brief
 * This interprets the rotational part of a 4x4 transformation
 *  matrix in terms of unit quaternions. The result is stored as a vector in
 * the order x,y,z,w.
 * The algorithm is from Ken Shoemake, Animating Rotation with Quaternion
 * Curves, 1985 SIGGraph Conference Proceeding, p.245.
 */
int anim_mat2quat(quat_t quat, const mat_t viewrot)
{
        int i;
        fastf_t qdiff[4], square, mag1, mag2;
        static fastf_t prev_quat[4];

        square = 0.25 * (1 + viewrot[0] + viewrot[5] + viewrot[10]);
        if ( square != 0.0 ) {
                quat[W] = sqrt(square);
                quat[X] = 0.25 * (viewrot[9] - viewrot[6])/ quat[W];
                quat[Y] = 0.25 * (viewrot[2] - viewrot[8])/ quat[W];
                quat[Z] = 0.25 * (viewrot[4] - viewrot[1])/ quat[W];
        }
        else {
                quat[W] = 0.0;
                square = -0.5 * (viewrot[5] + viewrot[10]);
                if (square != 0.0 ) {
                        quat[X] = sqrt(square);
                        quat[Y] = 0.5 * viewrot[4] / quat[X];
                        quat[Z] = 0.5 * viewrot[8] / quat[X];
                }
                else {
                        quat[X] = 0.0;
                        square = 0.5 * (1 - viewrot[10]);
                        if (square != 0.0){
                                quat[Y] = sqrt(square);
                                quat[Z] = 0.5 * viewrot[9]/ quat[Y];
                        }
                        else {
                                quat[Y] = 0.0;
                                quat[Z] = 1.0;
                        }
                }
        }

        /* quaternions on opposite sides of a four-dimensional sphere
                are equivalent. Take the quaternion closest to the previous
                one */

        for (i=0; i<4; i++)
                qdiff[i] = prev_quat[i] - quat[i];
        mag1 = QMAGSQ(qdiff);
        for (i=0; i<4; i++)
                qdiff[i] = prev_quat[i] + quat[i];
        mag2 = QMAGSQ(qdiff);

        for (i=0; i<4; i++) {
                if (mag1 > mag2)  /* inverse of quat would be closer */
                        quat[i] = -quat[i];
                prev_quat[i] = quat[i];
        }

        return(1);
}

/***************************************
 *ANIM_*2MAT - Conversions to matrices
 ***************************************/

/** ANIM_YPR2MAT 
 * @brief Create a premultiplication rotation matrix to turn the front
 * of an object (its x-axis) to the given yaw, pitch, and roll,
 * which is stored in radians in the vector a.
 */
void anim_ypr2mat(mat_t m, const vect_t a)
{
        fastf_t cos_y,cos_p,cos_r,sin_y,sin_p,sin_r;

        cos_y = cos(a[0]);
        cos_p = cos(a[1]);
        cos_r = cos(a[2]);
        sin_y = sin(a[0]);
        sin_p = sin(a[1]);
        sin_r = sin(a[2]);

        m[0] =   cos_y*cos_p;
        m[1] =   -cos_y*sin_p*sin_r-sin_y*cos_r;
        m[2] =   -cos_y*sin_p*cos_r+sin_y*sin_r;
        m[3] =  0;
        m[4] =  sin_y*cos_p;
        m[5] =  -sin_y*sin_p*sin_r+cos_y*cos_r;
        m[6] =  -sin_y*sin_p*cos_r-cos_y*sin_r;
        m[7] =  0;
        m[8]=   sin_p;
        m[9] =  cos_p*sin_r;
        m[10] = cos_p*cos_r;
        m[11] = 0.0;
        m[12] = 0.0;
        m[13] = 0.0;
        m[14] = 0.0;
        m[15] = 1.0;
}

/** ANIM_YPR2VMAT 
 * @brief Create a post-multiplication rotation matrix ,which could
 * be used to move the virtual camera to the given yaw, pitch,
 * and roll,  which are stored in radians in the given vector a. The
 * following are equivalent sets of commands:
 *      ypr2vmat(matrix,a);
 *              or
 *      ypr2mat(matrix,a);
 *      v_permute(matrix);
 *      transpose(matrix;
 */
void anim_ypr2vmat(mat_t m, const vect_t a)
{
        fastf_t cos_y,cos_p,cos_r,sin_y,sin_p,sin_r;

        cos_y = cos(a[0]);
        cos_p = cos(a[1]);
        cos_r = cos(a[2]);
        sin_y = sin(a[0]);
        sin_p = sin(a[1]);
        sin_r = sin(a[2]);

        m[0] =    -cos_y*sin_p*sin_r-sin_y*cos_r;
        m[1] =    -sin_y*sin_p*sin_r+cos_y*cos_r;
        m[2] =     cos_p*sin_r;
        m[3] =     0;
        m[4] =    -cos_y*sin_p*cos_r+sin_y*sin_r;
        m[5] =    -sin_y*sin_p*cos_r-cos_y*sin_r;
        m[6] =     cos_p*cos_r;
        m[7] =     0;
        m[8] =     cos_y*cos_p;
        m[9] =     sin_y*cos_p;
        m[10] =    sin_p;
        m[11] =    0.0;
        m[12] =    0.0;
        m[13] =    0.0;
        m[14] =    0.0;
        m[15] =    1.0;
}

/** ANIM_Y_P_R2MAT 
 * @brief
 * Make matrix to rotate an object to the given yaw,
 * pitch, and roll. (Specified in radians.)
 */
void anim_y_p_r2mat(mat_t m, double y, double p, double r)
{
        fastf_t cos_y = cos(y);
        fastf_t sin_y = sin(y);
        fastf_t cos_p = cos(p);
        fastf_t sin_p = sin(p);
        fastf_t cos_r = cos(r);
        fastf_t sin_r = sin(r);

        m[0] = cos_y*cos_p;
        m[1] = -cos_y*sin_p*sin_r-sin_y*cos_r;
        m[2] = -cos_y*sin_p*cos_r+sin_y*sin_r;
        m[4] = sin_y*cos_p;
        m[5] = -sin_y*sin_p*sin_r+cos_y*cos_r;
        m[6] = -sin_y*sin_p*cos_r-cos_y*sin_r;
        m[8]= sin_p;
        m[9] = cos_p*sin_r;
        m[10] = cos_p*cos_r;
        m[3]=m[7]=m[11]=m[12]=m[13]=m[14]=0;
        m[15]=1;
}




/** ANIM_DY_P_R2MAT
 * @brief Make matrix to rotate an object to the given yaw,
 * pitch, and roll. (Specified in degrees.)
 */
void anim_dy_p_r2mat(mat_t m, double y, double p, double r)
{
        fastf_t radian_yaw = y*(M_PI*0.0055555555556);
        fastf_t radian_pitch = p*(M_PI*0.0055555555556);
        fastf_t radian_roll = r*(M_PI*0.0055555555556);

        fastf_t cos_y = cos(radian_yaw);
        fastf_t sin_y = sin(radian_yaw);
        fastf_t cos_p = cos(radian_pitch);
        fastf_t sin_p = sin(radian_pitch);
        fastf_t cos_r = cos(radian_roll);
        fastf_t sin_r = sin(radian_roll);

        m[0] = cos_y*cos_p;
        m[1] = -cos_y*sin_p*sin_r-sin_y*cos_r;
        m[2] = -cos_y*sin_p*cos_r+sin_y*sin_r;
        m[4] = sin_y*cos_p;
        m[5] = -sin_y*sin_p*sin_r+cos_y*cos_r;
        m[6] = -sin_y*sin_p*cos_r-cos_y*sin_r;
        m[8]= sin_p;
        m[9] = cos_p*sin_r;
        m[10] = cos_p*cos_r;
        m[3]=m[7]=m[11]=m[12]=m[13]=m[14]=0;
        m[15]=1;
}


/** ANIM_DY_P_R2VMAT
 * @brief  Make a view rotation matrix, given desired yaw, pitch
 * and roll. (Note that the matrix is a permutation of the object rotation
 * matrix).
 */
void anim_dy_p_r2vmat(mat_t m, double yaw, double pch, double rll)
{

        float ryaw = yaw*(M_PI*0.0055555555556);
        float rpch = pch*(M_PI*0.0055555555556);
        float rrll = rll*(M_PI*0.0055555555556);

        float cos_y = cos(ryaw);
        float sin_y = sin(ryaw);
        float cos_p = cos(rpch);
        float sin_p = sin(rpch);
        float cos_r = cos(rrll);
        float sin_r = sin(rrll);

        m[0] = -cos_y*sin_p*sin_r-sin_y*cos_r;
        m[1] = -sin_y*sin_p*sin_r+cos_y*cos_r;
        m[2] = cos_p*sin_r;
        m[4] = -cos_y*sin_p*cos_r+sin_y*sin_r;
        m[5] = -sin_y*sin_p*cos_r-cos_y*sin_r;
        m[6] = cos_p*cos_r;
        m[8] = cos_y*cos_p;
        m[9] = sin_y*cos_p;
        m[10]= sin_p;
        m[3]=m[7]=m[11]=0;
        m[12]=m[13]=m[14]=0;
        m[15]=1;

}

/** ANIM_X_Y_Z2MAT 
 * @brief Make a rotation matrix corresponding to a rotation of
 * "x" radians about the x-axis, "y" radians about the y-axis, and
 * then "z" radians about the z-axis.
 */
void anim_x_y_z2mat(mat_t m, double x, double y, double z)
{
        fastf_t cosx = cos(x);
        fastf_t sinx = sin(x);
        fastf_t cosy = cos(y);
        fastf_t siny = sin(y);
        fastf_t cosz = cos(z);
        fastf_t sinz = sin(z);

        m[0] = cosz*cosy;
        m[1] = cosz*siny*sinx-sinz*cosx;
        m[2] = cosz*siny*cosx+sinz*sinx;
        m[4] = sinz*cosy;
        m[5] = sinz*siny*sinx+cosz*cosx;
        m[6] = sinz*siny*cosx-cosz*sinx;
        m[8] = -siny;
        m[9] = cosy*sinx;
        m[10] = cosy*cosx;
        m[3]=m[7]=m[11]=m[12]=m[13]=m[14]=0;
        m[15]=1;
}



/** ANIM_DX_Y_Z2MAT 
 * @brief Make a rotation matrix corresponding to a rotation of
 * "x" degrees about the x-axis, "y" degrees about the y-axis, and
 * then "z" degrees about the z-axis.
 */
void anim_dx_y_z2mat(mat_t m, double x, double y, double z)
{
        fastf_t cosx,cosy,cosz,sinx,siny,sinz;

        x *= (M_PI*0.0055555555556);
        y *= (M_PI*0.0055555555556);
        z *= (M_PI*0.0055555555556);

        cosx = cos(x);
        sinx = sin(x);
        cosy = cos(y);
        siny = sin(y);
        cosz = cos(z);
        sinz = sin(z);

        m[0] = cosz*cosy;
        m[1] = cosz*siny*sinx-sinz*cosx;
        m[2] = cosz*siny*cosx+sinz*sinx;
        m[4] = sinz*cosy;
        m[5] = sinz*siny*sinx+cosz*cosx;
        m[6] = sinz*siny*cosx-cosz*sinx;
        m[8] = -siny;
        m[9] = cosy*sinx;
        m[10] = cosy*cosx;
        m[3]=m[7]=m[11]=m[12]=m[13]=m[14]=0.0;
        m[15]=1.0;
}

/* ANIM_ZYX2MAT @brief Make a rotation matrix corresponding to a rotation of
 * "z" radians about the z-axis, "y" radians about the y-axis, and
 * then "x" radians about the x-axis.
 */
void anim_zyx2mat(mat_t m, const vect_t a)
{
        fastf_t cosX,cosY,cosZ,sinX,sinY,sinZ;

        cosX = cos(a[0]);
        cosY = cos(a[1]);
        cosZ = cos(a[2]);
        sinX = sin(a[0]);
        sinY = sin(a[1]);
        sinZ = sin(a[2]);

        m[0] =     cosY*cosZ;
        m[1] =    -cosY*sinZ;
        m[2] =     sinY;
        m[3] =     0;
        m[4] =     cosX*sinZ + sinX*sinY*cosZ;
        m[5] =     cosX*cosZ - sinX*sinY*sinZ;
        m[6] =    -sinX*cosY;
        m[7] =     0;
        m[8] =     sinX*sinZ - cosX*sinY*cosZ;
        m[9] =     sinX*cosZ + cosX*sinY*sinZ;
        m[10] =    cosX*cosY;
        m[11] =    0.0;
        m[12] =    0.0;
        m[13] =    0.0;
        m[14] =    0.0;
        m[15] =    1.0;

}

/** ANIM_Z_Y_X2MAT @brief Make a rotation matrix corresponding to a rotation of
 * "z" radians about the z-axis, "y" radians about the y-axis, and
 * then "x" radians about the x-axis.
 */
void anim_z_y_x2mat(mat_t m, double x, double y, double z)
{
        fastf_t cosx = cos(x);
        fastf_t sinx = sin(x);
        fastf_t cosy = cos(y);
        fastf_t siny = sin(y);
        fastf_t cosz = cos(z);
        fastf_t sinz = sin(z);

        m[0] =  cosy*cosz;
        m[1] = -cosy*sinz;
        m[2] =  siny;
        m[4] =  cosx*sinz + sinx*siny*cosz;
        m[5] =  cosx*cosz - sinx*siny*sinz;
        m[6] = -sinx*cosy;
        m[8] =  sinx*sinz - cosx*siny*cosz;
        m[9] =  sinx*cosz + cosx*siny*sinz;
        m[10]=  cosx*cosy;
        m[3]=m[7]=m[11]=m[12]=m[13]=m[14]=0.0;
        m[15]=1.0;
}


/** ANIM_DZ_Y_X2MAT 
 * @brief 
 * Make a rotation matrix corresponding to a rotation of
 * "z" degrees about the z-axis, "y" degrees about the y-axis, and
 * then "x" degrees about the x-axis.
 */
void anim_dz_y_x2mat(mat_t m, double x, double y, double z)
{
        fastf_t cosx,cosy,cosz,sinx,siny,sinz;

        x *= (M_PI*0.0055555555556);
        y *= (M_PI*0.0055555555556);
        z *= (M_PI*0.0055555555556);

        cosx = cos(x);
        sinx = sin(x);
        cosy = cos(y);
        siny = sin(y);
        cosz = cos(z);
        sinz = sin(z);

        m[0] =  cosy*cosz;
        m[1] = -cosy*sinz;
        m[2] =  siny;
        m[4] =  cosx*sinz + sinx*siny*cosz;
        m[5] =  cosx*cosz - sinx*siny*sinz;
        m[6] = -sinx*cosy;
        m[8] =  sinx*sinz - cosx*siny*cosz;
        m[9] =  sinx*cosz + cosx*siny*sinz;
        m[10]=  cosx*cosy;
        m[3]=m[7]=m[11]=m[12]=m[13]=m[14]=0;
        m[15]=1;
}


/* ANIM_QUAT2MAT 
 * @brief
 *  Make 4x4 matrix from the given quaternion
 * Note: these quaternions are the conjugates of the quaternions
 * used in the librt/qmath.c quat_quat2mat()
 */

void anim_quat2mat(mat_t m, const quat_t qq)
{
        fastf_t two_q[4];
        quat_t q;

        QMOVE(q, qq);
        QUNITIZE(q);

        VADD2N(two_q,q,q,4);

        m[0]    = 1.0 - two_q[Y]*q[Y] - two_q[Z]*q[Z];
        m[1]    = two_q[X]*q[Y] - two_q[W]*q[Z];
        m[2]    = two_q[X]*q[Z] + two_q[W]*q[Y];
        m[3]    = 0.0;
        m[4]    = two_q[X]*q[Y] + two_q[W]*q[Z];
        m[5]    = 1.0 - two_q[X]*q[X] - two_q[Z]*q[Z];
        m[6]    = two_q[Y]*q[Z] - two_q[W]*q[X];
        m[7]    = 0.0;
        m[8]    = two_q[X]*q[Z] - two_q[W]*q[Y];
        m[9]    = two_q[Y]*q[Z] + two_q[W]*q[X];
        m[10]   = 1.0 - two_q[X]*q[X] - two_q[Y]*q[Y];
        m[11]   = 0.0;
        m[12]   = 0.0;
        m[13]   = 0.0;
        m[14]   = 0.0;
        m[15]   = 1.0;
}



/* ANIM_DIR2MAT 
 * @brief
 *  make a matrix which turns a vehicle from the x-axis to
 * point in the desired direction, staying "right-side up" (ie the y-axis
 * never has a z-component). A second direction vector is consulted when
 * the given direction is vertical. This is intended to represent the
 * the direction from a previous frame.
 */
void anim_dir2mat(mat_t m, const vect_t d, const vect_t d2b)
{
        fastf_t hypotenuse, sign;
        vect_t d2;

        VMOVE( d2, d2b );
        sign = 1.0;
        hypotenuse = sqrt(d[0]*d[0]+d[1]*d[1]);
        if (hypotenuse < VDIVIDE_TOL){ /* vertical direction - use d2 to
                                        * determine roll */
                hypotenuse = sqrt(d2[0]*d2[0]+d2[1]*d2[1]);
                if (hypotenuse < VDIVIDE_TOL){ /* use x-axis as default*/
                        VSET(d2,1,0,0);
                        hypotenuse = 1;
                }
                if (d[2] < 0)
                        sign = -1.0;
                m[1] = -d2[1]/hypotenuse;
                m[5] = d2[0]/hypotenuse;
                m[2] = -sign * d2[0]/hypotenuse;
                m[6] = -sign * d2[1]/hypotenuse;
                m[8] = sign;
                m[0]=m[4]=m[9]=m[10]=0.0;
        }
        else { /* normal - no roll*/
                m[0] = d[0];
                m[1] = -d[1]/hypotenuse;
                m[2] = -d[0]*d[2]/hypotenuse;
                m[4] = d[1];
                m[5] = d[0]/hypotenuse;
                m[6] = -d[1]*d[2]/hypotenuse;
                m[8] = d[2];
                m[9] = 0.0;
                m[10] = hypotenuse;
        }
        m[3]=m[7]=m[11]=0.0;
        m[12]=m[13]=m[14]=0.0;
        m[15]=1.0;

}

/* ANIM_DIRN2MAT 
 * @brief
 *  make a matrix which turns a vehicle from the x-axis to
 * point in the desired direction, staying "right-side up". In cases where
 * the direction is vertical, the second vector is consulted. The second
 * vector defines a normal to the the vertical plane into which the vehicle's
 * x and z axes should be put. A good choice to put here is the direction
 * of the vehicle's y-axis in the previous frame.
 */
void anim_dirn2mat(mat_t m, const vect_t dx2, const vect_t dn)
{
        vect_t temp;
        fastf_t hyp, sign,inv,mag;
        vect_t dx;

        VMOVE( dx, dx2 );
        sign = 1.0;
        mag = MAGNITUDE(dx);
        if (mag < VDIVIDE_TOL) {
                bu_log("anim_dirn2mat: Need non-zero vector");
                return;
        }
        inv = 1.0/mag;
        dx[0] *= inv;
        dx[1] *= inv;
        dx[2] *= inv;
        hyp = sqrt(dx[0]*dx[0]+dx[1]*dx[1]);
        if (hyp < VDIVIDE_TOL) { /* vertical - special handling */
                sign = (dx[2] < 0) ? -1.0 : 1.0;
                VSET(temp, dn[0], dn[1], 0.0);
                mag = MAGNITUDE(temp);
                if (mag < VDIVIDE_TOL) {
                        /* use default */
                        VSET(temp, 0.0, 1.0, 0.0);
                        mag = 1.0;
                } else {
                        inv = 1.0/mag;
                        temp[0] *= inv;
                        temp[1] *= inv;
                }
                m[0] = 0.0;
                m[4] = 0.0;
                m[8] = sign;
                m[1] = temp[0];
                m[5] = temp[1];
                m[9] = 0.0;
                m[2] = -sign*temp[1];
                m[6] = sign*temp[0];
                m[10] = 0.0;
                m[3]=m[7]=m[11]=0.0;
                m[12]=m[13]=m[14]=0.0;
                m[15]=1.0;
                return;
        }

        /*else normal*/
        m[0] = dx[0];
        m[1] = -dx[1]/hyp;
        m[2] = -dx[0]*dx[2]/hyp;
        m[4] = dx[1];
        m[5] = dx[0]/hyp;
        m[6] = -dx[1]*dx[2]/hyp;
        m[8] = dx[2];
        m[9] = 0.0;
        m[10] = hyp;
        m[3]=m[7]=m[11]=0.0;
        m[12]=m[13]=m[14]=0.0;
        m[15]=1.0;

}



#define ASM_EMPTY 0
#define ASM_FIRST 1
#define ASM_FULL  2

/*ANIM_STEER_MAT 
 * @brief
 *  given the next frame's position, remember the value of
the previous frame's position and calculate a matrix which points the x-axis
in the direction defined by those two positions. Return new matrix, and the
remembered value of the current position, as arguments; return 1 as the
normal value, and 0 when there is not yet information to remember.
*/
int anim_steer_mat(mat_t  mat, vect_t point, int end)
{
        static vect_t p1, p2, p3;
        vect_t dir;
        static vect_t norm;
        static int state = ASM_EMPTY;

        VMOVE(p1,p2);
        VMOVE(p2,p3);
        VMOVE(p3,point);

        switch(state) {
        case ASM_EMPTY:
                if (end) {
                        state = ASM_EMPTY;
                } else {
                        state = ASM_FIRST;
                        /* "don't print yet */
                }
                return 0;
        case ASM_FIRST:
                if (end) {
                        /* only one point specified, use default direction*/
                        VSET(dir,1.0,0.0,0.0);
                        VSET(norm,0.0,1.0,0.0);
                        state = ASM_EMPTY;
                } else {
                        VSUBUNIT(dir,p3,p2);
                        VSET(norm, 0.0, 1.0, 0.0);
                        state = ASM_FULL;
                }
                break;
        case ASM_FULL:
                if (end) {
                        VSUBUNIT(dir,p2,p1);
                        state = ASM_EMPTY;
                } else {
                        VSUBUNIT(dir,p3,p1);
                        state = ASM_FULL;
                }
        }

        /* go for it */
        anim_dirn2mat(mat,dir,norm); /* create basic rotation matrix */
        VSET(norm, mat[1], mat[5], 0.0); /* save for next time */
        VMOVE(point,p2); /* for main's purposes, the current point is p2 */
        return(1); /* return signal go ahead and print */

}


/***************************************
 * Other animation routines
 ***************************************/


/* ANIM_ADD_TRANS 
 * @brief
 *  Add pre- and post- translation to a rotation matrix.
 * The resulting matrix has the effect of performing the first
 * translation, followed by the rotation, followed by the second translation.
 */
void anim_add_trans(mat_t m, const vect_t post, const vect_t pre)
{
        int i;
        for (i=0; i<3; i++)
        m[3+i*4] += m[i*4]*pre[0] + m[1+i*4]*pre[1]+m[2+i*4]*pre[2] + post[i];

}

/* ANIM_ROTATEZ 
 * @brief
 *  Rotate the vector "d" through "a" radians about the z-axis.
 */
void anim_rotatez(fastf_t a, vect_t d)
{
        fastf_t temp[3];
        fastf_t cos_y = cos(a);
        fastf_t sin_y = sin(a);
        temp[0] = d[0]*cos_y - d[1]*sin_y;
        temp[1] = d[0]*sin_y + d[1]*cos_y;
        d[0]=temp[0];
        d[1]=temp[1];
}

/* ANIM_MAT_PRINT 
 * @brief
 *  print out 4X4 matrix, with optional colon
 */
void anim_mat_print(FILE *fp, const mat_t m, int s_colon)
{
        bu_flog( fp,"%.10g %.10g %.10g %.10g\n", m[0], m[1], m[2], m[3]);
        bu_flog( fp,"%.10g %.10g %.10g %.10g\n", m[4], m[5], m[6], m[7]);
        bu_flog( fp,"%.10g %.10g %.10g %.10g\n", m[8], m[9], m[10], m[11]);
        bu_flog( fp,"%.10g %.10g %.10g %.10g", m[12], m[13], m[14], m[15]);
        if (s_colon)
                bu_flog( fp,";");
        bu_flog( fp,"\n");
}



/* ANIM_MAT_PRINTF 
 * @brief
 *  print out 4X4 matrix
 * formstr must be less than twenty chars
 */
void anim_mat_printf(
        FILE *fp,
        const mat_t m,
        const char *formstr,
        const char *linestr,
        const char *endstr)
{
        char mystr[80];
        sprintf(mystr,"%s%s%s%s%%s",formstr,formstr,formstr,formstr);
        bu_flog( fp,mystr, m[0], m[1], m[2], m[3], linestr);
        bu_flog( fp,mystr, m[4], m[5], m[6], m[7], linestr);
        bu_flog( fp,mystr, m[8], m[9], m[10], m[11], linestr);
        bu_flog( fp,mystr, m[12], m[13], m[14], m[15], endstr);
}

/* ANIM_VIEW_REV 
 * @brief
 *  Reverse the direction of a view matrix, keeping it
 * right-side up
 */
void anim_view_rev(mat_t m)
{
        m[0] = -m[0];
        m[1] = -m[1];
        m[4] = -m[4];
        m[5] = -m[5];
        m[8] = -m[8];
        m[9] = -m[9];
}
/*@}*/
/*
 * Local Variables:
 * mode: C
 * tab-width: 8
 * c-basic-offset: 4
 * indent-tabs-mode: t
 * End:
 * ex: shiftwidth=4 tabstop=8
 */

---