All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Valid-inl.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2013, SimQuest Solutions Inc.
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 
18 
19 #ifndef SURGSIM_MATH_VALID_INL_H
20 #define SURGSIM_MATH_VALID_INL_H
21 
22 #include <boost/math/special_functions/fpclassify.hpp>
23 
24 
25 namespace SurgSim
26 {
27 namespace Math
28 {
29 
30 namespace internal
31 {
32 
33 template <typename T, class V>
34 class PredicateAlwaysTrueVisitor
35 {
36 public:
37  typedef typename T::Index Index;
38  typedef typename T::Scalar Scalar;
39 
40  PredicateAlwaysTrueVisitor() : m_result(true)
41  {
42  }
43 
44  inline bool getResult() const
45  {
46  return m_result;
47  }
48 
49  inline void init(const Scalar& value, Index i, Index j)
50  {
51  if (! V::evaluate(value))
52  {
53  m_result = false;
54  }
55  }
56 
57  inline void operator()(const Scalar& value, Index i, Index j)
58  {
59  if (! V::evaluate(value))
60  {
61  m_result = false;
62  }
63  }
64 
65 private:
66  bool m_result;
67 };
68 
69 template <typename T>
70 class ValidVisitor : public PredicateAlwaysTrueVisitor<T, ValidVisitor<T>>
71 {
72 public:
73  typedef typename PredicateAlwaysTrueVisitor<T, ValidVisitor<T>>::Scalar Scalar;
74 
75  static bool evaluate(const Scalar& value)
76  {
77  // The extra parentheses protect from pain if isfinite() is also defined as a macro.
78  return (boost::math::isfinite)(value);
79  }
80 };
81 
82 template <typename T>
83 class NonSubnormalVisitor : public PredicateAlwaysTrueVisitor<T, NonSubnormalVisitor<T>>
84 {
85 public:
86  typedef typename PredicateAlwaysTrueVisitor<T, ValidVisitor<T>>::Scalar Scalar;
87 
88  static bool evaluate(const Scalar& value)
89  {
90  // The extra parentheses protect from pain if fpclassify() is also defined as a macro.
91  return ((boost::math::fpclassify)(value) != FP_SUBNORMAL);
92  }
93 };
94 
95 }; // namespace internal
96 
97 
98 inline bool isValid(float value)
99 {
100  // The extra parentheses protect from pain if isfinite() is also defined as a macro.
101  return (boost::math::isfinite)(value);
102 }
103 
104 inline bool isValid(double value)
105 {
106  // The extra parentheses protect from pain if isfinite() is also defined as a macro.
107  return (boost::math::isfinite)(value);
108 }
109 
110 template <typename T>
111 inline bool isValid(const Eigen::DenseBase<T>& value)
112 {
113  internal::ValidVisitor<T> visitor;
114  value.visit(visitor);
115  return visitor.getResult();
116 }
117 
118 template <typename T>
119 inline bool isValid(const Eigen::QuaternionBase<T>& value)
120 {
121  return isValid(value.coeffs());
122 }
123 
124 template <typename T>
125 inline bool isValid(const Eigen::AngleAxis<T>& value)
126 {
127  return isValid(value.angle()) && isValid(value.axis());
128 }
129 
130 template <typename T>
131 inline bool isValid(const Eigen::Rotation2D<T>& value)
132 {
133  return isValid(value.angle());
134 }
135 
136 template <typename T, int D, int M, int O>
137 inline bool isValid(const Eigen::Transform<T, D, M, O>& value)
138 {
139  return isValid(value.matrix());
140 }
141 
142 inline bool isSubnormal(float value)
143 {
144  // The extra parentheses protect from pain if fpclassify() is also defined as a macro.
145  return ((boost::math::fpclassify)(value) == FP_SUBNORMAL);
146 }
147 
148 inline bool isSubnormal(double value)
149 {
150  // The extra parentheses protect from pain if fpclassify() is also defined as a macro.
151  return ((boost::math::fpclassify)(value) == FP_SUBNORMAL);
152 }
153 
154 template <typename T>
155 inline bool isSubnormal(const Eigen::DenseBase<T>& value)
156 {
157  internal::NonSubnormalVisitor<T> visitor;
158  value.visit(visitor);
159  // The visitor checks whether *all* entries are "non-subnormal", i.e. false means some are subnormal.
160  // This is a consequence of deriving from PredicateAlwaysTrueVisitor.
161  return ! visitor.getResult();
162 }
163 
164 template <typename T>
165 inline bool isSubnormal(const Eigen::QuaternionBase<T>& value)
166 {
167  return isSubnormal(value.coeffs());
168 }
169 
170 template <typename T>
171 inline bool isSubnormal(const Eigen::AngleAxis<T>& value)
172 {
173  return isSubnormal(value.angle()) || isSubnormal(value.axis());
174 }
175 
176 template <typename T>
177 inline bool isSubnormal(const Eigen::Rotation2D<T>& value)
178 {
179  return isSubnormal(value.angle());
180 }
181 
182 template <typename T, int D, int M, int O>
183 inline bool isSubnormal(const Eigen::Transform<T, D, M, O>& value)
184 {
185  return isSubnormal(value.matrix());
186 }
187 
188 inline bool setSubnormalToZero(float* value)
189 {
190  // The extra parentheses protect from pain if fpclassify() is also defined as a macro.
191  if ((boost::math::fpclassify)(*value) != FP_SUBNORMAL)
192  {
193  return false;
194  }
195  else
196  {
197  *value = 0.0f;
198  return true;
199  }
200 }
201 
202 inline bool setSubnormalToZero(double* value)
203 {
204  // The extra parentheses protect from pain if fpclassify() is also defined as a macro.
205  if ((boost::math::fpclassify)(*value) != FP_SUBNORMAL)
206  {
207  return false;
208  }
209  else
210  {
211  *value = 0.0;
212  return true;
213  }
214 }
215 
216 template <typename T>
217 inline bool setSubnormalToZero(Eigen::DenseBase<T>* value)
218 {
219  if (! isSubnormal(*value)) // optimize for the common case where nothing is subnormal
220  {
221  return false;
222  }
223 
224  // TODO(bert): This is a simple implementation; it could be much more optimized by e.g. unrolling loops along
225  // the lines of the implementation of Eigen::DenseBase<T>::visit(). Unfortunately, we can't just *use* Eigen's
226  // visitors here, because the visitor API doesn't allow modifying the values.
227 
228  typedef typename Eigen::DenseBase<T>::Index Index;
229  const Index numColumns = value->cols();
230  const Index numRows = value->rows();
231 
232  for (Index j = 0; j < numColumns; ++j)
233  {
234  for (Index i = 0; i < numRows; ++i)
235  {
236  // The extra parentheses protect from pain if fpclassify() is also defined as a macro.
237  if ((boost::math::fpclassify)(value->coeffRef(i, j)) == FP_SUBNORMAL)
238  {
239  value->coeffRef(i, j) = 0;
240  }
241  }
242  }
243  return true;
244 }
245 
246 template <typename T>
247 inline bool setSubnormalToZero(Eigen::QuaternionBase<T>* value)
248 {
249  return setSubnormalToZero(&(value->coeffs()));
250 }
251 
252 template <typename T>
253 inline bool setSubnormalToZero(Eigen::AngleAxis<T>* value)
254 {
255  bool angleChanged = setSubnormalToZero(&(value->angle()));
256  bool axisChanged = setSubnormalToZero(&(value->axis()));
257  return angleChanged || axisChanged; // careful about short-circuiting!
258 }
259 
260 template <typename T>
261 inline bool setSubnormalToZero(Eigen::Rotation2D<T>* value)
262 {
263  return setSubnormalToZero(&(value->angle()));
264 }
265 
266 template <typename T, int D, int M, int O>
267 inline bool setSubnormalToZero(Eigen::Transform<T, D, M, O>* value)
268 {
269  return setSubnormalToZero(&(value->matrix()));
270 }
271 
272 }; // namespace Math
273 }; // namespace SurgSim
274 
275 #endif // SURGSIM_MATH_VALID_INL_H
bool isSubnormal(float value)
Check if a float value is subnormal.
Definition: Valid-inl.h:142
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