#include "OBB.h"

set<fkut_OBB *>	fkut_OBB::instanceList;

fkut_OBB::fkut_OBB(void)
{
	ref_model = NULL;
	elemD = 1.0;
	sphRad = 0.0;

	// 全インスタンスに自分のポインタをキーとして最近分離軸IDを保持する要素を追加
	for(set<fkut_OBB *>::iterator it = instanceList.begin();
		it != instanceList.end(); it++) {

		(*it)->nearestArray.insert(pair<fkut_OBB *, int>(this, -1));
	}
	// インスタンスリストに追加
	instanceList.insert(this);
}

fkut_OBB::~fkut_OBB(void)
{
	instanceList.erase(this);
	for(set<fkut_OBB *>::iterator it = instanceList.begin();
		it != instanceList.end(); it++) {

		(*it)->nearestArray.erase(this);
	}
}

void fkut_OBB::setDivDist(double argD)
{
	elemD = argD;
}

// 指定軸番号の方向ベクトルを取得
fk_Vector fkut_OBB::GetDirect(int axis)
{
	return m_NormaDirect[axis];
}

// 指定軸方向の長さを取得
double fkut_OBB::GetLen_W(int axis)
{
	return m_fLength[axis];
}

// 位置を取得
fk_Vector fkut_OBB::GetPos_W(void)
{
	return m_Pos;
}

// Blockサイズをセット
void fkut_OBB::setBlockSize(double w, double h, double d)
{
	m_fLength[fk_X] = w*0.5;
	m_fLength[fk_Y] = h*0.5;
	m_fLength[fk_Z] = d*0.5;

	sphRad = sqrt(m_fLength[fk_X]*m_fLength[fk_X]
				+ m_fLength[fk_Y]*m_fLength[fk_Y]
				+ m_fLength[fk_Z]*m_fLength[fk_Z]);

	return;
}

// 参照するfk_Modelをセット
void fkut_OBB::setModel(fk_Model *argModel)
{
	ref_model = argModel;
}

bool fkut_OBB::Update(void)
{
	if(ref_model == NULL) return false;

	m_Pos = ref_model->getInhPosition();
	m_NormaDirect[fk_Z] = ref_model->getInhVec();
	m_NormaDirect[fk_Y] = ref_model->getInhUpvec();
	m_NormaDirect[fk_X] = m_NormaDirect[fk_Y] ^ m_NormaDirect[fk_Z];

	return true;
}

