#include <stdio.h>
#include"libps2.h"
#include"ps2util.h"
#include"col.h"
#include"mytype.h"
#include <stdbool.h>
#include <string.h>

extern FILE *main_fp;

/**
 * v0, v1が描く三角形と点pの内外判定(approx detection)
 * 点pはv0, v1が描く面のどこかに位置している。
 * pが面の三角形の外側にあるときTRUEを戻し、
 * そうでなければFALSEを戻す。境界は含む。
 */


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;

  return false;
}


/*
 * detection face作成
 * Collision XML から Polygon を読み込んで、
 * Normal Vector(Polygon に直交した Vector) を生成する。
 * face の中に、既に Normal Vector が計算されているので、
 * それを利用してもよい。
 */


void 
col_init(ColFaceHd* hd, OBJECT* face)
{
  int i;
  FVECTOR p1, p2;
  ColFacePtr colface;

  malloc_align16(&hd->free_addr, &colface, sizeof(ColFace)*face->pp_num);

  hd->facenum = face->pp_num;
  
  for (i=0; i < hd->facenum; i++) {
    memcpy(colface[i].p0, &face->pp->tri[i].ver1, sizeof(FVECTOR));
    memcpy(p1, &face->pp->tri[i].ver3, sizeof(FVECTOR));
    memcpy(p2, &face->pp->tri[i].ver2, 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;

}