def simulationTask(task):
# Step the simulation and set the new positions
world.quickStep(globalClock.getDt())
sphere_a.set_pos(da.pdv3_to_npv3(body_sphere_a.getPosition()))
sphere_b.set_pos(da.pdv3_to_npv3(body_sphere_b.getPosition()))
gm.gen_linesegs([[np.zeros(3), sphere_a.get_pos()]], thickness=.05, rgba=[0, 1, 0, 1]).attach_to(base)
gm.gen_linesegs([[sphere_a.get_pos(), sphere_b.get_pos()]], thickness=.05, rgba=[0, 0, 1, 1]).attach_to(base)
return task.cont
import numpy as np base = wd.World(cam_pos=[7, 0, 0], lookat_pos=[0, 0, -.5], toggle_debug=True) radius = .1 sphere_a = cm.gen_sphere(radius=radius) sphere_a.set_pos([0, .3, -.3]) sphere_a.set_rgba([1, .2, .3, 1]) sphere_a.attach_to(base) sphere_b = cm.gen_sphere(radius=radius) sphere_b.set_pos([0, 1.25, -.7]) sphere_b.set_rgba([.3, .2, 1, 1]) sphere_b.attach_to(base) gm.gen_linesegs([[np.zeros(3), sphere_a.get_pos()]], thickness=.05, rgba=[0, 1, 0, 1]).attach_to(base) gm.gen_linesegs([[sphere_a.get_pos(), sphere_b.get_pos()]], thickness=.05, rgba=[0, 0, 1, 1]).attach_to(base) # Setup our physics world and the body world = OdeWorld() world.setGravity(0, 0, -9.81) body_sphere_a = OdeBody(world) M = OdeMass() M.setSphere(7874, radius) body_sphere_a.setMass(M) body_sphere_a.setPosition(da.npv3_to_pdv3(sphere_a.get_pos())) body_sphere_b = OdeBody(world) M = OdeMass() M.setSphere(7874, radius)
toggle_face_id_pair_for_curvature=True)
pr.disable()
pstats.Stats(pr).sort_stats(pstats.SortKey.CUMULATIVE).print_stats(10)
# TODO Extract Facet
for i in range(len(facet_nested_face_id_list)):
offset_pos = facet_normal_list[i] * np.random.rand() * .0
# segment
tmp_trm = tg.extract_subtrimesh(bunnycm.objtrm,
facet_nested_face_id_list[i],
offset_pos) # TODO submesh
tmp_gm = gm.StaticGeometricModel(tmp_trm, btwosided=True)
tmp_gm.attach_to(base)
tmp_gm.set_rgba(rm.random_rgba())
# edge
edge_list = (np.array(seg_nested_edge_list[i]) + offset_pos).tolist()
gm.gen_linesegs(edge_list, thickness=.05, rgba=[1, 0, 0,
1]).attach_to(base)
# seed segment
tmp_trm = tg.extract_subtrimesh(bunnycm.objtrm, facet_seed_list[i],
offset_pos)
tmp_gm = gm.StaticGeometricModel(tmp_trm, btwosided=True)
tmp_gm.attach_to(base)
tmp_gm.set_rgba([1, 0, 0, 1])
# face center and normal
seed_center = np.mean(tmp_trm.vertices, axis=0)
gm.gen_sphere(pos=seed_center, radius=.001).attach_to(base)
gm.gen_arrow(spos=seed_center,
epos=seed_center + tmp_trm.face_normals[0] * .01,
thickness=.0006).attach_to(base)
for face_id_for_curvature in face_id_pair_list_for_curvature[i]:
rgba = [1, 1, 1, 1]
tmp_trm = tg.extract_subtrimesh(bunnycm.objtrm,
twod_plane = gm.gen_box(np.array([.2, .2, .001]),
homomat=new_plane_homomat,
rgba=[1, 1, 1, .3])
twod_plane.attach_to(base)
new_line_segs = [
[cpt, cpt + rotmat.dot(pt_direction) * .05],
[
cpt + rotmat.dot(pt_direction) * .05,
cpt + rotmat.dot(pt_direction) * .05 + rotmat.dot(tmp_direction) * .05
],
[
cpt + rotmat.dot(pt_direction) * .05 + rotmat.dot(tmp_direction) * .05,
cpt + rotmat.dot(tmp_direction) * .05
], [cpt + rotmat.dot(tmp_direction) * .05, cpt]
]
gm.gen_linesegs(new_line_segs).attach_to(base)
for sec in new_line_segs:
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)
n = 7
for tick in range(1, n + 1):
t_npt = cpt + direction * .05 / n
plane_rotmat = np.column_stack((pt_direction, tmp_direction, pn_direction))
homomat = np.eye(4)
homomat[:3, :3] = plane_rotmat
homomat[:3, 3] = np.array([-.07, -.03, .1])
twod_plane = gm.gen_box(np.array([.2, .2, .001]),
homomat=homomat,
rgba=[1, 1, 1, .3])
twod_plane.attach_to(base)
circle_radius = .05
line_segs = [[homomat[:3, 3], homomat[:3, 3] + pt_direction * .05],
[
homomat[:3, 3] + pt_direction * .05,
homomat[:3, 3] + pt_direction * .05 + tmp_direction * .05
]]
gm.gen_linesegs(line_segs).attach_to(base)
gm.gen_arrow(spos=line_segs[0][0], epos=line_segs[0][1],
thickness=0.004).attach_to(base)
spt = homomat[:3, 3]
# gm.gen_stick(spt, spt + pn_direction * 10, rgba=[0,1,0,1]).attach_to(base)
# base.run()
gm.gen_dasharrow(spt, spt - pn_direction * .07,
thickness=.004).attach_to(base) # p0
cpt, cnrml = bowl_model.ray_hit(spt,
spt + pn_direction * 10000,
option='closest')
gm.gen_dashstick(spt, cpt, rgba=[.57, .57, .57, .7],
thickness=0.003).attach_to(base)
gm.gen_sphere(pos=cpt, radius=.005).attach_to(base)
gm.gen_dasharrow(cpt, cpt - pn_direction * .07,
thickness=.004).attach_to(base) # p0