Python cosox2示例

示例#1

文件： quaternion.py 项目： niberger/joysix

def dexp(v):
	x = np.linalg.norm(v)
	a = trig.sinox(x)
	b = trig.cosox2(x)
	c = trig.sinox3(x)
	I = np.identity(3)
	S = skew(v)
	W = v * v.transpose()
	return a*I + b*S + c*W

示例#2

文件： pose.py 项目： niberger/joysix

def mt(v):
	r = v[0:3]
	t = v[3:6]
	x = np.linalg.norm(r)
	b = trig.cosox2(x)
	c = trig.sinox3(x)
	g = trig.specialFun1(x)
	h = trig.specialFun3(x)
	I = np.identity(3)
	return b*quat.skew(t) + c*(r*t.transpose() + t*r.transpose()) + v3.dot(r,t)*((c - b) * I + g*quat.skew(r) + h*r*r.transpose())