def swingTwistDecomposition( rotation, twistAxis ):
ra = LVector3f( rotation.getI(), rotation.getJ(), rotation.getK() )
p = ra.project( twistAxis ) # return projection v1 onto v2
twist = Quat( rotation.getR(), p.getX(), p.getY(), p.getZ() )
twist.normalize()
swing = rotation * twist.conjugate()
return swing, twist
def shortestRotation( a, b ):
if a.dot(b) < 0:
b2 = Quat( -b.getR(), -b.getI(), -b.getJ(), -b.getK() )
q = a * b2.conjugate()
else:
q = a * b.conjugate()
q.normalize()
return q
