#include "stdafx.h"

#include "Geom2d.h"

#include <C2/lm/math_util.h>


namespace lib_geo
{


//! 線分対線分の交差を判定する.
//! @param[in] a0 , a1 - 1本目の線分の両端点
//! @param[in] b0 , b1 - 2本目の線分の両端点
//! @retval - 2本の線分が交差している場合はtrue、それ以外はfalseを返す.
bool Geom2d::Intersect_Segments( const vec3f& a0 , const vec3f& a1 , const vec3f& b0 , const vec3f& b1 )
{
	float m , n;
	return Intersect_Segments( a0 , a1 , b0 , b1 , m , n );
}

//! 線分対線分の交差を判定する.
//! @param[in]  a0 , a1         - 1本目の線分の両端点
//! @param[in]  b0 , b1         - 2本目の線分の両端点
//! @param[out] intersect_point - 線分の交差点. z座標値は常に0.
//! @retval - 2本の線分が交差している場合はtrue、それ以外はfalseを返す.
bool Geom2d::Intersect_Segments( const vec3f& a0 , const vec3f& a1 , const vec3f& b0 , const vec3f& b1 , vec3f& intersect_point )
{
	float m , n;
	if( !Intersect_Segments( a0 , a1 , b0 , b1 , m , n ) )
		return false;

	intersect_point = a0 * m + a1 * (1.0f-m);
	return true;
}

//! 2本の線分の交差の方程式 [ (1-m) * a0 + m * a1 = (1-n) * b0 + n * b1 ] から, 線分の交差を判定する.
//! @param[in]  a0 , a1 - 1本目の線分の両端点
//! @param[in]  b0 , b1 - 2本目の線分の両端点
//! @param[out] m  , n  - 上記式のmとnの値を返す. ( 0ならa0,b0と一致, 1ならa1,b1と一致 )
//! @retval - 2本の線分が交差している場合はtrue, それ以外はfalseを返す.
bool Geom2d::Intersect_Segments( const vec3f& a0 , const vec3f& a1 , const vec3f& b0 , const vec3f& b1 , float& m , float& n )
{
	float u = a1.x * (b0.y - b1.y) + a0.x * (b1.y - b0.y) + (a0.y - a1.y) * b0.x + (a1.y - a0.y) * b1.x;
	if( u == 0.0f ) return false;

	m = -( a0.x * (b0.y - b1.y) + b1.x * (a0.y - b0.y) + (b1.y - a0.y) * b0.x ) / u;
	n =  ( a1.x * (b0.y - a0.y) + a0.x * (a1.y - b0.y) + (a0.y - a1.y) * b0.x ) / u;

	if( m < 0.0f ) return false;
	if( m > 1.0f ) return false;
	if( n < 0.0f ) return false;
	if( n > 1.0f ) return false;

	return true;
}


// 線分対直線の交差
bool Geom2d::Intersect_Segment_Line( const vec3f& e0 , const vec3f& e1 , const vec3f& line_pos , const vec3f& line_dir )
{
	float m , n;
	return Intersect_Segment_Line( e0 , e1 , line_pos , line_dir , m , n );
}

bool Geom2d::Intersect_Segment_Line( const vec3f& e0 , const vec3f& e1 , const vec3f& line_pos , const vec3f& line_dir , vec3f& intersect_point )
{
	float m , n;
	if( !Intersect_Segment_Line( e0 , e1 , line_pos , line_dir , m , n ) )
		return false;

	intersect_point = e0 * m + e1 * (1.0f-m);
	return true;
}

//! 線分対直線の交差
//! @param[in] seg0 , seg1 - 線分の両端点
//! @param[in] line_pos    - 直線の通過点
//! @param[in] line_dir    - 直線の方向
//! @param[out] m          - 交点位置を表す線分上の位置パラメータ ( 交点 = m * e0 + (1-m) * e1 )
//! @param[out] n          - 交点位置を表す直線上の位置パラメータ ( 交点 = line_pos + line_dir * n )
bool Geom2d::Intersect_Segment_Line( const vec3f& e0 , const vec3f& e1 , const vec3f& line_pos , const vec3f& line_dir , float& m , float& n )
{
	float cx = e0.x - e1.x;
	float cy = e0.y - e1.y;
	float dx = line_pos.x + e1.x;
	float dy = line_pos.y + e1.y;

	float mb = cx * line_dir.y - cy  * line_dir.x;
	if( mb == 0.0f )
		return false;

	float mu = dx * line_dir.y - dy * line_dir.x;

	m = mu / mb;
	if( m < 0.0f || 1.0f < m )
		return false;

	if( abs(line_dir.x) >= abs(line_dir.y) )
		n = ( cx * m - dx ) / line_dir.x;
	else
		n = ( cy * m - dy ) / line_dir.y;

	return true;
}


//! 円対線分の交差判定
//! @param[in] center , radius - 円の中心と半径
//! @param[in] e0 , e1         - 線分の両端点
//! @retval - 円と線分が交差している回数を返す(交差していなければ0,そうでなければ1or2).
int Geom2d::Intersect_Circle_Segment( const vec3f& center , const float& radius , const vec3f& e0 , const vec3f& e1 )
{
	float m , n;
	return Intersect_Circle_Segment( center , radius , e0 , e1 , m , n );
}

//! 円対線分の交差判定. 交差計算のパラメータを返す.
//! @param[in]  center , radius - 円の中心と半径
//! @param[in]  e0 , e1         - 線分の両端点
//! @param[out] m , n           - 線分の位置を表すパラメータ[ 交差点 = (1-t) * e0 + t * e1 ]. { (retval==1) => n未定義 | (retval==2) => m<n }
//! @retval - 円と線分が交差している回数を返す(交差していなければ0,そうでなければ1or2).
int Geom2d::Intersect_Circle_Segment( const vec3f& center , const float& radius , const vec3f& e0 , const vec3f& e1 , float& m , float& n )
{
	vec3f A = center - e0;
	vec3f B = e1 - e0;

	float X = dot( B , B );
	float Y = dot( A , B );
	float Z = dot( A , A ) - radius * radius;

	float sqrt_part = Y * Y - Z * X;
	if( sqrt_part < 0.0f ) return 0;

	if( sqrt_part == 0.0f )
	{
		m = Y / X;
		if( m < 0.0f || 1.0f < m ) return 0;
		return 1;
	}
	else
	{
		sqrt_part = sqrt( sqrt_part );

		m = ( Y - sqrt_part ) / X;
		n = ( Y + sqrt_part ) / Y;

		if( m < 0.0f || 1.0f < m )
		{
			(std::swap)( m , n );
		}

		if( m < 0.0f || 1.0f < m ) return 0;
		if( n < 0.0f || 1.0f < n ) return 1;
		return 2;
	}
}

//! AABB対線分の交差判定
//! @param[in] e0 , e1 - 線分の始点と終点
//! @retval true - 交差している場合 , false - 交差していない場合
bool Geom2d::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;
	}

	return true;
}


