/*
 *    PROJECT:      DVS
 *    SUBSYSTEM:    matrix/linear algebra mosdule
 *    MODULE:       dmtypes.h
 *
 *    File:         $RCSfile: dm.h,v $
 *    Revision:     $Revision: 2.23 $
 *    Date:         $Date: 1995/05/18 09:46:38 $
 *    Author:       $Author: jeff $
 *    RCS Ident:    $Id: dm.h,v 2.23 1995/05/18 09:46:38 jeff beta $
 *
 *    FUNCTION:
 *    This file contains the external declarations of the dm library
 *    routines and types. 
 *
 *
 * Copyright (c) 1994 Division Ltd.
 *
 * All Rights Reserved.
 *
 * This Document may not, in whole or in part, be copied,
 * photocopied, reproduced, translated, or reduced to any
 * electronic medium or machine readable form without prior
 * written consent from Division Ltd.
 */
#ifndef _DMTYPES_H
#define _DMTYPES_H

#include <stdio.h>

#if defined(_TRANSPUTER) || defined(_I860)
/*
 * These are not defined on transputers.
 */
#define M_E             2.71828182845904523540
#define M_LOG2E         1.44269504088896340740
#define M_LOG10E        0.43429448190325182765
#define M_LN2           0.69314718055994530942
#define M_LN10          2.30258509299404568402
#define M_PI            3.14159265358979323846
#define M_PI_2          1.57079632679489661923
#define M_PI_4          0.78539816339744830962
#define M_1_PI          0.31830988618379067154
#define M_2_PI          0.63661977236758134308
#define M_2_SQRTPI      1.12837916709551257390
#define M_SQRT2         1.41421356237309504880
#define M_SQRT1_2       0.70710678118654752440
#endif

#ifdef _TRANSPUTER
#include <mathf.h>
#else
#include <math.h>
#endif

#ifdef _I860
#define const /*broken pgcc doesn't understand const properly*/
#endif

/* Windows mathf functions. */
#if defined(_WIN31) || defined(_DOS)
#define fabsf(x) (float) fabs((double)(x))
#define sqrtf(x) (float) sqrt((double)(x))
#define sinf(x)  (float)  sin((double)(x))
#define cosf(x)  (float)  cos((double)(x))
#define asinf(x) (float) asin((double)(x))
#define acosf(x) (float) acos((double)(x))
#endif


#include <stdlib.h>
#include <float.h>
#include <dsys/divtypes.h>

#ifdef __cplusplus
extern "C" {
#endif
/*
 * The basic linear algebra types.
 */
typedef enum {
        DM_X     = 0,
        DM_Y     = 1,
        DM_Z     = 2,
        DM_W     = 3,
        DM_PITCH = 0,
        DM_YAW   = 1,
        DM_ROLL  = 2
} dmIndex;

typedef float32 dmVector[3];
typedef float32 dmScale[3];
typedef float32 dmPoint[3];
typedef float32 dmEuler[3];
typedef float32 dmQuaternion[4];
typedef float32 dmMatrix[4][4];

typedef struct {
    dmMatrix   pos;
    dmVector   linearVel;
    dmVector   linearAccn;
    dmVector   angularVel;
    dmVector   angularAccn;
} dmPosition, *dmPosition_Ptr;

/*
 * Small value used to check floating point values for equality
 * or smallness. This number is withing 2 bits of the smallest
 * representable relative differance between two numbers.
 */
#define DM_EPSILON ((float32)(FLT_EPSILON * 4.0f))

/*
 * fast macros for common ops.
 */
#define dmSqr(x) ((x) * (x))

#define dmCrossProd3(r,a,b)                   \
do {                                          \
    float32 _ax = (a)[DM_X], _bx = (b)[DM_X]; \
    float32 _ay = (a)[DM_Y], _by = (b)[DM_Y]; \
    float32 _az = (a)[DM_Z], _bz = (b)[DM_Z]; \
                                              \
    (r)[DM_X] = (_ay * _bz) - (_az * _by);    \
    (r)[DM_Y] = (_az * _bx) - (_ax * _bz);    \
    (r)[DM_Z] = (_ax * _by) - (_ay * _bx);    \
} while (0)

#define dmDotProd3(x, y) (  ((x)[DM_X] * (y)[DM_X])  \
                          + ((x)[DM_Y] * (y)[DM_Y])  \
                          + ((x)[DM_Z] * (y)[DM_Z]))

