direction = rotmat.dot(pt_direction)
extended_len = 0
n = 30
for tick in range(1, 2 * n + 1):
t_npt = cpt + direction * .05 / n
# gm.gen_arrow(cpt, t_npt, rgba=[1,1,0,1]).attach_to(base)
z_surface = surface.get_zdata(np.array([t_npt[:2]]))
projected_point = np.array([t_npt[0], t_npt[1], z_surface[0]])
domain_grid = np.meshgrid(np.linspace(-.005, .005, 100, endpoint=True),
np.linspace(-.005, .005, 100, endpoint=True))
domain_0, domain_1 = domain_grid
domain = np.column_stack(
(domain_0.ravel() + t_npt[0], domain_1.ravel() + t_npt[1]))
codomain = surface.get_zdata(domain)
vertices = np.column_stack((domain, codomain))
plane_center, plane_normal = rm.fit_plane(vertices)
new_normal = plane_normal
if pn_direction.dot(new_normal) > .1:
new_normal = -new_normal
# gm.gen_arrow(spos=projected_point, epos=projected_point+plane_normal*.015).attach_to(base)
angle = rm.angle_between_vectors(last_normal, new_normal)
vec = rm.unit_vector(np.cross(last_normal, new_normal))
new_rotmat = rm.rotmat_from_axangle(vec, angle)
direction = new_rotmat.dot(direction)
gm.gen_stick(spos=cpt,
epos=projected_point,
rgba=[1, .6, 0, 1],
thickness=.002,
type='round').attach_to(base)
tmp_len = np.linalg.norm(projected_point - cpt)
extended_len += tmp_len
gm.gen_stick(spos=sec[0], epos=sec[1], rgba=[0, 0, 0, 1], thickness=.002, type='round').attach_to(base)
epos = (new_line_segs[0][1]-new_line_segs[0][0])*.7+new_line_segs[0][0]
gm.gen_arrow(spos=new_line_segs[0][0], epos=epos, thickness=0.004).attach_to(base)
t_cpt = cpt
last_normal = cnrml
direction = rotmat.dot(pt_direction)
tmp_direction = rotmat.dot(tmp_direction)
n=50
for tick in range(1, n+1):
t_npt = cpt+direction*.07/n
# gm.gen_arrow(spos=t_npt, epos=t_npt+last_normal*.015, thickness=0.001, rgba=[1, 1, 0, 1]).attach_to(base)
nearby_sample_ids = tree.query_ball_point(t_npt, .005)
nearby_samples = bowl_samples[nearby_sample_ids]
# gm.GeometricModel(nearby_samples).attach_to(base)
plane_center, plane_normal = rm.fit_plane(nearby_samples)
plane_tangential = rm.orthogonal_vector(plane_normal)
plane_tmp = np.cross(plane_normal, plane_tangential)
plane_rotmat = np.column_stack((plane_tangential, plane_tmp, plane_normal))
homomat = np.eye(4)
homomat[:3,:3]=plane_rotmat
homomat[:3,3]=plane_center
# twod_plane = gm.gen_box(np.array([.2, .2, .001]), homomat=homomat, rgba=[.5,.7,1,.3]).attach_to(base)
projected_point = rm.project_to_plane(t_npt, plane_center, plane_normal)
# gm.gen_stick(t_npt, projected_point, thickness=.002).attach_to(base)
new_normal = rm.unit_vector(t_npt-projected_point)
if pn_direction.dot(new_normal) > .1:
new_normal = -new_normal
# gm.gen_arrow(spos=projected_point, epos=projected_point+new_normal*.015, thickness=0.001).attach_to(base)
angle = rm.angle_between_vectors(last_normal, new_normal)
vec = rm.unit_vector(np.cross(last_normal, new_normal))