summaryrefslogtreecommitdiff
path: root/libmoped/libs/sba-1.6/utils/quats.pl
blob: b1ccc7e5e40aa649a0cb9c1db22e145f1c2eed3c (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
#!/usr/bin/perl -w

#################################################################################
## 
##  Perl functions for manipulating quaternions. Currently includes converters
##  between rotation matrices and quaternions.
##  Copyright (C) 2008  Manolis Lourakis (lourakis at ics.forth.gr)
##  Institute of Computer Science, Foundation for Research & Technology - Hellas
##  Heraklion, Crete, Greece.
##
##  This program is free software; you can redistribute it and/or modify
##  it under the terms of the GNU General Public License as published by
##  the Free Software Foundation; either version 2 of the License, or
##  (at your option) any later version.
##
##  This program is distributed in the hope that it will be useful,
##  but WITHOUT ANY WARRANTY; without even the implied warranty of
##  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
##  GNU General Public License for more details.
##
#################################################################################


$R[0]=3.2471667291e-01; $R[1]=-1.0372666302e-01; $R[2]=-9.4010630341e-01;
$R[3]=2.7245486596e-01; $R[4]=9.6209316145e-01;  $R[5]=-1.2045526681e-02;
$R[6]=9.0571928783e-01; $R[7]=-2.5222515354e-01; $R[8]=3.4066852448e-01;

@q=rotmat2quat(@R);

printf "Quat from R: %g %g %g %g\n\n", $q[0], $q[1], $q[2], $q[3];
@Rn=quat2rotmat(@q);
printf "R converted back from quat\n";
printf "%g %g %g\n", $Rn[0], $Rn[1], $Rn[2];
printf "%g %g %g\n", $Rn[3], $Rn[4], $Rn[5];
printf "%g %g %g\n", $Rn[6], $Rn[7], $Rn[8];



# compute the quaternion corresponding to a rotation matrix; see A8 in Horn's paper 
sub rotmat2quat{
  my ($R)=@_;
  my ($q, $i, $maxpos, @tmp, $mag);  

	# find the maximum of the 4 quantities
	$tmp[0]=1.0 + $R[0] + $R[4] + $R[8];
	$tmp[1]=1.0 + $R[0] - $R[4] - $R[8];
	$tmp[2]=1.0 - $R[0] + $R[4] - $R[8];
	$tmp[3]=1.0 - $R[0] - $R[4] + $R[8];

	for($i=0, $mag=-1.0; $i<4; $i++){
		if($tmp[$i]>$mag){
			$mag=$tmp[$i];
			$maxpos=$i;
		}
  }

  if($maxpos==0){
		$q[0]=sqrt($tmp[0])*0.5;
		$q[1]=($R[7] - $R[5])/(4.0*$q[0]);
		$q[2]=($R[2] - $R[6])/(4.0*$q[0]);
		$q[3]=($R[3] - $R[1])/(4.0*$q[0]);
  }
  elsif($maxpos==1){
		$q[1]=sqrt($tmp[1])*0.5;
		$q[0]=($R[7] - $R[5])/(4.0*$q[1]);
		$q[2]=($R[3] + $R[1])/(4.0*$q[1]);
		$q[3]=($R[2] + $R[6])/(4.0*$q[1]);
  }
  elsif($maxpos==2){
		$q[2]=sqrt($tmp[2])*0.5;
		$q[0]=($R[2] - $R[6])/(4.0*$q[2]);
		$q[1]=($R[3] + $R[1])/(4.0*$q[2]);
		$q[3]=($R[7] + $R[5])/(4.0*$q[2]);
  }
  elsif($maxpos==3){
		$q[3]=sqrt($tmp[3])*0.5;
		$q[0]=($R[3] - $R[1])/(4.0*$q[3]);
		$q[1]=($R[2] + $R[6])/(4.0*$q[3]);
		$q[2]=($R[7] + $R[5])/(4.0*$q[3]);
  }
  else{ # should not happen
		die "Internal error in rotmat2quat\n";
	}

	# enforce unit length
	$mag=$q[0]*$q[0] + $q[1]*$q[1] + $q[2]*$q[2] + $q[3]*$q[3];

	return @q if($mag==1.0);

	$mag=1.0/sqrt($mag);
	$q[0]*=$mag; $q[1]*=$mag; $q[2]*=$mag; $q[3]*=$mag;

	return @q;
}


# compute the rotation matrix corresponding to a quaternion; see Horn's paper
sub quat2rotmat{
  my ($q)=@_;
  my ($R, $mag);

	# ensure unit length
	$mag=$q[0]*$q[0] + $q[1]*$q[1] + $q[2]*$q[2] + $q[3]*$q[3];
	if($mag!=1.0){
		$mag=1.0/sqrt($mag);
		$q[0]*=$mag; $q[1]*=$mag; $q[2]*=$mag; $q[3]*=$mag;
	}

  $R[0]=$q[0]*$q[0]+$q[1]*$q[1]-$q[2]*$q[2]-$q[3]*$q[3];
	$R[1]=2*($q[1]*$q[2]-$q[0]*$q[3]);
	$R[2]=2*($q[1]*$q[3]+$q[0]*$q[2]);

	$R[3]=2*($q[1]*$q[2]+$q[0]*$q[3]);
	$R[4]=$q[0]*$q[0]+$q[2]*$q[2]-$q[1]*$q[1]-$q[3]*$q[3];
	$R[5]=2*($q[2]*$q[3]-$q[0]*$q[1]);

	$R[6]=2*($q[1]*$q[3]-$q[0]*$q[2]);
	$R[7]=2*($q[2]*$q[3]+$q[0]*$q[1]);
	$R[8]=$q[0]*$q[0]+$q[3]*$q[3]-$q[1]*$q[1]-$q[2]*$q[2];

  return @R;
}