def step1NodeQ(start,end,step): # return a list of nodes start from the s to e, with a specific step l = distXQ(start,end) if l <= step: return end else: t = step/l qs = cg.quat(start[3:7]) qe = cg.quat(end[3:7]) qt = cg.slerp(t, qs, qe, shortest=True) return list(xyzt(start[0:3],end[0:3],t)+[qt.w,qt.x,qt.y,qt.z])
def stepXQ(start,end,n): # return a configuration sequence in the form of [X,Q] # n >=2 if n == 2: return [start,end] qs = cg.quat(start[3:7]) qe = cg.quat(end[3:7]) xyzs = start[0:3] xyze = end[0:3] nodes = [] for i in range(0,n): t = float(i)/(n-1) # print t qt = cg.slerp(t, qs, qe, shortest=True) nodes.append(list(xyzt(xyzs,xyze,t)+[qt.w,qt.x,qt.y,qt.z])) return nodes