#include "Collision.h"

#include <stdio.h>
#define EPS 0.005f
///collision detection///

NewCCollisionDetection::NewCCollisionDetection() {

	object = new CCollisionUnit;
	m_bs = 0;
	m_fs = 0;
	faces = NULL;
	m_FaceNum = 0;



}
NewCCollisionDetection::~NewCCollisionDetection() {
	if(object != NULL) 
		delete object;
	if(faces != NULL)
		delete[] faces;
}
// Get Slide vector 
D3DXVECTOR3 NewCCollisionDetection::SlidePolygonVector(D3DXVECTOR3 Velocity,D3DXVECTOR3 PlaneNormal) {
	//((velocity dot planenormal) *planenormal) - velocity
	

	D3DXVec3Normalize(&PlaneNormal,&PlaneNormal);
	float distance = D3DXVec3Dot(&Velocity,&PlaneNormal);
	D3DXVECTOR3 tmp = (-PlaneNormal) * distance;
	tmp = tmp - Velocity;
	D3DXVec3Normalize(&tmp,&tmp);

	//tmp *=(-10.0f);
	return tmp;

}

//return value :  pos ������ �Ÿ� 
D3DXVECTOR3 NewCCollisionDetection::GetDistanceClosetPointOnLine(D3DXVECTOR3 pos,D3DXVECTOR3 a,D3DXVECTOR3 b){
	//line �󿡼��� �浹�Ÿ�  
	
	D3DXVECTOR3	pline = pos - a;
	D3DXVECTOR3 line = b-a;
	
	
	float line_length = D3DXVec3Length(&line);

	D3DXVec3Normalize(&line,&line);
	//float line_length = D3DXVec3Length(&line);

	// pline ���� line ���������� ũ�� 
	float distance = D3DXVec3Dot(&line,&pline);

	if(distance <0.0f) {
		return a;	
	}
	else if(distance > line_length) {
		return b;
	}
	line.x = line.x * distance;
	line.y = line.y * distance;
	line.z = line.z * distance;
	
	return (a + line);	
}
D3DXVECTOR3 NewCCollisionDetection::GetDistanceClosetPointOnTriangle(D3DXVECTOR3 pos,D3DXVECTOR3 a,D3DXVECTOR3 b,D3DXVECTOR3 c) { // triangle ���� �浹�� ���ϱ�

	
	// triangle ���� ���ο� ���� ����� �Ÿ��� point ���� �Ÿ� vector
	float plength[3] = {0.0f,0.0f,0.0f};
	float min_length = 0.0f;

	D3DXVECTOR3 tline[3];

	D3DXVECTOR3 return_vec(0.0f,0.0f,0.0f);
	tline[0] = GetDistanceClosetPointOnLine(pos,a,b);
	tline[1] = GetDistanceClosetPointOnLine(pos,b,c);
	tline[2] = GetDistanceClosetPointOnLine(pos,c,a);
	
	plength[0] = D3DXVec3Length(&(pos-tline[0]));
	plength[1] = D3DXVec3Length(&(pos-tline[1]));
	plength[2] = D3DXVec3Length(&(pos-tline[2]));

	for(int i=0;i<3;i++) {
		if(i==0) {
			min_length = plength[i];
			return_vec = tline[i]; 
		}
		else{
			if(min_length >plength[i]) {
				min_length = plength[i];
				return_vec = tline[i];
			}
		}

	}

	return return_vec;	
}

bool NewCCollisionDetection::CheckCollisionSphere(D3DXVECTOR3 pos,D3DXVECTOR3 spherecenter,float sphererad) { // sphere���� �浹 üũ

	D3DXVECTOR3 distancevec = pos - spherecenter;
	float distance = D3DXVec3Length(&distancevec);
	if(distance <= sphererad) {
		return true;
	}

	return false;
}

