cobra.ino 10.0 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439
  1. #include <EEPROM.h>
  2. #include <SPI.h>
  3. #include <GD2.h>
  4. #include "cobra_assets.h"
  5. ////////////////////////////////////////////////////////////////////////////////
  6. // 3D Projection
  7. ////////////////////////////////////////////////////////////////////////////////
  8. static float model_mat[9] = { 1.0, 0.0, 0.0,
  9. 0.0, 1.0, 0.0,
  10. 0.0, 0.0, 1.0 };
  11. static float normal_mat[9] = { 1.0, 0.0, 0.0,
  12. 0.0, 1.0, 0.0,
  13. 0.0, 0.0, 1.0 };
  14. #define M(nm,i,j) ((nm)[3 * (i) + (j)])
  15. void mult_matrices(float *a, float *b, float *c)
  16. {
  17. int i, j, k;
  18. float result[9];
  19. for(i = 0; i < 3; i++) {
  20. for(j = 0; j < 3; j++) {
  21. M(result,i,j) = 0.0f;
  22. for(k = 0; k < 3; k++) {
  23. M(result,i,j) += M(a,i,k) * M(b,k,j);
  24. }
  25. }
  26. }
  27. memcpy(c, result, sizeof(result));
  28. }
  29. // Based on glRotate()
  30. // Returns 3x3 rotation matrix in 'm'
  31. // and its invese in 'mi'
  32. static void rotate(float *m, float *mi, float angle, float *axis)
  33. {
  34. float x = axis[0];
  35. float y = axis[1];
  36. float z = axis[2];
  37. float s = sin(angle);
  38. float c = cos(angle);
  39. float xx = x*x*(1-c);
  40. float xy = x*y*(1-c);
  41. float xz = x*z*(1-c);
  42. float yy = y*y*(1-c);
  43. float yz = y*z*(1-c);
  44. float zz = z*z*(1-c);
  45. float xs = x * s;
  46. float ys = y * s;
  47. float zs = z * s;
  48. m[0] = xx + c;
  49. m[1] = xy - zs;
  50. m[2] = xz + ys;
  51. m[3] = xy + zs;
  52. m[4] = yy + c;
  53. m[5] = yz - xs;
  54. m[6] = xz - ys;
  55. m[7] = yz + xs;
  56. m[8] = zz + c;
  57. mi[0] = m[0];
  58. mi[1] = xy + zs;
  59. mi[2] = xz - ys;
  60. mi[3] = xy - zs;
  61. mi[4] = m[4];
  62. mi[5] = yz + xs;
  63. mi[6] = xz + ys;
  64. mi[7] = yz - xs;
  65. mi[8] = m[8];
  66. }
  67. static void rotation(float angle, float *axis)
  68. {
  69. float mat[9];
  70. float mati[9];
  71. rotate(mat, mati, angle, axis);
  72. mult_matrices(model_mat, mat, model_mat);
  73. mult_matrices(mati, normal_mat, normal_mat);
  74. }
  75. #if 0 // JCB{
  76. class Vector3
  77. {
  78. public:
  79. float x, y, z;
  80. void set(float _x, float _y, float _z) {
  81. x = _x;
  82. y = _y;
  83. z = _z;
  84. }
  85. // functions
  86. void normalize() {
  87. float invLength = 1 / sqrtf(x*x + y*y + z*z);
  88. x *= invLength;
  89. y *= invLength;
  90. z *= invLength;
  91. }
  92. Vector3& operator-=(const Vector3& rhs) {
  93. x -= rhs.x;
  94. y -= rhs.y;
  95. z -= rhs.z;
  96. }
  97. void cross(const Vector3& rhs) {
  98. float _x = y*rhs.z - z*rhs.y;
  99. float _y = z*rhs.x - x*rhs.z;
  100. float _z = x*rhs.y - y*rhs.x;
  101. set(_x, _y, _z);
  102. }
  103. };
  104. #endif
  105. // }JCB
  106. #define N_VERTICES (sizeof(COBRA_vertices) / 3)
  107. typedef struct {
  108. int x, y;
  109. float z;
  110. } xyz;
  111. static xyz projected[N_VERTICES];
  112. void project(float distance)
  113. {
  114. const PROGMEM int8_t *pm = COBRA_vertices;
  115. const PROGMEM int8_t *pm_e = pm + sizeof(COBRA_vertices);
  116. xyz *dst = projected;
  117. int8_t x, y, z;
  118. while (pm < pm_e) {
  119. x = pgm_read_byte_near(pm++);
  120. y = pgm_read_byte_near(pm++);
  121. z = pgm_read_byte_near(pm++);
  122. float xx = x * model_mat[0] + y * model_mat[3] + z * model_mat[6];
  123. float yy = x * model_mat[1] + y * model_mat[4] + z * model_mat[7];
  124. float zz = x * model_mat[2] + y * model_mat[5] + z * model_mat[8] + distance;
  125. float q = 240 / (100 + zz);
  126. dst->x = 16 * (240 + xx * q);
  127. dst->y = 16 * (136 + yy * q);
  128. dst->z = zz;
  129. dst++;
  130. }
  131. }
  132. static void transform_normal(int8_t &nx, int8_t &ny, int8_t &nz)
  133. {
  134. int8_t xx = nx * normal_mat[0] + ny * normal_mat[1] + nz * normal_mat[2];
  135. int8_t yy = nx * normal_mat[3] + ny * normal_mat[4] + nz * normal_mat[5];
  136. int8_t zz = nx * normal_mat[6] + ny * normal_mat[7] + nz * normal_mat[8];
  137. nx = xx;
  138. ny = yy;
  139. nz = zz;
  140. }
  141. #define EDGE_BYTES 5
  142. static byte visible_edges[EDGE_BYTES];
  143. void draw_faces()
  144. {
  145. memset(visible_edges, 0, sizeof(visible_edges));
  146. const PROGMEM int8_t *p = COBRA_faces;
  147. byte n;
  148. int c = 1;
  149. Poly po;
  150. while ((n = pgm_read_byte_near(p++)) != 0xff) {
  151. int8_t nx = pgm_read_byte_near(p++);
  152. int8_t ny = pgm_read_byte_near(p++);
  153. int8_t nz = pgm_read_byte_near(p++);
  154. byte face_edges[EDGE_BYTES];
  155. for (byte i = 0; i < EDGE_BYTES; i++)
  156. face_edges[i] = pgm_read_byte_near(p++);
  157. byte v1 = pgm_read_byte_near(p);
  158. byte v2 = pgm_read_byte_near(p + 1);
  159. byte v3 = pgm_read_byte_near(p + 2);
  160. long x1 = projected[v1].x;
  161. long y1 = projected[v1].y;
  162. long x2 = projected[v2].x;
  163. long y2 = projected[v2].y;
  164. long x3 = projected[v3].x;
  165. long y3 = projected[v3].y;
  166. long area = (x1 - x3) * (y2 - y1) - (x1 - x2) * (y3 - y1);
  167. if (area > 0) {
  168. for (byte i = 0; i < EDGE_BYTES; i++)
  169. visible_edges[i] |= face_edges[i];
  170. po.begin();
  171. for (int i = 0; i < n; i++) {
  172. byte vi = pgm_read_byte_near(p++);
  173. xyz *v = &projected[vi];
  174. po.v(v->x, v->y);
  175. }
  176. {
  177. transform_normal(nx, ny, nz);
  178. uint16_t r = 10, g = 10, b = 20; // Ambient
  179. int d = -ny; // diffuse light from +ve Y
  180. if (d > 0) {
  181. r += d >> 2;
  182. g += d >> 1;
  183. b += d;
  184. }
  185. // use specular half angle
  186. d = ny * -90 + nz * -90; // Range -16384 to +16384
  187. if (d > 8192) {
  188. byte l = pgm_read_byte_near(shiny + ((d - 8192) >> 4));
  189. r += l;
  190. g += l;
  191. b += l;
  192. }
  193. /* JCB{
  194. d = nx;
  195. if (d > 0)
  196. r += d >> 3;
  197. d = -nx;
  198. if (d > 0)
  199. g += d >> 3; }JCB */
  200. GD.ColorRGB(min(255, r), min(255, g), min(255, b));
  201. }
  202. po.draw();
  203. } else {
  204. p += n;
  205. }
  206. c += 1;
  207. }
  208. }
  209. void draw_edges()
  210. {
  211. GD.ColorRGB(0x2e666e);
  212. GD.Begin(LINES);
  213. GD.LineWidth(20);
  214. const PROGMEM uint8_t *p = COBRA_edges;
  215. byte *pvis = visible_edges;
  216. byte vis = 0;
  217. for (byte i = 0; i < sizeof(COBRA_edges) / 2; i++) {
  218. if ((i & 7) == 0)
  219. vis = *pvis++;
  220. byte v0 = pgm_read_byte_near(p++);
  221. byte v1 = pgm_read_byte_near(p++);
  222. if (vis & 1) {
  223. int x0 = projected[v0].x;
  224. int y0 = projected[v0].y;
  225. int x1 = projected[v1].x;
  226. int y1 = projected[v1].y;
  227. GD.Vertex2f(x0,y0);
  228. GD.Vertex2f(x1,y1);
  229. }
  230. vis >>= 1;
  231. }
  232. }
  233. static void draw_navlight(byte nf)
  234. {
  235. float l0z = projected[N_VERTICES - 2].z;
  236. float l1z = projected[N_VERTICES - 1].z;
  237. byte i;
  238. if (nf == 0) // draw the one with smallest z
  239. i = (l0z < l1z) ? (N_VERTICES - 2) : (N_VERTICES - 1);
  240. else
  241. i = (l0z < l1z) ? (N_VERTICES - 1) : (N_VERTICES - 2);
  242. GD.SaveContext();
  243. GD.BlendFunc(SRC_ALPHA, ONE);
  244. GD.Begin(BITMAPS);
  245. GD.BitmapHandle(LIGHT_HANDLE);
  246. GD.ColorRGB((i == N_VERTICES - 2) ? 0xfe2b18 : 0x4fff82);
  247. GD.Vertex2f(projected[i].x - (16 * LIGHT_WIDTH / 2),
  248. projected[i].y - (16 * LIGHT_WIDTH / 2));
  249. GD.RestoreContext();
  250. }
  251. /*****************************************************************/
  252. /* simple trackball-like motion control */
  253. /* Based on projtex.c - by David Yu and David Blythe, SGI */
  254. float angle, axis[3] = {0,1,0};
  255. float lastPos[3];
  256. void
  257. ptov(int x, int y, int width, int height, float v[3])
  258. {
  259. float d, a;
  260. /* project x,y onto a hemi-sphere centered within width, height */
  261. v[0] = (2.0 * x - width) / width;
  262. v[1] = (2.0 * y - height) / height;
  263. d = sqrt(v[0] * v[0] + v[1] * v[1]);
  264. v[2] = cos((M_PI / 2.0) * ((d < 1.0) ? d : 1.0));
  265. a = 1.0 / sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
  266. v[0] *= a;
  267. v[1] *= a;
  268. v[2] *= a;
  269. }
  270. void
  271. startMotion(int x, int y)
  272. {
  273. angle = 0.0;
  274. ptov(x, y, 480, 272, lastPos);
  275. }
  276. void
  277. trackMotion(int x, int y)
  278. {
  279. float curPos[3], dx, dy, dz;
  280. ptov(x, y, 480, 272, curPos);
  281. dx = curPos[0] - lastPos[0];
  282. dy = curPos[1] - lastPos[1];
  283. dz = curPos[2] - lastPos[2];
  284. angle = (M_PI / 2) * sqrt(dx * dx + dy * dy + dz * dz);
  285. axis[0] = lastPos[1] * curPos[2] - lastPos[2] * curPos[1];
  286. axis[1] = lastPos[2] * curPos[0] - lastPos[0] * curPos[2];
  287. axis[2] = lastPos[0] * curPos[1] - lastPos[1] * curPos[0];
  288. float mag = 1 / sqrt(axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]);
  289. axis[0] *= mag;
  290. axis[1] *= mag;
  291. axis[2] *= mag;
  292. lastPos[0] = curPos[0];
  293. lastPos[1] = curPos[1];
  294. lastPos[2] = curPos[2];
  295. }
  296. /*****************************************************************/
  297. void setup()
  298. {
  299. Serial.begin(115200);
  300. GD.begin();
  301. LOAD_ASSETS();
  302. GD.BitmapHandle(BACKGROUND_HANDLE);
  303. GD.BitmapSize(BILINEAR, REPEAT, REPEAT, 480, 272);
  304. #ifdef DUMPDEV // JCB{
  305. // startMotion(240, 136);
  306. // trackMotion(240, 138);
  307. #endif // }JCB
  308. }
  309. static byte prev_touching;
  310. static uint16_t t;
  311. static void draw_sun(int x, int y, int rot)
  312. {
  313. GD.cmd_loadidentity();
  314. GD.cmd_translate(F16(SUN_WIDTH / 2), F16(SUN_WIDTH / 2));
  315. GD.cmd_rotate(rot);
  316. GD.cmd_translate(-F16(SUN_WIDTH / 2), -F16(SUN_WIDTH / 2));
  317. GD.cmd_setmatrix();
  318. GD.Vertex2f(x - (16 * SUN_WIDTH / 2), y - (16 * SUN_WIDTH / 2));
  319. }
  320. void loop()
  321. {
  322. GD.Begin(BITMAPS);
  323. GD.SaveContext();
  324. GD.BitmapHandle(BACKGROUND_HANDLE);
  325. GD.cmd_translate(-(long)t << 14, (long)t << 13);
  326. GD.cmd_rotate(3312);
  327. GD.cmd_setmatrix();
  328. GD.Vertex2ii(0, 0, 0, 0);
  329. GD.RestoreContext();
  330. int et = t - 720;
  331. int sun_x = (480 * 16) - (et << 2),
  332. sun_y = (100 * 16) + (et << 1);
  333. GD.SaveContext();
  334. GD.PointSize(52 * 16);
  335. GD.ColorRGB(0x000000);
  336. GD.Begin(POINTS);
  337. GD.Vertex2f(sun_x, sun_y);
  338. GD.RestoreContext();
  339. GD.SaveContext();
  340. GD.Begin(BITMAPS);
  341. GD.BlendFunc(ONE, ONE);
  342. GD.BitmapHandle(SUN_HANDLE);
  343. GD.ColorRGB(0xb0a090);
  344. draw_sun(sun_x, sun_y, t << 6);
  345. draw_sun(sun_x, sun_y, -t << 6);
  346. GD.RestoreContext();
  347. GD.get_inputs();
  348. byte touching = (GD.inputs.x != -32768);
  349. if (!prev_touching && touching)
  350. startMotion(GD.inputs.x, GD.inputs.y);
  351. else if (touching)
  352. trackMotion(GD.inputs.x, GD.inputs.y);
  353. prev_touching = touching;
  354. unsigned long t0 = micros();
  355. if (angle != 0.0f)
  356. rotation(angle, axis);
  357. project(0);
  358. draw_navlight(1);
  359. draw_faces();
  360. GD.RestoreContext();
  361. draw_edges();
  362. draw_navlight(0);
  363. GD.RestoreContext();
  364. #ifndef DUMPDEV // JCB{
  365. GD.cmd_number(240, 7, 26, OPT_CENTER, micros() - t0);
  366. #else
  367. GD.cmd_number(240, 7, 26, OPT_CENTER, t);
  368. #endif // }JCB
  369. GD.swap();
  370. t++;
  371. }