[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[Toon-members] TooN so3.h [Maintenance_Branch_1_x]
From: |
Gerhard Reitmayr |
Subject: |
[Toon-members] TooN so3.h [Maintenance_Branch_1_x] |
Date: |
Thu, 12 Mar 2009 16:49:45 +0000 |
CVSROOT: /cvsroot/toon
Module name: TooN
Branch: Maintenance_Branch_1_x
Changes by: Gerhard Reitmayr <gerhard> 09/03/12 16:49:45
Modified files:
. : so3.h
Log message:
really stable SO3::ln now
CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/TooN/so3.h?cvsroot=toon&only_with_tag=Maintenance_Branch_1_x&r1=1.23.2.1&r2=1.23.2.2
Patches:
Index: so3.h
===================================================================
RCS file: /cvsroot/toon/TooN/so3.h,v
retrieving revision 1.23.2.1
retrieving revision 1.23.2.2
diff -u -b -r1.23.2.1 -r1.23.2.2
--- so3.h 11 Mar 2009 15:30:03 -0000 1.23.2.1
+++ so3.h 12 Mar 2009 16:49:45 -0000 1.23.2.2
@@ -210,15 +210,14 @@
inline Vector<3> SO3::ln() const{
Vector<3> result;
- const double trace = my_matrix[0][0] + my_matrix[1][1] + my_matrix[2][2];
-
// 2 * cos(theta) + 1 == trace
+ const double trace = my_matrix[0][0] + my_matrix[1][1] + my_matrix[2][2];
result[0] = (my_matrix[2][1]-my_matrix[1][2])/2;
result[1] = (my_matrix[0][2]-my_matrix[2][0])/2;
result[2] = (my_matrix[1][0]-my_matrix[0][1])/2;
double sin_angle_abs = sqrt(result*result);
- if (sin_angle_abs > 0.000001 ) { // cannot use small angle approximation
+ if (sin_angle_abs > 1e-6 && trace >= 1) { // cannot use small angle
approximation
double angle;
if(sin_angle_abs > 1.0) {
sin_angle_abs = 1.0;
@@ -230,26 +229,30 @@
}
result *= angle / sin_angle_abs;
} else if( trace < 1) { // antisymmetric part vanishes, but still large
rotation, need information from symmetric part
- // diagonal elements provide squares of rotation axis elements
- const double ANom = 1.0/(3.0 - trace);
- // explicit solution of the linear system for the squares of rotation
axis elements
- TooN::Vector<3> r2 = TooN::makeVector(
- sqrt((+ my_matrix[0][0] - my_matrix[1][1] - my_matrix[2][2] +
1.0) * ANom),
- sqrt((- my_matrix[0][0] + my_matrix[1][1] - my_matrix[2][2] +
1.0) * ANom),
- sqrt((- my_matrix[0][0] - my_matrix[1][1] + my_matrix[2][2] +
1.0) * ANom)
+ const double cos_angle = (trace - 3) * 0.5 + 1; // cos(angle)
+ const double angle = M_PI - ((sin_angle_abs >
1e-6)?asin(sin_angle_abs):sin_angle_abs); // small angle approximation, if
possible
+ const TooN::Vector<3> diag = TooN::makeVector(
+ my_matrix[0][0] - cos_angle,
+ my_matrix[1][1] - cos_angle,
+ my_matrix[2][2] - cos_angle
);
- // antisymmetric part constrains sign (where possible)
- if(result[0] < 0.0)
- r2[0] *= -1;
- if(result[1] < 0.0)
- r2[1] *= -1;
- if(result[2] < 0.0)
- r2[2] *= -1;
- // r2 contains now the rotation axis as unit vector, need to scale by
correct angle !
- const double angle = acos((trace - 1.0)* 0.5);
- result = r2 * angle;
+ if(fabs(diag[0]) > fabs(diag[1]) && fabs(diag[0]) > fabs(diag[2])){ //
first is largest, fill with first column
+ result[0] = diag[0];
+ result[1] = (my_matrix[1][0]+my_matrix[0][1])/2;
+ result[2] = (my_matrix[0][2]+my_matrix[2][0])/2;
+ } else if(fabs(diag[1]) > fabs(diag[2])) { //
second is largest, fill with second column
+ result[0] = (my_matrix[1][0]+my_matrix[0][1])/2;
+ result[1] = diag[1];
+ result[2] = (my_matrix[2][1]+my_matrix[1][2])/2;
+ } else { //
third is largest, fill with third column
+ result[0] = (my_matrix[0][2]+my_matrix[2][0])/2;
+ result[1] = (my_matrix[2][1]+my_matrix[1][2])/2;
+ result[2] = diag[2];
+ }
+ normalize(result);
+ result *= angle;
} else { // can use small angle approximation, because we are around zero
- // nothing todo
+ // nothing to do here
}
return result;
}