// OBB vs OBB
bool fkut_OBB::checkVSOBB(fkut_OBB &other, fk_Vector &backV)
{
	if(!this->Update() & !other.Update()) return false;

	static fk_Vector	tmpDiff;
	tmpDiff = other.m_Pos - m_Pos;
	double tmpDist = this->sphRad + other.sphRad + elemD + other.elemD;
	if(tmpDist*tmpDist < tmpDiff.dist2()) {
		 return false;
	}

	// 各方向ベクトルの確保
	// （N***:標準化方向ベクトル）
	static fk_Vector NAe1, Ae1, NAe2, Ae2, NAe3, Ae3;
	static fk_Vector NBe1, Be1, NBe2, Be2, NBe3, Be3;
	static fk_Vector Interval;
	NAe1 = this->GetDirect(0); Ae1 = NAe1 * this->GetLen_W(0);
	NAe2 = this->GetDirect(1); Ae2 = NAe2 * this->GetLen_W(1);
	NAe3 = this->GetDirect(2); Ae3 = NAe3 * this->GetLen_W(2);
	NBe1 = other.GetDirect(0); Be1 = NBe1 * other.GetLen_W(0);
	NBe2 = other.GetDirect(1); Be2 = NBe2 * other.GetLen_W(1);
	NBe3 = other.GetDirect(2); Be3 = NBe3 * other.GetLen_W(2);
	Interval = this->GetPos_W() - other.GetPos_W();

	// 分離軸 : Ae1
	double rA = Ae1.dist();
	double rB = LenSegOnSeparateAxis(&NAe1, &Be1, &Be2, &Be3);
	double L = fabs(Interval*NAe1);
	if(L > rA + rB) {
		nearestArray[&other] = 0;
		backVec[0].init();
		return false; // 衝突していない
	} else {
		backVec[0] = NAe1*(rA + rB - L + FK_EPS);
		if((Interval*NAe1) < 0.0) backVec[0] *= -1.0;
	}

	// 分離軸 : Ae2
	rA = Ae2.dist();
	rB = LenSegOnSeparateAxis(&NAe2, &Be1, &Be2, &Be3);
	L = fabs(Interval*NAe2);
	if(L > rA + rB) {
		nearestArray[&other] = 1;
		backVec[1].init();
		return false; // 衝突していない
	} else {
		backVec[1] = NAe2*(rA + rB - L + FK_EPS);
		if((Interval*NAe2) < 0.0) backVec[1] *= -1.0;
	}

	// 分離軸 : Ae3
	rA = Ae3.dist();
	rB = LenSegOnSeparateAxis(&NAe3, &Be1, &Be2, &Be3);
	L = fabs(Interval*NAe3);
	if(L > rA + rB) {
		nearestArray[&other] = 2;
		backVec[2].init();
		return false; // 衝突していない
	} else {
		backVec[2] = NAe3*(rA + rB - L + FK_EPS);
		if((Interval*NAe3) < 0.0) backVec[2] *= -1.0;
	}

	// 分離軸 : Be1
	rA = LenSegOnSeparateAxis(&NBe1, &Ae1, &Ae2, &Ae3);
	rB = Be1.dist();
	L = fabs(Interval*NBe1);
	if(L > rA + rB) {
		nearestArray[&other] = 3;
		backVec[3].init();
		return false; // 衝突していない
	} else {
		backVec[3] = NBe1*(rA + rB - L + FK_EPS);
		if((Interval*NBe1) < 0.0) backVec[3] *= -1.0;
	}

	// 分離軸 : Be2
	rA = LenSegOnSeparateAxis(&NBe2, &Ae1, &Ae2, &Ae3);
	rB = Be2.dist();
	L = fabs(Interval*NBe2);
	if(L > rA + rB) {
		nearestArray[&other] = 4;
		backVec[4].init();
		return false; // 衝突していない
	} else {
		backVec[4] = NBe2*(rA + rB - L + FK_EPS);
		if((Interval*NBe2) < 0.0) backVec[4] *= -1.0;
	}

	// 分離軸 : Be3
	rA = LenSegOnSeparateAxis(&NBe3, &Ae1, &Ae2, &Ae3);
	rB = Be3.dist();
	L = fabs(Interval*NBe3);
	if(L > rA + rB) {
		nearestArray[&other] = 5;
		backVec[5].init();
		return false; // 衝突していない
	} else {
		backVec[5] = NBe3*(rA + rB - L + FK_EPS);
		if((Interval*NBe3) < 0.0) backVec[5] *= -1.0;
	}

	// 分離軸 : C11
	fk_Vector Cross;
	Cross = NAe1^NBe1;
	rA = LenSegOnSeparateAxis(&Cross, &Ae2, &Ae3);
	rB = LenSegOnSeparateAxis(&Cross, &Be2, &Be3);
	L = fabs(Interval*Cross);
	if(L > rA + rB) {
		nearestArray[&other] = 6;
		backVec[6].init();
		return false; // 衝突していない
	} else {
		Cross.normalize();
		backVec[6] = Cross*(rA + rB - L + FK_EPS);
		if((Interval*Cross) < 0.0) backVec[6] *= -1.0;
	}

	// 分離軸 : C12
	Cross = NAe1^NBe2;
	rA = LenSegOnSeparateAxis(&Cross, &Ae2, &Ae3);
	rB = LenSegOnSeparateAxis(&Cross, &Be1, &Be3);
	L = fabs(Interval*Cross);
	if(L > rA + rB) {
		nearestArray[&other] = 7;
		backVec[7].init();
		return false; // 衝突していない
	} else {
		Cross.normalize();
		backVec[7] = Cross*(rA + rB - L + FK_EPS);
		if((Interval*Cross) < 0.0) backVec[7] *= -1.0;
	}

	// 分離軸 : C13
	Cross = NAe1^NBe3;
	rA = LenSegOnSeparateAxis(&Cross, &Ae2, &Ae3);
	rB = LenSegOnSeparateAxis(&Cross, &Be1, &Be2);
	L = fabs(Interval*Cross);
	if(L > rA + rB) {
		nearestArray[&other] = 8;
		backVec[8].init();
		return false; // 衝突していない
	} else {
		Cross.normalize();
		backVec[8] = Cross*(rA + rB - L + FK_EPS);
		if((Interval*Cross) < 0.0) backVec[8] *= -1.0;
	}

	// 分離軸 : C21
	Cross = NAe2^NBe1;
	rA = LenSegOnSeparateAxis(&Cross, &Ae1, &Ae3);
	rB = LenSegOnSeparateAxis(&Cross, &Be2, &Be3);
	L = fabs(Interval*Cross);
	if(L > rA + rB) {
		nearestArray[&other] = 9;
		backVec[9].init();
		return false; // 衝突していない
	} else {
		Cross.normalize();
		backVec[9] = Cross*(rA + rB - L + FK_EPS);
		if((Interval*Cross) < 0.0) backVec[9] *= -1.0;
	}

	// 分離軸 : C22
	Cross = NAe2^NBe2;
	rA = LenSegOnSeparateAxis(&Cross, &Ae1, &Ae3);
	rB = LenSegOnSeparateAxis(&Cross, &Be1, &Be3);
	L = fabs(Interval*Cross);
	if(L > rA + rB) {
		nearestArray[&other] = 10;
		backVec[10].init();
		return false; // 衝突していない
	} else {
		Cross.normalize();
		backVec[10] = Cross*(rA + rB - L + FK_EPS);
		if((Interval*Cross) < 0.0) backVec[10] *= -1.0;
	}

	// 分離軸 : C23
	Cross = NAe2^NBe3;
	rA = LenSegOnSeparateAxis(&Cross, &Ae1, &Ae3);
	rB = LenSegOnSeparateAxis(&Cross, &Be1, &Be2);
	L = fabs(Interval*Cross);
	if(L > rA + rB) {
		nearestArray[&other] = 11;
		backVec[11].init();
		return false; // 衝突していない
	} else {
		Cross.normalize();
		backVec[11] = Cross*(rA + rB - L + FK_EPS);
		if((Interval*Cross) < 0.0) backVec[11] *= -1.0;
	}

	// 分離軸 : C31
	Cross = NAe3^NBe1;
	rA = LenSegOnSeparateAxis(&Cross, &Ae1, &Ae2);
	rB = LenSegOnSeparateAxis(&Cross, &Be2, &Be3);
	L = fabs(Interval*Cross);
	if(L > rA + rB) {
		nearestArray[&other] = 12;
		backVec[12].init();
		return false; // 衝突していない
	} else {
		Cross.normalize();
		backVec[12] = Cross*(rA + rB - L + FK_EPS);
		if((Interval*Cross) < 0.0) backVec[12] *= -1.0;
	}

	// 分離軸 : C32
	Cross = NAe3^NBe2;
	rA = LenSegOnSeparateAxis(&Cross, &Ae1, &Ae2);
	rB = LenSegOnSeparateAxis(&Cross, &Be1, &Be3);
	L = fabs(Interval*Cross);
	if(L > rA + rB) {
		nearestArray[&other] = 13;
		backVec[13].init();
		return false; // 衝突していない
	} else {
		Cross.normalize();
		backVec[13] = Cross*(rA + rB - L + FK_EPS);
		if((Interval*Cross) < 0.0) backVec[13] *= -1.0;
	}

	// 分離軸 : C33
	Cross = NAe3^NBe3;
	rA = LenSegOnSeparateAxis(&Cross, &Ae1, &Ae2);
	rB = LenSegOnSeparateAxis(&Cross, &Be1, &Be2);
	L = fabs(Interval*Cross);
	if(L > rA + rB) {
		nearestArray[&other] = 14;
		backVec[14].init();
		return false; // 衝突していない
	} else {
		Cross.normalize();
		backVec[14] = Cross*(rA + rB - L + FK_EPS);
		if((Interval*Cross) < 0.0) backVec[14] *= -1.0;
	}

	// 分離平面が存在しないので「衝突している」
	if(nearestArray[&other] == -1) {
		backV.init();
		fk_Printf("Collision detected, but can't calculate a back vector!");
	} else {
		backV = backVec[nearestArray[&other]];
	}
	return true;
}

