//=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*
//  "isosurf.h"
//  3次元密度分布の等高面を描くライブラリ
//  製作 渡辺尚貴    変更日 2001年5月21日
//=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*

// 3次元ベクトルの構造体
struct Vector3
{
  double x, y, z;
  inline Vector3( void ){}
  inline Vector3( const double& _x, const double& _y, const double& _z ) : 
  x(_x), y(_y), z(_z) {
    ;
  }
  inline Vector3& operator *= ( const double& d ){
    x *= d;
    y *= d;
    z *= d;
    return *this;
  }
  inline Vector3& operator += ( const Vector3& v ){
    x += v.x;
    y += v.y;
    z += v.z;
    return *this;
  }
};

// 3次元ベクトルの演算規則
inline Vector3 operator + ( const Vector3& v1, const Vector3& v2 )
{
  Vector3 t;
  t.x = v1.x + v2.x;
  t.y = v1.y + v2.y;
  t.z = v1.z + v2.z;
  return t;
}
inline Vector3 operator - ( const Vector3& v1, const Vector3& v2 )
{
  Vector3 t;
  t.x = v1.x - v2.x;
  t.y = v1.y - v2.y;
  t.z = v1.z - v2.z;
  return t;
}

inline Vector3 operator * ( const Vector3& v, const double& d )
{
  Vector3 t;
  t.x = v.x*d;
  t.y = v.y*d;
  t.z = v.z*d;
  return t;
}

inline Vector3 operator * ( const double& d, const Vector3& v )
{
  Vector3 t;
  t.x = v.x*d;
  t.y = v.y*d;
  t.z = v.z*d;
  return t;
}


inline Vector3 operator % ( const Vector3& v1, const Vector3& v2 )
{
  Vector3 t;
  t.x = v1.y*v2.z - v2.y*v1.z;
  t.y = v1.z*v2.x - v2.z*v1.x;
  t.z = v1.x*v2.y - v2.x*v1.y;
  return t;
}

inline double operator * ( const Vector3& v1, const Vector3& v2 )
{
  return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
}

inline double Norm( const Vector3& v )
{
  return v.x*v.x + v.y*v.y + v.z*v.z;
}

inline double Distance( const Vector3& v1, const Vector3& v2 )
{
  return Norm(v1 - v2);
}

inline Vector3& Normalize( Vector3 v )
{
  return v *= 1.0/sqrt(Norm(v));
}


// 格子点が作る軸と等高面が交わる点(結接点)を管理する構造体
struct Node
{
  Vector3 vertex;
  Vector3 normal;
  char flag;

  inline operator char (){
    return flag;
  }
  inline operator Vector3(){
    return vertex;
  }
  friend inline double Distance( const Node& n1, const Node& n2 ){
    return Norm(n1.vertex-n2.vertex);
  }
};


// 等高面を作成管理するクラス
template <int N>
class Isosurf
{
  double (*data)[N][N];  // 3次元格子点上でのデータへのポインタ
  double iso_level;      // 等高値
  Node nodex[N][N][N];   // X軸に平行な格子軸上の結接点
  Node nodey[N][N][N];   // Y軸に平行な格子軸上の結接点
  Node nodez[N][N][N];   // Z軸に平行な格子軸上の結接点

  const double dL;
public:
  Isosurf( void ) : dL(1.0/N) {}

  void Assign( double _data[N][N][N], double _iso_level ){
    data = _data;
    iso_level = _iso_level;
  }

  void Make( void ){
    CalcVertexes();
    CalcNormals();
    MakeElements();
  }

private:
  inline int Intersect( double& vm, double& vp ){
    return vm*vp <= 0.0 && (vm<0.0 || vp<0.0);
  }
  void CalcVertex( int ix, int iy, int iz );
  void CalcVertexes( void );
  int IndexSelected( Node& node0, Node& node1, Node& node2, Node& node3 );
  Node& NodeSelected( Node& node0, Node& node1, Node& node2, Node& node3 );
  Vector3 CalcNormalX( int ix, int iy, int iz );
  Vector3 CalcNormalY( int ix, int iy, int iz );
  Vector3 CalcNormalZ( int ix, int iy, int iz );
  void CalcNormals( void );
  void DrawPolygon( int n, Node polygon[] );
  void MakeElement( int ix, int iy, int iz );
  void MakeElements( void );
};

