include/Matrix3D.h

   1 #ifndef MATRIX_3D_H
   2 #define MATRIX_3D_H
   3 /*
   4 # Matrix3D.h  1.13
   5 # The latest update : 10/01/97 at 02:34:25
   6 #
   7 #@(#) Matrix3D.h ver 1.13
   8 #@(#) Created by
   9 #@(#)
  10 #@(#) Usage : Matrix3D.h
  11 #t@(#) Attention
  12 #@(#)
  13 */
  14 /* constant begin */
  15 #define MATRIX_3D_WIDTH  4
  16 #define MATRIX_3D_HEIGHT 4
  17
  18 #define MATRIX_3D_MODE_NOT_INITIALIZE 0
  19 #define MATRIX_3D_MODE_INITIALIZE 1
  20
  21 #define MATRIX_3D_NEGLECT_VALUE 1e-6
  22
  23 /* constant end */
  24
  25 /* struct begin */
  26 typedef float matrix3DParaTypeReal;
  27 typedef matrix3DParaTypeReal Matrix3D[MATRIX_3D_WIDTH][MATRIX_3D_HEIGHT];
  28
  29 typedef char  matrix3DEulerAngleMode;
  30
  31 typedef struct EulerAngleInfo {
  32         char   EAMode[5];
  33         double Rot1;
  34         double Rot2;
  35         double Rot3;
  36 } EulerAngleInfo;
  37
  38 /* struct end */
  39
  40 /*
  41         Matrix3D: Attention Arrays Order
  42
  43         [0][0] [1][0] [2][0] [3][0]
  44         [0][1] [1][1] [2][1] [3][1]
  45         [0][2] [1][2] [2][2] [3][2]
  46         [0][3] [1][3] [2][3] [3][3]
  47 */
  48
  49 #include "Vector.h"
  50 #include <stdio.h>
  51
  52 #ifdef __cplusplus
  53 extern "C" {
  54 #endif
  55
  56 /* prototype begin */
  57
  58 extern void matrix3DInit(Matrix3D Matrix);
  59
  60 /* matrix3DUtil.c */
  61 #include "Array.h"
  62 extern void matrix3DCopy(Matrix3D dst, Matrix3D src);
  63
  64 extern void matrix3DFromRotationMatrix(Matrix3D dst, Array a);
  65 extern void matrix3DInverse(Matrix3D mat);
  66
  67 extern void matrix3DMultiply(Matrix3D A, Matrix3D B);     /* A = A*B */
  68 extern void matrix3DMultiplyInv(Matrix3D A, Matrix3D B);  /* B = A*B */
  69
  70 extern void matrix3DMultiplyVector(floatVector* v, Matrix3D A);
  71 extern void matrix3DMultiplyVectors(floatVector* vs, int n, Matrix3D A);
  72
  73 extern void matrix3DFileFormat(FILE* fpt);
  74 extern void matrix3DFileRead(FILE* fpt, Matrix3D Matrix);
  75 extern void matrix3DFileWrite(FILE* fpt, Matrix3D Matrix);
  76
  77
  78 extern void matrix3DRotationSet(Matrix3D Matrix, char mode, matrix3DParaTypeReal rot, long Mode);
  79 extern void matrix3DTranslationSet(Matrix3D Matrix, matrix3DParaTypeReal rotx, matrix3DParaTypeReal roty, matrix3DParaTypeReal rotz, long mode);
  80 extern void matrix3DRotationSetXYZ(Matrix3D Matrix, matrix3DParaTypeReal rotx, matrix3DParaTypeReal roty, matrix3DParaTypeReal rotz, long mode);
  81 extern void matrix3DRotationSetZYX(Matrix3D Matrix, matrix3DParaTypeReal rotx, matrix3DParaTypeReal roty, matrix3DParaTypeReal rotz, long mode);
  82 extern void matrix3DRotationSetZXY(Matrix3D Matrix, matrix3DParaTypeReal rotx, matrix3DParaTypeReal roty, matrix3DParaTypeReal rotz, long mode);
  83 extern void matrix3DRotationSetYXZ(Matrix3D Matrix, matrix3DParaTypeReal rotx, matrix3DParaTypeReal roty, matrix3DParaTypeReal rotz, long mode);
  84
  85 /*
  86  *
  87 */
  88 extern void matrix3DRotationSetFollowingEulerAngle(Matrix3D Matrix, const char Mode[4], matrix3DParaTypeReal rot1, matrix3DParaTypeReal rot2, matrix3DParaTypeReal rot3, long mode);
  89 extern void matrix3DRotationAntiSetFollowingEulerAngle(Matrix3D Matrix, const char Mode[4], matrix3DParaTypeReal rot1, matrix3DParaTypeReal rot2, matrix3DParaTypeReal rot3, long mode);
  90 extern void matrix3DEulerAngleGetFromMatrix3D(Matrix3D Matrix, const char Mode[4], matrix3DParaTypeReal* rot1, matrix3DParaTypeReal* rot2, matrix3DParaTypeReal* rot3, long mode);
  91
  92
  93 /* prototype end */
  94
  95 #ifdef __cplusplus
  96 };
  97 #endif
  98
  99 /*** Definitions ***/
 100 /* struct begin */
 101 typedef struct {float x, y, z, w;} Quat; /* Quaternion */
 102 typedef matrix3DParaTypeReal HMatrix3D[4][4];
 103 typedef Quat EulerAngles;    /* (x,y,z)=ang 1,2,3, w=order code  */
 104 /* struct end */
 105 /* constant begin */
 106 enum QuatPart {X, Y, Z, W};
 107 /* constant end */
 108
 109 /* by Ken Shoemake, 1993 */
 110 /*** Order type constants, constructors, extractors ***/
 111     /* There are 24 possible conventions, designated by:       */
 112     /*    o EulAxI = axis used initially                   : 3 */
 113     /*    o EulPar = parity of axis permutation            : 2 */
 114     /*    o EulRep = repetition of initial axis as last    : 2 */
 115     /*    o EulFrm = frame from which axes are taken       : 2 */
 116     /* Axes I,J,K will be a permutation of X,Y,Z.              */
 117     /* Axis H will be either I or K, depending on EulRep.      */
 118     /* Frame S takes axes from initial static frame.           */
 119     /* If ord = (AxI=X, Par=Even, Rep=No, Frm=S), then         */
 120     /* {a,b,c,ord} means Rz(c)Ry(b)Rx(a), where Rz(c)v         */
 121     /* rotates v around Z by c radians.                                */
 122
 123 /* constant begin */
 124 #define EulFrmS      0
 125 #define EulFrmR      1
 126 #define EulFrm(ord)  ((unsigned)(ord)&1)
 127 #define EulRepNo     0
 128 #define EulRepYes    1
 129 #define EulRep(ord)  (((unsigned)(ord)>>1)&1)
 130 #define EulParEven   0
 131 #define EulParOdd    1
 132 #define EulPar(ord)  (((unsigned)(ord)>>2)&1)
 133 #define EulSafe      "\000\001\002\000"
 134 #define EulNext      "\001\002\000\001"
 135 /* constant end */
 136
 137 /* prototype begin */
 138 #define EulAxI(ord)  ((int)(EulSafe[(((unsigned)(ord)>>3)&3)]))
 139 #define EulAxJ(ord)  ((int)(EulNext[EulAxI(ord)+(EulPar(ord)==EulParOdd)]))
 140 #define EulAxK(ord)  ((int)(EulNext[EulAxI(ord)+(EulPar(ord)!=EulParOdd)]))
 141 #define EulAxH(ord)  ((EulRep(ord)==EulRepNo)?EulAxK(ord):EulAxI(ord))
 142     /* EulGetOrd unpacks all useful information about order simultaneously. */
 143 #define EulGetOrd(ord,i,j,k,h,n,s,f) {unsigned o=ord;f=o&1;o>>=1;s=o&1;o>>=1;\
 144     n=o&1;o>>=1;i=EulSafe[o&3];j=EulNext[i+n];k=EulNext[i+1-n];h=s?k:i;}
 145     /* EulOrd creates an order value between 0 and 23 from 4-tuple choices. */
 146 #define EulOrd(i,p,r,f)    (((((((i)<<1)+(p))<<1)+(r))<<1)+(f))
 147     /* Static axes */
 148 #define EulOrdXYZs    EulOrd(X,EulParEven,EulRepNo,EulFrmS)
 149 #define EulOrdXYXs    EulOrd(X,EulParEven,EulRepYes,EulFrmS)
 150 #define EulOrdXZYs    EulOrd(X,EulParOdd,EulRepNo,EulFrmS)
 151 #define EulOrdXZXs    EulOrd(X,EulParOdd,EulRepYes,EulFrmS)
 152 #define EulOrdYZXs    EulOrd(Y,EulParEven,EulRepNo,EulFrmS)
 153 #define EulOrdYZYs    EulOrd(Y,EulParEven,EulRepYes,EulFrmS)
 154 #define EulOrdYXZs    EulOrd(Y,EulParOdd,EulRepNo,EulFrmS)
 155 #define EulOrdYXYs    EulOrd(Y,EulParOdd,EulRepYes,EulFrmS)
 156 #define EulOrdZXYs    EulOrd(Z,EulParEven,EulRepNo,EulFrmS)
 157 #define EulOrdZXZs    EulOrd(Z,EulParEven,EulRepYes,EulFrmS)
 158 #define EulOrdZYXs    EulOrd(Z,EulParOdd,EulRepNo,EulFrmS)
 159 #define EulOrdZYZs    EulOrd(Z,EulParOdd,EulRepYes,EulFrmS)
 160     /* Rotating axes */
 161 #define EulOrdZYXr    EulOrd(X,EulParEven,EulRepNo,EulFrmR)
 162 #define EulOrdXYXr    EulOrd(X,EulParEven,EulRepYes,EulFrmR)
 163 #define EulOrdYZXr    EulOrd(X,EulParOdd,EulRepNo,EulFrmR)
 164 #define EulOrdXZXr    EulOrd(X,EulParOdd,EulRepYes,EulFrmR)
 165 #define EulOrdXZYr    EulOrd(Y,EulParEven,EulRepNo,EulFrmR)
 166 #define EulOrdYZYr    EulOrd(Y,EulParEven,EulRepYes,EulFrmR)
 167 #define EulOrdZXYr    EulOrd(Y,EulParOdd,EulRepNo,EulFrmR)
 168 #define EulOrdYXYr    EulOrd(Y,EulParOdd,EulRepYes,EulFrmR)
 169 #define EulOrdYXZr    EulOrd(Z,EulParEven,EulRepNo,EulFrmR)
 170 #define EulOrdZXZr    EulOrd(Z,EulParEven,EulRepYes,EulFrmR)
 171 #define EulOrdXYZr    EulOrd(Z,EulParOdd,EulRepNo,EulFrmR)
 172 #define EulOrdZYZr    EulOrd(Z,EulParOdd,EulRepYes,EulFrmR)
 173
 174 EulerAngles Eul_(float ai, float aj, float ah, int order);
 175 Quat Eul_ToQuat(EulerAngles ea);
 176 void Eul_ToHMatrix(EulerAngles ea, HMatrix3D M);
 177 EulerAngles Eul_FromHMatrix(HMatrix3D M, int order);
 178 EulerAngles Eul_FromQuat(Quat q, int order);
 179 /* prototype end */
 180
 181 #endif /* MATRIX_3D_H */