PS2SDK
PS2 Homebrew Libraries
math3d.c
1 /*
2 # _____ ___ ____ ___ ____
3 # ____| | ____| | | |____|
4 # | ___| |____ ___| ____| | \ PS2DEV Open Source Project.
5 #-----------------------------------------------------------------------
6 # (c) 2005 Naomi Peori <naomi@peori.ca>
7 # Licenced under Academic Free License version 2.0
8 # Review ps2sdk README & LICENSE files for further details.
9 */
10 
11  #include <tamtypes.h>
12 
13  #include <graph.h>
14  #include <math3d.h>
15  #include <string.h>
16  #include <math.h>
17 
18  /* VECTOR FUNCTIONS */
19 
20  void vector_apply(VECTOR output, VECTOR input0, MATRIX input1) {
21 #ifdef _EE
22  __asm__ __volatile__ (
23 #if __GNUC__ > 3
24  "lqc2 $vf1, 0x00(%2) \n"
25  "lqc2 $vf2, 0x10(%2) \n"
26  "lqc2 $vf3, 0x20(%2) \n"
27  "lqc2 $vf4, 0x30(%2) \n"
28  "lqc2 $vf5, 0x00(%1) \n"
29  "vmulaw $ACC, $vf4, $vf0\n"
30  "vmaddax $ACC, $vf1, $vf5\n"
31  "vmadday $ACC, $vf2, $vf5\n"
32  "vmaddz $vf6, $vf3, $vf5\n"
33  "sqc2 $vf6, 0x00(%0) \n"
34 #else
35  "lqc2 vf1, 0x00(%2) \n"
36  "lqc2 vf2, 0x10(%2) \n"
37  "lqc2 vf3, 0x20(%2) \n"
38  "lqc2 vf4, 0x30(%2) \n"
39  "lqc2 vf5, 0x00(%1) \n"
40  "vmulaw ACC, vf4, vf0 \n"
41  "vmaddax ACC, vf1, vf5 \n"
42  "vmadday ACC, vf2, vf5 \n"
43  "vmaddz vf6, vf3, vf5 \n"
44  "sqc2 vf6, 0x00(%0) \n"
45 #endif
46  : : "r" (output), "r" (input0), "r" (input1)
47  : "memory"
48  );
49 #else
50  int i;
51  memset(output, 0, sizeof(VECTOR));
52  for (i = 0; i < 4; i += 1)
53  {
54  int j;
55  for (j = 0; j < 4; j += 1)
56  {
57  output[j] += input1[(4 * i) + j] * (i != 3 ? input0[i] : 1.0f);
58  }
59  }
60 #endif
61  }
62 
63  void vector_clamp(VECTOR output, VECTOR input0, float min, float max) {
64  VECTOR work;
65 
66  // Copy the vector.
67  vector_copy(work, input0);
68 
69  // Clamp the minimum values.
70  if (work[0] < min) { work[0] = min; }
71  if (work[1] < min) { work[1] = min; }
72  if (work[2] < min) { work[2] = min; }
73  if (work[3] < min) { work[3] = min; }
74 
75  // Clamp the maximum values.
76  if (work[0] > max) { work[0] = max; }
77  if (work[1] > max) { work[1] = max; }
78  if (work[2] > max) { work[2] = max; }
79  if (work[3] > max) { work[3] = max; }
80 
81  // Output the result.
82  vector_copy(output, work);
83 
84  }
85 
86  void vector_copy(VECTOR output, VECTOR input0) {
87 #ifdef _EE
88  __asm__ __volatile__ (
89 #if __GNUC__ > 3
90  "lqc2 $vf1, 0x00(%1) \n"
91  "sqc2 $vf1, 0x00(%0) \n"
92 #else
93  "lqc2 vf1, 0x00(%1) \n"
94  "sqc2 vf1, 0x00(%0) \n"
95 #endif
96  : : "r" (output), "r" (input0)
97  : "memory"
98  );
99 #else
100  memcpy(output, input0, sizeof(VECTOR));
101 #endif
102  }
103 
104  float vector_innerproduct(VECTOR input0, VECTOR input1) {
105  VECTOR work0, work1;
106 
107  // Normalize the first vector.
108  work0[0] = (input0[0] / input0[3]);
109  work0[1] = (input0[1] / input0[3]);
110  work0[2] = (input0[2] / input0[3]);
111  work0[3] = 1.00f;
112 
113  // Normalize the second vector.
114  work1[0] = (input1[0] / input1[3]);
115  work1[1] = (input1[1] / input1[3]);
116  work1[2] = (input1[2] / input1[3]);
117  work1[3] = 1.00f;
118 
119  // Return the inner product.
120  return (work0[0] * work1[0]) + (work0[1] * work1[1]) + (work0[2] * work1[2]);
121 
122  }
123 
124  void vector_multiply(VECTOR output, VECTOR input0, VECTOR input1) {
125  VECTOR work;
126 
127  // Multiply the vectors together.
128  work[0] = input0[0] * input1[0];
129  work[1] = input0[1] * input1[1];
130  work[2] = input0[2] * input1[2];
131  work[3] = input0[3] * input1[3];
132 
133  // Output the result.
134  vector_copy(output, work);
135 
136  }
137 
138  void vector_normalize(VECTOR output, VECTOR input0) {
139 #ifdef _EE
140  __asm__ __volatile__ (
141 #if __GNUC__ > 3
142  "lqc2 $vf1, 0x00(%1) \n"
143  "vmul.xyz $vf2, $vf1, $vf1\n"
144  "vmulax.w $ACC, $vf0, $vf2\n"
145  "vmadday.w $ACC, $vf0, $vf2\n"
146  "vmaddz.w $vf2, $vf0, $vf2\n"
147  "vrsqrt $Q, $vf0w, $vf2w\n"
148  "vsub.w $vf1, $vf0, $vf0\n"
149  "vwaitq \n"
150  "vmulq.xyz $vf1, $vf1, $Q \n"
151  "sqc2 $vf1, 0x00(%0) \n"
152 #else
153  "lqc2 vf1, 0x00(%1) \n"
154  "vmul.xyz vf2, vf1, vf1 \n"
155  "vmulax.w ACC, vf0, vf2 \n"
156  "vmadday.w ACC, vf0, vf2 \n"
157  "vmaddz.w vf2, vf0, vf2 \n"
158  "vrsqrt Q, vf0w, vf2w \n"
159  "vsub.w vf1, vf0, vf0 \n"
160  "vwaitq \n"
161  "vmulq.xyz vf1, vf1, Q \n"
162  "sqc2 vf1, 0x00(%0) \n"
163 #endif
164  : : "r" (output), "r" (input0)
165  : "memory"
166  );
167 #else
168  float q;
169  q = 1.0f / sqrtf((input0[0] * input0[0]) + (input0[1] * input0[1]) + (input0[2] * input0[2]));
170  output[0] = input0[0] * q;
171  output[1] = input0[1] * q;
172  output[2] = input0[2] * q;
173  output[3] = 0.0f;
174 #endif
175  }
176 
177  void vector_outerproduct(VECTOR output, VECTOR input0, VECTOR input1) {
178 #ifdef _EE
179  __asm__ __volatile__ (
180 #if __GNUC__ > 3
181  "lqc2 $vf1, 0x00(%1) \n"
182  "lqc2 $vf2, 0x00(%2) \n"
183  "vopmula.xyz $ACC, $vf1, $vf2\n"
184  "vopmsub.xyz $vf2, $vf2, $vf1\n"
185  "vsub.w $vf2, $vf0, $vf0\n"
186  "sqc2 $vf2, 0x00(%0) \n"
187 #else
188  "lqc2 vf1, 0x00(%1) \n"
189  "lqc2 vf2, 0x00(%2) \n"
190  "vopmula.xyz ACC, vf1, vf2 \n"
191  "vopmsub.xyz vf2, vf2, vf1 \n"
192  "vsub.w vf2, vf0, vf0 \n"
193  "sqc2 vf2, 0x00(%0) \n"
194 #endif
195  : : "r" (output), "r" (input0), "r" (input1)
196  : "memory"
197  );
198 #else
199  output[0] = input0[1] * input1[2] - input1[1] * input0[2];
200  output[1] = input0[2] * input1[0] - input1[2] * input0[0];
201  output[2] = input0[0] * input1[1] - input1[0] * input0[1];
202  output[3] = 0.0f;
203 #endif
204  }
205 
206  void vector_add(VECTOR sum, VECTOR addend, VECTOR summand) {
207  VECTOR work;
208  work[0] = addend[0]+summand[0];
209  work[1] = addend[1]+summand[1];
210  work[2] = addend[2]+summand[2];
211  work[3] = addend[3]+summand[3];
212  vector_copy(sum, work);
213 }
214 
215 void vector_cross_product(VECTOR product, VECTOR multiplicand, VECTOR multiplier) {
216  VECTOR work;
217  work[0] = multiplicand[1] * multiplier[2] - multiplicand[2] * multiplier[1];
218  work[1] = multiplicand[2] * multiplier[0] - multiplicand[0] * multiplier[2];
219  work[2] = multiplicand[0] * multiplier[1] - multiplicand[1] * multiplier[0];
220  work[3] = 1.0f;
221  vector_copy(product, work);
222 }
223 
224 void vector_triangle_normal(VECTOR output, VECTOR a, VECTOR b, VECTOR c) {
225  VECTOR ac;
226  VECTOR bc;
227  VECTOR ac_bc;
228  VECTOR triangle_normal;
229 
230  vector_add(ac, a, c);
231  vector_add(bc, b, c);
232  vector_cross_product(ac_bc, ac, bc);
233  vector_normalize(triangle_normal, ac_bc);
234 
235  vector_copy(output, triangle_normal);
236 }
237 
238  /* MATRIX FUNCTIONS */
239 
240  void matrix_copy(MATRIX output, MATRIX input0) {
241 #ifdef _EE
242  __asm__ __volatile__ (
243 #if __GNUC__ > 3
244  "lqc2 $vf1, 0x00(%1) \n"
245  "lqc2 $vf2, 0x10(%1) \n"
246  "lqc2 $vf3, 0x20(%1) \n"
247  "lqc2 $vf4, 0x30(%1) \n"
248  "sqc2 $vf1, 0x00(%0) \n"
249  "sqc2 $vf2, 0x10(%0) \n"
250  "sqc2 $vf3, 0x20(%0) \n"
251  "sqc2 $vf4, 0x30(%0) \n"
252 #else
253  "lqc2 vf1, 0x00(%1) \n"
254  "lqc2 vf2, 0x10(%1) \n"
255  "lqc2 vf3, 0x20(%1) \n"
256  "lqc2 vf4, 0x30(%1) \n"
257  "sqc2 vf1, 0x00(%0) \n"
258  "sqc2 vf2, 0x10(%0) \n"
259  "sqc2 vf3, 0x20(%0) \n"
260  "sqc2 vf4, 0x30(%0) \n"
261 #endif
262  : : "r" (output), "r" (input0)
263  : "memory"
264  );
265 #else
266  memcpy(output, input0, sizeof(MATRIX));
267 #endif
268  }
269 
270  void matrix_inverse(MATRIX output, MATRIX input0) {
271  MATRIX work;
272 
273  // Calculate the inverse of the matrix.
274  matrix_transpose(work, input0);
275  work[0x03] = 0.00f;
276  work[0x07] = 0.00f;
277  work[0x0B] = 0.00f;
278  work[0x0C] = -(input0[0x0C] * work[0x00] + input0[0x0D] * work[0x04] + input0[0x0E] * work[0x08]);
279  work[0x0D] = -(input0[0x0C] * work[0x01] + input0[0x0D] * work[0x05] + input0[0x0E] * work[0x09]);
280  work[0x0E] = -(input0[0x0C] * work[0x02] + input0[0x0D] * work[0x06] + input0[0x0E] * work[0x0A]);
281  work[0x0F] = 1.00f;
282 
283  // Output the result.
284  matrix_copy(output, work);
285 
286  }
287 
288  void matrix_multiply(MATRIX output, MATRIX input0, MATRIX input1) {
289 #ifdef _EE
290  __asm__ __volatile__ (
291 #if __GNUC__ > 3
292  "lqc2 $vf1, 0x00(%1) \n"
293  "lqc2 $vf2, 0x10(%1) \n"
294  "lqc2 $vf3, 0x20(%1) \n"
295  "lqc2 $vf4, 0x30(%1) \n"
296  "lqc2 $vf5, 0x00(%2) \n"
297  "lqc2 $vf6, 0x10(%2) \n"
298  "lqc2 $vf7, 0x20(%2) \n"
299  "lqc2 $vf8, 0x30(%2) \n"
300  "vmulax.xyzw $ACC, $vf5, $vf1\n"
301  "vmadday.xyzw $ACC, $vf6, $vf1\n"
302  "vmaddaz.xyzw $ACC, $vf7, $vf1\n"
303  "vmaddw.xyzw $vf1, $vf8, $vf1\n"
304  "vmulax.xyzw $ACC, $vf5, $vf2\n"
305  "vmadday.xyzw $ACC, $vf6, $vf2\n"
306  "vmaddaz.xyzw $ACC, $vf7, $vf2\n"
307  "vmaddw.xyzw $vf2, $vf8, $vf2\n"
308  "vmulax.xyzw $ACC, $vf5, $vf3\n"
309  "vmadday.xyzw $ACC, $vf6, $vf3\n"
310  "vmaddaz.xyzw $ACC, $vf7, $vf3\n"
311  "vmaddw.xyzw $vf3, $vf8, $vf3\n"
312  "vmulax.xyzw $ACC, $vf5, $vf4\n"
313  "vmadday.xyzw $ACC, $vf6, $vf4\n"
314  "vmaddaz.xyzw $ACC, $vf7, $vf4\n"
315  "vmaddw.xyzw $vf4, $vf8, $vf4\n"
316  "sqc2 $vf1, 0x00(%0) \n"
317  "sqc2 $vf2, 0x10(%0) \n"
318  "sqc2 $vf3, 0x20(%0) \n"
319  "sqc2 $vf4, 0x30(%0) \n"
320 #else
321  "lqc2 vf1, 0x00(%1) \n"
322  "lqc2 vf2, 0x10(%1) \n"
323  "lqc2 vf3, 0x20(%1) \n"
324  "lqc2 vf4, 0x30(%1) \n"
325  "lqc2 vf5, 0x00(%2) \n"
326  "lqc2 vf6, 0x10(%2) \n"
327  "lqc2 vf7, 0x20(%2) \n"
328  "lqc2 vf8, 0x30(%2) \n"
329  "vmulax.xyzw ACC, vf5, vf1 \n"
330  "vmadday.xyzw ACC, vf6, vf1 \n"
331  "vmaddaz.xyzw ACC, vf7, vf1 \n"
332  "vmaddw.xyzw vf1, vf8, vf1 \n"
333  "vmulax.xyzw ACC, vf5, vf2 \n"
334  "vmadday.xyzw ACC, vf6, vf2 \n"
335  "vmaddaz.xyzw ACC, vf7, vf2 \n"
336  "vmaddw.xyzw vf2, vf8, vf2 \n"
337  "vmulax.xyzw ACC, vf5, vf3 \n"
338  "vmadday.xyzw ACC, vf6, vf3 \n"
339  "vmaddaz.xyzw ACC, vf7, vf3 \n"
340  "vmaddw.xyzw vf3, vf8, vf3 \n"
341  "vmulax.xyzw ACC, vf5, vf4 \n"
342  "vmadday.xyzw ACC, vf6, vf4 \n"
343  "vmaddaz.xyzw ACC, vf7, vf4 \n"
344  "vmaddw.xyzw vf4, vf8, vf4 \n"
345  "sqc2 vf1, 0x00(%0) \n"
346  "sqc2 vf2, 0x10(%0) \n"
347  "sqc2 vf3, 0x20(%0) \n"
348  "sqc2 vf4, 0x30(%0) \n"
349 #endif
350  : : "r" (output), "r" (input0), "r" (input1)
351  : "memory"
352  );
353 #else
354  int i;
355  memset(output, 0, sizeof(MATRIX));
356  for (i = 0; i < 4; i += 1)
357  {
358  int j;
359  for (j = 0; j < 4; j += 1)
360  {
361  int k;
362  for (k = 0; k < 4; k += 1)
363  {
364  output[(4 * i) + k] = input1[(4 * j) + k] * input0[(4 * i) + j];
365  }
366  }
367  }
368 #endif
369  }
370 
371  void matrix_rotate(MATRIX output, MATRIX input0, VECTOR input1) {
372  MATRIX work;
373 
374  // Apply the z-axis rotation.
375  matrix_unit(work);
376  work[0x00] = cosf(input1[2]);
377  work[0x01] = sinf(input1[2]);
378  work[0x04] = -sinf(input1[2]);
379  work[0x05] = cosf(input1[2]);
380  matrix_multiply(output, input0, work);
381 
382  // Apply the y-axis rotation.
383  matrix_unit(work);
384  work[0x00] = cosf(input1[1]);
385  work[0x02] = -sinf(input1[1]);
386  work[0x08] = sinf(input1[1]);
387  work[0x0A] = cosf(input1[1]);
388  matrix_multiply(output, output, work);
389 
390  // Apply the x-axis rotation.
391  matrix_unit(work);
392  work[0x05] = cosf(input1[0]);
393  work[0x06] = sinf(input1[0]);
394  work[0x09] = -sinf(input1[0]);
395  work[0x0A] = cosf(input1[0]);
396  matrix_multiply(output, output, work);
397 
398  }
399 
400  void matrix_scale(MATRIX output, MATRIX input0, VECTOR input1) {
401  MATRIX work;
402 
403  // Apply the scaling.
404  matrix_unit(work);
405  work[0x00] = input1[0];
406  work[0x05] = input1[1];
407  work[0x0A] = input1[2];
408  matrix_multiply(output, input0, work);
409 
410  }
411 
412  void matrix_translate(MATRIX output, MATRIX input0, VECTOR input1) {
413  MATRIX work;
414 
415  // Apply the translation.
416  matrix_unit(work);
417  work[0x0C] = input1[0];
418  work[0x0D] = input1[1];
419  work[0x0E] = input1[2];
420  matrix_multiply(output, input0, work);
421 
422  }
423 
424  void matrix_transpose(MATRIX output, MATRIX input0) {
425  MATRIX work;
426 
427  // Transpose the matrix.
428  work[0x00] = input0[0x00];
429  work[0x01] = input0[0x04];
430  work[0x02] = input0[0x08];
431  work[0x03] = input0[0x0C];
432  work[0x04] = input0[0x01];
433  work[0x05] = input0[0x05];
434  work[0x06] = input0[0x09];
435  work[0x07] = input0[0x0D];
436  work[0x08] = input0[0x02];
437  work[0x09] = input0[0x06];
438  work[0x0A] = input0[0x0A];
439  work[0x0B] = input0[0x0E];
440  work[0x0C] = input0[0x03];
441  work[0x0D] = input0[0x07];
442  work[0x0E] = input0[0x0B];
443  work[0x0F] = input0[0x0F];
444 
445  // Output the result.
446  matrix_copy(output, work);
447 
448  }
449 
450  void matrix_unit(MATRIX output) {
451 
452  // Create a unit matrix.
453  memset(output, 0, sizeof(MATRIX));
454  output[0x00] = 1.00f;
455  output[0x05] = 1.00f;
456  output[0x0A] = 1.00f;
457  output[0x0F] = 1.00f;
458 
459  }
460 
461  /* CREATE FUNCTIONS */
462 
463  void create_local_world(MATRIX local_world, VECTOR translation, VECTOR rotation) {
464 
465  // Create the local_world matrix.
466  matrix_unit(local_world);
467  matrix_rotate(local_world, local_world, rotation);
468  matrix_translate(local_world, local_world, translation);
469 
470  }
471 
472  void create_local_light(MATRIX local_light, VECTOR rotation) {
473 
474  // Create the local_light matrix.
475  matrix_unit(local_light);
476  matrix_rotate(local_light, local_light, rotation);
477 
478  }
479 
480  void create_world_view(MATRIX world_view, VECTOR translation, VECTOR rotation) {
481  VECTOR work0, work1;
482 
483  // Reverse the translation.
484  work0[0] = -translation[0];
485  work0[1] = -translation[1];
486  work0[2] = -translation[2];
487  work0[3] = translation[3];
488 
489  // Reverse the rotation.
490  work1[0] = -rotation[0];
491  work1[1] = -rotation[1];
492  work1[2] = -rotation[2];
493  work1[3] = rotation[3];
494 
495  // Create the world_view matrix.
496  matrix_unit(world_view);
497  matrix_translate(world_view, world_view, work0);
498  matrix_rotate(world_view, world_view, work1);
499 
500  }
501 
502  void create_view_screen(MATRIX view_screen, float aspect, float left, float right, float bottom, float top, float near, float far) {
503 
504  // Apply the aspect ratio adjustment.
505  left = (left * aspect); right = (right * aspect);
506 
507  // Create the view_screen matrix.
508  matrix_unit(view_screen);
509  view_screen[0x00] = (2 * near) / (right - left);
510  view_screen[0x05] = (2 * near) / (top - bottom);
511  view_screen[0x08] = (right + left) / (right - left);
512  view_screen[0x09] = (top + bottom) / (top - bottom);
513  view_screen[0x0A] = (far + near) / (far - near);
514  view_screen[0x0B] = -1.00f;
515  view_screen[0x0E] = (2 * far * near) / (far - near);
516  view_screen[0x0F] = 0.00f;
517 
518  }
519 
520  void create_local_screen(MATRIX local_screen, MATRIX local_world, MATRIX world_view, MATRIX view_screen) {
521 
522  // Create the local_screen matrix.
523  matrix_unit(local_screen);
524  matrix_multiply(local_screen, local_screen, local_world);
525  matrix_multiply(local_screen, local_screen, world_view);
526  matrix_multiply(local_screen, local_screen, view_screen);
527 
528  }
529 
530  /* CALCULATE FUNCTIONS */
531 
532  void calculate_normals(VECTOR *output, int count, VECTOR *normals, MATRIX local_light) {
533 #ifdef _EE
534  __asm__ __volatile__ (
535 #if __GNUC__ > 3
536  "lqc2 $vf1, 0x00(%3) \n"
537  "lqc2 $vf2, 0x10(%3) \n"
538  "lqc2 $vf3, 0x20(%3) \n"
539  "lqc2 $vf4, 0x30(%3) \n"
540  "1: \n"
541  "lqc2 $vf6, 0x00(%2) \n"
542  "vmulaw $ACC, $vf4, $vf0\n"
543  "vmaddax $ACC, $vf1, $vf6\n"
544  "vmadday $ACC, $vf2, $vf6\n"
545  "vmaddz $vf7, $vf3, $vf6\n"
546  "vdiv $Q, $vf0w, $vf7w\n"
547  "vwaitq \n"
548  "vmulq.xyzw $vf7, $vf7, $Q \n"
549  "sqc2 $vf7, 0x00(%0) \n"
550  "addi %0, 0x10 \n"
551  "addi %2, 0x10 \n"
552  "addi %1, -1 \n"
553  "bne $0, %1, 1b \n"
554 #else
555  "lqc2 vf1, 0x00(%3) \n"
556  "lqc2 vf2, 0x10(%3) \n"
557  "lqc2 vf3, 0x20(%3) \n"
558  "lqc2 vf4, 0x30(%3) \n"
559  "1: \n"
560  "lqc2 vf6, 0x00(%2) \n"
561  "vmulaw ACC, vf4, vf0 \n"
562  "vmaddax ACC, vf1, vf6 \n"
563  "vmadday ACC, vf2, vf6 \n"
564  "vmaddz vf7, vf3, vf6 \n"
565  "vdiv Q, vf0w, vf7w \n"
566  "vwaitq \n"
567  "vmulq.xyzw vf7, vf7, Q \n"
568  "sqc2 vf7, 0x00(%0) \n"
569  "addi %0, 0x10 \n"
570  "addi %2, 0x10 \n"
571  "addi %1, -1 \n"
572  "bne $0, %1, 1b \n"
573 #endif
574  : "+r" (output), "+r" (count), "+r" (normals) : "r" (local_light)
575  : "memory"
576  );
577 #else
578  int i;
579  for (i = 0; i < count; i += 1)
580  {
581  int j;
582  memset(output[i], 0, sizeof(output[i]));
583  for (j = 0; j < 4; j += 1)
584  {
585  int k;
586  for (k = 0; k < 4; k += 1)
587  {
588  output[i][k] += local_light[(4 * j) + k] * (j != 3 ? normals[i][j] : 1.0f);
589  }
590  }
591  for (j = 0; j < 4; j += 1)
592  {
593  output[i][j] *= 1.0f / output[i][3];
594  }
595  }
596 #endif
597  }
598 
599  void calculate_lights(VECTOR *output, int count, VECTOR *normals, VECTOR *light_direction, VECTOR *light_colour, const int *light_type, int light_count) {
600  int loop0, loop1; float intensity;
601 
602  // Clear the output values.
603  memset(output, 0, sizeof(VECTOR) * count);
604 
605  // For each normal...
606  for (loop0=0;loop0<count;loop0++) {
607 
608  // For each light...
609  for (loop1=0;loop1<light_count;loop1++) {
610 
611  // If this is an ambient light...
612  if (light_type[loop1] == LIGHT_AMBIENT) {
613 
614  // Set the intensity to full.
615  intensity = 1.00f;
616 
617  // Else, if this is a directional light...
618  } else if (light_type[loop1] == LIGHT_DIRECTIONAL) {
619 
620  // Get the light intensity.
621  intensity = -vector_innerproduct(normals[loop0], light_direction[loop1]);
622 
623  // Clamp the minimum intensity.
624  if (intensity < 0.00f) { intensity = 0.00f; }
625 
626  // Else, this is an invalid light type.
627  } else { intensity = 0.00f; }
628 
629  // If the light has intensity...
630  if (intensity > 0.00f) {
631 
632  // Add the light value.
633  output[loop0][0] += (light_colour[loop1][0] * intensity);
634  output[loop0][1] += (light_colour[loop1][1] * intensity);
635  output[loop0][2] += (light_colour[loop1][2] * intensity);
636  output[loop0][3] = 1.00f;
637 
638  }
639 
640  }
641 
642  }
643 
644  }
645 
646  void calculate_colours(VECTOR *output, int count, VECTOR *colours, VECTOR *lights) {
647  int loop0;
648 
649  // For each colour...
650  for (loop0=0;loop0<count;loop0++) {
651 
652  // Apply the light value to the colour.
653  output[loop0][0] = (colours[loop0][0] * lights[loop0][0]);
654  output[loop0][1] = (colours[loop0][1] * lights[loop0][1]);
655  output[loop0][2] = (colours[loop0][2] * lights[loop0][2]);
656 
657  // Clamp the colour value.
658  vector_clamp(output[loop0], output[loop0], 0.00f, 1.99f);
659 
660  }
661 
662  }
663 
664  void calculate_vertices(VECTOR *output, int count, VECTOR *vertices, MATRIX local_screen) {
665 #ifdef _EE
666  __asm__ __volatile__ (
667 #if __GNUC__ > 3
668  "lqc2 $vf1, 0x00(%3) \n"
669  "lqc2 $vf2, 0x10(%3) \n"
670  "lqc2 $vf3, 0x20(%3) \n"
671  "lqc2 $vf4, 0x30(%3) \n"
672  "1: \n"
673  "lqc2 $vf6, 0x00(%2) \n"
674  "vmulaw $ACC, $vf4, $vf0\n"
675  "vmaddax $ACC, $vf1, $vf6\n"
676  "vmadday $ACC, $vf2, $vf6\n"
677  "vmaddz $vf7, $vf3, $vf6\n"
678  "vclipw.xyz $vf7, $vf7 \n" // FIXME: Clip detection is still kinda broken.
679  "vnop \n"
680  "vnop \n"
681  "vnop \n"
682  "vnop \n"
683  "cfc2 $10, $18 \n"
684  "beq $10, $0, 3f \n"
685  "2: \n"
686  "sqc2 $0, 0x00(%0) \n"
687  "j 4f \n"
688  "3: \n"
689  "vdiv $Q, $vf0w, $vf7w\n"
690  "vwaitq \n"
691  "vmulq.xyz $vf7, $vf7, $Q \n"
692  "sqc2 $vf7, 0x00(%0) \n"
693 #else
694  "lqc2 vf1, 0x00(%3) \n"
695  "lqc2 vf2, 0x10(%3) \n"
696  "lqc2 vf3, 0x20(%3) \n"
697  "lqc2 vf4, 0x30(%3) \n"
698  "1: \n"
699  "lqc2 vf6, 0x00(%2) \n"
700  "vmulaw ACC, vf4, vf0 \n"
701  "vmaddax ACC, vf1, vf6 \n"
702  "vmadday ACC, vf2, vf6 \n"
703  "vmaddz vf7, vf3, vf6 \n"
704  "vclipw.xyz vf7, vf7 \n"
705  "vnop \n"
706  "vnop \n"
707  "vnop \n"
708  "vnop \n" // FIXME: Clip detection is still kinda broken.
709  "cfc2 $10, $18 \n"
710  "beq $10, $0, 3f \n"
711  "2: \n"
712  "sqc2 vi00, 0x00(%0) \n"
713  "j 4f \n"
714  "3: \n"
715  "vdiv Q, vf0w, vf7w \n"
716  "vwaitq \n"
717  "vmulq.xyz vf7, vf7, Q \n"
718  "sqc2 vf7, 0x00(%0) \n"
719 #endif
720  "4: \n"
721  "addi %0, 0x10 \n"
722  "addi %2, 0x10 \n"
723  "addi %1, -1 \n"
724  "bne $0, %1, 1b \n"
725  : "+r" (output), "+r" (count), "+r" (vertices) : "r" (local_screen)
726  : "$10", "memory"
727  );
728 #else
729  int i;
730  for (i = 0; i < count; i += 1)
731  {
732  int j;
733  int clipped;
734  memset(output[i], 0, sizeof(output[i]));
735  for (j = 0; j < 4; j += 1)
736  {
737  int k;
738  for (k = 0; k < 4; k += 1)
739  {
740  output[i][k] += local_screen[(4 * j) + k] * (j != 3 ? vertices[i][j] : 1.0f);
741  }
742  }
743  clipped = 0;
744  for (j = 0; j < 3; j += 1)
745  {
746  // Note on differing behavior: vclipw will shift clip flag to the left 6 bits
747  // However, in this implementation, the leftover high bits are not saved or checked
748  if ((output[i][j] > fabsf(output[i][3])) || (output[i][j] < -fabsf(output[i][3])))
749  {
750  clipped = 1;
751  break;
752  }
753  }
754  if (clipped)
755  {
756  for (j = 0; j < 3; j += 1)
757  {
758  output[i][j] *= 0.0f;
759  }
760  output[i][3] = 1.0f;
761  }
762  else
763  {
764  for (j = 0; j < 3; j += 1)
765  {
766  output[i][j] *= 1.0f / output[i][3];
767  }
768  }
769  }
770 #endif
771  }
vector_normalize
void vector_normalize(VECTOR output, VECTOR input0)
Definition: math3d.c:138
calculate_colours
void calculate_colours(VECTOR *output, int count, VECTOR *colours, VECTOR *lights)
Definition: math3d.c:646
create_local_screen
void create_local_screen(MATRIX local_screen, MATRIX local_world, MATRIX world_view, MATRIX view_screen)
Definition: math3d.c:520
LIGHT_AMBIENT
#define LIGHT_AMBIENT
Definition: math3d.h:115
calculate_vertices
void calculate_vertices(VECTOR *output, int count, VECTOR *vertices, MATRIX local_screen)
Definition: math3d.c:664
matrix_translate
void matrix_translate(MATRIX output, MATRIX input0, VECTOR input1)
Definition: math3d.c:412
LIGHT_DIRECTIONAL
#define LIGHT_DIRECTIONAL
Definition: math3d.h:117
vector_clamp
void vector_clamp(VECTOR output, VECTOR input0, float min, float max)
Definition: math3d.c:63
vector_apply
void vector_apply(VECTOR output, VECTOR input0, MATRIX input1)
Definition: math3d.c:20
vector_outerproduct
void vector_outerproduct(VECTOR output, VECTOR input0, VECTOR input1)
Definition: math3d.c:177
calculate_lights
void calculate_lights(VECTOR *output, int count, VECTOR *normals, VECTOR *light_directions, VECTOR *light_colours, const int *light_types, int light_count)
Definition: math3d.c:599
vector_multiply
void vector_multiply(VECTOR output, VECTOR input0, VECTOR input1)
Definition: math3d.c:124
matrix_rotate
void matrix_rotate(MATRIX output, MATRIX input0, VECTOR input1)
Definition: math3d.c:371
count
u32 count
start sector of fragmented bd/file
Definition: usbhdfsd-common.h:3
create_local_light
void create_local_light(MATRIX local_light, VECTOR rotation)
Definition: math3d.c:472
matrix_copy
void matrix_copy(MATRIX output, MATRIX input0)
Definition: math3d.c:240
vector_triangle_normal
void vector_triangle_normal(VECTOR output, VECTOR a, VECTOR b, VECTOR c)
Definition: math3d.c:224
math3d.h
vector_innerproduct
float vector_innerproduct(VECTOR input0, VECTOR input1)
Definition: math3d.c:104
tamtypes.h
matrix_inverse
void matrix_inverse(MATRIX output, MATRIX input0)
Definition: math3d.c:270
calculate_normals
void calculate_normals(VECTOR *output, int count, VECTOR *normals, MATRIX local_light)
Definition: math3d.c:532
matrix_unit
void matrix_unit(MATRIX output)
Definition: math3d.c:450
graph.h
matrix_multiply
void matrix_multiply(MATRIX output, MATRIX input0, MATRIX input1)
Definition: math3d.c:288
vector_copy
void vector_copy(VECTOR output, VECTOR input0)
Definition: math3d.c:86
create_view_screen
void create_view_screen(MATRIX view_screen, float aspect, float left, float right, float bottom, float top, float near, float far)
Definition: math3d.c:502
matrix_scale
void matrix_scale(MATRIX output, MATRIX input0, VECTOR input1)
Definition: math3d.c:400
vector_cross_product
void vector_cross_product(VECTOR product, VECTOR multiplicand, VECTOR multiplier)
Definition: math3d.c:215
create_local_world
void create_local_world(MATRIX local_world, VECTOR translation, VECTOR rotation)
Definition: math3d.c:463
create_world_view
void create_world_view(MATRIX world_view, VECTOR translation, VECTOR rotation)
Definition: math3d.c:480
vector_add
void vector_add(VECTOR sum, VECTOR addend, VECTOR summand)
Definition: math3d.c:206
matrix_transpose
void matrix_transpose(MATRIX output, MATRIX input0)
Definition: math3d.c:424