def __call__(self):

        children = AnimationStructure.children_list(self.animation.parents)

        for i in range(self.iterations):

            for j in AnimationStructure.joints(self.animation.parents):

                c = np.array(children[j])
                if len(c) == 0: continue

                anim_transforms = Animation.transforms_global(self.animation)
                anim_positions = anim_transforms[:, :, :3, 3]
                anim_rotations = Quaternions.from_transforms(anim_transforms)

                jdirs = anim_positions[:, c] - anim_positions[:, np.newaxis, j]
                ddirs = self.positions[:, c] - anim_positions[:, np.newaxis, j]

                jsums = np.sqrt(np.sum(jdirs**2.0, axis=-1)) + 1e-10
                dsums = np.sqrt(np.sum(ddirs**2.0, axis=-1)) + 1e-10

                jdirs = jdirs / jsums[:, :, np.newaxis]
                ddirs = ddirs / dsums[:, :, np.newaxis]

                angles = np.arccos(np.sum(jdirs * ddirs, axis=2).clip(-1, 1))
                axises = np.cross(jdirs, ddirs)
                axises = -anim_rotations[:, j, np.newaxis] * axises

                rotations = Quaternions.from_angle_axis(angles, axises)

                if rotations.shape[1] == 1:
                    averages = rotations[:, 0]
                else:
                    averages = Quaternions.exp(rotations.log().mean(axis=-2))

                self.animation.rotations[:,
                                         j] = self.animation.rotations[:,
                                                                       j] * averages

            if not self.silent:
                anim_positions = Animation.positions_global(self.animation)
                error = np.mean(np.sum((anim_positions - self.positions)**2.0,
                                       axis=-1)**0.5,
                                axis=-1)
                print('[BasicInverseKinematics] Iteration %i Error: %f' %
                      (i + 1, error))

        return self.animation
    def __call__(self,
                 descendants=None,
                 maxjoints=4,
                 gamma=1.0,
                 transpose=False):
        """ Calculate Masses """
        if self.weights is None:
            self.weights = np.ones(self.animation.shape[1])

        if self.weights_translate is None:
            self.weights_translate = np.ones(self.animation.shape[1])

        nf = len(self.animation)
        nj = self.animation.shape[1]
        nv = self.goal.shape[1]

        weightids = np.argsort(-self.vweights, axis=1)[:, :maxjoints]
        weightvls = np.array(
            list(map(lambda w, i: w[i], self.vweights, weightids)))
        weightvls = weightvls / weightvls.sum(axis=1)[..., np.newaxis]

        if descendants is None:
            self.descendants = AnimationStructure.descendants_mask(
                self.animation.parents)
        else:
            self.descendants = descendants

        des_r = np.eye(nj) + self.descendants
        des_r = des_r[:, weightids].repeat(3, axis=0)

        des_t = np.eye(nj) + self.descendants
        des_t = des_t[:, weightids].repeat(3, axis=0)

        if not self.silent:
            curr = Animation.skin(self.animation,
                                  self.rest,
                                  self.vweights,
                                  self.mesh,
                                  maxjoints=maxjoints)
            error = np.mean(np.sqrt(np.sum((curr - self.goal)**2.0, axis=-1)))
            print('[ICP] Start | Error: %f' % error)

        for i in range(self.iterations):
            """ Get Global Rotations & Positions """
            gt = Animation.transforms_global(self.animation)
            gp = gt[:, :, :, 3]
            gp = gp[:, :, :3] / gp[:, :, 3, np.newaxis]
            gr = Quaternions.from_transforms(gt)

            x = self.animation.rotations.euler().reshape(nf, -1)
            w = self.weights.repeat(3)

            if self.translate:
                x = np.hstack([x, self.animation.positions.reshape(nf, -1)])
                w = np.hstack([w, self.weights_translate.repeat(3)])
            """ Get Current State """
            curr = Animation.skin(self.animation,
                                  self.rest,
                                  self.vweights,
                                  self.mesh,
                                  maxjoints=maxjoints)
            """ Find Cloest Points """
            if self.find_closest:
                mapping = np.argmin((curr[:, :, np.newaxis] -
                                     self.goal[:, np.newaxis, :])**2.0,
                                    axis=2)
                e = gamma * (np.array(
                    list(map(lambda g, m: g[m], self.goal, mapping))) -
                             curr).reshape(nf, -1)
            else:
                e = gamma * (self.goal - curr).reshape(nf, -1)
            """ Generate Jacobian """
            if self.recalculate or i == 0:
                j = self.jacobian(x, gp, gr, self.goal, weightvls, des_r,
                                  des_t)
            """ Update Variables """
            l = self.damping * (1.0 / (w + 1e-10))
            d = (l * l) * np.eye(x.shape[1])

            if transpose:
                x += np.array(list(map(lambda jf, ef: jf.T.dot(ef), j, e)))
            else:
                x += np.array(
                    list(
                        map(
                            lambda jf, ef: linalg.lu_solve(
                                linalg.lu_factor(jf.T.dot(jf) + d), jf.T.dot(
                                    ef)), j, e)))
            """ Set Back Rotations / Translations """
            self.animation.rotations = Quaternions.from_euler(
                x[:, :nj * 3].reshape((nf, nj, 3)), order='xyz', world=True)

            if self.translate:
                self.animation.positions = x[:, nj * 3:].reshape((nf, nj, 3))

            if not self.silent:
                curr = Animation.skin(self.animation, self.rest, self.vweights,
                                      self.mesh)
                error = np.mean(
                    np.sqrt(np.sum((curr - self.goal)**2.0, axis=-1)))
                print('[ICP] Iteration %i | Error: %f' % (i + 1, error))
    def __call__(self, descendants=None, gamma=1.0):

        self.descendants = descendants
        """ Calculate Masses """
        if self.weights is None:
            self.weights = np.ones(self.animation.shape[1])

        if self.weights_translate is None:
            self.weights_translate = np.ones(self.animation.shape[1])
        """ Calculate Descendants """
        if self.descendants is None:
            self.descendants = AnimationStructure.descendants_mask(
                self.animation.parents)

        self.tdescendants = np.eye(self.animation.shape[1]) + self.descendants

        self.first_descendants = self.descendants[:,
                                                  np.array(
                                                      list(self.targets.keys())
                                                  )].repeat(3,
                                                            axis=0).astype(int)
        self.first_tdescendants = self.tdescendants[:,
                                                    np.array(
                                                        list(self.targets.keys(
                                                        )))].repeat(
                                                            3,
                                                            axis=0).astype(int)
        """ Calculate End Effectors """
        self.endeff = np.array(list(self.targets.values()))
        self.endeff = np.swapaxes(self.endeff, 0, 1)

        if not self.references is None:
            self.second_descendants = self.descendants.repeat(
                3, axis=0).astype(int)
            self.second_tdescendants = self.tdescendants.repeat(
                3, axis=0).astype(int)
            self.second_targets = dict([
                (i, self.references[:, i])
                for i in xrange(self.references.shape[1])
            ])

        nf = len(self.animation)
        nj = self.animation.shape[1]

        if not self.silent:
            gp = Animation.positions_global(self.animation)
            gp = gp[:, np.array(list(self.targets.keys()))]
            error = np.mean(np.sqrt(np.sum((self.endeff - gp)**2.0, axis=2)))
            print('[JacobianInverseKinematics] Start | Error: %f' % error)

        for i in range(self.iterations):
            """ Get Global Rotations & Positions """
            gt = Animation.transforms_global(self.animation)
            gp = gt[:, :, :, 3]
            gp = gp[:, :, :3] / gp[:, :, 3, np.newaxis]
            gr = Quaternions.from_transforms(gt)

            x = self.animation.rotations.euler().reshape(nf, -1)
            w = self.weights.repeat(3)

            if self.translate:
                x = np.hstack([x, self.animation.positions.reshape(nf, -1)])
                w = np.hstack([w, self.weights_translate.repeat(3)])
            """ Generate Jacobian """
            if self.recalculate or i == 0:
                j = self.jacobian(x, gp, gr, self.targets,
                                  self.first_descendants,
                                  self.first_tdescendants)
            """ Update Variables """
            l = self.damping * (1.0 / (w + 0.001))
            d = (l * l) * np.eye(x.shape[1])
            e = gamma * (
                self.endeff.reshape(nf, -1) -
                gp[:, np.array(list(self.targets.keys()))].reshape(nf, -1))

            x += np.array(
                list(
                    map(
                        lambda jf, ef: linalg.lu_solve(
                            linalg.lu_factor(jf.T.dot(jf) + d), jf.T.dot(ef)),
                        j, e)))
            """ Generate Secondary Jacobian """
            if self.references is not None:

                ns = np.array(
                    list(
                        map(
                            lambda jf: np.eye(x.shape[1]) - linalg.solve(
                                jf.T.dot(jf) + d, jf.T.dot(jf)), j)))

                if self.recalculate or i == 0:
                    j2 = self.jacobian(x, gp, gr, self.second_targets,
                                       self.second_descendants,
                                       self.second_tdescendants)

                e2 = self.secondary * (self.references.reshape(nf, -1) -
                                       gp.reshape(nf, -1))

                x += np.array(
                    list(
                        map(
                            lambda nsf, j2f, e2f: nsf.dot(
                                linalg.lu_solve(
                                    linalg.lu_factor(j2f.T.dot(j2f) + d),
                                    j2f.T.dot(e2f))), ns, j2, e2)))
            """ Set Back Rotations / Translations """
            self.animation.rotations = Quaternions.from_euler(
                x[:, :nj * 3].reshape((nf, nj, 3)), order='xyz', world=True)

            if self.translate:
                self.animation.positions = x[:, nj * 3:].reshape((nf, nj, 3))
            """ Generate Error """

            if not self.silent:
                gp = Animation.positions_global(self.animation)
                gp = gp[:, np.array(list(self.targets.keys()))]
                error = np.mean(np.sum((self.endeff - gp)**2.0, axis=2)**0.5)
                print('[JacobianInverseKinematics] Iteration %i | Error: %f' %
                      (i + 1, error))