triangle_test.py

from PIL import Image, ImageDraw, ImageSequence
from random import random as rnd
from math import sqrt

def vec3_scale(v,f):
    return (v[0]*f,v[1]*f,v[2]*f)

def vec3_add(a,b):
    return (a[0]+b[0],a[1]+b[1],a[2]+b[2])

def vec3_sub(a,b):
    return (a[0]-b[0],a[1]-b[1],a[2]-b[2])

def vec3_dot(a,b):
    return a[0]*b[0]+a[1]*b[1]+a[2]*b[2]

def vec3_cross(a,b):
    return (a[1]*b[2]-a[2]*b[1],a[2]*b[0]-a[0]*b[2],a[0]*b[1]-a[1]*b[0])
    
def vec3_length(v):
    return sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2])
    
def vec3_distance_sqrd(a,b):
    d = vec3_sub(a,b)
    return vec3_dot(d,d)

BLACK = (0,0,0)
GREEN = (0,255,0)
BLUE = (0,0,255)
RED = (255,0,0)
WHITE = (255,255,255)
CYAN = (128,255,255)
YELLOW = (255,255,128)

def tri_seg_loci_will(a,b,c,seg_origin,seg_dir,seg_radius):
    # get triangle edge vectors and plane normal
    u = vec3_sub(b,a)
    v = vec3_sub(c,a)
    n = vec3_cross(u,v)
    if n[0]==0 and n[1]==0 and n[2]==0:
        return RED # triangle is degenerate
    w0 = vec3_sub(seg_origin,a)
    j = vec3_dot(n,seg_dir)
    if abs(j) < 0.00000001:
        #TODO parallel?
        return BLACK # parallel, disjoint or on plane
    # get intersect point of ray with triangle plane
    i = -vec3_dot(n,w0)
    k = i / j
    # going away?
    if k < 0: # we don't care for ball at start end
        return BLACK # ray goes away from triangle
    # too far ahead?
    if k > 1:
        hit_length = vec3_length(vec3_scale(seg_dir,k))
        if hit_length > 1+seg_radius: # too far
            return BLACK
        seg_radius = seg_radius-(seg_radius*(k-1)) # we are past the end, so capsule is not so wide
    # intersect point of seg and plane
    hit = vec3_add(seg_origin,vec3_scale(seg_dir,k))
    # now test each edge; we are inside (right), or are we too far away (left)
    for side in ((a,b,c,BLUE),(b,a,c,CYAN),(c,a,b,YELLOW)):
        # check if inside
        v = vec3_sub(side[2],side[1])
        w = vec3_sub(hit,side[1])
        cp1 = vec3_cross(v,w)
        cp2 = vec3_cross(v,vec3_sub(side[0],side[1]))
        if vec3_dot(cp1, cp2) >= 0:
            continue # check next side
        # outside, so where on the side line does it hit?
        c1 = vec3_dot(w,v)
        if c1 <= 0:
            nearest = side[1]
        else:
            c2 = vec3_dot(v,v)
            if c2 <= c1:
                nearest = side[2]
            else:
                b = c1 / c2;
                nearest = vec3_add(side[1],vec3_scale(v,b))
        if vec3_distance_sqrd(hit,nearest) < seg_radius*seg_radius:
            return side[3]
        return BLACK
    # inside all three sides, so hit
    return GREEN

def test():
    test_func = tri_seg_loci_will
    dim = 500
    a = (rnd()*(dim-1)+1,rnd()*(dim-1)+1,rnd()*(dim-1)+1)
    b = (rnd()*(dim-1)+1,rnd()*(dim-1)+1,rnd()*(dim-1)+1)
    c = (rnd()*(dim-1)+1,rnd()*(dim-1)+1,rnd()*(dim-1)+1)
    r = 30
    img = Image.new("RGB",(dim,dim))
    pix = img.load()
    for y in xrange(dim):
        for x in xrange(dim):
            col = test_func(a,b,c,[x,y,0],[0,0,dim-r/2],r)
            pix[x,y] = col
    def marker(x,y,col):
        for i in range(-1,2):
            for j in range(-1,2):
                pix[round(x)+j,round(y)+i] = col
    marker(a[0],a[1],WHITE)
    marker(b[0],b[1],WHITE)
    marker(c[0],c[1],WHITE)
    img.show()
    
test()