19 #ifndef SURGSIM_MATH_VALID_INL_H
20 #define SURGSIM_MATH_VALID_INL_H
22 #include <boost/math/special_functions/fpclassify.hpp>
33 template <
typename T,
class V>
34 class PredicateAlwaysTrueVisitor
37 typedef typename T::Index Index;
38 typedef typename T::Scalar Scalar;
40 PredicateAlwaysTrueVisitor() : m_result(true)
44 inline bool getResult()
const
49 inline void init(
const Scalar& value, Index i, Index j)
51 if (! V::evaluate(value))
57 inline void operator()(
const Scalar& value, Index i, Index j)
59 if (! V::evaluate(value))
70 class ValidVisitor :
public PredicateAlwaysTrueVisitor<T, ValidVisitor<T>>
73 typedef typename PredicateAlwaysTrueVisitor<T, ValidVisitor<T>>::Scalar Scalar;
75 static bool evaluate(
const Scalar& value)
78 return (boost::math::isfinite)(value);
83 class NonSubnormalVisitor :
public PredicateAlwaysTrueVisitor<T, NonSubnormalVisitor<T>>
86 typedef typename PredicateAlwaysTrueVisitor<T, ValidVisitor<T>>::Scalar Scalar;
88 static bool evaluate(
const Scalar& value)
91 return ((boost::math::fpclassify)(value) != FP_SUBNORMAL);
101 return (boost::math::isfinite)(value);
107 return (boost::math::isfinite)(value);
110 template <
typename T>
111 inline bool isValid(
const Eigen::DenseBase<T>& value)
113 internal::ValidVisitor<T> visitor;
114 value.visit(visitor);
115 return visitor.getResult();
118 template <
typename T>
119 inline bool isValid(
const Eigen::QuaternionBase<T>& value)
121 return isValid(value.coeffs());
124 template <
typename T>
125 inline bool isValid(
const Eigen::AngleAxis<T>& value)
130 template <
typename T>
131 inline bool isValid(
const Eigen::Rotation2D<T>& value)
136 template <
typename T,
int D,
int M,
int O>
137 inline bool isValid(
const Eigen::Transform<T, D, M, O>& value)
139 return isValid(value.matrix());
145 return ((boost::math::fpclassify)(value) == FP_SUBNORMAL);
151 return ((boost::math::fpclassify)(value) == FP_SUBNORMAL);
154 template <
typename T>
157 internal::NonSubnormalVisitor<T> visitor;
158 value.visit(visitor);
161 return ! visitor.getResult();
164 template <
typename T>
170 template <
typename T>
176 template <
typename T>
182 template <
typename T,
int D,
int M,
int O>
183 inline bool isSubnormal(
const Eigen::Transform<T, D, M, O>& value)
191 if ((boost::math::fpclassify)(*value) != FP_SUBNORMAL)
205 if ((boost::math::fpclassify)(*value) != FP_SUBNORMAL)
216 template <
typename T>
228 typedef typename Eigen::DenseBase<T>::Index Index;
229 const Index numColumns = value->cols();
230 const Index numRows = value->rows();
232 for (Index j = 0; j < numColumns; ++j)
234 for (Index i = 0; i < numRows; ++i)
237 if ((boost::math::fpclassify)(value->coeffRef(i, j)) == FP_SUBNORMAL)
239 value->coeffRef(i, j) = 0;
246 template <
typename T>
252 template <
typename T>
257 return angleChanged || axisChanged;
260 template <
typename T>
266 template <
typename T,
int D,
int M,
int O>
275 #endif // SURGSIM_MATH_VALID_INL_H
bool isSubnormal(float value)
Check if a float value is subnormal.
Definition: Valid-inl.h:142
Definition: DriveElementFromInputBehavior.cpp:27
bool setSubnormalToZero(float *value)
If the float value is subnormal, set it to zero.
Definition: Valid-inl.h:188
bool isValid(float value)
Check if a float value is valid.
Definition: Valid-inl.h:98