Cross product
//====================================================================
void Helpers::crossProduct(double *a, double *b, double *c)
{
c[0] = a[1]*b[2] - a[2]*b[1];
c[1] = a[2]*b[0] - a[0]*b[2];
c[2] = a[0]*b[1] - a[1]*b[0];
}
//====================================================================
// Invert 4x4 matrix (for visualiztion only)
//====================================================================
void Helpers::invertMatrix(const double *a, double *inva)
{
#define ENTRY(A, i, j) (A)[4*(i)+(j)]
int i, j, k;
double c[4][8];
double cik, ckk;
// Initialize:
//-------------
for(i = 0; i < 4; i++) {
for(j = 0; j < 4; j++) {
c[i][j] = ENTRY(a, i, j);
c[i][j+4] = 0.0;
}
c[i][i+4] = 1.0;
}
// Eliminate:
//------------
for(k = 0; k < 4; k++) {
ckk = c[k][k];
if(ckk == 0.0) return;
for(j = 0; j < 8; j++)
c[k][j] /= ckk;
for(i = 0; i < 4; i++) {
if(i == k) continue;
cik = c[i][k];
for(j = 0; j < 8; j++)
c[i][j] -= cik * c[k][j];
}
}
// Result:
//---------
for(i = 0; i < 4; i++) {
for(j = 0; j < 4; j++)
ENTRY(inva, i, j) = c[i][j+4];
}
#undef ENTRY
}
//====================================================================
void Helpers::crossProduct(double *a, double *b, double *c)
{
c[0] = a[1]*b[2] - a[2]*b[1];
c[1] = a[2]*b[0] - a[0]*b[2];
c[2] = a[0]*b[1] - a[1]*b[0];
}
//====================================================================
// Invert 4x4 matrix (for visualiztion only)
//====================================================================
void Helpers::invertMatrix(const double *a, double *inva)
{
#define ENTRY(A, i, j) (A)[4*(i)+(j)]
int i, j, k;
double c[4][8];
double cik, ckk;
// Initialize:
//-------------
for(i = 0; i < 4; i++) {
for(j = 0; j < 4; j++) {
c[i][j] = ENTRY(a, i, j);
c[i][j+4] = 0.0;
}
c[i][i+4] = 1.0;
}
// Eliminate:
//------------
for(k = 0; k < 4; k++) {
ckk = c[k][k];
if(ckk == 0.0) return;
for(j = 0; j < 8; j++)
c[k][j] /= ckk;
for(i = 0; i < 4; i++) {
if(i == k) continue;
cik = c[i][k];
for(j = 0; j < 8; j++)
c[i][j] -= cik * c[k][j];
}
}
// Result:
//---------
for(i = 0; i < 4; i++) {
for(j = 0; j < 4; j++)
ENTRY(inva, i, j) = c[i][j+4];
}
#undef ENTRY
}