def cumQuat2(bs,t,qc):
si = bs.segmentIndex(t)
cidx = bs.localCoefficientVectorIndices(t)
Mi = bs.Mi(si)
c = bs.coefficients()
dc = c[0,1:] - c[0,0:-1]
u = bs.u(t,0)
bi = np.dot(u.T,Mi)
ci = bs.localCoefficientVectorIndices(t)
qci = qc[:,ci]
rval = qci[:,0]
Q = np.zeros([4,len(ci)])
Q[:,0] = qci[:,0]
#print bi
#print sum(bi)
#print ci
for i in range(0,len(ci) - 1):
dqi = qdot(qinv(qci[:,i]),qci[:,i+1])
ldqi = qlog(dqi)
tildeB = np.sum(bi[i+1:])
qq = qexp(ldqi * tildeB)
rval= qdot(rval, qq)
Q[:,i+1] = qq
rval = np.eye(4)
for i in range(len(ci) - 1,0,-1):
rval = np.dot(rval, asrl.quatPlus(qinv(Q[:,i])))
rval = qinv(np.dot(rval,qinv(Q[:,0])))
return rval
def cumQuat2(bs, t, qc):
si = bs.segmentIndex(t)
cidx = bs.localCoefficientVectorIndices(t)
Mi = bs.Mi(si)
c = bs.coefficients()
dc = c[0, 1:] - c[0, 0:-1]
u = bs.u(t, 0)
bi = np.dot(u.T, Mi)
ci = bs.localCoefficientVectorIndices(t)
qci = qc[:, ci]
rval = qci[:, 0]
Q = np.zeros([4, len(ci)])
Q[:, 0] = qci[:, 0]
#print bi
#print sum(bi)
#print ci
for i in range(0, len(ci) - 1):
dqi = qdot(qinv(qci[:, i]), qci[:, i + 1])
ldqi = qlog(dqi)
tildeB = np.sum(bi[i + 1:])
qq = qexp(ldqi * tildeB)
rval = qdot(rval, qq)
Q[:, i + 1] = qq
rval = np.eye(4)
for i in range(len(ci) - 1, 0, -1):
rval = np.dot(rval, asrl.quatPlus(qinv(Q[:, i])))
rval = qinv(np.dot(rval, qinv(Q[:, 0])))
return rval
def qdot(q1,q2):
return numpy.dot(sm.quatPlus(q1),q2)
def qdot(q1,q2):
return dot(asrl.quatPlus(q1),q2)
def qdot(q1, q2):
return numpy.dot(sm.quatPlus(q1), q2)
def qdot(q1, q2):
return dot(asrl.quatPlus(q1), q2)