Exemplos de Matrix3d::isIdentity em C++ (Cpp)

Exemplo n.º 1

Arquivo: kinematic_constraint.cpp Projeto: jonbinney/moveit-core

bool kinematic_constraints::OrientationConstraint::equal(const KinematicConstraint &other, double margin) const
{
  if (other.getType() != type_)
    return false;
  const OrientationConstraint &o = static_cast<const OrientationConstraint&>(other);
  
  if (o.link_model_ == link_model_ && desired_rotation_frame_id_ == o.desired_rotation_frame_id_)
  {
    Eigen::Matrix3d diff = desired_rotation_matrix_.inverse() * o.desired_rotation_matrix_;
    if (!diff.isIdentity(margin))
      return false;
    return fabs(absolute_x_axis_tolerance_ - o.absolute_x_axis_tolerance_) <= margin &&
      fabs(absolute_y_axis_tolerance_ - o.absolute_y_axis_tolerance_) <= margin &&
      fabs(absolute_z_axis_tolerance_ - o.absolute_z_axis_tolerance_) <= margin;
  }
  return false;
}