Exemplo n.º 1
0
class PlaneScene():
    def __init__(self, index):
        #self.depth = cv2.imread('dump/' + str(index) + '_depth_pred.png').astype(np.float32) / 255 * 10
        self.depth = np.load('dump/' + str(index) + '_depth.npy')
        #cv2.imwrite('dump/alpha_0.5.png', np.zeros(self.depth[:, :, 0].shape).astype(np.uint8))
        self.segmentation = np.load('dump/' + str(index) + '_segmentation.npy')

        #print(self.segmentation.shape)
        self.segmentation[self.segmentation == -1] = 10
        cv2.imwrite('dump/2_segmentation.png',
                    drawSegmentationImage(self.segmentation, blackIndex=10))
        exit(1)

        width = 640
        height = 480
        self.depth = cv2.resize(self.depth, (width, height))
        self.segmentation = cv2.resize(self.segmentation, (width, height),
                                       interpolation=cv2.INTER_NEAREST)
        self.planes = np.load('dump/' + str(index) + '_planes.npy')
        self.numPlanes = self.planes.shape[0]

        self.imageTexture = ObjMaterial()
        self.imageTexture.name = 'image'
        self.imageTexture.put('map_Kd', 'dump/' + str(index) + '_image.png')
        self.width = self.depth.shape[1]
        self.height = self.depth.shape[0]
        self.info = np.load('dump/' + str(index) + '_info.npy')
        self.camera = getCameraFromInfo(self.info)
        self.scene_index = index
        self.calcHorizontalPlanes()
        return

    def addRectangle(self, parent):
        planesGroup = EggGroup('planes')
        parent.addChild(planesGroup)
        vp = EggVertexPool('plane_vertex')
        parent.addChild(vp)

        p0 = Point3D(-10, 1, 0)
        p1 = Point3D(-10, 10, 0)
        p2 = Point3D(10, 1, 0)
        p3 = Point3D(10, 10, 0)
        # p0 = Point3D(-10, , 0)
        # p1 = Point3D(-10, 100, 0)
        # p3 = Point3D(10, 100, 0)
        # p2 = Point3D(10, 90, 0)

        planeGroup = EggGroup('plane')
        planesGroup.addChild(planeGroup)
        poly = EggPolygon()
        planeGroup.addChild(poly)
        vertex = EggVertex()
        vertex.setPos(p0)
        vertex.setUv(Point2D(0, 0))
        poly.addVertex(vp.addVertex(vertex))
        vertex = EggVertex()
        vertex.setPos(p1)
        vertex.setUv(Point2D(0, 1))
        poly.addVertex(vp.addVertex(vertex))
        vertex = EggVertex()
        vertex.setPos(p2)
        vertex.setUv(Point2D(1, 1))
        poly.addVertex(vp.addVertex(vertex))

        poly = EggPolygon()
        planeGroup.addChild(poly)

        vertex = EggVertex()
        vertex.setPos(p1)
        vertex.setUv(Point2D(0, 1))
        poly.addVertex(vp.addVertex(vertex))
        vertex = EggVertex()
        vertex.setPos(p2)
        vertex.setUv(Point2D(1, 1))
        poly.addVertex(vp.addVertex(vertex))
        vertex = EggVertex()
        vertex.setPos(p3)
        vertex.setUv(Point2D(1, 0))
        poly.addVertex(vp.addVertex(vertex))

        # vertex = EggVertex()
        # vertex.setPos(p2)
        # vertex.setUv(Point2D(1, 1))
        # poly.addVertex(vp.addVertex(vertex))

        return

    def generatePlanes(self, parent):
        planesGroup = EggGroup('planes')
        parent.addChild(planesGroup)
        vp = EggVertexPool('plane_vertex')
        parent.addChild(vp)

        for planeIndex in range(self.numPlanes):
            mask = (self.segmentation == planeIndex).astype(np.uint8) * 255
            cv2.imwrite('test/mask_' + str(planeIndex) + '.png', mask)
            contours, _ = cv2.findContours(mask, cv2.RETR_LIST,
                                           cv2.CHAIN_APPROX_SIMPLE)
            plane = self.planes[planeIndex]
            planeD = np.linalg.norm(plane)
            planeNormal = plane / planeD
            for contour in contours:
                planeGroup = EggGroup('plane')
                planesGroup.addChild(planeGroup)
                poly = EggPolygon()
                planeGroup.addChild(poly)

                poly.setTexture(self.imageTexture.getEggTexture())
                poly.setMaterial(self.imageTexture.getEggMaterial())

                contour = contour.astype(np.float32)
                u = (contour[:, 0, 0].astype(np.float32) / self.width *
                     self.info[16] - self.camera['cx']) / self.camera['fx']
                v = -(contour[:, 0, 1].astype(np.float32) / self.height *
                      self.info[17] - self.camera['cy']) / self.camera['fy']
                ranges = np.stack([u, np.ones(u.shape), v], axis=1)
                depth = planeD / np.dot(ranges, planeNormal)
                XYZ = ranges * np.expand_dims(depth, -1)
                #print(contour)
                #print(XYZ)
                #exit(1)
                for vertexIndex, uv in enumerate(contour):
                    vertex = EggVertex()
                    X, Y, Z = XYZ[vertexIndex]
                    vertex.setPos(Point3D(X, Y, Z))
                    u, v = uv[0]
                    vertex.setUv(Point2D(u / self.width, 1 - v / self.height))
                    poly.addVertex(vp.addVertex(vertex))
                    continue
                continue
            continue
        return

    def generateRectangle(self, parent):
        planesGroup = EggGroup('planes')
        parent.addChild(planesGroup)
        vp = EggVertexPool('plane_vertex')
        parent.addChild(vp)

        poly = EggPolygon()
        planesGroup.addChild(poly)

        w = 0.5
        p0 = Point3D(-w / 2, 0, -w / 2)
        p1 = Point3D(-w / 2, 0, w / 2)
        p2 = Point3D(w / 2, 0, w / 2)
        p3 = Point3D(w / 2, 0, -w / 2)

        poly.setTexture(self.plateTexture.getEggTexture())
        poly.setMaterial(self.plateTexture.getEggMaterial())
        vertex = EggVertex()
        vertex.setPos(Point3D(0, 1, 0))
        vertex.setUv(Point2D(0, 0))
        poly.addVertex(vp.addVertex(vertex))
        vertex = EggVertex()
        vertex.setPos(Point3D(0, 1, 1))
        vertex.setUv(Point2D(0, 1))
        poly.addVertex(vp.addVertex(vertex))
        vertex = EggVertex()
        vertex.setPos(Point3D(1, 1, 1))
        vertex.setUv(Point2D(1, 1))
        poly.addVertex(vp.addVertex(vertex))
        vertex = EggVertex()
        vertex.setPos(Point3D(1, 1, 0))
        vertex.setUv(Point2D(1, 0))
        poly.addVertex(vp.addVertex(vertex))

        return

    def addCollisionPolygons(self, scene):

        polygons = scene.findAllMatches("**/plane")
        mesh = BulletTriangleMesh()
        for polygon in polygons:
            #cNode = scene.attachNewNode(CollisionNode('plane_solid'))
            #cNode.node().addSolid(CollisionPolygon(polygon))
            #polygon.setCollideMask(BitMask32.bit(1))
            node = polygon.node()
            print((node.getNumGeoms()))
            for i in range(node.getNumGeoms()):
                geom = node.getGeom(i)
                mesh.addGeom(geom)
                continue
            continue

    def test(self, scene):
        groundMask = BitMask32(0b1)
        parent = NodePath('cGeomConversionParent')
        for c in incomingNode.findAllMatches('**/+GeomNode'):
            if relativeTo:
                xform = c.getMat(relativeTo).xformPoint
            else:
                xform = c.getMat().xformPoint
            gni = 0
            geomNode = c.node()
            for g in range(geomNode.getNumGeoms()):
                geom = geomNode.getGeom(g).decompose()
                vdata = geom.getVertexData()
                vreader = GeomVertexReader(vdata, 'vertex')
                cChild = CollisionNode('cGeom-%s-gni%i' % (c.getName(), gni))

                gni += 1
                for p in range(geom.getNumPrimitives()):
                    prim = geom.getPrimitive(p)
                    for p2 in range(prim.getNumPrimitives()):
                        s = prim.getPrimitiveStart(p2)
                        e = prim.getPrimitiveEnd(p2)
                        v = []
                        for vi in range(s, e):
                            vreader.setRow(prim.getVertex(vi))
                            v.append(xform(vreader.getData3f()))
                        colPoly = CollisionPolygon(*v)
                        cChild.addSolid(colPoly)

                n = parent.attachNewNode(cChild)
                #n.show()

        return parent

    def generateEggModel(self):
        data = EggData()
        model = EggGroup('model')
        data.addChild(model)
        self.generatePlanes(model)
        #self.generateRectangle(model)
        data.writeEgg(Filename("test/plane.egg"))
        scene = NodePath(loadEggData(data))
        #self.addCollisionPolygons(scene)

        return scene

    def getPlaneTriangles(self):
        from skimage import measure

        planeTriangles = []
        planeNormals = []
        horizontalPlaneTriangles = []

        for planeIndex in range(self.numPlanes):
            mask = (self.segmentation == planeIndex).astype(np.uint8) * 255
            mask_ori = mask.copy()
            #contours, _ = cv2.findContours(mask, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
            #contours, _ = cv2.findContours(mask, cv2.RETR_LIST, cv2.CHAIN_APPROX_TC89_KCOS)
            masks = measure.label(mask.astype(np.int32), background=0)
            contours = []
            for maskIndex in range(masks.min() + 1, masks.max() + 1):
                mask = masks == maskIndex
                contour_mask = mask - np.logical_and(
                    np.logical_and(np.roll(mask, shift=1, axis=0),
                                   np.roll(mask, shift=-1, axis=0)),
                    np.logical_and(np.roll(mask, shift=1, axis=1),
                                   np.roll(mask, shift=-1, axis=1)))
                contour_v, contour_u = contour_mask.nonzero()
                contours.append(np.stack([contour_u, contour_v], axis=1))
                continue

            plane = self.planes[planeIndex]
            planeD = np.linalg.norm(plane)
            planeNormal = plane / np.maximum(planeD, 1e-4)

            # cv2.imwrite('test/mask.png', mask_ori)
            # #print(len(contours))

            # mask_ori = np.stack([mask_ori, mask_ori, mask_ori], 2)
            # count = 0
            # for contour in contours:
            #   count += contour.shape[0]
            #   for uv in contour:
            #     #uv = uv[0]
            #     mask_ori[uv[1]][uv[0]] = np.array([255, 0, 0])
            #     continue
            #   continue
            # cv2.imwrite('test/mask_contour.png', mask_ori)
            # if planeIndex == 1:
            #   exit(1)

            indices = np.arange(self.width * self.height).astype(np.float32)
            us = indices % self.width
            us = us / self.width * self.info[16] - self.camera['cx']
            vs = indices / self.width
            vs = -(vs / self.height * self.info[17] - self.camera['cy'])
            ranges = np.stack([
                us / self.camera['fx'],
                np.ones(us.shape), vs / self.camera['fy']
            ],
                              axis=1)
            #print(ranges)
            #print(np.dot(ranges, planeNormal).shape)
            #print(np.dot(ranges, planeNormal))
            #print(ranges)
            #exit(1)
            depth = planeD / np.tensordot(ranges, planeNormal, axes=([1], [0]))
            XYZ = ranges * np.expand_dims(depth, -1)
            XYZ = XYZ.reshape((self.height, self.width, 3))
            for contour in contours:
                contour = contour.astype(np.float32)[::20]
                if contour.shape[0] < 3:
                    continue
                rect = (0, 0, self.width, self.height)
                subdiv = cv2.Subdiv2D(rect)

                for point in contour:
                    subdiv.insert((point[0], point[1]))
                    continue
                triangleList = subdiv.getTriangleList()

                #print(contour)
                #print(triangleList)
                #exit(1)
                for triangle2D in triangleList:
                    triangle = []
                    for vertexIndex in range(3):
                        x = int(triangle2D[vertexIndex * 2 + 0])
                        y = int(triangle2D[vertexIndex * 2 + 1])
                        #print(x, y)
                        if x < 0 or x >= self.width or y < 0 or y >= self.height:
                            continue
                        triangle.append(XYZ[y][x])
                        continue
                    if len(triangle) == 3:
                        #print(triangle)
                        if np.dot(
                                np.cross(planeNormal,
                                         triangle[1] - triangle[0]),
                                triangle[2] - triangle[0]) > 0:
                            triangle = [triangle[0], triangle[2], triangle[1]]
                            pass
                        if planeIndex in self.horizontalPlanes:
                            horizontalPlaneTriangles.append(triangle)
                        else:
                            planeTriangles.append(triangle)
                            pass
                        #planeNormals.append(planeNormal)
                        pass
                    continue
            continue
        planeTriangles = np.array(planeTriangles)
        #planeNormals = np.array(planeNormals)
        np.save('dump/' + str(self.scene_index) + '_plane_triangles.npy',
                planeTriangles)
        #np.save('dump/' + str(self.scene_index) + '_plane_normals.npy', planeNormals)
        return planeTriangles, horizontalPlaneTriangles, self.gravityDirection

    def getPlaneGeometries(self):
        if os.path.exists('dump/' + str(self.scene_index) +
                          '_plane_triangles.npy'):
            print('loading')
            planeTriangles = np.load('dump/' + str(self.scene_index) +
                                     '_plane_triangles.npy')
            planeNormals = np.load('dump/' + str(self.scene_index) +
                                   '_plane_normals.npy')
            return planeTriangles, planeNormals
            pass

        planeNormals = []
        planeTriangles = []
        for planeIndex in range(self.numPlanes):
            mask = (self.segmentation == planeIndex).astype(np.uint8) * 255
            #mask_ori = mask.copy()
            #contours, _ = cv2.findContours(mask, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
            plane = self.planes[planeIndex]
            planeD = np.linalg.norm(plane)
            planeNormal = plane / np.maximum(planeD, 1e-4)

            #cv2.imwrite('test/mask.png', mask)
            #v, u = mask.nonzero()
            u = np.arange(self.width * self.height) % self.width
            v = np.arange(self.width * self.height) / self.width
            u = u.astype(
                np.float32) / self.width * self.info[16] - self.camera['cx']
            v = -(v.astype(np.float32) / self.height * self.info[17] -
                  self.camera['cy'])
            ranges = np.stack([
                u / self.camera['fx'],
                np.ones(u.shape), v / self.camera['fy']
            ],
                              axis=1)
            depth = planeD / np.dot(ranges, planeNormal)
            XYZ = ranges * np.expand_dims(depth, -1)
            XYZ = XYZ.reshape((self.height, self.width, 3))

            triangles = []
            for pixel in mask.reshape(-1).nonzero()[0]:
                x = pixel % self.width
                y = pixel / self.width
                for neighbors in [((x - 1, y), (x, y - 1)),
                                  ((x - 1, y), (x, y + 1)),
                                  ((x + 1, y), (x, y - 1)),
                                  ((x + 1, y), (x, y + 1))]:
                    valid = True
                    for neighbor in neighbors:
                        if neighbor[0] < 0 or neighbor[
                                0] >= self.width or neighbor[1] < 0 or neighbor[
                                    1] >= self.height or mask[neighbor[1]][
                                        neighbor[0]] == False:
                            valid = False
                            break
                        continue
                    if valid:
                        triangle = [XYZ[y][x]]
                        for neighbor in neighbors:
                            triangle.append(XYZ[neighbor[1], neighbor[0]])
                            continue
                        triangles.append(triangle)
                        pass
                    continue
                continue
            planeTriangles.append(triangles)
            planeNormals.append(planeNormal)
            continue

        planeTriangles = np.array(planeTriangles)
        #planeNormals = np.array(planeNormals)
        #np.save('dump/' + str(self.scene_index) + '_plane_triangles.npy', planeTriangles)
        #np.save('dump/' + str(self.scene_index) + '_plane_normals.npy', planeNormals)
        return planeTriangles, planeNormals

    def calcHorizontalPlanes(self):
        from sklearn.cluster import KMeans

        planesD = np.linalg.norm(self.planes, axis=-1, keepdims=True)
        normals = self.planes / np.maximum(planesD, 1e-4)

        normals[normals[:, 1] < 0] *= -1

        kmeans = KMeans(n_clusters=3).fit(normals)
        dominantNormals = kmeans.cluster_centers_
        dominantNormals = dominantNormals / np.maximum(
            np.linalg.norm(dominantNormals, axis=-1, keepdims=True), 1e-4)

        planeClusters = kmeans.predict(normals)

        horizontalNormalIndex = np.argmax(np.abs(dominantNormals[:, 2]))
        self.gravityDirection = dominantNormals[horizontalNormalIndex]
        self.horizontalPlanes = (
            planeClusters == horizontalNormalIndex).nonzero()[0]
        if self.gravityDirection[2] > 0:
            self.gravityDirection *= -1
            pass

        print((self.horizontalPlanes))
        print((self.gravityDirection))
        return

    def getHorizontalPlanes(self):
        return self.gravityDirection, self.horizontalPlanes

    def getHolePos(self):
        floorPlaneIndex = 2
        closePoint = np.array([0., 1.22, -0.2])
        plane = self.planes[floorPlaneIndex]
        planeD = np.linalg.norm(plane)
        planeNormal = plane / planeD
        distance = planeD - np.dot(planeNormal, closePoint)
        distance *= 0.99
        holePos = closePoint + planeNormal * distance

        H = P = R = 0
        H = -90 + np.rad2deg(np.arctan2(planeNormal[1], planeNormal[0]))
        #P = 90 - np.rad2deg(np.arccos(np.abs(planeNormal[2])))
        P = -90 + np.rad2deg(np.arccos(np.abs(planeNormal[2])))
        #print(H, P, R)
        return holePos, np.array([H, P, R])

    def getPortalPos(self):
        wallPlaneIndex = 1
        closePoint_1 = np.array([0.5, 1.35, -0.5])
        closePoint_2 = np.array([-0.4, 1, 0.19])
        plane = self.planes[wallPlaneIndex]
        planeD = np.linalg.norm(plane)
        planeNormal = plane / planeD

        distance = planeD - np.dot(planeNormal, closePoint_1)
        distance *= 0.95
        portalPos_1 = closePoint_1 + planeNormal * distance

        distance = planeD - np.dot(planeNormal, closePoint_2)
        distance *= 0.95
        portalPos_2 = closePoint_2 + planeNormal * distance

        H = P = R = 0
        H = -90 + np.rad2deg(np.arctan2(planeNormal[1], planeNormal[0]))
        #P = 90 - np.rad2deg(np.arccos(np.abs(planeNormal[2])))
        P = -90 + np.rad2deg(np.arccos(-np.abs(planeNormal[2])))
        #print(H, P, R)
        return portalPos_1, np.array([H, P, R]), portalPos_2, np.array(
            [H, P, R]), planeNormal
