Mercurial > hg > old > magoroku_racing

annotate col.cc @ 112:5106d8d12ded

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Changing Magoroku.cc
author Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
date Thu, 09 Jun 2011 14:59:24 +0900
parents c534f339ee8b
children 6ef3b0efdeaf
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
1 #include <stdio.h>
91
cb6c6de125dc halfway
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 90
diff changeset
2 #include"libps2.h"
cb6c6de125dc halfway
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 90
diff changeset
3 #include"ps2util.h"
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
4 #include"col.h"
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
5 #include"mytype.h"
91
cb6c6de125dc halfway
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 90
diff changeset
6 #include <stdbool.h>
cb6c6de125dc halfway
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 90
diff changeset
7 #include <string.h>
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
8
39
9ec616de33a8 *** empty log message ***
gongo
parents: 20
diff changeset
9 extern FILE *main_fp;
9ec616de33a8 *** empty log message ***
gongo
parents: 20
diff changeset
10
20
b1ba4dad7f6e *** empty log message ***
gongo
parents: 0
diff changeset
11 /**
88
ce9ef7dcf2f0 convert UTF-8
Shinji KONO <kono@ie.u-ryukyu.ac.jp>
parents: 44
diff changeset
12 * v0, v1が描く三角形と点pの内外判定(approx detection)
ce9ef7dcf2f0 convert UTF-8
Shinji KONO <kono@ie.u-ryukyu.ac.jp>
parents: 44
diff changeset
13 * 点pはv0, v1が描く面のどこかに位置している。
ce9ef7dcf2f0 convert UTF-8
Shinji KONO <kono@ie.u-ryukyu.ac.jp>
parents: 44
diff changeset
14 * pが面の三角形の外側にあるときTRUEを戻し、
ce9ef7dcf2f0 convert UTF-8
Shinji KONO <kono@ie.u-ryukyu.ac.jp>
parents: 44
diff changeset
15 * そうでなければFALSEを戻す。境界は含む。
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
16 */
90
8edae89a3877 compile on Mac OS X
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 88
diff changeset
17
91
cb6c6de125dc halfway
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 90
diff changeset
18
cb6c6de125dc halfway
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 90
diff changeset
19 bool
cb6c6de125dc halfway
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 90
diff changeset
20 col_detect_approx(FVECTOR p, FVECTOR v0, FVECTOR v1){
90
8edae89a3877 compile on Mac OS X
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 88
diff changeset
21
96
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
22 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
23 float a, b, r;
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
24
96
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
25 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
26 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
27 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
28 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
29 r2_r2 = ps2_vu0_inner_product( v1, v1 );
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
30
96
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
31 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
32 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
33 r = (r1_r1 * r2_r2 - r1_r2 * r1_r2);
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
34
96
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
35 return (a>=0 && b>=0 && a+b-r<=0) ? true : false;
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
36 }
91
cb6c6de125dc halfway
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 90
diff changeset
37
90
8edae89a3877 compile on Mac OS X
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 88
diff changeset
38
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
39
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
40 /**
88
ce9ef7dcf2f0 convert UTF-8
Shinji KONO <kono@ie.u-ryukyu.ac.jp>
parents: 44
diff changeset
41 * 垂心oの算出. 点pからv0, v1が描く面へ垂線を引いたときの交点oを
ce9ef7dcf2f0 convert UTF-8
Shinji KONO <kono@ie.u-ryukyu.ac.jp>
parents: 44
diff changeset
42 * 垂心(orthocenter)と呼ぶ。
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
43 */
91
cb6c6de125dc halfway
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 90
diff changeset
44 //なんか一回も使われていないらしいので消す、はずだった
cb6c6de125dc halfway
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 90
diff changeset
45 static void
cb6c6de125dc halfway
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 90
diff changeset
46 col_orthocenter(FVECTOR o, FVECTOR p, FVECTOR p0, FVECTOR normal)
cb6c6de125dc halfway
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 90
diff changeset
47 {
cb6c6de125dc halfway
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 90
diff changeset
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
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
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
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
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
cb6c6de125dc halfway
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 90
diff changeset
60 }
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
61
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
62 /**
88
ce9ef7dcf2f0 convert UTF-8
Shinji KONO <kono@ie.u-ryukyu.ac.jp>
parents: 44
diff changeset
63 * 点pと面との衝突(点pとdetection face面との垂心oがdetection face領域の
ce9ef7dcf2f0 convert UTF-8
Shinji KONO <kono@ie.u-ryukyu.ac.jp>
parents: 44
diff changeset
64 * 中にあるかを)判定衝突時(oがdetection face領域の外)はFALSEを戻し、そう
ce9ef7dcf2f0 convert UTF-8
Shinji KONO <kono@ie.u-ryukyu.ac.jp>
parents: 44
diff changeset
65 * でなければTRUE、 垂心(orthocenter)、垂心のある面の法線(normal
ce9ef7dcf2f0 convert UTF-8
Shinji KONO <kono@ie.u-ryukyu.ac.jp>
parents: 44
diff changeset
66 * vector)を戻す.
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
67 */
90
8edae89a3877 compile on Mac OS X
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 88
diff changeset
68
91
cb6c6de125dc halfway
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 90
diff changeset
69
90
8edae89a3877 compile on Mac OS X
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 88
diff changeset
70 bool
91
cb6c6de125dc halfway
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 90
diff changeset
71 col_detect(ColFaceHd* hd, FVECTOR pose, FVECTOR p)
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
72 {
96
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
73 FVECTOR o, o0;
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
74 int i;
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
75 ColFacePtr col_face=hd->colface;
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
76 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
77
96
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
78 // * 垂心oを求める
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
79 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
80
96
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
81 // * detection face上のある点p0から垂心oへのベクトルo0を求める
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
82 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
83
96
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
84 // * ベクトル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
85 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
86 goto NOCOLLISION;
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
87 }
96
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
88 }
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
89 return false;
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
90 NOCOLLISION:
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
91 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
92 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
93 return true;
90
8edae89a3877 compile on Mac OS X
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 88
diff changeset
94
96
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
95 return false;
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
96 }
91
cb6c6de125dc halfway
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 90
diff changeset
97
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
98
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
99 /*
88
ce9ef7dcf2f0 convert UTF-8
Shinji KONO <kono@ie.u-ryukyu.ac.jp>
parents: 44
diff changeset
100 * detection face作成
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
101 */
90
8edae89a3877 compile on Mac OS X
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 88
diff changeset
102
8edae89a3877 compile on Mac OS X
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 88
diff changeset
103
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
104 void
91
cb6c6de125dc halfway
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 90
diff changeset
105 col_init(ColFaceHd* hd, PolygonInfo* face)
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
106 {
91
cb6c6de125dc halfway
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 90
diff changeset
107
96
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
108 int i;
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
109 FVECTOR p1, p2;
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
110 ColFacePtr colface;
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
111
96
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
112 malloc_align16(&hd->free_addr, &colface, sizeof(ColFace)*face->siz/3);
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
113
96
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
114 hd->facenum = face->siz/3;
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
115
96
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
116 for (i=0; i<hd->facenum; i++) {
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
117 memcpy(colface[i].p0, ((FVECTOR*)face->pnts)[i*3], sizeof(FVECTOR));
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
118 memcpy(p1, ((FVECTOR*)face->pnts)[i*3+2], sizeof(FVECTOR));
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
119 memcpy(p2, ((FVECTOR*)face->pnts)[i*3+1], sizeof(FVECTOR));
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
120
96
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
121 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
122 ps2_vu0_sub_vector(colface[i].v1, p2, colface[i].p0);
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
123
96
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
124 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
125 ps2_vu0_normalize(colface[i].normal, colface[i].normal);
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
126
96
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
127 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
128 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
129 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
130 colface[i].normal[2] = -colface[i].normal[2];
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
131 }
96
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
132 }
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
133
96
0b65ca27f113 compile on Mac OS X 2
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 91
diff changeset
134 hd->colface = colface;
90
8edae89a3877 compile on Mac OS X
Atuto SHIROMA <e095729@ie.u-ryukyu.ac.jp>
parents: 88
diff changeset
135
0
0fae5658fb0b Initial revision
gongo
parents:
diff changeset
136 }