Solid spinning cubes on textured background

cubes.py

from LCD_3inch5 import LCD_3inch5
from machine import Pin
import framebuf
from time import sleep_ms, sleep_us, ticks_diff, ticks_us, sleep
from micropython import const
import array
from usys import exit
import gc
#from random import randint
from math import sin,cos,pi,radians

MAXSCREEN_X = const(200)
MAXSCREEN_Y = const(200)
NUM_TRIANGLES = const(12)
NUM_POINTS = const(8)
SCALE = const(14)
CUBE_SIZE = const(60) 
TEXTURE_HEIGHT = const(47)
TEXTURE_WIDTH  = const(90)

xcoord=array.array('b', 0 for _ in range(NUM_POINTS))
ycoord=array.array('b', 0 for _ in range(NUM_POINTS))
zcoord=array.array('b', 0 for _ in range(NUM_POINTS))

rot_x=array.array('i', 0 for _ in range(NUM_POINTS))
rot_y=array.array('i', 0 for _ in range(NUM_POINTS))
rot_z=array.array('i', 0 for _ in range(NUM_POINTS))

tri_a=array.array('b', 0 for _ in range(NUM_TRIANGLES))
tri_b=array.array('b', 0 for _ in range(NUM_TRIANGLES))
tri_c=array.array('b', 0 for _ in range(NUM_TRIANGLES))

edgebuf_l=array.array('h', 0 for _ in range(MAXSCREEN_Y))
edgebuf_r=array.array('h', 0 for _ in range(MAXSCREEN_Y))
zbuffer=bytearray(MAXSCREEN_X*MAXSCREEN_Y)

isin=array.array('i',range(0,361))
icos=array.array('i',range(0,361))

# blit_image_file borrowed from Stewart Watkiss
# http://www.penguintutor.com/programming/picodisplayanimations
def blit_image_file(buf,filename,width,height,cw,ch): # file width, file height, char width, char height
    with open (filename, "rb") as file:
        file_position = 0
        char_position = 0
        ecount = 0
        current_byte = file.read(4) # header
        while file_position < (width * height * 2):
            current_byte = file.read(1)
            # if eof
            if len(current_byte) == 0:
                break
            # copy to buffer
            buf[char_position] = ord(current_byte)
            char_position += 1
            file_position += 1
            if char_position == (cw * ch* 2):
                char_position = 0
    file.close()


@micropython.viper
def draw_line( x1:int, y1:int, x2:int, y2:int , z1:int,z2:int,color:int):
    width=MAXSCREEN_X
    dest=ptr16(screen)
    ebufl=ptr16(edgebuf_l)
    ebufr=ptr16(edgebuf_r)
    zbuf=ptr8(zbuffer)
    color_addr=ptr16(color_l2)
    zspan=z1-z2 
    yLonger=False  
    shortLen=y2-y1 
    longLen=x2-x1 
    if (abs(shortLen)>abs(longLen)):
        swap=shortLen 
        shortLen=longLen
        longLen=swap
        yLonger=True
    if (longLen==0):
        decInc=0
        zdecInc=0
    else:
        decInc = (shortLen << 16) // longLen
        zdecInc = (zspan << 16) // longLen
        #print(shortLen,zspan)
    if z1==z2:     # hack...fix
        if z1==47:
            z2=0
        else:
            z2=47
    if (yLonger):
        if (longLen>0):
            longLen+=y1
            j=0x8000+(x1<<16)
            j2=0x8000+(z2<<16)
            while y1<=longLen:
                x2=j>>16
                i = y1 * width + x2
                #dest[i]=color_addr[j2>>16]
                if x2<ebufl[y1]:
                    ebufl[y1] = x2
                if x2>ebufr[y1]:
                    ebufr[y1] = x2
                zbuf[i]=j2>>16
                #print(j2>>16)
                j2+=zdecInc
                j+=decInc
                y1+=1 
            return
        longLen+=y1
        j=0x8000+(x1<<16)
        j2=0x8000+(z2<<16)
        while y1>=longLen:
            x2=j>>16
            i = y1 * width + x2
            #dest[i]=color_addr[j2>>16]
            if x2<ebufl[y1]:
                ebufl[y1] = x2
            if x2>ebufr[y1]:
                ebufr[y1] = x2
            zbuf[i]=j2>>16                 # working ?
            #print(j2>>16)
            j2-=zdecInc
            j-=decInc
            y1-=1
        return   
    if (longLen>0):
        longLen+=x1
        j=0x8000+(y1<<16)
        j2=0x8000+(z2<<16)
        while x1<=longLen:
            y2=j>>16
            i = y2 * width + x1
            #dest[i]=color_addr[j2>>16]
            if x1<ebufl[y2]:
                ebufl[y2] = x1
            if x1>ebufr[y2]:
                ebufr[y2] = x1
            zbuf[i]=j2>>16
            #print((j2>>16)+0)
            j2+=zdecInc
            j+=decInc
            x1+=1
        return   
    longLen+=x1
    j=0x8000+(y1<<16)
    j2=0x8000+(z2<<16)
    while x1>=longLen:
        y2=j>>16
        i = y2 * width + x1   
        #dest[i]=color_addr[j2>>16]
        if x1<ebufl[y2]:
            ebufl[y2] = x1
        if x1>ebufr[y2]:
            ebufr[y2] = x1
        zbuf[i]=j2>>16                 # working ?
        #print(j2>>16,zdecInc)
        j2-=zdecInc
        j-=decInc
        x1-=1

