#include "Stdafx.h"
#include "UniformGrid.h"

#include <algorithm>

#include <C2/lm/lm.h>

using namespace lm;



namespace lib_geo
{


//! グリッドの範囲とグリッドオブジェクトをクリアする
void UniformGrid::Clear(void)
{
	ClearRange();
	ClearGrid();
}

//! グリッドの範囲をクリアする
void UniformGrid::ClearRange(void)
{
	m_TotalRange.clear();
}

//! グリッドをクリアする
void UniformGrid::ClearGrid(void)
{
	m_Grid.clear();
}


//! グリッドの範囲を広げる
void UniformGrid::Expand( const lm::vec3f& pos )
{
	m_TotalRange.expand(pos);
}

//! グリッドの範囲を広げる
void UniformGrid::Expand( const lm::range3f& range )
{
	m_TotalRange.expand( range );
}

//! 指定した分割数でグリッドを生成する.
void UniformGrid::CreateGrid( size_t sx , size_t sy , size_t sz )
{
	m_Grid.clear();
	m_Grid.resize(sx,sy,sz);

	for( size_t x = 0 ; x < sx ; ++x )
	{
		float xb = m_TotalRange.length_x() * (float)(x+0) / (float)sx + m_TotalRange.min_x();
		float xt = m_TotalRange.length_x() * (float)(x+1) / (float)sx + m_TotalRange.min_x();

		for( size_t y = 0 ; y < sy ; ++y )
		{
			float yb = m_TotalRange.length_y() * (float)(y+0) / (float)sy + m_TotalRange.min_y();
			float yt = m_TotalRange.length_y() * (float)(y+1) / (float)sy + m_TotalRange.min_y();

			for( size_t z = 0 ; z < sz ; ++z )
			{
				float zb = m_TotalRange.length_z() * (float)(z+0) / (float)sz + m_TotalRange.min_z();
				float zt = m_TotalRange.length_z() * (float)(z+1) / (float)sz + m_TotalRange.min_z();

				m_Grid(x,y,z).range.min_point().set( xb , yb , zb );
				m_Grid(x,y,z).range.max_point().set( xt , yt , zt );
			}
		}
	}
}

//! posにインデックスを登録する
void UniformGrid::AddIndex( int index , const lm::vec3f& pos )
{
	std::vector<UniformGridCell*> cells;
	GetCells( pos , cells );
	for( size_t i = 0 ; i < cells.size() ; ++i )
		cells[i]->indices.push_back(index);
}

//! rangeにインデックスを登録する
void UniformGrid::AddIndex( int index , const lm::range3f& range )
{
	std::vector<UniformGridCell*> cells;
	GetCells( range , cells );
	for( size_t i = 0 ; i < cells.size() ; ++i )
		cells[i]->indices.push_back(index);
}

//! posを含むセルを返す
void UniformGrid::GetCells( const lm::vec3f& pos , std::vector<UniformGridCell*>& cells )
{
	if( m_Grid.empty() ) return;

	int ix , iy , iz;
	GetCellIndex( pos , ix , iy , iz );
	cells.push_back( &m_Grid(ix,iy,iz) );
}

//! posを含むセルを返す
void UniformGrid::GetCells( const lm::vec3f& pos , std::vector<const UniformGridCell*>& cells ) const
{
	if( m_Grid.empty() ) return;

	int ix , iy , iz;
	GetCellIndex( pos , ix , iy , iz );
	cells.push_back( &m_Grid(ix,iy,iz) );
}

//! posを含むセルのインデックスを返す
void UniformGrid::GetCellIndex( const lm::vec3f& pos , int& ix , int& iy , int& iz ) const
{
	if( m_Grid.empty() ) return;

	lm::vec3f pos_l = pos - m_TotalRange.min_point();

	pos_l.x = (float)m_Grid.size_x() * pos_l.x / m_TotalRange.length_x();
	pos_l.y = (float)m_Grid.size_y() * pos_l.y / m_TotalRange.length_y();
	pos_l.z = (float)m_Grid.size_z() * pos_l.z / m_TotalRange.length_z();

	ix = (lm::clamp)( 0 , (int)pos_l.x , (int)m_Grid.size_x() - 1 );
	iy = (lm::clamp)( 0 , (int)pos_l.y , (int)m_Grid.size_y() - 1 );
	iz = (lm::clamp)( 0 , (int)pos_l.z , (int)m_Grid.size_z() - 1 );
}


//! rangeと交差するすべてのセルを返す
void UniformGrid::GetCells( const lm::range3f& range , std::vector<UniformGridCell*>& cells )
{
	if( m_Grid.empty() ) return;

	int ix0 , ix1 , iy0 , iy1 , iz0 , iz1;
	GetCellIndex( range , ix0 , ix1 , iy0 , iy1 , iz0 , iz1 );

	cells.reserve( (ix1-ix0+1) * (iy1-iy0+1) * (iz1-iz0+1) );
	for( int x = ix0 ; x <= ix1 ; ++x )
	{
		for( int y = iy0 ; y <= iy1 ; ++y )
		{
			for( int z = iz0 ; z <= iz1 ; ++z )
			{
				cells.push_back( &m_Grid(x,y,z) );
			}
		}
	}
}

//! rangeと交差するすべてのセルを返す
void UniformGrid::GetCells( const lm::range3f& range , std::vector<const UniformGridCell*>& cells ) const
{
	if( m_Grid.empty() ) return;

	int ix0 , ix1 , iy0 , iy1 , iz0 , iz1;
	GetCellIndex( range , ix0 , ix1 , iy0 , iy1 , iz0 , iz1 );

	cells.reserve( (ix1-ix0+1) * (iy1-iy0+1) * (iz1-iz0+1) );
	for( int x = ix0 ; x <= ix1 ; ++x )
	{
		for( int y = iy0 ; y <= iy1 ; ++y )
		{
			for( int z = iz0 ; z <= iz1 ; ++z )
			{
				cells.push_back( &m_Grid(x,y,z) );
			}
		}
	}
}

//! rangeと接するセルの範囲を求める
void UniformGrid::GetCellIndex( const lm::range3f& range , int& ix0 , int& ix1 , int& iy0 , int& iy1 , int& iz0 , int& iz1 ) const
{
	if( m_Grid.empty() ) return;

	range3f aloc = range;
	aloc.min_point() -= m_TotalRange.min_point();
	aloc.max_point() -= m_TotalRange.min_point();

	aloc.min_x() = (float)m_Grid.size_x() * aloc.min_x() / m_TotalRange.length_x();
	aloc.min_y() = (float)m_Grid.size_y() * aloc.min_y() / m_TotalRange.length_y();
	aloc.min_z() = (float)m_Grid.size_z() * aloc.min_z() / m_TotalRange.length_z();

	aloc.max_x() = (float)m_Grid.size_x() * aloc.max_x() / m_TotalRange.length_x();
	aloc.max_y() = (float)m_Grid.size_y() * aloc.max_y() / m_TotalRange.length_y();
	aloc.max_z() = (float)m_Grid.size_z() * aloc.max_z() / m_TotalRange.length_z();

	ix0 = (lm::clamp)( 0 , (int)aloc.min_x() , (int)m_Grid.size_x() - 1 );
	ix1 = (lm::clamp)( 0 , (int)aloc.max_x() , (int)m_Grid.size_x() - 1 );
	iy0 = (lm::clamp)( 0 , (int)aloc.min_y() , (int)m_Grid.size_y() - 1 );
	iy1 = (lm::clamp)( 0 , (int)aloc.max_y() , (int)m_Grid.size_y() - 1 );
	iz0 = (lm::clamp)( 0 , (int)aloc.min_z() , (int)m_Grid.size_z() - 1 );
	iz1 = (lm::clamp)( 0 , (int)aloc.max_z() , (int)m_Grid.size_z() - 1 );
}


//! posと交差するセル内のインデックスを返す
void UniformGrid::GetIndices( const lm::vec3f& pos , std::vector<int>& indices ) const
{
	if( m_Grid.empty() ) return;

	int ix , iy , iz;
	GetCellIndex( pos , ix , iy , iz );

	GetIndicesFromMatrixIndex( indices , ix , ix , iy , iy , iz , iz );
}


//! rangeと交差するすべてのセル内のインデックスを返す
void UniformGrid::GetIndices( const lm::range3f& range , std::vector<int>& indices ) const
{
	if( m_Grid.empty() ) return;

	int ix0 , ix1 , iy0 , iy1 , iz0 , iz1;
	GetCellIndex( range , ix0 , ix1 , iy0 , iy1 , iz0 , iz1 );

	GetIndicesFromMatrixIndex( indices , ix0 , ix1 , iy0 , iy1 , iz0 , iz1 );
}


//! 指定した行列の範囲に含まれる、すべてのインデックスを返す
void UniformGrid::GetIndicesFromMatrixIndex( std::vector<int>& indices , int ix0 , int ix1 , int iy0 , int iy1 , int iz0 , int iz1 ) const
{
	size_t num_total_indices = 0;
	for( int x = ix0 ; x <= ix1 ; ++x )
	{
		for( int y = iy0 ; y <= iy1 ; ++y )
		{
			for( int z = iz0 ; z <= iz1 ; ++z )
			{
				num_total_indices += m_Grid(x,y,z).indices.size();
			}
		}
	}

	indices.clear();
	indices.reserve(num_total_indices);
	for( int x = ix0 ; x <= ix1 ; ++x )
	{
		for( int y = iy0 ; y <= iy1 ; ++y )
		{
			for( int z = iz0 ; z <= iz1 ; ++z )
			{
				const UniformGridCell& cell = m_Grid(x,y,z);
				for( size_t i = 0 ; i < cell.indices.size() ; ++i )
					indices.push_back( cell.indices[i] );
			}
		}
	}

	std::sort( indices.begin() , indices.end() );
	indices.erase( std::unique( indices.begin() , indices.end() ) , indices.end() );
}


}
