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