#define dmDotProd4(x, y) (  ((x)[DM_X] * (y)[DM_X])  \
                          + ((x)[DM_Y] * (y)[DM_Y])  \
                          + ((x)[DM_Z] * (y)[DM_Z])  \
                          + ((x)[DM_W] * (y)[DM_W]))

#define dmLength3(x) sqrtf( dmSqr(x[DM_X]) \
                           +dmSqr(x[DM_Y]) \
                           +dmSqr(x[DM_Z]))

#define dmVectorCrossProd(r,a,b)  dmCrossProd3((r),(a),(b)) 
#define dmVectorDotProd(a,b)        dmDotProd3((a),(b))
#define dmVectorLength(a)            dmLength3(a)



#define dmScaleInvert(d,s)    (((d)[DM_X] = 1.0f/(s)[DM_X]), \
                               ((d)[DM_Y] = 1.0f/(s)[DM_Y]), \
                               ((d)[DM_Z] = 1.0f/(s)[DM_Z]))

#define dmVectorInvert(d,s)   (((d)[DM_X] = -(s)[DM_X]), \
                               ((d)[DM_Y] = -(s)[DM_Y]), \
                               ((d)[DM_Z] = -(s)[DM_Z])) 


#define dmDegToRad(x) ((x) * ((float32) M_PI / 180.0))
#define dmRadToDeg(x) ((x) * ((float32) M_1_PI * 180.0))



/*
 * Optimised and Safe trig functions.
 */
extern void          dmSinCosf(float32 *s,
                               float32 *c,
                               float32 angle);
extern float32     dmSafeAtan2(float32 opposite,
                               float32 adjacent);

/*
 * Copy functions
 */
#ifdef _TRANSPUTER
#include <string.h>
#define    dmMatCopy(d,s) memcpy(d,s,sizeof(dmMatrix))
#define   dmQuatCopy(d,s) memcpy(d,s,sizeof(dmQuaternion))
#define  dmEulerCopy(d,s) memcpy(d,s,sizeof(dmEuler))
#define dmVectorCopy(d,s) memcpy(d,s,sizeof(dmVector))
#define  dmPointCopy(d,s) memcpy(d,s,sizeof(dmPoint))
#define  dmScaleCopy(d,s) memcpy(d,s,sizeof(dmScale))
#define    dmPosCopy(d,s) ((d) = (s))
#define    __dmCopy3(d,s) memcpy(d,s,3 * sizeof(float32))
#define    __dmCopy4(d,s) memcpy(d,s,4 * sizeof(float32))
#else
#define __dmCopy3(d,s) do { \
    (d)[DM_X] = (s)[DM_X];  \
    (d)[DM_Y] = (s)[DM_Y];  \
    (d)[DM_Z] = (s)[DM_Z];  \
} while (0) 

#define __dmCopy4(d,s) do { \
    (d)[DM_X] = (s)[DM_X];  \
    (d)[DM_Y] = (s)[DM_Y];  \
    (d)[DM_Z] = (s)[DM_Z];  \
    (d)[DM_W] = (s)[DM_W];  \
} while (0) 

#define     dmMatCopy(d,s) do {        \
    __dmCopy4((d)[DM_X],(s)[DM_X]);    \
    __dmCopy4((d)[DM_Y],(s)[DM_Y]);    \
    __dmCopy4((d)[DM_Z],(s)[DM_Z]);    \
    __dmCopy4((d)[DM_W],(s)[DM_W]);    \
} while (0)

#define dmQuatCopy(d,s)   __dmCopy4(d,s)
#define dmEulerCopy(d,s)  __dmCopy3(d,s)
#define dmVectorCopy(d,s) __dmCopy3(d,s)
#define dmPointCopy(d,s)  __dmCopy3(d,s)
#define dmScaleCopy(d,s)  __dmCopy3(d,s)

