OBBを計算する（プログラム）

#pragma once
#include <vector>
#include <array>
#include <limits>
#include <valarray>

template<typename real_t>
real_t Length3(const real_t * const vec) {
  const int X = 0;
  const int Y = 1;
  const int Z = 2;
  return sqrt(vec[X] * vec[X] + vec[Y] * vec[Y] + vec[Z] * vec[Z]);
}
template<typename real_t>
bool Normalize3(real_t* const pV)
{

  const int X = 0;
  const int Y = 1;
  const int Z = 2;

  real_t len;
  real_t& x = pV[X];
  real_t& y = pV[Y];
  real_t& z = pV[Z];

  len = static_cast<real_t>(sqrt(x * x + y * y + z * z));

  if (len < static_cast<real_t>(1e-6)) return false;

  len = static_cast<real_t>(1.0) / len;
  x *= len;
  y *= len;
  z *= len;

  return true;
}

template<typename real_t>
real_t Inner3(real_t const* const vec1, real_t const* const vec2) {
  const int X = 0;
  const int Y = 1;
  const int Z = 2;
  return ((vec1[X]) * (vec2[X]) + (vec1[Y]) * (vec2[Y]) + (vec1[Z]) * (vec2[Z]));
}

/*!
 * Jacobi法による固有値の算出
 * @param[inout] a 実対称行列．計算後，対角要素に固有値が入る
 * @param[out] v 固有ベクトル(aと同じサイズ)
 * @param[in] n 行列のサイズ(n×n)
 * @param[in] eps 収束誤差
 * @param[in] iter_max 最大反復回数
 * @return 反復回数
 * @see http://www.slis.tsukuba.ac.jp/~fujisawa.makoto.fu/cgi-bin/wiki/index.php?%B8%C7%CD%AD%C3%CD/%B8%C7%CD%AD%A5%D9%A5%AF%A5%C8%A5%EB
 */
template<typename real_t>
int EigenJacobiMethod(real_t* a, real_t* v, int n, real_t eps = 1e-8, int iter_max = 100)
{
  real_t bii, bij, bjj, bji;

  auto bim = std::vector<real_t>(n);
  auto bjm = std::vector<real_t>(n);

  for (int i = 0; i < n; ++i) {
    for (int j = 0; j < n; ++j) {
      v[i * n + j] = (i == j) ? 1.0 : 0.0;
    }
  }

  int cnt = 0;
  for (;;) {
    int i, j;

    float x = 0.0;
    for (int ia = 0; ia < n; ++ia) {
      for (int ja = 0; ja < n; ++ja) {
        int idx = ia * n + ja;
        if (ia != ja && fabs(a[idx]) > x) {
          i = ia;
          j = ja;
          x = std::abs(a[idx]);
        }
      }
    }

    float aii = a[i * n + i];
    float ajj = a[j * n + j];
    float aij = a[i * n + j];

    float alpha, beta;
    alpha = (aii - ajj) / 2.0;
    beta = sqrt(alpha * alpha + aij * aij);

    float st, ct;
    ct = sqrt((1.0 + fabs(alpha) / beta) / 2.0);    // sinθ
    st = (((aii - ajj) >= 0.0) ? 1.0 : -1.0) * aij / (2.0 * beta * ct);    // cosθ

    // A = PAPの計算
    for (int m = 0; m < n; ++m) {
      if (m == i || m == j) continue;

      float aim = a[i * n + m];
      float ajm = a[j * n + m];

      bim[m] = aim * ct + ajm * st;
      bjm[m] = -aim * st + ajm * ct;
    }

    bii = aii * ct * ct + 2.0 * aij * ct * st + ajj * st * st;
    bij = 0.0;

    bjj = aii * st * st - 2.0 * aij * ct * st + ajj * ct * ct;
    bji = 0.0;

    for (int m = 0; m < n; ++m) {
      a[i * n + m] = a[m * n + i] = bim[m];
      a[j * n + m] = a[m * n + j] = bjm[m];
    }
    a[i * n + i] = bii;
    a[i * n + j] = bij;
    a[j * n + j] = bjj;
    a[j * n + i] = bji;

    // V = PVの計算
    for (int m = 0; m < n; ++m) {
      float vmi = v[m * n + i];
      float vmj = v[m * n + j];

      bim[m] = vmi * ct + vmj * st;
      bjm[m] = -vmi * st + vmj * ct;
    }
    for (int m = 0; m < n; ++m) {
      v[m * n + i] = bim[m];
      v[m * n + j] = bjm[m];
    }

    float e = 0.0;
    for (int ja = 0; ja < n; ++ja) {
      for (int ia = 0; ia < n; ++ia) {
        if (ia != ja) {
          e += fabs(a[ja * n + ia]);
        }
      }
    }
    if (e < eps) break;

    cnt++;
    if (cnt > iter_max) break;
  }

  return cnt;
}

