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