#define   dmPosCopy(d,s) (*(d) = *(s))
#endif

/*
 * Ident functions. 
 */
#define dmMatIdent(m)       dmMatCopy(m, dmIdentM)
#define dmQuatIdent(q)     dmQuatCopy(q, dmIdentQ)
#define dmEulerIdent(e)   dmEulerCopy(e, dmIdentE)
#define dmVectorIdent(v) dmVectorCopy(v, dmIdentV)
#define dmPointIdent(p)   dmPointCopy(p, dmIdentP)
#define dmScaleIdent(s)   dmScaleCopy(s, dmIdentS)
#define dmPosIdent(p)       dmPosCopy(p, &dmIdentPos)

extern const dmMatrix     dmIdentM;
extern const dmQuaternion dmIdentQ;
extern const dmEuler      dmIdentE;
extern const dmVector     dmIdentV;
extern const dmPoint      dmIdentP;
extern const dmScale      dmIdentS;
extern const dmPosition   dmIdentPos;

extern void    (dmMatIdent)(dmMatrix mat);
extern void   (dmQuatIdent)(dmQuaternion q);
extern void  (dmEulerIdent)(dmEuler e);
extern void (dmVectorIdent)(dmVector s);
extern void  (dmPointIdent)(dmPoint s);
extern void  (dmScaleIdent)(dmScale s);
extern void    (dmPosIdent)(dmPosition_Ptr p);

/*
 * initialisation functions
 */
#define  dmEulerSet(e,p,y,r) ((e)[DM_PITCH] = (p), \
                              (e)[DM_YAW]   = (y), \
                              (e)[DM_ROLL]  = (r)) 

#define dmEulerSetD(e,p,y,r) ((e)[DM_PITCH] =  dmDegToRad(p), \
                              (e)[DM_YAW]   =  dmDegToRad(y), \
                              (e)[DM_ROLL]  =  dmDegToRad(r)) 

#define dmQuatSet(q,x,y,z,w) ((q)[DM_X] = (x), \
                              (q)[DM_Y] = (y), \
                              (q)[DM_Z] = (z), \
                              (q)[DM_W] = (w))

#define  dmPointSet(p,x,y,z) ((p)[DM_X] = (x), \
                              (p)[DM_Y] = (y), \
                              (p)[DM_Z] = (z))

#define  dmVectorSet(v,x,y,z) ((v)[DM_X] = (x), \
                               (v)[DM_Y] = (y), \
                               (v)[DM_Z] = (z))

#define  dmScaleSet(s,x,y,z) ((s)[DM_X] = (x), \
                              (s)[DM_Y] = (y), \
                              (s)[DM_Z] = (z))


extern void     (dmEulerSet)(dmEuler e,
                             float32 pitch,
                             float32 yaw,
                             float32 roll);
extern void    (dmEulerSetD)(dmEuler e,
                             float32 pitch,
                             float32 yaw,
                             float32 roll);
extern void      (dmQuatSet)(dmQuaternion q,
                             float32 x,
                             float32 y,
                             float32 z,
                             float32 w);
extern void     (dmPointSet)(dmPoint p,
                             float32 x,
                             float32 y, 
                             float32 z);
extern void    (dmVectorSet)(dmVector p,
                             float32 x,
                             float32 y, 
                             float32 z);
extern void     (dmScaleSet)(dmScale p,
                             float32 x,
                             float32 y, 
                             float32 z);

/*
 * Matrix generation functions
 */
#define dmMatFromScale(m,s) do { \
    dmMatIdent(m);               \
    (m)[DM_X][DM_X] = s[DM_X];   \
    (m)[DM_Y][DM_Y] = s[DM_Y];   \
    (m)[DM_Z][DM_Z] = s[DM_Z];   \
} while (0)

#define dmMatFromEuler(m,e) do { \
    dmQuaternion __q;            \
    dmQuatFromEuler(__q,e);      \
    dmMatFromQuat(m,__q);        \
} while (0)