// 各結接点の位置を計算するメソッド
template <int N>
void Isosurf<N>::CalcVertex( int ix, int iy, int iz )
{
  double vo, vx, vy, vz;

  vo = data[ix][iy][iz] - iso_level;
  if( ix != N-1 ) vx = data[ix+1][iy][iz] - iso_level;
  if( iy != N-1 ) vy = data[ix][iy+1][iz] - iso_level;
  if( iz != N-1 ) vz = data[ix][iy][iz+1] - iso_level;

  if( ix != N-1 && Intersect(vo,vx) ){
    nodex[ix][iy][iz].vertex = dL*Vector3( ix+vo/(vo-vx), iy, iz );
    nodex[ix][iy][iz].flag   = 1;
  }else{
    nodex[ix][iy][iz].flag   = 0;
  }
  if( iy != N-1 && Intersect(vo,vy) ){
    nodey[ix][iy][iz].vertex = dL*Vector3( ix, iy+vo/(vo-vy), iz );
    nodey[ix][iy][iz].flag   = 1;
  }else{
    nodey[ix][iy][iz].flag   = 0;
  }
  if( iz != N-1 && Intersect(vo,vz) ){
    nodez[ix][iy][iz].vertex = dL*Vector3( ix, iy, iz+vo/(vo-vz) );
    nodez[ix][iy][iz].flag   = 1;
  }else{
    nodez[ix][iy][iz].flag   = 0;
  }
}

// 結接点の位置の計算を指揮するメソッド
template <int N>
void Isosurf<N>::CalcVertexes( void )
{
  for( int ix=0; ix<N; ix++ ){
    for( int iy=0; iy<N; iy++ ){
      for( int iz=0; iz<N; iz++ ){
	CalcVertex(ix,iy,iz);
      }
    }
  }
}

// 指定の4点の結接点から適切な接続法を返答するメソッド
template <int N>
int Isosurf<N>::IndexSelected( Node& node0, Node& node1, Node& node2, Node& node3 )
{
  if( node1 && node2 && node3 ){
    double d1 = Distance( node0, node1 ) + Distance( node3, node2 );
    double d2 = Distance( node0, node2 ) + Distance( node3, node1 );
    if( d1 > d2 ) return 2; else return 1;
  }else{
    if(      node1 )   return 1;
    else if( node2 )   return 2;
    else if( node3 )   return 3;
  }
  return 0;
}

// 指定の4点の結接点から適切な接続法を返答するメソッド
template <int N>
Node& Isosurf<N>::NodeSelected( Node& node0, Node& node1, Node& node2, Node& node3 )
{
  if( node1 && node2 && node3 ){
    double d1 = Distance( node0, node1 ) + Distance( node3, node2 );
    double d2 = Distance( node0, node2 ) + Distance( node3, node1 );
    if( d1 > d2 ) return node2; else return node1;
  }else{
    if(      node1 )   return node1;
    else if( node2 )   return node2;
    else if( node3 )   return node3;
  }
  return node0;
}

// X軸結接点での法線を計算するメソッド
template <int N>
Vector3 Isosurf<N>::CalcNormalX( int ix, int iy, int iz )
{
  Vector3 tang[4];

  if( iy == N-1 ){
    tang[0] = nodex[ix][iy][iz];
  }else{
    tang[0] = NodeSelected(nodex[ix][iy][iz],nodey[ix][iy][iz],
			   nodey[ix+1][iy][iz],nodex[ix][iy+1][iz]);
  }

  if( iy == 0 ){
    tang[1] = nodex[ix][iy][iz];
  }else{
    tang[1] = NodeSelected(nodex[ix][iy][iz],nodey[ix][iy-1][iz],
			   nodey[ix+1][iy-1][iz],nodex[ix][iy-1][iz]);
  }

  if( iz == N-1 ){
    tang[2] = nodex[ix][iy][iz];
  }else{
    tang[2] = NodeSelected(nodex[ix][iy][iz],nodez[ix][iy][iz],
			   nodez[ix+1][iy][iz],nodex[ix][iy][iz+1]);
  }

  if( iz == 0 ){
    tang[3] = nodex[ix][iy][iz];
  }else{
    tang[3] = NodeSelected(nodex[ix][iy][iz],nodez[ix][iy][iz-1],
			   nodez[ix+1][iy][iz-1],nodex[ix][iy][iz-1]);
  }

  return Normalize((tang[0] - tang[1])%(tang[2] - tang[3]));
}

