mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-13 20:48:42 +01:00
80 lines
2.0 KiB
C++
80 lines
2.0 KiB
C++
/*
|
|
* assertions.hpp
|
|
*
|
|
* Created on: Aug 6, 2009
|
|
* Author: Jonathan Brandmeyer
|
|
*
|
|
* This file is part of libeknav, imported into the OpenPilot
|
|
* project by Jonathan Brandmeyer.
|
|
*
|
|
* Libeknav 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, version 3.
|
|
*
|
|
* Libeknav 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.
|
|
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with libeknav. If not, see <http://www.gnu.org/licenses/>.
|
|
*
|
|
*/
|
|
|
|
#ifndef AHRS_ASSERTIONS_HPP
|
|
#define AHRS_ASSERTIONS_HPP
|
|
|
|
#include <Eigen/Core>
|
|
#include <cmath>
|
|
|
|
template<typename MatrixBase>
|
|
bool hasNaN(const MatrixBase & expr);
|
|
|
|
template<typename MatrixBase>
|
|
bool hasInf(const MatrixBase & expr);
|
|
|
|
template<typename MatrixBase>
|
|
bool perpendicular(const MatrixBase & expl, const MatrixBase & expr);
|
|
|
|
template<typename MatrixBase>
|
|
bool hasNaN(const MatrixBase & expr)
|
|
{
|
|
for (int j = 0; j != expr.cols(); ++j) {
|
|
for (int i = 0; i != expr.rows(); ++i) {
|
|
if (std::isnan(expr.coeff(i, j))) {
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
template<typename MatrixBase>
|
|
bool hasInf(const MatrixBase & expr)
|
|
{
|
|
for (int i = 0; i != expr.cols(); ++i) {
|
|
for (int j = 0; j != expr.rows(); ++j) {
|
|
if (std::isinf(expr.coeff(j, i))) {
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
template<typename MatrixBase>
|
|
bool isReal(const MatrixBase & exp)
|
|
{
|
|
return !hasNaN(exp) && !hasInf(exp);
|
|
}
|
|
|
|
template<typename MatrixBase>
|
|
bool perpendicular(const MatrixBase & lhs, const MatrixBase & rhs)
|
|
{
|
|
// A really weak check for "almost perpendicular". Use it for debugging
|
|
// purposes only.
|
|
return fabs(rhs.dot(lhs)) < 0.0001;
|
|
}
|
|
|
|
#endif /* ASSERTIONS_HPP_ */
|