/*************************************************************************/
/*                                                                       */
/*  Copyright (c) 1994 Stanford University                               */
/*                                                                       */
/*  All rights reserved.                                                 */
/*                                                                       */
/*  Permission is given to use, copy, and modify this software for any   */
/*  non-commercial purpose as long as this copyright notice is not       */
/*  removed.  All other uses, including redistribution in whole or in    */
/*  part, are forbidden without prior written permission.                */
/*                                                                       */
/*  This software is provided with absolutely no warranty and no         */
/*  support.                                                             */
/*                                                                       */
/*************************************************************************/

/*
 * VECTMATH.H: include file for vector/matrix operations.
 */

#ifndef _VECMATH_H_
#define _VECMATH_H_


#  define NDIM 3

typedef real vector[NDIM], matrix[NDIM][NDIM];

/*
 * Vector operations.
 */

#define CLRV(v)                   /* CLeaR Vector */                        \
{                                                                        \
    register int _i;                                                        \
    for (_i = 0; _i < NDIM; _i++)                                        \
        (v)[_i] = 0.0;                                                        \
}

#define UNITV(v,j)                /* UNIT Vector */                        \
{                                                                        \
    register int _i;                                                        \
    for (_i = 0; _i < NDIM; _i++)                                        \
        (v)[_i] = (_i == (j) ? 1.0 : 0.0);                                \
}

#define SETV(v,u)                /* SET Vector */                        \
{                                                                         \
    register int _i;                                                         \
    for (_i = 0; _i < NDIM; _i++)                                         \
        (v)[_i] = (u)[_i];                                                 \
}


#define ADDV(v,u,w)                /* ADD Vector */                        \
{                                                                        \
    register real *_vp = (v), *_up = (u), *_wp = (w);                        \
    *_vp++ = (*_up++) + (*_wp++);                                        \
    *_vp++ = (*_up++) + (*_wp++);                                        \
    *_vp   = (*_up  ) + (*_wp  );                                        \
}

#define SUBV(v,u,w)            /* SUBtract Vector */                        \
{                                                                        \
    register real *_vp = (v), *_up = (u), *_wp = (w);                        \
    *_vp++ = (*_up++) - (*_wp++);                                        \
    *_vp++ = (*_up++) - (*_wp++);                                        \
    *_vp   = (*_up  ) - (*_wp  );                                        \
}

#define MULVS(v,u,s)         /* MULtiply Vector by Scalar */                \
{                                                                        \
    register real *_vp = (v), *_up = (u);                                \
    *_vp++ = (*_up++) * (s);                                                \
    *_vp++ = (*_up++) * (s);                                                \
    *_vp   = (*_up  ) * (s);                                                \
}


#define DIVVS(v,u,s)           /* DIVide Vector by Scalar */                \
{                                                                        \
    register int _i;                                                        \
    for (_i = 0; _i < NDIM; _i++)                                        \
        (v)[_i] = (u)[_i] / (s);                                        \
}


#define DOTVP(s,v,u)                /* DOT Vector Product */                \
{                                                                        \
    register real *_vp = (v), *_up = (u);                                \
    (s)  = (*_vp++) * (*_up++);                                               \
    (s) += (*_vp++) * (*_up++);                                               \
    (s) += (*_vp  ) * (*_up  );                                               \
}


#define ABSV(s,v)                /* ABSolute value of a Vector */        \
{                                                                        \
    double _tmp, sqrt();                                                \
    register int _i;                                                        \
    _tmp = 0.0;                                                               \
    for (_i = 0; _i < NDIM; _i++)                                        \
        _tmp += (v)[_i] * (v)[_i];                                        \
    (s) = sqrt(_tmp);                                                   \
}