@micropython.viper
def draw_zbuf():               #test routine
    ebufl=ptr16(edgebuf_l)
    ebufr=ptr16(edgebuf_r)
    zbuf=ptr8(zbuffer)
    screen_addr=ptr16(screen)
    color=ptr16(color_l2)
    for y in range(MAXSCREEN_Y):                    # scan top to bottom of screen
        for x in range(MAXSCREEN_X):
            if ebufl[y]<MAXSCREEN_X:                    # is triangle on this row?
                for x in range(ebufl[y],ebufr[y]):    # from left edge to right edge
                    i=y*MAXSCREEN_X+x
                    screen_addr[i]=color[zbuf[i]]
   
   
@micropython.viper
def texture_background():
    screen_addr=ptr16(screen)
    texture_addr=ptr16(texture_wood)
    for y in range(MAXSCREEN_Y):                  
        for x in range(MAXSCREEN_X):
            i=y*MAXSCREEN_X+x
            screen_addr[i]=texture_addr[TEXTURE_WIDTH*(y%(TEXTURE_HEIGHT-1)) + (x%(TEXTURE_WIDTH-1))]

@micropython.asm_thumb
def background_asm(r0,r1,r2,r3): # r0=screen, r1=texture, r2=height, r3=width
    label(HEIGHT_LOOP)
    mov(r4,r3)          # r4=x    
    label(WIDTH_LOOP)    
    mov(r5,r2)           # r5 = texture_y <- screen_y
    label(Y_TEST)
    cmp(r5,47)           # screen y less than texture y
    blt(Y_OK)
    sub(r5,47)
    b(Y_TEST)
    label(Y_OK)
    mov(r7,r4)           # r7 = texture_x <- screen_x
    label(X_TEST)
    cmp(r7,90)           # screen x less than texture x
    blt(X_OK)
    sub(r7,90)
    b(X_TEST)
    label(X_OK)   
    mov(r6,90)
    mul(r5,r6)           # y * width
    add(r5,r5,r7)        # y * width + x
    add(r5,r5,r5)        # double for 2 bytes per pixel
    add(r5,r5,r1)        # source[y * width + x]
    ldrh(r6, [r5, 0])    # r6 = texture   
    mov(r7,r2)           # r2=y
    mov(r5,r3)
    mul(r7,r5)           # y * width
    add(r7,r7,r4)        # y * width + x
    add(r7,r7,r7)        # double for 2 bytes per pixel
    add(r7,r7,r0)        # dest[y * width + x]    
    strh(r6, [r7, 0])
    sub(r4, 1)          # sub width by 2
    bgt(WIDTH_LOOP)
    sub(r2, 1)          # sub height by 1
    bgt(HEIGHT_LOOP)
    label(EXIT)
    mov(r0,r3)

@micropython.viper
def fill_triangle(solid_color:int):
    ebufl=ptr16(edgebuf_l)
    ebufr=ptr16(edgebuf_r)
    zbuf=ptr8(zbuffer)
    screen_addr=ptr16(screen)
    color=ptr16(color_l2)
    texture_addr=ptr16(texture_marble)
    for y in range(MAXSCREEN_Y):                    # scan top to bottom of screen
        if ebufl[y]<MAXSCREEN_X and ebufl[y]>0:     # is triangle on this row?
