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
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 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 xrange(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 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 __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
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)
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 xrange(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 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("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 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