def matrix(self):
if self.R != None:
return self.R
elif self.Q != None:
self.R = rot.rotation_matrix(self.Q)
return self.matrix()
elif (self.phi != None) and (self.u != None):
ux = rot.skew(self.u)
self.R = np.eye(3) + math.sin(
self.phi) * ux + (1.0 - math.cos(self.phi)) * np.dot(ux, ux)
return self.R
elif self.p != None:
px = rot.skew(self.p)
px2 = np.dot(px, px)
p = np.linalg.norm(self.p)
self.phi = self.gen_fun_inv(p)
v = self.noo()
z = self.zeta()
v2 = v * v
v2_z = v2 * gen.inv(z)
self.R = np.eye(3) + v2_z * px + 0.5 * v2 * px2
return self.R
else:
return None
def matrix(self):
if self.R != None:
return self.R
elif self.Q != None:
self.R = rot.rotation_matrix(self.Q)
return self.matrix()
elif (self.phi != None) and (self.u != None):
ux = rot.skew(self.u)
self.R = np.eye(3) + math.sin(self.phi)*ux + (1.0 - math.cos(self.phi))*np.dot(ux,ux)
return self.R
elif self.p != None:
px = rot.skew(self.p)
px2 = np.dot(px, px)
p = np.linalg.norm(self.p)
self.phi = self.gen_fun_inv(p)
v = self.noo()
z = self.zeta()
v2 = v*v
v2_z = v2*gen.inv(z)
self.R = np.eye(3) + v2_z*px + 0.5*v2*px2
return self.R
else:
return None
def zeta(self, phi = None):
if phi == None:
phi = self.angle()
if gen.equal(phi, 0.0) and self.parametrization == 'identity':
return self.m
p = self.gen_fun(phi)
return 2*math.tan(phi/2)*gen.inv(p)
def tensor(self):
if self.H == None:
if gen.equal(self.angle(), 0.0) :
return np.eye(3)
p = self.vector()
px = rot.skew(p)
px2 = np.dot(px, px)
p_nrm = np.linalg.norm(p)
p2 = p_nrm*p_nrm
p2_inv = gen.inv(p2)
v = self.noo()
z = self.zeta()
m = self.moo()
v2 = v*v
v2_z = v2*gen.inv(z)
self.H = m*np.eye(3) + 0.5*v2*px + p2_inv*(m - v2_z)*px2
return self.H
def zeta(self, phi=None):
if phi == None:
phi = self.angle()
if gen.equal(phi, 0.0) and self.parametrization == 'identity':
return self.m
p = self.gen_fun(phi)
return 2 * math.tan(phi / 2) * gen.inv(p)
def tensor(self):
if self.H == None:
if gen.equal(self.angle(), 0.0):
return np.eye(3)
p = self.vector()
px = rot.skew(p)
px2 = np.dot(px, px)
p_nrm = np.linalg.norm(p)
p2 = p_nrm * p_nrm
p2_inv = gen.inv(p2)
v = self.noo()
z = self.zeta()
m = self.moo()
v2 = v * v
v2_z = v2 * gen.inv(z)
self.H = m * np.eye(3) + 0.5 * v2 * px + p2_inv * (m - v2_z) * px2
return self.H
def gen_fun_derivative(self, phi):
if self.parametrization == 'linear':
return math.cos(phi/self.m)
elif self.parametrization == 'identity':
return 1.0/self.m
elif self.parametrization == 'cayley-gibbs-rodrigues':
t = math.tan(phi/self.m)
return 1.0 + t*t
elif self.parametrization == 'bauchau-trainelli': # Bauchau-Trainelli parametrization
s = math.sin(phi)
c = math.cos(phi)
a = 6*(phi - s)
f = a**0.33333333
b = gen.inv(f)
return 2*(1.0-c)*b*b
elif self.parametrization == 'exponential': # Exponential
return math.exp(phi)
else:
assert False,"Not Supported!"
def gen_fun_derivative(self, phi):
if self.parametrization == 'linear':
return math.cos(phi / self.m)
elif self.parametrization == 'identity':
return 1.0 / self.m
elif self.parametrization == 'cayley-gibbs-rodrigues':
t = math.tan(phi / self.m)
return 1.0 + t * t
elif self.parametrization == 'bauchau-trainelli': # Bauchau-Trainelli parametrization
s = math.sin(phi)
c = math.cos(phi)
a = 6 * (phi - s)
f = a**0.33333333
b = gen.inv(f)
return 2 * (1.0 - c) * b * b
elif self.parametrization == 'exponential': # Exponential
return math.exp(phi)
else:
assert False, "Not Supported!"
def moo(self, phi = None):
if phi == None:
phi = self.angle()
pp = self.gen_fun_derivative(phi)
return gen.inv(pp)
def moo(self, phi=None):
if phi == None:
phi = self.angle()
pp = self.gen_fun_derivative(phi)
return gen.inv(pp)