转置

 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
}