00001 /* 00002 * Copyright (c) 2013, Fraunhofer FKIE 00003 * 00004 * Authors: Bastian Gaspers 00005 * 00006 * Redistribution and use in source and binary forms, with or without 00007 * modification, are permitted provided that the following conditions are met: 00008 * 00009 * * Redistributions of source code must retain the above copyright 00010 * notice, this list of conditions and the following disclaimer. 00011 * * Redistributions in binary form must reproduce the above copyright 00012 * notice, this list of conditions and the following disclaimer in the 00013 * documentation and/or other materials provided with the distribution. 00014 * * Neither the name of the Fraunhofer FKIE nor the names of its 00015 * contributors may be used to endorse or promote products derived from 00016 * this software without specific prior written permission. 00017 * 00018 * This file is part of the StructureColoring ROS package. 00019 * 00020 * The StructureColoring ROS package is free software: 00021 * you can redistribute it and/or modify it under the terms of the 00022 * GNU Lesser General Public License as published by the Free 00023 * Software Foundation, either version 3 of the License, or 00024 * (at your option) any later version. 00025 * 00026 * The StructureColoring ROS package is distributed in the hope that it will be useful, 00027 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00028 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00029 * GNU Lesser General Public License for more details. 00030 * 00031 * You should have received a copy of the GNU Lesser General Public License 00032 * along with The StructureColoring ROS package. 00033 * If not, see <http://www.gnu.org/licenses/>. 00034 */ 00035 00036 #ifndef ROTATIONHELPER_H_ 00037 #define ROTATIONHELPER_H_ 00038 00039 #include <Eigen/Dense> 00040 #include <limits.h> 00041 #include <cmath> 00042 00043 inline void calculateRotation(Eigen::Vector3f& axis, float& theta, const Eigen::Vector3f& a, const Eigen::Vector3f& b){ 00044 float x = (a.dot(b)) / (std::sqrt(a.squaredNorm() * b.squaredNorm())); // with normalization 00045 if ((x - 1.0) > -std::numeric_limits<float>::epsilon()) { // a colinear to b in the same direction (cross(a,b) = 0) 00046 theta = 0.0; 00047 Eigen::Vector3f c(0.0, 0.0, 0.0); 00048 if ((a[1] * a[1] + a[2] * a[2]) < std::numeric_limits<float>::epsilon()) // a colinear to x-axis 00049 c[1] = 1; // y-axis 00050 else 00051 c[0] = 1; // x-axis 00052 axis = a.cross(c); 00053 axis.normalize(); 00054 } else if ((x + 1.0) < std::numeric_limits<float>::epsilon()) { // a colinear to b in opposing directions (cross(a,b) = 0) 00055 theta = M_PI; 00056 Eigen::Vector3f c(0.0, 0.0, 0.0); 00057 if ((a[1] * a[1] + a[2] * a[2]) < std::numeric_limits<float>::epsilon()) // a colinear to x-axis 00058 c[1] = 1; // y-axis 00059 else 00060 c[0] = 1; // x-axis 00061 axis = a.cross(c); 00062 axis.normalize(); 00063 } else { 00064 theta = (x < 0.0) ? M_PI - 2 * asin(0.5 * (-a - b).norm()) : 2 * asin(0.5 * (a - b).norm()); 00065 axis = a.cross(b); 00066 axis.normalize(); 00067 } 00068 } 00069 00070 #endif /* ROTATIONHELPER_H_ */