PSMoveFloatVector2 PSMoveFloatVector2::safe_divide(const PSMoveFloatVector2 &v, const PSMoveFloatVector2 &default_result) const
{
return
PSMoveFloatVector2::create(
!is_nearly_zero(v.i) ? i / v.i : default_result.i,
!is_nearly_zero(v.j) ? j / v.j : default_result.j);
}
PSMoveFloatVector3 PSMoveFloatVector3::safe_divide(const PSMoveFloatVector3 &v, const PSMoveFloatVector3 &default_result) const
{
return
PSMoveFloatVector3::create(
!is_nearly_zero(v.i) ? i/v.i : default_result.i,
!is_nearly_zero(v.j) ? j/v.j : default_result.j,
!is_nearly_zero(v.k) ? k/v.k : default_result.k);
}
Eigen::Quaternionf OrientationFilter::getOrientation(float time) const
{
Eigen::Quaternionf predicted_orientation =
is_nearly_zero(time)
? m_FusionState->orientation
: Eigen::Quaternionf(
m_FusionState->orientation.coeffs()
+ m_FusionState->orientation_first_derivative.coeffs()*time).normalized();
Eigen::Quaternionf result = m_FusionState->reset_orientation * predicted_orientation;
return result;
}
PSMoveFloatVector2 PSMoveFloatVector2::safe_divide(const float s, const PSMoveFloatVector2 &default_result) const
{
return !is_nearly_zero(s) ? unsafe_divide(s) : default_result;
}
PSMoveQuaternion PSMoveQuaternion::safe_divide(const float s, const PSMoveQuaternion &default_result) const
{
return !is_nearly_zero(s) ? unsafe_divide(s) : default_result;
}
float safe_sqrt_with_default(float square, float default_result)
{
return is_nearly_zero(square) ? default_result : sqrtf(square);
}
//-- float methods -----
float safe_divide_with_default(float numerator, float denomenator, float default_result)
{
return is_nearly_zero(denomenator) ? default_result : (numerator / denomenator);
}