#             z1=zbuf[y*MAXSCREEN_X+ebufl[y]]         # left line z value
#             z2=zbuf[y*MAXSCREEN_X+ebufr[y]]         # right line z value
#             xlen=ebufr[y]-ebufl[y]                  # length of row
#             zspan=int(abs((z1)-(z2)))               # span of z across triangle
#             if z1>z2:
#                 sign = -1
#             else:
#                 sign = 1
#             if zspan == 0:
#                 zinc = 0   
#             else:
#                 zinc=(zspan<<16) // xlen            # get increment of z per pixel
#             j=0x8000+(z1<<16)                       # left line zbuf start
            for x in range(ebufl[y],ebufr[y]):      # from left edge to right edge
                i=y*MAXSCREEN_X+x
                screen_addr[i] = solid_color
                continue
#                 z=j>>16
#                 if z < zbuf[i]:                     # texture map attempt 
#                     texture_color = texture_addr[TEXTURE_WIDTH*(z%(TEXTURE_HEIGHT-1)) + (x%(TEXTURE_WIDTH-1))]
#                     screen_addr[i] = texture_color #color[z]
#                     zbuf[i]= z
#                     j+=(zinc*sign)
#                     z=j>>16
        ebufl[y] = MAXSCREEN_X                      # reset left edge
        ebufr[y] = 0                                # reset right edge
        

