collision.ino 3.7 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180
  1. #include <SPI.h>
  2. #include <GD.h>
  3. void readn(byte *dst, unsigned int addr, int c)
  4. {
  5. GD.__start(addr);
  6. while (c--)
  7. *dst++ = SPI.transfer(0);
  8. GD.__end();
  9. }
  10. #define NBALLS 80
  11. static byte coll[NBALLS];
  12. static void load_coll()
  13. {
  14. while (GD.rd(VBLANK) == 0) // Wait until vblank
  15. ;
  16. while (GD.rd(VBLANK) == 1) // Wait until display
  17. ;
  18. while (GD.rd(VBLANK) == 0) // Wait until vblank
  19. ;
  20. readn(coll, COLLISION, NBALLS);
  21. }
  22. struct ball {
  23. int x, y;
  24. char vx, vy;
  25. byte lasthit;
  26. };
  27. static struct ball balls[NBALLS];
  28. #include "stone_wall_texture.h" // texture from 3dmd.net project
  29. #include "sphere.h"
  30. static void plot_balls()
  31. {
  32. byte i;
  33. for (i = 0; i < NBALLS; i++)
  34. GD.sprite(i, balls[i].x >> 4, balls[i].y >> 4, 0, 0, 0);
  35. }
  36. // Place all balls so that none collide. Do this by placing all at
  37. // random, then moving until there are no collisions
  38. static byte anycolliding()
  39. {
  40. plot_balls();
  41. load_coll();
  42. byte i;
  43. for (i = 0; i < NBALLS; i++)
  44. if (coll[i] != 0xff)
  45. return 1;
  46. return 0;
  47. }
  48. static void place_balls()
  49. {
  50. byte i;
  51. for (i = 0; i < NBALLS; i++) {
  52. balls[i].x = (2 + random(380)) << 4;
  53. balls[i].y = (2 + random(280)) << 4;
  54. balls[i].vx = random(-128,127);
  55. balls[i].vy = random(-128,127);
  56. balls[i].lasthit = 255;
  57. }
  58. while (anycolliding()) {
  59. for (i = 0; i < NBALLS; i++) {
  60. if (coll[i] != 0xff) {
  61. balls[i].x = (2 + random(380)) << 4;
  62. balls[i].y = (2 + random(280)) << 4;
  63. }
  64. }
  65. }
  66. }
  67. void setup()
  68. {
  69. int i;
  70. GD.begin();
  71. GD.wr(JK_MODE, 0);
  72. GD.copy(RAM_CHR, stone_wall_texture_chr, sizeof(stone_wall_texture_chr));
  73. GD.copy(RAM_PAL, stone_wall_texture_pal, sizeof(stone_wall_texture_pal));
  74. for (i = 0; i < 4096; i++)
  75. GD.wr(RAM_PIC + i, (i & 15) + ((i >> 6) << 4));
  76. GD.copy(RAM_SPRIMG, sphere_img, sizeof(sphere_img));
  77. GD.copy(RAM_SPRPAL, sphere_pal, sizeof(sphere_pal));
  78. for (i = 0; i < 256; i++)
  79. GD.sprite(i, 400, 400, 0, 0, 0);
  80. place_balls();
  81. }
  82. float dot(float x1, float y1, float x2, float y2)
  83. {
  84. return (x1 * x2) + (y1 * y2);
  85. }
  86. // Collide ball a with ball b, compute new velocities.
  87. // Algorithm from
  88. // http://stackoverflow.com/questions/345838/ball-to-ball-collision-detection-and-handling
  89. void collide(struct ball *a, struct ball *b)
  90. {
  91. float collision_x, collision_y;
  92. collision_x = a->x - b->x;
  93. collision_y = a->y - b->y;
  94. float distance = sqrt(collision_x * collision_x + collision_y * collision_y);
  95. float rdistance = 1.0 / distance;
  96. collision_x *= rdistance;
  97. collision_y *= rdistance;
  98. float aci = dot(a->vx, a->vy, collision_x, collision_y);
  99. float bci = dot(b->vx, b->vy, collision_x, collision_y);
  100. float acf = bci;
  101. float bcf = aci;
  102. a->vx += int((acf - aci) * collision_x);
  103. a->vy += int((acf - aci) * collision_y);
  104. b->vx += int((bcf - bci) * collision_x);
  105. b->vy += int((bcf - bci) * collision_y);
  106. }
  107. #define LWALL (0 << 4)
  108. #define RWALL (384 << 4)
  109. #define TWALL (0 << 4)
  110. #define BWALL (284 << 4)
  111. static int timer;
  112. void loop()
  113. {
  114. int i;
  115. plot_balls();
  116. load_coll();
  117. struct ball *pb;
  118. for (i = NBALLS, pb = balls; i; pb++, i--) {
  119. if ((pb->x <= LWALL)) {
  120. pb->x = LWALL;
  121. pb->vx = -pb->vx;
  122. }
  123. if ((pb->x >= RWALL)) {
  124. pb->x = RWALL;
  125. pb->vx = -pb->vx;
  126. }
  127. if ((pb->y <= TWALL)) {
  128. pb->y = TWALL;
  129. pb->vy = -pb->vy;
  130. }
  131. if ((pb->y >= BWALL)) {
  132. pb->y = BWALL;
  133. pb->vy = -pb->vy;
  134. }
  135. }
  136. for (i = 1; i < NBALLS; i++) {
  137. byte other = coll[i];
  138. if ((balls[i].lasthit != other) && other != 0xff) {
  139. collide(&balls[i], &balls[other]);
  140. }
  141. balls[i].lasthit = other;
  142. }
  143. for (i = NBALLS, pb = balls; i; pb++, i--) {
  144. pb->x += pb->vx;
  145. pb->y += pb->vy;
  146. }
  147. if (++timer == 2000) {
  148. place_balls();
  149. delay(1000);
  150. timer = 0;
  151. }
  152. }