|
0
|
1 /*
|
|
|
2 * $Id$
|
|
|
3 */
|
|
|
4 #include <stdio.h>
|
|
|
5 #include"libps2.h"
|
|
|
6 #include"ps2util.h"
|
|
|
7 #include"col.h"
|
|
|
8 #include"mytype.h"
|
|
|
9
|
|
39
|
10 extern FILE *main_fp;
|
|
|
11
|
|
20
|
12 /**
|
|
0
|
13 * v0, v1が描く三角形と点pの内外判定(approx detection)
|
|
|
14 * 点pはv0, v1が描く面のどこかに位置している。
|
|
|
15 * pが面の三角形の外側にあるときTRUEを戻し、
|
|
|
16 * そうでなければFALSEを戻す。境界は含む。
|
|
|
17 */
|
|
|
18 static Bool
|
|
|
19 col_detect_approx(FVECTOR p, FVECTOR v0, FVECTOR v1)
|
|
|
20 {
|
|
20
|
21 float v_r1, v_r2, r1_r2, r1_r1, r2_r2;
|
|
|
22 float a, b, r;
|
|
0
|
23
|
|
20
|
24 v_r1 = ps2_vu0_inner_product( p, v0 );
|
|
|
25 v_r2 = ps2_vu0_inner_product( p, v1 );
|
|
|
26 r1_r2 = ps2_vu0_inner_product( v0, v1 );
|
|
|
27 r1_r1 = ps2_vu0_inner_product( v0, v0 );
|
|
|
28 r2_r2 = ps2_vu0_inner_product( v1, v1 );
|
|
0
|
29
|
|
20
|
30 a = (v_r1 * r2_r2 - v_r2 * r1_r2);
|
|
|
31 b = (v_r2 * r1_r1 - v_r1 * r1_r2);
|
|
|
32 r = (r1_r1 * r2_r2 - r1_r2 * r1_r2);
|
|
0
|
33
|
|
20
|
34 return (a>=0 && b>=0 && a+b-r<=0) ? TRUE : FALSE;
|
|
0
|
35 }
|
|
|
36
|
|
|
37 /**
|
|
|
38 * 垂心oの算出. 点pからv0, v1が描く面へ垂線を引いたときの交点oを
|
|
|
39 * 垂心(orthocenter)と呼ぶ。
|
|
|
40 */
|
|
|
41 static void
|
|
|
42 col_orthocenter(FVECTOR o, FVECTOR p, FVECTOR p0, FVECTOR normal)
|
|
|
43 {
|
|
|
44 FVECTOR o0, d;
|
|
|
45
|
|
|
46 ps2_vu0_sub_vector(o0, p, p0);
|
|
|
47 o0[W_AXIS] = 0;
|
|
|
48 ps2_vu0_scale_vector(
|
|
|
49 d, normal, ps2_vu0_inner_product(o0, normal));
|
|
|
50
|
|
|
51 o[X_AXIS] = p[X_AXIS] - d[X_AXIS];
|
|
|
52 o[Y_AXIS] = p[Y_AXIS] - d[Y_AXIS];
|
|
|
53 o[Z_AXIS] = p[Z_AXIS] - d[Z_AXIS];
|
|
|
54 o[W_AXIS] = 1;
|
|
|
55 }
|
|
|
56
|
|
|
57 /**
|
|
|
58 * 点pと面との衝突(点pとdetection face面との垂心oがdetection face領域の
|
|
|
59 * 中にあるかを)判定衝突時(oがdetection face領域の外)はFALSEを戻し、そう
|
|
|
60 * でなければTRUE、 垂心(orthocenter)、垂心のある面の法線(normal
|
|
|
61 * vector)を戻す.
|
|
|
62 */
|
|
|
63 Bool
|
|
|
64 col_detect(ColFaceHd* hd, FVECTOR pose, FVECTOR p)
|
|
|
65 {
|
|
|
66 FVECTOR o, o0;
|
|
|
67 int i;
|
|
|
68 ColFacePtr col_face=hd->colface;
|
|
|
69 for (i=0; i<hd->facenum; i++) {
|
|
|
70 /*
|
|
|
71 * 垂心oを求める
|
|
|
72 */
|
|
|
73 col_orthocenter(o, p, col_face[i].p0, col_face[i].normal);
|
|
|
74 /*
|
|
|
75 * detection face上のある点p0から垂心oへのベクトルo0を求める
|
|
|
76 */
|
|
|
77 ps2_vu0_sub_vector(o0, o, col_face[i].p0);
|
|
|
78 /*
|
|
|
79 * ベクトルv0, v1の描く三角形とo0とで内外判定(range approx detection)を行う
|
|
|
80 */
|
|
|
81 if (col_detect_approx(o0, col_face[i].v0, col_face[i].v1)==TRUE) {
|
|
|
82 goto NOCOLLISION;
|
|
|
83 }
|
|
|
84 }
|
|
|
85 return FALSE;
|
|
20
|
86 NOCOLLISION:
|
|
0
|
87 ps2_vu0_copy_vector(p, o);
|
|
|
88 ps2_vu0_copy_vector(pose, col_face[i].normal);
|
|
|
89 return TRUE;
|
|
|
90 }
|
|
|
91
|
|
|
92 /*
|
|
|
93 * detection face作成
|
|
|
94 */
|
|
|
95 void
|
|
|
96 col_init(ColFaceHd* hd, PolygonInfo* face)
|
|
|
97 {
|
|
|
98 int i;
|
|
|
99 FVECTOR p1, p2;
|
|
|
100 ColFacePtr colface;
|
|
|
101
|
|
|
102 malloc_align16(&hd->free_addr, &colface, sizeof(ColFace)*face->siz/3);
|
|
|
103
|
|
|
104 hd->facenum = face->siz/3;
|
|
|
105
|
|
|
106 for (i=0; i<hd->facenum; i++) {
|
|
|
107 memcpy(colface[i].p0, ((FVECTOR*)face->pnts)[i*3], sizeof(FVECTOR));
|
|
|
108 memcpy(p1, ((FVECTOR*)face->pnts)[i*3+2], sizeof(FVECTOR));
|
|
|
109 memcpy(p2, ((FVECTOR*)face->pnts)[i*3+1], sizeof(FVECTOR));
|
|
|
110
|
|
|
111 ps2_vu0_sub_vector(colface[i].v0, p1, colface[i].p0);
|
|
|
112 ps2_vu0_sub_vector(colface[i].v1, p2, colface[i].p0);
|
|
|
113
|
|
|
114 ps2_vu0_outer_product(colface[i].normal, colface[i].v0, colface[i].v1);
|
|
|
115 ps2_vu0_normalize(colface[i].normal, colface[i].normal);
|
|
|
116
|
|
|
117 if (colface[i].normal[1] > 0) {
|
|
|
118 colface[i].normal[0] = -colface[i].normal[0];
|
|
|
119 colface[i].normal[1] = -colface[i].normal[1];
|
|
|
120 colface[i].normal[2] = -colface[i].normal[2];
|
|
|
121 }
|
|
|
122 }
|
|
|
123
|
|
|
124 hd->colface = colface;
|
|
|
125 }
|
