4×4行列の積を実装（C++）

inline int f(const int i, const int k) {
  return i + 4*k;
}
//4x4行列の乗算関数
void multMatrix(double* C, double* A, double* B) {

  for (int i = 0; i < 4; i++)
    for (int k = 0; k < 4; k++) {
      C[f(i, k)] = 0.0;
      for (int j = 0; j < 4; j++) {
        C[f(i, k)] += A[f(i, j)]*B[f(j, k)];
      }
    }
}

//描画関数
void disp(void) {

  glViewport(0, 0, width, height);

  glClearColor(0.2, 0.2, 0.2, 1);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  double in[4] = { 0.2,0.1,0.1,1 };

  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

  double rmat[16];
  double tmat[16];

  ////////////////////// OpenGLで計算
  glPushMatrix();
  glRotated(120, 0, 0, 1);
  glTranslated(0.5, 0, 0);
  glColor3d(1, 0, 0);
  glPointSize(15);
  glBegin(GL_POINTS);
  glVertex3d(in[0], in[1], in[2]);
  glEnd();
  glPopMatrix();


  ////////////////////// 自前実装
  ////////////////////// OpenGLで行列作成
  glPushMatrix();
  glRotated(120, 0, 0, 1);
  glGetDoublev(GL_MODELVIEW_MATRIX, rmat);
  glPopMatrix();
  glPushMatrix();
  glTranslated(0.5, 0, 0);
  glGetDoublev(GL_MODELVIEW_MATRIX, tmat);
  glPopMatrix();

  //行列のかけ算
  double amat[16];
  glPushMatrix();
  multMatrix(amat, rmat, tmat);
  glLoadMatrixd(amat);
  glColor3d(0, 1, 0);
  glPointSize(10);
  glBegin(GL_POINTS);
  glVertex3d(in[0], in[1], in[2]);
  glEnd();
  glPopMatrix();

  glFlush();
}