#include "collision_detec_3d.h"


void Rotate3(double &x1, double &y1, double &z1,  double alphaX,double alphaY,double alphaZ){
	double x2,  y2,  z2;

	//Z Axis Rotation
	double x3 = x1 * cos(alphaZ) - y1 * sin(alphaZ);
	double y3 = x1 * sin(alphaZ) + y1 * cos(alphaZ);
	double z3 = z1;

	//Y Axis Rotation
	double z4 = z3 * cos(alphaY) - x3 * sin(alphaY);
	double x4 = z3 * sin(alphaY) + x3 * cos(alphaY);
	double y4 = y3;

	//X Axis Rotation
	y2 = y4 * cos(alphaX) - z4 * sin(alphaX);
	z2 = y4 * sin(alphaX) + z4 * cos(alphaX);
	x2 = x4;

	x1=x2;
	y1=y2;
	z1=z2;

}


//法線ベクトルの算出
void normal_calc(xyz tri[], xyz &normal){
	double p1[3],p2[3],p3[3],v_nl[3];

	p1[0]=tri[0].x;
	p1[1]=tri[0].y;
	p1[2]=tri[0].z;
	p2[0]=tri[1].x;
	p2[1]=tri[1].y;
	p2[2]=tri[1].z;
	p3[0]=tri[2].x;
	p3[1]=tri[2].y;
	p3[2]=tri[2].z;

	double v1[3],v2[3];
	double length_nl;
	//ベクトル計算
	v1[0]=p2[0] - p1[0];
	v1[1]=p2[1] - p1[1];
	v1[2]=p2[2] - p1[2];
	v2[0]=p3[0] - p1[0];
	v2[1]=p3[1] - p1[1];
	v2[2]=p3[2] - p1[2];
	//外積
	v_nl[0] = v1[1] * v2[2] - v1[2] * v2[1];
	v_nl[1] = v1[2] * v2[0] - v1[0] * v2[2];
	v_nl[2] = v1[0] * v2[1] - v1[1] * v2[0];
	//正規化
	length_nl = sqrt(v_nl[0]*v_nl[0]+v_nl[1]*v_nl[1]+v_nl[2]*v_nl[2]);
	if(length_nl != 0){
		v_nl[0] = v_nl[0]/length_nl;
		v_nl[1] = v_nl[1]/length_nl;
		v_nl[2] = v_nl[2]/length_nl;
	}

	normal.x=v_nl[0];
	normal.y=v_nl[1];
	normal.z=v_nl[2];
	return;
}



//外積の算出
xyz crossproduct_calc(xyz xyz1,xyz xyz2){
	double v1[3],v2[3],v_nl[3];
	xyz out;

	//ベクトル計算
	v1[0]=xyz1.x;
	v1[1]=xyz1.y;
	v1[2]=xyz1.z;
	v2[0]=xyz2.x;
	v2[1]=xyz2.y;
	v2[2]=xyz2.z;

	//外積
	v_nl[0] = v1[1] * v2[2] - v1[2] * v2[1];
	v_nl[1] = v1[2] * v2[0] - v1[0] * v2[2];
	v_nl[2] = v1[0] * v2[1] - v1[1] * v2[0];

	out.x=v_nl[0];
	out.y=v_nl[1];
	out.z=v_nl[2];
	return out;
}


//内積
double innerproduct_calc(xyz xyz1, xyz xyz2) {
	int i;
	double s = 0.0;

	double vec1[3];
	double vec2[3];

	vec1[0]=xyz1.x;
	vec1[1]=xyz1.y;
	vec1[2]=xyz1.z;

	vec2[0]=xyz2.x;
	vec2[1]=xyz2.y;
	vec2[2]=xyz2.z;	

	for ( i = 0; i < 3; i++ ) {
		s += vec1[i] * vec2[i];
	}

	return s;
}

//XYZ座標の引き算

xyz sub_calc(xyz xyz1, xyz xyz2) {
	xyz out;
	out.x=xyz1.x-xyz2.x;
	out.y=xyz1.y-xyz2.y;
	out.z=xyz1.z-xyz2.z;
	return out;
}

//XYZ座標のスカラー量を求める
double norm_calc(xyz xyz1){
	double vec[3];
	int i;
	double s = 0.0;

	vec[0]=xyz1.x;
	vec[1]=xyz1.y;
	vec[2]=xyz1.z;

	for ( i = 0; i < 3; i++ ) {
		s += vec[i] * vec[i];
	}

	return sqrt(s);
}


//XYZ座標の掛け算
xyz scale_calc(xyz xyz1,double scale){
	xyz xyz2;

	xyz2.x=xyz1.x*scale;
	xyz2.y=xyz1.y*scale;
	xyz2.z=xyz1.z*scale;

	return xyz2;
}