//! 点が3角形に内包されているか調べる
float Geom2d::Intersect_Triangle_Point( const vec3f& t0 , const vec3f& t1 , const vec3f& t2 , const vec3f& p )
{
	vec2f d10( t1.x - t0.x , t1.y - t0.y );
	vec2f d21( t2.x - t1.x , t2.y - t1.y );
	vec2f d02( t0.x - t2.x , t0.y - t2.y );

	vec2f dp0( p.x - t0.x , p.y - t0.y );
	vec2f dp1( p.x - t1.x , p.y - t1.y );
	vec2f dp2( p.x - t2.x , p.y - t2.y );

	float nd0 = dp0.x * d10.y - dp0.y * d10.x;
	float nd1 = dp1.x * d21.y - dp1.y * d21.x;
	if( ( nd0 * nd1 ) < 0.0f ) return false;
	float nd2 = dp2.x * d02.y - dp2.y * d02.x;
	if( ( nd1 * nd2 ) < 0.0f ) return false;

	return true;
}


//! 線分e0-e1上で、pに最も近い点を求める.
//! @param[in]  e0 , e1  - 線分の端点
//! @param[in]  p        - 点の位置
lm::vec3f Geom2d::Closest_Segment_Point( const vec3f& e0 , const vec3f& e1 , const vec3f& p )
{
	float dummy_weight;
	return Closest_Segment_Point( e0 , e1 , p , dummy_weight );
}

//! 線分e0-e1上で、pに最も近い点を求める.
//! @param[in]  e0 , e1  - 線分の端点
//! @param[in]  p        - 点の位置
//! @param[out] o_weight - returnする座標の線分e0-e1上での正規化位置.
lm::vec3f Geom2d::Closest_Segment_Point( const vec3f& e0 , const vec3f& e1 , const vec3f& p , float& o_weight )
{
	// xy平面に投影
	vec2f e0_2d ( e0.x , e0.y );
	vec2f e1_2d ( e1.x , e1.y );
	vec2f p_2d  ( p.x  , p.y  );

	// 線分の長さが0の場合
	if( e0_2d == e1_2d )
	{
		o_weight = 0.0f;

		// z座標値ははe0とe1の平均を使用
		return ( e0 + e1 ) * 0.5f;
	}
	else
	{
		vec2f e1_e0 = e1_2d - e0_2d;
		vec2f p_e0  = p_2d  - e0_2d;

		o_weight = dot( e1_e0 , p_e0 ) / dot( e1_e0 , e1_e0 );
		// 値域を線分両端に制限
		o_weight = lm::clamp( 0.0f , o_weight , 1.0f );
		
		// z座標値はe0とe1の補間値を使用
		return ( e1 - e0 ) * o_weight + e0;
	}
}


//! 点列で構成された曲線line上でpに最も近い点を求める.
lm::vec3f Geom2d::Closest_CurveLine_Point( const std::vector<vec3f>& line , const vec3f& p )
{
	int dummy_segment_idx;
	float dummy_norm_pos;
	return Closest_CurveLine_Point( line , p , dummy_segment_idx , dummy_norm_pos );
}

