/*
 * $Id$
 */
#include <stdio.h>
#include"libps2.h"
#include"ps2util.h"
#include"col.h"
#include"mytype.h"

extern FILE *main_fp;

/**
 * v0, v1が描く三角形と点pの内外判定(approx detection)
 * 点pはv0, v1が描く面のどこかに位置している。
 * pが面の三角形の外側にあるときTRUEを戻し、
 * そうでなければFALSEを戻す。境界は含む。
 */
static Bool
col_detect_approx(FVECTOR p, FVECTOR v0, FVECTOR v1)
{
    float v_r1, v_r2, r1_r2, r1_r1, r2_r2;
    float a, b, r;

    v_r1  = ps2_vu0_inner_product( p,  v0 );
    v_r2  = ps2_vu0_inner_product( p,  v1 );
    r1_r2 = ps2_vu0_inner_product( v0, v1 );
    r1_r1 = ps2_vu0_inner_product( v0, v0 );
    r2_r2 = ps2_vu0_inner_product( v1, v1 );

    a = (v_r1  * r2_r2 - v_r2  * r1_r2);
    b = (v_r2  * r1_r1 - v_r1  * r1_r2);
    r = (r1_r1 * r2_r2 - r1_r2 * r1_r2);

    return (a>=0 && b>=0 && a+b-r<=0) ? TRUE : FALSE;
}

/**
 * 垂心oの算出. 点pからv0, v1が描く面へ垂線を引いたときの交点oを
 * 垂心(orthocenter)と呼ぶ。
 */
static void
col_orthocenter(FVECTOR o, FVECTOR p, FVECTOR p0, FVECTOR normal)
{
    FVECTOR o0, d;

    ps2_vu0_sub_vector(o0, p, p0);
    o0[W_AXIS] = 0;
    ps2_vu0_scale_vector(
	d, normal, ps2_vu0_inner_product(o0, normal));

    o[X_AXIS] = p[X_AXIS] - d[X_AXIS];
    o[Y_AXIS] = p[Y_AXIS] - d[Y_AXIS];
    o[Z_AXIS] = p[Z_AXIS] - d[Z_AXIS];
    o[W_AXIS] = 1;
}

/**
 * 点pと面との衝突(点pとdetection face面との垂心oがdetection face領域の
 * 中にあるかを)判定衝突時(oがdetection face領域の外)はFALSEを戻し、そう
 * でなければTRUE、 垂心(orthocenter)、垂心のある面の法線(normal
 * vector)を戻す.
 */
Bool
col_detect(ColFaceHd* hd, FVECTOR pose, FVECTOR p)
{
    FVECTOR o, o0;
    int i;
    ColFacePtr col_face=hd->colface;
    for (i=0; i<hd->facenum; i++) {
	/*
	 * 垂心oを求める
	 */
	col_orthocenter(o, p, col_face[i].p0, col_face[i].normal);
	/*
	 * detection face上のある点p0から垂心oへのベクトルo0を求める
	 */
	ps2_vu0_sub_vector(o0, o, col_face[i].p0);
	/*
	 * ベクトルv0, v1の描く三角形とo0とで内外判定(range approx detection)を行う
	 */
	if (col_detect_approx(o0, col_face[i].v0, col_face[i].v1)==TRUE) {
	    goto NOCOLLISION;
	}
    }
    return FALSE;
NOCOLLISION:
    ps2_vu0_copy_vector(p, o);
    ps2_vu0_copy_vector(pose, col_face[i].normal);
    return TRUE;
}

/*
 * detection face作成
 */
void 
col_init(ColFaceHd* hd, PolygonInfo* face)
{
    int i;
    FVECTOR p1, p2;
    ColFacePtr colface;

    malloc_align16(&hd->free_addr, &colface, sizeof(ColFace)*face->siz/3);

    hd->facenum = face->siz/3;
    
    for (i=0; i<hd->facenum; i++) {
	memcpy(colface[i].p0, ((FVECTOR*)face->pnts)[i*3], sizeof(FVECTOR));
 	memcpy(p1, ((FVECTOR*)face->pnts)[i*3+2], sizeof(FVECTOR));
 	memcpy(p2, ((FVECTOR*)face->pnts)[i*3+1], sizeof(FVECTOR));

   	ps2_vu0_sub_vector(colface[i].v0, p1, colface[i].p0);
   	ps2_vu0_sub_vector(colface[i].v1, p2, colface[i].p0);

        ps2_vu0_outer_product(colface[i].normal, colface[i].v0, colface[i].v1);
        ps2_vu0_normalize(colface[i].normal, colface[i].normal); 

	if (colface[i].normal[1] > 0) {
	    colface[i].normal[0] = -colface[i].normal[0];
	    colface[i].normal[1] = -colface[i].normal[1];
	    colface[i].normal[2] = -colface[i].normal[2];
	}
    }

    hd->colface = colface;
}