#define DISTV(s,u,v)           /* DISTance between Vectors */                \
{                                                                        \
    double _tmp, sqrt();                                                \
    register int _i;                                                        \
    _tmp = 0.0;                                                               \
    for (_i = 0; _i < NDIM; _i++)                                        \
        _tmp += ((u)[_i]-(v)[_i]) * ((u)[_i]-(v)[_i]);                        \
    (s) = sqrt(_tmp);                                                   \
}


#define CROSSVP(v,u,w)            /* CROSS Vector Product */           \
{                                                                      \
    (v)[0] = (u)[1]*(w)[2] - (u)[2]*(w)[1];                            \
    (v)[1] = (u)[2]*(w)[0] - (u)[0]*(w)[2];                            \
    (v)[2] = (u)[0]*(w)[1] - (u)[1]*(w)[0];                            \
}


#define INCADDV(v,u)             /* INCrementally ADD Vector */         \
{                                                                        \
    register int _i;                                                    \
    for (_i = 0; _i < NDIM; _i++)                                       \
        (v)[_i] += (u)[_i];                                             \
}

#define INCSUBV(v,u)             /* INCrementally SUBtract Vector */    \
{                                                                        \
    register int _i;                                                    \
    for (_i = 0; _i < NDIM; _i++)                                       \
        (v)[_i] -= (u)[_i];                                             \
}

#define INCMULVS(v,s)  /* INCrementally MULtiply Vector by Scalar */        \
{                                                                        \
    register int _i;                                                    \
    for (_i = 0; _i < NDIM; _i++)                                       \
        (v)[_i] *= (s);                                                 \
}

#define INCDIVVS(v,s)   /* INCrementally DIVide Vector by Scalar */        \
{                                                                        \
    register int _i;                                                    \
    for (_i = 0; _i < NDIM; _i++)                                       \
        (v)[_i] /= (s);                                                 \
}

/*
 * Matrix operations.
 */

#define CLRM(p)                    /* CLeaR Matrix */                        \
{                                                                        \
    register int _i, _j;                                                \
    for (_i = 0; _i < NDIM; _i++)                                        \
        for (_j = 0; _j < NDIM; _j++)                                        \
            (p)[_i][_j] = 0.0;                                                \
}

#define SETMI(p)                /* SET Matrix to Identity */                \
{                                                                        \
    register int _i, _j;                                                \
    for (_i = 0; _i < NDIM; _i++)                                        \
        for (_j = 0; _j < NDIM; _j++)                                        \
            (p)[_i][_j] = (_i == _j ? 1.0 : 0.0);                        \
}

#define SETM(p,q)                /* SET Matrix */                        \
{                                                                        \
    register int _i, _j;                                                \
    for (_i = 0; _i < NDIM; _i++)                                        \
        for (_j = 0; _j < NDIM; _j++)                                        \
            (p)[_i][_j] = (q)[_i][_j];                                        \
}

#define TRANM(p,q)            /* TRANspose Matrix */                        \
{                                                                        \
    register int _i, _j;                                                \
    for (_i = 0; _i < NDIM; _i++)                                        \
        for (_j = 0; _j < NDIM; _j++)                                        \
            (p)[_i][_j] = (q)[_j][_i];                                        \
}

#define ADDM(p,q,r)                /* ADD Matrix */                        \
{                                                                        \
    register int _i, _j;                                                \
    for (_i = 0; _i < NDIM; _i++)                                        \
        for (_j = 0; _j < NDIM; _j++)                                        \
            (p)[_i][_j] = (q)[_i][_j] + (r)[_i][_j];                        \
}

#define SUBM(p,q,r)            /* SUBtract Matrix */                        \
{                                                                        \
    register int _i, _j;                                                \
    for (_i = 0; _i < NDIM; _i++)                                        \
        for (_j = 0; _j < NDIM; _j++)                                        \
            (p)[_i][_j] = (q)[_i][_j] - (r)[_i][_j];                        \
}

