File Coverage

blib/lib/Math/Vector/BestRotation.pm

Criterion	Covered	Total	%
statement	138	165	83.6
branch	23	44	52.2
condition			n/a
subroutine	23	25	92.0
pod	14	14	100.0
total	198	248	79.8

package Math::Vector::BestRotation;

use warnings;
use strict;

use 5.008008;

use Carp;
use List::Util qw(sum max);
use Math::MatrixReal;

=head1 NAME

C - best rotation to match two vector sets

=head1 VERSION

Version 0.009

=cut

our $VERSION = '0.009';


###########################################################################
#                                                                         #
#                             Init Process                                #
#                                                                         #
###########################################################################

sub new {
    my ($class, @args) = @_;

    my $self = bless {}, $class;
    $self->init(@args);
    return $self;
}

sub init {
    my ($self, %args) = @_;

    $self->{matrix_r} = [0, 0, 0, 0, 0, 0, 0, 0, 0];

    foreach(keys %args) {
	my $meth = $_;
	if($self->can($meth)) { $self->$meth($args{$meth}) }
	else { carp "Unrecognized init parameter $meth.\n" }
    }
}

###########################################################################
#                                                                         #
#                               Accessors                                 #
#                                                                         #
###########################################################################

sub matrix_r {
    my ($self, @args) = @_;

    croak "Attribute matrix_r is readonly.\n" if(@args);
    return Math::MatrixReal->new_from_rows
	([[$self->{matrix_r}->[0],
	   $self->{matrix_r}->[1],
	   $self->{matrix_r}->[2]],
	  [$self->{matrix_r}->[3],
	   $self->{matrix_r}->[4],
	   $self->{matrix_r}->[5]],
	  [$self->{matrix_r}->[6],
	   $self->{matrix_r}->[7],
	   $self->{matrix_r}->[8]]]);
}

sub matrix_u {
    my ($self, @args) = @_;

    croak "Attribute matrix_u is readonly.\n" if(@args);
    return $self->{matrix_u};
}

###########################################################################
#                                                                         #
#                             Retrieve Data                               #
#                                                                         #
###########################################################################

sub _compute_bases {
    my ($self)   = @_;
    my $r        = $self->matrix_r;
    my $rtr      = (~$r) * $r;
    my ($mu, $a) = $rtr->sym_diagonalize;

    my $sorted = 0;
    while(!$sorted) {
	$sorted = 1;
	if(abs($mu->element(1, 1)) < abs($mu->element(2, 1))) {
	    $sorted = 0;
	    $mu->swap_row(1, 2);
	    $a->swap_col(1, 2);
	}
	if(abs($mu->element(2, 1)) < abs($mu->element(3, 1))) {
	    $sorted = 0;
	    $mu->swap_row(2, 3);
	    $a->swap_col(2, 3);
	}
    }

    # make sure, eigensystem is right-handed
    my $a3 = $a->column(1)->vector_product($a->column(2));
    $a->assign(1, 3, $a3->element(1, 1));
    $a->assign(2, 3, $a3->element(2, 1));
    $a->assign(3, 3, $a3->element(3, 1));

    if($mu->element(1, 1) < 1e-12) {
	carp("The largest eigenvalue of R^t*R is smaller than ".
	     "1e-12. The code will try to go on, but may die with ".
	     "division by zero later. In any case, numerical ".
	     "instability of the result has to be expected.\n");
    }

    my $b = [];  # matrix of column vectors b_k
    my $v;       # vector buffer
    my $l;       # length buffer
    my $col;     # column matrix buffer

    # determine b_1
    $col = $r * $a->column(1);
    $v   = [map { $col->element($_, 1) } (1, 2, 3)];
    $l   = sqrt(sum(map { $v->[$_]**2 } (0, 1, 2)));

    # If length is zero all b's are arbitrary. Otherwise normalize.
    if($l == 0) {
	carp("R is (too close to) the nullmatrix. ".
	     "The result will be arbitrary.\n");
	$v = [1, 0, 0];
    }
    else { @$v = map { $_ / $l } @$v }
    @$b[0, 3, 6] = @$v;

    # determine b_2
    $col = $r * $a->column(2);
    $v   = [map { $col->element($_, 1) } (1, 2, 3)];
    $l   = sqrt(sum(map { $v->[$_]**2 } (0, 1, 2)));

    # if length is zero, we choose a vector perpendicular to b_1
    if($l == 0) {
	carp("The two smaller eigenvalues are 0. The result is not ".
	     "unique.\n");

	if(abs($b->[0]) < abs($b->[3])) {
	    if(abs($b->[0]) < abs($b->[6])) {
		$b->[1] = 0;
		$b->[4] = -$b->[6];
		$b->[7] = $b->[3];
	    }
	    else {
		$b->[1] = -$b->[3];
		$b->[4] = $b->[0];
		$b->[7] = 0;
	    }
	}
	else {
	    if(abs($b->[3]) < abs($b->[6])) {
		$b->[1] = -$b->[6];
		$b->[4] = 0;
		$b->[7] = $b->[0];
	    }
	    else {
		$b->[1] = -$b->[3];
		$b->[4] = $b->[0];
		$b->[7] = 0;
	    }
	}
    }
    # otherwise we normalize carefully
    else {
	@$v = map { $_ / $l } @$v;

	# $l could have been very small, though.
	# Therefore, we make b_2 orthogonal to b_1 and normalize again.
	my $p = $v->[0] * $b->[0] + $v->[1] * $b->[3] + $v->[2] * $b->[6];
	@$v = map { $v->[$_] - $p * $b->[3*$_] } (0, 1, 2);
	$l  = sqrt(sum(map { $v->[$_]**2 } (0, 1, 2)));
	@$b[1, 4, 7] = map { $_ / $l } @$v;
    }

    # determine b_3 as by cross product
    $b->[2] = $b->[3] * $b->[7] - $b->[6] * $b->[4];
    $b->[5] = $b->[6] * $b->[1] - $b->[0] * $b->[7];
    $b->[8] = $b->[0] * $b->[4] - $b->[3] * $b->[1];

    return($a, Math::MatrixReal->new_from_rows
	([[@$b[0, 1, 2]], [@$b[3, 4, 5]], [@$b[6, 7, 8]]]));
}

