Skip to content

Instantly share code, notes, and snippets.

@halferty
Created March 23, 2015 08:38
Show Gist options
  • Select an option

  • Save halferty/d6421e36259f4b1d56f8 to your computer and use it in GitHub Desktop.

Select an option

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.
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