#define MULM(p,q,r)            /* Multiply Matrix */                        \
{                                                                        \
    register int _i, _j, _k;                                                \
    for (_i = 0; _i < NDIM; _i++)                                        \
        for (_j = 0; _j < NDIM; _j++) {                                      \
            (p)[_i][_j] = 0.0;                                                \
            for (_k = 0; _k < NDIM; _k++)                                \
                (p)[_i][_j] += (q)[_i][_k] * (r)[_k][_j];                \
        }                                                                \
}

#define MULMS(p,q,s)          /* MULtiply Matrix by Scalar */                \
{                                                                        \
    register int _i, _j;                                                \
    for (_i = 0; _i < NDIM; _i++)                                        \
        for (_j = 0; _j < NDIM; _j++)                                        \
            (p)[_i][_j] = (q)[_i][_j] * (s);                                \
}

#define DIVMS(p,q,s)         /* DIVide Matrix by Scalar */                \
{                                                                        \
    register int _i, _j;                                                \
    for (_i = 0; _i < NDIM; _i++)                                        \
        for (_j = 0; _j < NDIM; _j++)                                        \
            (p)[_i][_j] = (q)[_i][_j] / (s);                                \
}

#define MULMV(v,p,u)       /* MULtiply Matrix by Vector */                \
{                                                                        \
    register int _i, _j;                                                \
    for (_i = 0; _i < NDIM; _i++) {                                        \
        (v)[_i] = 0.0;                                                        \
        for (_j = 0; _j < NDIM; _j++)                                        \
            (v)[_i] += (p)[_i][_j] * (u)[_j];                                \
    }                                                                        \
}

#define OUTVP(p,v,u)         /* OUTer Vector Product */                \
{                                                                        \
    register int _i, _j;                                                \
    for (_i = 0; _i < NDIM; _i++)                                        \
        for (_j = 0; _j < NDIM; _j++)                                        \
            (p)[_i][_j] = (v)[_i] * (u)[_j];                                \
}

#define TRACEM(s,p)         /* TRACE of Matrix */                        \
{                                                                        \
    register int _i;                                                        \
    (s) = 0.0;                                                                \
    for (_i = 0.0; _i < NDIM; _i++)                                        \
        (s) += (p)[_i][_i];                                                \
}

/*
 * Misc. impure operations.
 */

#define SETVS(v,s)                /* SET Vector to Scalar */                \
{                                                                        \
    register int _i;                                                        \
    for (_i = 0; _i < NDIM; _i++)                                        \
        (v)[_i] = (s);                                                        \
}

#define ADDVS(v,u,s)             /* ADD Vector and Scalar */                \
{                                                                        \
    register int _i;                                                        \
    for (_i = 0; _i < NDIM; _i++)                                        \
        (v)[_i] = (u)[_i] + (s);                                        \
}

#define SETMS(p,s)                /* SET Matrix to Scalar */                \
{                                                                        \
    register int _i, _j;                                                \
    for (_i = 0; _i < NDIM; _i++)                                        \
        for (_j = 0; _j < NDIM; _j++)                                        \
            (p)[_i][_j] = (s);                                                \
}

#define PRTV(name, vec)           /* PRinT Vector */                      \
{                                                                         \
   fprintf(stdout,"%s = [%9.4f,%9.4f,%9.4f] ",name,vec[0],vec[1],vec[2]); \
}
#define PRIV(name, vec)           /* PRint Integer Vector */               \
{                                                                         \
   fprintf(stdout,"%s = [%d,%d,%d] ",name,vec[0],vec[1],vec[2]); \
}
#define PROV(name, vec)           /* PRint Integer Vector */               \
{                                                                         \
   fprintf(stdout,"%s = [%o,%o,%o] ",name,vec[0],vec[1],vec[2]); \
}
#define PRHV(name, vec)           /* PRint Integer Vector */               \
{                                                                         \
   fprintf(stdout,"%s = [%x,%x,%x] ",name,vec[0],vec[1],vec[2]); \
}

#endif