def updateGeoPoints(self):
gps = self.gPoints
acs = self.attach.corners
#make sure the gpoints are left-up and right-down
dis = [[], []]
xyzs = [
gps[0].xyz, (gps[1].xyz[0], gps[0].xyz[1], gps[1].xyz[2]),
gps[1].xyz, (gps[0].xyz[0], gps[1].xyz[1], gps[0].xyz[2])
]
for i in range(2):
for xyz in xyzs:
dis[i].append(utils.pointsDistance(xyz, acs[i * 2].xyz))
xyz = xyzs[dis[i].index(min(dis[i]))]
gps[i] = data.GeoPoint(self.__scene, None, xyz)
# stick to wall boundary
localBbox2d = []
for i in range(2):
xyz = list(gps[i].xyz)
dis = utils.pointsDirectionPow(acs[i * 2].xyz, gps[i].xyz, 2)
cxz = math.sqrt(dis[0] + dis[2]) / self.attach.width
cy = math.sqrt(dis[1]) / self.__scene.label.getLayoutHeight()
if cxz <= 0.03:
xyz[0] = acs[i * 2].xyz[0]
xyz[2] = acs[i * 2].xyz[2]
cxz = 0
if cy <= 0.03:
xyz[1] = acs[i * 2].xyz[1]
cy = 0
gps[i] = data.GeoPoint(self.__scene, None, tuple(xyz))
coord = (cxz, cy) if i == 0 else (1 - cxz, 1 - cy)
localBbox2d.append(coord)
self.localBbox2d = tuple(localBbox2d)
def genLayoutDepthMap(scene, size):
depthMap = np.zeros(size)
for y in range(0, size[0]):
for x in range(0, size[1]):
coords = utils.pos2coords((y,x), size)
coordsT = utils.posTranspose(coords)
vec = utils.coords2xyz(coordsT, 1)
if y <= int(size[0]/2):
plane = scene.label.getLayoutCeiling().planeEquation
else:
plane = scene.label.getLayoutFloor().planeEquation
point = utils.vectorPlaneHit(vec, plane)
depth = 0 if point is None else utils.pointsDistance((0,0,0), point)
depthMap[y,x] = depth
for wall in scene.label.getLayoutWalls():
if wall.planeEquation[3] > 0:
continue
isCross, polygon = genWallPolygon2d(size, wall)
if not isCross:
utils.imageDrawWallDepth(depthMap, polygon, wall)
else:
utils.imageDrawWallDepth(depthMap, polygon[0], wall)
utils.imageDrawWallDepth(depthMap, polygon[1], wall)
return depthMap
def sortWallsByDist(walls):
return sorted(
walls,
key=lambda x: utils.pointsDistance(
(0, 0, 0), utils.pointsMean([x.corners[0].xyz, x.corners[2].xyz])),
reverse=True)
def checkRayHit(self, vec, orig=(0, 0, 0)):
point = utils.vectorPlaneHit(vec, self.planeEquation)
if point is None:
return False, None
cs = self.corners
if cs[2].xyz[1] <= point[1] <= cs[0].xyz[1]:
p1 = (point[0], cs[0].xyz[1], point[2])
dis1 = utils.pointsDistance(p1, cs[0].xyz)
dis2 = utils.pointsDistance(p1, cs[1].xyz)
dis3 = utils.pointsDistance(cs[0].xyz, cs[1].xyz)
if dis1 + dis2 <= dis3 * 1.0005:
return True, point
return False, None
def updateGeometry(self):
self.updateGeoPoints()
self.updateCorners()
self.updateEdges()
self.updateBbox2d()
self.normal = utils.pointsNormal(self.corners[0].xyz,
self.corners[1].xyz,
self.corners[3].xyz)
#self.color = utils.normal2color(self.normal)
self.planeEquation = utils.planeEquation(self.normal,
self.corners[0].xyz)
self.width = utils.pointsDistance(self.corners[0].xyz,
self.corners[1].xyz)
def updateGeometry(self):
self.updateCorners()
self.updateEdges()
self.normal = utils.pointsNormal(self.corners[0].xyz,
self.corners[1].xyz,
self.corners[3].xyz)
self.color = utils.normal2color(self.normal)
self.planeEquation = utils.planeEquation(self.normal,
self.corners[0].xyz)
self.width = utils.pointsDistance(self.corners[0].xyz,
self.corners[1].xyz)
for obj2d in self.attached:
obj2d.updateGeometry()
def imageDrawWallDepth(data, polygon, wall):
size = (data.shape[1], data.shape[0])
polyx = np.array([p[0] for p in polygon])
polyy = np.array([p[1] for p in polygon])
posy, posx = draw.polygon(polyy, polyx)
for i in range(len(posy)):
coords = utils.pos2coords((posx[i], posy[i]), size)
vec = utils.coords2xyz(coords, 1)
point = utils.vectorPlaneHit(vec, wall.planeEquation)
depth = 0 if point is None else utils.pointsDistance((0, 0, 0), point)
color = (depth, depth, depth)
draw.set_color(data, [posy[i], posx[i]], list(color))