#define dmMatFromPoint(m,p) do {              \
    dmMatIdent(m);                            \
    m[DM_W][DM_X] = p[DM_X];                  \
    m[DM_W][DM_Y] = p[DM_Y];                  \
    m[DM_W][DM_Z] = p[DM_Z];                  \
} while (0)

#define dmMatFromPointEulerScale(m, p,e,s) do { \
    dmMatFromScale(m,s);                        \
    dmMatRotEuler(m,e);                         \
    m[DM_W][DM_X] = p[DM_X];                    \
    m[DM_W][DM_Y] = p[DM_Y];                    \
    m[DM_W][DM_Z] = p[DM_Z];                    \
} while (0)

#define dmMatFromPointQuatScale(m, p,q,s) do {  \
    dmMatFromScale(m,s);                        \
    dmMatRotQuat(m,q);                          \
    m[DM_W][DM_X] = p[DM_X];                    \
    m[DM_W][DM_Y] = p[DM_Y];                    \
    m[DM_W][DM_Z] = p[DM_Z];                    \
} while (0)

extern void              dmMatFromQuat(      dmMatrix mat,
                                       const dmQuaternion quat);
extern void       dmMatFromGeneralQuat(      dmMatrix mat,
                                       const dmQuaternion quat);
extern void           (dmMatFromEuler)(      dmMatrix m,
                                       const dmEuler e);
extern void           (dmMatFromScale)(      dmMatrix m,
                                       const dmScale  s);
extern void           (dmMatFromPoint)(      dmMatrix m,
                                       const dmPoint  p);
extern void (dmMatFromPointEulerScale)(      dmMatrix m,
                                       const dmPoint p,
                                       const dmEuler e,
                                       const dmScale s);
extern void  (dmMatFromPointQuatScale)(      dmMatrix m,
                                       const dmPoint p,
                                       const dmQuaternion q,
                                       const dmScale s);



/*
 * matrix manipulation functions.
 */
#define dmMatRotQuat(m,q) do { \
    dmMatrix __m;              \
    dmMatFromQuat(__m, (q));   \
    dmMatMultH((m),(m),__m);   \
} while (0)

#define dmMatRotEuler(m, e) do { \
    dmMatrix __m;                \
    dmMatFromEuler(__m,e);       \
    dmMatMultH((m), (m), __m);   \
} while (0)


extern void       dmMatRotX(dmMatrix m,
                            float32  angle);
extern void       dmMatRotY(dmMatrix m,
                            float32  angle);
extern void       dmMatRotZ(dmMatrix m,
                            float32  angle);

extern void (dmMatRotEuler)(      dmMatrix m,
                            const dmEuler e);
extern void  (dmMatRotQuat)(      dmMatrix m,
                            const dmQuaternion q);
extern void      dmMatScale(      dmMatrix m,
                            const dmScale s);

extern void      dmMatXlate(      dmMatrix m,
                            const dmVector v);

extern void    dmMatInvertH(      dmMatrix inv,
                            const dmMatrix mat);

extern void      dmMatMultH(      dmMatrix res,
                            const dmMatrix left,
                            const dmMatrix right);

extern int      dmMatInvert(      dmMatrix inv,
                            const dmMatrix mat);

extern void       dmMatMult(      dmMatrix res,
                            const dmMatrix left,
                            const dmMatrix right);

/*
 * Quaternion manipulation ops.
 */
#define dmQuatNorm(x) do {      \
    if ((x)[DM_W] < 0.0f) {     \
        (x)[DM_X] = -(x)[DM_X]; \
        (x)[DM_Y] = -(x)[DM_Y]; \
        (x)[DM_Z] = -(x)[DM_Z]; \
        (x)[DM_W] = -(x)[DM_W]; \
    }                           \
}while(0)

extern void       dmQuatInvert(      dmQuaternion res,
                               const dmQuaternion q);
extern void         dmQuatMult(      dmQuaternion res,
                               const dmQuaternion left,
                               const dmQuaternion right);
extern void         dmQuatMake(      dmQuaternion quat,
                               const dmVector axis,
                                     float32  angle);
