Mercurial > hg > old > magoroku_racing

comparison col.c @ 0:0fae5658fb0b

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Initial revision
author gongo
date Thu, 02 Nov 2006 08:55:19 +0000
parents
children b1ba4dad7f6e
comparison
equal deleted inserted replaced
-1:000000000000 0:0fae5658fb0b
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 }