bool NewCCollisionDetection::CheckInTriangle(D3DXVECTOR3 pos,D3DXVECTOR3 a,D3DXVECTOR3 b,D3DXVECTOR3 c){ // triangle ���� �浹 üũ
	

	float length[3] = {0.0f,0.0f,0.0f};
	double radian = 0.0f;

	D3DXVECTOR3 pa = pos - a;
	D3DXVECTOR3 pb = pos - b;
	D3DXVECTOR3 pc = pos - c;
	
	D3DXVec3Normalize(&pa,&pa);
	D3DXVec3Normalize(&pb,&pb);
	D3DXVec3Normalize(&pc,&pc);
	
	length[0] = D3DXVec3Dot(&pa,&pb);
	length[1] = D3DXVec3Dot(&pb,&pc);
	length[2] = D3DXVec3Dot(&pc,&pa);
	
	radian = acos(length[0]);
	radian += acos(length[1]);
	radian += acos(length[2]);
	//radian = acos(length[0]);
	// 360 ���� �Ѵ°� �˻� 
	if(fabs(radian - (2*D3DX_PI)) <= 0.005)
		return true;

	return false;
}
float NewCCollisionDetection::RayCollisionPlane(D3DXVECTOR3 rcenter,D3DXVECTOR3 rfwd,D3DXVECTOR3 plane,D3DXVECTOR3 plane_normal) {
	
	//ax + by + cz = d
	//d = -(a dot x)
	float distance = -D3DXVec3Dot(&plane_normal,&plane);
	
	float numer = D3DXVec3Dot(&plane_normal,&rcenter) + distance;
	// �÷��� �븻���� ray ���������� ũ�� 
	float denom = D3DXVec3Dot(&plane_normal,&rfwd);

	if (denom == 0)  
		return (-1.0f);

	return (-(numer / denom));	
}

float NewCCollisionDetection::RayCollisionSphere(D3DXVECTOR3 rcenter,D3DXVECTOR3 rfwd,D3DXVECTOR3 spherecenter,float sphererad) {

	//ray �� sphere������ �Ÿ� �� return
	D3DXVECTOR3 distancevector = spherecenter - rcenter;
	float distance = D3DXVec3Length(&distancevector);
	float dotvector = D3DXVec3Dot(&distancevector,&rfwd);
	// rad ȹ��
	float rad = sphererad * sphererad - (distance * distance - dotvector * dotvector);

	if(rad <0.0f)
		return -1.0f;
    
    return (dotvector - sqrt(rad));

}
/****/

