コード例 #1
0
ファイル: 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
0
ファイル: quaternion.py プロジェクト: niberger/joysix 
def exp(v):
	hv = 0.5*v
	theta = np.linalg.norm(hv)
	a = trig.sinox(theta)
	b = math.cos(theta)
	return Quaternion(b, a*hv)