#include <stdio.h>
#include <math.h>

/* 位置座標 */
typedef struct Position
{
	double x,y,z;
} Position;

/* 三角形 */
typedef struct Triangle
{
	Position p1,p2,p3;
} Triangle;

/* 座標の足し算 */
void add_position(Position* ret, const Position* left, const Position* right){
	ret->x = left->x + right->x;
	ret->y = left->y + right->y;
	ret->z = left->z + right->z;
}

/* 座標の引き算 */
void sub_position(Position* ret, const Position* left, const Position* right){
	ret->x = left->x - right->x;
	ret->y = left->y - right->y;
	ret->z = left->z - right->z;
}

/* 外積の計算 */
void cross_position(Position* ret, const Position* left, const Position* right){
	ret->x = left->y * right->z - left->z * right->y;
	ret->y = left->z * right->x - left->x * right->z;
	ret->z = left->x * right->y - left->y * right->x;
}

/* 3x3行列の逆行列の計算 */
int inv_mat33(double ret[][3], const double mat[][3]){
	double det
		= mat[0][0] * mat[1][1] * mat[2][2]
		+ mat[0][1] * mat[1][2] * mat[2][0]
		+ mat[0][2] * mat[1][0] * mat[2][1]
		- mat[0][0] * mat[1][2] * mat[2][1]
		- mat[0][1] * mat[1][0] * mat[2][2]
		- mat[0][2] * mat[1][1] * mat[2][0]
	;
	if( det == 0.0 ) return -1;
	{
		double inv_det = 1.0 / det;
		ret[0][0] = inv_det * (mat[1][1]*mat[2][2] - mat[1][2]*mat[2][1]);
		ret[0][1] = inv_det * (mat[0][2]*mat[2][1] - mat[0][1]*mat[2][2]);
		ret[0][2] = inv_det * (mat[0][1]*mat[1][2] - mat[0][2]*mat[1][1]);
		ret[1][0] = inv_det * (mat[1][2]*mat[2][0] - mat[1][0]*mat[2][2]);
		ret[1][1] = inv_det * (mat[0][0]*mat[2][2] - mat[0][2]*mat[2][0]);
		ret[1][2] = inv_det * (mat[0][2]*mat[1][0] - mat[0][0]*mat[1][2]);
		ret[2][0] = inv_det * (mat[1][0]*mat[2][1] - mat[1][1]*mat[2][0]);
		ret[2][1] = inv_det * (mat[0][1]*mat[2][0] - mat[0][0]*mat[2][1]);
		ret[2][2] = inv_det * (mat[0][0]*mat[1][1] - mat[0][1]*mat[1][0]);
	}
	return 1;
}

/* 行列を掛ける */
void mul_mat(Position* ret, const double mat[][3], Position* p){
	ret->x = mat[0][0] * p->x + mat[0][1] * p->y + mat[0][2] * p->z;
	ret->y = mat[1][0] * p->x + mat[1][1] * p->y + mat[1][2] * p->z;
	ret->z = mat[2][0] * p->x + mat[2][1] * p->y + mat[2][2] * p->z;
}

/* 行列の列を設定 */
void set_col(double mat[][3], const Position* p, int col){
	mat[0][col] = p->x;
	mat[1][col] = p->y;
	mat[2][col] = p->z;
}

/* 点間の距離 */
double get_distance(const Position* a, const Position* b){
	Position sub;
	sub_position(&sub, a, b);
	return sqrt(sub.x*sub.x + sub.y*sub.y + sub.z*sub.z);
}


/* 点pに一番近い三角形t上の点を求める */
Position GetNearestPosition(Triangle *t, Position* p){
	Position x0, x1, x2, ret;
	double mat[3][3], inv[3][3];
	/* mat = (p2-p1, p3-p1, (p2-p1)×(p3-p1) */
	sub_position(&x0, &t->p2, &t->p1);
	sub_position(&x1, &t->p3, &t->p1);
	cross_position(&x2, &x0, &x1);
	set_col(mat, &x0, 0);
	set_col(mat, &x1, 1);
	set_col(mat, &x2, 2);
	if( inv_mat33(inv, mat) < 0){
		/* 三角形がつぶれていた場合 */
		/* TODO:直線に一番近い点を計算 */
	}else{
		/* pを三角形のローカル座標に変換 */
		/* ローカル座標では
		  p1 = (0,0)'
		  p2 = (1,0)'
		  p3 = (0,1)'
		  になる。
		 */
		Position q;
		mul_mat(&q, inv, p);
/*
                ／
     ②       ／
            ／
        p3／
 --------+
         | ＼      ⑦
         |   ＼            ／
   ⑤    |     ＼        ／
         | ①    ＼    ／
       p1|         ＼／
 --------+-----------+ p2
         |           |   ④
   ③    |    ⑥     |
         |           |
*/
		/* ① */
		if(q.x >= 0 && q.y >= 0 && q.z >= 0 && q.x+q.y+q.z <= 1){
			q.z = 0.0; /* qは垂線の足 */
			mul_mat(&ret, mat, &q); /* 元の座標系に戻す */
		}
		/* ② */
		else if(q.y >= 1 && q.y-q.x >= 1){
			ret = t->p2;
		}
		/* ③ */
		else if(q.x <= 0 && q.y <= 0){
			ret = t->p1;
		}
		/* ④ */
		else if(q.x >= 1 && q.y-q.x <= -1){
			ret = t->p3;
		}
		/* ⑤ */
		else if(q.x <= 0){ /* 先に②と③を判定してるのでこれで十分 */
			q.z = 0.0; q.x = 0.0; /* qはp1とp3の間 */
			mul_mat(&ret, mat, &q);
		}
		/* ⑥ */
		else if(q.y <= 0){ /* 先に③と④を判定しているのでこれで十分 */
			q.z = 0.0; q.y = 0.0; /* qはp1とp2の間 */
			mul_mat(&ret, mat, &q);
		}
		/* ⑦ */
		else{
			/* qのp1-p3への正射影rを求める */
			/* s = (q-p3)・(1,-1)' */
			/* r = p3 + s (1,-1)' */
			double s = q.x - (q.y-1);
			Position r;
			r.x = s;
			r.y = 1 - s;
			r.z = 0;
			mul_mat(&ret, mat, &r);
		}
	}
	return ret;
}