Created
March 23, 2015 08:38
-
-
Save halferty/d6421e36259f4b1d56f8 to your computer and use it in GitHub Desktop.
Linear algebra basic 101 stuff. I gotta start somewhere, I suppose. Sacrificing efficiency for readability here.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| void PointFrom3Planes(float x1, float y1, float z1, float d1, float x2, float y2, float z2, float d2, float x3, float y3, float z3, float d3, float *px, float *py, float *pz) { | |
| // Calculate matrix of minors | |
| float matrixOfMinors[3][3] = { | |
| { | |
| (y2 * z3 - y3 * z2), | |
| (x2 * z3 - x3 * z2), | |
| (x2 * y3 - x3 * y2), | |
| }, | |
| { | |
| (y1 * z3 - y3 * z1), | |
| (x1 * z3 - x3 * z1), | |
| (x1 * y3 - x3 * y1), | |
| }, | |
| { | |
| (y1 * z2 - y2 * z1), | |
| (x1 * z2 - x2 * z1), | |
| (x1 * y2 - x2 * y1), | |
| } | |
| }; | |
| // Create matrix of cofactors | |
| float matrixOfCofactors[3][3] = { | |
| { | |
| matrixOfMinors[0][0], | |
| -matrixOfMinors[0][1], | |
| matrixOfMinors[0][2] | |
| }, | |
| { | |
| -matrixOfMinors[1][0], | |
| matrixOfMinors[1][1], | |
| -matrixOfMinors[1][2] | |
| }, | |
| { | |
| matrixOfMinors[2][0], | |
| -matrixOfMinors[2][1], | |
| matrixOfMinors[2][2] | |
| } | |
| }; | |
| // Get adjugate | |
| float adjugate[3][3] = { | |
| { | |
| matrixOfCofactors[0][0], | |
| matrixOfCofactors[1][0], | |
| matrixOfCofactors[2][0] | |
| }, | |
| { | |
| matrixOfCofactors[0][1], | |
| matrixOfCofactors[1][1], | |
| matrixOfCofactors[2][1] | |
| }, | |
| { | |
| matrixOfCofactors[0][2], | |
| matrixOfCofactors[1][2], | |
| matrixOfCofactors[2][2] | |
| } | |
| }; | |
| // Get determinant | |
| float determinant = matrixOfMinors[0][0] * x1 - matrixOfMinors[0][1] * y1 + matrixOfMinors[0][2] * z1; | |
| if (determinant == 0) { | |
| printf("Matrix determinant = 0, planes do no intersect at single point!"); | |
| } | |
| // Get inverse | |
| float inverse[3][3] = { | |
| { | |
| adjugate[0][0] * (1/determinant), | |
| adjugate[0][1] * (1/determinant), | |
| adjugate[0][2] * (1/determinant) | |
| }, | |
| { | |
| adjugate[1][0] * (1/determinant), | |
| adjugate[1][1] * (1/determinant), | |
| adjugate[1][2] * (1/determinant) | |
| }, | |
| { | |
| adjugate[2][0] * (1/determinant), | |
| adjugate[2][1] * (1/determinant), | |
| adjugate[2][2] * (1/determinant) | |
| } | |
| }; | |
| // Matrix multiply by distances matrix to get result | |
| *px = inverse[0][0] * -d1 + inverse[0][1] * -d2 + inverse[0][2] * -d3; | |
| *py = inverse[1][0] * -d1 + inverse[1][1] * -d2 + inverse[1][2] * -d3; | |
| *pz = inverse[2][0] * -d1 + inverse[2][1] * -d2 + inverse[2][2] * -d3; | |
| return; | |
| } |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment