{ ****************************************** Cordic Demo 2.0.15 - FPGA Image V30 An experiment in autonomous navigation Ozpropdev 18th December 2017 ****************************************** } con sys_clk = 80_000_000 rx_pin = 63 tx_pin = 62 baudrate = 115_200 clocksperbit = float(sys_clk) / float(baudrate) nco = round(clocksperbit * 65536.0) & $FFFFFC00 nco2 = (sys_clk / baudrate) << 16 mode_address = $c0 #0 mode_trails mode_biv edge = 10 '20 size = 1 trail_length = 100 bugz = 12 '38 max init_radius = 120 body_points = 50 '60 body_radius = 8 * size head_points = 30 '40 head_radius = 5 * size feeler_length = 25 * size feeler_angle = 25 leg_length = 10 * size speed = 0 '3 move_step = 1 '6 edge_angle = 3 * 10 react_angle = 10 * 10 react_range = 170 * size cw = edge_angle * 10 ccw = (360 - edge_angle) * 10 #0 _color _body_x _body_y _heading _head_x _head_y _left_feeler_x1 _left_feeler_y1 _left_feeler_x2 _left_feeler_y2 _right_feeler_x1 _right_feeler_y1 _right_feeler_x2 _right_feeler_y2 _closest_bug _entries 'flags #0 _bugz_in_view h_size = 640 v_size = 480 scale = 8 sx = 1 << scale fclk = float(sys_clk) fpix = 25_000_000.0 fset = (fpix / fclk * 2.0) * float($4000_0000) vsync = 0 'vsync pin (all FPGA boards now) tx = 62 rx = 63 #0 video_cog plot_cog main_cog plot2_cog plot3_cog comms_cog = 7 '0000000000000000000000000000000000000000000000000000000000000000 dat org clkset #$ff coginit #plot_cog,##@plot_boot coginit #main_cog,##@main coginit #plot2_cog,##@plot2_boot coginit #comms_cog,##@comms coginit #plot3_cog,##@plot3_boot mov spdx,#0 setxfrq ##round(fset) 'set transfer frequency to 25MHz wrlut ##$00001000,#0 'black wrlut ##$ff000000,#1 'red wrlut ##$00ff0000,#2 'green wrlut ##$0000ff00,#3 'blue wrlut ##$ffff0000,#4 'yellow wrlut ##$00ffff00,#5 'cyan wrlut ##$ff00ff00,#6 'magenta wrlut ##$ffffff00,#7 'white wrlut ##$7fff7f00,#8 'light green wrlut ##$ff7f7f00,#9 'pink wrlut ##$7f7fff00,#10 'light blue wrlut ##$ffcf7f00,#11 wrlut ##$cf7fff00,#12 wrlut ##$7fffcf00,#13 wrlut ##$7fcfff00,#14 wrlut ##$cfff7f00,#15 wrpin dacmode,#0 'enable dac modes in pins 0..3 wrpin dacmode,#1 wrpin dacmode,#2 wrpin dacmode,#3 wrbyte #0,#0 field rdbyte pa,#0 wz bitz pa,#0 wrbyte pa,#0 cogatn #|< plot_cog | |< main_cog if_z rdfast ##(h_size/2)*v_size/64,##video_buffer0 if_nz rdfast ##(h_size/2)*v_size/64,##video_buffer1 mov x,#33 'top blanks call #blank mov x,##v_size 'set visible lines line call #hsync 'do horizontal sync xcont m_rf,#0 'visible line djnz x,#line 'another line? mov x,#10 'bottom blanks call #blank drvnot #vsync 'sync on mov x,#2 'sync blanks call #blank drvnot #vsync 'sync off jmp #field 'loop blank call #hsync 'blank lines xcont m_vi,#0 _ret_ djnz x,#blank hsync xcont m_bs,#0 'horizontal sync xcont m_sn,#1 _ret_ xcont m_bv,#0 dacmode long %0000_0000_000_1010000000000_01_00000_0 m_bs long $CF000000+16 'before sync m_sn long $CF000000+96 'sync m_bv long $CF000000+48 'before visible m_vi long $CF000000+h_size 'visible m_rf long $6F000000+h_size 'visible rlong 4bpp lut x res 1 y res 1 ' '22222222222222222222222222222222222222222222222222222222222222222222222222 orgh $400 org main mov pa,# |< mode_trails wrlong pa,#mode_address call #init_constants wrlut ##-1,#0 mov spdx,#0 waitx ##sys_clk * 4 ddd waitatn incmod spdx,#speed wz if_nz jmp #ddd mov qx,#bugz-1 mov lutx,#0 ccc mov selected_bug,qx call #get_bug_info 'Screen edge detect and react mov react,#0 cmps lfx2,##edge * sx wcz if_b mov react,##cw if_b jmp #rrr cmps lfy2,##edge*sx wcz if_b mov react,##cw if_b jmp #rrr cmp lfx2,##(h_size-edge) * sx wcz if_a mov react,##cw if_a jmp #rrr cmp lfy2,##(v_size-edge) * sx wcz if_a mov react,##cw if_a jmp #rrr cmps rfx2,##edge*sx wcz if_b mov react,##ccw if_b jmp #rrr cmps rfy2,##edge*sx wcz if_b mov react,##ccw if_b jmp #rrr cmp rfx2,##(h_size-edge) * sx wcz if_a mov react,##ccw if_a jmp #rrr cmp rfy2,##(v_size-edge) * sx wcz if_a mov react,##ccw tjnz react,#rrr 'turn away from screen edge 'check stats of other bugz $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$ ' tjnz selected_bug,#mmm 'bug 0 only qfrac bugz_heading,_3600 mov distance,##-1 mov sel_bug2,#bugz-1 bitl flags,#_bugz_in_view getqx hd2 loopk cmp selected_bug,sel_bug2 wz if_e jmp #next_bug 'skip if same bug# 'get bug stats loc ptrb,#@bugz_data mov tmp,sel_bug2 mul tmp,#4 * _entries add ptrb,tmp rdlong bug2x,ptrb[_body_x] rdlong bug2y,ptrb[_body_y] sub bug2x,head_x sub bug2y,head_y qvector bug2x,bug2y getqx pa 'distance to bug getqy pb 'angle 'is bug in forward view qdiv pb,_1deg getqx angle2 sub angle2,bugz_heading wc wrc direction abs angle2 cmp angle2,##1800 wcz if_a subr angle2,_3600 cmp angle2,##feeler_angle * 10 wcz if_be jmp #c_i_v jmp #next_bug 'not in v'iew 'save if closest in view c_i_v cmp pa,distance wcz 'is bug closest so far? if_ae jmp #next_bug mov dir2,direction mov distance,pa mov angle,pb mov closest_bug,sel_bug2 bith flags,#_bugz_in_view next_bug djnf sel_bug2,#loopk testb flags,#_bugz_in_view wc if_c wrlut angle,lutx 'save data for cog#3 if_c add lutx,#1 if_c wrlut head_x,lutx if_c add lutx,#1 if_c wrlut head_y,lutx if_c add lutx,#1 mov react,#0 testb flags,#_bugz_in_view wc if_nc jmp #mmm 'is bug in range? cmp distance,##react_range*sx wcz if_a jmp #mmm 'calculate proportional reaction mov tmp,distance subr tmp,##react_range*sx qmul tmp,##react_angle * 10 getqx react qdiv react,##react_range*sx mov tmp,bugz_heading sub tmp,angle wc wrc direction abs tmp getqx react turn_away testb dir2,#0 wc if_nc subr react,_3600 mmm tjz react,#no_edge 'no turn required 'turn away from edge or bug rrr add bugz_heading,react cmp bugz_heading,_3599 wcz if_a sub bugz_heading,_3600 cmps bugz_heading,#0 wcz if_b add bugz_heading,_3600 call #calc_bug no_edge call #move_bug mov selected_bug,qx call #get_bug_info call #calc_bug all_done djnf qx,#ccc wrlut ##-1,lutx 'mark end of list incmod trail_index,#trail_length-1 jmp #ddd trail_index long 0 direction long 0 lutx long 0 dir2 long 0 spdx long 0 '========================================================== move_bug qfrac bugz_heading,_3600 getqx hd2 qrotate ##move_step*sx,hd2 getqx temp_x getqy temp_y add bugz_x,temp_x add bugz_y,temp_y wrlong bugz_x,ptra[_body_x] wrlong bugz_y,ptra[_body_y] wrlong bugz_heading,ptra[_heading] 'update trail data qmul qx,##trail_length * 4 loc pb,#@trails mov pa,trail_index shl pa,#2 '*4 add pb,pa getqx pa add pb,pa sar bugz_y,#scale sar bugz_x,#scale setword bugz_y,bugz_x,#1 wrlong bugz_y,pb ret '1111111111111111111111111111111111111111111111111111111111111111111111111111 orgh org plot_boot long 0 call #init_constants call #init_bugz call #calc_circles plot_loop waitatn call #clearscreen cogatn #|< plot2_cog | |< plot3_cog call #show_bugz jmp #plot_loop plot cmps x1,##h_size-1 wcz if_a ret cmps y1,##v_size-1 wcz if_a ret testb x1,#31 wc testb y1,#31 orc if_c ret qmul y1,#h_size/2 mov x9,x1 mov y9,y1 mov ptrb,current_buffer testb x1,#0 wc shr x1,#1 add ptrb,x1 getqx y1 add ptrb,y1 rdbyte tempr,ptrb mov x1,x9 mov y1,y9 if_nc setnib tempr,color,#0 if_c setnib tempr,color,#1 wrbyte tempr,ptrb ret 'plot line from x1,y1 to x2,y2 in current color plot_line mov x5,x1 mov y5,y1 mov x1,x2 mov y1,y2 call #plot 'set_pixel mov y1,y5 mov x1,x5 mov x_inc,#1 mov y_inc,#1 mov width,x2 subs width,x1 abs width wc 'get width negc x_inc mov height,y2 subs height,y1 abs height wc 'get height negc y_inc cmp width,height wcz if_b jmp #.pl2 'x > y and width,##$ffff wz if_z mov width,#1 shl height,#10 '*1924 mov temp2,#0 qdiv height,width getqx temp .pl1 call #plot 'set_pixel add x1,x_inc add temp2,temp cmp temp2,_1023 wcz if_a add y1,y_inc if_a sub temp2,_1024 djnz width,#.pl1 ret 'y > x .pl2 and height,##$ffff wz if_z mov height,#1 shl width,#10 '*1024 mov temp2,#0 qdiv width,height getqx temp .pl2a call #plot 'set_pixel add y1,y_inc add temp2,temp cmp temp2,_1023 wcz if_a add x1,x_inc if_a sub temp2,_1024 djnz height,#.pl2a ret orgf $1a0 _3600 long 3600 _3599 long 3599 _1000 long 1000 _1deg long 0 _1024 long 1024 _1023 long 1023 bugz_heading long 0 '450 heading long 0 lut_index long 0 bug_color long 0 hd2 long 0 spacing long 0 spc2 long 0 count long 0 count2 long 0 bugz_x long 0 bugz_y long 0 temp_x long 0 temp_y long 0 kx long 0 ky long 0 kh long 0 loop_count long 200 qx long 0 selected_bug long 0 head_x long 0 head_y long 0 leg_count long 0 sa long 0 sa2 long 0 tmp long 0 react long 0 sel_bug2 long 0 bug2x long 0 bug2y long 0 distance long 0 angle long 0 closest_bug long 0 height long 0 width long 0 x_inc long 0 y_inc long 0 flags long 0 angle2 long 0 radius res 1 points res 1 tempr res 1 x1 res 1 y1 res 1 x2 res 1 y2 res 1 x9 res 1 y9 res 1 temp res 1 temp2 res 1 lfx1 res 1 lfy1 res 1 lfx2 res 1 lfy2 res 1 rfx1 res 1 rfy1 res 1 rfx2 res 1 rfy2 res 1 color res 1 xcount res 1 fa2 res 1 current_buffer res 1 x5 res 1 y5 res 1 mode res 1 'HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH orgh init_constants mov _3600,##3600 qfrac #1,_3600 mov _3599,##3599 mov _1000,##1000 mov _1024,##1024 mov _1023,##1023 getqx _1deg ret clearscreen rdbyte pa,#0 wz if_z loc ptra,#@video_buffer0 if_nz loc ptra,#@video_buffer1 mov current_buffer,ptra setq ##(h_size/8)*v_size _ret_ wrlong #0,ptra calc_circles mov radius,##body_radius * sx mov points,#body_points mov lut_index,#0 call #calc_circle mov radius,##head_radius * sx mov points,#head_points mov lut_index,#body_points*2 call #calc_circle ret get_bug_info call #bug_address rdlong color,ptra[_color] rdlong bug_color,ptra[_color] rdlong bugz_x,ptra[_body_x] rdlong bugz_y,ptra[_body_y] rdlong head_x,ptra[_head_x] rdlong head_y,ptra[_head_y] rdlong lfx1,ptra[_left_feeler_x1] rdlong lfy1,ptra[_left_feeler_y1] rdlong lfx2,ptra[_left_feeler_x2] rdlong lfy2,ptra[_left_feeler_y2] rdlong rfx1,ptra[_right_feeler_x1] rdlong rfy1,ptra[_right_feeler_y1] rdlong rfx2,ptra[_right_feeler_x2] rdlong rfy2,ptra[_right_feeler_y2] rdlong bugz_heading,ptra[_heading] ret plot_bug call #get_bug_info plot_bug_2 call #plot_body call #plot_legs call #plot_head call #plot_left_feeler call #plot_right_feeler ret '========================================================== plot_legs mov sa,##700 mov sa2,##250 call #plot_leg2 mov sa,##2900 mov sa2,##-250 call #plot_leg2 ret plot_leg2 mov leg_count,#4 plot_leg3 mov hd2,bugz_heading sub hd2,sa cmps hd2,#0 wcz if_b add hd2,_3600 cmps hd2,_3599 wcz if_a sub hd2,_3600 qfrac hd2,_3600 getqx hd2 qrotate ##body_radius*sx,hd2 getqx x1 getqy y1 add x1,bugz_x add y1,bugz_y qrotate ##(body_radius+leg_length)*sx,hd2 getqx x2 getqy y2 add x2,bugz_x add y2,bugz_y sar x1,#scale sar y1,#scale sar x2,#scale sar y2,#scale call #plot_line add sa,sa2 djnz leg_count,#plot_leg3 ret '========================================================== plot_left_feeler mov x1,lfx1 mov y1,lfy1 mov x2,lfx2 mov y2,lfy2 jmp #plot_feeler plot_right_feeler mov x1,rfx1 mov y1,rfy1 mov x2,rfx2 mov y2,rfy2 plot_feeler sar x1,#scale sar y1,#scale sar x2,#scale sar y2,#scale call #plot_line ret '************************************************************ plot_head mov count2,#head_points mov lut_index,#body_points*2 .ph_loop rdlut x1,lut_index add lut_index,#1 rdlut y1,lut_index add lut_index,#1 add x1,head_x add y1,head_y sar x1,#scale sar y1,#scale call #plot _ret_ djnz count2,#.ph_loop '========================================================== plot_body mov count2,#body_points mov lut_index,#0 .pb_loop rdlut x1,lut_index add lut_index,#1 rdlut y1,lut_index add lut_index,#1 add x1,bugz_x add y1,bugz_y sar x1,#scale sar y1,#scale call #plot _ret_ djnz count2,#.pb_loop '========================================================== calc_bug qfrac bugz_heading,_3600 getqx hd2 qrotate ##(body_radius+head_radius)*sx,hd2 getqx head_x getqy head_y add head_x,bugz_x add head_y,bugz_y mov hd2,bugz_heading sub hd2,##feeler_angle * 10 wc if_c add hd2,_3600 qfrac hd2,_3600 getqx hd2 qrotate ##head_radius*sx,hd2 getqx lfx1 getqy lfy1 add lfx1,head_x add lfy1,head_y qrotate ##feeler_length*sx,hd2 getqx lfx2 getqy lfy2 add lfx2,head_x add lfy2,head_y mov hd2,bugz_heading add hd2,##feeler_angle * 10 cmps hd2,_3599 wcz if_a sub hd2,_3600 qfrac hd2,_3600 getqx hd2 qrotate ##head_radius*sx,hd2 getqx rfx1 getqy rfy1 add rfx1,head_x add rfy1,head_y qrotate ##feeler_length*sx,hd2 getqx rfx2 getqy rfy2 add rfx2,head_x add rfy2,head_y save_bug wrlong head_x,ptra[_head_x] wrlong head_y,ptra[_head_y] wrlong lfx1,ptra[_left_feeler_x1] wrlong lfy1,ptra[_left_feeler_y1] wrlong lfx2,ptra[_left_feeler_x2] wrlong lfy2,ptra[_left_feeler_y2] wrlong rfx1,ptra[_right_feeler_x1] wrlong rfy1,ptra[_right_feeler_y1] wrlong rfx2,ptra[_right_feeler_x2] wrlong rfy2,ptra[_right_feeler_y2] ret init_bugz qfrac #1,_3600 getqx _1deg qfrac ##feeler_angle*10,_3600 getqx fa2 mov color,#1 'bug #0 = red qdiv _3600,#bugz getqx spacing qfrac spacing,_3600 getqx spc2 mov pb,#0 mov count,#bugz loc ptra,#@bugz_data .ib_loop qrotate ##init_radius * sx,pb getqx bugz_x getqy bugz_y add bugz_x,##h_size/2 * sx add bugz_y,##v_size/2 * sx wrlong color,ptra[_color] wrlong bugz_x,ptra[_body_x] wrlong bugz_y,ptra[_body_y] wrlong bugz_heading,ptra[_heading] call #calc_bug add bugz_heading,spacing cmp bugz_heading,_3599 wcz if_a sub bugz_heading,_3600 add pb,spc2 add ptra,#4 * _entries incmod color,#15 wz if_z add color,#1 djnz count,#.ib_loop ret '========================================================== calc_circle qdiv _3600,points mov pa,#0 getqx pb qfrac pb,_3600 getqx pb .cc_loop qrotate radius,pa add pa,pb getqx temp_x wrlut temp_x,lut_index add lut_index,#1 getqy temp_y wrlut temp_y,lut_index add lut_index,#1 _ret_ djnz points,#.cc_loop plot_border mov color,#7 mov x1,#0 mov y1,#0 mov x2,##h_size mov y2,#0 call #plot_line mov x1,#0 mov y1,##v_size-1 mov x2,##h_size mov y2,##v_size-1 call #plot_line mov x1,#0 mov y1,#0 mov x2,#0 mov y2,##v_size-1 call #plot_line mov x1,##h_size-1 mov y1,#0 mov x2,##h_size-1 mov y2,##v_size-1 call #plot_line ret show_bugz mov xcount,#bugz-1 .sb_loop mov selected_bug,xcount call #plot_bug djnf xcount,#.sb_loop ret bug_address mov tmp,selected_bug loc ptra,#@bugz_data mul selected_bug,#4 *_entries add ptra,selected_bug mov selected_bug,tmp ret 'plot line from x1,y1 to x2,y2 in current color plot_lineh2 mov ptra,#4 wrlong x1,ptra++ wrlong y1,ptra++ wrlong x2,ptra++ wrlong y2,ptra++ mov x1,x3 mov y1,y3 mov x2,x4 mov y2,y4 call #plot_lineh mov ptra,#4 rdlong x1,ptra++ rdlong y1,ptra++ rdlong x2,ptra++ rdlong y2,ptra++ ret plot_lineh mov x5,x1 mov y5,y1 mov x1,x2 mov y1,y2 call #ploth 'set_pixel mov x1,x5 mov y1,y5 mov x_inc,#1 mov y_inc,#1 mov width,x2 subs width,x1 abs width wc 'get width negc x_inc mov height,y2 subs height,y1 abs height wc 'get height negc y_inc cmp width,height wcz if_b jmp #.pl2 'x > y and width,##$1ff wz if_z mov width,#1 shl height,#10 '*1024 mov temp2,#0 qdiv height,width getqx temp .pl1 call #ploth 'set_pixel add x1,x_inc add temp2,temp cmp temp2,_1023 wcz if_a add y1,y_inc if_a sub temp2,_1024 djnz width,#.pl1 mov x1,x5 mov y1,y5 ret 'y > x .pl2 and height,##$1ff wz if_z mov height,#1 shl width,#10 '*1924 mov temp2,#0 qdiv width,height getqx temp .pl2a call #ploth 'set_pixel add y1,y_inc add temp2,temp cmp temp2,_1023 wcz if_a add x1,x_inc if_a sub temp2,_1024 djnz height,#.pl2a mov x1,x5 mov y1,y5 ret ploth cmps x1,##h_size-1 wcz if_a ret cmps y1,##v_size-1 wcz if_a ret cmps x1,#0 wcz if_b ret cmps y1,#0 wcz if_b ret qmul y1,#h_size/2 mov x9,x1 mov y9,y1 mov ptrb,current_buffer testb x1,#0 wc shr x1,#1 add ptrb,x1 getqx y1 add ptrb,y1 rdbyte tempr,ptrb mov x1,x9 mov y1,y9 if_nc setnib tempr,color,#0 if_c setnib tempr,color,#1 wrbyte tempr,ptrb ret '777777777777777777777777777777777777777777777777777777777777777777777777777777777 orgh org comms wrpin #%11111_0,#rx_pin wxpin ##nco | 7,#rx_pin dirh #rx_pin wrpin #%1_11110_0,#tx_pin wxpin ##nco | 7,#tx_pin dirh #tx_pin mov char,#16 call #send_char comms_main mov char,#1 'call #send_char loc ptrb,#@msg2 math_stuff mov r3,#7 '24 mov ptra,#$20 .loopm call #print rdlong r0,ptra++ call #hex8 call #show_dec djnz r3,#.loopm mov char,#13 call #send_char waitx ##sys_clk / 2 jmp #comms_main mov r2,#3 mul r2,#4 * _entries loc ptra,#@bugz_data add ptra,r2 loc ptrb,#@labels mov r3,#_entries .loop call #print rdlong r0,ptra++ call #hex8 djnz r3,#.loop waitx ##sys_clk / 5 jmp #comms_main hex8 mov r1,#8 .loop getnib char,r0,#7 cmp char,#9 wcz if_a add char,#"A"-10 if_be add char,#"0" call #send_char rol r0,#4 djnz r1,#.loop mov char,#32 '13 send_char rdpin status,#tx_pin wc if_c jmp #send_char wypin char,#tx_pin ret wc print rdbyte char,ptrb++ wz if_z ret call #send_char jmp #print show_dec mov r7,#32 mov r6,#0 mov r5,#0 testb r0,#31 wc wrc sign if_c neg r0 .loop getnib char,r5,#1 cmp char,#4 wcz if_a add char,#3 setnib r5,char,#1 getnib char,r5,#0 cmp char,#4 wcz if_a add char,#3 setnib r5,char,#0 rep @.loop2,#8 getnib char,r6,#0 cmp char,#4 wcz if_a add char,#3 setnib r6,char,#0 ror r6,#4 .loop2 rcl r0,#1 wc rcl r6,#1 wc rcl r5,#1 djnz r7,#.loop mov char,#" " testb sign,#0 wc if_c mov char,#"-" call #send_char mov r0,r5 call #hex2 mov r0,r6 call #hex8 mov char,#13 call #send_char ret hex2 getnib char,r0,#1 cmp char,#9 wcz if_a add char,#"A"-10 if_be add char,#"0" call #send_char getnib char,r0,#0 cmp char,#9 wcz if_a add char,#"A"-10 if_be add char,#"0" call #send_char ret sign res 1 r0 res 1 r1 res 1 r2 res 1 r3 res 1 r4 res 1 r5 res 1 r6 res 1 r7 res 1 char res 1 status res 1 '3333333333333333333333333333333333333333333333333333333333333333333333333333333 org plot2_boot long 0 setluts #1 'lut can be written to by cog #2 call #init_constants .loop waitatn rdlong mode,#mode_address testb mode,#mode_biv wc if_nc jmp #.loop mov idx,#0 rdbyte pa,#0 wz if_z loc ptra,#@video_buffer0 if_nz loc ptra,#@video_buffer1 mov current_buffer,ptra .loop2 rdlut v_angle,idx cmp v_angle,##-1 wcz if_e jmp #.loop add idx,#1 rdlut x1,idx add idx,#1 rdlut y1,idx add idx,#1 qrotate ##react_range * sx,v_angle mov color,#2 getqx x2 getqy y2 add x2,x1 add y2,y1 sar x1,#scale sar y1,#scale sar x2,#scale sar y2,#scale call #plot_line2 jmp #.loop2 idx long 0 v_angle long 0 'plot line from x1,y1 to x2,y2 in current color plot_line2 mov x5,x1 mov y5,y1 mov x1,x2 mov y1,y2 call #plot2 'set_pixel mov y1,y5 mov x1,x5 mov x_inc,#1 mov y_inc,#1 mov width,x2 subs width,x1 abs width wc 'get width negc x_inc mov height,y2 subs height,y1 abs height wc 'get height negc y_inc cmp width,height wcz if_b jmp #.pl2 'x > y and width,##$ffff wz if_z mov width,#1 shl height,#10 '*1024 mov temp2,#0 qdiv height,width getqx temp .pl1 call #plot2 'set_pixel add x1,x_inc add temp2,temp cmp temp2,_1023 wcz if_a add y1,y_inc if_a sub temp2,_1024 djnz width,#.pl1 ret 'y > x .pl2 and height,##$ffff wz if_z mov height,#1 shl width,#10 '*1024 mov temp2,#0 qdiv width,height getqx temp .pl2a call #plot2 'set_pixel add y1,y_inc add temp2,temp cmp temp2,_1023 wcz if_a add x1,x_inc if_a sub temp2,_1024 djnz height,#.pl2a ret plot2 cmps x1,##h_size-1 wcz if_a ret cmps y1,##v_size-1 wcz if_a ret testb x1,#31 wc testb y1,#31 orc if_c ret qmul y1,#h_size/2 mov x9,x1 mov y9,y1 mov ptrb,current_buffer testb x1,#0 wc shr x1,#1 add ptrb,x1 getqx y1 add ptrb,y1 rdbyte tempr,ptrb mov x1,x9 mov y1,y9 if_nc setnib tempr,color,#0 if_c setnib tempr,color,#1 wrbyte tempr,ptrb ret '5555555555555555555555555555555555555555555555555555555555555555555555 org plot3_boot long 0 call #init_constants .loop waitatn rdlong mode,#mode_address rdbyte pa,#0 wz if_z loc ptra,#@video_buffer0 if_nz loc ptra,#@video_buffer1 mov current_buffer,ptra rdlong mode,#mode_address mov bug1,#0 .lp2 mov bug2,bug1 add bug2,#1 .lp1 loc ptra,#@bugz_data mov pa,bug1 mul pa,#4*_entries add ptra,pa rdlong pa,ptra[_heading] rdlong x1,ptra[_head_x] rdlong y1,ptra[_head_y] qfrac pa,_3600 getqx pa qrotate ##react_range*sx,pa getqx x2 add x2,x1 sar x1,#scale sar x2,#scale getqy y2 add y2,y1 sar y1,#scale sar y2,#scale mov color,#7 ' call #plot_lineh loc ptra,#@bugz_data mov pa,bug2 mul pa,#4*_entries add ptra,pa rdlong pa,ptra[_heading] rdlong x3,ptra[_head_x] rdlong y3,ptra[_head_y] qfrac pa,_3600 getqx pa qrotate ##react_range*sx,pa getqx x4 add x4,x3 sar x3,#scale sar x4,#scale getqy y4 add y4,y3 sar y3,#scale sar y4,#scale mov color,#7 ' call #plot_lineh2 mov k0,x1 sub k0,x2 mov k1,y1 sub k1,y2 qvector k0,k1 getqx k0 'length cmp k0,##react_range * sx wcz if_be call #math incmod bug2,#bugz-1 wz if_nz jmp #.lp1 incmod bug1,#bugz-2 wz if_nz jmp #.lp2 ' jmp #.loop 'show vapor trails testb mode,#mode_trails wc if_nc jmp #.loop loc pb,#@bugz_data loc ptra,#@trails mov bz,#bugz .loop77 rdlong color,pb ' mov color,#2 mov trc,##trail_length .loop6 rdlong y1,ptra++ getword x1,y1,#1 getword y1,y1,#0 call #ploth djnz trc,#.loop6 add pb,##_entries*4 djnz bz,#.loop77 jmp #.loop trc long 0 bz long 0 'line intersection test ' d = ((y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1)) math ' getct start ' mov ptra,#$20 altr 0,#_y4y3 sub y4,y3 'y4 - y3 altr 0,#_x2x1 sub x2,x1 'x2 - x1 qmul _y4y3,_x2x1 altr 0,#_x4x3 sub x4,x3 'x4 - x3 altr 0,#_y2y1 sub y2,y1 'y2 - y1 getqx d0 qmul _x4x3,_y2y1 getqx d1 subs d0,d1 ' wrlong d0,ptra++ abs d0 wc wrc d_sign 'save sign ' getqx d1 ' ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3)) / d altr 0,#_y1y3 sub y1,y3 'y1 - y3 qmul _x4x3,_y1y3 altr 0,#_x1x3 sub x1,x3 'x1 - x3 qmul _y4y3,_x1x3 getqx ua0 getqx ua1 sub ua0,ua1 ' wrlong ua0,ptra++ qmul ua0,_10_000 getqx ua ' wrlong ua,ptra++ abs ua wc wrc ua_sign qdiv ua,d0 getqx k0 testb d_sign,#0 xorc negc k0 ' ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3)) / d qmul _x2x1,_y1y3 qmul _y2y1,_x1x3 getqx ub0 getqx ub1 sub ub0,ub1 ' wrlong ub0,ptra++ qmul ub0,_10_000 getqx ub ' wrlong ub,ptra++ abs ub wc wrc ub_sign qdiv ub,d0 getqx k1 testb d_sign,#0 xorc negc k1 'if ua > 0 and ua < 1000 and ub > 0 and ub < 1000 'intersects cmps k0,#1 wcz if_b jmp #not_intersect cmps k1,#1 wcz if_b jmp #not_intersect cmps k0,_9999 wcz if_a ret cmps k1,_9999 wcz if_a ret ' lx = x1 + ua * (x2 - x1) ' ly = y1 + ua * (y2 - y1) qmul _x2x1,k0 qmul _y2y1,k0 getqx k0 abs k0 wc wrc k0_sign 'save sign getqx k1 abs k1 wc wrc k1_sign 'save sign qdiv k0,_10_000 qdiv k1,_10_000 getqx k0 testb k0_sign,#0 wc negc k0 getqx k1 testb k1_sign,#0 wc negc k1 altr 0,#lx add x1,k0 altr 0,#ly add y1,k1 '********* ' getct finish ' sub finish,start ' wrlong finish,ptra++ ' qmul finish,#125 ' getqx finish ' qdiv finish,#10 ' getqx finish ' wrlong finish,ptra++ mov x1,lx mov y1,ly sub y1,#5 mov x2,lx mov y2,ly add y2,#5 mov color,#7 call #plot_lineh mov x1,lx sub x1,#5 mov y1,ly mov x2,lx add x2,#5 mov y2,ly mov color,#7 call #plot_lineh not_intersect ' wrlong ##-1,ptra++ ret uaq long 0 ubq long 0 ua_sign long 0 ub_sign long 0 _9999 long 9999 _10_000 long 10_000 start res 1 finish res 1 intersect res 1 k0_sign res 1 k1_sign res 1 lx res 1 ly res 1 ua res 1 ub res 1 x3 res 1 x4 res 1 y3 res 1 y4 res 1 d0 res 1 d1 res 1 _y4y3 res 1 _y2y1 res 1 _x4x3 res 1 _x2x1 res 1 _y1y2 res 1 _x1x2 res 1 _y1y3 res 1 _x1x3 res 1 ua0 res 1 ua1 res 1 ub0 res 1 ub1 res 1 k0 res 1 k1 res 1 d_sign res 1 bug1 res 1 bug2 res 1 orgh labels byte "color ",0 byte "body_x ",0 byte "body_y ",0 byte "heading ",0 byte "head_x ",0 byte "head_y ",0 byte "left_feeler_x1 ",0 byte "left_feeler_y1 ",0 byte "left_feeler_x2 ",0 byte "left_feeler_y2 ",0 byte "right_feeler_x1 ",0 byte "right_feeler_y1 ",0 byte "right_feeler_x2 ",0 byte "right_feeler_y2 ",0 byte "closest_bug ",0 msg2 byte "d= ",0 byte "ua= ",0 byte "ua*10000= ",0 byte "ua/d= ",0 byte "ub= ",0 byte "ub*10000= ",0 byte "ub/d= ",0 byte "intersects ",0 byte "x2x1= ",0 byte "y2y1= ",0 byte "x offset ",0 byte "y offset ",0 byte "lx = ",0 byte "ly = ",0 byte "cycles = ",0 byte "elapsed = ",0 byte " ",0 byte " ",0 byte " ",0 'Vector font primary table word $5A4B '0 : 11:587 word $8A56 '1 : 17:598 word $6267 '2 : 12:615 word $4273 '3 : 8:627 word $527B '4 : 10:635 word $6285 '5 : 12:645 word $7291 '6 : 14:657 word $4A9F '7 : 9:671 word $52A8 '8 : 10:680 word $42B2 '9 : 8:690 word $42BA '10 : 8:698 word $32C2 '11 : 6:706 word $3AC8 '12 : 7:712 word $5ACF '13 : 11:719 word $12DA '14 : 2:730 word $22DC '15 : 4:732 word $5AE0 '16 : 11:736 word $3AEB '17 : 7:747 word $4AF2 '18 : 9:754 word $430 '19 : 8:768 word $338 '20 : 6:776 word $53E '21 : 10:782 word $7318 '22 : 14:792 word $7326 '23 : 14:806 word $3334 '24 : 6:820 word $433A '25 : 8:826 word $3342 '26 : 6:834 word $5348 '27 : 10:840 word $2352 '28 : 4:850 word $3356 '29 : 6:854 word $4B5C '30 : 9:860 word $3365 '31 : 6:869 word $B6B '32 : 1:875 word $58F5 '33 : 11:245 word $616 '34 : 12:262 word $4112 '35 : 8:274 word $7116 '36 : 14:278 word $4924 '37 : 9:292 word $612D '38 : 12:301 word $310 '39 : 6:256 word $3143 '40 : 6:323 word $3149 '41 : 6:329 word $3139 '42 : 6:313 word $313D '43 : 6:317 word $38F3 '44 : 7:243 word $113D '45 : 2:317 word $28F5 '46 : 5:245 word $114F '47 : 2:335 word $6893 '48 : 13:147 word $48A5 '49 : 9:165 word $58AE '50 : 11:174 word $20B9 '51 : 4:185 word $48BD '52 : 9:189 word $58CE '53 : 11:206 word $28D9 '54 : 5:217 word $88C6 '55 : 17:198 word $58DE '56 : 11:222 word $50E9 '57 : 10:233 word $60EE '58 : 12:238 word $1958 '59 : 3:344 word $2151 '60 : 4:337 word $1955 '61 : 3:341 word $615B '62 : 12:347 word $9967 '63 : 19:359 word $400 '64 : 8:0 word $608 '65 : 12:8 word $4017 '66 : 8:23 word $3822 '67 : 7:34 word $3029 '68 : 6:41 word $2829 '69 : 5:41 word $5814 '70 : 11:20 word $302F '71 : 6:47 word $4035 '72 : 8:53 word $303D '73 : 6:61 word $3043 '74 : 6:67 word $1849 '75 : 3:73 word $304C '76 : 6:76 word $2052 '77 : 4:82 word $4817 '78 : 9:23 word $3856 '79 : 7:86 word $5817 '80 : 11:23 word $4856 '81 : 9:86 word $605F '82 : 12:95 word $306B '83 : 6:107 word $3071 '84 : 6:113 word $2077 '85 : 4:119 word $307B '86 : 6:123 word $2081 '87 : 4:129 word $4085 '88 : 8:133 word $308D '89 : 6:141 word $217A '90 : 4:378 word $117E '91 : 2:382 word $2180 '92 : 4:384 word $1984 '93 : 3:388 word $1187 '94 : 2:391 word $1189 '95 : 2:393 word $518B '96 : 10:395 word $5197 '97 : 10:407 word $418D '98 : 8:397 word $21A2 '99 : 4:418 word $518D '100 : 10:397 word $41A6 '101 : 8:422 word $39AE '102 : 7:430 word $39B5 '103 : 7:437 word $51BC '104 : 10:444 word $51BE '105 : 10:446 word $31C8 '106 : 6:456 word $29CE '107 : 5:462 word $61D3 '108 : 12:467 word $21DF '109 : 4:479 word $4999 '110 : 9:409 word $21E3 '111 : 4:483 word $21E7 '112 : 4:487 word $31EB '113 : 6:491 word $61F1 '114 : 12:497 word $320 '115 : 6:512 word $3A6 '116 : 7:518 word $22D '117 : 4:525 word $2A11 '118 : 5:529 word $2216 '119 : 4:534 word $3A1A '120 : 7:538 word $3221 '121 : 6:545 word $3A27 '122 : 7:551 word $1235 '123 : 2:565 word $3A2E '124 : 7:558 word $2237 '125 : 4:567 word $823B '126 : 16:571 bugz_data long 0[bugz * _entries] trails long 0[bugz * trail_length] alignl video_buffer0 long 0[(h_size / 8) * v_size] video_buffer1 long 0[(h_size / 8) * v_size]