//XYZ座標の足し算
xyz add_calc(xyz xyz1,xyz xyz2){
	xyz xyz3;

	xyz3.x=xyz1.x+xyz2.x;
	xyz3.y=xyz1.y+xyz2.y;
	xyz3.z=xyz1.z+xyz2.z;

	return xyz3;
}


//三角形と線分のあたり判定
bool collision_tri_line(xyz tri[],xyz line[],xyz &contact_pt){
	//あたり判定 
	//true→衝突
	//false→あたっていない

	//xyz tri[3];
	//xyz line[2];

	xyz normal_vec;
	normal_calc(tri,normal_vec);

	///三角形を含む面と線分の交差
	//平面上のある点から両端に引いたベクトルと法線ベクトルとの符号を調べる
	xyz sub1,sub2;
	double h1,h2;
	//
	sub1=sub_calc(tri[0],line[0]);
	sub2=sub_calc(tri[0],line[1]);

	h1=innerproduct_calc(sub1,normal_vec);
	h2=innerproduct_calc(sub2,normal_vec);

	//符号を調べる
	int sign_h1 = (h1 > 0) - (h1 < 0);
	int sign_h2 = (h2 > 0) - (h2 < 0);

	//符号が一緒であれば、面と線分は交差していない
	if(sign_h1==sign_h2){
		//	cout<<"交差していない"<<endl;
		return false;
	}

	//三角形と線分の交点を求める。
	//i = (p1 * h2 + p2 * h1) / (h1 + h2)
	xyz node;
	node=add_calc(scale_calc(line[0],fabs(h2)),scale_calc(line[1],fabs(h1)));
	double h3=(fabs(h1)+fabs(h2));
	if(h3!=0)	node=scale_calc(node,(1/h3));
	else if(h3==0)	cout<<"error"<<endl;

	contact_pt=node;

	//交点から三角形頂点へのベクトル
	//n1n2n3とするとn1・nとn2・nとn3・nの符合が一致すること
	xyz pp,qq,rr;
	xyz np,nq,nr;
	double dot1,dot2,dot3;
	pp=sub_calc(tri[0],node);
	qq=sub_calc(tri[1],node);
	rr=sub_calc(tri[2],node);

	np=crossproduct_calc(pp,qq);
	nq=crossproduct_calc(qq,rr);
	nr=crossproduct_calc(rr,pp);

	dot1=innerproduct_calc(np,normal_vec);
	dot2=innerproduct_calc(nq,normal_vec);
	dot3=innerproduct_calc(nr,normal_vec);


	//符号を調べる
	int sign_dot1 = (dot1 > 0) - (dot1 < 0);
	int sign_dot2 = (dot2 > 0) - (dot2 < 0);
	int sign_dot3 = (dot3 > 0) - (dot3 < 0);


	if((sign_dot1==sign_dot2)&&(sign_dot2==sign_dot3)){
		//	cout<<"交差している"<<endl;
		return true;
	}
	else{
		//	cout<<"交差していない"<<endl;
		return false;

	}

	//cout<<"交差していない"<<endl;
	return false;
}

//三角形と三角形のあたり判定
bool collision_tri_tri(	xyz in_tri[],	xyz in_tri2[]){

	xyz line[2];
	xyz out[6];

	bool flag[6]={0,0,0,0,0,0};
	xyz tri[3];

	tri[0]=in_tri[0];
	tri[1]=in_tri[1];
	tri[2]=in_tri[2];

	line[0]=in_tri2[0];
	line[1]=in_tri2[1];
	flag[0]=collision_tri_line(tri,line,out[0]);

	line[0]=in_tri2[1];
	line[1]=in_tri2[2];
	flag[1]=collision_tri_line(tri,line,out[1]);

	line[0]=in_tri2[2];
	line[1]=in_tri2[0];
	flag[2]=collision_tri_line(tri,line,out[2]);

	tri[0]=in_tri2[0];
	tri[1]=in_tri2[1];
	tri[2]=in_tri2[2];

	line[0]=in_tri[0];
	line[1]=in_tri[1];
	flag[3]=collision_tri_line(tri,line,out[3]);

	line[0]=in_tri[1];
	line[1]=in_tri[2];
	flag[4]=collision_tri_line(tri,line,out[4]);

	line[0]=in_tri[2];
	line[1]=in_tri[0];
	flag[5]=collision_tri_line(tri,line,out[5]);


	if(flag[0]||flag[1]||flag[2]||flag[3]||flag[4]||flag[5]){
		///cout<<"交差している"<<endl;
		return true;

	}
	else{
		//cout<<"交差していない"<<endl;
		return false;

	}
	return 0;

}