bool fkut_OBB::checkVSMovingOBB(fkut_OBB &other, fk_Vector &spd, fk_Vector &backV)
{
	bool	result;
	if(!this->Update()) return false;

	double	len = spd.dist();
	if(len < FK_EPS) return checkVSOBB(other, backV);

	if(len < elemD) {
		other.ref_model->glTranslate(spd);
		result = checkVSOBB(other, backV);
		other.ref_model->glTranslate(-spd);
		return result;
	}

	fk_Model	*other_modelP = other.ref_model;
	static fk_Vector	elemV, tmpVec;
	elemV = spd/len;
	tmpVec.init();
	while(true) {
		len -= elemD;
		tmpVec += elemV*elemD;
		other_modelP->glTranslate(tmpVec);
		result = checkVSOBB(other, backV);
		other_modelP->glTranslate(-tmpVec);
		if(result == true) {
			backV += len*elemV;
			return true;
		} else if(len < elemD) {
			tmpVec += len*elemV;
			other_modelP->glTranslate(tmpVec);
			result = checkVSOBB(other, backV);
			other_modelP->glTranslate(-tmpVec);
			return result;
		}
	}
}

// 分離軸に投影された軸成分から投影線分長を算出
double fkut_OBB::LenSegOnSeparateAxis(fk_Vector *Sep, fk_Vector *e1, fk_Vector *e2, fk_Vector *e3)
{
	// 3つの内積の絶対値の和で投影線分長を計算
	// 分離軸Sepは標準化されていること
	double r1 = fabs((*Sep)*(*e1));
	double r2 = fabs((*Sep)*(*e2));
	double r3 = e3 ? fabs((*Sep)*(*e3)) : 0;
	return r1 + r2 + r3;
}