//! 点列で構成された曲線line上でpに最も近い点を求める.
//! @param[in]  line          - 曲線をあらわす点列
//! @param[in]  p             - lineとの最近傍点を調べる点
//! @param[out] o_segment_idx - 最近傍点が位置するセグメントのインデックス
//! @param[out] o_norm_pos    - セグメント内での正規化位置を表す値.
//! @retval - line上でpにもっと近い点
lm::vec3f Geom2d::Closest_CurveLine_Point( const std::vector<vec3f>& line , const vec3f& p , int& o_segment_idx , float& o_norm_pos )
{
	// TODO : 高速化できそう.

	_ASSERTE( line.size() >= 2 );

	vec3f closest_pos;
	float min_sq_length = (std::numeric_limits<float>::max)();
	o_segment_idx = -1;
	o_norm_pos = -1.0f;
	for( size_t i = 1 ; i < line.size() ; ++i )
	{
		int segment_idx = static_cast<int>(i-1);

		const vec3f& pos0 = line[i-1];
		const vec3f& pos1 = line[i  ];

		float weight;
		vec3f segment_closest = Closest_Segment_Point( pos0 , pos1 , p , weight );

		float dist_sq = ( p - segment_closest ).square_length();
		if( min_sq_length > dist_sq )
		{
			min_sq_length = dist_sq;

			closest_pos = segment_closest;
			o_segment_idx = segment_idx;
			o_norm_pos = weight;
		}
	}

	return closest_pos;
}


//! 2頂点をxy平面と垂直な方向から見たときの距離を求める
float Geom2d::DistanceXY( const vec3f& p0 , const vec3f& p1 )
{
	return sqrt( SqDistanceXY( p0 , p1 ) );
}

//! 2頂点をxy平面と垂直な方向から見たときの距離の二乗を求める
float Geom2d::SqDistanceXY( const vec3f& p0 , const vec3f& p1 )
{
	vec2f d( p0.x - p1.x , p0.y - p1.y );
	return d.square_length();
}


//! 3角形をxy平面に投影したときの面積を返す
float Geom2d::AreaSize_Triangle( const vec3f& t0 , const vec3f& t1 , const vec3f& t2 )
{
	vec2f d01( t0.x - t1.x , t0.y - t1.y );
	vec2f d12( t1.x - t2.x , t1.y - t2.y );
	vec2f d20( t2.x - t0.x , t2.y - t0.y );

	float len0 = d01.length();
	float len1 = d12.length();
	float len2 = d20.length();

	// ヘロンの公式
	float s = ( len0 + len1 + len2 ) / 0.5f;

	float sq = s * ( s - len0 ) * ( s - len1 ) * ( s - len2 );
	if( sq <= 0.0f ) return 0.0f;
	
	return sqrt( sq );
}


//! 3dベクトルを2dに投影する
lm::vec2f Geom2d::Proj_To2d( const vec3f& v )
{
	return vec2f( v.x , v.y );
}


//! 重心座標を取得する. ( [ p = t0 * u + t1 * v + t2 * w ] となる u , v , w を求める ).
//! @param[in]  t0 , t1 , t2 - 三角形の頂点座標
//! @param[in]  p            - 重心座標を計算する頂点
//! @param[out] u , v , w    - 重心座標値.
void Geom2d::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;
	v0.z = 0.0f;
	v1.z = 0.0f;
	v2.z = 0.0f;

	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;
}

//! 重心座標をvec3fの形式で取得する.
void Geom2d::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 );
}


//! 2つのベクトルの成す角を求める.
float Geom2d::Get_Angle( const vec3f& v0 , const vec3f& v1 )
{
	vec2f v20( v0.x , v0.y );
	vec2f v21( v1.x , v1.y );
	v20.normalize();
	v21.normalize();

	return acos( dot( v20 , v21 ) );
}

//! 2つのベクトルの角度を0〜2PIの範囲で求める.
float Geom2d::Get_RoundAngle( const vec3f& v0 , const vec3f& v1 )
{
	vec2f v20( v0.x , v0.y );  // v0の2D化
	vec2f v21( v1.x , v1.y );  // v1の2D化
	v20.normalize();
	v21.normalize();

	// v0 と直交する方向
	vec2f v20_perp = v20.get_perpendicular();

	// [ v0 , v0_perp ] からなる座標系での, v1のx座標値.
	float v1_local_x = dot( v20 , v21 );
	if( v1_local_x >=  1.0f ) return 0.0f;
	if( v1_local_x <= -1.0f ) return static_cast<float>(M_PI);

	// [ v0 , v0_perp ] からなる座標系での, v1のy座標値.
	float v1_local_y = dot( v20_perp , v21 );
	if( v1_local_y == 0.0f )
	{
		if( v1_local_x > 0.0f ) return 0.0f;
		else                    return static_cast<float>(M_PI);
	}
	else if( v1_local_y > 0.0f )
	{
		return acos( v1_local_x );
	}
	else
	{
		return 2.0f * static_cast<float>(M_PI) - acos( v1_local_x );
	}
}


}
