def gen_frame(pos=np.array([0, 0, 0]),
rotmat=np.eye(3),
length=.1,
thickness=.005,
rgbmatrix=None,
alpha=None,
plotname="frame"):
"""
gen an axis for attaching
:param pos:
:param rotmat:
:param length:
:param thickness:
:param rgbmatrix: each column indicates the color of each base
:param plotname:
:return:
author: weiwei
date: 20161212tsukuba, 20191228osaka
"""
endx = pos + rotmat[:, 0] * length
endy = pos + rotmat[:, 1] * length
endz = pos + rotmat[:, 2] * length
if rgbmatrix is None:
rgbx = np.array([1, 0, 0])
rgby = np.array([0, 1, 0])
rgbz = np.array([0, 0, 1])
else:
rgbx = rgbmatrix[:, 0]
rgby = rgbmatrix[:, 1]
rgbz = rgbmatrix[:, 2]
if alpha is None:
alphax = alphay = alphaz = 1
elif isinstance(alpha, np.ndarray):
alphax = alpha[0]
alphay = alpha[1]
alphaz = alpha[2]
else:
alphax = alphay = alphaz = alpha
# TODO 20201202 change it to StaticGeometricModelCollection
frame_nodepath = NodePath(plotname)
arrowx_trm = trihelper.gen_arrow(spos=pos, epos=endx, thickness=thickness)
arrowx_nodepath = da.trimesh_to_nodepath(arrowx_trm)
arrowx_nodepath.setTransparency(TransparencyAttrib.MDual)
arrowx_nodepath.setColor(rgbx[0], rgbx[1], rgbx[2], alphax)
arrowy_trm = trihelper.gen_arrow(spos=pos, epos=endy, thickness=thickness)
arrowy_nodepath = da.trimesh_to_nodepath(arrowy_trm)
arrowy_nodepath.setTransparency(TransparencyAttrib.MDual)
arrowy_nodepath.setColor(rgby[0], rgby[1], rgby[2], alphay)
arrowz_trm = trihelper.gen_arrow(spos=pos, epos=endz, thickness=thickness)
arrowz_nodepath = da.trimesh_to_nodepath(arrowz_trm)
arrowz_nodepath.setTransparency(TransparencyAttrib.MDual)
arrowz_nodepath.setColor(rgbz[0], rgbz[1], rgbz[2], alphaz)
arrowx_nodepath.reparentTo(frame_nodepath)
arrowy_nodepath.reparentTo(frame_nodepath)
arrowz_nodepath.reparentTo(frame_nodepath)
frame_sgm = StaticGeometricModel(frame_nodepath)
return frame_sgm
def gen_arrow(spos=np.array([0, 0, 0]),
epos=np.array([.1, 0, 0]),
thickness=.005,
rgba=[1, 0, 0, 1],
type="rect"):
"""
:param spos:
:param epos:
:param thickness:
:param rgba:
:return:
author: weiwei
date: 20200115osaka
"""
arrow_trm = trihelper.gen_arrow(spos=spos,
epos=epos,
thickness=thickness,
sticktype=type)
arrow_sgm = StaticGeometricModel(arrow_trm)
arrow_sgm.set_rgba(rgba=rgba)
return arrow_sgm