// 点 vs OBB
fk_Vector fkut_OBB::checkVSPoint(fk_Vector p)
{
	fk_Vector	vec;   // 最終的に長さを求めるベクトル

	if(!this->Update()) return vec;

	// 各軸についてはみ出た部分のベクトルを算出
	for(int i = 0; i < 3; i++) {
		double L = this->GetLen_W(i);
		if(L < FK_EPS) continue;  // L=0は計算できない
		double s = ((p - this->GetPos_W()) * this->GetDirect(i)) / L;

		// sの値から、はみ出した部分があればそのベクトルを加算
		if(fabs(s) > 1.0) vec += (fabs(s)/s)*(1.0 - fabs(s))*(L+FK_EPS)*this->GetDirect(i);   // はみ出した部分のベクトル算出
	}

	return vec;   // 長さを出力
}

// 球 vs OBB
bool fkut_OBB::checkVSSphere(const fk_Vector &pos, double rad, fk_Vector &backV)
{
	backV = checkVSPoint(pos);
	double refD = backV.dist();

	if(refD < rad) {
		backV.normalize();
		backV *= (rad-refD+FK_EPS);
		return true;
	}

	backV.init();
	return false;
}

bool fkut_OBB::checkVSMovingSphere(const fk_Vector &pos, double rad, const fk_Vector &spd, fk_Vector &backV)
{
	if(!this->Update()) return false;

	double	len = spd.dist();
	if(len < FK_EPS) return checkVSSphere(pos, rad, backV);

	if(len < elemD) return checkVSSphere(pos+spd, rad, backV);

	bool				result;
	static fk_Vector	elemV, tmpPos;

	elemV = spd/len;
	tmpPos = pos;

	while(true) {
		len -= elemD;
		tmpPos += elemV*elemD;
		result = checkVSSphere(tmpPos, rad, backV);
		if(result == true) {
			backV += len*elemV;
			return true;
		} else if(len < elemD) {
			return checkVSSphere(tmpPos + len*elemV, rad, backV);
		}
	}
}

