/* * 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 #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 #else #include #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 #include #include #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 #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 */