@micropython.viper
def init_points():
    xc=ptr8(xcoord)
    yc=ptr8(ycoord)
    zc=ptr8(zcoord)
    points=[(0,0,0),(CUBE_SIZE,0,0),(0,CUBE_SIZE,0),(CUBE_SIZE,CUBE_SIZE,0),
            (0,0,CUBE_SIZE//2),(CUBE_SIZE,0,CUBE_SIZE//2),(0,CUBE_SIZE,CUBE_SIZE//2),(CUBE_SIZE,CUBE_SIZE,CUBE_SIZE//2)]
    i=0
    for p in points:
        x,y,z = p
        xc[i] = int(x)
        yc[i] = int(y)
        zc[i] = int(z)
        i+=1
        
@micropython.viper
def init_triangles():
    a_addr=ptr8(tri_a)
    b_addr=ptr8(tri_b)
    c_addr=ptr8(tri_c)
    faces=[(0,1,2),(1,3,2),(0,4,1),(4,5,1),(1,7,3),(5,7,1), 
           (2,3,6),(3,7,6),(6,5,4),(6,7,5),(0,2,6),(6,4,0)] 
    i=0
    for f in faces:
        a,b,c = f
        a_addr[i] = int(a)
        b_addr[i] = int(b)
        c_addr[i] = int(c)
        i+=1

@micropython.viper
def rotate(num_points:int,deg:int):
    x_addr=ptr32(rot_x)
    y_addr=ptr32(rot_y)
    z_addr=ptr32(rot_z)
    xc=ptr8(xcoord)
    yc=ptr8(ycoord)
    zc=ptr8(zcoord)
    sin_addr=ptr32(isin)
    cos_addr=ptr32(icos)
    offset = CUBE_SIZE//2
    for i in range(num_points):
        x_addr[i] = int((xc[i]-offset)*cos_addr[deg]+(yc[i]-offset)*sin_addr[deg])>>SCALE        
        y_addr[i] = int((xc[i]-offset)*sin_addr[deg]-(yc[i]-offset)*cos_addr[deg])>>SCALE        
        z_addr[i] = zc[i]
  

@micropython.viper
def draw_rotate(num_triangles:int,xoffset:int,yoffset:int):
    x=ptr32(rot_x)
    y=ptr32(rot_y)
    z=ptr32(rot_z)
    a=ptr8(tri_a)
    b=ptr8(tri_b)
    c=ptr8(tri_c)    
    for j in range(0,12):  # num_triangles
        i=int(j)
        ta=a[i]
        tb=b[i]
        tc=c[i]
        x1=x[ta]+z[ta]+xoffset
        y1=y[ta]-z[ta]+yoffset
        x2=x[tb]+z[tb]+xoffset
        y2=y[tb]-z[tb]+yoffset
        x3=x[tc]+z[tc]+xoffset
        y3=y[tc]-z[tc]+yoffset
        z1=z[ta]
        z2=z[tb]
        z3=z[tc]
        area = x1 * y2 - y1 * x2 + x2 * y3 - y2 * x3 + x3 * y1 - y3 * x1
        if area>0:  # hide back faces
            continue
        draw_line(x1,y1,x2,y2,z1,z2,lcd.WHITE)
        draw_line(x2,y2,x3,y3,z2,z3,lcd.WHITE)
        draw_line(x3,y3,x1,y1,z3,z1,lcd.WHITE)
        fill_triangle(solid_colors[i//2])
    
    
def fade(input_color1, input_color2):
    color1=input_color1<<8 | input_color1>>8 # byte swap to normal RBG565
    red1  =color1>>11& 0b11111               # extract red #13
    green1=color1>>6 & 0b11111               # extract green
    blue1 =color1    & 0b11111               # extract blue
    color2=input_color2<<8 | input_color2>>8 # byte swap
    red2  =color2>>11& 0b11111               # extract red
    green2=color2>>6 & 0b11111               # extract green
    blue2 =color2    & 0b11111               # extract blue
    inc_red  =(red2-  red1)/31               # find increment step
    inc_green=(green2-green1)/31
    inc_blue =(blue2- blue1)/31
    for i in range(0,32):       
        red3  =red1  +int(i*inc_red)          # build colors by steps
        green3=green1+int(i*inc_green)
        blue3 =blue1 +int(i*inc_blue)
        color3=red3<<11 | green3<<6 | blue3   # combine RGB  
        color_l2.append((color3 & 0x00FF)<<8 | (color3>>8))    # byte swap to LCD RGB565

def init_isin():            # integer sin lookup table    
    for i in range(0,361):
        isin[i]=int(sin(radians(i))*(1<<SCALE))
        

def init_icos():            # integer cos lookup table 
    for i in range(0,361):
        icos[i]=int(cos(radians(i))*(1<<SCALE))

@micropython.asm_thumb
def clear_zbuf(r0,r1): # r0=address, r1= # of words
    label(LOOP)
    mov(r2,0xff) 
    strb(r2, [r0, 0]) #r2
    add(r0, 1)  # add 1 to address (next word)
    sub(r1, 1)  # dec number of words
    bgt(LOOP)   # branch if not done


if __name__=='__main__':
    lcd = LCD_3inch5()
    lcd.bl_ctrl(100)
    lcd.Fill(lcd.BLACK)
    display_buffer=bytearray(MAXSCREEN_X * MAXSCREEN_Y * 2)
    screen=framebuf.FrameBuffer(display_buffer, MAXSCREEN_X , MAXSCREEN_Y, framebuf.RGB565)
    wood_buff=bytearray(TEXTURE_WIDTH*TEXTURE_HEIGHT*2)
    texture_wood=framebuf.FrameBuffer(wood_buff,TEXTURE_WIDTH,TEXTURE_HEIGHT,framebuf.RGB565)
    marble_buff=bytearray(TEXTURE_WIDTH*TEXTURE_HEIGHT*2)
    texture_marble=framebuf.FrameBuffer(marble_buff,TEXTURE_WIDTH,TEXTURE_HEIGHT,framebuf.RGB565)
    blit_image_file(wood_buff,"wood.bin",90,47,90,47)
    blit_image_file(marble_buff,"wall.bin",90,47,90,47)    
    color_l2 = array.array('H', [])
    solid_colors = array.array('H', [lcd.BLUE,lcd.ORANGE,lcd.RED,lcd.YELLOW,lcd.WHITE,lcd.GREEN])
    fade(lcd.BLUE,lcd.GREEN)
    fade(lcd.GREEN,lcd.YELLOW)
    fade(lcd.YELLOW,lcd.ORANGE)
    fade(lcd.ORANGE,lcd.RED)
    fade(lcd.RED,lcd.BLACK)
    init_isin()
    init_icos()
    init_points()
    init_triangles()
    gc.collect()
    print('memory free:',gc.mem_free())
    clear_zbuf(zbuffer,MAXSCREEN_X*MAXSCREEN_Y)
    while(1):
        for deg in range(0,360,8):
            gticks=ticks_us() 
            rotate(NUM_POINTS,deg)
            draw_rotate(NUM_TRIANGLES,60,80)
            rotate(NUM_POINTS,360-deg)
            draw_rotate(NUM_TRIANGLES,100,80)
            rotate(NUM_POINTS,360-deg)
            draw_rotate(NUM_TRIANGLES,60,130)
            rotate(NUM_POINTS,deg)
            draw_rotate(NUM_TRIANGLES,100,130)
            lcd.show_xy(0,0,MAXSCREEN_X-1,MAXSCREEN_Y-1,screen)
            background_asm(screen,wood_buff,MAXSCREEN_Y,MAXSCREEN_X)
            clear_zbuf(zbuffer,MAXSCREEN_X*MAXSCREEN_Y)
            #print(gc.mem_free())
            print(1_000_000//ticks_diff(ticks_us(), gticks),end='\r')