CCOLLISIONSIDE NewCCollisionDetection::ClassifyPoint(D3DXVECTOR3 point, D3DXVECTOR3 plane, D3DXVECTOR3 plane_normal) {
//point�� plane�� ���� return
	/*
	if(fabs(plane.x) < 0.01) {
		plane.x = 0.0f;
	}
	if(fabs(plane.y) < 0.01) {
		plane.y = 0.0f;
	}
	if(fabs(plane.z) < 0.01) {
		plane.z = 0.0f;
	}
	*/
	D3DXVECTOR3 dir = plane - point;
	//D3DXVec3Normalize(&dir,&dir);

	float d = D3DXVec3Dot(&dir, &plane_normal);
 
	if (d<-0.001f)
		return CCOLLISIONFRONTSIDE;	
	else if (d>0.001f)
		return CCOLLISIONBACKSIDE;	

	return CCOLLISIONONSIDE;	
}
D3DXVECTOR3 NewCCollisionDetection::SetVectorEllipsoidSpace(D3DXVECTOR3 vector,D3DXVECTOR3 spacerad) {

	D3DXVECTOR3 convert;
	convert.x = vector.x / spacerad.x;
	convert.y = vector.y / spacerad.y;
	convert.z = vector.z / spacerad.z;
	return convert;

}
bool NewCCollisionDetection::CheckPointInSphere(D3DXVECTOR3 point,D3DXVECTOR3 scenter,float rad) {
	
	// rad�� �̹� sphere space �� ��ȯ�� ���¿����� ���̹Ƿ� 1.0f �̴�.
	D3DXVECTOR3 tmp = point - scenter;
	float tmp_length = D3DXVec3Length(&tmp);
	
	if(tmp_length < rad)
		return true;
	
	return false;

}
// collision�� �Ͼ���� üũ �Լ� 
bool NewCCollisionDetection::CollisionCheck(CCollisionUnit *move_object,int faceindex) {

//bool CCollisionDetection::CollisionCheck(D3DXVECTOR3 pos,D3DXVECTOR3 rad,D3DXVECTOR3 velocity,
//						   triangle *polylist,int facenum,bool &col) {

	bool return_value = false;

	//plane
	D3DXVECTOR3 plane;
	D3DXVECTOR3 pnormal;
	D3DXVECTOR3 line1,line2;

	D3DXVECTOR3 UnitSphereCollisionPoint;
	D3DXVECTOR3 UnitPlaneCollisionPoint;
	D3DXVECTOR3 InTrianglePoint;

	D3DXVECTOR3 UnitPos = move_object->m_UnitPos;
	D3DXVECTOR3 UnitRad = move_object->m_UnitRad;
	D3DXVECTOR3 UnitVelocity = move_object->m_UnitVelocity;
	D3DXVECTOR3 UnitNormalVelocity = UnitVelocity;

	float vellength = D3DXVec3Length(&UnitVelocity);
	float distpolytosphere = 0.0f;
	D3DXVec3Normalize(&UnitNormalVelocity,&UnitNormalVelocity);
	//face collision check
	D3DXVECTOR3 apoint,bpoint,cpoint;
	CCOLLISIONSIDE CheckSide;
	float dist;
	// sphere space �� setting
	apoint = SetVectorEllipsoidSpace(faces[faceindex].a,UnitRad);
	bpoint = SetVectorEllipsoidSpace(faces[faceindex].b,UnitRad);
	cpoint = SetVectorEllipsoidSpace(faces[faceindex].c,UnitRad);

	plane = apoint;
	//pnormal = faces[faceindex].normal;
	
	line1 = bpoint - apoint;
	line2 = cpoint - bpoint;
/*
	line1 = bpoint - apoint;
	line2 = cpoint - apoint;
*/
  D3DXVec3Cross(&pnormal,&line1,&line2);

	D3DXVec3Normalize(&pnormal,&pnormal);
	faces[faceindex].normal = pnormal;	
	//pos + (-pnormal)
	
	UnitSphereCollisionPoint = move_object->m_UnitPos - pnormal;
	
	//����Ʈ�� �ո鿡 �ִ��� �޸鿡 �ִ��� �˻� 
	CheckSide = ClassifyPoint(UnitSphereCollisionPoint,plane,pnormal);
	
	if(CheckSide == CCOLLISIONBACKSIDE) {
		/*
		/// ������� �ȿ� �ִٰ� ������ �ÿ� ����� �� �ִ�.
		/// backside �� ���� �� ������ ���� ��ġ�� ������ ��ƾ
		move_object->m_bError = true;
		move_object->m_bCollision = true;
		return_value = true;			
		///
		*/
		dist = RayCollisionPlane(UnitSphereCollisionPoint,pnormal,plane,pnormal);
        UnitPlaneCollisionPoint.x = UnitSphereCollisionPoint.x + (dist * pnormal.x);
		UnitPlaneCollisionPoint.y = UnitSphereCollisionPoint.y + (dist * pnormal.y);
		UnitPlaneCollisionPoint.z = UnitSphereCollisionPoint.z + (dist * pnormal.z);
		m_bs++;
	}
	else {
		//plane�� �����浹���� 
		dist = RayCollisionPlane(UnitSphereCollisionPoint,UnitVelocity,plane,pnormal);
		UnitPlaneCollisionPoint.x = UnitSphereCollisionPoint.x + (dist * UnitNormalVelocity.x);
		UnitPlaneCollisionPoint.y = UnitSphereCollisionPoint.y + (dist * UnitNormalVelocity.y);
		UnitPlaneCollisionPoint.z = UnitSphereCollisionPoint.z + (dist * UnitNormalVelocity.z);
		m_fs++;
	}// triangle������ �˻�
	bool CTriangleValue = CheckInTriangle(UnitPlaneCollisionPoint,apoint,bpoint,cpoint);

	if(CTriangleValue == false) {

		InTrianglePoint = GetDistanceClosetPointOnTriangle(UnitPlaneCollisionPoint,apoint,bpoint,cpoint);

		distpolytosphere = RayCollisionSphere(InTrianglePoint,-UnitNormalVelocity,move_object->m_UnitPos,1.0f);
		//distpolytosphere = RayCollisionSphere(InTrianglePoint,-UnitNormalVelocity,move_object->m_UnitPos,move_object->m_UnitRad.y);
		if(distpolytosphere>0.0f) {
			UnitSphereCollisionPoint.x = InTrianglePoint.x + (distpolytosphere * (-UnitNormalVelocity.x));
			UnitSphereCollisionPoint.y = InTrianglePoint.y + (distpolytosphere * (-UnitNormalVelocity.y));				
			UnitSphereCollisionPoint.z = InTrianglePoint.z + (distpolytosphere * (-UnitNormalVelocity.z));
		}

	}
	else {

		InTrianglePoint = UnitPlaneCollisionPoint;
		distpolytosphere =  dist;
	}

	//if (CheckPointInSphere(InTrianglePoint,move_object->m_UnitPos,move_object->m_UnitRad.y)) { //point�� sphere �ȿ� ������
	if (CheckPointInSphere(InTrianglePoint,move_object->m_UnitPos,1.0f)) { //point�� sphere �ȿ� ������ 

		//move_object->m_UnitBeforePos = move_object->m_SphereCollisionPoint;
		move_object->m_bError = true;
	}
	if ((distpolytosphere > 0) && (distpolytosphere <= vellength)) { //�浹
     	//***
		// �浹�� �Ͼ�� �ʾҰų� �Ͼ �������� �� ���ʿ� ������
		if (((move_object->m_bCollision) ==false) || (distpolytosphere < (move_object->m_NearDistPolytoSphere)))  {
				move_object->m_NearDistPolytoSphere = distpolytosphere;
				move_object->m_SphereCollisionPoint = UnitSphereCollisionPoint;
      			move_object->m_TriangleCollisionPoint = InTrianglePoint;
				move_object->m_bCollision = true;
				move_object->m_Index = faceindex;
				move_object->m_PlaneOrigin = apoint;
				move_object->m_PlaneNormal = pnormal;
				col.a = faces[faceindex].a;
				col.b = faces[faceindex].b;
				col.c = faces[faceindex].c;
				col.normal = faces[faceindex].normal;

				return_value = true;		
		}

    } 
	return return_value;
}
void NewCCollisionDetection::SetMeshInfo(triangle *polygon,int facenum) {
	if(faces != NULL) {
		delete[] faces;
		faces = NULL;
	}
	faces = new triangle[facenum];
	memcpy(faces,polygon,sizeof(triangle) * facenum);
	m_FaceNum = facenum;
}

