10 '
   20 ' 3D view of a cube using the Arcade Shield, LCD Shield,
   30 ' and the CoreMPU.
   40 ' Based on the Wikipedia article:
   50 '   http://en.wikipedia.org/wiki/3D_projection
   60 '
   70 '
   80 ' Working storage
   90 DIM BX(450), BY(450)
  100 '
  110 ' Determine what we are running on
  120 PC = CORE.MODEL = "SOLDERCORE-V1-EMULATOR"
  130 '
  140 ' Emulator or hardware?
  150 IF PC THEN
  160   ' Emulator: use the emulator's graphics
  170   LCD = TRUE
  180   '
  190   ' Use the InvenSense MPU-9150 SDK wireless board.
  200   ' Edit the port for your Bluetooth COM port.
  210   INSTALL "INVENSENSE-MPU-9150-SDK PORT=23" AS MPU
  220 ELSE
  230   ' SolderCore: use either a SolderCore LCD Shield
  240   ' or a SolderCore Arcade Shield.
  250   INSTALL "SOLDERCORE-GRAPHICS-SHIELD" AS G
  260   '
  270   ' Crank SPI speed to 40MHz.  If you have more than a
  280   ' simple setup, 40MHz might be too fast, so stick with
  290   ' the default 20MHz.
  300   G.SPEED = 40000000
  310   '
  320   ' Determine if we've installed an LCD or Arcade.
  330   LCD = G.MODEL = "LCD-SHIELD-V1"
  340   '
  350   ' Use a nunchuck to control things.
  360   '  INSTALL "NINTENDO-NUNCHUK" AS WII
  370   '
  380   ' Use a CoreMPU and an AHRS to fuse the sensors.
  390   INSTALL "CORE-MPU" AS MPU
  400   INSTALL "AHRS" USING MPU AS AHRS
  410   '
  420   ' Increase gyro range to 2000 dps if you're going to be violent.
  430   ' MPU.RANGE(1) = 2000
  440   '
  450 ENDIF
  460 '
  470 ' Nice clean display.
  480 CLG
  490 '
  500 ' Slab vertices.
  510 AX = [ + 1,  + 1,  - 1,  - 1,  + 1,  + 1,  - 1,  - 1] * 2
  520 AY = [ - 1,  - 1,  - 1,  - 1,  + 1,  + 1,  + 1,  + 1] * 0.25
  530 AZ = [ - 1,  + 1,  + 1,  - 1,  - 1,  + 1,  + 1,  - 1]
  540 A = []
  550 FOR I = 0 TO HIGH AX
  560   A = A & [QUAT(0, AX(I), AY(I), AZ(I))]
  570 NEXT I
  580 '
  590 ' Position camera of camera; vector part is 3D position.
  600 C = QUAT(0, 0, 0, 5)
  610 '
  620 ' Create plane.  Say "IF 0" if you just want the cube
  630 IF 1 THEN
  640   FOR X =  - 4 TO 4 IN 5
  650     FOR Z =  - 4 TO 4 IN 5
  660       A = A & [QUAT(0, X, 2, Z)]
  670     NEXT Z
  680   NEXT X
  690 ENDIF
  700 '
  710 ' Keep going until we press the Z button.
  720 WHILE TIMER < 10000 ' WII.Z = 0
  730   ' Move camera position using Nunchuk joystick
  740   ' IF WII.H < - 0.5 THEN C = C + [0.1, 0, 0]
  750   ' IF WII.H > 0.5 THEN C = C + [- 0.1, 0, 0]
  760   ' IF WII.V < - 0.5 THEN C = C + [0, 0, 0.1]
  770   ' IF WII.V > 0.5 THEN C = C + [0, 0, - 0.1]
  790   CALL RENDER
  800   '
  810   ' Show how many frames per second we're generating.
  820   CALL SHOW_FPS
  830 WEND
  840 '
  850 ' All done.
  860 END
  870 '
  880 DEFPROC SHOW_FPS
  890   ' Figure out how many FPS we're running.
  900   T1 = CORE.TICK
  910   FPS = INT(CORE.FREQUENCY / (T1 - T0))
  920   T0 = T1
  930   MOVE 10, 200
  940   FONT "ROMAN-SIMPLEX"
  950   DRAW STR(FPS) + " FPS"
  960 ENDPROC
  970 '
  980 DEFPROC RENDER
  990   '
 1000   ' Use quaternion output of AHRS driver to rotate.
 1010   Q = AHRS.Q
 1020   '
 1030   ' Transform sensor frame of reference to view frame of reference.
 1040   Q = QUAT(Q.R, Q.I, -Q.K, Q.J)
 1050   '
 1060   ' Project all 3D points onto view
 1070   FOR I = 0 TO HIGH A
 1080     D = C - Q * A(I) * CNJ Q
 1090     BX(I) = GFX.WIDTH / 2 + (GFX.WIDTH / 2) * D.I / D.K
 1100     BY(I) = GFX.HEIGHT / 2 + (GFX.HEIGHT / 2) * D.J / D.K
 1110   NEXT I
 1120   '
 1130   ' Display cube and plane
 1140   CLG
 1150   COLOR IFF(LCD, %WHITE, 15)
 1160   FOR I = 0 TO 3
 1170     J = (I + 1) MOD 4
 1180     LINE BX(I), BY(I) TO BX(J), BY(J)
 1190     LINE BX(4 + I), BY(4 + I) TO BX(4 + J), BY(4 + J)
 1200     LINE BX(I), BY(I) TO BX(4 + I), BY(4 + I)
 1210   NEXT I
 1220   COLOR IFF(LCD, %GREEN, 2)
 1230   FOR I = 8 TO HIGH A
 1240     CIRCLE BX(I), BY(I), 2
 1250   NEXT I
 1260 ENDPROC