git.mutantstargoat.com Git - freeglut/blob - progs/demos/shapes/glmatrix.c

   1 #include <string.h>\r
   2 #define _USE_MATH_DEFINES\r
   3 #include <math.h>\r
   4 #include "glmatrix.h"\r
   5 \r
   6 #ifndef M_PI\r
   7 #define M_PI    3.141592653589793\r
   8 #endif\r
   9 \r
  10 #define MMODE_IDX(x)    ((x) - GL_MODELVIEW)\r
  11 #define MAT_STACK_SIZE  32\r
  12 #define MAT_IDENT       {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}\r
  13 \r
  14 static int mm_idx = 0;\r
  15 static float mat_stack[3][MAT_STACK_SIZE][16] = {{MAT_IDENT}, {MAT_IDENT}, {MAT_IDENT}};\r
  16 static int stack_top[3];\r
  17 \r
  18 void gl_matrix_mode(int mm)\r
  19 {\r
  20     mm_idx = MMODE_IDX(mm);\r
  21 }\r
  22 \r
  23 void gl_push_matrix(void)\r
  24 {\r
  25     int top = stack_top[mm_idx];\r
  26 \r
  27     memcpy(mat_stack[mm_idx][top + 1], mat_stack[mm_idx][top], 16 * sizeof(float));\r
  28     stack_top[mm_idx]++;\r
  29 }\r
  30 \r
  31 void gl_pop_matrix(void)\r
  32 {\r
  33     stack_top[mm_idx]--;\r
  34 }\r
  35 \r
  36 void gl_load_identity(void)\r
  37 {\r
  38     static const float idmat[] = MAT_IDENT;\r
  39     int top = stack_top[mm_idx];\r
  40     float *mat = mat_stack[mm_idx][top];\r
  41 \r
  42     memcpy(mat, idmat, sizeof idmat);\r
  43 }\r
  44 \r
  45 void gl_load_matrixf(const float *m)\r
  46 {\r
  47     int top = stack_top[mm_idx];\r
  48     float *mat = mat_stack[mm_idx][top];\r
  49 \r
  50     memcpy(mat, m, 16 * sizeof *mat);\r
  51 }\r
  52 \r
  53 #define M4(i, j)        ((i << 2) + j)\r
  54 \r
  55 void gl_mult_matrixf(const float *m2)\r
  56 {\r
  57     int i, j;\r
  58     int top = stack_top[mm_idx];\r
  59     float *m1 = mat_stack[mm_idx][top];\r
  60     float res[16];\r
  61 \r
  62     for(i=0; i<4; i++) {\r
  63         for(j=0; j<4; j++) {\r
  64             res[M4(i,j)] = m1[M4(i,0)] * m2[M4(0,j)] +\r
  65                         m1[M4(i,1)] * m2[M4(1,j)] +\r
  66                         m1[M4(i,2)] * m2[M4(2,j)] +\r
  67                         m1[M4(i,3)] * m2[M4(3,j)];\r
  68         }\r
  69     }\r
  70 \r
  71     memcpy(m1, res, sizeof res);\r
  72 }\r
  73 \r
  74 void gl_translatef(float x, float y, float z)\r
  75 {\r
  76     float mat[] = MAT_IDENT;\r
  77 \r
  78     mat[12] = x;\r
  79     mat[13] = y;\r
  80     mat[14] = z;\r
  81 \r
  82     gl_mult_matrixf(mat);\r
  83 }\r
  84 \r
  85 void gl_rotatef(float angle, float x, float y, float z)\r
  86 {\r
  87     float mat[] = MAT_IDENT;\r
  88 \r
  89     float angle_rad = (float)M_PI * angle / 180.f;\r
  90     float sina = (float)sin(angle_rad);\r
  91     float cosa = (float)cos(angle_rad);\r
  92     float one_minus_cosa = 1.f - cosa;\r
  93     float nxsq = x * x;\r
  94     float nysq = y * y;\r
  95     float nzsq = z * z;\r
  96 \r
  97     mat[0] = nxsq + (1.f - nxsq) * cosa;\r
  98     mat[4] = x * y * one_minus_cosa - z * sina;\r
  99     mat[8] = x * z * one_minus_cosa + y * sina;\r
 100     mat[1] = x * y * one_minus_cosa + z * sina;\r
 101     mat[5] = nysq + (1.f - nysq) * cosa;\r
 102     mat[9] = y * z * one_minus_cosa - x * sina;\r
 103     mat[2] = x * z * one_minus_cosa - y * sina;\r
 104     mat[6] = y * z * one_minus_cosa + x * sina;\r
 105     mat[10] = nzsq + (1.f - nzsq) * cosa;\r
 106 \r
 107     gl_mult_matrixf(mat);\r
 108 }\r
 109 \r
 110 void gl_scalef(float x, float y, float z)\r
 111 {\r
 112     float mat[] = MAT_IDENT;\r
 113 \r
 114     mat[0] = x;\r
 115     mat[5] = y;\r
 116     mat[10] = z;\r
 117 \r
 118     gl_mult_matrixf(mat);\r
 119 }\r
 120 \r
 121 void gl_ortho(float left, float right, float bottom, float top, float znear, float zfar)\r
 122 {\r
 123     float mat[] = MAT_IDENT;\r
 124 \r
 125     float dx = right - left;\r
 126     float dy = top - bottom;\r
 127     float dz = zfar - znear;\r
 128 \r
 129     float tx = -(right + left) / dx;\r
 130     float ty = -(top + bottom) / dy;\r
 131     float tz = -(zfar + znear) / dz;\r
 132 \r
 133     float sx = 2.f / dx;\r
 134     float sy = 2.f / dy;\r
 135     float sz = -2.f / dz;\r
 136 \r
 137     mat[0] = sx;\r
 138     mat[5] = sy;\r
 139     mat[10] = sz;\r
 140     mat[12] = tx;\r
 141     mat[13] = ty;\r
 142     mat[14] = tz;\r
 143 \r
 144     gl_mult_matrixf(mat);\r
 145 }\r
 146 \r
 147 void gl_frustum(float left, float right, float bottom, float top, float znear, float zfar)\r
 148 {\r
 149     float mat[] = MAT_IDENT;\r
 150 \r
 151     float dx = right - left;\r
 152     float dy = top - bottom;\r
 153     float dz = zfar - znear;\r
 154 \r
 155     float a = (right + left) / dx;\r
 156     float b = (top + bottom) / dy;\r
 157     float c = -(zfar + znear) / dz;\r
 158     float d = -2.f * zfar * znear / dz;\r
 159 \r
 160     mat[0] = 2.f * znear / dx;\r
 161     mat[5] = 2.f * znear / dy;\r
 162     mat[8] = a;\r
 163     mat[9] = b;\r
 164     mat[10] = c;\r
 165     mat[11] = -1.f;\r
 166     mat[14] = d;\r
 167     mat[15] = 0;\r
 168 \r
 169     gl_mult_matrixf(mat);\r
 170 }\r
 171 \r
 172 void glu_perspective(float vfov, float aspect, float znear, float zfar)\r
 173 {\r
 174     float vfov_rad = (float)M_PI * vfov / 180.f;\r
 175     float x = znear * (float)tan(vfov_rad / 2.f);\r
 176     gl_frustum(-aspect * x, aspect * x, -x, x, znear, zfar);\r
 177 }\r
 178 \r
 179 /* return the matrix (16 elements, 4x4 matrix, row-major order */\r
 180 float* get_matrix(int mm)\r
 181 {\r
 182     int idx = MMODE_IDX(mm);\r
 183     int top = stack_top[idx];\r
 184     return mat_stack[idx][top];\r
 185 }\r
 186 \r
 187 \r
 188 #define M3(i, j)        ((i * 3) + j)\r
 189 static float inv_transpose_result[9];\r
 190 \r
 191 /* return the inverse transpose of the left-upper 3x3 of a matrix\r
 192    The returned pointer is only valid until the next time this function is\r
 193    called, so make a deep copy when you want to keep it around.\r
 194  */\r
 195 float* get_inv_transpose_3x3(int mm)\r
 196 {\r
 197     int idx = MMODE_IDX(mm);\r
 198     int top = stack_top[idx];\r
 199     float *m1 = mat_stack[idx][top];\r
 200     \r
 201 \r
 202     float determinant = +m1[M4(0,0)]*(m1[M4(1,1)]*m1[M4(2,2)]-m1[M4(2,1)]*m1[M4(1,2)])\r
 203                         -m1[M4(0,1)]*(m1[M4(1,0)]*m1[M4(2,2)]-m1[M4(1,2)]*m1[M4(2,0)])\r
 204                         +m1[M4(0,2)]*(m1[M4(1,0)]*m1[M4(2,1)]-m1[M4(1,1)]*m1[M4(2,0)]);\r
 205 \r
 206     float invdet = 1/determinant;\r
 207 \r
 208     inv_transpose_result[M3(0,0)] =  (m1[M4(1,1)]*m1[M4(2,2)]-m1[M4(2,1)]*m1[M4(1,2)])*invdet;\r
 209     inv_transpose_result[M3(1,0)] = -(m1[M4(0,1)]*m1[M4(2,2)]-m1[M4(0,2)]*m1[M4(2,1)])*invdet;\r
 210     inv_transpose_result[M3(2,0)] =  (m1[M4(0,1)]*m1[M4(1,2)]-m1[M4(0,2)]*m1[M4(1,1)])*invdet;\r
 211     inv_transpose_result[M3(0,1)] = -(m1[M4(1,0)]*m1[M4(2,2)]-m1[M4(1,2)]*m1[M4(2,0)])*invdet;\r
 212     inv_transpose_result[M3(1,1)] =  (m1[M4(0,0)]*m1[M4(2,2)]-m1[M4(0,2)]*m1[M4(2,0)])*invdet;\r
 213     inv_transpose_result[M3(2,1)] = -(m1[M4(0,0)]*m1[M4(1,2)]-m1[M4(1,0)]*m1[M4(0,2)])*invdet;\r
 214     inv_transpose_result[M3(0,2)] =  (m1[M4(1,0)]*m1[M4(2,1)]-m1[M4(2,0)]*m1[M4(1,1)])*invdet;\r
 215     inv_transpose_result[M3(1,2)] = -(m1[M4(0,0)]*m1[M4(2,1)]-m1[M4(2,0)]*m1[M4(0,1)])*invdet;\r
 216     inv_transpose_result[M3(2,2)] =  (m1[M4(0,0)]*m1[M4(1,1)]-m1[M4(1,0)]*m1[M4(0,1)])*invdet;\r
 217 \r
 218     return inv_transpose_result;\r
 219 }\r