D3DXVECTOR3 NewCCollisionDetection::Check(D3DXVECTOR3 pos,D3DXVECTOR3 rad,D3DXVECTOR3 vel,D3DXVECTOR3 gravity){
	D3DXVECTOR3 unitpos,unitvel;
	D3DXVECTOR3 last_pos;
	// �ٴ� ���� �о�´�
/*	float ground_height = GetHeight(pos.x,pos.y,pos.z);	
	// �߷� ����
	//if(vel.x != 0.0f || vel.y != 0.0f || vel.z != 0.0f)
	if(ground_height >0.0f) {
		printf("");
	}
	if(pos.y > (ground_height + (rad.y + 20.0f)))	
		vel +=gravity;
*/	
	vector3 t_pos;
	vector3 t_vel;
	
	t_vel.x = vel.x;
	t_vel.y=  vel.y;
	t_vel.z = vel.z;

	t_pos.x = pos.x;
	t_pos.y = pos.y;
	t_pos.z = pos.z;

	matrix matInHousePos;
	matInHousePos.Translation(t_pos);
	matInHousePos=matInHousePos*m_LocalInvPosMatrix;
	t_pos=matInHousePos.GetLoc();

	matrix matInHouseVel;
	matInHouseVel.Translation(t_vel);
	matInHouseVel=matInHouseVel*m_LocalInvRotMatrix;
	t_vel=matInHouseVel.GetLoc();

	unitpos.x = t_pos.x;
	unitpos.y = t_pos.y;
	unitpos.z = t_pos.z;

	unitvel.x = t_vel.x;
	unitvel.y = t_vel.y;
	unitvel.z = t_vel.z;

/*	// �� sphere ��ǥ��� ��ȯ
	unitpos.x = pos.x / rad.x;
	unitpos.y = pos.y / rad.y;
	unitpos.z = pos.z / rad.z;

	unitvel.x = vel.x / rad.x;
	unitvel.y = vel.y / rad.y;
	unitvel.z = vel.z / rad.z;
*/	

 //	last_pos = Collision(pos,vel);
	last_pos = Collision(unitpos,unitvel);
	
	
	last_pos.x = last_pos.x * rad.x;
	last_pos.y = last_pos.y * rad.y;
	last_pos.z = last_pos.z * rad.z;

	return last_pos;

}

