bool ScreenProject::invertMatrix(const double * m, double * out) const { /* * Compute inverse of 4x4 transformation matrix. * Code contributed by Jacques Leroy [email protected] * Return GL_TRUE for success, GL_FALSE for failure (singular matrix) */ // http://www.mesa3d.org/license.html // http://webcvs.freedesktop.org/mesa/Mesa/src/glu/mesa/project.c?content-type=text%2Fplain&view=co //static GLboolean invert_matrix(const GLdouble * m, GLdouble * out) /* NB. OpenGL Matrices are COLUMN major. */ #define SWAP_ROWS(a, b) { GLdouble *_tmp = a; (a)=(b); (b)=_tmp; } #define MAT(m,r,c) (m)[(c)*4+(r)] double wtmp[4][8]; double m0, m1, m2, m3, s; double *r0, *r1, *r2, *r3; r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3]; r0[0] = MAT(m, 0, 0), r0[1] = MAT(m, 0, 1), r0[2] = MAT(m, 0, 2), r0[3] = MAT(m, 0, 3), r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0, r1[0] = MAT(m, 1, 0), r1[1] = MAT(m, 1, 1), r1[2] = MAT(m, 1, 2), r1[3] = MAT(m, 1, 3), r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0, r2[0] = MAT(m, 2, 0), r2[1] = MAT(m, 2, 1), r2[2] = MAT(m, 2, 2), r2[3] = MAT(m, 2, 3), r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0, r3[0] = MAT(m, 3, 0), r3[1] = MAT(m, 3, 1), r3[2] = MAT(m, 3, 2), r3[3] = MAT(m, 3, 3), r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0; /* choose pivot - or die */ if (fabs(r3[0]) > fabs(r2[0])) SWAP_ROWS(r3, r2); if (fabs(r2[0]) > fabs(r1[0])) SWAP_ROWS(r2, r1); if (fabs(r1[0]) > fabs(r0[0])) SWAP_ROWS(r1, r0); if (0.0 == r0[0]) return false; /* eliminate first variable */ m1 = r1[0] / r0[0]; m2 = r2[0] / r0[0]; m3 = r3[0] / r0[0]; s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s; s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s; s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s; s = r0[4]; if (s != 0.0) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; } s = r0[5]; if (s != 0.0) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; } s = r0[6]; if (s != 0.0) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; } s = r0[7]; if (s != 0.0) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; } /* choose pivot - or die */ if (fabs(r3[1]) > fabs(r2[1])) SWAP_ROWS(r3, r2); if (fabs(r2[1]) > fabs(r1[1])) SWAP_ROWS(r2, r1); if (0.0 == r1[1]) return false; /* eliminate second variable */ m2 = r2[1] / r1[1]; m3 = r3[1] / r1[1]; r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2]; r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3]; s = r1[4]; if (0.0 != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; } s = r1[5]; if (0.0 != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; } s = r1[6]; if (0.0 != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; } s = r1[7]; if (0.0 != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; } /* choose pivot - or die */ if (fabs(r3[2]) > fabs(r2[2])) SWAP_ROWS(r3, r2); if (0.0 == r2[2]) return false; /* eliminate third variable */ m3 = r3[2] / r2[2]; r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4], r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6], r3[7] -= m3 * r2[7]; /* last check */ if (0.0 == r3[3]) return false; s = 1.0 / r3[3]; /* now back substitute row 3 */ r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s; m2 = r2[3]; /* now back substitute row 2 */ s = 1.0 / r2[2]; r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2), r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2); m1 = r1[3]; r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1, r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1; m0 = r0[3]; r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0, r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0; m1 = r1[2]; /* now back substitute row 1 */ s = 1.0 / r1[1]; r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1), r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1); m0 = r0[2]; r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0, r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0; m0 = r0[1]; /* now back substitute row 0 */ s = 1.0 / r0[0]; r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0), r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0); MAT(out, 0, 0) = r0[4]; MAT(out, 0, 1) = r0[5], MAT(out, 0, 2) = r0[6]; MAT(out, 0, 3) = r0[7], MAT(out, 1, 0) = r1[4]; MAT(out, 1, 1) = r1[5], MAT(out, 1, 2) = r1[6]; MAT(out, 1, 3) = r1[7], MAT(out, 2, 0) = r2[4]; MAT(out, 2, 1) = r2[5], MAT(out, 2, 2) = r2[6]; MAT(out, 2, 3) = r2[7], MAT(out, 3, 0) = r3[4]; MAT(out, 3, 1) = r3[5], MAT(out, 3, 2) = r3[6]; MAT(out, 3, 3) = r3[7]; return true; #undef MAT #undef SWAP_ROWS }
compress_data (j_compress_ptr cinfo, JSAMPIMAGE input_buf) { j_lossless_c_ptr losslsc = (j_lossless_c_ptr) cinfo->codec; c_diff_ptr diff = (c_diff_ptr) losslsc->diff_private; JDIMENSION MCU_col_num; /* index of current MCU within row */ JDIMENSION MCU_count; /* number of MCUs encoded */ /* JDIMENSION last_MCU_col = cinfo->MCUs_per_row - 1; */ JDIMENSION last_iMCU_row = cinfo->total_iMCU_rows - 1; int comp, ci, yoffset, samp_row, samp_rows, samps_across; jpeg_component_info *compptr; /* Loop to write as much as one whole iMCU row */ for (yoffset = diff->MCU_vert_offset; yoffset < diff->MCU_rows_per_iMCU_row; yoffset++) { MCU_col_num = diff->mcu_ctr; /* Scale and predict each scanline of the MCU-row separately. * * Note: We only do this if we are at the start of a MCU-row, ie, * we don't want to reprocess a row suspended by the output. */ if (MCU_col_num == 0) { for (comp = 0; comp < cinfo->comps_in_scan; comp++) { compptr = cinfo->cur_comp_info[comp]; ci = compptr->component_index; if (diff->iMCU_row_num < last_iMCU_row) samp_rows = compptr->v_samp_factor; else { /* NB: can't use last_row_height here, since may not be set! */ samp_rows = (int) (compptr->height_in_data_units % compptr->v_samp_factor); if (samp_rows == 0) samp_rows = compptr->v_samp_factor; else { /* Fill dummy difference rows at the bottom edge with zeros, which * will encode to the smallest amount of data. */ for (samp_row = samp_rows; samp_row < compptr->v_samp_factor; samp_row++) MEMZERO(diff->diff_buf[ci][samp_row], jround_up((long) compptr->width_in_data_units, (long) compptr->h_samp_factor) * SIZEOF(JDIFF)); } } samps_across = compptr->width_in_data_units; for (samp_row = 0; samp_row < samp_rows; samp_row++) { (*losslsc->scaler_scale) (cinfo, input_buf[ci][samp_row], diff->cur_row[ci], samps_across); (*losslsc->predict_difference[ci]) (cinfo, ci, diff->cur_row[ci], diff->prev_row[ci], diff->diff_buf[ci][samp_row], samps_across); SWAP_ROWS(diff->cur_row[ci], diff->prev_row[ci]); } } } /* Try to write the MCU-row (or remaining portion of suspended MCU-row). */ MCU_count = (*losslsc->entropy_encode_mcus) (cinfo, diff->diff_buf, yoffset, MCU_col_num, cinfo->MCUs_per_row - MCU_col_num); if (MCU_count != cinfo->MCUs_per_row - MCU_col_num) { /* Suspension forced; update state counters and exit */ diff->MCU_vert_offset = yoffset; diff->mcu_ctr += MCU_col_num; return FALSE; } /* Completed an MCU row, but perhaps not an iMCU row */ diff->mcu_ctr = 0; } /* Completed the iMCU row, advance counters for next one */ diff->iMCU_row_num++; start_iMCU_row(cinfo); return TRUE; }
int invert(Matrix4x4* mat, Matrix4x4* inv) { double wtmp[4][8]; double m0, m1, m2, m3, s; double *r0, *r1, *r2, *r3; r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3]; r0[0] = MAT(mat,0,0); r0[1] = MAT(mat,0,1); r0[2] = MAT(mat,0,2); r0[3] = MAT(mat,0,3); r0[4] = 1.0; r0[5] = r0[6] = r0[7] = 0.0; r1[0] = MAT(mat,1,0); r1[1] = MAT(mat,1,1); r1[2] = MAT(mat,1,2); r1[3] = MAT(mat,1,3); r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0; r2[0] = MAT(mat,2,0); r2[1] = MAT(mat,2,1); r2[2] = MAT(mat,2,2); r2[3] = MAT(mat,2,3); r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0; r3[0] = MAT(mat,3,0); r3[1] = MAT(mat,3,1); r3[2] = MAT(mat,3,2); r3[3] = MAT(mat,3,3); r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0; /* choose pivot - or die */ if (fabs(r3[0])>fabs(r2[0])) SWAP_ROWS(r3, r2); if (fabs(r2[0])>fabs(r1[0])) SWAP_ROWS(r2, r1); if (fabs(r1[0])>fabs(r0[0])) SWAP_ROWS(r1, r0); if (0.0 == r0[0]) return 0; /* eliminate first variable */ m1 = r1[0]/r0[0]; m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0]; s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s; s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s; s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s; s = r0[4]; if (s != 0.0) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; } s = r0[5]; if (s != 0.0) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; } s = r0[6]; if (s != 0.0) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; } s = r0[7]; if (s != 0.0) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; } /* choose pivot - or die */ if (fabs(r3[1])>fabs(r2[1])) SWAP_ROWS(r3, r2); if (fabs(r2[1])>fabs(r1[1])) SWAP_ROWS(r2, r1); if (0.0 == r1[1]) return 0; /* eliminate second variable */ m2 = r2[1]/r1[1]; m3 = r3[1]/r1[1]; r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2]; r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3]; s = r1[4]; if (0.0 != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; } s = r1[5]; if (0.0 != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; } s = r1[6]; if (0.0 != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; } s = r1[7]; if (0.0 != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; } /* choose pivot - or die */ if (fabs(r3[2])>fabs(r2[2])) SWAP_ROWS(r3, r2); if (0.0 == r2[2]) return 0; /* eliminate third variable */ m3 = r3[2]/r2[2]; r3[3] -= m3 * r2[3]; r3[4] -= m3 * r2[4]; r3[5] -= m3 * r2[5]; r3[6] -= m3 * r2[6]; r3[7] -= m3 * r2[7]; /* last check */ if (0.0 == r3[3]) return 0; s = 1.0F/r3[3]; /* now back substitute row 3 */ r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s; m2 = r2[3]; /* now back substitute row 2 */ s = 1.0F/r2[2]; r2[4] = s * (r2[4] - r3[4] * m2); r2[5] = s * (r2[5] - r3[5] * m2); r2[6] = s * (r2[6] - r3[6] * m2); r2[7] = s * (r2[7] - r3[7] * m2); m1 = r1[3]; r1[4] -= r3[4] * m1; r1[5] -= r3[5] * m1; r1[6] -= r3[6] * m1; r1[7] -= r3[7] * m1; m0 = r0[3]; r0[4] -= r3[4] * m0; r0[5] -= r3[5] * m0; r0[6] -= r3[6] * m0; r0[7] -= r3[7] * m0; m1 = r1[2]; /* now back substitute row 1 */ s = 1.0F/r1[1]; r1[4] = s * (r1[4] - r2[4] * m1); r1[5] = s * (r1[5] - r2[5] * m1); r1[6] = s * (r1[6] - r2[6] * m1); r1[7] = s * (r1[7] - r2[7] * m1); m0 = r0[2]; r0[4] -= r2[4] * m0; r0[5] -= r2[5] * m0; r0[6] -= r2[6] * m0; r0[7] -= r2[7] * m0; m0 = r0[1]; /* now back substitute row 0 */ s = 1.0F/r0[0]; r0[4] = s * (r0[4] - r1[4] * m0); r0[5] = s * (r0[5] - r1[5] * m0); r0[6] = s * (r0[6] - r1[6] * m0); r0[7] = s * (r0[7] - r1[7] * m0); MAT(inv,0,0) = r0[4]; MAT(inv,0,1) = r0[5], MAT(inv,0,2) = r0[6]; MAT(inv,0,3) = r0[7], MAT(inv,1,0) = r1[4]; MAT(inv,1,1) = r1[5], MAT(inv,1,2) = r1[6]; MAT(inv,1,3) = r1[7], MAT(inv,2,0) = r2[4]; MAT(inv,2,1) = r2[5], MAT(inv,2,2) = r2[6]; MAT(inv,2,3) = r2[7], MAT(inv,3,0) = r3[4]; MAT(inv,3,1) = r3[5], MAT(inv,3,2) = r3[6]; MAT(inv,3,3) = r3[7]; return 1; }
// Matrix Inverse bool Matrix::Invert() { #define SWAP_ROWS(a , b) { float *_tmp = a; (a)=(b); (b)=_tmp; } #define MAT(m,r,c) ((m)[(c)*4+(r)]) float dst[16]; //destination matrix (i.e. inverse) float wtmp[4][8]; float m0, m1, m2, m3, s; float *r0, *r1, *r2, *r3; r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3]; r0[0] = MAT(fMatrix,0,0), r0[1] = MAT(fMatrix,0,1), r0[2] = MAT(fMatrix,0,2), r0[3] = MAT(fMatrix,0,3), r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0, r1[0] = MAT(fMatrix,1,0), r1[1] = MAT(fMatrix,1,1), r1[2] = MAT(fMatrix,1,2), r1[3] = MAT(fMatrix,1,3), r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0, r2[0] = MAT(fMatrix,2,0), r2[1] = MAT(fMatrix,2,1), r2[2] = MAT(fMatrix,2,2), r2[3] = MAT(fMatrix,2,3), r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0, r3[0] = MAT(fMatrix,3,0), r3[1] = MAT(fMatrix,3,1), r3[2] = MAT(fMatrix,3,2), r3[3] = MAT(fMatrix,3,3), r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0; /* choose pivot - or die */ if (fabs(r3[0])>fabs(r2[0])) SWAP_ROWS(r3, r2); if (fabs(r2[0])>fabs(r1[0])) SWAP_ROWS(r2, r1); if (fabs(r1[0])>fabs(r0[0])) SWAP_ROWS(r1, r0); if (0.0 == r0[0]) return false; /* eliminate first variable */ m1 = r1[0]/r0[0]; m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0]; s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s; s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s; s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s; s = r0[4]; if (s != 0.0) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; } s = r0[5]; if (s != 0.0) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; } s = r0[6]; if (s != 0.0) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; } s = r0[7]; if (s != 0.0) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; } /* choose pivot - or die */ if (fabs(r3[1])>fabs(r2[1])) SWAP_ROWS(r3, r2); if (fabs(r2[1])>fabs(r1[1])) SWAP_ROWS(r2, r1); if (0.0 == r1[1]) return false; /* eliminate second variable */ m2 = r2[1]/r1[1]; m3 = r3[1]/r1[1]; r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2]; r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3]; s = r1[4]; if (0.0 != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; } s = r1[5]; if (0.0 != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; } s = r1[6]; if (0.0 != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; } s = r1[7]; if (0.0 != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; } /* choose pivot - or die */ if (fabs(r3[2])>fabs(r2[2])) SWAP_ROWS(r3, r2); if (0.0 == r2[2]) return false; /* eliminate third variable */ m3 = r3[2]/r2[2]; r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4], r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6], r3[7] -= m3 * r2[7]; /* last check */ if (0.0 == r3[3]) return false; s = 1.0f/r3[3]; /* now back substitute row 3 */ r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s; m2 = r2[3]; /* now back substitute row 2 */ s = 1.0f/r2[2]; r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2), r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2); m1 = r1[3]; r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1, r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1; m0 = r0[3]; r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0, r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0; m1 = r1[2]; /* now back substitute row 1 */ s = 1.0f/r1[1]; r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1), r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1); m0 = r0[2]; r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0, r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0; m0 = r0[1]; /* now back substitute row 0 */ s = 1.0f/r0[0]; r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0), r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0); MAT(dst,0,0) = r0[4]; MAT(dst,0,1) = r0[5], MAT(dst,0,2) = r0[6]; MAT(dst,0,3) = r0[7], MAT(dst,1,0) = r1[4]; MAT(dst,1,1) = r1[5], MAT(dst,1,2) = r1[6]; MAT(dst,1,3) = r1[7], MAT(dst,2,0) = r2[4]; MAT(dst,2,1) = r2[5], MAT(dst,2,2) = r2[6]; MAT(dst,2,3) = r2[7], MAT(dst,3,0) = r3[4]; MAT(dst,3,1) = r3[5], MAT(dst,3,2) = r3[6]; MAT(dst,3,3) = r3[7]; memcpy (fMatrix, dst, 64); return true; #undef MAT #undef SWAP_ROWS }
/* * Compute inverse of 4x4 transformation matrix. * Code contributed by Jacques Leroy [email protected] * Return GL_TRUE for success, GL_FALSE for failure (singular matrix) */ static GLboolean invert_matrix( const GLdouble *m, GLdouble *out ){ /* NB. OpenGL Matrices are COLUMN major. */ #define SWAP_ROWS( a, b ) { GLdouble *_tmp = a; ( a ) = ( b ); ( b ) = _tmp; } #define MAT( m,r,c ) ( m )[( c ) * 4 + ( r )] GLdouble wtmp[4][8]; GLdouble m0, m1, m2, m3, s; GLdouble *r0, *r1, *r2, *r3; r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3]; r0[0] = MAT( m,0,0 ), r0[1] = MAT( m,0,1 ), r0[2] = MAT( m,0,2 ), r0[3] = MAT( m,0,3 ), r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0, r1[0] = MAT( m,1,0 ), r1[1] = MAT( m,1,1 ), r1[2] = MAT( m,1,2 ), r1[3] = MAT( m,1,3 ), r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0, r2[0] = MAT( m,2,0 ), r2[1] = MAT( m,2,1 ), r2[2] = MAT( m,2,2 ), r2[3] = MAT( m,2,3 ), r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0, r3[0] = MAT( m,3,0 ), r3[1] = MAT( m,3,1 ), r3[2] = MAT( m,3,2 ), r3[3] = MAT( m,3,3 ), r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0; /* choose pivot - or die */ if ( fabs( r3[0] ) > fabs( r2[0] ) ) { SWAP_ROWS( r3, r2 ); } if ( fabs( r2[0] ) > fabs( r1[0] ) ) { SWAP_ROWS( r2, r1 ); } if ( fabs( r1[0] ) > fabs( r0[0] ) ) { SWAP_ROWS( r1, r0 ); } if ( 0.0 == r0[0] ) { return GL_FALSE; } /* eliminate first variable */ m1 = r1[0] / r0[0]; m2 = r2[0] / r0[0]; m3 = r3[0] / r0[0]; s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s; s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s; s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s; s = r0[4]; if ( s != 0.0 ) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; } s = r0[5]; if ( s != 0.0 ) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; } s = r0[6]; if ( s != 0.0 ) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; } s = r0[7]; if ( s != 0.0 ) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; } /* choose pivot - or die */ if ( fabs( r3[1] ) > fabs( r2[1] ) ) { SWAP_ROWS( r3, r2 ); } if ( fabs( r2[1] ) > fabs( r1[1] ) ) { SWAP_ROWS( r2, r1 ); } if ( 0.0 == r1[1] ) { return GL_FALSE; } /* eliminate second variable */ m2 = r2[1] / r1[1]; m3 = r3[1] / r1[1]; r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2]; r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3]; s = r1[4]; if ( 0.0 != s ) { r2[4] -= m2 * s; r3[4] -= m3 * s; } s = r1[5]; if ( 0.0 != s ) { r2[5] -= m2 * s; r3[5] -= m3 * s; } s = r1[6]; if ( 0.0 != s ) { r2[6] -= m2 * s; r3[6] -= m3 * s; } s = r1[7]; if ( 0.0 != s ) { r2[7] -= m2 * s; r3[7] -= m3 * s; } /* choose pivot - or die */ if ( fabs( r3[2] ) > fabs( r2[2] ) ) { SWAP_ROWS( r3, r2 ); } if ( 0.0 == r2[2] ) { return GL_FALSE; } /* eliminate third variable */ m3 = r3[2] / r2[2]; r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4], r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6], r3[7] -= m3 * r2[7]; /* last check */ if ( 0.0 == r3[3] ) { return GL_FALSE; } s = 1.0 / r3[3]; /* now back substitute row 3 */ r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s; m2 = r2[3]; /* now back substitute row 2 */ s = 1.0 / r2[2]; r2[4] = s * ( r2[4] - r3[4] * m2 ), r2[5] = s * ( r2[5] - r3[5] * m2 ), r2[6] = s * ( r2[6] - r3[6] * m2 ), r2[7] = s * ( r2[7] - r3[7] * m2 ); m1 = r1[3]; r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1, r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1; m0 = r0[3]; r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0, r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0; m1 = r1[2]; /* now back substitute row 1 */ s = 1.0 / r1[1]; r1[4] = s * ( r1[4] - r2[4] * m1 ), r1[5] = s * ( r1[5] - r2[5] * m1 ), r1[6] = s * ( r1[6] - r2[6] * m1 ), r1[7] = s * ( r1[7] - r2[7] * m1 ); m0 = r0[2]; r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0, r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0; m0 = r0[1]; /* now back substitute row 0 */ s = 1.0 / r0[0]; r0[4] = s * ( r0[4] - r1[4] * m0 ), r0[5] = s * ( r0[5] - r1[5] * m0 ), r0[6] = s * ( r0[6] - r1[6] * m0 ), r0[7] = s * ( r0[7] - r1[7] * m0 ); MAT( out,0,0 ) = r0[4]; MAT( out,0,1 ) = r0[5], MAT( out,0,2 ) = r0[6]; MAT( out,0,3 ) = r0[7], MAT( out,1,0 ) = r1[4]; MAT( out,1,1 ) = r1[5], MAT( out,1,2 ) = r1[6]; MAT( out,1,3 ) = r1[7], MAT( out,2,0 ) = r2[4]; MAT( out,2,1 ) = r2[5], MAT( out,2,2 ) = r2[6]; MAT( out,2,3 ) = r2[7], MAT( out,3,0 ) = r3[4]; MAT( out,3,1 ) = r3[5], MAT( out,3,2 ) = r3[6]; MAT( out,3,3 ) = r3[7]; return GL_TRUE; #undef MAT #undef SWAP_ROWS }
/* Invert a row-major (C-style) 4x4 matrix. */ static void InvertMatrix(var *out, const var *m) { /* Assumes matrices are ROW major. */ #define SWAP_ROWS(a, b) { double *_tmp = a; (a)=(b); (b)=_tmp; } #define MAT(m,r,c) (m)[(r)*4+(c)] double wtmp[4][8]; double m0, m1, m2, m3, s; double *r0, *r1, *r2, *r3; r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3]; r0[0] = MAT(m,0,0), r0[1] = MAT(m,0,1), r0[2] = MAT(m,0,2), r0[3] = MAT(m,0,3), r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0, r1[0] = MAT(m,1,0), r1[1] = MAT(m,1,1), r1[2] = MAT(m,1,2), r1[3] = MAT(m,1,3), r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0, r2[0] = MAT(m,2,0), r2[1] = MAT(m,2,1), r2[2] = MAT(m,2,2), r2[3] = MAT(m,2,3), r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0, r3[0] = MAT(m,3,0), r3[1] = MAT(m,3,1), r3[2] = MAT(m,3,2), r3[3] = MAT(m,3,3), r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0; /* Choose myPivot, or die. */ if (fabs(r3[0])>fabs(r2[0])) SWAP_ROWS(r3, r2); if (fabs(r2[0])>fabs(r1[0])) SWAP_ROWS(r2, r1); if (fabs(r1[0])>fabs(r0[0])) SWAP_ROWS(r1, r0); /* Eliminate first variable. */ m1 = r1[0]/r0[0]; m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0]; s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s; s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s; s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s; s = r0[4]; if (s != 0.0) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; } s = r0[5]; if (s != 0.0) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; } s = r0[6]; if (s != 0.0) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; } s = r0[7]; if (s != 0.0) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; } /* Choose myPivot, or die. */ if (fabs(r3[1])>fabs(r2[1])) SWAP_ROWS(r3, r2); if (fabs(r2[1])>fabs(r1[1])) SWAP_ROWS(r2, r1); /* Eliminate second variable. */ m2 = r2[1]/r1[1]; m3 = r3[1]/r1[1]; r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2]; r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3]; s = r1[4]; if (0.0 != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; } s = r1[5]; if (0.0 != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; } s = r1[6]; if (0.0 != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; } s = r1[7]; if (0.0 != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; } /* Choose myPivot, or die. */ if (fabs(r3[2])>fabs(r2[2])) SWAP_ROWS(r3, r2); /* Eliminate third variable. */ m3 = r3[2]/r2[2]; r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4], r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6], r3[7] -= m3 * r2[7]; s = 1.0/r3[3]; /* Now back substitute row 3. */ r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s; m2 = r2[3]; /* Now back substitute row 2. */ s = 1.0/r2[2]; r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2), r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2); m1 = r1[3]; r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1, r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1; m0 = r0[3]; r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0, r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0; m1 = r1[2]; /* Now back substitute row 1. */ s = 1.0/r1[1]; r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1), r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1); m0 = r0[2]; r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0, r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0; m0 = r0[1]; /* Now back substitute row 0. */ s = 1.0/r0[0]; r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0), r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0); MAT(out,0,0) = r0[4]; MAT(out,0,1) = r0[5], MAT(out,0,2) = r0[6]; MAT(out,0,3) = r0[7], MAT(out,1,0) = r1[4]; MAT(out,1,1) = r1[5], MAT(out,1,2) = r1[6]; MAT(out,1,3) = r1[7], MAT(out,2,0) = r2[4]; MAT(out,2,1) = r2[5], MAT(out,2,2) = r2[6]; MAT(out,2,3) = r2[7], MAT(out,3,0) = r3[4]; MAT(out,3,1) = r3[5], MAT(out,3,2) = r3[6]; MAT(out,3,3) = r3[7]; #undef MAT #undef SWAP_ROWS }
void Matrix4x4<T>::copyInverseTo(Matrix4x4<T>& inMatrix) const { #define SWAP_ROWS(a, b) { T* _tmp = a; (a) = (b); (b) = _tmp; } T wtmp[4][8]; T m0; T m1; T m2; T m3; T s; T* r0; T* r1; T* r2; T* r3; r0 = wtmp[0]; r1 = wtmp[1]; r2 = wtmp[2]; r3 = wtmp[3]; r0[0] = at(0, 0); r0[1] = at(0, 1); r0[2] = at(0, 2); r0[3] = at(0, 3); r0[4] = 1.0; r0[5] = r0[6] = r0[7] = 0.0; r1[0] = at(1, 0); r1[1] = at(1, 1); r1[2] = at(1, 2); r1[3] = at(1, 3); r1[5] = 1.0; r1[4] = r1[6] = r1[7] = 0.0; r2[0] = at(2, 0); r2[1] = at(2, 1); r2[2] = at(2, 2); r2[3] = at(2, 3); r2[6] = 1.0; r2[4] = r2[5] = r2[7] = 0.0; r3[0] = at(3, 0); r3[1] = at(3, 1); r3[2] = at(3, 2); r3[3] = at(3, 3); r3[7] = 1.0; r3[4] = r3[5] = r3[6] = 0.0; if (fabs(r3[0]) > fabs(r2[0])) SWAP_ROWS(r3, r2); if (fabs(r2[0]) > fabs(r1[0])) SWAP_ROWS(r2, r1); if (fabs(r1[0]) > fabs(r0[0])) SWAP_ROWS(r1, r0); if (0.0 == r0[0]) return; m1 = r1[0] / r0[0]; m2 = r2[0] / r0[0]; m3 = r3[0] / r0[0]; s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s; s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s; s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s; s = r0[4]; if (s != 0.0) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; } s = r0[5]; if (s != 0.0) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; } s = r0[6]; if (s != 0.0) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; } s = r0[7]; if (s != 0.0) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; } if (fabs(r3[1]) > fabs(r2[1])) SWAP_ROWS(r3, r2); if (fabs(r2[1]) > fabs(r1[1])) SWAP_ROWS(r2, r1); if (0.0 == r1[1]) return; m2 = r2[1] / r1[1]; m3 = r3[1] / r1[1]; r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2]; r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3]; s = r1[4]; if (0.0 != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; } s = r1[5]; if (0.0 != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; } s = r1[6]; if (0.0 != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; } s = r1[7]; if (0.0 != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; } if (fabs(r3[2]) > fabs(r2[2])) SWAP_ROWS(r3, r2); if (0.0 == r2[2]) return; m3 = r3[2] / r2[2]; r3[3] -= m3 * r2[3]; r3[4] -= m3 * r2[4]; r3[5] -= m3 * r2[5]; r3[6] -= m3 * r2[6]; r3[7] -= m3 * r2[7]; if (0.0 == r3[3]) return; s = 1.0 / r3[3]; r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s; m2 = r2[3]; s = 1.0 / r2[2]; r2[4] = s * (r2[4] - r3[4] * m2); r2[5] = s * (r2[5] - r3[5] * m2); r2[6] = s * (r2[6] - r3[6] * m2); r2[7] = s * (r2[7] - r3[7] * m2); m1 = r1[3]; r1[4] -= r3[4] * m1; r1[5] -= r3[5] * m1; r1[6] -= r3[6] * m1; r1[7] -= r3[7] * m1; m0 = r0[3]; r0[4] -= r3[4] * m0; r0[5] -= r3[5] * m0; r0[6] -= r3[6] * m0; r0[7] -= r3[7] * m0; m1 = r1[2]; s = 1.0 / r1[1]; r1[4] = s * (r1[4] - r2[4] * m1); r1[5] = s * (r1[5] - r2[5] * m1); r1[6] = s * (r1[6] - r2[6] * m1); r1[7] = s * (r1[7] - r2[7] * m1); m0 = r0[2]; r0[4] -= r2[4] * m0; r0[5] -= r2[5] * m0; r0[6] -= r2[6] * m0; r0[7] -= r2[7] * m0; m0 = r0[1]; s = 1.0 / r0[0]; r0[4] = s * (r0[4] - r1[4] * m0); r0[5] = s * (r0[5] - r1[5] * m0); r0[6] = s * (r0[6] - r1[6] * m0); r0[7] = s * (r0[7] - r1[7] * m0); inMatrix(0, 0) = r0[4]; inMatrix(0, 1) = r0[5]; inMatrix(0, 2) = r0[6]; inMatrix(0, 3) = r0[7]; inMatrix(1, 0) = r1[4]; inMatrix(1, 1) = r1[5]; inMatrix(1, 2) = r1[6]; inMatrix(1, 3) = r1[7]; inMatrix(2, 0) = r2[4]; inMatrix(2, 1) = r2[5]; inMatrix(2, 2) = r2[6]; inMatrix(2, 3) = r2[7]; inMatrix(3, 0) = r3[4]; inMatrix(3, 1) = r3[5]; inMatrix(3, 2) = r3[6]; inMatrix(3, 3) = r3[7]; }
/** * Compute inverse of 4x4 transformation matrix. * * \param mat pointer to a GLmatrix structure. The matrix inverse will be * stored in the GLmatrix::inv attribute. * * \return GL_TRUE for success, GL_FALSE for failure (\p singular matrix). * * \author * Code contributed by Jacques Leroy [email protected] * * Calculates the inverse matrix by performing the gaussian matrix reduction * with partial pivoting followed by back/substitution with the loops manually * unrolled. */ static GLboolean invert_matrix_general( GLmatrix *mat ) { const GLfloat *m = mat->m; GLfloat *out = mat->inv; GLfloat wtmp[4][8]; GLfloat m0, m1, m2, m3, s; GLfloat *r0, *r1, *r2, *r3; r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3]; r0[0] = MAT(m,0,0), r0[1] = MAT(m,0,1), r0[2] = MAT(m,0,2), r0[3] = MAT(m,0,3), r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0, r1[0] = MAT(m,1,0), r1[1] = MAT(m,1,1), r1[2] = MAT(m,1,2), r1[3] = MAT(m,1,3), r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0, r2[0] = MAT(m,2,0), r2[1] = MAT(m,2,1), r2[2] = MAT(m,2,2), r2[3] = MAT(m,2,3), r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0, r3[0] = MAT(m,3,0), r3[1] = MAT(m,3,1), r3[2] = MAT(m,3,2), r3[3] = MAT(m,3,3), r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0; /* choose pivot - or die */ if (FABSF(r3[0])>FABSF(r2[0])) SWAP_ROWS(r3, r2); if (FABSF(r2[0])>FABSF(r1[0])) SWAP_ROWS(r2, r1); if (FABSF(r1[0])>FABSF(r0[0])) SWAP_ROWS(r1, r0); if (0.0 == r0[0]) return GL_FALSE; /* eliminate first variable */ m1 = r1[0]/r0[0]; m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0]; s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s; s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s; s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s; s = r0[4]; if (s != 0.0) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; } s = r0[5]; if (s != 0.0) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; } s = r0[6]; if (s != 0.0) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; } s = r0[7]; if (s != 0.0) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; } /* choose pivot - or die */ if (FABSF(r3[1])>FABSF(r2[1])) SWAP_ROWS(r3, r2); if (FABSF(r2[1])>FABSF(r1[1])) SWAP_ROWS(r2, r1); if (0.0 == r1[1]) return GL_FALSE; /* eliminate second variable */ m2 = r2[1]/r1[1]; m3 = r3[1]/r1[1]; r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2]; r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3]; s = r1[4]; if (0.0 != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; } s = r1[5]; if (0.0 != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; } s = r1[6]; if (0.0 != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; } s = r1[7]; if (0.0 != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; } /* choose pivot - or die */ if (FABSF(r3[2])>FABSF(r2[2])) SWAP_ROWS(r3, r2); if (0.0 == r2[2]) return GL_FALSE; /* eliminate third variable */ m3 = r3[2]/r2[2]; r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4], r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6], r3[7] -= m3 * r2[7]; /* last check */ if (0.0 == r3[3]) return GL_FALSE; s = 1.0F/r3[3]; /* now back substitute row 3 */ r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s; m2 = r2[3]; /* now back substitute row 2 */ s = 1.0F/r2[2]; r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2), r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2); m1 = r1[3]; r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1, r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1; m0 = r0[3]; r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0, r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0; m1 = r1[2]; /* now back substitute row 1 */ s = 1.0F/r1[1]; r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1), r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1); m0 = r0[2]; r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0, r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0; m0 = r0[1]; /* now back substitute row 0 */ s = 1.0F/r0[0]; r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0), r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0); MAT(out,0,0) = r0[4]; MAT(out,0,1) = r0[5], MAT(out,0,2) = r0[6]; MAT(out,0,3) = r0[7], MAT(out,1,0) = r1[4]; MAT(out,1,1) = r1[5], MAT(out,1,2) = r1[6]; MAT(out,1,3) = r1[7], MAT(out,2,0) = r2[4]; MAT(out,2,1) = r2[5], MAT(out,2,2) = r2[6]; MAT(out,2,3) = r2[7], MAT(out,3,0) = r3[4]; MAT(out,3,1) = r3[5], MAT(out,3,2) = r3[6]; MAT(out,3,3) = r3[7]; return GL_TRUE; }
Matrix mInversed(const Matrix matrix) { float* m = (float*)&matrix.values[0][0]; Matrix returnValue; float* out = &returnValue.values[0][0]; /* NB. OpenGL Matrices are COLUMN major. */ #define SWAP_ROWS(a, b) { float *_tmp = a; (a)=(b); (b)=_tmp; } #define MAT(m,r,c) (m)[(c)*4+(r)] float wtmp[4][8]; float m0, m1, m2, m3, s; float *r0, *r1, *r2, *r3; r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3]; r0[0] = MAT(m, 0, 0), r0[1] = MAT(m, 0, 1), r0[2] = MAT(m, 0, 2), r0[3] = MAT(m, 0, 3), r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0, r1[0] = MAT(m, 1, 0), r1[1] = MAT(m, 1, 1), r1[2] = MAT(m, 1, 2), r1[3] = MAT(m, 1, 3), r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0, r2[0] = MAT(m, 2, 0), r2[1] = MAT(m, 2, 1), r2[2] = MAT(m, 2, 2), r2[3] = MAT(m, 2, 3), r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0, r3[0] = MAT(m, 3, 0), r3[1] = MAT(m, 3, 1), r3[2] = MAT(m, 3, 2), r3[3] = MAT(m, 3, 3), r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0; /* choose pivot - or die */ if (fabs(r3[0]) > fabs(r2[0])) SWAP_ROWS(r3, r2); if (fabs(r2[0]) > fabs(r1[0])) SWAP_ROWS(r2, r1); if (fabs(r1[0]) > fabs(r0[0])) SWAP_ROWS(r1, r0); if (0.0 == r0[0]) return makeMatrixZero(); /* eliminate first variable */ m1 = r1[0] / r0[0]; m2 = r2[0] / r0[0]; m3 = r3[0] / r0[0]; s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s; s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s; s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s; s = r0[4]; if (s != 0.0) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; } s = r0[5]; if (s != 0.0) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; } s = r0[6]; if (s != 0.0) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; } s = r0[7]; if (s != 0.0) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; } /* choose pivot - or die */ if (fabs(r3[1]) > fabs(r2[1])) SWAP_ROWS(r3, r2); if (fabs(r2[1]) > fabs(r1[1])) SWAP_ROWS(r2, r1); if (0.0 == r1[1]) return makeMatrixZero(); /* eliminate second variable */ m2 = r2[1] / r1[1]; m3 = r3[1] / r1[1]; r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2]; r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3]; s = r1[4]; if (0.0 != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; } s = r1[5]; if (0.0 != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; } s = r1[6]; if (0.0 != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; } s = r1[7]; if (0.0 != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; } /* choose pivot - or die */ if (fabs(r3[2]) > fabs(r2[2])) SWAP_ROWS(r3, r2); if (0.0 == r2[2]) return makeMatrixZero(); /* eliminate third variable */ m3 = r3[2] / r2[2]; r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4], r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6], r3[7] -= m3 * r2[7]; /* last check */ if (0.0 == r3[3]) return makeMatrixZero(); s = 1.0 / r3[3]; /* now back substitute row 3 */ r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s; m2 = r2[3]; /* now back substitute row 2 */ s = 1.0 / r2[2]; r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2), r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2); m1 = r1[3]; r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1, r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1; m0 = r0[3]; r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0, r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0; m1 = r1[2]; /* now back substitute row 1 */ s = 1.0 / r1[1]; r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1), r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1); m0 = r0[2]; r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0, r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0; m0 = r0[1]; /* now back substitute row 0 */ s = 1.0 / r0[0]; r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0), r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0); MAT(out, 0, 0) = r0[4]; MAT(out, 0, 1) = r0[5], MAT(out, 0, 2) = r0[6]; MAT(out, 0, 3) = r0[7], MAT(out, 1, 0) = r1[4]; MAT(out, 1, 1) = r1[5], MAT(out, 1, 2) = r1[6]; MAT(out, 1, 3) = r1[7], MAT(out, 2, 0) = r2[4]; MAT(out, 2, 1) = r2[5], MAT(out, 2, 2) = r2[6]; MAT(out, 2, 3) = r2[7], MAT(out, 3, 0) = r3[4]; MAT(out, 3, 1) = r3[5], MAT(out, 3, 2) = r3[6]; MAT(out, 3, 3) = r3[7]; #undef MAT #undef SWAP_ROWS return returnValue; }