sub _matrix_a_to_b {
    my ($self, $a, $b) = @_;

    return($b * (~$a));
}

sub best_orthogonal {
    my ($self)  = @_;
    my ($a, $b) = $self->_compute_bases;

    if($self->matrix_r->det < 0) {
	$b = $b * Math::MatrixReal->new_from_rows
	    ([[1, 0, 0], [0, 1, 0], [0, 0, -1]]);
    }

    $self->{matrix_u} = $self->_matrix_a_to_b($a, $b);
    return $self->{matrix_u};
}

sub best_rotation {
    my ($self)  = @_;
    my ($a, $b) = $self->_compute_bases;

    $self->{matrix_u} =  $self->_matrix_a_to_b($a, $b);
    return $self->{matrix_u};
}

sub best_proper_rotation { shift(@_)->best_rotation(@_) }

sub best_improper_rotation {
    my ($self)  = @_;
    my ($a, $b) = $self->_compute_bases;

    $b = $b * Math::MatrixReal->new_diag([1, 1, -1]);

    $self->{matrix_u} =  $self->_matrix_a_to_b($a, $b);
    return $self->{matrix_u};
}

sub best_flipped_rotation { shift(@_)->best_improper_rotation(@_) }

sub rotation_axis {
    my ($self, @args) = @_;
    my $matrix        = @args > 0 ? $args[0] : $self->matrix_u;

    croak("Unable to find the rotation axis of an undefined matrix. ".
	  "Has any rotation been calculated, yet?.\n")
	if(!defined($matrix));
    croak("Wrong type of matrix given to rotation_matrix.\n")
	if(!eval { $matrix->isa('Math::MatrixReal') } );
    croak("The calculation of a rotation axis of an improper rotation ".
	  "is not supported.\n")
	if($matrix->det < 0);

    my $unity = Math::MatrixReal->new_diag([1, 1, 1]);
    my $m     = $matrix + ~$matrix - ($matrix->trace - 1) * $unity;

    my @col_lengths = map { $m->col($_)->length } (1, 2, 3);

    my $axis;
    if($col_lengths[0] >= max($col_lengths[1], $col_lengths[2])) {
	$axis = $m->col(1)->each(sub { $_[0] / $col_lengths[0] });
    }
    elsif($col_lengths[1] >= max($col_lengths[2], $col_lengths[0])) {
	$axis = $m->col(2)->each(sub { $_[0] / $col_lengths[1] });
    }
    else {
	$axis = $m->col(3)->each(sub { $_[0] / $col_lengths[2] });
    }

    return $axis;
}

sub rotation_angle {
    my ($self, @args) = @_;
    my $matrix        = @args > 0 ? $args[0] : $self->matrix_u;

    croak("Unable to find the rotation angle of an undefined matrix. ".
	  "Has any rotation been calculated, yet?.\n")
	if(!defined($matrix));
    croak("Wrong type of matrix given to rotation_matrix.\n")
	if(!eval { $matrix->isa('Math::MatrixReal') } );
    croak("The calculation of a rotation angle of an improper rotation ".
	  "is not supported.\n")
	if($matrix->det < 0);

    my $cos = ($matrix->trace - 1) / 2;

    return abs(atan2(sqrt(1 - $cos**2), $cos));
}

###########################################################################
#                                                                         #
#                              Change Data                                #
#                                                                         #
###########################################################################

sub add_pair {
    my ($self, $x, $y) = @_;
    my $matrix_r       = $self->{matrix_r};

    for(my $i=0;$i<3;$i++) {
	for(my $j=0;$j<3;$j++) {
	    $matrix_r->[3*$i+$j] += $y->[$i] * $x->[$j];
	}
    }
}

sub add_many_pairs {
    my ($self, $x, $y) = @_;
    my $matrix_r       = $self->{matrix_r};

    for(my $n=0;$n<@$x;$n++) {
	for(my $i=0;$i<3;$i++) {
	    for(my $j=0;$j<3;$j++) {
		$matrix_r->[3*$i+$j] += $y->[$n]->[$i] * $x->[$n]->[$j];
	    }
	}
    }
}

sub clear {
    my ($self) = @_;

    $self->{matrix_r} = [0, 0, 0, 0, 0, 0, 0, 0, 0];
    $self->{matrix_u} = undef;
}


1;

__END__