C++ Vector3::isNaN方法代码示例

    //-----------------------------------------------------------------------
    Real ScaledPixelCountLodStrategy::getValueImpl(const MovableObject *movableObject, const Ogre::Camera *camera) const
    {
        // Get viewport
        const Viewport *viewport = camera->getViewport();

        // Get viewport area
        Real viewportArea = static_cast<Real>(viewport->getActualWidth() * viewport->getActualHeight());

        Ogre::Real boundingRadius = movableObject->getBoundingRadius();

        // Get area of unprojected circle with object bounding radius
        const Ogre::Vector3 nodeScale = movableObject->getParentNode()->getScale();

        if (!nodeScale.isNaN()) {
            //Get the largest scale of the parent node.
            //This is a bit inexact, since if the node is scaled with different values for the different axes we won't be using the most optimal one.
            //However, that would be too expensive (we would need to check with how the bounding box is rotated against the camera and so on.)
            //And further the vast majority of meshes we use are uniformly scaled.
            Ogre::Real scale = std::max(nodeScale.x, std::max(nodeScale.y, nodeScale.z));
            boundingRadius *= scale;
        }

        //Increase the bounding area by the scale
        Real boundingArea = Math::PI * Math::Sqr(boundingRadius);

        // Base computation on projection type
        switch (camera->getProjectionType())
        {
        case PT_PERSPECTIVE:
            {
                // Get camera distance
                Real distanceSquared = movableObject->getParentNode()->getSquaredViewDepth(camera);

                // Check for 0 distance
                if (distanceSquared <= std::numeric_limits<Real>::epsilon())
                    return getBaseValue();

                // Get projection matrix (this is done to avoid computation of tan(fov / 2))
                const Matrix4& projectionMatrix = camera->getProjectionMatrix();

                // Estimate pixel count
                return (boundingArea * viewportArea * projectionMatrix[0][0] * projectionMatrix[1][1]) / distanceSquared;
            }
        case PT_ORTHOGRAPHIC:
            {
                // Compute orthographic area
                Real orthoArea = camera->getOrthoWindowHeight() * camera->getOrthoWindowWidth();

                // Check for 0 orthographic area
                if (orthoArea <= std::numeric_limits<Real>::epsilon())
                    return getBaseValue();

                // Estimate pixel count
                return (boundingArea * viewportArea) / orthoArea;
            }
        default:
            {
                // This case is not covered for obvious reasons
                assert(0);
				return 0;
            }
        }
    }

    void CovarianceVisual::setMessage(const geometry_msgs::PoseWithCovariance& msg)
    {
        // Construct pose position and orientation.
        const geometry_msgs::Point& p = msg.pose.position;
        const geometry_msgs::Quaternion& q = msg.pose.orientation;

        Ogre::Vector3 position(p.x, p.y, p.z);
        Ogre::Quaternion orientation(q.w, q.x, q.y, q.z);

        // Set position and orientation for axes scene node.
        if(!position.isNaN())
            axes_->setPosition(position);
        else
            ROS_WARN_STREAM_THROTTLE(1, "position contains NaN: " << position);

        if(!orientation.isNaN())
            axes_->setOrientation (orientation);
        else
            ROS_WARN_STREAM_THROTTLE(1, "orientation contains NaN: " << orientation);

        // check for NaN in covariance
        for (unsigned i = 0; i < 3; ++i)
        {
            if(isnan(msg.covariance[i]))
            {
                ROS_WARN_THROTTLE(1, "covariance contains NaN");
                return;
            }
        }

        // Compute eigen values and vectors for both shapes.
        std::pair<Eigen::Matrix3d, Eigen::Vector3d> positionEigenVectorsAndValues(computeEigenValuesAndVectors(msg, 0));
        std::pair<Eigen::Matrix3d, Eigen::Vector3d> orientationEigenVectorsAndValues(computeEigenValuesAndVectors(msg, 3));

        Ogre::Quaternion positionQuaternion(computeRotation(msg, positionEigenVectorsAndValues));
        Ogre::Quaternion orientationQuaternion(computeRotation(msg, orientationEigenVectorsAndValues));

        positionNode_->setOrientation(positionQuaternion);
        orientationNode_->setOrientation(orientationQuaternion);

        // Compute scaling.
        //Ogre::Vector3 axesScaling(1, 1, 1);

        //axesScaling *= scaleFactor_;

        Ogre::Vector3 positionScaling
                      (std::sqrt (positionEigenVectorsAndValues.second[0]),
                       std::sqrt (positionEigenVectorsAndValues.second[1]),
                       std::sqrt (positionEigenVectorsAndValues.second[2]));

        positionScaling *= scaleFactor_;

        Ogre::Vector3 orientationScaling
                      (std::sqrt (orientationEigenVectorsAndValues.second[0]),
                       std::sqrt (orientationEigenVectorsAndValues.second[1]),
                       std::sqrt (orientationEigenVectorsAndValues.second[2]));

        orientationScaling *= scaleFactor_;

        // Set the scaling.
        /*if(!axesScaling.isNaN())
            axes_->setScale(axesScaling);
        else
            ROS_WARN_STREAM("axesScaling contains NaN: " << axesScaling);*/

        if(!positionScaling.isNaN())
            positionNode_->setScale(positionScaling);
        else
            ROS_WARN_STREAM("positionScaling contains NaN: " << positionScaling);

        if(!orientationScaling.isNaN())
            orientationNode_->setScale(orientationScaling);
        else
            ROS_WARN_STREAM("orientationScaling contains NaN: " << orientationScaling);

        // Debugging.
        ROS_INFO_STREAM_THROTTLE
        (1.,
        "Position:\n"
        << position << "\n"
        << "Positional part 3x3 eigen values:\n"
        << positionEigenVectorsAndValues.second << "\n"
        << "Positional part 3x3 eigen vectors:\n"
        << positionEigenVectorsAndValues.first << "\n"
        << "Sphere orientation:\n"
        << positionQuaternion << "\n"
        << positionQuaternion.getRoll () << " "
        << positionQuaternion.getPitch () << " "
        << positionQuaternion.getYaw () << "\n"
        << "Sphere scaling:\n"
        << positionScaling << "\n"
        << "Rotational part 3x3 eigen values:\n"
        << orientationEigenVectorsAndValues.second << "\n"
        << "Rotational part 3x3 eigen vectors:\n"
        << orientationEigenVectorsAndValues.first << "\n"
        << "Cone orientation:\n"
        << orientationQuaternion << "\n"
        << orientationQuaternion.getRoll () << " "
        << orientationQuaternion.getPitch () << " "
        << orientationQuaternion.getYaw () << "\n"
//.........这里部分代码省略.........