1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
|
#include "common.h"
#ifdef VU_COLLISION
#include "VuVector.h"
#include "VuCollision.h"
#ifndef GTA_PS2
int16 vi01;
CVuVector vf01;
CVuVector vf02;
CVuVector vf03;
CVuVector
DistanceBetweenSphereAndLine(const CVuVector ¢er, const CVuVector &p0, const CVuVector &line)
{
// center VF12
// p0 VF14
// line VF15
CVuVector ret; // VF16
CVuVector p1 = p0+line;
CVuVector dist0 = center - p0; // VF20
CVuVector dist1 = center - p1; // VF25
float lenSq = line.MagnitudeSqr(); // VF21
float distSq0 = dist0.MagnitudeSqr(); // VF22
float distSq1 = dist1.MagnitudeSqr();
float dot = DotProduct(dist0, line); // VF23
if(dot < 0.0f){
// not above line, closest to p0
ret = p0;
ret.w = distSq0;
return ret;
}
float t = dot/lenSq; // param of nearest point on infinite line
if(t > 1.0f){
// not above line, closest to p1
ret = p1;
ret.w = distSq1;
return ret;
}
// closest to line
ret = p0 + line*t;
ret.w = (ret - center).MagnitudeSqr();
return ret;
}
inline int SignFlags(const CVector &v)
{
int f = 0;
if(v.x < 0.0f) f |= 1;
if(v.y < 0.0f) f |= 2;
if(v.z < 0.0f) f |= 4;
return f;
}
#endif
extern "C" void
LineToTriangleCollision(const CVuVector &p0, const CVuVector &p1,
const CVuVector &v0, const CVuVector &v1, const CVuVector &v2,
const CVuVector &plane)
{
#ifdef GTA_PS2
__asm__ volatile (
".set noreorder\n"
"lqc2 vf12, 0x0(%0)\n"
"lqc2 vf13, 0x0(%1)\n"
"lqc2 vf14, 0x0(%2)\n"
"lqc2 vf15, 0x0(%3)\n"
"lqc2 vf16, 0x0(%4)\n"
"lqc2 vf17, 0x0(%5)\n"
"vcallms Vu0LineToTriangleCollisionStart\n"
".set reorder\n"
:
: "r" (&p0), "r" (&p1), "r" (&v0), "r" (&v1), "r" (&v2), "r" (&plane)
);
#else
float dot0 = DotProduct(plane, p0);
float dot1 = DotProduct(plane, p1);
float dist0 = plane.w - dot0;
float dist1 = plane.w - dot1;
// if points are on the same side, no collision
if(dist0 * dist1 > 0.0f){
vi01 = 0;
return;
}
CVuVector diff = p1 - p0;
float t = dist0/(dot1 - dot0);
CVuVector p = p0 + diff*t;
p.w = 0.0f;
vf01 = p;
vf03.x = t;
// Check if point is inside
CVector cross1 = CrossProduct(p-v0, v1-v0);
CVector cross2 = CrossProduct(p-v1, v2-v1);
CVector cross3 = CrossProduct(p-v2, v0-v2);
// Only check relevant directions
int flagmask = 0;
if(Abs(plane.x) > 0.5f) flagmask |= 1;
if(Abs(plane.y) > 0.5f) flagmask |= 2;
if(Abs(plane.z) > 0.5f) flagmask |= 4;
int flags1 = SignFlags(cross1) & flagmask;
int flags2 = SignFlags(cross2) & flagmask;
int flags3 = SignFlags(cross3) & flagmask;
// inside if on the same side of all edges
if(flags1 != flags2 || flags1 != flags3){
vi01 = 0;
return;
}
vi01 = 1;
vf02 = plane;
return;
#endif
}
extern "C" void
LineToTriangleCollisionCompressed(const CVuVector &p0, const CVuVector &p1, VuTriangle &tri)
{
#ifdef GTA_PS2
__asm__ volatile (
".set noreorder\n"
"lqc2 vf12, 0x0(%0)\n"
"lqc2 vf13, 0x0(%1)\n"
"lqc2 vf14, 0x0(%2)\n"
"lqc2 vf15, 0x10(%2)\n"
"lqc2 vf16, 0x20(%2)\n"
"lqc2 vf17, 0x30(%2)\n"
"vcallms Vu0LineToTriangleCollisionCompressedStart\n"
".set reorder\n"
:
: "r" (&p0), "r" (&p1), "r" (&tri)
);
#else
CVuVector v0, v1, v2, plane;
v0.x = tri.v0[0]/128.0f;
v0.y = tri.v0[1]/128.0f;
v0.z = tri.v0[2]/128.0f;
v0.w = tri.v0[3]/128.0f;
v1.x = tri.v1[0]/128.0f;
v1.y = tri.v1[1]/128.0f;
v1.z = tri.v1[2]/128.0f;
v1.w = tri.v1[3]/128.0f;
v2.x = tri.v2[0]/128.0f;
v2.y = tri.v2[1]/128.0f;
v2.z = tri.v2[2]/128.0f;
v2.w = tri.v2[3]/128.0f;
plane.x = tri.plane[0]/4096.0f;
plane.y = tri.plane[1]/4096.0f;
plane.z = tri.plane[2]/4096.0f;
plane.w = tri.plane[3]/128.0f;
LineToTriangleCollision(p0, p1, v0, v1, v2, plane);
#endif
}
extern "C" void
SphereToTriangleCollision(const CVuVector &sph,
const CVuVector &v0, const CVuVector &v1, const CVuVector &v2,
const CVuVector &plane)
{
#ifdef GTA_PS2
__asm__ volatile (
".set noreorder\n"
"lqc2 vf12, 0x0(%0)\n"
"lqc2 vf14, 0x0(%1)\n"
"lqc2 vf15, 0x0(%2)\n"
"lqc2 vf16, 0x0(%3)\n"
"lqc2 vf17, 0x0(%4)\n"
"vcallms Vu0SphereToTriangleCollisionStart\n"
".set reorder\n"
:
: "r" (&sph), "r" (&v0), "r" (&v1), "r" (&v2), "r" (&plane)
);
#else
float planedist = DotProduct(plane, sph) - plane.w; // VF02
if(Abs(planedist) > sph.w){
vi01 = 0;
return;
}
// point on plane
CVuVector p = sph - planedist*plane;
p.w = 0.0f;
vf01 = p;
planedist = Abs(planedist);
// edges
CVuVector v01 = v1 - v0;
CVuVector v12 = v2 - v1;
CVuVector v20 = v0 - v2;
// VU code calculates normal again for some weird reason...
// Check sides of point
CVector cross1 = CrossProduct(p-v0, v01);
CVector cross2 = CrossProduct(p-v1, v12);
CVector cross3 = CrossProduct(p-v2, v20);
// Only check relevant directions
int flagmask = 0;
if(Abs(plane.x) > 0.1f) flagmask |= 1;
if(Abs(plane.y) > 0.1f) flagmask |= 2;
if(Abs(plane.z) > 0.1f) flagmask |= 4;
int nflags = SignFlags(plane) & flagmask;
int flags1 = SignFlags(cross1) & flagmask;
int flags2 = SignFlags(cross2) & flagmask;
int flags3 = SignFlags(cross3) & flagmask;
int testcase = 0;
CVuVector closest(0.0f, 0.0f, 0.0f); // VF04
if(flags1 == nflags){
closest += v2;
testcase++;
}
if(flags2 == nflags){
closest += v0;
testcase++;
}
if(flags3 == nflags){
closest += v1;
testcase++;
}
if(testcase == 3){
// inside triangle - dist to plane already checked
vf02 = plane;
vf02.w = vf03.x = planedist;
vi01 = 1;
}else if(testcase == 1){
// outside two sides - closest to point opposide inside edge
vf01 = closest;
vf02 = sph - closest;
float distSq = vf02.MagnitudeSqr();
vi01 = sph.w*sph.w > distSq;
vf03.x = Sqrt(distSq);
vf02 *= 1.0f/vf03.x;
}else{
// inside two sides - closest to third edge
if(flags1 != nflags)
closest = DistanceBetweenSphereAndLine(sph, v0, v01);
else if(flags2 != nflags)
closest = DistanceBetweenSphereAndLine(sph, v1, v12);
else
closest = DistanceBetweenSphereAndLine(sph, v2, v20);
vi01 = sph.w*sph.w > closest.w;
vf01 = closest;
vf02 = sph - closest;
vf03.x = Sqrt(closest.w);
vf02 *= 1.0f/vf03.x;
}
#endif
}
extern "C" void
SphereToTriangleCollisionCompressed(const CVuVector &sph, VuTriangle &tri)
{
#ifdef GTA_PS2
__asm__ volatile (
".set noreorder\n"
"lqc2 vf12, 0x0(%0)\n"
"lqc2 vf14, 0x0(%1)\n"
"lqc2 vf15, 0x10(%1)\n"
"lqc2 vf16, 0x20(%1)\n"
"lqc2 vf17, 0x30(%1)\n"
"vcallms Vu0SphereToTriangleCollisionCompressedStart\n"
".set reorder\n"
:
: "r" (&sph), "r" (&tri)
);
#else
CVuVector v0, v1, v2, plane;
v0.x = tri.v0[0]/128.0f;
v0.y = tri.v0[1]/128.0f;
v0.z = tri.v0[2]/128.0f;
v0.w = tri.v0[3]/128.0f;
v1.x = tri.v1[0]/128.0f;
v1.y = tri.v1[1]/128.0f;
v1.z = tri.v1[2]/128.0f;
v1.w = tri.v1[3]/128.0f;
v2.x = tri.v2[0]/128.0f;
v2.y = tri.v2[1]/128.0f;
v2.z = tri.v2[2]/128.0f;
v2.w = tri.v2[3]/128.0f;
plane.x = tri.plane[0]/4096.0f;
plane.y = tri.plane[1]/4096.0f;
plane.z = tri.plane[2]/4096.0f;
plane.w = tri.plane[3]/128.0f;
SphereToTriangleCollision(sph, v0, v1, v2, plane);
#endif
}
#endif
|
