home/diploma/lws_root/intersect.py

from collections import defaultdict

from utils.objparser import Mesh

from PIL import Image
from PIL import ImageDraw

class Mesh2D(object):
    def __init__(self, mesh, xlim, ylim, pixel_per_meter):
        self.mesh = mesh
        
        self.pixel_per_meter = pixel_per_meter
        x_range = xlim[1] - xlim[0]
        y_range = ylim[1] - ylim[0]
        
        self.width = x_range * pixel_per_meter
        self.height = y_range * pixel_per_meter
        
        self.dx = pixel_per_meter
        self.dy = pixel_per_meter
        
        
        self.offset_x = xlim[0] * self.dx
        self.offset_y = ylim[0] * self.dy
        
        self.objnames = mesh.objects.keys()
        
    def getLines(self, objname, z):
        triangles = self.mesh.itertriangles(objname=objname)

        cut_plane_normal = (0, 0, 1) # normal vector in direction of z-axis
        cut_plane_p0 = (0, 0, z) # stuetzvektor 1m above floor

        lines = []
        for j, (p1, p2, p3) in enumerate(triangles):
            #~ print 'triangle %s' % j
            # connect p1->p2
            l0 = p1 # stuetzvektor
            l = (p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]) # richtungsvektor
            line1 = (l0, l, p1, p2)
            
            # connect p2->p3
            l0 = p2 # stuetzvektor
            l = (p3[0] - p2[0], p3[1] - p2[1], p3[2] - p2[2]) # richtungsvektor
            line2 = (l0, l, p2, p3)
            
            # connect p3->p1
            l0 = p3 # stuetzvektor
            l = (p1[0] - p3[0], p1[1] - p3[1], p1[2] - p3[2]) # richtungsvektor
            line3 = (l0, l, p3, p1)
            
            # calculate d = (p0 - l0) * n / (l * n)
            n = cut_plane_normal
            p0 = cut_plane_p0
            intersects = []
            for i, (l0, l, p1, p2) in enumerate((line1, line2, line3), 1):
                a = p0[0] - l0[0], p0[1] - l0[1], p0[2] - l0[2]
                b = a[0] * n[0] + a[1] * n[1] + a[2] * n[2] # (p0 - l0) * n
                
                c = l[0] * n[0] + l[1] * n[1] + l[2] * n[2] # l * n
                if c == 0:
                    pass # parallel
                else:
                    
                    d = b / float(c)
                    # if d > 1 the point is farther away than p2
                    if 0 < d < 1:
                        p = l0[0] + l[0] * d, l0[1] + l[1] * d, l0[2] + l[2] * d
                        #~ print 'triangle %s, intersect line %s at %s' % (j, i, p)
                        intersects.append(p)
            
            assert len(intersects) in (0, 2)
            if len(intersects) == 2:
                lines.append((intersects[0], intersects[1]))
        return lines
    
    def meter2pixel(self, x, y):
        return int(x * self.dx - self.offset_x), int(self.height - y * self.dy + self.offset_y)
    
    def pixel2meter(self, x, y):
        return float((x + self.offset_x) / float(self.dx)), float((self.height + self.offset_y - y) / float(self.dy))

    def cut(self, objname, z):
        img = Image.new('L', (self.width, self.height), color=255)
        draw = ImageDraw.Draw(img)
        #~ print objname
        #~ print 'get lines'
        lines = self.getLines(objname, z)
        
        #~ if len(lines) == 0:
            #~ return None
            
        #~ print 'draw lines'
        for p1, p2 in lines:
            _p1 = self.meter2pixel(p1[0], p1[1])
            _p2 = self.meter2pixel(p2[0], p2[1])
            draw.line([_p1, _p2], fill=128)

        #~ print 'floodfill'
        ImageDraw.floodfill(img, [0, 0], 0)
        pixels = img.load()
        
        #~ print 'recolor lines'
        for x in range(self.width):
            for y in range(self.height):
                if pixels[x, y] == 128:
                    pixels[x, y] = 255
        
        return img


if __name__ == '__main__':
    mesh = Mesh()
    #~ mesh.parseObjFile('../maps/onewall/One-Wall.obj')
    mesh.parseObjFile('../maps/umic/umic.obj')

    xlim = (-1, 60)
    ylim = (-1, 20)
    print mesh.objects.keys()
    prepareLayer(mesh, 'Asd', xlim, ylim, 860, 413, 0.1)