D3DXVECTOR3 NewCCollisionDetection::Collision(D3DXVECTOR3 pos,D3DXVECTOR3 velocity) {

	D3DXVECTOR3 tmppos;
 	
	D3DXVECTOR3 nextpos = pos + velocity;

	float vellength = D3DXVec3Length(&velocity);
	if(vellength < EPS)
		return pos;
	
	object->m_UnitVelocity = velocity;
	object->m_UnitPos = pos;
	object->m_NearDistPolytoSphere = -1;
	object->m_bCollision = false;
	object->m_bError = false;
	
		 
	for(int i=0;i<m_FaceNum;i++) {
		CollisionCheck(object,i);
	}
	
	m_fs = m_bs  =0;

	if(object->m_bCollision) { // collision �߻� 
		
		char buf[256] = {0};
		
		
		sprintf(buf,"�浹 �̶�ϱ�.. index : %d",object->m_Index);
		MessageBox(NULL,buf,"test",MB_OK);
		
		
//		if (object->m_bError) { // float error ���� ���̸� ���� ��ġ�� �ǵ�����
							  // ���� sphere ������ ���� �̵��Ÿ��� ������ ��������� ���� �߻��� ���ִ�.
		/*	D3DXVECTOR3 tmp_comp = (object->m_UnitBeforePos) - pos;
			
			if(D3DXVec3Length(&tmp_comp) <1.0f) { //�� ����
				//1.0 ���� ���߾� �ش�
				SetVectorLength(tmp_comp,1.0f);
				return (pos + tmp_comp);
			}					
			else*/ 
			return object->m_UnitBeforePos; 
//		}
		D3DXVECTOR3 newpos;
	
		if (object->m_NearDistPolytoSphere >= EPS) {
     
			D3DXVECTOR3 tmp_v = velocity;
			SetVectorLength(tmp_v,object->m_NearDistPolytoSphere-EPS);
			newpos = object->m_UnitPos + tmp_v;
		}
		else
			newpos = object->m_UnitPos;

		
		// slide plane 
 
		D3DXVECTOR3 SlidePlane = object->m_TriangleCollisionPoint;
		D3DXVECTOR3 SlideNormal = newpos - (object->m_TriangleCollisionPoint);

		D3DXVec3Normalize(&SlideNormal,&SlideNormal);

		//SlideNormal *=(-1.0f);
//////////////

		float slidelength = RayCollisionPlane(nextpos,SlideNormal,SlidePlane,SlideNormal);
		
		D3DXVECTOR3 newnextPoint;
		
		//���ο� pos

		newnextPoint.x = nextpos.x + slidelength * SlideNormal.x;
		newnextPoint.y = nextpos.y + slidelength * SlideNormal.y;
		newnextPoint.z = nextpos.z + slidelength * SlideNormal.z;
	
		D3DXVECTOR3 newVelocityVector = newnextPoint - object->m_TriangleCollisionPoint;
		
	/*	float newVelocityLength = D3DXVec3Length(&newVelocityVector);
		if(newVelocityLength < 0.1f) {
			
			D3DXMATRIX trans_matrix;
			D3DXMATRIX pos_matrix;
			D3DXMatrixIdentity(&trans_matrix);
			D3DXMatrixIdentity(&pos_matrix);
			
			pos_matrix._41 = newVelocityVector.x;
			pos_matrix._42 = newVelocityVector.y;
			pos_matrix._43 = newVelocityVector.z;

			D3DXMatrixRotationYawPitchRoll(&trans_matrix,D3DXToRadian(45.0f),D3DXToRadian(45.0f),D3DXToRadian(45.0f));
			D3DXMatrixMultiply(&pos_matrix,&pos_matrix,&trans_matrix);
			
			newVelocityVector.x = pos_matrix._41;
			newVelocityVector.y = pos_matrix._42;
			newVelocityVector.z = pos_matrix._43;


		}*/
		//D3DXVECTOR3 newVelocityVector = SlidePolygonVector(object->m_UnitVelocity,SlideNormal);
		/*
		
		D3DXVECTOR3 SlidePlane = object->m_TriangleCollisionPoint;
		D3DXVECTOR3 SlideNormal = newpos - (object->m_TriangleCollisionPoint);
		//SlideNormal *=(-1.0f);
		
		//**
		float slidelength = RayCollisionPlane(nextpos,SlideNormal,SlidePlane,SlideNormal);
		D3DXVECTOR3 newnextPoint;
		//���ο� pos
		newnextPoint.x = nextpos.x + slidelength * SlideNormal.x;
		newnextPoint.y = nextpos.y + slidelength * SlideNormal.y;
		newnextPoint.z = nextpos.z + slidelength * SlideNormal.z;

		D3DXVECTOR3 newVelocityVector = newnextPoint - object->m_TriangleCollisionPoint;
				
		//D3DXVECTOR3 newVelocityVector = SlidePolygonVector(object->m_UnitVelocity,SlideNormal);
		*/
		object->m_UnitBeforePos = pos;
	
		return Collision(newpos,newVelocityVector); 

	}
	else { // no collision
		float vec_length = D3DXVec3Length(&velocity);
		D3DXVECTOR3 vec = velocity;
   
		SetVectorLength(vec,vec_length - EPS);
		object->m_UnitBeforePos = pos;

		return (pos + vec);

	}
 
}
/*****/
void NewCCollisionDetection::SetVectorLength(D3DXVECTOR3 &vec,float veclength) {
	

	float len = sqrt(vec.x*vec.x + vec.y*vec.y + vec.z*vec.z);	

/*	vec.x *= (veclength - EPS)/len;
	vec.y *= (veclength - EPS)/len;
	vec.z *= (veclength - EPS)/len;
*/	
	vec.x *= (veclength)/len;
	vec.y *= (veclength)/len;
	vec.z *= (veclength)/len; 
}
float NewCCollisionDetection::GetHeight(float x,float y,float z) {
	int i;
	for(i=0;i<m_FaceNum;i++) {
		if((faces[i].a.y == faces[i].b.y) && (faces[i].b.y== faces[i].c.y) && (faces[i].a.y == faces[i].c.y)) { // polygon �� �ٴ��� ��� 
			D3DXVECTOR3 check_pos;
			check_pos.x = x;
			check_pos.z = z;
			check_pos.y = faces[i].a.y;

			if(CheckInTriangle(check_pos,faces[i].a,faces[i].b,faces[i].c)) { //����� �ϴ� ��ġ
				if((faces[i].a.y) < y) // �� face�� �� ��ġ ���� �Ʒ��� ��ġ �ҽÿ�
					return (faces[i].a.y);
			}
		}
	}
	return 0.0f;
}
void NewCCollisionDetection::SetLocalVector(float x,float y,float z,float vx,float vy,float vz,float vellength,matrix inv_pos,matrix inv_rot) {
	m_LocalPos.x = x;
	m_LocalPos.y = y;
	m_LocalPos.z = z;

	m_LocalVelocityNormalize.x = vx;
	m_LocalVelocityNormalize.y = vy;
	m_LocalVelocityNormalize.z = vz;

	m_LocalVelocityLength = vellength;
	m_LocalInvPosMatrix = inv_pos;
	m_LocalInvRotMatrix = inv_rot;
}