// Y軸結接点での法線を計算するメソッド
template <int N>
Vector3 Isosurf<N>::CalcNormalY( int ix, int iy, int iz )
{
  Vector3 tang[4];

  if( ix == N-1 ){
    tang[0] = nodey[ix][iy][iz];
  }else{
    tang[0] = NodeSelected(nodey[ix][iy][iz],nodex[ix][iy][iz],
			   nodex[ix][iy+1][iz],nodey[ix+1][iy][iz]);
  }

  if( ix == 0 ){
    tang[1] = nodey[ix][iy][iz];
  }else{
    tang[1] = NodeSelected(nodey[ix][iy][iz],nodex[ix-1][iy][iz],
			   nodex[ix-1][iy+1][iz],nodey[ix-1][iy][iz]);
  }

  if( iz == N-1 ){
    tang[2] = nodey[ix][iy][iz];
  }else{
    tang[2] = NodeSelected(nodey[ix][iy][iz],nodez[ix][iy][iz],
			   nodez[ix][iy+1][iz],nodey[ix][iy][iz+1]);
  }

  if( iz == 0 ){
    tang[3] = nodey[ix][iy][iz];
  }else{
    tang[3] = NodeSelected(nodey[ix][iy][iz],nodez[ix][iy][iz-1],
			   nodez[ix][iy+1][iz-1],nodey[ix][iy][iz-1]);
  }

  return Normalize((tang[2] - tang[3])%(tang[0] - tang[1]));
}

// Z軸結接点での法線を計算するメソッド
template <int N>
Vector3 Isosurf<N>::CalcNormalZ( int ix, int iy, int iz )
{
  Vector3 tang[4];

  if( ix == N-1 ){
    tang[0] = nodez[ix][iy][iz];
  }else{
    tang[0] = NodeSelected(nodez[ix][iy][iz],nodex[ix][iy][iz],
			   nodex[ix][iy][iz+1],nodez[ix+1][iy][iz]);
  }

  if( ix == 0 ){
    tang[1] = nodez[ix][iy][iz];
  }else{
    tang[1] = NodeSelected(nodez[ix][iy][iz],nodex[ix-1][iy][iz],
			   nodex[ix-1][iy][iz+1],nodez[ix-1][iy][iz]);
  }

  if( iy == N-1 ){
    tang[2] = nodez[ix][iy][iz];
  }else{
    tang[2] = NodeSelected(nodez[ix][iy][iz],nodey[ix][iy][iz],
			   nodey[ix][iy][iz+1],nodez[ix][iy+1][iz]);
  }

  if( iy == 0 ){
    tang[3] = nodez[ix][iy][iz];
  }else{
    tang[3] = NodeSelected(nodez[ix][iy][iz],nodey[ix][iy-1][iz],
			   nodey[ix][iy-1][iz+1],nodez[ix][iy-1][iz]);
  }

  return Normalize((tang[0] - tang[1])%(tang[2] - tang[3]));
}


// 結接点での法線の計算を指揮するメソッド
template <int N>
void Isosurf<N>::CalcNormals( void )
{
  for( int ix=0; ix<N; ix++ ){
    for( int iy=0; iy<N; iy++ ){
      for( int iz=0; iz<N; iz++ ){
	if( nodex[ix][iy][iz] ){
	  nodex[ix][iy][iz].normal = CalcNormalX(ix,iy,iz);
	}
	if( nodey[ix][iy][iz] ){
	  nodey[ix][iy][iz].normal = CalcNormalY(ix,iy,iz);
	}
	if( nodez[ix][iy][iz] ){
	  nodez[ix][iy][iz].normal = CalcNormalZ(ix,iy,iz);
	}
      }
    }
  }
}

