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