Valid-inl.h
Go to the documentation of this file.
// This file is a part of the OpenSurgSim project.
// Copyright 2013, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef SURGSIM_MATH_VALID_INL_H
#define SURGSIM_MATH_VALID_INL_H
#include <boost/math/special_functions/fpclassify.hpp>
namespace SurgSim
{
namespace Math
{
namespace internal
{
template <typename T, class V>
class PredicateAlwaysTrueVisitor
{
public:
typedef typename T::Index Index;
typedef typename T::Scalar Scalar;
PredicateAlwaysTrueVisitor() : m_result(true)
{
}
inline bool getResult() const
{
return m_result;
}
inline void init(const Scalar& value, Index i, Index j)
{
if (! V::evaluate(value))
{
m_result = false;
}
}
inline void operator()(const Scalar& value, Index i, Index j)
{
if (! V::evaluate(value))
{
m_result = false;
}
}
private:
bool m_result;
};
template <typename T>
class ValidVisitor : public PredicateAlwaysTrueVisitor<T, ValidVisitor<T>>
{
public:
typedef typename PredicateAlwaysTrueVisitor<T, ValidVisitor<T>>::Scalar Scalar;
static bool evaluate(const Scalar& value)
{
// The extra parentheses protect from pain if isfinite() is also defined as a macro.
return (boost::math::isfinite)(value);
}
};
template <typename T>
class NonSubnormalVisitor : public PredicateAlwaysTrueVisitor<T, NonSubnormalVisitor<T>>
{
public:
typedef typename PredicateAlwaysTrueVisitor<T, ValidVisitor<T>>::Scalar Scalar;
static bool evaluate(const Scalar& value)
{
// The extra parentheses protect from pain if fpclassify() is also defined as a macro.
return ((boost::math::fpclassify)(value) != FP_SUBNORMAL);
}
};
}; // namespace internal
inline bool isValid(float value)
{
// The extra parentheses protect from pain if isfinite() is also defined as a macro.
return (boost::math::isfinite)(value);
}
inline bool isValid(double value)
{
// The extra parentheses protect from pain if isfinite() is also defined as a macro.
return (boost::math::isfinite)(value);
}
template <typename T>
inline bool isValid(const Eigen::DenseBase<T>& value)
{
internal::ValidVisitor<T> visitor;
value.visit(visitor);
return visitor.getResult();
}
template <typename T>
inline bool isValid(const Eigen::QuaternionBase<T>& value)
{
return isValid(value.coeffs());
}
template <typename T>
inline bool isValid(const Eigen::AngleAxis<T>& value)
{
return isValid(value.angle()) && isValid(value.axis());
}
template <typename T>
inline bool isValid(const Eigen::Rotation2D<T>& value)
{
return isValid(value.angle());
}
template <typename T, int D, int M, int O>
inline bool isValid(const Eigen::Transform<T, D, M, O>& value)
{
return isValid(value.matrix());
}
inline bool isSubnormal(float value)
{
// The extra parentheses protect from pain if fpclassify() is also defined as a macro.
return ((boost::math::fpclassify)(value) == FP_SUBNORMAL);
}
inline bool isSubnormal(double value)
{
// The extra parentheses protect from pain if fpclassify() is also defined as a macro.
return ((boost::math::fpclassify)(value) == FP_SUBNORMAL);
}
template <typename T>
inline bool isSubnormal(const Eigen::DenseBase<T>& value)
{
internal::NonSubnormalVisitor<T> visitor;
value.visit(visitor);
// The visitor checks whether *all* entries are "non-subnormal", i.e. false means some are subnormal.
// This is a consequence of deriving from PredicateAlwaysTrueVisitor.
return ! visitor.getResult();
}
template <typename T>
inline bool isSubnormal(const Eigen::QuaternionBase<T>& value)
{
return isSubnormal(value.coeffs());
}
template <typename T>
inline bool isSubnormal(const Eigen::AngleAxis<T>& value)
{
return isSubnormal(value.angle()) || isSubnormal(value.axis());
}
template <typename T>
inline bool isSubnormal(const Eigen::Rotation2D<T>& value)
{
return isSubnormal(value.angle());
}
template <typename T, int D, int M, int O>
inline bool isSubnormal(const Eigen::Transform<T, D, M, O>& value)
{
return isSubnormal(value.matrix());
}
inline bool setSubnormalToZero(float* value)
{
// The extra parentheses protect from pain if fpclassify() is also defined as a macro.
if ((boost::math::fpclassify)(*value) != FP_SUBNORMAL)
{
return false;
}
else
{
*value = 0.0f;
return true;
}
}
inline bool setSubnormalToZero(double* value)
{
// The extra parentheses protect from pain if fpclassify() is also defined as a macro.
if ((boost::math::fpclassify)(*value) != FP_SUBNORMAL)
{
return false;
}
else
{
*value = 0.0;
return true;
}
}
template <typename T>
inline bool setSubnormalToZero(Eigen::DenseBase<T>* value)
{
if (! isSubnormal(*value)) // optimize for the common case where nothing is subnormal
{
return false;
}
// TODO(bert): This is a simple implementation; it could be much more optimized by e.g. unrolling loops along
// the lines of the implementation of Eigen::DenseBase<T>::visit(). Unfortunately, we can't just *use* Eigen's
// visitors here, because the visitor API doesn't allow modifying the values.
typedef typename Eigen::DenseBase<T>::Index Index;
const Index numColumns = value->cols();
const Index numRows = value->rows();
for (Index j = 0; j < numColumns; ++j)
{
for (Index i = 0; i < numRows; ++i)
{
// The extra parentheses protect from pain if fpclassify() is also defined as a macro.
if ((boost::math::fpclassify)(value->coeffRef(i, j)) == FP_SUBNORMAL)
{
value->coeffRef(i, j) = 0;
}
}
}
return true;
}
template <typename T>
inline bool setSubnormalToZero(Eigen::QuaternionBase<T>* value)
{
return setSubnormalToZero(&(value->coeffs()));
}
template <typename T>
inline bool setSubnormalToZero(Eigen::AngleAxis<T>* value)
{
bool angleChanged = setSubnormalToZero(&(value->angle()));
bool axisChanged = setSubnormalToZero(&(value->axis()));
return angleChanged || axisChanged; // careful about short-circuiting!
}
template <typename T>
inline bool setSubnormalToZero(Eigen::Rotation2D<T>* value)
{
return setSubnormalToZero(&(value->angle()));
}
template <typename T, int D, int M, int O>
inline bool setSubnormalToZero(Eigen::Transform<T, D, M, O>* value)
{
return setSubnormalToZero(&(value->matrix()));
}
}; // namespace Math
}; // namespace SurgSim
#endif // SURGSIM_MATH_VALID_INL_H
SurgSim::Math::isSubnormal
bool isSubnormal(float value)
Check if a float value is subnormal.
Definition: Valid-inl.h:142
SurgSim
Definition: CompoundShapeToGraphics.cpp:29
SurgSim::Math::setSubnormalToZero
bool setSubnormalToZero(float *value)
If the float value is subnormal, set it to zero.
Definition: Valid-inl.h:188
SurgSim::Math::isValid
bool isValid(float value)
Check if a float value is valid.
Definition: Valid-inl.h:98