extern void dmQuatMakeUnitAxis(      dmQuaternion rot,
                               const dmVector axis,
                               float    angle);
extern void       (dmQuatNorm)(dmQuaternion q);


/*
 * Conversions between Euler angles and
 * quaternions.
 */

extern void    dmQuatFromEuler(      dmQuaternion quat,
                               const dmEuler erot);

extern void    dmEulerFromQuat(      dmEuler e,
                               const dmQuaternion q);


/*
 * Functions to split matricies in various ways.
 */

#define dmPointFromMat(p,m)   dmPointQuatScaleFromMat((p),  NULL, NULL, (m))
#define dmScaleFromMat(s,m)   dmPointQuatScaleFromMat(NULL, NULL, (s),  (m))
#define dmQuatFromMat(q,m)    dmPointQuatScaleFromMat(NULL, (q),  NULL, (m))
#define dmEulerFromMat(e,m)  dmPointEulerScaleFromMat(NULL, (e),  NULL, (m))

extern int          (dmQuatFromMat)(      dmQuaternion quat,
                                    const dmMatrix mat);
extern int         (dmEulerFromMat)(      dmEuler e,
                                    const dmMatrix m);
extern int         (dmPointFromMat)(      dmPoint p,
                                    const dmMatrix m);
extern int         (dmScaleFromMat)(      dmScale s,
                                    const dmMatrix m);

extern int  dmPointQuatScaleFromMat(      dmPoint p,
                                          dmQuaternion q,
                                          dmScale s,
                                    const dmMatrix m);

extern int dmPointEulerScaleFromMat(      dmPoint p,
                                          dmEuler e,
                                          dmScale s,
                                    const dmMatrix m);

/*
 * Functions to manipulate vectors and points
 * using matricies, quaternions, rotations and scales
 */
#define __dmAdd3(r,a,b)   ((r)[DM_X] = (a)[DM_X] + (b)[DM_X], \
                           (r)[DM_Y] = (a)[DM_Y] + (b)[DM_Y], \
                           (r)[DM_Z] = (a)[DM_Z] + (b)[DM_Z])
#define __dmSub3(r,a,b)   ((r)[DM_X] = (a)[DM_X] - (b)[DM_X], \
                           (r)[DM_Y] = (a)[DM_Y] - (b)[DM_Y], \
                           (r)[DM_Z] = (a)[DM_Z] - (b)[DM_Z])
#define __dmScale3(r,a,b) ((r)[DM_X] = (a)[DM_X] * (b)[DM_X], \
                           (r)[DM_Y] = (a)[DM_Y] * (b)[DM_Y], \
                           (r)[DM_Z] = (a)[DM_Z] * (b)[DM_Z])

#define __dmScaleScalar3(r,a,b)   ((r)[DM_X] = (a)[DM_X] * b, \
                                   (r)[DM_Y] = (a)[DM_Y] * b, \
                                   (r)[DM_Z] = (a)[DM_Z] * b)


#define __dmRotEuler3(r,a,b)                    \
do {                                            \
    dmQuaternion __q;                           \
    dmQuatFromEuler(__q,b);                     \
    __dmRotQuat3((r),(a),__q);                  \
} while (0)

extern void       __dmRotQuat3(      float32 *r,
                               const float32 *a,
                               const dmQuaternion q);

#define dmVectorAdd(r,a,b)         __dmAdd3(r,a,b)
#define dmVectorSub(r,a,b)         __dmSub3(r,a,b)
#define dmPointSub(r,a,b)          __dmSub3(r,a,b)
#define dmPointAddVector(r,a,b)    __dmAdd3(r,a,b)

#define dmVectorScale(r,a,b)       __dmScale3(r,a,b)
#define dmPointScale(r,a,b)        __dmScale3(r,a,b)
#define dmVectorScaleScalar(r,a,b) __dmScaleScalar3(r,a,b)
#define dmVectorRotQuat(r,v,q)     __dmRotQuat3(r,v,q) 
#define dmPointRotQuat(r,p,q)      __dmRotQuat3(r,p,q) 