// n を min 以上 max 以下にまるめる
double Clamp(double n, double min, double max)
{
	if(n < min) return min;
	if(n > max) return max;
	return n;
}

// 線分 P1Q1 と P2Q2 における最近接点 C1 および C2 を求める
// P1Q1 上の C1 の位置を示す s と、P2Q2 上の C2 の位置を示す t も返す
// 返り値は C1 と C2 の距離の平方となる
double ClosestPtSegmentSegment(fk_Vector p1, fk_Vector q1, fk_Vector p2, fk_Vector q2,
							   double &s, double &t, fk_Vector &c1, fk_Vector &c2)
{
	fk_Vector d1 = q1 - p1;
	fk_Vector d2 = q2 - p2;
	fk_Vector r = p1 - p2;
	double a = d1*d1;
	double e = d2*d2;
	double f = d2*r;
	if(a < FK_EPS && e < FK_EPS) {
		s = t = 0.0;
		c1 = p1;
		c2 = p2;
	}
	if(a < FK_EPS) {
		s = 0.0;
		t = f/e;
		t = Clamp(t, 0.0, 1.0);
	} else {
		double c = d1*r;
		if(e < FK_EPS) {
			t = 0.0;
			s = Clamp(-c/a, 0.0, 1.0);
		} else {
			double b = d1*d2;
			double denom = a*e - b*b;
			if(denom != 0.0) {
				s = Clamp((b*f - c*e)/denom, 0.0, 1.0);
			} else {
				s = 0.0;
			}
			t = (b*s + f)/e;
			if(t < 0.0) {
				t = 0.0;
				s = Clamp(-c/a, 0.0, 1.0);
			} else if(t > 1.0) {
				t = 1.0;
				s = Clamp((b - c)/a, 0.0, 1.0);
			}
		}
	}
	c1 = p1 + d1*s;
	c2 = p2 + d2*t;
	return (c1 - c2)*(c1 - c2);
}

bool fkut_Capsule::Update(void)
{
	if(ref_model == NULL) return false;

	p = ref_model->getInhPosition();
	q = p + ref_model->getInhVec()*len;

	return true;
}

void fkut_Capsule::setCapsuleSize(double argRad, double argLen)
{
	r = argRad;
	len = argLen;
}

void fkut_Capsule::setModel(fk_Model *argModel)
{
	ref_model = argModel;
}

bool fkut_Capsule::checkVSCapsule(fkut_Capsule &other, fk_Vector *revVec)
{
	static fk_Vector	c1, c2;
	static double		s, t, retVal, tmpDist;

	if(!Update() || !other.Update()) return false;

	retVal = ClosestPtSegmentSegment(p, q, other.p, other.q, s, t, c1, c2);

	if(revVec != NULL) {
		*revVec = c2-c1;
		tmpDist = (*revVec).dist() - (r + other.r);
		(*revVec).normalize();
		*revVec *= tmpDist;
	}

	return bool(retVal < pow(r + other.r, 2.0));
}
