三次元座標をアフィン変換する

template<typename real_t>
inline void multMatrixVec(real_t* const out4, const real_t* const matrix, const real_t* const in4)
{
  int i;

  for (i = 0; i<4; i++) {
    out4[i] =
      in4[0] * matrix[0 * 4 + i] +
      in4[1] * matrix[1 * 4 + i] +
      in4[2] * matrix[2 * 4 + i] +
      in4[3] * matrix[3 * 4 + i];
  }
}

     
template<typename real_t>
inline void multMatrixVec(real_t* const out4, const real_t* const matrix, const real_t* const in4)
{
  int i;

  for (i = 0; i < 4; i++) {
    out4[i] =
      in4[0] * matrix[0 * 4 + i] +
      in4[1] * matrix[1 * 4 + i] +
      in4[2] * matrix[2 * 4 + i] +
      in4[3] * matrix[3 * 4 + i];
  }
}


//描画関数
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.1,0.2,0.3,1 };

  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

  double mat[16];
  ////////////////////// OpenGLで描画
  glPushMatrix();
  glRotated(25, 0, 0, 1);
  glTranslated(0.5, 0, 0);

  glGetDoublev(GL_MODELVIEW_MATRIX, mat); //行列を退避

  glColor3d(1, 0, 0);
  glPointSize(15);
  glBegin(GL_POINTS);
  glVertex3d(in[0], in[1], in[2]);
  glEnd();

  glPopMatrix();

  ////////////////////// 自前実装
  double out4[4];
  multMatrixVec(out4, mat, in);//退避した行列で座標を変換
  glColor3d(0, 1, 0);
  glPointSize(10);
  glBegin(GL_POINTS);
  glVertex3d(out4[0], out4[1], out4[2]);
  glEnd();

  glFlush();
}