Mercurial > hg > old > magoroku_racing
annotate col.cc @ 152:9fc2d52b28c5 default
merge
author | Shinji KONO <kono@ie.u-ryukyu.ac.jp> |
---|---|
date | Sat, 30 Jul 2011 14:44:30 +0900 |
parents | d1ddd095ab03 |
children |
rev | line source |
---|---|
0 | 1 #include <stdio.h> |
91 | 2 #include"libps2.h" |
3 #include"ps2util.h" | |
0 | 4 #include"col.h" |
5 #include"mytype.h" | |
91 | 6 #include <stdbool.h> |
7 #include <string.h> | |
0 | 8 |
130 | 9 #include "gSprite.h" |
10 | |
11 extern Viewer *droot; | |
12 | |
20 | 13 /** |
88 | 14 * v0, v1が描く三角形と点pの内外判定(approx detection) |
15 * 点pはv0, v1が描く面のどこかに位置している。 | |
16 * pが面の三角形の外側にあるときTRUEを戻し、 | |
17 * そうでなければFALSEを戻す。境界は含む。 | |
0 | 18 */ |
90
8edae89a3877
compile on Mac OS X
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
88
diff
changeset
|
19 |
91 | 20 bool |
128 | 21 col_detect_approx(FVECTOR p, FVECTOR v0, FVECTOR v1) |
22 { | |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
23 float v_r1, v_r2, r1_r2, r1_r1, r2_r2; |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
24 float a, b, r; |
0 | 25 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
26 v_r1 = ps2_vu0_inner_product( p, v0 ); |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
27 v_r2 = ps2_vu0_inner_product( p, v1 ); |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
28 r1_r2 = ps2_vu0_inner_product( v0, v1 ); |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
29 r1_r1 = ps2_vu0_inner_product( v0, v0 ); |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
30 r2_r2 = ps2_vu0_inner_product( v1, v1 ); |
0 | 31 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
32 a = (v_r1 * r2_r2 - v_r2 * r1_r2); |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
33 b = (v_r2 * r1_r1 - v_r1 * r1_r2); |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
34 r = (r1_r1 * r2_r2 - r1_r2 * r1_r2); |
0 | 35 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
36 return (a>=0 && b>=0 && a+b-r<=0) ? true : false; |
0 | 37 } |
91 | 38 |
90
8edae89a3877
compile on Mac OS X
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
88
diff
changeset
|
39 |
0 | 40 |
41 /** | |
88 | 42 * 垂心oの算出. 点pからv0, v1が描く面へ垂線を引いたときの交点oを |
43 * 垂心(orthocenter)と呼ぶ。 | |
0 | 44 */ |
91 | 45 static void |
46 col_orthocenter(FVECTOR o, FVECTOR p, FVECTOR p0, FVECTOR normal) | |
47 { | |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
48 FVECTOR o0, d; |
0 | 49 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
50 ps2_vu0_sub_vector(o0, p, p0); |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
51 o0[W_AXIS] = 0; |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
52 ps2_vu0_scale_vector( |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
53 d, normal, ps2_vu0_inner_product(o0, normal)); |
0 | 54 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
55 o[X_AXIS] = p[X_AXIS] - d[X_AXIS]; |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
56 o[Y_AXIS] = p[Y_AXIS] - d[Y_AXIS]; |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
57 o[Z_AXIS] = p[Z_AXIS] - d[Z_AXIS]; |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
58 o[W_AXIS] = 1; |
91 | 59 } |
0 | 60 |
61 /** | |
88 | 62 * 点pと面との衝突(点pとdetection face面との垂心oがdetection face領域の |
63 * 中にあるかを)判定衝突時(oがdetection face領域の外)はFALSEを戻し、そう | |
64 * でなければTRUE、 垂心(orthocenter)、垂心のある面の法線(normal | |
65 * vector)を戻す. | |
0 | 66 */ |
90
8edae89a3877
compile on Mac OS X
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
88
diff
changeset
|
67 bool |
91 | 68 col_detect(ColFaceHd* hd, FVECTOR pose, FVECTOR p) |
0 | 69 { |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
70 FVECTOR o, o0; |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
71 int i; |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
72 ColFacePtr col_face=hd->colface; |
128 | 73 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
74 for (i=0; i<hd->facenum; i++) { |
90
8edae89a3877
compile on Mac OS X
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
88
diff
changeset
|
75 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
76 // * 垂心oを求める |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
77 col_orthocenter(o, p, col_face[i].p0, col_face[i].normal); |
90
8edae89a3877
compile on Mac OS X
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
88
diff
changeset
|
78 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
79 // * detection face上のある点p0から垂心oへのベクトルo0を求める |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
80 ps2_vu0_sub_vector(o0, o, col_face[i].p0); |
90
8edae89a3877
compile on Mac OS X
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
88
diff
changeset
|
81 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
82 // * ベクトルv0, v1の描く三角形とo0とで内外判定(range approx detection)を行う |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
83 if (col_detect_approx(o0, col_face[i].v0, col_face[i].v1)==true) { |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
84 goto NOCOLLISION; |
0 | 85 } |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
86 } |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
87 return false; |
146 | 88 |
89 NOCOLLISION: // "壁に"ぶつかっていない = course上にいる | |
143 | 90 printf("---on hd->facenum = %d/%d ----- ",i, hd->facenum); |
91 printf("P x,y,z = %f,%f,%f ",col_face[i].p0[0],col_face[i].p0[1],col_face[i].p0[2]); | |
92 printf("N x,y,z = %f,%f,%f\n",col_face[i].normal[0],col_face[i].normal[1],col_face[i].normal[2]); | |
93 | |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
94 ps2_vu0_copy_vector(p, o); |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
95 ps2_vu0_copy_vector(pose, col_face[i].normal); |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
96 return true; |
90
8edae89a3877
compile on Mac OS X
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
88
diff
changeset
|
97 |
0 | 98 } |
91 | 99 |
0 | 100 |
101 /* | |
88 | 102 * detection face作成 |
126 | 103 * Collision XML から Polygon を読み込んで、 |
104 * Normal Vector(Polygon に直交した Vector) を生成する。 | |
105 * face の中に、既に Normal Vector が計算されているので、 | |
106 * それを利用してもよい。 | |
0 | 107 */ |
90
8edae89a3877
compile on Mac OS X
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
88
diff
changeset
|
108 |
130 | 109 void col_init(ColFaceHd* hd, char *colImg) |
0 | 110 { |
130 | 111 static int col_num = 0; |
128 | 112 |
130 | 113 col_num ++; |
128 | 114 |
130 | 115 // xmlから読み込み |
116 call_createFromXMLfile(colImg); | |
128 | 117 |
130 | 118 char str[100]; |
119 | |
120 sprintf(str,"hoge%d",col_num); | |
128 | 121 |
130 | 122 printf("%s\n",str); |
123 printf("%d\n",col_num); | |
128 | 124 |
130 | 125 // SceneGraphPtr face = droot->createSceneGraph("hoge"); |
126 SceneGraphPtr face = droot->createSceneGraph(str); | |
128 | 127 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
128 int i; |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
129 FVECTOR p1, p2; |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
130 ColFacePtr colface; |
0 | 131 |
139
dbf8858aceed
malloc error in col.cc
Shinji KONO <kono@ie.u-ryukyu.ac.jp>
parents:
138
diff
changeset
|
132 hd->facenum = face->size/3; |
dbf8858aceed
malloc error in col.cc
Shinji KONO <kono@ie.u-ryukyu.ac.jp>
parents:
138
diff
changeset
|
133 malloc_align16(&hd->free_addr, &colface, sizeof(ColFace)*hd->facenum); |
130 | 134 |
0 | 135 |
124 | 136 for (i=0; i < hd->facenum; i++) { |
126 | 137 memcpy(colface[i].p0, &face->pp->tri[i].ver1, sizeof(FVECTOR)); |
138 memcpy(p1, &face->pp->tri[i].ver3, sizeof(FVECTOR)); | |
139 memcpy(p2, &face->pp->tri[i].ver2, sizeof(FVECTOR)); | |
146 | 140 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
141 ps2_vu0_sub_vector(colface[i].v0, p1, colface[i].p0); |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
142 ps2_vu0_sub_vector(colface[i].v1, p2, colface[i].p0); |
0 | 143 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
144 ps2_vu0_outer_product(colface[i].normal, colface[i].v0, colface[i].v1); |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
145 ps2_vu0_normalize(colface[i].normal, colface[i].normal); |
0 | 146 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
147 if (colface[i].normal[1] > 0) { |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
148 colface[i].normal[0] = -colface[i].normal[0]; |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
149 colface[i].normal[1] = -colface[i].normal[1]; |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
150 colface[i].normal[2] = -colface[i].normal[2]; |
0 | 151 } |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
152 } |
130 | 153 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
154 hd->colface = colface; |
0 | 155 } |