// ポリゴンを作成するメソッド
template <int N> void Isosurf<N>::DrawPolygon( int n, Node polygon[] )
{
  int i;

  if( n==3 ){
    glBegin( GL_TRIANGLES );
    for( i=0; i<n; i++ ){
      glNormal3dv( (double*)&polygon[i].normal );
      glVertex3dv( (double*)&polygon[i].vertex );
    }
    glEnd();
  }else{
    // 四角形以上なら中心点まわりの TRIANGLE_FAN とする
    Vector3 mid_ver = Vector3(0,0,0);
    Vector3 mid_nor = Vector3(0,0,0);
    for( i=0; i<n; i++ ){
      mid_ver += polygon[i].vertex;
      mid_nor += polygon[i].normal;
    }
    mid_ver *= (1.0/n);
    Normalize(mid_nor);
      
    glBegin( GL_TRIANGLE_FAN );
    glNormal3dv( (double*)&mid_nor );
    glVertex3dv( (double*)&mid_ver );

    for( i=0; i<n; i++ ){
      glNormal3dv( (double*)&polygon[i].normal );
      glVertex3dv( (double*)&polygon[i].vertex );
    }
    i=0;{
      glNormal3dv( (double*)&polygon[i].normal );
      glVertex3dv( (double*)&polygon[i].vertex );
    }
    glEnd();
  }
  
}


// 各要素内での等高面ポリゴンを作成するメソッド
template <int N>
void Isosurf<N>::MakeElement( int ix, int iy, int iz )
{
  //各結節点の接続可能性を表すデータ
  static char conn[12][2][4] = {
    {{ 0, 1, 7, 6}, { 0, 2, 8, 3}},  //  0
    {{ 1, 2, 5, 4}, { 1, 0, 6, 7}},  //  1
    {{ 2, 0, 3, 8}, { 2, 1, 4, 5}},  //  2
    {{ 3, 8, 2, 0}, { 3, 4,10, 9}},  //  3
    {{ 4, 3, 9,10}, { 4, 5, 2, 1}},  //  4
    {{ 5, 4, 1, 2}, { 5, 6, 9,11}},  //  5
    {{ 6, 5,11, 9}, { 6, 7, 1, 0}},  //  6
    {{ 7, 6, 0, 1}, { 7, 8,11,10}},  //  7
    {{ 8, 7,10,11}, { 8, 3, 0, 2}},  //  8
    {{ 9,10, 4, 3}, { 9,11, 5, 6}},  //  9
    {{10,11, 8, 7}, {10, 9, 3, 4}},  // 10
    {{11, 9, 6, 5}, {11,10, 7, 8}}   // 11
  };
  static Node node[12];
  static Node polygon[12];

  node[ 0] = nodex[ix][iy][iz];
  node[ 1] = nodey[ix][iy][iz];
  node[ 2] = nodez[ix][iy][iz];
  node[ 3] = nodex[ix][iy][iz+1];
  node[ 4] = nodey[ix][iy][iz+1];
  node[ 5] = nodez[ix][iy+1][iz];
  node[ 6] = nodex[ix][iy+1][iz];
  node[ 7] = nodey[ix+1][iy][iz];
  node[ 8] = nodez[ix+1][iy][iz];
  node[ 9] = nodex[ix][iy+1][iz+1];
  node[10] = nodey[ix+1][iy][iz+1];
  node[11] = nodez[ix+1][iy+1][iz];

  for( int is=0; is<12; is++ ){
    if( !node[is] ) continue;

    int n=0, i=is, m=0;
    do {
      if(m) node[i].normal *= -1;

      polygon[n++]= node[i];

      int sol = IndexSelected(node[conn[i][m][0]],node[conn[i][m][1]],
			      node[conn[i][m][2]],node[conn[i][m][3]]);

      i = conn[i][m][sol];
      if( sol == 2 ) m ^= 1;
      node[i].flag = 0;
    }while( i!=is );

    DrawPolygon( n, polygon );
  }
}

// 等高面ポリゴンの作成を指揮するメソッド
template <int N>
void Isosurf<N>::MakeElements( void )
{
  for( int ix=0; ix<N-1; ix++ ){
    for( int iy=0; iy<N-1; iy++ ){
      for( int iz=0; iz<N-1; iz++ ){
	MakeElement(ix,iy,iz);
      }
    }
  }
}