#define dmVectorRotEuler(r,v,e)    __dmRotEuler3(r,v,e)
#define dmPointRotEuler(r,p,e)     __dmRotEuler3(r,p,e)


extern void         (dmVectorAdd)(      dmVector r,
                                  const dmVector a,
                                  const dmVector b);

extern void         (dmVectorSub)(      dmVector r,
                                  const dmVector a,
                                  const dmVector b);

extern void          (dmPointSub)(      dmVector r,
                                  const dmPoint  a,
                                  const dmPoint  b);

extern void    (dmPointAddVector)(      dmPoint  r,
                                  const dmPoint  a,
                                  const dmVector b);

extern void       (dmVectorScale)(      dmVector r,
                                  const dmVector a,
                                  const dmScale  b);

extern void (dmVectorScaleScalar)(      dmVector r,
                                  const dmVector a,
                                        float32  b);

extern void        (dmPointScale)(      dmPoint r,
                                  const dmPoint a,
                                  const dmScale b);

extern void     (dmVectorRotQuat)(      dmVector r,
                                  const dmVector v,
                                  const dmQuaternion);

extern void    (dmVectorRotEuler)(      dmVector r,
                                  const dmEuler  e,
                                  const dmVector v);


extern void      (dmPointRotQuat)(      dmPoint r,
                                  const dmPoint p,
                                  const dmQuaternion q);
extern void     (dmPointRotEuler)(      dmPoint r,
                                  const dmPoint p,
                                  const dmEuler  e);

extern void      dmVectorXformMat(      dmVector r,
                                  const dmVector v,
                                  const dmMatrix m);
extern void       dmPointXformMat(      dmPoint r,
                                  const dmPoint v,
                                  const dmMatrix m);



/*
 * Dead reckoning et al. Actually most of these functions only 
 * deal with zero order stuff.
 */

#define dmPosFromMat(p,m) do {                  \
    dmMatCopy((p)->pos, m);                     \
    dmVectorIdent((p)->linearVel);              \
    dmVectorIdent((p)->linearAccn);             \
    dmVectorIdent((p)->angularVel);             \
    dmVectorIdent((p)->angularAccn);            \
} while (0)

#define dmPosFromPointEulerScale(p,p2,e,s) do {      \
    dmMatFromPointEulerScale((p)->pos,(p2),(e),(s)); \
    dmVectorIdent((p)->linearVel);                   \
    dmVectorIdent((p)->linearAccn);                  \
    dmVectorIdent((p)->angularVel);                  \
    dmVectorIdent((p)->angularAccn);                 \
}while(0)
 
#define dmPosFromPointQuatScale(p,p2,q,s) do {       \
    dmMatFromPointQuatScale((p)->pos,(p2),(q),(s));  \
    dmVectorIdent((p)->linearVel);                   \
    dmVectorIdent((p)->linearAccn);                  \
    dmVectorIdent((p)->angularVel);                  \
    dmVectorIdent((p)->angularAccn);                 \
}while(0)
 
extern void             (dmPosFromMat)(      dmPosition_Ptr p,
                                       const dmMatrix       m);
extern void (dmPosFromPointEulerScale)(      dmPosition_Ptr p,
                                       const dmPoint        p2,
                                       const dmEuler        e,
                                       const dmScale        s);
extern void  (dmPosFromPointQuatScale)(     dmPosition_Ptr  p,
                                       const dmPoint        p2,
                                       const dmQuaternion   q,
                                       const dmScale        s);

extern void               dmDeadReckon(      dmMatrix       m,
                                             float32        time,
                                       const dmPosition_Ptr pos);
/*
 *
 *    given four points a, b, c, d defining two line segments AB and CD,
 *    generate a matrix m that will transform AB to CD.
 *    If the length of either line segment is zero, the results are undefined.
 */
extern void dmPointToPoint(dmMatrix m,
                           dmPoint  a,
                           dmPoint  b,
                           dmPoint  c,
                           dmPoint  d);



/*
 * version function
 */
extern void               dmVersion(FILE *fp);


    
#ifdef __cplusplus
}
#endif
#endif /*_DMTYPES_H */
