#include "stdafx.h"

#include "Geom3d.h"

#include <C2/lm/math_util.h>


namespace lib_geo
{


//! 平面と線の交差判定.
bool Geom3d::Intersect_Plane_Segment( const Plane& i_plane , const Segment& i_segment )
{
	return Intersect_Plane_Segment( i_plane.normal , i_plane.origin , i_segment.p0 , i_segment.p1 );
}

//! 平面と線の交差判定. intersect_posに交差位置を返す.
bool Geom3d::Intersect_Plane_Segment( const Plane& i_plane , const Segment& i_segment , vec3f& intersect_pos )
{
	return Intersect_Plane_Segment( i_plane.normal , i_plane.origin , i_segment.p0 , i_segment.p1 , intersect_pos );
}

//! 平面と線の交差判定.
bool Geom3d::Intersect_Plane_Segment( const vec3f& plane_normal , const vec3f& plane_point , const vec3f& e0 , const vec3f& e1 )
{
	vec3f dummy_pos;
	return Intersect_Plane_Segment( plane_normal , plane_point , e0 , e1 , dummy_pos );
}

//! 平面と線の交差判定. intersect_posに交差位置を返す.
//! @param[in]  plane_normal  - 平面の法線
//! @param[in]  plane_point   - 平面の通過点
//! @param[in]  e0 , e1       - 線分の端点
//! @param[out] intersect_pos - 線分と平面の交差点
bool Geom3d::Intersect_Plane_Segment( const vec3f& plane_normal , const vec3f& plane_point , const vec3f& e0 , const vec3f& e1 , vec3f& intersect_pos )
{
	// 方程式 [ n*(v-p)=0 , v = (1-k) * e0 + k * e1 ] から平面と線の交差を求める.

	float ku = dot( plane_normal , plane_point - e0 );
	float kb = dot( e1 - e0 , plane_normal );
	if( kb == 0.0f ) return false;
	float k = ku / kb;
	
	if( k < 0.0f ) return false;
	if( k > 1.0f ) return false;

	intersect_pos = k * ( e1 - e0 ) + e0;

	return true;
}


//! 線分対三角形の交差判定.
bool Geom3d::Intersect_Triangle_Segment( const Triangle& i_triangle , const Segment& i_segment )
{
	return Intersect_Triangle_Segment( i_triangle.p0 , i_triangle.p1 , i_triangle.p2 , i_segment.p0 , i_segment.p1 );
}

//! 線分対三角形の交差判定. intersect_posに交差位置を返す.
bool Geom3d::Intersect_Triangle_Segment( const Triangle& i_triangle , const Segment& i_segment , vec3f& intersect_pos )
{
	return Intersect_Triangle_Segment( i_triangle.p0 , i_triangle.p1 , i_triangle.p2 , i_segment.p0 , i_segment.p1 , intersect_pos );
}

//! 線分対三角形の交差判定.
bool Geom3d::Intersect_Triangle_Segment( const vec3f& t0 , const vec3f& t1 , const vec3f& t2 , const vec3f& e0 , const vec3f& e1 )
{
	vec3f dummy_pos;
	return Intersect_Triangle_Segment( t0 , t1 , t2 , e0 , e1 , dummy_pos );
}

//! 線分対三角形の交差判定. intersect_pos に交差位置を返す.
//! @param[in]  t0 , t1 , t2  - 三角形の頂点座標
//! @param[in]  e0 , e1       - 線分の端点座標
//! @param[out] intersect_pos - 交差位置
bool Geom3d::Intersect_Triangle_Segment( const vec3f& t0 , const vec3f& t1 , const vec3f& t2 , const vec3f& e0 , const vec3f& e1 , vec3f& intersect_pos )
{
	vec3f n = cross( t1 - t0 , t2 - t0 );
	n.normalize();

	// nと垂直な無限遠平面と線分との交差
	if( !Intersect_Plane_Segment( n , t0 , e0 , e1 , intersect_pos ) ) return false;

	bool c0 = ( dot( cross( intersect_pos - t0 , t1 - t0 ) , n ) > 0.0f );
	bool c1 = ( dot( cross( intersect_pos - t1 , t2 - t1 ) , n ) > 0.0f );
	if( c0 != c1 ) return false;

	bool c2 = ( dot( cross( intersect_pos - t2 , t0 - t2 ) , n ) > 0.0f );
	if( c1 != c2 ) return false;
	if( c2 != c0 ) return false;

	return true;
}

//! 三角形対三角形の交差判定.
//! @param[in] t00 , t01 , t02 - 三角形0の頂点位置
//! @param[in] t10 , t11 , t12 - 三角形1の頂点位置
//! @retval true - 交差している場合 , false - 交差していない場合
bool Geom3d::Intersect_Triangle_Triangle( const vec3f& t00 , const vec3f& t01 , const vec3f& t02 , const vec3f& t10 , const vec3f& t11 , const vec3f& t12 )
{
	vec3f n0 = cross( t01 - t00 , t02 - t00 );
	vec3f n1 = cross( t11 - t10 , t12 - t10 );
	n0.normalize();
	n1.normalize();

	float h00 = dot( n0 , t10 - t00 );
	float h01 = dot( n0 , t11 - t00 );
	float h02 = dot( n0 , t12 - t00 );
	float min_h0 = (lm::min)( h00 , h01 , h02 );
	float max_h0 = (lm::max)( h00 , h01 , h02 );
	if( min_h0 * max_h0 >= 0.0f ) return false;

	float h10 = dot( n1 , t00 - t10 );
	float h11 = dot( n1 , t01 - t10 );
	float h12 = dot( n1 , t02 - t10 );
	float min_h1 = (lm::min)( h10 , h11 , h12 );
	float max_h1 = (lm::max)( h10 , h11 , h12 );
	if( min_h1 * max_h1 >= 0.0f ) return false;

	if( Geom3d::Intersect_Triangle_Segment( t00 , t01 , t02 , t10 , t11 ) ) return true;
	if( Geom3d::Intersect_Triangle_Segment( t00 , t01 , t02 , t11 , t12 ) ) return true;
	if( Geom3d::Intersect_Triangle_Segment( t00 , t01 , t02 , t12 , t10 ) ) return true;

	if( Geom3d::Intersect_Triangle_Segment( t10 , t11 , t12 , t00 , t01 ) ) return true;
	if( Geom3d::Intersect_Triangle_Segment( t10 , t11 , t12 , t01 , t02 ) ) return true;
	if( Geom3d::Intersect_Triangle_Segment( t10 , t11 , t12 , t02 , t00 ) ) return true;

	return false;
}


//! 線分同士の擬似交差判定. 大体平面上にある2線分の交差位置を求める.
//! 平面性の判断はしない. いわゆるねじれの位置にある場合にも交差が判定されることがある.
//! @param[in]  a0 , a1      - 線分aの端点
//! @param[in]  b0 , b1      - 線分aの端点
//! @param[out] inter_pos    - 交点
bool Geom3d::Intersect_Segments( const vec3f& a0 , const vec3f& a1 , const vec3f& b0 , const vec3f& b1 , vec3f& inter_pos )
{
	vec3f dir_a = ( a1 - a0 ).get_normalize();
	vec3f dir_b = ( b1 - b0 ).get_normalize();

	// 線分a,bが大体乗っている平面の法線を求める
	vec3f plane_normal = cross( dir_a , dir_b );
	
	// 線分が平行なら交差していないと判断する
	if( plane_normal.is_zero() )
		return false;
	
	vec3f cut_normal = cross( plane_normal , dir_a ).get_normalize();

	// plane_normalの方向から見てaをとおり視線と平行な平面とbが交差しているか判断する.
	vec3f plane_interpos;
	if( !Intersect_Plane_Segment( cut_normal , a0 , b0 , b1 , plane_interpos ) )
		return false;

	// plane_interposが a0 から a1 の範囲にあるかしらべる
	if( dot( a1 - plane_interpos , a0 - plane_interpos ) < 0.0f )
		return false;

	inter_pos = plane_interpos;
	return true;
}


//! AABB対線分の交差判定
//! @param[in] e0 , e1 - 線分の始点と終点
//! @retval true - 交差している場合 , false - 交差していない場合
bool Geom3d::Intersect_AABB_Segment( const range3f& aabb , const vec3f& e0 , const vec3f& e1 )
{
	float t_top_max = -(std::numeric_limits<float>::max)();
	float t_end_min =  (std::numeric_limits<float>::max)();

	vec3f v10 = e1 - e0;
	if( v10.x == 0.0f )
	{
		if( v10.x < aabb.min_x() || aabb.max_x() < v10.x ) return false;
	}
	else
	{
		float t_top = ( aabb.min_x() - e0.x ) / v10.x;
		float t_end = ( aabb.max_x() - e0.x ) / v10.x;
		if( t_top > t_end ) std::swap( t_top , t_end );

		t_top_max = (std::max)( t_top_max , t_top );
		t_end_min = (std::min)( t_end_min , t_end );
		if( t_top_max > t_end_min ) return false;
	}

	if( v10.y == 0.0f )
	{
		if( v10.y < aabb.min_y() || aabb.max_y() < v10.y ) return false;
	}
	else
	{
		float t_top = ( aabb.min_y() - e0.y ) / v10.y;
		float t_end = ( aabb.max_y() - e0.y ) / v10.y;
		if( t_top > t_end ) std::swap( t_top , t_end );

		t_top_max = (std::max)( t_top_max , t_top );
		t_end_min = (std::min)( t_end_min , t_end );
		if( t_top_max > t_end_min ) return false;
	}

	if( v10.z == 0.0f )
	{
		if( v10.z < aabb.min_z() || aabb.max_z() < v10.z ) return false;
	}
	else
	{
		float t_top = ( aabb.min_z() - e0.z ) / v10.z;
		float t_end = ( aabb.max_z() - e0.z ) / v10.z;
		if( t_top > t_end ) std::swap( t_top , t_end );

		t_top_max = (std::max)( t_top_max , t_top );
		t_end_min = (std::min)( t_end_min , t_end );
		if( t_top_max > t_end_min ) return false;
	}

	return true;
}

	
//! 線分[e0--e1]上で、pに最も近い位置を求める
lm::vec3f Geom3d::Closest_Segment_Point( const vec3f& e0 , const vec3f& e1 , const vec3f& p )
{
	// 線分の長さが0の場合
	if( e0 == e1 )
	{
		return e0;
	}
	else
	{
		vec3f e1_e0 = e1 - e0;
		vec3f p_e0  = p  - e0;

		float t = dot( e1_e0 , p_e0 ) / dot( e1_e0 , e1_e0 );
		t = lm::clamp( 0.0f , t , 1.0f );

		return ( e1 - e0 ) * t + e0;
	}
}


//! 三角形[t0--t1--t2]の面上で、pに最も近い位置を求める
lm::vec3f Geom3d::Closest_Triangle_Point( const vec3f& t0 , const vec3f& t1 , const vec3f& t2 , const vec3f& p )
{
	if( t0 == t1 ) return Closest_Segment_Point( t0 , t2 , p );
	if( t1 == t2 ) return Closest_Segment_Point( t1 , t0 , p );
	if( t2 == t0 ) return Closest_Segment_Point( t2 , t1 , p );

	// pがt0の外側の頂点領域の中にあるかどうかチェック
	vec3f t10 = t1 - t0;
	vec3f t20 = t2 - t0;
	vec3f pt0 = p  - t0;
	float d1 = dot( t10 , pt0 );
	float d2 = dot( t20 , pt0 );
	if( d1 <= 0.0f && d2 <= 0.0f ) return t0;

	// pがt1の外側の頂点領域にあるかどうかチェック
	vec3f bp = p - t1;
	float d3 = dot( t10 , bp );
	float d4 = dot( t20 , bp );
	if( d3 >= 0.0f && d4 <= d3 ) return t1;

	// pが[t0--t1]の辺領域の中にあるかどうかチェックし、あればpの[t0--t1]上に対する射影を返す
	float vc = d1 * d4 - d3 * d2;
	if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
	{
		float v = d1 / ( d1 - d3 );
		return t0 + v * t10;
	}

	// pがt2の外側の頂点領域にあるかどうかチェック
	vec3f cp = p - t2;
	float d5 = dot( t10 , cp );
	float d6 = dot( t20 , cp );
	if( d6 >= 0.0f && d5 <= d6 ) return t2;

	// Pが[t2--t0]の辺領域の中になるかどうかチェック
	float vb = d5 * d2 - d1 * d6;
	if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
	{
		float w = d2 / ( d2 - d6 );
		return t0 + w * t20;
	}

	// Pが[t1--t2]の辺領域の中になるかどうかチェック
	float va = d3 * d6 - d5 * d4;
	if( va <= 0.0f && ( d4 - d3 ) >= 0.0f && ( d5 - d6 ) >= 0.0f )
	{
		float w = ( d4 - d3 ) / ( ( d4 - d3 ) + ( d5 - d6 ) );
		return t1 + w * ( t2 - t1 );
	}
	
	// pは面領域の中にある. Qをその重心座標(u,v,w)を用いて計算
	float denom = 1.0f / ( va + vb + vc );
	float v = vb * denom;
	float w = vc * denom;
	return t0 + t10 * v + t20 * w;
}


//! 三角形内での, pの重心座標を取得する.
void Geom3d::Get_BaryCentCoord( const Triangle& i_triangle , const vec3f& p , float& u , float& v , float& w )
{
	Get_BaryCentCoord( i_triangle.p0 , i_triangle.p1 , i_triangle.p2 , p , u , v , w );
}

//! 三角形内での, pの重心座標を取得する.
void Geom3d::Get_BaryCentCoord( const Triangle& i_triangle , const vec3f& p , vec3f& bary_coord )
{
	Get_BaryCentCoord( i_triangle , p , bary_coord.x , bary_coord.y , bary_coord.z );
}

//! 三角形[t0--t1--t2]内での, pの重心座標を取得する.
void Geom3d::Get_BaryCentCoord( const vec3f& t0 , const vec3f& t1 , const vec3f& t2 , const vec3f& p , float& u , float& v , float& w )
{
	vec3f v0 = t1 - t0;
	vec3f v1 = t2 - t0;
	vec3f v2 = p  - t0;

	float d00 = dot( v0 , v0 );
	float d01 = dot( v0 , v1 );
	float d11 = dot( v1 , v1 );
	float d20 = dot( v2 , v0 );
	float d21 = dot( v2 , v1 );
	float denom = d00 * d11 - d01 * d01;
	v = ( d11 * d20 - d01 * d21 ) / denom;
	w = ( d00 * d21 - d01 * d20 ) / denom;
	u = 1.0f - v - w;
}

//! 三角形[t0--t1--t2]内での, pの重心座標を取得する.
void Geom3d::Get_BaryCentCoord( const vec3f& t0 , const vec3f& t1 , const vec3f& t2 , const vec3f& p , vec3f& bary_coord )
{
	Get_BaryCentCoord( t0 , t1 , t2 , p , bary_coord.x , bary_coord.y , bary_coord.z );
}


float Geom3d::Areasize_Triangle( const vec3f& t0 , const vec3f& t1 , const vec3f& t2 )
{
	lm::vec3f v20 = t2 - t0;
	lm::vec3f v10 = t1 - t0;

	float in_sq = v20.square_length() * v10.square_length() - lm::pow2( dot( v20 , v10 ) );
	if( in_sq <= 0.0f )
		return 0.0f;

	return 0.5f * sqrt( in_sq );
}


}
