def trace_path(fcs, path_coords, mesh, save_ims = True):
coords_proj = mathu.project_points_to_plane(fcs.origin,
fcs.a3,
path_coords)
coords_proj_offset_world = numpy.subtract(coords_proj, fcs.origin)
coords_proj_offset_floor = fcs.convert_points_from_world_frame(coords_proj_offset_world)
last_cpof = None
path_cnt = 0
for cpof in coords_proj_offset_floor:
if last_cpof is not None:
offset = cpof - last_cpof
else:
offset = cpof
slide_mesh_along_floor(mesh, fcs, offset[0], offset[1])
last_cpof = cpof
if save_ims:
mayavi.mlab.savefig("path_{:08d}.png".format(path_cnt))
path_cnt += 1
yield
def trace_path(fcs, path_coords, mesh, save_ims=True):
coords_proj = mathu.project_points_to_plane(fcs.origin, fcs.a3,
path_coords)
coords_proj_offset_world = numpy.subtract(coords_proj, fcs.origin)
coords_proj_offset_floor = fcs.convert_points_from_world_frame(
coords_proj_offset_world)
last_cpof = None
path_cnt = 0
for cpof in coords_proj_offset_floor:
if last_cpof is not None:
offset = cpof - last_cpof
else:
offset = cpof
slide_mesh_along_floor(mesh, fcs, offset[0], offset[1])
last_cpof = cpof
if save_ims:
mayavi.mlab.savefig("path_{:08d}.png".format(path_cnt))
path_cnt += 1
yield
def calc_scene_floor_from_gravity_and_points(normal, data, lowest_n = 500):
dists = numpy.dot(data[:, :3], -normal)
lowest_point_inds = numpy.argsort(dists)[::-1]
median_lowest_point = data[lowest_point_inds[250], :3]
# mayavi.mlab.points3d(*median_lowest_point, scale_factor = .1)
print "projecting points to plane"
proj_points = mathu.project_points_to_plane(median_lowest_point,
normal,
data[:, :3])
#calcuate floor extent and coordinate system
floor_center = proj_points.mean(axis = 0)
floor_height = numpy.dot(floor_center, normal / numpy.linalg.norm(normal))
floor_vecs = proj_points - floor_center
floor_dists = numpy.linalg.norm(floor_vecs, axis = 1)
furthest_floor_vec = floor_vecs[numpy.argmax(floor_dists), :]
bvec1 = furthest_floor_vec
bvec2 = numpy.cross(normal, furthest_floor_vec)
v1dists = numpy.dot(floor_vecs, bvec1)
v1as = numpy.argsort(v1dists)
v2dists = numpy.dot(floor_vecs, bvec2)
v2as = numpy.argsort(v2dists)
v1min, v1max = floor_vecs[v1as[0], :] + floor_center, floor_vecs[v1as[-1], :] + floor_center
v2min, v2max = floor_vecs[v2as[0], :] + floor_center, floor_vecs[v2as[-1], :] + floor_center
ff2 = floor_vecs[v2as[-1], :]
# set up coordinate system
tds_floor = mathu.three_d_coord_system(floor_center, bvec1, bvec2, normal)
# return tds_floor, v1min, v1max, v2min, v2max
return tds_floor
def calc_scene_floor_from_gravity_and_points(normal, data, lowest_n=500):
dists = numpy.dot(data[:, :3], -normal)
lowest_point_inds = numpy.argsort(dists)[::-1]
median_lowest_point = data[lowest_point_inds[250], :3]
# mayavi.mlab.points3d(*median_lowest_point, scale_factor = .1)
print "projecting points to plane"
proj_points = mathu.project_points_to_plane(median_lowest_point, normal,
data[:, :3])
#calcuate floor extent and coordinate system
floor_center = proj_points.mean(axis=0)
floor_height = numpy.dot(floor_center, normal / numpy.linalg.norm(normal))
floor_vecs = proj_points - floor_center
floor_dists = numpy.linalg.norm(floor_vecs, axis=1)
furthest_floor_vec = floor_vecs[numpy.argmax(floor_dists), :]
bvec1 = furthest_floor_vec
bvec2 = numpy.cross(normal, furthest_floor_vec)
v1dists = numpy.dot(floor_vecs, bvec1)
v1as = numpy.argsort(v1dists)
v2dists = numpy.dot(floor_vecs, bvec2)
v2as = numpy.argsort(v2dists)
v1min, v1max = floor_vecs[v1as[0], :] + floor_center, floor_vecs[
v1as[-1], :] + floor_center
v2min, v2max = floor_vecs[v2as[0], :] + floor_center, floor_vecs[
v2as[-1], :] + floor_center
ff2 = floor_vecs[v2as[-1], :]
# set up coordinate system
tds_floor = mathu.three_d_coord_system(floor_center, bvec1, bvec2, normal)
# return tds_floor, v1min, v1max, v2min, v2max
return tds_floor
