summaryrefslogtreecommitdiff
path: root/libmoped/libs/sba-1.6/utils/quats.pl
diff options
context:
space:
mode:
Diffstat (limited to 'libmoped/libs/sba-1.6/utils/quats.pl')
-rwxr-xr-xlibmoped/libs/sba-1.6/utils/quats.pl122
1 files changed, 122 insertions, 0 deletions
diff --git a/libmoped/libs/sba-1.6/utils/quats.pl b/libmoped/libs/sba-1.6/utils/quats.pl
new file mode 100755
index 0000000..b1ccc7e
--- /dev/null
+++ b/libmoped/libs/sba-1.6/utils/quats.pl
@@ -0,0 +1,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;
+}