Exemplo n.º 2
0
class PartsScene():
  def __init__(self, index):
    #self.depth = cv2.imread('dump/' + str(index) + '_depth_pred.png').astype(np.float32) / 255 * 10
    self.depth = np.load('dump/' + str(index) + '_depth.npy')
    self.segmentation = np.load('dump/' + str(index) + '_segmentation.npy')

    width = 640
    height = 480
    self.depth = cv2.resize(self.depth, (width, height))
    self.segmentation = cv2.resize(self.segmentation, (width, height), interpolation=cv2.INTER_NEAREST)
    
    self.planes = np.load('dump/' + str(index) + '_planes.npy')
    self.numPlanes = self.planes.shape[0]

    self.imageTexture = ObjMaterial()
    self.imageTexture.name = 'image'
    self.imageTexture.put('map_Kd', 'dump/' + str(index) + '_image.png')
    self.width = self.depth.shape[1]
    self.height = self.depth.shape[0]
    self.info = np.load('dump/' + str(index) + '_info.npy')
    self.camera = getCameraFromInfo(self.info)

    return

  def addRectangle(self, parent):
    planesGroup = EggGroup('planes')
    parent.addChild(planesGroup)
    vp = EggVertexPool('plane_vertex')
    parent.addChild(vp)
    
    p0 = Point3D(-10, 1, 0)
    p1 = Point3D(-10, 10, 0)
    p2 = Point3D(10, 1, 0)
    p3 = Point3D(10, 10, 0)    
    # p0 = Point3D(-10, , 0)
    # p1 = Point3D(-10, 100, 0)
    # p3 = Point3D(10, 100, 0)
    # p2 = Point3D(10, 90, 0)
    
    planeGroup = EggGroup('plane')
    planesGroup.addChild(planeGroup)
    poly = EggPolygon()
    planeGroup.addChild(poly)
    vertex = EggVertex()
    vertex.setPos(p0)
    vertex.setUv(Point2D(0, 0))
    poly.addVertex(vp.addVertex(vertex))
    vertex = EggVertex()
    vertex.setPos(p1)
    vertex.setUv(Point2D(0, 1))
    poly.addVertex(vp.addVertex(vertex))
    vertex = EggVertex()
    vertex.setPos(p2)
    vertex.setUv(Point2D(1, 1))
    poly.addVertex(vp.addVertex(vertex))

    
    poly = EggPolygon()
    planeGroup.addChild(poly)
    
    vertex = EggVertex()
    vertex.setPos(p1)
    vertex.setUv(Point2D(0, 1))
    poly.addVertex(vp.addVertex(vertex))
    vertex = EggVertex()
    vertex.setPos(p2)
    vertex.setUv(Point2D(1, 1))
    poly.addVertex(vp.addVertex(vertex))
    vertex = EggVertex()
    vertex.setPos(p3)
    vertex.setUv(Point2D(1, 0))
    poly.addVertex(vp.addVertex(vertex))

    # vertex = EggVertex()
    # vertex.setPos(p2)
    # vertex.setUv(Point2D(1, 1))
    # poly.addVertex(vp.addVertex(vertex))
    
    return
    

  def generateEggModel(self):
  
    self.planeNPs = []
    self.planeCenters = []

    print((self.numPlanes))
    for planeIndex in range(self.numPlanes):
      mask = (self.segmentation == planeIndex).astype(np.uint8) * 255
      #cv2.imwrite('test/mask_' + str(planeIndex) + '.png', mask)
      contours, _ = cv2.findContours(mask, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
      plane = self.planes[planeIndex]
      planeD = np.linalg.norm(plane)
      planeNormal = plane / planeD

      for contour in contours:
        data = EggData()
        model = EggGroup('model')
        data.addChild(model)

        vp = EggVertexPool('plane_vertex')
        model.addChild(vp)
        
        planeGroup = EggGroup('plane')
        model.addChild(planeGroup)
        poly = EggPolygon()
        planeGroup.addChild(poly)

        poly.setTexture(self.imageTexture.getEggTexture())
        poly.setMaterial(self.imageTexture.getEggMaterial())

        contour = contour.astype(np.float32)
        #u = (contour[:, 0, 0] - self.width / 2) / self.width * 640 / self.focalLength
        #v = -(contour[:, 0, 1] - self.height / 2) / self.height * 480 / self.focalLength
        u = (contour[:, 0, 0].astype(np.float32) / self.width * self.info[16] - self.camera['cx']) / self.camera['fx']
        v = -(contour[:, 0, 1].astype(np.float32) / self.height * self.info[17] - self.camera['cy']) / self.camera['fy']

        ranges = np.stack([u, np.ones(u.shape), v], axis=1)
        depth = planeD / np.dot(ranges, planeNormal)
        XYZ = ranges * np.expand_dims(depth, -1)
        center = XYZ.mean(0)
        #print(contour)
        #print(XYZ)
        #exit(1)
        for vertexIndex, uv in enumerate(contour):
          vertex = EggVertex()
          X, Y, Z = XYZ[vertexIndex]
          vertex.setPos(Point3D(X - center[0], Y - center[1], Z - center[2]))
          u, v = uv[0]
          vertex.setUv(Point2D(u / self.width, 1 - v / self.height))
          poly.addVertex(vp.addVertex(vertex))
          continue
        scene = NodePath(loadEggData(data))
        self.planeNPs.append(scene)
        self.planeCenters.append(center)
        continue
    return self.planeNPs, self.planeCenters


  def generateRectangle(self, parent):    
    planesGroup = EggGroup('planes')
    parent.addChild(planesGroup)
    vp = EggVertexPool('plane_vertex')
    parent.addChild(vp)

    poly = EggPolygon()
    planesGroup.addChild(poly)


    w = 0.5
    p0 = Point3D(-w / 2, 0, -w / 2)
    p1 = Point3D(-w / 2, 0, w / 2)
    p2 = Point3D(w / 2, 0, w / 2)
    p3 = Point3D(w / 2, 0, -w / 2)    
  
    
    poly.setTexture(self.plateTexture.getEggTexture())
    poly.setMaterial(self.plateTexture.getEggMaterial())
    vertex = EggVertex()
    vertex.setPos(Point3D(0, 1, 0))
    vertex.setUv(Point2D(0, 0))
    poly.addVertex(vp.addVertex(vertex))
    vertex = EggVertex()
    vertex.setPos(Point3D(0, 1, 1))
    vertex.setUv(Point2D(0, 1))
    poly.addVertex(vp.addVertex(vertex))
    vertex = EggVertex()
    vertex.setPos(Point3D(1, 1, 1))
    vertex.setUv(Point2D(1, 1))
    poly.addVertex(vp.addVertex(vertex))
    vertex = EggVertex()
    vertex.setPos(Point3D(1, 1, 0))
    vertex.setUv(Point2D(1, 0))
    poly.addVertex(vp.addVertex(vertex))
    
    return

  def addCollisionPolygons(self, scene):
    
    polygons = scene.findAllMatches("**/plane")
    mesh = BulletTriangleMesh()      
    for polygon in polygons:
      #cNode = scene.attachNewNode(CollisionNode('plane_solid'))
      #cNode.node().addSolid(CollisionPolygon(polygon))
      #polygon.setCollideMask(BitMask32.bit(1))
      node = polygon.node()
      print((node.getNumGeoms()))
      for i in range(node.getNumGeoms()):
        geom = node.getGeom(i)
        mesh.addGeom(geom)
        continue
      continue

  def test(self, scene):
    groundMask=BitMask32(0b1)
    parent = NodePath('cGeomConversionParent') 
    for c in incomingNode.findAllMatches('**/+GeomNode'): 
        if relativeTo:
            xform=c.getMat(relativeTo).xformPoint
        else:
            xform=c.getMat().xformPoint
        gni = 0 
        geomNode = c.node() 
        for g in range(geomNode.getNumGeoms()): 
            geom = geomNode.getGeom(g).decompose() 
            vdata = geom.getVertexData() 
            vreader = GeomVertexReader(vdata, 'vertex') 
            cChild = CollisionNode('cGeom-%s-gni%i' % (c.getName(), gni)) 
            
            gni += 1 
            for p in range(geom.getNumPrimitives()): 
                prim = geom.getPrimitive(p) 
                for p2 in range(prim.getNumPrimitives()): 
                    s = prim.getPrimitiveStart(p2) 
                    e = prim.getPrimitiveEnd(p2) 
                    v = [] 
                    for vi in range (s, e): 
                        vreader.setRow(prim.getVertex(vi)) 
                        v.append (xform(vreader.getData3f())) 
                    colPoly = CollisionPolygon(*v) 
                    cChild.addSolid(colPoly) 

            n=parent.attachNewNode (cChild) 
            #n.show()
            
    return parent
Exemplo n.º 3
0
class PlaneScene():
  def __init__(self, index):
    #self.depth = cv2.imread('dump/' + str(index) + '_depth_pred.png').astype(np.float32) / 255 * 10
    self.depth = np.load('dump/' + str(index) + '_depth.npy')
    #cv2.imwrite('dump/alpha_0.5.png', np.zeros(self.depth[:, :, 0].shape).astype(np.uint8))
    self.segmentation = np.load('dump/' + str(index) + '_segmentation.npy')

    width = 640
    height = 480
    self.depth = cv2.resize(self.depth, (width, height))
    self.segmentation = cv2.resize(self.segmentation, (width, height), interpolation=cv2.INTER_NEAREST)
    self.planes = np.load('dump/' + str(index) + '_planes.npy')
    self.numPlanes = self.planes.shape[0]

    self.imageTexture = ObjMaterial()
    self.imageTexture.name = 'image'
    self.imageTexture.put('map_Kd', 'dump/' + str(index) + '_image.png')
    self.width = self.depth.shape[1]
    self.height = self.depth.shape[0]
    self.info = np.load('dump/' + str(index) + '_info.npy')
    self.camera = getCameraFromInfo(self.info)
    self.scene_index = index
    self.calcHorizontalPlanes()
    return

  def addRectangle(self, parent):
    planesGroup = EggGroup('planes')
    parent.addChild(planesGroup)
    vp = EggVertexPool('plane_vertex')
    parent.addChild(vp)
    
    p0 = Point3D(-10, 1, 0)
    p1 = Point3D(-10, 10, 0)
    p2 = Point3D(10, 1, 0)
    p3 = Point3D(10, 10, 0)    
    # p0 = Point3D(-10, , 0)
    # p1 = Point3D(-10, 100, 0)
    # p3 = Point3D(10, 100, 0)
    # p2 = Point3D(10, 90, 0)
    
    planeGroup = EggGroup('plane')
    planesGroup.addChild(planeGroup)
    poly = EggPolygon()
    planeGroup.addChild(poly)
    vertex = EggVertex()
    vertex.setPos(p0)
    vertex.setUv(Point2D(0, 0))
    poly.addVertex(vp.addVertex(vertex))
    vertex = EggVertex()
    vertex.setPos(p1)
    vertex.setUv(Point2D(0, 1))
    poly.addVertex(vp.addVertex(vertex))
    vertex = EggVertex()
    vertex.setPos(p2)
    vertex.setUv(Point2D(1, 1))
    poly.addVertex(vp.addVertex(vertex))

    
    poly = EggPolygon()
    planeGroup.addChild(poly)
    
    vertex = EggVertex()
    vertex.setPos(p1)
    vertex.setUv(Point2D(0, 1))
    poly.addVertex(vp.addVertex(vertex))
    vertex = EggVertex()
    vertex.setPos(p2)
    vertex.setUv(Point2D(1, 1))
    poly.addVertex(vp.addVertex(vertex))
    vertex = EggVertex()
    vertex.setPos(p3)
    vertex.setUv(Point2D(1, 0))
    poly.addVertex(vp.addVertex(vertex))

    # vertex = EggVertex()
    # vertex.setPos(p2)
    # vertex.setUv(Point2D(1, 1))
    # poly.addVertex(vp.addVertex(vertex))
    
    return
    

  def generatePlanes(self, parent):
    planesGroup = EggGroup('planes')
    parent.addChild(planesGroup)
    vp = EggVertexPool('plane_vertex')
    parent.addChild(vp)

    for planeIndex in xrange(self.numPlanes):
      mask = (self.segmentation == planeIndex).astype(np.uint8) * 255
      cv2.imwrite('dump/mask_' + str(planeIndex) + '.png', mask)
      contours, _ = cv2.findContours(mask, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
      plane = self.planes[planeIndex]
      planeD = np.linalg.norm(plane)
      planeNormal = plane / planeD
      for contour in contours:
        planeGroup = EggGroup('plane')
        planesGroup.addChild(planeGroup)
        poly = EggPolygon()
        planeGroup.addChild(poly)

        poly.setTexture(self.imageTexture.getEggTexture())
        poly.setMaterial(self.imageTexture.getEggMaterial())

        contour = contour.astype(np.float32)
        u = (contour[:, 0, 0].astype(np.float32) / self.width * self.info[16] - self.camera['cx']) / self.camera['fx']
        v = -(contour[:, 0, 1].astype(np.float32) / self.height * self.info[17] - self.camera['cy']) / self.camera['fy']
        ranges = np.stack([u, np.ones(u.shape), v], axis=1)
        depth = planeD / np.dot(ranges, planeNormal)
        XYZ = ranges * np.expand_dims(depth, -1)
        #print(contour)
        #print(XYZ)
        #exit(1)
        for vertexIndex, uv in enumerate(contour):
          vertex = EggVertex()
          X, Y, Z = XYZ[vertexIndex]
          vertex.setPos(Point3D(X, Y, Z))
          u, v = uv[0]
          vertex.setUv(Point2D(u / self.width, 1 - v / self.height))
          poly.addVertex(vp.addVertex(vertex))
          continue
        continue
      continue
    return


  def generateRectangle(self, parent):    
    planesGroup = EggGroup('planes')
    parent.addChild(planesGroup)
    vp = EggVertexPool('plane_vertex')
    parent.addChild(vp)

    poly = EggPolygon()
    planesGroup.addChild(poly)


    w = 0.5
    p0 = Point3D(-w / 2, 0, -w / 2)
    p1 = Point3D(-w / 2, 0, w / 2)
    p2 = Point3D(w / 2, 0, w / 2)
    p3 = Point3D(w / 2, 0, -w / 2)    
  
    
    poly.setTexture(self.plateTexture.getEggTexture())
    poly.setMaterial(self.plateTexture.getEggMaterial())
    vertex = EggVertex()
    vertex.setPos(Point3D(0, 1, 0))
    vertex.setUv(Point2D(0, 0))
    poly.addVertex(vp.addVertex(vertex))
    vertex = EggVertex()
    vertex.setPos(Point3D(0, 1, 1))
    vertex.setUv(Point2D(0, 1))
    poly.addVertex(vp.addVertex(vertex))
    vertex = EggVertex()
    vertex.setPos(Point3D(1, 1, 1))
    vertex.setUv(Point2D(1, 1))
    poly.addVertex(vp.addVertex(vertex))
    vertex = EggVertex()
    vertex.setPos(Point3D(1, 1, 0))
    vertex.setUv(Point2D(1, 0))
    poly.addVertex(vp.addVertex(vertex))
    
    return

  def addCollisionPolygons(self, scene):
    
    polygons = scene.findAllMatches("**/plane")
    mesh = BulletTriangleMesh()      
    for polygon in polygons:
      #cNode = scene.attachNewNode(CollisionNode('plane_solid'))
      #cNode.node().addSolid(CollisionPolygon(polygon))
      #polygon.setCollideMask(BitMask32.bit(1))
      node = polygon.node()
      print(node.getNumGeoms())
      for i in xrange(node.getNumGeoms()):
        geom = node.getGeom(i)
        mesh.addGeom(geom)
        continue
      continue

  def test(self, scene):
    groundMask=BitMask32(0b1)
    parent = NodePath('cGeomConversionParent') 
    for c in incomingNode.findAllMatches('**/+GeomNode'): 
        if relativeTo:
            xform=c.getMat(relativeTo).xformPoint
        else:
            xform=c.getMat().xformPoint
        gni = 0 
        geomNode = c.node() 
        for g in range(geomNode.getNumGeoms()): 
            geom = geomNode.getGeom(g).decompose() 
            vdata = geom.getVertexData() 
            vreader = GeomVertexReader(vdata, 'vertex') 
            cChild = CollisionNode('cGeom-%s-gni%i' % (c.getName(), gni)) 
            
            gni += 1 
            for p in range(geom.getNumPrimitives()): 
                prim = geom.getPrimitive(p) 
                for p2 in range(prim.getNumPrimitives()): 
                    s = prim.getPrimitiveStart(p2) 
                    e = prim.getPrimitiveEnd(p2) 
                    v = [] 
                    for vi in range (s, e): 
                        vreader.setRow(prim.getVertex(vi)) 
                        v.append (xform(vreader.getData3f())) 
                    colPoly = CollisionPolygon(*v) 
                    cChild.addSolid(colPoly) 

            n=parent.attachNewNode (cChild) 
            #n.show()
            
    return parent

  
  def generateEggModel(self):
    data = EggData()
    model = EggGroup('model')
    data.addChild(model)
    self.generatePlanes(model)
    #self.generateRectangle(model)
    data.writeEgg(Filename("dump/plane.egg"))
    scene = NodePath(loadEggData(data))
    #self.addCollisionPolygons(scene)
    
    return scene  

    

  def getPlaneTriangles(self):
    from skimage import measure
    
    planeTriangles = []
    planeNormals = []    
    horizontalPlaneTriangles = []
    
    for planeIndex in xrange(self.numPlanes):
      mask = (self.segmentation == planeIndex).astype(np.uint8) * 255
      mask_ori = mask.copy()
      #contours, _ = cv2.findContours(mask, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
      #contours, _ = cv2.findContours(mask, cv2.RETR_LIST, cv2.CHAIN_APPROX_TC89_KCOS)
      masks = measure.label(mask.astype(np.int32), background=0)
      contours = []
      for maskIndex in xrange(masks.min() + 1, masks.max() + 1):
        mask = masks == maskIndex
        contour_mask = mask - np.logical_and(np.logical_and(np.roll(mask, shift=1, axis=0), np.roll(mask, shift=-1, axis=0)), np.logical_and(np.roll(mask, shift=1, axis=1), np.roll(mask, shift=-1, axis=1)))
        contour_v, contour_u = contour_mask.nonzero()
        contours.append(np.stack([contour_u, contour_v], axis=1))
        continue
        
        
      plane = self.planes[planeIndex]
      planeD = np.linalg.norm(plane)
      planeNormal = plane / np.maximum(planeD, 1e-4)

      # cv2.imwrite('test/mask.png', mask_ori)
      # #print(len(contours))

      # mask_ori = np.stack([mask_ori, mask_ori, mask_ori], 2)
      # count = 0
      # for contour in contours:
      #   count += contour.shape[0]
      #   for uv in contour:
      #     #uv = uv[0]
      #     mask_ori[uv[1]][uv[0]] = np.array([255, 0, 0])
      #     continue
      #   continue
      # cv2.imwrite('test/mask_contour.png', mask_ori)
      # if planeIndex == 1:
      #   exit(1)

      indices = np.arange(self.width * self.height).astype(np.float32)
      us = indices % self.width
      us = us / self.width * self.info[16] - self.camera['cx']
      vs = indices / self.width      
      vs = -(vs / self.height * self.info[17] - self.camera['cy'])
      ranges = np.stack([us / self.camera['fx'], np.ones(us.shape), vs / self.camera['fy']], axis=1)
      #print(ranges)
      #print(np.dot(ranges, planeNormal).shape)
      #print(np.dot(ranges, planeNormal))
      #print(ranges)
      #exit(1)
      depth = planeD / np.tensordot(ranges, planeNormal, axes=([1], [0]))
      XYZ = ranges * np.expand_dims(depth, -1)
      XYZ = XYZ.reshape((self.height, self.width, 3))
      for contour in contours:
        contour = contour.astype(np.float32)[::20]
        if contour.shape[0] < 3:
          continue
        rect = (0, 0, self.width, self.height)
        subdiv = cv2.Subdiv2D(rect)

        for point in contour:
          subdiv.insert((point[0], point[1]))
          continue
        triangleList = subdiv.getTriangleList()

        #print(contour)                
        #print(triangleList)
        #exit(1)
        for triangle2D in triangleList:
          triangle = []
          for vertexIndex in xrange(3):
            x = int(triangle2D[vertexIndex * 2 + 0])
            y = int(triangle2D[vertexIndex * 2 + 1])
            #print(x, y)
            if x < 0 or x >= self.width or y < 0 or y >= self.height:
              continue
            triangle.append(XYZ[y][x])
            continue
          if len(triangle) == 3:
            #print(triangle)
            if np.dot(np.cross(planeNormal, triangle[1] - triangle[0]), triangle[2] - triangle[0]) > 0:
              triangle = [triangle[0], triangle[2], triangle[1]]
              pass
            if planeIndex in self.horizontalPlanes:
              horizontalPlaneTriangles.append(triangle)
            else:
              planeTriangles.append(triangle)
              pass
            #planeNormals.append(planeNormal)
            pass
          continue
      continue
    planeTriangles = np.array(planeTriangles)
    #planeNormals = np.array(planeNormals)
    np.save('dump/' + str(self.scene_index) + '_plane_triangles.npy', planeTriangles)
    #np.save('dump/' + str(self.scene_index) + '_plane_normals.npy', planeNormals)
    return planeTriangles, horizontalPlaneTriangles, self.gravityDirection
  

  def getPlaneGeometries(self):
    if os.path.exists('dump/' + str(self.scene_index) + '_plane_triangles.npy'):
      print('loading')
      planeTriangles = np.load('dump/' + str(self.scene_index) + '_plane_triangles.npy')
      planeNormals =  np.load('dump/' + str(self.scene_index) + '_plane_normals.npy')
      return planeTriangles, planeNormals
      pass
    
    planeNormals = []
    planeTriangles = []
    for planeIndex in xrange(self.numPlanes):
      mask = (self.segmentation == planeIndex).astype(np.uint8) * 255
      #mask_ori = mask.copy()
      #contours, _ = cv2.findContours(mask, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
      plane = self.planes[planeIndex]
      planeD = np.linalg.norm(plane)
      planeNormal = plane / np.maximum(planeD, 1e-4)

      #cv2.imwrite('test/mask.png', mask)
      #v, u = mask.nonzero()
      u = np.arange(self.width * self.height) % self.width
      v = np.arange(self.width * self.height) / self.width
      u = u.astype(np.float32) / self.width * self.info[16] - self.camera['cx']
      v = -(v.astype(np.float32) / self.height * self.info[17] - self.camera['cy'])
      ranges = np.stack([u / self.camera['fx'], np.ones(u.shape), v / self.camera['fy']], axis=1)
      depth = planeD / np.dot(ranges, planeNormal)
      XYZ = ranges * np.expand_dims(depth, -1)
      XYZ = XYZ.reshape((self.height, self.width, 3))

      triangles = []
      for pixel in mask.reshape(-1).nonzero()[0]:
        x = pixel % self.width
        y = pixel / self.width
        for neighbors in [((x - 1, y), (x, y - 1)), ((x - 1, y), (x, y + 1)), ((x + 1, y), (x, y - 1)), ((x + 1, y), (x, y + 1))]:
          valid = True
          for neighbor in neighbors:
            if neighbor[0] < 0 or neighbor[0] >= self.width or neighbor[1] < 0 or neighbor[1] >= self.height or mask[neighbor[1]][neighbor[0]] == False:
              valid = False
              break
            continue
          if valid:
            triangle = [XYZ[y][x]]
            for neighbor in neighbors:
              triangle.append(XYZ[neighbor[1], neighbor[0]])
              continue
            triangles.append(triangle)
            pass
          continue
        continue
      planeTriangles.append(triangles)
      planeNormals.append(planeNormal)
      continue

    planeTriangles = np.array(planeTriangles)
    #planeNormals = np.array(planeNormals)
    #np.save('dump/' + str(self.scene_index) + '_plane_triangles.npy', planeTriangles)
    #np.save('dump/' + str(self.scene_index) + '_plane_normals.npy', planeNormals)
    return planeTriangles, planeNormals
  

  def calcHorizontalPlanes(self):
    from sklearn.cluster import KMeans
    
    planesD = np.linalg.norm(self.planes, axis=-1, keepdims=True)
    normals = self.planes / np.maximum(planesD, 1e-4)
    
    normals[normals[:, 1] < 0] *= -1    

    kmeans = KMeans(n_clusters=3).fit(normals)
    dominantNormals = kmeans.cluster_centers_
    dominantNormals = dominantNormals / np.maximum(np.linalg.norm(dominantNormals, axis=-1, keepdims=True), 1e-4)

    planeClusters = kmeans.predict(normals)
    
    horizontalNormalIndex = np.argmax(np.abs(dominantNormals[:, 2]))
    self.gravityDirection = dominantNormals[horizontalNormalIndex]
    self.horizontalPlanes = (planeClusters == horizontalNormalIndex).nonzero()[0]
    if self.gravityDirection[2] > 0:
      self.gravityDirection *= -1
      pass

    print(self.horizontalPlanes)
    print(self.gravityDirection)
    return
    
  def getHorizontalPlanes(self):
    return self.gravityDirection, self.horizontalPlanes

  def getHolePos(self):
    floorPlaneIndex = 2
    closePoint = np.array([0., 1.22, -0.2])
    plane = self.planes[floorPlaneIndex]
    planeD = np.linalg.norm(plane)
    planeNormal = plane / planeD
    distance = planeD - np.dot(planeNormal, closePoint)
    distance *= 0.99
    holePos = closePoint + planeNormal * distance

    H = P = R = 0
    H = -90 + np.rad2deg(np.arctan2(planeNormal[1], planeNormal[0]))
    #P = 90 - np.rad2deg(np.arccos(np.abs(planeNormal[2])))
    P = -90 + np.rad2deg(np.arccos(np.abs(planeNormal[2])))
    #print(H, P, R)
    return holePos, np.array([H, P, R])


  def getPortalPos(self):
    wallPlaneIndex = 1
    closePoint_1 = np.array([0.5, 1.35, -0.5])
    closePoint_2 = np.array([-0.4, 1, 0.19])
    plane = self.planes[wallPlaneIndex]
    planeD = np.linalg.norm(plane)
    planeNormal = plane / planeD
    
    distance = planeD - np.dot(planeNormal, closePoint_1)
    distance *= 0.95
    portalPos_1 = closePoint_1 + planeNormal * distance

    distance = planeD - np.dot(planeNormal, closePoint_2)
    distance *= 0.95
    portalPos_2 = closePoint_2 + planeNormal * distance    

    H = P = R = 0
    H = -90 + np.rad2deg(np.arctan2(planeNormal[1], planeNormal[0]))
    #P = 90 - np.rad2deg(np.arccos(np.abs(planeNormal[2])))
    P = -90 + np.rad2deg(np.arccos(-np.abs(planeNormal[2])))
    #print(H, P, R)
    return portalPos_1, np.array([H, P, R]), portalPos_2, np.array([H, P, R]), planeNormal
class Floorplan():
    def __init__(self, filename):
        self.wallWidth = 0.005
        self.wallHeight = 0.3
        self.doorWidth = self.wallWidth
        self.doorHeight = self.wallHeight * 0.8
        self.windowWidth = self.wallWidth + 0.0001
        self.windowHeight = self.wallHeight * 0.5
        self.windowOffset = self.wallHeight * 0.4
        self.filename = filename

        self.floorMat = ObjMaterial()
        #self.floorMat.filename = self.filename + '.png'
        self.floorMat.name = 'floor'
        self.floorMat.put('map_Kd', self.filename + '.png')

        self.ceilingMat = ObjMaterial()
        self.ceilingMat.name = 'ceiling'
        self.ceilingMat.put('map_Kd', 'data/ceiling.jpg')

        self.wallMats = []
        wallMat_1 = ObjMaterial()
        wallMat_1.name = 'wall_1'
        wallMat_1.put('map_Kd', 'data/bedroom_wall.jpg')
        wallMat_2 = ObjMaterial()
        wallMat_2.name = 'wall_2'
        wallMat_2.put('map_Kd', 'data/kitchen_wall.jpg')
        wallMat_3 = ObjMaterial()
        wallMat_3.name = 'wall_3'
        wallMat_3.put('map_Kd', 'data/dining_wall.jpg')
        wallMat_4 = ObjMaterial()
        wallMat_4.name = 'wall_4'
        wallMat_4.put('map_Kd', 'data/bathroom_wall.jpg')
        wallMat_5 = ObjMaterial()
        wallMat_5.name = 'wall_5'
        wallMat_5.put('map_Kd', 'data/wall.jpg')

        self.wallMats.append(wallMat_3)
        self.wallMats.append(wallMat_2)
        self.wallMats.append(wallMat_1)
        self.wallMats.append(wallMat_4)
        self.wallMats.append(wallMat_4)
        self.wallMats.append(wallMat_1)
        self.wallMats.append(wallMat_5)
        self.wallMats.append(wallMat_2)
        self.wallMats.append(wallMat_4)
        self.wallMats.append(wallMat_5)
        self.wallMats.append(wallMat_5)

        self.doorMat = ObjMaterial()
        self.doorMat.name = 'door'
        self.doorMat.put('map_Kd', 'data/door.jpg')

        self.windowMat = ObjMaterial()
        self.windowMat.name = 'window'
        self.windowMat.put('map_Kd', 'data/window.jpg')

        self.iconNodes = {}
        self.iconNodes['cooking_counter'] = base.loader.loadModel(
            'data/cooking_counter.egg')
        self.iconNodes['bathtub'] = base.loader.loadModel('data/bathtub.egg')
        self.iconNodes['toilet'] = base.loader.loadModel('data/toilet.egg')
        self.iconNodes['washing_basin'] = base.loader.loadModel(
            'data/washing_basin.egg')
        return

    def read(self):
        floorplanFile = open(self.filename + '.txt', 'r')
        self.walls = []
        self.doors = []
        self.icons = []
        self.wallsInt = []
        for line in floorplanFile.readlines():
            line = line.strip()
            values = line.split('\t')
            if len(values) == 2:
                self.width = float(values[0])
                self.height = float(values[1])
                self.maxDim = max(self.width, self.height)
            elif len(values) == 6:
                wall = []
                for i in xrange(4):
                    wall.append(float(values[i]))
                    continue
                lineDim = calcLineDim(((wall[0], wall[1]), (wall[2], wall[3])))
                wall[lineDim], wall[2 + lineDim] = min(wall[lineDim],
                                                       wall[2 + lineDim]), max(
                                                           wall[lineDim],
                                                           wall[2 + lineDim])
                wall[1 - lineDim] = wall[3 - lineDim] = (wall[1 - lineDim] +
                                                         wall[3 - lineDim]) / 2
                wall.append(int(values[4]) - 1)
                wall.append(int(values[5]) - 1)
                for pointIndex in xrange(2):
                    wall[pointIndex * 2 + 0] /= self.maxDim
                    wall[pointIndex * 2 + 1] /= self.maxDim
                    continue
                self.walls.append(wall)

                wallInt = []
                for i in xrange(4):
                    wallInt.append(int(values[i]))
                    continue
                wallInt[lineDim], wallInt[2 + lineDim] = min(
                    wallInt[lineDim],
                    wallInt[2 + lineDim]), max(wallInt[lineDim],
                                               wallInt[2 + lineDim])
                self.wallsInt.append(wallInt)
            elif len(values) == 7:
                item = []
                for i in xrange(4):
                    item.append(float(values[i]))

                for pointIndex in xrange(2):
                    item[pointIndex * 2 + 0] /= self.maxDim
                    item[pointIndex * 2 + 1] /= self.maxDim
                    continue

                if values[4] == 'door':
                    self.doors.append(item)
                else:
                    item.append(values[4])
                    self.icons.append(item)
                    pass
                pass
            continue
        return

    def generateFloor(self, data):
        floorGroup = EggGroup('floor')
        data.addChild(floorGroup)

        vp = EggVertexPool('floor_vertex')
        floorGroup.addChild(vp)

        exteriorWalls = []
        for wall in self.walls:
            if wall[4] == 10 or wall[5] == 10:
                exteriorWalls.append(copy.deepcopy(wall))
                pass
            continue

        exteriorOpenings = []
        for wall in exteriorWalls:
            lineDim = calcLineDim((wall[:2], wall[2:4]))
            for doorIndex, door in enumerate(self.doors):
                if calcLineDim((door[:2], door[2:4])) != lineDim:
                    continue
                if door[lineDim] >= wall[lineDim] and door[
                        2 + lineDim] <= wall[2 + lineDim] and abs(
                            door[1 - lineDim] -
                            wall[1 - lineDim]) <= self.wallWidth:
                    exteriorOpenings.append(doorIndex)
                    pass
                continue
            continue

        minDistance = 10000
        mainDoorIndex = -1
        for icon in self.icons:
            if icon[4] == 'entrance':
                for doorIndex in exteriorOpenings:
                    door = self.doors[doorIndex]
                    distance = pow(
                        pow((door[0] + door[2]) / 2 -
                            (icon[0] + icon[2]) / 2, 2) + pow(
                                (door[1] + door[3]) / 2 -
                                (icon[1] + icon[3]) / 2, 2), 0.5)
                    if distance < minDistance:
                        minDistance = distance
                        mainDoorIndex = doorIndex
                        pass
                    continue
                break
            continue

        self.startCameraPos = [0.5, -0.5, self.wallHeight * 0.5]
        self.startTarget = [0.5, 0.5, self.wallHeight * 0.5]
        if mainDoorIndex >= 0:
            mainDoor = self.doors[mainDoorIndex]
            lineDim = calcLineDim((mainDoor[:2], mainDoor[2:4]))
            fixedValue = (mainDoor[1 - lineDim] + mainDoor[3 - lineDim]) / 2
            imageSize = [self.width / self.maxDim, self.height / self.maxDim]
            side = int(fixedValue < imageSize[1 - lineDim] * 0.5) * 2 - 1
            self.startCameraPos[lineDim] = (mainDoor[lineDim] +
                                            mainDoor[2 + lineDim]) / 2
            self.startTarget[lineDim] = (mainDoor[lineDim] +
                                         mainDoor[2 + lineDim]) / 2
            self.startCameraPos[1 - lineDim] = fixedValue - 0.5 * side
            self.startTarget[1 - lineDim] = fixedValue + 0.5 * side

            self.startCameraPos[0] = 1 - self.startCameraPos[0]
            self.startTarget[0] = 1 - self.startTarget[0]
            pass

        newDoors = []
        self.windows = []
        for doorIndex, door in enumerate(self.doors):
            if doorIndex == mainDoorIndex or doorIndex not in exteriorOpenings:
                newDoors.append(door)
            else:
                self.windows.append(door)
                pass
            continue
        self.doors = newDoors

        exteriorWallLoops = []
        visitedMask = {}
        gap = 5.0 / self.maxDim
        for wallIndex, wall in enumerate(exteriorWalls):
            if wallIndex in visitedMask:
                continue
            visitedMask[wallIndex] = True
            exteriorWallLoop = []
            exteriorWallLoop.append(wall)
            for loopWall in exteriorWallLoop:
                for neighborWallIndex, neighborWall in enumerate(
                        exteriorWalls):
                    if neighborWallIndex in visitedMask:
                        continue
                    #if calcDistance(neighborWall[:2], loopWall[:2]) < gap or calcDistance(neighborWall[2:4], loopWall[:2]) < gap or calcDistance(neighborWall[:2], loopWall[2:4]) < gap or calcDistance(neighborWall[2:4], loopWall[2:4]) < gap:
                    if calcDistance(neighborWall[:2], loopWall[2:4]) < gap:
                        exteriorWallLoop.append(neighborWall)
                        visitedMask[neighborWallIndex] = True
                        break
                    elif calcDistance(neighborWall[2:4], loopWall[2:4]) < gap:
                        neighborWall[0], neighborWall[2] = neighborWall[
                            2], neighborWall[0]
                        neighborWall[1], neighborWall[3] = neighborWall[
                            3], neighborWall[1]
                        exteriorWallLoop.append(neighborWall)
                        visitedMask[neighborWallIndex] = True
                        break
                    continue
                continue
            exteriorWallLoops.append(exteriorWallLoop)
            continue

        for exteriorWallLoop in exteriorWallLoops:
            poly = EggPolygon()
            floorGroup.addChild(poly)

            poly.setTexture(self.floorMat.getEggTexture())
            poly.setMaterial(self.floorMat.getEggMaterial())

            for wallIndex, wall in enumerate(exteriorWallLoop):
                if wallIndex == 0:
                    v = EggVertex()
                    v.setPos(Point3D(1 - wall[0], wall[1], 0))
                    v.setUv(
                        Point2D(wall[0] * self.maxDim / self.width,
                                1 - wall[1] * self.maxDim / self.height))
                    poly.addVertex(vp.addVertex(v))
                else:
                    v = EggVertex()
                    v.setPos(
                        Point3D(
                            1 -
                            (wall[0] + exteriorWallLoop[wallIndex - 1][2]) / 2,
                            (wall[1] + exteriorWallLoop[wallIndex - 1][3]) / 2,
                            0))
                    v.setUv(
                        Point2D(
                            (wall[0] + exteriorWallLoop[wallIndex - 1][2]) /
                            2 * self.maxDim / self.width, 1 -
                            (wall[1] + exteriorWallLoop[wallIndex - 1][3]) /
                            2 * self.maxDim / self.height))
                    poly.addVertex(vp.addVertex(v))
                    pass
                if wallIndex == len(exteriorWallLoop) - 1:
                    v = EggVertex()
                    v.setPos(Point3D(1 - wall[2], wall[3], 0))
                    v.setUv(
                        Point2D(wall[2] * self.maxDim / self.width,
                                1 - wall[3] * self.maxDim / self.height))
                    poly.addVertex(vp.addVertex(v))
                    pass
                continue
            continue

        ceilingGroup = EggGroup('ceiling')
        data.addChild(ceilingGroup)

        vp = EggVertexPool('ceiling_vertex')
        ceilingGroup.addChild(vp)

        for exteriorWallLoop in exteriorWallLoops:
            poly = EggPolygon()
            ceilingGroup.addChild(poly)

            poly.setTexture(self.ceilingMat.getEggTexture())
            poly.setMaterial(self.ceilingMat.getEggMaterial())

            for wallIndex, wall in enumerate(exteriorWallLoop):
                if wallIndex == 0:
                    v = EggVertex()
                    v.setPos(Point3D(1 - wall[0], wall[1], self.wallHeight))
                    v.setUv(Point2D(wall[0], 1 - wall[1]))
                    poly.addVertex(vp.addVertex(v))
                else:
                    v = EggVertex()
                    v.setPos(
                        Point3D(
                            1 -
                            (wall[0] + exteriorWallLoop[wallIndex - 1][2]) / 2,
                            (wall[1] + exteriorWallLoop[wallIndex - 1][3]) / 2,
                            self.wallHeight))
                    v.setUv(
                        Point2D(
                            (wall[0] + exteriorWallLoop[wallIndex - 1][2]) / 2,
                            1 -
                            (wall[1] + exteriorWallLoop[wallIndex - 1][3]) /
                            2))
                    poly.addVertex(vp.addVertex(v))
                    pass
                if wallIndex == len(exteriorWallLoop) - 1:
                    v = EggVertex()
                    v.setPos(Point3D(1 - wall[2], wall[3], self.wallHeight))
                    v.setUv(Point2D(wall[2], 1 - wall[3]))
                    poly.addVertex(vp.addVertex(v))
                    pass
                continue
            continue

        return

    def generateWalls(self, data):

        wallsGroup = EggGroup('walls')
        data.addChild(wallsGroup)

        vp = EggVertexPool('wall_vertex')
        data.addChild(vp)

        for wallIndex, wall in enumerate(self.walls):
            wallGroup = EggGroup('wall')
            wallsGroup.addChild(wallGroup)
            lineDim = calcLineDim((wall[:2], wall[2:4]))
            # if lineDim == 0:
            #   if wall[lineDim] < wall[2 + lineDim]:
            #     deltas = (-self.wallWidth, self.wallWidth, self.wallWidth, self.wallWidth)
            #   else:
            #     deltas = (self.wallWidth, self.wallWidth, -self.wallWidth, self.wallWidth)
            #     pass
            # else:
            #   if wall[lineDim] < wall[2 + lineDim]:
            #     deltas = (self.wallWidth, -self.wallWidth, self.wallWidth, self.wallWidth)
            #   else:
            #     deltas = (self.wallWidth, self.wallWidth, self.wallWidth, -self.wallWidth)
            #     pass
            #   pass

            #print(self.wallsInt[wallIndex])
            #print(wall)
            if lineDim == 0:
                deltas = (0, self.wallWidth)
            else:
                deltas = (self.wallWidth, 0)
                pass

            poly = EggPolygon()
            wallGroup.addChild(poly)

            if lineDim == 0:
                poly.setTexture(self.wallMats[wall[4]].getEggTexture())
                poly.setMaterial(self.wallMats[wall[4]].getEggMaterial())
            else:
                poly.setTexture(self.wallMats[wall[5]].getEggTexture())
                poly.setMaterial(self.wallMats[wall[5]].getEggMaterial())
                pass

            values = [
                wall[lineDim] - self.wallWidth + 0.0001,
                wall[2 + lineDim] + self.wallWidth - 0.0001
            ]
            for door in self.doors:
                if calcLineDim((door[:2], door[2:4])) != lineDim:
                    continue
                if door[lineDim] >= wall[lineDim] and door[
                        2 + lineDim] <= wall[2 + lineDim] and abs(
                            door[1 - lineDim] -
                            wall[1 - lineDim]) <= self.wallWidth:
                    values.append(door[lineDim])
                    values.append(door[2 + lineDim])
                    #doors.append(door)
                    pass
                continue

            values.sort()

            fixedValue = (wall[1 - lineDim] + wall[3 - lineDim]) / 2
            for valueIndex, value in enumerate(values):
                if valueIndex % 2 == 0 and valueIndex > 0:
                    v = EggVertex()
                    if lineDim == 0:
                        v.setPos(
                            Point3D(1 - (value - deltas[0]),
                                    fixedValue - deltas[1], self.doorHeight))
                    else:
                        v.setPos(
                            Point3D(1 - (fixedValue - deltas[0]),
                                    value - deltas[1], self.doorHeight))
                        pass
                    v.setUv(
                        Point2D(self.doorHeight / self.wallHeight,
                                (value - wall[lineDim]) /
                                (wall[2 + lineDim] - wall[lineDim])))
                    poly.addVertex(vp.addVertex(v))
                    pass

                v = EggVertex()
                if lineDim == 0:
                    v.setPos(
                        Point3D(1 - (value - deltas[0]),
                                fixedValue - deltas[1], 0))
                else:
                    v.setPos(
                        Point3D(1 - (fixedValue - deltas[0]),
                                value - deltas[1], 0))
                    pass
                v.setUv(
                    Point2D(0, (value - wall[lineDim]) /
                            (wall[2 + lineDim] - wall[lineDim])))
                poly.addVertex(vp.addVertex(v))

                if valueIndex % 2 == 1 and valueIndex + 1 < len(values):
                    v = EggVertex()
                    if lineDim == 0:
                        v.setPos(
                            Point3D(1 - (value - deltas[0]),
                                    fixedValue - deltas[1], self.doorHeight))
                    else:
                        v.setPos(
                            Point3D(1 - (fixedValue - deltas[0]),
                                    value - deltas[1], self.doorHeight))
                        pass
                    v.setUv(
                        Point2D(self.doorHeight / self.wallHeight,
                                (value - wall[lineDim]) /
                                (wall[2 + lineDim] - wall[lineDim])))
                    poly.addVertex(vp.addVertex(v))
                    pass
                continue
            # v = EggVertex()
            # v.setPos(Point3D(wall[0] - deltas[0], wall[1] - deltas[1], 0))
            # v.setUv(Point2D(0, 0))
            # poly.addVertex(vp.addVertex(v))

            # v = EggVertex()
            # v.setPos(Point3D(wall[2] - deltas[0], wall[3] - deltas[1], 0))
            # v.setUv(Point2D(0, 1))
            # poly.addVertex(vp.addVertex(v))

            v = EggVertex()
            if lineDim == 0:
                v.setPos(
                    Point3D(1 - (values[len(values) - 1] - deltas[0]),
                            fixedValue - deltas[1], self.wallHeight))
            else:
                v.setPos(
                    Point3D(1 - (fixedValue - deltas[0]),
                            values[len(values) - 1] - deltas[1],
                            self.wallHeight))
                pass
            v.setUv(Point2D(1, 1))
            poly.addVertex(vp.addVertex(v))

            v = EggVertex()
            if lineDim == 0:
                v.setPos(
                    Point3D(1 - (values[0] - deltas[0]),
                            fixedValue - deltas[1], self.wallHeight))
            else:
                v.setPos(
                    Point3D(1 - (fixedValue - deltas[0]),
                            values[0] - deltas[1], self.wallHeight))
                pass
            v.setUv(Point2D(1, 0))
            poly.addVertex(vp.addVertex(v))

            # for v in poly.getVertices():
            #   print(v)
            #   continue

            poly = EggPolygon()
            wallGroup.addChild(poly)
            if lineDim == 0:
                poly.setTexture(self.wallMats[wall[5]].getEggTexture())
                poly.setMaterial(self.wallMats[wall[5]].getEggMaterial())
            else:
                poly.setTexture(self.wallMats[wall[4]].getEggTexture())
                poly.setMaterial(self.wallMats[wall[4]].getEggMaterial())
                pass

            #deltas = (0.1, 0.1)

            for valueIndex, value in enumerate(values):
                if valueIndex % 2 == 0 and valueIndex > 0:
                    v = EggVertex()
                    if lineDim == 0:
                        v.setPos(
                            Point3D(1 - (value + deltas[0]),
                                    fixedValue + deltas[1], self.doorHeight))
                    else:
                        v.setPos(
                            Point3D(1 - (fixedValue + deltas[0]),
                                    value + deltas[1], self.doorHeight))
                        pass
                    v.setUv(
                        Point2D(self.doorHeight / self.wallHeight,
                                (value - wall[lineDim]) /
                                (wall[2 + lineDim] - wall[lineDim])))
                    poly.addVertex(vp.addVertex(v))
                    pass

                v = EggVertex()
                if lineDim == 0:
                    v.setPos(
                        Point3D(1 - (value + deltas[0]),
                                fixedValue + deltas[1], 0))
                else:
                    v.setPos(
                        Point3D(1 - (fixedValue + deltas[0]),
                                value + deltas[1], 0))
                    pass
                v.setUv(
                    Point2D(0, (value - wall[lineDim]) /
                            (wall[2 + lineDim] - wall[lineDim])))
                poly.addVertex(vp.addVertex(v))

                if valueIndex % 2 == 1 and valueIndex + 1 < len(values):
                    v = EggVertex()
                    if lineDim == 0:
                        v.setPos(
                            Point3D(1 - (value + deltas[0]),
                                    fixedValue + deltas[1], self.doorHeight))
                    else:
                        v.setPos(
                            Point3D(1 - (fixedValue + deltas[0]),
                                    value + deltas[1], self.doorHeight))
                        pass
                    v.setUv(
                        Point2D(self.doorHeight / self.wallHeight,
                                (value - wall[lineDim]) /
                                (wall[2 + lineDim] - wall[lineDim])))
                    poly.addVertex(vp.addVertex(v))
                    pass
                continue
            # v = EggVertex()
            # v.setPos(Point3D(wall[0] + deltas[0], wall[1] + deltas[1], 0))
            # v.setUv(Point2D(0, 0))
            # poly.addVertex(vp.addVertex(v))

            # v = EggVertex()
            # v.setPos(Point3D(wall[2] + deltas[0], wall[3] + deltas[1], 0))
            # v.setUv(Point2D(0, 1))
            # poly.addVertex(vp.addVertex(v))

            v = EggVertex()
            if lineDim == 0:
                v.setPos(
                    Point3D(1 - (values[len(values) - 1] + deltas[0]),
                            fixedValue + deltas[1], self.wallHeight))
            else:
                v.setPos(
                    Point3D(1 - (fixedValue + deltas[0]),
                            values[len(values) - 1] + deltas[1],
                            self.wallHeight))
                pass
            v.setUv(Point2D(1, 1))
            poly.addVertex(vp.addVertex(v))

            v = EggVertex()
            if lineDim == 0:
                v.setPos(
                    Point3D(1 - (values[0] + deltas[0]),
                            fixedValue + deltas[1], self.wallHeight))
            else:
                v.setPos(
                    Point3D(1 - (fixedValue + deltas[0]),
                            values[0] + deltas[1], self.wallHeight))
                pass
            v.setUv(Point2D(1, 0))
            poly.addVertex(vp.addVertex(v))

            if lineDim == 0:
                poly = EggPolygon()
                wallGroup.addChild(poly)
                poly.setTexture(self.wallMats[10].getEggTexture())
                poly.setMaterial(self.wallMats[10].getEggMaterial())

                v = EggVertex()
                v.setPos(Point3D(1 - values[0], fixedValue - deltas[1], 0))
                v.setUv(Point2D(0, 0))
                poly.addVertex(vp.addVertex(v))

                v = EggVertex()
                v.setPos(
                    Point3D(1 - values[0], fixedValue - deltas[1],
                            self.wallHeight))
                v.setUv(Point2D(0, 1))
                poly.addVertex(vp.addVertex(v))

                v = EggVertex()
                v.setPos(
                    Point3D(1 - values[0], fixedValue + deltas[1],
                            self.wallHeight))
                v.setUv(Point2D(1, 1))
                poly.addVertex(vp.addVertex(v))

                v = EggVertex()
                v.setPos(Point3D(1 - values[0], fixedValue + deltas[1], 0))
                v.setUv(Point2D(1, 0))
                poly.addVertex(vp.addVertex(v))

                poly = EggPolygon()
                wallGroup.addChild(poly)
                poly.setTexture(self.wallMats[10].getEggTexture())
                poly.setMaterial(self.wallMats[10].getEggMaterial())

                v = EggVertex()
                v.setPos(
                    Point3D(1 - values[0], fixedValue - deltas[1],
                            self.wallHeight))
                v.setUv(Point2D(0, 0))
                poly.addVertex(vp.addVertex(v))

                v = EggVertex()
                v.setPos(
                    Point3D(1 - values[len(values) - 1],
                            fixedValue - deltas[1], self.wallHeight))
                v.setUv(Point2D(0, 1))
                poly.addVertex(vp.addVertex(v))

                v = EggVertex()
                v.setPos(
                    Point3D(1 - values[len(values) - 1],
                            fixedValue + deltas[1], self.wallHeight))
                v.setUv(Point2D(1, 1))
                poly.addVertex(vp.addVertex(v))

                v = EggVertex()
                v.setPos(
                    Point3D(1 - values[0], fixedValue + deltas[1],
                            self.wallHeight))
                v.setUv(Point2D(1, 0))
                poly.addVertex(vp.addVertex(v))

                poly = EggPolygon()
                wallGroup.addChild(poly)
                poly.setTexture(self.wallMats[10].getEggTexture())
                poly.setMaterial(self.wallMats[10].getEggMaterial())

                v = EggVertex()
                v.setPos(
                    Point3D(1 - values[len(values) - 1],
                            fixedValue - deltas[1], self.wallHeight))
                v.setUv(Point2D(0, 0))
                poly.addVertex(vp.addVertex(v))

                v = EggVertex()
                v.setPos(
                    Point3D(1 - values[len(values) - 1],
                            fixedValue - deltas[1], 0))
                v.setUv(Point2D(0, 1))
                poly.addVertex(vp.addVertex(v))

                v = EggVertex()
                v.setPos(
                    Point3D(1 - values[len(values) - 1],
                            fixedValue + deltas[1], 0))
                v.setUv(Point2D(1, 1))
                poly.addVertex(vp.addVertex(v))

                v = EggVertex()
                v.setPos(
                    Point3D(1 - values[len(values) - 1],
                            fixedValue + deltas[1], self.wallHeight))
                v.setUv(Point2D(1, 0))
                poly.addVertex(vp.addVertex(v))

            else:
                poly = EggPolygon()
                wallGroup.addChild(poly)
                poly.setTexture(self.wallMats[10].getEggTexture())
                poly.setMaterial(self.wallMats[10].getEggMaterial())

                v = EggVertex()
                v.setPos(Point3D(1 - (fixedValue - deltas[0]), values[0], 0))
                v.setUv(Point2D(0, 0))
                poly.addVertex(vp.addVertex(v))

                v = EggVertex()
                v.setPos(
                    Point3D(1 - (fixedValue - deltas[0]), values[0],
                            self.wallHeight))
                v.setUv(Point2D(0, 1))
                poly.addVertex(vp.addVertex(v))

                v = EggVertex()
                v.setPos(
                    Point3D(1 - (fixedValue + deltas[0]), values[0],
                            self.wallHeight))
                v.setUv(Point2D(1, 1))
                poly.addVertex(vp.addVertex(v))

                v = EggVertex()
                v.setPos(Point3D(1 - (fixedValue + deltas[0]), values[0], 0))
                v.setUv(Point2D(1, 0))
                poly.addVertex(vp.addVertex(v))

                poly = EggPolygon()
                wallGroup.addChild(poly)
                poly.setTexture(self.wallMats[10].getEggTexture())
                poly.setMaterial(self.wallMats[10].getEggMaterial())

                v = EggVertex()
                v.setPos(
                    Point3D(1 - (fixedValue - deltas[0]), values[0],
                            self.wallHeight))
                v.setUv(Point2D(0, 0))
                poly.addVertex(vp.addVertex(v))

                v = EggVertex()
                v.setPos(
                    Point3D(1 - (fixedValue - deltas[0]),
                            values[len(values) - 1], self.wallHeight))
                v.setUv(Point2D(0, 1))
                poly.addVertex(vp.addVertex(v))

                v = EggVertex()
                v.setPos(
                    Point3D(1 - (fixedValue + deltas[0]),
                            values[len(values) - 1], self.wallHeight))
                v.setUv(Point2D(1, 1))
                poly.addVertex(vp.addVertex(v))

                v = EggVertex()
                v.setPos(
                    Point3D(1 - (fixedValue + deltas[0]), values[0],
                            self.wallHeight))
                v.setUv(Point2D(1, 0))
                poly.addVertex(vp.addVertex(v))

                poly = EggPolygon()
                wallGroup.addChild(poly)
                poly.setTexture(self.wallMats[10].getEggTexture())
                poly.setMaterial(self.wallMats[10].getEggMaterial())

                v = EggVertex()
                v.setPos(
                    Point3D(1 - (fixedValue - deltas[0]),
                            values[len(values) - 1], self.wallHeight))
                v.setUv(Point2D(0, 0))
                poly.addVertex(vp.addVertex(v))

                v = EggVertex()
                v.setPos(
                    Point3D(1 - (fixedValue - deltas[0]),
                            values[len(values) - 1], 0))
                v.setUv(Point2D(0, 1))
                poly.addVertex(vp.addVertex(v))

                v = EggVertex()
                v.setPos(
                    Point3D(1 - (fixedValue + deltas[0]),
                            values[len(values) - 1], 0))
                v.setUv(Point2D(1, 1))
                poly.addVertex(vp.addVertex(v))

                v = EggVertex()
                v.setPos(
                    Point3D(1 - (fixedValue + deltas[0]),
                            values[len(values) - 1], self.wallHeight))
                v.setUv(Point2D(1, 0))
                poly.addVertex(vp.addVertex(v))

            continue
        return

    def generateDoors(self, data):
        doorsGroup = EggGroup('doors')
        data.addChild(doorsGroup)

        vp = EggVertexPool('door_vertex')
        doorsGroup.addChild(vp)

        for doorIndex, door in enumerate(self.doors):
            doorGroup = EggGroup('door_' + str(doorIndex))
            doorsGroup.addChild(doorGroup)

            lineDim = calcLineDim((door[:2], door[2:4]))

            if lineDim == 0:
                deltas = (0, self.doorWidth)
            else:
                deltas = (self.doorWidth, 0)
                pass

            poly = EggPolygon()
            doorGroup.addChild(poly)
            poly.setTexture(self.doorMat.getEggTexture())
            poly.setMaterial(self.doorMat.getEggMaterial())

            v = EggVertex()
            v.setPos(Point3D(1 - (door[0] - deltas[0]), door[1] - deltas[1],
                             0))
            v.setUv(Point2D(0, 0))
            poly.addVertex(vp.addVertex(v))

            v = EggVertex()
            v.setPos(Point3D(1 - (door[2] - deltas[0]), door[3] - deltas[1],
                             0))
            v.setUv(Point2D(1, 0))
            poly.addVertex(vp.addVertex(v))

            v = EggVertex()
            v.setPos(
                Point3D(1 - (door[2] - deltas[0]), door[3] - deltas[1],
                        self.doorHeight))
            v.setUv(Point2D(1, 1))
            poly.addVertex(vp.addVertex(v))

            v = EggVertex()
            v.setPos(
                Point3D(1 - (door[0] - deltas[0]), door[1] - deltas[1],
                        self.doorHeight))
            v.setUv(Point2D(0, 1))
            poly.addVertex(vp.addVertex(v))

            poly = EggPolygon()
            doorGroup.addChild(poly)
            poly.setTexture(self.doorMat.getEggTexture())
            poly.setMaterial(self.doorMat.getEggMaterial())

            v = EggVertex()
            v.setPos(Point3D(1 - (door[0] + deltas[0]), door[1] + deltas[1],
                             0))
            v.setUv(Point2D(0, 0))
            poly.addVertex(vp.addVertex(v))

            v = EggVertex()
            v.setPos(Point3D(1 - (door[2] + deltas[0]), door[3] + deltas[1],
                             0))
            v.setUv(Point2D(1, 0))
            poly.addVertex(vp.addVertex(v))

            v = EggVertex()
            v.setPos(
                Point3D(1 - (door[2] + deltas[0]), door[3] + deltas[1],
                        self.doorHeight))
            v.setUv(Point2D(1, 1))
            poly.addVertex(vp.addVertex(v))

            v = EggVertex()
            v.setPos(
                Point3D(1 - (door[0] + deltas[0]), door[1] + deltas[1],
                        self.doorHeight))
            v.setUv(Point2D(0, 1))
            poly.addVertex(vp.addVertex(v))

            continue
        return

    def generateWindows(self, data):
        windowsGroup = EggGroup('windows')
        data.addChild(windowsGroup)

        vp = EggVertexPool('window_vertex')
        windowsGroup.addChild(vp)

        for windowIndex, window in enumerate(self.windows):
            windowGroup = EggGroup('window_' + str(windowIndex))
            windowsGroup.addChild(windowGroup)

            lineDim = calcLineDim((window[:2], window[2:4]))

            if lineDim == 0:
                deltas = (0, self.windowWidth)
            else:
                deltas = (self.windowWidth, 0)
                pass

            poly = EggPolygon()
            windowGroup.addChild(poly)
            poly.setTexture(self.windowMat.getEggTexture())
            poly.setMaterial(self.windowMat.getEggMaterial())

            v = EggVertex()
            v.setPos(
                Point3D(1 - (window[0] - deltas[0]), window[1] - deltas[1],
                        self.windowOffset))
            v.setUv(Point2D(0, 0))
            poly.addVertex(vp.addVertex(v))

            v = EggVertex()
            v.setPos(
                Point3D(1 - (window[2] - deltas[0]), window[3] - deltas[1],
                        self.windowOffset))
            v.setUv(Point2D(1, 0))
            poly.addVertex(vp.addVertex(v))

            v = EggVertex()
            v.setPos(
                Point3D(1 - (window[2] - deltas[0]), window[3] - deltas[1],
                        self.windowOffset + self.windowHeight))
            v.setUv(Point2D(1, 1))
            poly.addVertex(vp.addVertex(v))

            v = EggVertex()
            v.setPos(
                Point3D(1 - (window[0] - deltas[0]), window[1] - deltas[1],
                        self.windowOffset + self.windowHeight))
            v.setUv(Point2D(0, 1))
            poly.addVertex(vp.addVertex(v))

            poly = EggPolygon()
            windowGroup.addChild(poly)
            poly.setTexture(self.windowMat.getEggTexture())
            poly.setMaterial(self.windowMat.getEggMaterial())

            v = EggVertex()
            v.setPos(
                Point3D(1 - (window[0] + deltas[0]), window[1] + deltas[1],
                        self.windowOffset))
            v.setUv(Point2D(0, 0))
            poly.addVertex(vp.addVertex(v))

            v = EggVertex()
            v.setPos(
                Point3D(1 - (window[2] + deltas[0]), window[3] + deltas[1],
                        self.windowOffset))
            v.setUv(Point2D(1, 0))
            poly.addVertex(vp.addVertex(v))

            v = EggVertex()
            v.setPos(
                Point3D(1 - (window[2] + deltas[0]), window[3] + deltas[1],
                        self.windowOffset + self.windowHeight))
            v.setUv(Point2D(1, 1))
            poly.addVertex(vp.addVertex(v))

            v = EggVertex()
            v.setPos(
                Point3D(1 - (window[0] + deltas[0]), window[1] + deltas[1],
                        self.windowOffset + self.windowHeight))
            v.setUv(Point2D(0, 1))
            poly.addVertex(vp.addVertex(v))

            continue
        return

    def generateIcons(self, scene):
        for icon in self.icons:
            if icon[4] not in self.iconNodes:
                continue
            #self.iconNodes[icon[4]].reparentTo(scene)
            node = deepcopy(self.iconNodes[icon[4]])
            node.setHpr(0, -90, 0)
            mins, maxs = node.getTightBounds()
            dimensions = Point3(maxs - mins)

            minDistances = [self.maxDim, self.maxDim, self.maxDim, self.maxDim]
            for wall in self.walls:
                lineDim = calcLineDim(((wall[0], wall[1]), (wall[2], wall[3])))
                if lineDim == -1:
                    continue
                if ((icon[lineDim] + icon[2 + lineDim]) / 2 -
                        wall[lineDim]) * (
                            (icon[lineDim] + icon[2 + lineDim]) / 2 -
                            wall[2 + lineDim]) > 0:
                    continue
                side = int(wall[1 - lineDim] >
                           (icon[1 - lineDim] + icon[3 - lineDim]) / 2)
                index = lineDim * 2 + side
                distance = abs(wall[1 - lineDim] -
                               icon[1 - lineDim + side * 2])
                if distance < minDistances[index]:
                    minDistances[index] = distance
                    pass
                continue

            orientation = 0
            if icon[4] in ['cooking_counter']:
                if icon[2] - icon[0] > icon[3] - icon[1]:
                    if minDistances[0] < minDistances[1]:
                        orientation = 0
                    else:
                        orientation = 1
                else:
                    if minDistances[2] < minDistances[3]:
                        orientation = 2
                    else:
                        orientation = 3
                        pass
                    pass
            elif icon[4] in ['toilet']:
                if icon[2] - icon[0] < icon[3] - icon[1]:
                    if minDistances[0] < minDistances[1]:
                        orientation = 0
                    else:
                        orientation = 1
                else:
                    if minDistances[2] < minDistances[3]:
                        orientation = 2
                    else:
                        orientation = 3
                        pass
                    pass
            elif icon[4] in ['washing_basin']:
                orientation = np.argmin(minDistances)
                pass

            if orientation == 1:
                node.setH(180)
            elif orientation == 2:
                node.setH(90)
            elif orientation == 3:
                node.setH(270)
                pass
            if icon[4] == 'washing_basin':
                node.setH(90 + node.getH())
            mins, maxs = node.getTightBounds()
            dimensions = Point3(maxs - mins)

            # if ((maxs.getX() - mins.getX()) - (maxs.getY() - mins.getY())) * ((icon[2] - icon[0]) - (icon[3] - icon[1])) > 0:
            #   scaleX = (icon[2] - icon[0]) / dimensions.getX()
            #   scaleZ = (icon[3] - icon[1]) / dimensions.getZ()
            #   scaleY = max(scaleX, scaleZ)
            #   node.setScale(scaleX, scaleY, scaleZ)
            #   node.setHpr(0, -90, 0)
            #   node.setPos(icon[0] - mins.getX() * scaleX, icon[1] - mins.getZ() * scaleZ, maxs.getY() * scaleY)
            #   orientation
            # else:
            #   scaleX = (icon[3] - icon[1]) / dimensions.getX()
            #   scaleZ = (icon[2] - icon[0]) / dimensions.getZ()
            #   scaleY = max(scaleX, scaleZ)
            #   node.setScale(scaleX, scaleY, scaleZ)
            #   node.setHpr(90, -90, 0)
            #   node.setPos(icon[0] + maxs.getZ() * scaleZ, icon[1] - mins.getX() * scaleX, maxs.getY() * scaleY)
            #   #pivotNode = scene.attachNewNode("pivot")
            #   #pivotNode.setPos(icon[0] - mins.getX() * scaleX, icon[1] - mins.getZ() * scaleZ, maxs.getY() * scaleY)
            #   #node.wrtReparentTo(pivotNode)
            #   #pivotNode.setHpr(90, 0, 0)
            #   pass

            scaleX = (icon[2] - icon[0]) / dimensions.getX()
            scaleY = (icon[3] - icon[1]) / dimensions.getY()
            scaleZ = max(scaleX, scaleY)
            node.setScale(scaleX, scaleY, scaleZ)
            node.setPos(1 - icon[0] - maxs.getX() * scaleX,
                        icon[1] - mins.getY() * scaleY, -mins.getZ() * scaleZ)

            node.setTwoSided(True)
            node.reparentTo(scene)
            #data.addChild(node.node())
            #node.setPos(icon[0], icon[1], 0)
            #node.reparentTo(data)
            continue
        return

    def generateEggModel(self):
        data = EggData()
        model = EggGroup('model')
        data.addChild(model)
        self.generateFloor(model)
        self.generateWalls(model)
        self.generateDoors(model)
        self.generateWindows(model)
        data.writeEgg(Filename("test/floorplan.egg"))
        scene = NodePath(loadEggData(data))
        self.generateIcons(scene)
        return scene

        poly = EggPolygon()
        data.addChild(poly)

        v = EggVertex()
        v.setPos(Point3D(0, 0, 0))
        v.setUv(Point2D(0, 0))
        poly.addVertex(vp.addVertex(v))

        v = EggVertex()
        v.setPos(Point3D(1, 0, 0))
        v.setUv(Point2D(1, 0))
        poly.addVertex(vp.addVertex(v))

        v = EggVertex()
        v.setPos(Point3D(1, 0, 1))
        v.setUv(Point2D(1, 1))
        poly.addVertex(vp.addVertex(v))

        v = EggVertex()
        v.setPos(Point3D(0, 0, 1))
        v.setUv(Point2D(0, 1))
        poly.addVertex(vp.addVertex(v))

        # To write the egg file to disk, use this:
        data.writeEgg(Filename("floorplan.egg"))

        # To load the egg file and render it immediately, use this:
        node = loadEggData(data)
        scene = NodePath(node)
        return scene

    def segmentRooms(self):
        wallMask = np.ones((self.height, self.width), np.uint8) * 255
        for wall in self.wallsInt:
            lineDim = calcLineDim(((wall[0], wall[1]), (wall[2], wall[3])))
            if lineDim == 0:
                wallMask[wall[1], wall[0]:wall[2] + 1] = 0
            else:
                wallMask[wall[1]:wall[3] + 1, wall[0]] = 0
                pass
            continue
        cv2.imwrite('test/walls.png', wallMask)

        numLabels, labels, stats, centroids = cv2.connectedComponentsWithStats(
            wallMask, 4)
        print(numLabels.shape)
        print(labels.shape)
        print(stats.shape)
        print(centroids.shape)
        cv2.imwrite('test/rooms.png', labels)