//! @brief 固有値・固有ベクトル
struct _eigen_ {
  double _value;   //!< 固有値
  double _vector[3];//!< 固有ベクトル
};


//! @brief データの平均を求める
//! @param [in] vec データの配列
//! @return 各要素の平均
template<typename real_t,typename PointT>
void Covariance_Ave(real_t* ave,const std::vector<PointT>& vec) {

  // 初期化
  ave[0] = 0;
  ave[1] = 0;
  ave[2] = 0;

  // 各要素平均
  for (size_t i = 0; i < vec.size(); i++) {
    ave[0] += vec[i][0];
    ave[1] += vec[i][1];
    ave[2] += vec[i][2];
  }
  ave[0] /= vec.size();
  ave[1] /= vec.size();
  ave[2] /= vec.size();

}

//! @brief 共分散を求める
//! @param [in] average 各要素の平均 3次元
//! @param [in] vec データ
//! @param [in] param どの要素に対して求めるか。例えばxyzの時、x,yに対する共分散なら{0,1}を与える。
//! @return 偏差の積の和の要素数分の一
template<typename real_t,typename PointT>
double Covariance(const real_t* const average, const std::vector<PointT>& vec, const std::array<int, 2>& param) {

  double sum = 0.0;
  for (size_t i = 0; i < vec.size(); i++) {

    //指定したパラメータの偏差を求める
    double deviation[3];
    for (size_t j = 0; j < param.size(); j++) {
      int target = param[j];
      deviation[target] = (vec[i][target] - average[target]);
    }

    //偏差の積
    double product = 1.0;
    for (size_t j = 0; j < param.size(); j++) {
      int target = param[j];
      product *= deviation[target];
    }

    //偏差の積の和を更新
    sum += product;
  }

  //偏差の積の和のN分の一
  return 1.0 / vec.size() * sum;
}


//! @brief データを主成分分析する
//! @param [out] average 全データの各要素の平均
//! @param [out] ev 固有値と固有ベクトル
//! @param [in] pt 三次元の座標値一覧
//! @return なし
template<typename PointT>
void PrincipalComponentAnalysis3D(_eigen_* e, const std::vector<PointT>& pt) {

  double average[3];

  //各要素の平均を求める。
  //これは共分散を求めるときに (x[i] - xの平均)×(y[i] - yの平均) 等の計算が必要なため
  Covariance_Ave(average, pt);

  //共分散を求める
  //第三引数の{0,0}はxxを表す。xyなら{0,1}。これはデータがxyzの順に並んでいる事が前提。
  double Sxx = static_cast<double>(Covariance(average, pt, { 0, 0 }));
  double Sxy = static_cast<double>(Covariance(average, pt, { 0, 1 }));
  double Sxz = static_cast<double>(Covariance(average, pt, { 0, 2 }));
  double Syy = static_cast<double>(Covariance(average, pt, { 1, 1 }));
  double Syz = static_cast<double>(Covariance(average, pt, { 1, 2 }));
  double Szz = static_cast<double>(Covariance(average, pt, { 2, 2 }));

  // 分散共分散行列
  double a[3 * 3] = {
     Sxx,Sxy,Sxz,
     Sxy,Syy,Syz,
     Sxz,Syz,Szz
  };
  double eigen[3 * 3];
  EigenJacobiMethod(a, eigen, 3);

  // eigenの対角線が固有値となっている

  e[0]._value = eigen[0];
  e[0]._vector[0] = eigen[0];
  e[0]._vector[1] = eigen[3];
  e[0]._vector[2] = eigen[6];

  e[1]._value = eigen[4];
  e[1]._vector[0] = eigen[1];
  e[1]._vector[1] = eigen[4];
  e[1]._vector[2] = eigen[7];

  e[2]._value = eigen[8];
  e[2]._vector[0] = eigen[2];
  e[2]._vector[1] = eigen[5];
  e[2]._vector[2] = eigen[8];
}