Mercurial > hg > old > magoroku_racing
annotate col.cc @ 143:be127b7d1812
minor fix
author | Shinji KONO <kono@ie.u-ryukyu.ac.jp> |
---|---|
date | Sat, 23 Jul 2011 10:18:28 +0900 |
parents | 8ac08eda80ac |
children | d1ddd095ab03 |
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 //なんか一回も使われていないらしいので消す、はずだった |
46 static void | |
47 col_orthocenter(FVECTOR o, FVECTOR p, FVECTOR p0, FVECTOR normal) | |
48 { | |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
49 FVECTOR o0, d; |
0 | 50 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
51 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
|
52 o0[W_AXIS] = 0; |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
53 ps2_vu0_scale_vector( |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
54 d, normal, ps2_vu0_inner_product(o0, normal)); |
0 | 55 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
56 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
|
57 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
|
58 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
|
59 o[W_AXIS] = 1; |
91 | 60 } |
0 | 61 |
128 | 62 |
0 | 63 /** |
88 | 64 * 点pと面との衝突(点pとdetection face面との垂心oがdetection face領域の |
65 * 中にあるかを)判定衝突時(oがdetection face領域の外)はFALSEを戻し、そう | |
66 * でなければTRUE、 垂心(orthocenter)、垂心のある面の法線(normal | |
67 * vector)を戻す. | |
0 | 68 */ |
90
8edae89a3877
compile on Mac OS X
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
88
diff
changeset
|
69 bool |
91 | 70 col_detect(ColFaceHd* hd, FVECTOR pose, FVECTOR p) |
0 | 71 { |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
72 FVECTOR o, o0; |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
73 int i; |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
74 ColFacePtr col_face=hd->colface; |
128 | 75 |
142 | 76 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
77 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
|
78 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
79 // * 垂心oを求める |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
80 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
|
81 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
82 // * detection face上のある点p0から垂心oへのベクトルo0を求める |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
83 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
|
84 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
85 // * ベクトル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
|
86 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
|
87 goto NOCOLLISION; |
0 | 88 } |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
89 } |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
90 return false; |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
91 NOCOLLISION: |
143 | 92 printf("---on hd->facenum = %d/%d ----- ",i, hd->facenum); |
93 printf("P x,y,z = %f,%f,%f ",col_face[i].p0[0],col_face[i].p0[1],col_face[i].p0[2]); | |
94 printf("N x,y,z = %f,%f,%f\n",col_face[i].normal[0],col_face[i].normal[1],col_face[i].normal[2]); | |
95 | |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
96 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
|
97 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
|
98 return true; |
90
8edae89a3877
compile on Mac OS X
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
88
diff
changeset
|
99 |
0 | 100 } |
91 | 101 |
0 | 102 |
103 /* | |
88 | 104 * detection face作成 |
126 | 105 * Collision XML から Polygon を読み込んで、 |
106 * Normal Vector(Polygon に直交した Vector) を生成する。 | |
107 * face の中に、既に Normal Vector が計算されているので、 | |
108 * それを利用してもよい。 | |
0 | 109 */ |
90
8edae89a3877
compile on Mac OS X
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
88
diff
changeset
|
110 |
8edae89a3877
compile on Mac OS X
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
88
diff
changeset
|
111 |
130 | 112 void col_init(ColFaceHd* hd, char *colImg) |
0 | 113 { |
130 | 114 static int col_num = 0; |
128 | 115 |
130 | 116 col_num ++; |
128 | 117 |
130 | 118 // xmlから読み込み |
119 call_createFromXMLfile(colImg); | |
128 | 120 |
130 | 121 char str[100]; |
122 | |
123 sprintf(str,"hoge%d",col_num); | |
128 | 124 |
130 | 125 printf("%s\n",str); |
126 printf("%d\n",col_num); | |
128 | 127 |
130 | 128 // SceneGraphPtr face = droot->createSceneGraph("hoge"); |
129 SceneGraphPtr face = droot->createSceneGraph(str); | |
128 | 130 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
131 int i; |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
132 FVECTOR p1, p2; |
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
133 ColFacePtr colface; |
0 | 134 |
139
dbf8858aceed
malloc error in col.cc
Shinji KONO <kono@ie.u-ryukyu.ac.jp>
parents:
138
diff
changeset
|
135 hd->facenum = face->size/3; |
dbf8858aceed
malloc error in col.cc
Shinji KONO <kono@ie.u-ryukyu.ac.jp>
parents:
138
diff
changeset
|
136 malloc_align16(&hd->free_addr, &colface, sizeof(ColFace)*hd->facenum); |
130 | 137 |
138 | 138 |
0 | 139 |
124 | 140 for (i=0; i < hd->facenum; i++) { |
126 | 141 memcpy(colface[i].p0, &face->pp->tri[i].ver1, sizeof(FVECTOR)); |
142 memcpy(p1, &face->pp->tri[i].ver3, sizeof(FVECTOR)); | |
143 memcpy(p2, &face->pp->tri[i].ver2, sizeof(FVECTOR)); | |
144 | |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
145 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
|
146 ps2_vu0_sub_vector(colface[i].v1, p2, colface[i].p0); |
0 | 147 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
148 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
|
149 ps2_vu0_normalize(colface[i].normal, colface[i].normal); |
0 | 150 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
151 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
|
152 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
|
153 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
|
154 colface[i].normal[2] = -colface[i].normal[2]; |
0 | 155 } |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
156 } |
130 | 157 |
96
0b65ca27f113
compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents:
91
diff
changeset
|
158 hd->colface = colface; |
0 | 159 } |