<commit_before>/*
 * groundManager.cpp
 *
 *  Created on: Sep 11, 2013
 *      Author: chpark
 */
#include <itomp_cio_planner/contact/ground_manager.h>
#include <itomp_cio_planner/optimization/phase_manager.h>
#include <itomp_cio_planner/util/planning_parameters.h>
#include <itomp_cio_planner/util/point_to_triangle_projection.h>
#include <itomp_cio_planner/util/exponential_map.h>
#include <itomp_cio_planner/visualization/new_viz_manager.h>
#include <geometric_shapes/mesh_operations.h>
#include <geometric_shapes/shape_operations.h>
#include <geometric_shapes/shapes.h>
#include <limits>

namespace itomp_cio_planner
{

Plane::Plane(const Triangle &triangle)
{
    normal_ = triangle.normal_;
    d_ = -( normal_(0) * triangle.points_[0](0) + normal_(1) * triangle.points_[0](1) + normal_(2) * triangle.points_[0](2));
}

bool Plane::isTriangleIn(const Triangle &triangle) const
{
    double normal_error = std::min((normal_ - triangle.normal_).norm(), (normal_ + triangle.normal_).norm());
    if (normal_error >= ITOMP_EPS)
        return false;
    double pos_error = std::abs(normal_(0) * triangle.points_[0](0) + normal_(1) * triangle.points_[0](1) + normal_(2) * triangle.points_[0](2) + d_);
    return pos_error < ITOMP_EPS;
}

bool Plane::projectionZ(const Eigen::Vector3d& position_in, Eigen::Vector3d& position_out) const
{
    if (normal_(2) < ITOMP_EPS) // ignore -z surfaces
        return false;

    position_out(0) = position_in(0);
    position_out(1) = position_in(1);
    position_out(2) = -(normal_(0) * position_in(0) + normal_(1) * position_in(1) + d_) / normal_(2);

    return true;
}

GroundManager::GroundManager()
{
}

GroundManager::~GroundManager()
{
}

void GroundManager::initialize(const planning_scene::PlanningSceneConstPtr& planning_scene)
{
	planning_scene_ = planning_scene;
    initializeContactSurfaces();
}

void GroundManager::getNearestContactPosition(const Eigen::Vector3d& position_in,
		const Eigen::Vector3d& orientation_in, Eigen::Vector3d& position_out,
        Eigen::Vector3d& orientation_out, Eigen::Vector3d& normal, bool include_ground, bool ignore_Z) const
{
    bool inc = PlanningParameters::getInstance()->getUseDefaultContactGround();

    double min_dist = (inc ? (position_in(2) - 0) : std::numeric_limits<double>::max());

    Eigen::Matrix3d orientation_in_mat = exponential_map::ExponentialMapToRotation(orientation_in);
    Eigen::Vector3d x_axis = orientation_in_mat.col(0);
    Eigen::Vector3d y_axis = orientation_in_mat.col(1);
    Eigen::Vector3d normal_in = orientation_in_mat.col(2);

    Eigen::Vector3d temp_position_out;

    if (inc)
    {
        // ground

        temp_position_out = Eigen::Vector3d(position_in(0), position_in(1), 0.0);
        normal = Eigen::Vector3d(0, 0, 1);
        temp_position_out(2) = 0.0;
    }

    getNearestMeshPosition(position_in, temp_position_out, normal_in, normal, min_dist, ignore_Z);

	Eigen::Vector3d proj_x_axis = x_axis - x_axis.dot(normal) * normal;
	Eigen::Vector3d proj_y_axis = y_axis - y_axis.dot(normal) * normal;
	if (proj_x_axis.norm() > proj_y_axis.norm())
	{
		proj_x_axis.normalize();
		proj_y_axis = normal.cross(proj_x_axis);
	}
	else
	{
		proj_y_axis.normalize();
		proj_x_axis = proj_y_axis.cross(normal);
	}
	Eigen::Matrix3d orientation_out_mat;
	orientation_out_mat.col(0) = proj_x_axis;
	orientation_out_mat.col(1) = proj_y_axis;
	orientation_out_mat.col(2) = normal;
    orientation_out = exponential_map::RotationToExponentialMap(orientation_out_mat);
    //orientation_out.normalize();

    position_out = temp_position_out;
}

bool GroundManager::getNearestMeshPosition(const Eigen::Vector3d& position_in,
		Eigen::Vector3d& position_out, const Eigen::Vector3d& normal_in, Eigen::Vector3d& normal,
        double current_min_distance, bool ignore_Z) const
{
    bool NO_INTERPOLATED = true;
	bool updated = false;

    if (PhaseManager::getInstance()->getPhase() == 2 || NO_INTERPOLATED)
	{
        for (int i = 0; i < triangles_.size(); ++i)
        {
            const Triangle& triangle = triangles_[i];

            Eigen::Vector3d projection = ProjPoint2Triangle(triangle.points_[0], triangle.points_[1],
                                         triangle.points_[2], position_in);

            double distance = (position_in - projection).norm();
            //if (ignore_Z)
            /*
            {
                Eigen::Vector3d diff = position_in - projection;
                diff(2) = 0.0;
                distance = diff.norm();
            }
            */

            if (distance < current_min_distance)
            {
                current_min_distance = distance;
                normal = triangle.normal_;
                position_out = projection;

                updated = true;
            }
		}
	}
    else
    {
        std::vector<double> weights;
        weights.reserve(triangles_.size());
        std::vector<Eigen::Vector3d> proj_points;
        proj_points.reserve(triangles_.size());
        double weight_sum = 0.0;

        const double SMOOTHNESS_PARAM = 1000;

        for (int i = 0; i < triangles_.size(); ++i)
        {
            const Triangle& triangle = triangles_[i];

            Eigen::Vector3d projection = ProjPoint2Triangle(triangle.points_[0], triangle.points_[1],
                                         triangle.points_[2], position_in);

            double distance = (position_in - projection).norm();
            //if (ignore_Z)
            {
                Eigen::Vector3d diff = position_in - projection;
                //diff(2) = 0.0;
                distance = diff.norm();
            }

            if (distance < current_min_distance)
            {
                current_min_distance = distance;
                normal = triangle.normal_;
                //position_out = projection;
            }

            updated = true;

            weights.push_back(1.0 / (1.0 + distance * distance * SMOOTHNESS_PARAM));
            proj_points.push_back(projection);
            weight_sum += weights.back();

        }

        if (updated)
        {
            //printf("Pos : %f %f %f\n", position_in(0), position_in(1), position_in(2));
            Eigen::Vector3d weighted_point = Eigen::Vector3d::Zero();

            for (int i = 0; i < proj_points.size(); ++i)
            {
                weighted_point += weights[i] / weight_sum * proj_points[i];
                // TODO: normal?
                //printf("Proj %d : (%f) %f %f %f\n", weights[i] / weight_sum, proj_points[i](0), proj_points[i](1), proj_points[i](2));
                //printf("WP %f %f %f\n", weighted_point(0), weighted_point(1), weighted_point(2));
            }
            position_out = weighted_point;

            //printf("WPos : %f %f %f\n", position_out(0), position_out(1), position_out(2));
        }
    }

	return updated;
}

void GroundManager::getNearestZPosition(const Eigen::Vector3d& position_in, Eigen::Vector3d& position_out, Eigen::Vector3d& normal) const
{
    double min_dist = std::numeric_limits<double>::max();

    if (PlanningParameters::getInstance()->getUseDefaultContactGround())
    {
        min_dist = position_in(2) - 0.0;
        position_out = Eigen::Vector3d(position_in(0), position_in(1), 0.0);
        normal = Eigen::Vector3d(0, 0, 1);
    }

    getNearestMeshZPosition(position_in, position_out, normal, min_dist);
}

void GroundManager::getNearestZPosition(const Eigen::Vector3d& position_in, const Eigen::Vector3d& orientation_in,
                         Eigen::Vector3d& position_out, Eigen::Vector3d& orientation_out, Eigen::Vector3d& normal) const
{
    double min_dist = std::numeric_limits<double>::max();

    Eigen::Matrix3d orientation_in_mat = exponential_map::ExponentialMapToRotation(orientation_in);
    Eigen::Vector3d x_axis = orientation_in_mat.col(0);
    Eigen::Vector3d y_axis = orientation_in_mat.col(1);
    Eigen::Vector3d normal_in = orientation_in_mat.col(2);

    if (PlanningParameters::getInstance()->getUseDefaultContactGround())
    {
        min_dist = position_in(2) - 0.0;
        position_out = Eigen::Vector3d(position_in(0), position_in(1), 0.0);
        normal = Eigen::Vector3d(0, 0, 1);
    }

    getNearestMeshZPosition(position_in, position_out, normal, min_dist);

    Eigen::Vector3d proj_x_axis = x_axis - x_axis.dot(normal) * normal;
    Eigen::Vector3d proj_y_axis = y_axis - y_axis.dot(normal) * normal;
    if (proj_x_axis.norm() > proj_y_axis.norm())
    {
        proj_x_axis.normalize();
        proj_y_axis = normal.cross(proj_x_axis);
    }
    else
    {
        proj_y_axis.normalize();
        proj_x_axis = proj_y_axis.cross(normal);
    }
    Eigen::Matrix3d orientation_out_mat;
    orientation_out_mat.col(0) = proj_x_axis;
    orientation_out_mat.col(1) = proj_y_axis;
    orientation_out_mat.col(2) = normal;
    orientation_out = exponential_map::RotationToExponentialMap(orientation_out_mat);

}

void GroundManager::getNearestMeshZPosition(const Eigen::Vector3d& position_in, Eigen::Vector3d& position_out, Eigen::Vector3d& normal, double current_min_distance) const
{
    for (int i = 0; i < triangles_.size(); ++i)
    {
        const Triangle& triangle = triangles_[i];
        const Plane& plane = planes_[triangle.plane_index_];

        Eigen::Vector3d projection;
        if (plane.projectionZ(position_in, projection) == false)
            continue;

        projection = ProjPoint2Triangle(triangle.points_[0], triangle.points_[1], triangle.points_[2], projection);

        Eigen::Vector3d diff = projection - position_in;
        if (std::abs(diff(0)) > ITOMP_EPS || std::abs(diff(1)) > ITOMP_EPS)
            continue;

        double distance = std::abs(diff(2));

        if (distance < current_min_distance)
        {
            current_min_distance = distance;
            normal = triangle.normal_;
            position_out = projection;
        }
    }
}

void GroundManager::initializeContactSurfaces()
{
	triangles_.clear();

    std::string contact_model = PlanningParameters::getInstance()->getContactModel();
    if (contact_model == "")
        return;

    const std::vector<double>& contact_model_position = PlanningParameters::getInstance()->getContactModelPosition();
    double contact_model_scale = PlanningParameters::getInstance()->getContactModelScale();
    Eigen::Vector3d scale(contact_model_scale, contact_model_scale, contact_model_scale);
    Eigen::Vector3d translation(contact_model_position[0], contact_model_position[1], contact_model_position[2]);

    shapes::Mesh* mesh = shapes::createMeshFromResource(contact_model, scale);
    if (mesh == NULL)
        return;

    for (int k = 0; k < mesh->triangle_count; ++k)
    {
        int triangle_vertex1 = mesh->triangles[3 * k];
        int triangle_vertex2 = mesh->triangles[3 * k + 1];
        int triangle_vertex3 = mesh->triangles[3 * k + 2];
        Eigen::Vector3d position1(mesh->vertices[3 * triangle_vertex1], mesh->vertices[3 * triangle_vertex1 + 1], mesh->vertices[3 * triangle_vertex1 + 2]);
        Eigen::Vector3d position2(mesh->vertices[3 * triangle_vertex2], mesh->vertices[3 * triangle_vertex2 + 1], mesh->vertices[3 * triangle_vertex2 + 2]);
        Eigen::Vector3d position3(mesh->vertices[3 * triangle_vertex3], mesh->vertices[3 * triangle_vertex3 + 1], mesh->vertices[3 * triangle_vertex3 + 2]);

        Eigen::Vector3d p0 = (position2 - position1);
        Eigen::Vector3d p1 = (position3 - position1);
        p0.normalize();
        p1.normalize();
        Eigen::Vector3d normal = p0.cross(p1);
        if (normal.norm() < ITOMP_EPS)
            continue;
        normal.normalize();

        Triangle tri;
        tri.points_[0] = position1 + translation;
        tri.points_[1] = position2 + translation;
        tri.points_[2] = position3 + translation;
        tri.normal_ = normal;

        tri.plane_index_ = -1;
        for (int i = 0; i < planes_.size(); ++i)
        {
            Plane& plane = planes_[i];
            if (plane.isTriangleIn(tri))
            {
                plane.triangle_indices_.insert(triangles_.size());
                tri.plane_index_ = i;
            }
        }
        if (tri.plane_index_ == -1)
        {
            tri.plane_index_ = planes_.size();
            planes_.push_back(Plane(tri));
            planes_.back().triangle_indices_.insert(triangles_.size());
        }
        triangles_.push_back(tri);
    }

    NewVizManager::getInstance()->renderContactSurface();
}

}

<commit_msg>compute blended contact surface<commit_after>/*
 * groundManager.cpp
 *
 *  Created on: Sep 11, 2013
 *      Author: chpark
 */
#include <itomp_cio_planner/contact/ground_manager.h>
#include <itomp_cio_planner/optimization/phase_manager.h>
#include <itomp_cio_planner/util/planning_parameters.h>
#include <itomp_cio_planner/util/point_to_triangle_projection.h>
#include <itomp_cio_planner/util/exponential_map.h>
#include <itomp_cio_planner/visualization/new_viz_manager.h>
#include <geometric_shapes/mesh_operations.h>
#include <geometric_shapes/shape_operations.h>
#include <geometric_shapes/shapes.h>
#include <limits>

namespace itomp_cio_planner
{

Plane::Plane(const Triangle &triangle)
{
    normal_ = triangle.normal_;
    d_ = -( normal_(0) * triangle.points_[0](0) + normal_(1) * triangle.points_[0](1) + normal_(2) * triangle.points_[0](2));
}

bool Plane::isTriangleIn(const Triangle &triangle) const
{
    double normal_error = std::min((normal_ - triangle.normal_).norm(), (normal_ + triangle.normal_).norm());
    if (normal_error >= ITOMP_EPS)
        return false;
    double pos_error = std::abs(normal_(0) * triangle.points_[0](0) + normal_(1) * triangle.points_[0](1) + normal_(2) * triangle.points_[0](2) + d_);
    return pos_error < ITOMP_EPS;
}

bool Plane::projectionZ(const Eigen::Vector3d& position_in, Eigen::Vector3d& position_out) const
{
    if (normal_(2) < ITOMP_EPS) // ignore -z surfaces
        return false;

    position_out(0) = position_in(0);
    position_out(1) = position_in(1);
    position_out(2) = -(normal_(0) * position_in(0) + normal_(1) * position_in(1) + d_) / normal_(2);

    return true;
}

GroundManager::GroundManager()
{
}

GroundManager::~GroundManager()
{
}

void GroundManager::initialize(const planning_scene::PlanningSceneConstPtr& planning_scene)
{
	planning_scene_ = planning_scene;
    initializeContactSurfaces();
}

void GroundManager::getNearestContactPosition(const Eigen::Vector3d& position_in,
		const Eigen::Vector3d& orientation_in, Eigen::Vector3d& position_out,
        Eigen::Vector3d& orientation_out, Eigen::Vector3d& normal, bool include_ground, bool ignore_Z) const
{
    bool inc = PlanningParameters::getInstance()->getUseDefaultContactGround();

    double min_dist = (inc ? (position_in(2) - 0) : std::numeric_limits<double>::max());

    Eigen::Matrix3d orientation_in_mat = exponential_map::ExponentialMapToRotation(orientation_in);
    Eigen::Vector3d x_axis = orientation_in_mat.col(0);
    Eigen::Vector3d y_axis = orientation_in_mat.col(1);
    Eigen::Vector3d normal_in = orientation_in_mat.col(2);

    Eigen::Vector3d temp_position_out;

    if (inc)
    {
        // ground

        temp_position_out = Eigen::Vector3d(position_in(0), position_in(1), 0.0);
        normal = Eigen::Vector3d(0, 0, 1);
        temp_position_out(2) = 0.0;
    }

    getNearestMeshPosition(position_in, temp_position_out, normal_in, normal, min_dist, ignore_Z);

	Eigen::Vector3d proj_x_axis = x_axis - x_axis.dot(normal) * normal;
	Eigen::Vector3d proj_y_axis = y_axis - y_axis.dot(normal) * normal;
	if (proj_x_axis.norm() > proj_y_axis.norm())
	{
		proj_x_axis.normalize();
		proj_y_axis = normal.cross(proj_x_axis);
	}
	else
	{
		proj_y_axis.normalize();
		proj_x_axis = proj_y_axis.cross(normal);
	}
	Eigen::Matrix3d orientation_out_mat;
	orientation_out_mat.col(0) = proj_x_axis;
	orientation_out_mat.col(1) = proj_y_axis;
	orientation_out_mat.col(2) = normal;
    orientation_out = exponential_map::RotationToExponentialMap(orientation_out_mat);
    //orientation_out.normalize();

    position_out = temp_position_out;
}

bool GroundManager::getNearestMeshPosition(const Eigen::Vector3d& position_in,
		Eigen::Vector3d& position_out, const Eigen::Vector3d& normal_in, Eigen::Vector3d& normal,
        double current_min_distance, bool ignore_Z) const
{
    bool NO_INTERPOLATED = false;
	bool updated = false;

    if (/*PhaseManager::getInstance()->getPhase() == 2 || */NO_INTERPOLATED)
	{
        for (int i = 0; i < triangles_.size(); ++i)
        {
            const Triangle& triangle = triangles_[i];

            Eigen::Vector3d projection = ProjPoint2Triangle(triangle.points_[0], triangle.points_[1],
                                         triangle.points_[2], position_in);

            double distance = (position_in - projection).norm();
            //if (ignore_Z)

            {
                Eigen::Vector3d diff = position_in - projection;
                diff(2) = 0.0;
                distance = diff.norm();
            }

            if (distance < current_min_distance)
            {
                current_min_distance = distance;
                normal = triangle.normal_;
                position_out = projection;

                updated = true;
            }
		}
	}
    else
    {
        std::vector<double> weights;
        weights.reserve(triangles_.size());
        std::vector<Eigen::Vector3d> proj_points;
        proj_points.reserve(triangles_.size());
        double weight_sum = 0.0;

        const double SMOOTHNESS_PARAM = 1000;

        for (int i = 0; i < triangles_.size(); ++i)
        {
            const Triangle& triangle = triangles_[i];

            Eigen::Vector3d projection = ProjPoint2Triangle(triangle.points_[0], triangle.points_[1],
                                         triangle.points_[2], position_in);

            double distance = (position_in - projection).norm();
            //if (ignore_Z)
            {
                Eigen::Vector3d diff = position_in - projection;
                diff(2) = 0.0;
                distance = diff.norm();
            }

            if (distance < current_min_distance)
            {
                current_min_distance = distance;
                normal = triangle.normal_;
                //position_out = projection;
            }

            updated = true;

            weights.push_back(1.0 / (1.0 + distance * distance * SMOOTHNESS_PARAM));
            proj_points.push_back(projection);
            weight_sum += weights.back();

        }

        if (updated)
        {
            //printf("Pos : %f %f %f\n", position_in(0), position_in(1), position_in(2));
            Eigen::Vector3d weighted_point = Eigen::Vector3d::Zero();

            for (int i = 0; i < proj_points.size(); ++i)
            {
                weighted_point += weights[i] / weight_sum * proj_points[i];
                // TODO: normal?
                //printf("Proj %d : (%f) %f %f %f\n", weights[i] / weight_sum, proj_points[i](0), proj_points[i](1), proj_points[i](2));
                //printf("WP %f %f %f\n", weighted_point(0), weighted_point(1), weighted_point(2));
            }
            position_out = weighted_point;

            //printf("WPos : %f %f %f\n", position_out(0), position_out(1), position_out(2));
        }
    }

	return updated;
}

void GroundManager::getNearestZPosition(const Eigen::Vector3d& position_in, Eigen::Vector3d& position_out, Eigen::Vector3d& normal) const
{
    double min_dist = std::numeric_limits<double>::max();

    if (PlanningParameters::getInstance()->getUseDefaultContactGround())
    {
        min_dist = position_in(2) - 0.0;
        position_out = Eigen::Vector3d(position_in(0), position_in(1), 0.0);
        normal = Eigen::Vector3d(0, 0, 1);
    }

    getNearestMeshZPosition(position_in, position_out, normal, min_dist);
}

void GroundManager::getNearestZPosition(const Eigen::Vector3d& position_in, const Eigen::Vector3d& orientation_in,
                         Eigen::Vector3d& position_out, Eigen::Vector3d& orientation_out, Eigen::Vector3d& normal) const
{
    double min_dist = std::numeric_limits<double>::max();

    Eigen::Matrix3d orientation_in_mat = exponential_map::ExponentialMapToRotation(orientation_in);
    Eigen::Vector3d x_axis = orientation_in_mat.col(0);
    Eigen::Vector3d y_axis = orientation_in_mat.col(1);
    Eigen::Vector3d normal_in = orientation_in_mat.col(2);

    if (PlanningParameters::getInstance()->getUseDefaultContactGround())
    {
        min_dist = position_in(2) - 0.0;
        position_out = Eigen::Vector3d(position_in(0), position_in(1), 0.0);
        normal = Eigen::Vector3d(0, 0, 1);
    }

    getNearestMeshZPosition(position_in, position_out, normal, min_dist);

    Eigen::Vector3d proj_x_axis = x_axis - x_axis.dot(normal) * normal;
    Eigen::Vector3d proj_y_axis = y_axis - y_axis.dot(normal) * normal;
    if (proj_x_axis.norm() > proj_y_axis.norm())
    {
        proj_x_axis.normalize();
        proj_y_axis = normal.cross(proj_x_axis);
    }
    else
    {
        proj_y_axis.normalize();
        proj_x_axis = proj_y_axis.cross(normal);
    }
    Eigen::Matrix3d orientation_out_mat;
    orientation_out_mat.col(0) = proj_x_axis;
    orientation_out_mat.col(1) = proj_y_axis;
    orientation_out_mat.col(2) = normal;
    orientation_out = exponential_map::RotationToExponentialMap(orientation_out_mat);

}

void GroundManager::getNearestMeshZPosition(const Eigen::Vector3d& position_in, Eigen::Vector3d& position_out, Eigen::Vector3d& normal, double current_min_distance) const
{
    for (int i = 0; i < triangles_.size(); ++i)
    {
        const Triangle& triangle = triangles_[i];
        const Plane& plane = planes_[triangle.plane_index_];

        Eigen::Vector3d projection;
        if (plane.projectionZ(position_in, projection) == false)
            continue;

        projection = ProjPoint2Triangle(triangle.points_[0], triangle.points_[1], triangle.points_[2], projection);

        Eigen::Vector3d diff = projection - position_in;
        if (std::abs(diff(0)) > ITOMP_EPS || std::abs(diff(1)) > ITOMP_EPS)
            continue;

        double distance = std::abs(diff(2));

        if (distance < current_min_distance)
        {
            current_min_distance = distance;
            normal = triangle.normal_;
            position_out = projection;
        }
    }
}

void GroundManager::initializeContactSurfaces()
{
	triangles_.clear();

    std::string contact_model = PlanningParameters::getInstance()->getContactModel();
    if (contact_model == "")
        return;

    const std::vector<double>& contact_model_position = PlanningParameters::getInstance()->getContactModelPosition();
    double contact_model_scale = PlanningParameters::getInstance()->getContactModelScale();
    Eigen::Vector3d scale(contact_model_scale, contact_model_scale, contact_model_scale);
    Eigen::Vector3d translation(contact_model_position[0], contact_model_position[1], contact_model_position[2]);

    shapes::Mesh* mesh = shapes::createMeshFromResource(contact_model, scale);
    if (mesh == NULL)
        return;

    for (int k = 0; k < mesh->triangle_count; ++k)
    {
        int triangle_vertex1 = mesh->triangles[3 * k];
        int triangle_vertex2 = mesh->triangles[3 * k + 1];
        int triangle_vertex3 = mesh->triangles[3 * k + 2];
        Eigen::Vector3d position1(mesh->vertices[3 * triangle_vertex1], mesh->vertices[3 * triangle_vertex1 + 1], mesh->vertices[3 * triangle_vertex1 + 2]);
        Eigen::Vector3d position2(mesh->vertices[3 * triangle_vertex2], mesh->vertices[3 * triangle_vertex2 + 1], mesh->vertices[3 * triangle_vertex2 + 2]);
        Eigen::Vector3d position3(mesh->vertices[3 * triangle_vertex3], mesh->vertices[3 * triangle_vertex3 + 1], mesh->vertices[3 * triangle_vertex3 + 2]);

        Eigen::Vector3d p0 = (position2 - position1);
        Eigen::Vector3d p1 = (position3 - position1);
        p0.normalize();
        p1.normalize();
        Eigen::Vector3d normal = p0.cross(p1);
        if (normal.norm() < ITOMP_EPS)
            continue;
        normal.normalize();

        Triangle tri;
        tri.points_[0] = position1 + translation;
        tri.points_[1] = position2 + translation;
        tri.points_[2] = position3 + translation;
        tri.normal_ = normal;

        tri.plane_index_ = -1;
        for (int i = 0; i < planes_.size(); ++i)
        {
            Plane& plane = planes_[i];
            if (plane.isTriangleIn(tri))
            {
                plane.triangle_indices_.insert(triangles_.size());
                tri.plane_index_ = i;
            }
        }
        if (tri.plane_index_ == -1)
        {
            tri.plane_index_ = planes_.size();
            planes_.push_back(Plane(tri));
            planes_.back().triangle_indices_.insert(triangles_.size());
        }
        triangles_.push_back(tri);
    }

    NewVizManager::getInstance()->renderContactSurface();
}

}

<|endoftext|>


<commit_before>//===========================================
//  Lumina-DE source code
//  Copyright (c) 2014, Ken Moore
//  Copyright (c) 2014, Antoine Jacoutot <ajacoutot@openbsd.org>
//  Available under the 3-clause BSD license
//  See the LICENSE file for full details
//===========================================
#ifdef __OpenBSD__
#include "LuminaOS.h"
#include <unistd.h>

//can't read xbrightness settings - assume invalid until set
static int screenbrightness = -1;

QString LOS::OSName(){ return "OpenBSD"; }

//OS-specific prefix(s)
QString LOS::AppPrefix(){ return "/usr/local/"; } //Prefix for applications
QString LOS::SysPrefix(){ return "/usr/"; } //Prefix for system

//OS-specific application shortcuts (*.desktop files)
QString LOS::ControlPanelShortcut(){ return ""; } //system control panel
QString LOS::AppStoreShortcut(){ return ""; } //graphical app/pkg manager
QString LOS::QtConfigShortcut(){ return "/usr/local/bin/qtconfig4"; } //qtconfig binary (NOT *.desktop file)

// ==== ExternalDevicePaths() ====
QStringList LOS::ExternalDevicePaths(){
    //Returns: QStringList[<type>::::<filesystem>::::<path>]
      //Note: <type> = [USB, HDRIVE, DVD, SDCARD, UNKNOWN]
  QStringList devs = LUtils::getCmdOutput("mount");
  //Now check the output
  for(int i=0; i<devs.length(); i++){
      QString type = devs[i].section(" ",0,0);
      type.remove("/dev/");
      //Determine the type of hardware device based on the dev node
      if(type.startsWith("sd")||type.startsWith("wd")){ type = "HDRIVE"; }
      else if(type.startsWith("cd")){ type="DVD"; }
      else{ type = "UNKNOWN"; }
      //Now put the device in the proper output format
      QString fs = devs[i].section(" ", 4, 4);
      QString path = devs[i].section(" ",2, 2);
      if (!fs.isEmpty() ) {   //we not found a filesystem, most probably this is an invalid row
          devs[i] = type+"::::"+fs+"::::"+path;
      }
      else {
          devs.removeAt(i);
          i--;
      }
  }
  return devs;
}

//Read screen brightness information
int LOS::ScreenBrightness(){
  //Returns: Screen Brightness as a percentage (0-100, with -1 for errors)
  //Make sure we are not running in a VM (does not work)
  QStringList info = LUtils::getCmdOutput("sysctl -n hw.product");
  if( !info.filter(QRegExp("VirtualBox|KVM")).isEmpty() ){ return -1; }
  //Now perform the standard brightness checks
  if(screenbrightness==-1){
    if(QFile::exists(QDir::homePath()+"/.lumina/.currentxbrightness")){
      int val = LUtils::readFile(QDir::homePath()+"/.lumina/.currentxbrightness").join("").simplified().toInt();
      screenbrightness = val;
    }
  }
  return screenbrightness;
}

//Set screen brightness
void LOS::setScreenBrightness(int percent){
  //ensure bounds
  if(percent<0){percent=0;}
  else if(percent>100){ percent=100; }
  //Run the command
  QString cmd = "xbacklight -time 0 -steps 1 -set %1";
  cmd = cmd.arg( QString::number(percent) );
  int ret = LUtils::runCmd(cmd);
  //Save the result for later
  if(ret!=0){ screenbrightness = -1; }
  else{ screenbrightness = percent; }
  LUtils::writeFile(QDir::homePath()+"/.lumina/.currentxbrightness", QStringList() << QString::number(screenbrightness), true);
}

//Read the current volume
int LOS::audioVolume(){ //Returns: audio volume as a percentage (0-100, with -1 for errors)
  QString info = LUtils::getCmdOutput("mixerctl -n outputs.master").join(",").simplified(); //ignores any other lines
  int out = -1;
  if(!info.isEmpty()){
    int L = info.section(",",0,0).toInt();
    int R = info.section(",",1,1).toInt();
    L = (L*100)/255; //percent
    R = (R*100)/255; //percent
    if(L>R){ out = L; }
    else{ out = R; }
  }
  return out;
}

//Set the current volume
void LOS::setAudioVolume(int percent){
  if(percent<0){percent=0;}
  else if(percent>100){percent=100;}
  QString info = LUtils::getCmdOutput("mixerctl -n outputs.master").join(",").simplified(); //ignores any other lines
  if(!info.isEmpty()){
    int L = info.section(",",0,0).toInt();
    int R = info.section(",",1,1).toInt();
    L = (L*100)/255; //percent
    R = (R*100)/255; //percent
    int diff = L-R;
    if(diff<0){ R=percent; L=percent+diff; } //R Greater
    else{ L=percent; R=percent-diff; } //L Greater or equal
    //Check bounds
    if(L<0){L=0;}else if(L>100){L=100;}
    if(R<0){R=0;}else if(R>100){R=100;}
    //Run Command
    L = (L*255)/100; //0-255
    R = (R*255)/100; //0-255
    LUtils::runCmd("mixerctl -q outputs.master="+QString::number(L)+","+QString::number(R));
  }
}

//Change the current volume a set amount (+ or -)
void LOS::changeAudioVolume(int percentdiff){
  QString info = LUtils::getCmdOutput("mixerctl -n outputs.master").join(",").simplified(); //ignores any other lines
  if(!info.isEmpty()){
    int L = info.section(",",0,0).toInt();
    int R = info.section(",",1,1).toInt();
    L = (L*100)/255; //percent
    R = (R*100)/255; //percent
    L = L + percentdiff;
    R = R + percentdiff;
    //Check bounds
    if(L<0){L=0;}else if(L>100){L=100;}
    if(R<0){R=0;}else if(R>100){R=100;}
    //Run Command
    L = (L*255)/100; //0-255
    R = (R*255)/100; //0-255
    LUtils::runCmd("mixerctl -q outputs.master="+QString::number(L)+","+QString::number(R));
  }
}

//Check if a graphical audio mixer is installed
bool LOS::hasMixerUtility(){
  return false; //not implemented yet for OpenBSD
}

//Launch the graphical audio mixer utility
void LOS::startMixerUtility(){
  //Not implemented yet for OpenBSD
}

//Check for user system permission (shutdown/restart)
bool LOS::userHasShutdownAccess(){
  //User needs to be a part of the operator group to be able to run the shutdown command
  QStringList groups = LUtils::getCmdOutput("id -Gn").join(" ").split(" ");
  return groups.contains("operator");
}

//System Shutdown
void LOS::systemShutdown(){ //start poweroff sequence
  QProcess::startDetached("shutdown -p now");
}

//System Restart
void LOS::systemRestart(){ //start reboot sequence
  QProcess::startDetached("shutdown -r now");
}

//Battery Availability
bool LOS::hasBattery(){
  int val = LUtils::getCmdOutput("apm -b").join("").toInt();
  return (val < 4);
}

//Battery Charge Level
int LOS::batteryCharge(){ //Returns: percent charge (0-100), anything outside that range is counted as an error
  int charge = LUtils::getCmdOutput("apm -l").join("").toInt();
  if(charge > 100){ charge = -1; } //invalid charge
  return charge;
}

//Battery Charging State
bool LOS::batteryIsCharging(){
  return (LUtils::getCmdOutput("apm -a").join("").simplified() == "1");
}

//Battery Time Remaining
int LOS::batterySecondsLeft(){ //Returns: estimated number of seconds remaining
  int min = LUtils::getCmdOutput("apm -m").join("").toInt();
  return (min * 60);
}

//File Checksums
QStringList LOS::Checksums(QStringList filepaths){ //Return: checksum of the input file
 return QStringList();
}
#endif
<commit_msg>add checksum for OpenBSD (this is in fact a copy of the code from freeBSD)<commit_after>//===========================================
//  Lumina-DE source code
//  Copyright (c) 2014, Ken Moore
//  Copyright (c) 2014, Antoine Jacoutot <ajacoutot@openbsd.org>
//  Available under the 3-clause BSD license
//  See the LICENSE file for full details
//===========================================
#ifdef __OpenBSD__
#include "LuminaOS.h"
#include <unistd.h>

//can't read xbrightness settings - assume invalid until set
static int screenbrightness = -1;

QString LOS::OSName(){ return "OpenBSD"; }

//OS-specific prefix(s)
QString LOS::AppPrefix(){ return "/usr/local/"; } //Prefix for applications
QString LOS::SysPrefix(){ return "/usr/"; } //Prefix for system

//OS-specific application shortcuts (*.desktop files)
QString LOS::ControlPanelShortcut(){ return ""; } //system control panel
QString LOS::AppStoreShortcut(){ return ""; } //graphical app/pkg manager
QString LOS::QtConfigShortcut(){ return "/usr/local/bin/qtconfig4"; } //qtconfig binary (NOT *.desktop file)

// ==== ExternalDevicePaths() ====
QStringList LOS::ExternalDevicePaths(){
    //Returns: QStringList[<type>::::<filesystem>::::<path>]
      //Note: <type> = [USB, HDRIVE, DVD, SDCARD, UNKNOWN]
  QStringList devs = LUtils::getCmdOutput("mount");
  //Now check the output
  for(int i=0; i<devs.length(); i++){
      QString type = devs[i].section(" ",0,0);
      type.remove("/dev/");
      //Determine the type of hardware device based on the dev node
      if(type.startsWith("sd")||type.startsWith("wd")){ type = "HDRIVE"; }
      else if(type.startsWith("cd")){ type="DVD"; }
      else{ type = "UNKNOWN"; }
      //Now put the device in the proper output format
      QString fs = devs[i].section(" ", 4, 4);
      QString path = devs[i].section(" ",2, 2);
      if (!fs.isEmpty() ) {   //we not found a filesystem, most probably this is an invalid row
          devs[i] = type+"::::"+fs+"::::"+path;
      }
      else {
          devs.removeAt(i);
          i--;
      }
  }
  return devs;
}

//Read screen brightness information
int LOS::ScreenBrightness(){
  //Returns: Screen Brightness as a percentage (0-100, with -1 for errors)
  //Make sure we are not running in a VM (does not work)
  QStringList info = LUtils::getCmdOutput("sysctl -n hw.product");
  if( !info.filter(QRegExp("VirtualBox|KVM")).isEmpty() ){ return -1; }
  //Now perform the standard brightness checks
  if(screenbrightness==-1){
    if(QFile::exists(QDir::homePath()+"/.lumina/.currentxbrightness")){
      int val = LUtils::readFile(QDir::homePath()+"/.lumina/.currentxbrightness").join("").simplified().toInt();
      screenbrightness = val;
    }
  }
  return screenbrightness;
}

//Set screen brightness
void LOS::setScreenBrightness(int percent){
  //ensure bounds
  if(percent<0){percent=0;}
  else if(percent>100){ percent=100; }
  //Run the command
  QString cmd = "xbacklight -time 0 -steps 1 -set %1";
  cmd = cmd.arg( QString::number(percent) );
  int ret = LUtils::runCmd(cmd);
  //Save the result for later
  if(ret!=0){ screenbrightness = -1; }
  else{ screenbrightness = percent; }
  LUtils::writeFile(QDir::homePath()+"/.lumina/.currentxbrightness", QStringList() << QString::number(screenbrightness), true);
}

//Read the current volume
int LOS::audioVolume(){ //Returns: audio volume as a percentage (0-100, with -1 for errors)
  QString info = LUtils::getCmdOutput("mixerctl -n outputs.master").join(",").simplified(); //ignores any other lines
  int out = -1;
  if(!info.isEmpty()){
    int L = info.section(",",0,0).toInt();
    int R = info.section(",",1,1).toInt();
    L = (L*100)/255; //percent
    R = (R*100)/255; //percent
    if(L>R){ out = L; }
    else{ out = R; }
  }
  return out;
}

//Set the current volume
void LOS::setAudioVolume(int percent){
  if(percent<0){percent=0;}
  else if(percent>100){percent=100;}
  QString info = LUtils::getCmdOutput("mixerctl -n outputs.master").join(",").simplified(); //ignores any other lines
  if(!info.isEmpty()){
    int L = info.section(",",0,0).toInt();
    int R = info.section(",",1,1).toInt();
    L = (L*100)/255; //percent
    R = (R*100)/255; //percent
    int diff = L-R;
    if(diff<0){ R=percent; L=percent+diff; } //R Greater
    else{ L=percent; R=percent-diff; } //L Greater or equal
    //Check bounds
    if(L<0){L=0;}else if(L>100){L=100;}
    if(R<0){R=0;}else if(R>100){R=100;}
    //Run Command
    L = (L*255)/100; //0-255
    R = (R*255)/100; //0-255
    LUtils::runCmd("mixerctl -q outputs.master="+QString::number(L)+","+QString::number(R));
  }
}

//Change the current volume a set amount (+ or -)
void LOS::changeAudioVolume(int percentdiff){
  QString info = LUtils::getCmdOutput("mixerctl -n outputs.master").join(",").simplified(); //ignores any other lines
  if(!info.isEmpty()){
    int L = info.section(",",0,0).toInt();
    int R = info.section(",",1,1).toInt();
    L = (L*100)/255; //percent
    R = (R*100)/255; //percent
    L = L + percentdiff;
    R = R + percentdiff;
    //Check bounds
    if(L<0){L=0;}else if(L>100){L=100;}
    if(R<0){R=0;}else if(R>100){R=100;}
    //Run Command
    L = (L*255)/100; //0-255
    R = (R*255)/100; //0-255
    LUtils::runCmd("mixerctl -q outputs.master="+QString::number(L)+","+QString::number(R));
  }
}

//Check if a graphical audio mixer is installed
bool LOS::hasMixerUtility(){
  return false; //not implemented yet for OpenBSD
}

//Launch the graphical audio mixer utility
void LOS::startMixerUtility(){
  //Not implemented yet for OpenBSD
}

//Check for user system permission (shutdown/restart)
bool LOS::userHasShutdownAccess(){
  //User needs to be a part of the operator group to be able to run the shutdown command
  QStringList groups = LUtils::getCmdOutput("id -Gn").join(" ").split(" ");
  return groups.contains("operator");
}

//System Shutdown
void LOS::systemShutdown(){ //start poweroff sequence
  QProcess::startDetached("shutdown -p now");
}

//System Restart
void LOS::systemRestart(){ //start reboot sequence
  QProcess::startDetached("shutdown -r now");
}

//Battery Availability
bool LOS::hasBattery(){
  int val = LUtils::getCmdOutput("apm -b").join("").toInt();
  return (val < 4);
}

//Battery Charge Level
int LOS::batteryCharge(){ //Returns: percent charge (0-100), anything outside that range is counted as an error
  int charge = LUtils::getCmdOutput("apm -l").join("").toInt();
  if(charge > 100){ charge = -1; } //invalid charge
  return charge;
}

//Battery Charging State
bool LOS::batteryIsCharging(){
  return (LUtils::getCmdOutput("apm -a").join("").simplified() == "1");
}

//Battery Time Remaining
int LOS::batterySecondsLeft(){ //Returns: estimated number of seconds remaining
  int min = LUtils::getCmdOutput("apm -m").join("").toInt();
  return (min * 60);
}

//File Checksums
QStringList LOS::Checksums(QStringList filepaths){ //Return: checksum of the input file
  //on OpenBSD md5 has the following layout
  //>md5 LuminaThemes.o LuminaUtils.o
  //MD5 (LuminaThemes.o) = 50006505d9d7e54e5154eeb095555055
  //MD5 (LuminaUtils.o) = d490878ee8866e55e5af571b98b4d448

  QStringList info = LUtils::getCmdOutput("md5 \""+filepaths.join("\" \"")+"\"");
  for(int i=0; i<info.length(); i++){
    if( !info[i].contains(" = ") ){ info.removeAt(i); i--; }
    else{
      //Strip out the extra information
      info[i] = info[i].section(" = ",1,1);
    }
  }
 return info;
}
#endif
<|endoftext|>


<commit_before>/*******************************************************************************
* Copyright 2016 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/

/* Authors: Taehoon Lim (Darby) */

#include <ros/ros.h>
#include <diagnostic_msgs/DiagnosticArray.h>
#include <sensor_msgs/BatteryState.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/MagneticField.h>
#include <sensor_msgs/LaserScan.h>
#include <turtlebot3_msgs/SensorState.h>
#include <turtlebot3_msgs/VersionInfo.h>

#define SOFTWARE_VERSION "1.0.0"
#define FIRMWARE_VERSION "1.0.16"
#define HARDWARE_VERSION "1.0.0"

ros::Publisher tb3_diagnostics_pub;
diagnostic_msgs::DiagnosticArray tb3_diagnostics;

diagnostic_msgs::DiagnosticStatus imu_state;
diagnostic_msgs::DiagnosticStatus motor_state;
diagnostic_msgs::DiagnosticStatus LDS_state;
diagnostic_msgs::DiagnosticStatus battery_state;
diagnostic_msgs::DiagnosticStatus button_state;

void setDiagnosisMsg(diagnostic_msgs::DiagnosticStatus *diag, uint8_t level, std::string name, std::string message, std::string hardware_id)
{
  diag->level = level;
  diag->name  = name;
  diag->message = message;
  diag->hardware_id = hardware_id;
}

void setIMUDiagnosis(uint8_t level, std::string message)
{
  setDiagnosisMsg(&imu_state, level, "IMU Sensor", message, "MPU9250");
}

void setMotorDiagnosis(uint8_t level, std::string message)
{
  setDiagnosisMsg(&motor_state, level, "Actuator", message, "DYNAMIXEL X");
}

void setBatteryDiagnosis(uint8_t level, std::string message)
{
  setDiagnosisMsg(&battery_state, level, "Power System", message, "Battery");
}

void setLDSDiagnosis(uint8_t level, std::string message)
{
  setDiagnosisMsg(&LDS_state, level, "Lidar Sensor", message, "HLS-LFCD-LDS");
}

void setButtonDiagnosis(uint8_t level, std::string message)
{
  setDiagnosisMsg(&button_state, level, "Analog Button", message, "OpenCR Button");
}

void imuMsgCallback(const sensor_msgs::Imu::ConstPtr &msg)
{
  setIMUDiagnosis(diagnostic_msgs::DiagnosticStatus::OK, "Good Condition");
}

void LDSMsgCallback(const sensor_msgs::LaserScan::ConstPtr &msg)
{
  setLDSDiagnosis(diagnostic_msgs::DiagnosticStatus::OK, "Good Condition");
}

void sensorStateMsgCallback(const turtlebot3_msgs::SensorState::ConstPtr &msg)
{
  if (msg->battery > 11.0)
    setBatteryDiagnosis(diagnostic_msgs::DiagnosticStatus::OK, "Good Condition");
  else
    setBatteryDiagnosis(diagnostic_msgs::DiagnosticStatus::WARN, "Charge!!! Charge!!!");

  if (msg->button == turtlebot3_msgs::SensorState::BUTTON0)
    setButtonDiagnosis(diagnostic_msgs::DiagnosticStatus::OK, "BUTTON 0 IS PUSHED");
  else if (msg->button == turtlebot3_msgs::SensorState::BUTTON1)
    setButtonDiagnosis(diagnostic_msgs::DiagnosticStatus::OK, "BUTTON 1 IS PUSHED");
  else
    setButtonDiagnosis(diagnostic_msgs::DiagnosticStatus::OK, "Pushed Nothing");

  if (msg->torque == true)
    setMotorDiagnosis(diagnostic_msgs::DiagnosticStatus::OK, "Torque ON");
  else
    setMotorDiagnosis(diagnostic_msgs::DiagnosticStatus::WARN, "Torque OFF");
}

void versionMsgCallback(const turtlebot3_msgs::VersionInfo::ConstPtr &msg)
{

  if (std::string(msg->software) != std::string(SOFTWARE_VERSION))
    ROS_WARN("Check turtlebot3 repository and Update your software!!");

  if (std::string(msg->hardware) != std::string(HARDWARE_VERSION))
    ROS_WARN("Check turtlebot3 wiki page and Update your hardware!!");

  if (std::string(msg->firmware) != std::string(FIRMWARE_VERSION))
    ROS_WARN("Check OpenCR update and change your firmware!!");
}

void msgPub()
{
  tb3_diagnostics.header.stamp = ros::Time::now();

  tb3_diagnostics.status.clear();
  tb3_diagnostics.status.push_back(imu_state);
  tb3_diagnostics.status.push_back(motor_state);
  tb3_diagnostics.status.push_back(LDS_state);
  tb3_diagnostics.status.push_back(battery_state);
  tb3_diagnostics.status.push_back(button_state);

  tb3_diagnostics_pub.publish(tb3_diagnostics);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "turtlebot3_diagnostic");
  ros::NodeHandle nh;

  tb3_diagnostics_pub  = nh.advertise<diagnostic_msgs::DiagnosticArray>("diagnostics", 10);

  ros::Subscriber imu         = nh.subscribe("/imu", 10, imuMsgCallback);
  ros::Subscriber lds         = nh.subscribe("/scan", 10, LDSMsgCallback);
  ros::Subscriber tb3_sensor  = nh.subscribe("/sensor_state", 10, sensorStateMsgCallback);
  ros::Subscriber version     = nh.subscribe("/version_info", 10, versionMsgCallback);

  ros::Rate loop_rate(1);

  while (ros::ok())
  {
    msgPub();
    ros::spinOnce();
    loop_rate.sleep();
  }

  return 0;
}
<commit_msg>update firmware version<commit_after>/*******************************************************************************
* Copyright 2016 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/

/* Authors: Taehoon Lim (Darby) */

#include <ros/ros.h>
#include <diagnostic_msgs/DiagnosticArray.h>
#include <sensor_msgs/BatteryState.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/MagneticField.h>
#include <sensor_msgs/LaserScan.h>
#include <turtlebot3_msgs/SensorState.h>
#include <turtlebot3_msgs/VersionInfo.h>

#define SOFTWARE_VERSION "1.0.0"
#define FIRMWARE_VERSION "1.0.18"
#define HARDWARE_VERSION "1.0.0"

ros::Publisher tb3_diagnostics_pub;
diagnostic_msgs::DiagnosticArray tb3_diagnostics;

diagnostic_msgs::DiagnosticStatus imu_state;
diagnostic_msgs::DiagnosticStatus motor_state;
diagnostic_msgs::DiagnosticStatus LDS_state;
diagnostic_msgs::DiagnosticStatus battery_state;
diagnostic_msgs::DiagnosticStatus button_state;

void setDiagnosisMsg(diagnostic_msgs::DiagnosticStatus *diag, uint8_t level, std::string name, std::string message, std::string hardware_id)
{
  diag->level = level;
  diag->name  = name;
  diag->message = message;
  diag->hardware_id = hardware_id;
}

void setIMUDiagnosis(uint8_t level, std::string message)
{
  setDiagnosisMsg(&imu_state, level, "IMU Sensor", message, "MPU9250");
}

void setMotorDiagnosis(uint8_t level, std::string message)
{
  setDiagnosisMsg(&motor_state, level, "Actuator", message, "DYNAMIXEL X");
}

void setBatteryDiagnosis(uint8_t level, std::string message)
{
  setDiagnosisMsg(&battery_state, level, "Power System", message, "Battery");
}

void setLDSDiagnosis(uint8_t level, std::string message)
{
  setDiagnosisMsg(&LDS_state, level, "Lidar Sensor", message, "HLS-LFCD-LDS");
}

void setButtonDiagnosis(uint8_t level, std::string message)
{
  setDiagnosisMsg(&button_state, level, "Analog Button", message, "OpenCR Button");
}

void imuMsgCallback(const sensor_msgs::Imu::ConstPtr &msg)
{
  setIMUDiagnosis(diagnostic_msgs::DiagnosticStatus::OK, "Good Condition");
}

void LDSMsgCallback(const sensor_msgs::LaserScan::ConstPtr &msg)
{
  setLDSDiagnosis(diagnostic_msgs::DiagnosticStatus::OK, "Good Condition");
}

void sensorStateMsgCallback(const turtlebot3_msgs::SensorState::ConstPtr &msg)
{
  if (msg->battery > 11.0)
    setBatteryDiagnosis(diagnostic_msgs::DiagnosticStatus::OK, "Good Condition");
  else
    setBatteryDiagnosis(diagnostic_msgs::DiagnosticStatus::WARN, "Charge!!! Charge!!!");

  if (msg->button == turtlebot3_msgs::SensorState::BUTTON0)
    setButtonDiagnosis(diagnostic_msgs::DiagnosticStatus::OK, "BUTTON 0 IS PUSHED");
  else if (msg->button == turtlebot3_msgs::SensorState::BUTTON1)
    setButtonDiagnosis(diagnostic_msgs::DiagnosticStatus::OK, "BUTTON 1 IS PUSHED");
  else
    setButtonDiagnosis(diagnostic_msgs::DiagnosticStatus::OK, "Pushed Nothing");

  if (msg->torque == true)
    setMotorDiagnosis(diagnostic_msgs::DiagnosticStatus::OK, "Torque ON");
  else
    setMotorDiagnosis(diagnostic_msgs::DiagnosticStatus::WARN, "Torque OFF");
}

void versionMsgCallback(const turtlebot3_msgs::VersionInfo::ConstPtr &msg)
{

  if (std::string(msg->software) != std::string(SOFTWARE_VERSION))
    ROS_WARN("Check turtlebot3 repository and Update your software!!");

  if (std::string(msg->hardware) != std::string(HARDWARE_VERSION))
    ROS_WARN("Check turtlebot3 wiki page and Update your hardware!!");

  if (std::string(msg->firmware) != std::string(FIRMWARE_VERSION))
    ROS_WARN("Check OpenCR update and change your firmware!!");
}

void msgPub()
{
  tb3_diagnostics.header.stamp = ros::Time::now();

  tb3_diagnostics.status.clear();
  tb3_diagnostics.status.push_back(imu_state);
  tb3_diagnostics.status.push_back(motor_state);
  tb3_diagnostics.status.push_back(LDS_state);
  tb3_diagnostics.status.push_back(battery_state);
  tb3_diagnostics.status.push_back(button_state);

  tb3_diagnostics_pub.publish(tb3_diagnostics);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "turtlebot3_diagnostic");
  ros::NodeHandle nh;

  tb3_diagnostics_pub  = nh.advertise<diagnostic_msgs::DiagnosticArray>("diagnostics", 10);

  ros::Subscriber imu         = nh.subscribe("/imu", 10, imuMsgCallback);
  ros::Subscriber lds         = nh.subscribe("/scan", 10, LDSMsgCallback);
  ros::Subscriber tb3_sensor  = nh.subscribe("/sensor_state", 10, sensorStateMsgCallback);
  ros::Subscriber version     = nh.subscribe("/version_info", 10, versionMsgCallback);

  ros::Rate loop_rate(1);

  while (ros::ok())
  {
    msgPub();
    ros::spinOnce();
    loop_rate.sleep();
  }

  return 0;
}
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2015, Intel Corporation
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in
 *       the documentation and/or other materials provided with the
 *       distribution.
 *
 *     * Neither the name of Intel Corporation nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY LOG OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <iostream>

#include "gtest/gtest.h"
#include "gmock/gmock.h"
#include "geopm_error.h"
#include "Exception.hpp"
#include "RAPLPlatform.hpp"
#include "MockPlatformImp.hpp"
#include "MockPlatformTopology.hpp"

using ::testing::Return;
using ::testing::Pointee;
using ::testing::_;
using ::testing::SetArgReferee;

class PlatformTest: public :: testing :: Test
{
    protected:
        void SetUp();
        void TearDown();
        geopm::Platform *platform;
        MockPlatformImp *platformimp;
        MockPlatformTopology *topo;
        std::vector<hwloc_obj_t> package;
        std::vector<hwloc_obj_t> package1_cpu;
        std::vector<hwloc_obj_t> package2_cpu;

};

void PlatformTest::SetUp()
{

    platform = new geopm::RAPLPlatform();
    platformimp = new MockPlatformImp();
    topo = new MockPlatformTopology();

    EXPECT_CALL(*platformimp, initialize());

    EXPECT_CALL(*platformimp, num_logical_cpu())
    .WillRepeatedly(Return(8));

    EXPECT_CALL(*platformimp, num_package())
    .WillRepeatedly(testing::Return(2));

    EXPECT_CALL(*platformimp, num_hw_cpu())
    .WillRepeatedly(testing::Return(8));

    EXPECT_CALL(*platformimp, num_package_signal())
    .WillRepeatedly(testing::Return(3));

    EXPECT_CALL(*platformimp, num_cpu_signal())
    .WillRepeatedly(testing::Return(5));

    platform->set_implementation((geopm::PlatformImp*)platformimp);
    hwloc_obj_t obj;

    for (int i = 0; i < 2; i++) {
        obj = (hwloc_obj_t)malloc(sizeof(struct hwloc_obj));
        obj->type = (hwloc_obj_type_t)(geopm::GEOPM_DOMAIN_PACKAGE);
        obj->os_index = i;
        obj->logical_index = i;
        package.push_back(obj);
    }
    for (int i = 0; i < 4; i++) {
        obj = (hwloc_obj_t)malloc(sizeof(struct hwloc_obj));
        obj->type = (hwloc_obj_type_t)(geopm::GEOPM_DOMAIN_CPU);
        obj->os_index = i;
        obj->logical_index = i;
        package1_cpu.push_back(obj);
    }
    for (int i = 4; i < 8; i++) {
        obj = (hwloc_obj_t)malloc(sizeof(struct hwloc_obj));
        obj->type = (hwloc_obj_type_t)(geopm::GEOPM_DOMAIN_CPU);
        obj->os_index = i;
        obj->logical_index = i;
        package2_cpu.push_back(obj);
    }
}

void PlatformTest::TearDown()
{
    delete topo;
    delete platformimp;
    delete platform;
}

MATCHER_P(Socket, x, "") {
    return (arg->logical_index == (unsigned)x);
}

TEST_F(PlatformTest, transform_init)
{
    std::vector<int> cpu_ranks({0, 0, 1, 1, 2, 2, 3, 3});
    std::vector<double> result(2 * GEOPM_NUM_TELEMETRY_TYPE);
    std::vector<double> expect({1, 1, 1, 4, 4, 4, 4, 4, 2, 2, 1, 1, 1, 4, 4, 4, 4, 4, 2, 2});

    EXPECT_EQ(expect.size(), result.size());

    EXPECT_CALL(*platformimp, topology())
    .WillOnce(testing::Return(topo));

    EXPECT_CALL(*platformimp, power_control_domain())
    .WillOnce(Return(geopm::GEOPM_DOMAIN_PACKAGE));

    EXPECT_CALL(*topo, num_domain(_))
    .WillOnce(Return(2));

    EXPECT_CALL(*topo, domain_by_type(_,_))
    .WillRepeatedly(SetArgReferee<1>(package));

    EXPECT_CALL(*topo, children_by_type(_,Socket(0),_))
    .WillRepeatedly(SetArgReferee<2>(package1_cpu));

    EXPECT_CALL(*topo, children_by_type(_,Socket(1),_))
    .WillRepeatedly(SetArgReferee<2>(package2_cpu));

    platform->init_transform(cpu_ranks);

    const std::vector<double> *transform = platform->signal_domain_transform();

    for (unsigned i = 0; i < 2 * GEOPM_NUM_TELEMETRY_TYPE; ++i) {
        result[i] = 0.0;
        for (unsigned j = 0; j < (platform->capacity() + (4 * 2)); ++j) {
            result[i] += (*transform)[i * (platform->capacity() + (4 * 2)) +j];
        }
    }

    for (unsigned i = 0; i < 2 * GEOPM_NUM_TELEMETRY_TYPE; ++i) {
        EXPECT_DOUBLE_EQ(expect[i], result[i]);
    }
}
<commit_msg>Defined out test code for transform initialization. - The code it is testing has changed.<commit_after>/*
 * Copyright (c) 2015, Intel Corporation
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in
 *       the documentation and/or other materials provided with the
 *       distribution.
 *
 *     * Neither the name of Intel Corporation nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY LOG OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <iostream>

#include "gtest/gtest.h"
#include "gmock/gmock.h"
#include "geopm_error.h"
#include "Exception.hpp"
#include "RAPLPlatform.hpp"
#include "MockPlatformImp.hpp"
#include "MockPlatformTopology.hpp"

using ::testing::Return;
using ::testing::Pointee;
using ::testing::_;
using ::testing::SetArgReferee;

class PlatformTest: public :: testing :: Test
{
    protected:
        void SetUp();
        void TearDown();
        geopm::Platform *platform;
        MockPlatformImp *platformimp;
        MockPlatformTopology *topo;
        std::vector<hwloc_obj_t> package;
        std::vector<hwloc_obj_t> package1_cpu;
        std::vector<hwloc_obj_t> package2_cpu;

};

void PlatformTest::SetUp()
{

    platform = new geopm::RAPLPlatform();
    platformimp = new MockPlatformImp();
    topo = new MockPlatformTopology();

    EXPECT_CALL(*platformimp, initialize());

    EXPECT_CALL(*platformimp, num_logical_cpu())
    .WillRepeatedly(Return(8));

    EXPECT_CALL(*platformimp, num_package())
    .WillRepeatedly(testing::Return(2));

    EXPECT_CALL(*platformimp, num_hw_cpu())
    .WillRepeatedly(testing::Return(8));

    EXPECT_CALL(*platformimp, num_package_signal())
    .WillRepeatedly(testing::Return(3));

    EXPECT_CALL(*platformimp, num_cpu_signal())
    .WillRepeatedly(testing::Return(5));

    platform->set_implementation((geopm::PlatformImp*)platformimp);
    hwloc_obj_t obj;

    for (int i = 0; i < 2; i++) {
        obj = (hwloc_obj_t)malloc(sizeof(struct hwloc_obj));
        obj->type = (hwloc_obj_type_t)(geopm::GEOPM_DOMAIN_PACKAGE);
        obj->os_index = i;
        obj->logical_index = i;
        package.push_back(obj);
    }
    for (int i = 0; i < 4; i++) {
        obj = (hwloc_obj_t)malloc(sizeof(struct hwloc_obj));
        obj->type = (hwloc_obj_type_t)(geopm::GEOPM_DOMAIN_CPU);
        obj->os_index = i;
        obj->logical_index = i;
        package1_cpu.push_back(obj);
    }
    for (int i = 4; i < 8; i++) {
        obj = (hwloc_obj_t)malloc(sizeof(struct hwloc_obj));
        obj->type = (hwloc_obj_type_t)(geopm::GEOPM_DOMAIN_CPU);
        obj->os_index = i;
        obj->logical_index = i;
        package2_cpu.push_back(obj);
    }
}

void PlatformTest::TearDown()
{
    delete topo;
    delete platformimp;
    delete platform;
}

MATCHER_P(Socket, x, "") {
    return (arg->logical_index == (unsigned)x);
}

TEST_F(PlatformTest, transform_init)
{
    std::vector<int> cpu_ranks({0, 0, 1, 1, 2, 2, 3, 3});
    std::vector<double> result(2 * GEOPM_NUM_TELEMETRY_TYPE);
    std::vector<double> expect({1, 1, 1, 4, 4, 4, 4, 4, 2, 2, 1, 1, 1, 4, 4, 4, 4, 4, 2, 2});

    EXPECT_EQ(expect.size(), result.size());

    EXPECT_CALL(*platformimp, topology())
    .WillOnce(testing::Return(topo));

    EXPECT_CALL(*platformimp, power_control_domain())
    .WillOnce(Return(geopm::GEOPM_DOMAIN_PACKAGE));

    EXPECT_CALL(*topo, num_domain(_))
    .WillOnce(Return(2));

    EXPECT_CALL(*topo, domain_by_type(_,_))
    .WillRepeatedly(SetArgReferee<1>(package));

    EXPECT_CALL(*topo, children_by_type(_,Socket(0),_))
    .WillRepeatedly(SetArgReferee<2>(package1_cpu));

    EXPECT_CALL(*topo, children_by_type(_,Socket(1),_))
    .WillRepeatedly(SetArgReferee<2>(package2_cpu));

    platform->init_transform(cpu_ranks);

#if 0 // This needs to be changed to match new code
    const std::vector<double> *transform = platform->signal_domain_transform();

    for (unsigned i = 0; i < 2 * GEOPM_NUM_TELEMETRY_TYPE; ++i) {
        result[i] = 0.0;
        for (unsigned j = 0; j < (platform->capacity() + (4 * 2)); ++j) {
            result[i] += (*transform)[i * (platform->capacity() + (4 * 2)) +j];
        }
    }

    for (unsigned i = 0; i < 2 * GEOPM_NUM_TELEMETRY_TYPE; ++i) {
        EXPECT_DOUBLE_EQ(expect[i], result[i]);
    }
#endif
}
<|endoftext|>


<commit_before>#ifndef FIND_TWO_ELEMENTS_HPP_
#define FIND_TWO_ELEMENTS_HPP_

// http://stackoverflow.com/questions/4720271/find-a-pair-of-elements-from-an-array-whose-sum-equals-a-given-number
// Given array of n integers and given a number X, find all the unique pairs of
// elements (a,b), whose summation is equal to X.

#include <vector>
#include <utility>
#include <algorithm>
#include <unordered_map>

#include "algorithms/search/binary_search.hpp"

std::vector<std::pair<int, int>> RemoveDuplicates(
        const std::vector<std::pair<int, int>>& data);

// Naive approach: double check all combinations
// Time complexity: O(n^2) - because we have two nested loops
std::vector<std::pair<int, int>> FindTwoElementsNaive(
        const std::vector<int>& data, const int& value)
{
    std::vector<std::pair<int, int>> result;
    for (size_t i = 0; i < data.size(); ++i)
    {
        for(size_t j = i; j < data.size(); ++j)
        {
            // Skip same element
            if (i == j)
            {
                continue;
            }

            if ( (data[i] + data[j]) == value )
            {
                result.push_back(std::pair<int, int>(data[i], data[j]));
            }
        }
    }

    return RemoveDuplicates(result);
}

// Slightly better approach - using binary search
// Time complexity: O(n * log n) - one loop and nested binary search
std::vector<std::pair<int, int>> FindTwoElementsBS(
        const std::vector<int>& data, const int& value)
{
    std::vector<int> srtData = data;
    std::sort(srtData.begin(), srtData.end());

    std::vector<std::pair<int, int>> result;
    for (size_t i = 0; i < srtData.size(); ++i)
    {
        size_t j = 0;
        BinarySearch::Solution solution;
        if (solution.Search(srtData, srtData.size(), value - srtData[i], j) &&
            i != j)
        {
            result.push_back(std::pair<int, int>(srtData[i], srtData[j]));
        }
    }

    return RemoveDuplicates(result);
}

// Optimal solution - using hash table
// Time complexity: O(n) - one loop, access to hash element O(1)
std::vector<std::pair<int, int>> FindTwoElementsHash(
        const std::vector<int>& data, const int& value)
{
    std::unordered_map<int, size_t> umap;
    std::vector<std::pair<int, int>> result;
    for (size_t i = 0; i < data.size(); ++i)
    {
        if (0 < umap.count(value - data[i]))
        {
            size_t j = umap[value - data[i]];
            result.push_back(std::pair<int, int>(data[i], data[j]));
        }
        else
        {
            umap[data[i]] = i;
        }
    }

    return RemoveDuplicates(result);
}

std::vector<std::pair<int, int>> RemoveDuplicates(
        const std::vector<std::pair<int, int>>& data)
{
    std::vector<std::pair<int, int>> result;
    for (size_t i = 0; i < data.size(); ++i)
    {
        std::pair<int, int> orig = data[i];
        std::pair<int, int> inv = {data[i].second, data[i].first};
        auto func = [&orig, &inv](const std::pair<int, int>& p)
        {
            return p == orig || p == inv;
        };

        if (!std::any_of(result.begin(), result.end(), func))
        {
            result.push_back(data[i]);
        }
    }

    return result;
}

#endif /* FIND_TWO_ELEMENTS_HPP_ */
<commit_msg>Add algorithm: find two integers in array with special sum<commit_after>#ifndef FIND_TWO_ELEMENTS_HPP_
#define FIND_TWO_ELEMENTS_HPP_

// http://stackoverflow.com/questions/4720271/find-a-pair-of-elements-from-an-array-whose-sum-equals-a-given-number
// Given array of n integers and given a number X, find all the unique pairs of
// elements (a,b), whose summation is equal to X.

#include <vector>
#include <utility>
#include <algorithm>
#include <unordered_map>

#include "algorithms/search/binary_search.hpp"

std::vector<std::pair<int, int>> RemoveDuplicates(
        const std::vector<std::pair<int, int>>& data);

// Naive approach: double check all combinations
// Time complexity: O(n^2) - because we have two nested loops
std::vector<std::pair<int, int>> FindTwoElementsNaive(
        const std::vector<int>& data, const int& value)
{
    std::vector<std::pair<int, int>> result;
    for (size_t i = 0; i < data.size(); ++i)
    {
        for(size_t j = i; j < data.size(); ++j)
        {
            // Skip same element
            if (i == j)
            {
                continue;
            }

            if ( (data[i] + data[j]) == value )
            {
                result.push_back(std::pair<int, int>(data[i], data[j]));
            }
        }
    }

    return RemoveDuplicates(result);
}

// Slightly better approach - using binary search
// Time complexity: O(n * log n) - one loop and nested binary search
std::vector<std::pair<int, int>> FindTwoElementsBS(
        const std::vector<int>& data, const int& value)
{
    std::vector<int> srtData = data;
    std::sort(srtData.begin(), srtData.end());

    std::vector<std::pair<int, int>> result;
    for (size_t i = 0; i < srtData.size(); ++i)
    {
        size_t j = 0;
        BinarySearch::Solution solution;
        if (solution.Search(srtData, srtData.size(), value - srtData[i], j) &&
            i != j)
        {
            result.push_back(std::pair<int, int>(srtData[i], srtData[j]));
        }
    }

    return RemoveDuplicates(result);
}

// Optimal solution - using hash table
// Time complexity: O(n) - one loop, access to hash element O(1)
std::vector<std::pair<int, int>> FindTwoElementsHash(
        const std::vector<int>& data, const int& value)
{
    std::unordered_map<int, size_t> umap;
    std::vector<std::pair<int, int>> result;
    for (size_t i = 0; i < data.size(); ++i)
    {
        if (0 < umap.count(value - data[i]))
        {
            size_t j = umap[value - data[i]];
            result.push_back(std::pair<int, int>(data[i], data[j]));
        }
        else
        {
            umap[data[i]] = i;
        }
    }

    return RemoveDuplicates(result);
}

std::vector<std::pair<int, int>> RemoveDuplicates(
        const std::vector<std::pair<int, int>>& data)
{
    std::vector<std::pair<int, int>> result;
    for (size_t i = 0; i < data.size(); ++i)
    {
        std::pair<int, int> orig = data[i];
        std::pair<int, int> inv = {data[i].second, data[i].first};
        auto func = [&orig, &inv](const std::pair<int, int>& p)
        {
            return p == orig || p == inv;
        };

        if (!std::any_of(result.begin(), result.end(), func))
        {
            result.push_back(data[i]);
        }
    }

    return result;
}

// https://leetcode.com/problems/two-sum/description/

// Given an array of integers, return indices of the two numbers such that
// they add up to a specific target.
// You may assume that each input would have exactly one solution, and you
// may not use the same element twice.

// Example:
// Given nums = [2, 7, 11, 15], target = 9,
// Because nums[0] + nums[1] = 2 + 7 = 9,
// return [0, 1].

namespace TwoSum {
class Solution {
public:
    std::vector<int> twoSum(std::vector<int>& nums, int target) {
        std::unordered_map<int, size_t> umap;
        for (size_t i = 0; i < nums.size(); ++i)
        {
            if (0 < umap.count(target - nums[i]))
            {
                size_t j = umap[target - nums[i]];

                std::vector<int> result;
                result.push_back(j);
                result.push_back(i);
                return result;
            }
            else
            {
                umap[nums[i]] = i;
            }
        }

        return std::vector<int>();
    }
};
}

#endif /* FIND_TWO_ELEMENTS_HPP_ */
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2006 Apple Computer, Inc.  All rights reserved.
 * Copyright (C) 2006 Justin Haygood <jhaygood@spsu.edu>.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY APPLE COMPUTER, INC. ``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL APPLE COMPUTER, INC. OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
 * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "config.h"
#include "ScrollView.h"

#include <algorithm>
#include "FloatRect.h"
#include "IntRect.h"
#include <windows.h>

using namespace std;

namespace WebCore {

class ScrollView::ScrollViewPrivate {
public:
    ScrollViewPrivate()
        : hasStaticBackground(false)
        , suppressScrollBars(false)
        , vScrollBarMode(ScrollBarAuto)
        , hScrollBarMode(ScrollBarAuto)
    {
    }
    IntSize scrollOffset;
    IntSize contentsSize;
    bool hasStaticBackground;
    bool suppressScrollBars;
    ScrollBarMode vScrollBarMode;
    ScrollBarMode hScrollBarMode;
};

ScrollView::ScrollView()
    : m_data(new ScrollViewPrivate())
{
}

ScrollView::~ScrollView()
{
    delete m_data;
}

void ScrollView::updateContents(const IntRect& updateRect, bool now)
{
    IntRect adjustedDirtyRect(updateRect);
    adjustedDirtyRect.move(-m_data->scrollOffset);

    RECT dirtyRect = RECT(adjustedDirtyRect);
#if PAINT_FLASHING_DEBUG
    HDC dc = GetDC(containingWindow());
    FillRect(dc, &dirtyRect, (HBRUSH)GetStockObject(BLACK_BRUSH));
    ReleaseDC(containingWindow(), dc);
#endif

    InvalidateRect(containingWindow(), &dirtyRect, true);
    if (now)
        UpdateWindow(containingWindow());
}

int ScrollView::visibleWidth() const
{
    RECT bounds;
    GetClientRect(containingWindow(), &bounds);
    return (bounds.right - bounds.left);
}

int ScrollView::visibleHeight() const
{
    RECT bounds;
    GetClientRect(containingWindow(), &bounds);
    return (bounds.bottom - bounds.top);
}

FloatRect ScrollView::visibleContentRect() const
{
    RECT bounds;
    GetClientRect(containingWindow(), &bounds);
    FloatRect contentRect = bounds;
    contentRect.move(m_data->scrollOffset);
    return contentRect;
}

void ScrollView::setContentsPos(int newX, int newY)
{
    int dx = newX - contentsX();
    int dy = newY - contentsY();
    scrollBy(dx, dy);
}

void ScrollView::resizeContents(int w,int h)
{
    IntSize newSize(w,h);
    if (m_data->contentsSize != newSize) {
        m_data->contentsSize = newSize;
        updateScrollBars(m_data->scrollOffset);
    }
}

int ScrollView::contentsX() const
{
    return scrollOffset().width();
}

int ScrollView::contentsY() const
{
    return scrollOffset().height();
}

int ScrollView::contentsWidth() const
{
    return m_data->contentsSize.width();
}

int ScrollView::contentsHeight() const
{
    return m_data->contentsSize.height();
}

IntPoint ScrollView::contentsToWindow(const IntPoint& point) const
{
    return point - scrollOffset();
}

IntPoint ScrollView::windowToContents(const IntPoint& point) const
{
    return point + scrollOffset();
}

IntSize ScrollView::scrollOffset() const
{
    return m_data->scrollOffset;
}

IntSize ScrollView::maximumScroll() const
{
    IntSize delta = m_data->contentsSize - m_data->scrollOffset;
    delta.clampNegativeToZero();
    return delta;
}

void ScrollView::scrollBy(int dx, int dy)
{
    IntSize scrollOffset = m_data->scrollOffset;
    IntSize maxScroll = maximumScroll();
    IntSize newScrollOffset = scrollOffset + IntSize(dx, dy);
    newScrollOffset.clampNegativeToZero();

    if (newScrollOffset != scrollOffset) {
        m_data->scrollOffset = newScrollOffset;
        updateScrollBars(m_data->scrollOffset);
        // ScrollBar updates can fail, so we check the final delta before scrolling
        IntSize scrollDelta = m_data->scrollOffset - scrollOffset;
        if (scrollDelta == IntSize())
            return;
        if (!m_data->hasStaticBackground)
            // FIXME: This could be made more efficient by passing a valid clip rect for only the document content.
            ScrollWindowEx(containingWindow(), -scrollDelta.width(), -scrollDelta.height(), 0, 0, 0, 0, SW_INVALIDATE);
        else
            InvalidateRect(containingWindow(), 0, true);
    }
}

WebCore::ScrollBarMode ScrollView::hScrollBarMode() const
{
    return m_data->hScrollBarMode;
}

WebCore::ScrollBarMode ScrollView::vScrollBarMode() const
{
    return m_data->vScrollBarMode;
}

void ScrollView::suppressScrollBars(bool suppressed, bool repaintOnSuppress)
{
    m_data->suppressScrollBars = suppressed;
    if (repaintOnSuppress)
        updateScrollBars(m_data->scrollOffset);
}

void ScrollView::setHScrollBarMode(ScrollBarMode newMode)
{
    if (m_data->hScrollBarMode != newMode) {
        m_data->hScrollBarMode = newMode;
        updateScrollBars(m_data->scrollOffset);
    }
}

void ScrollView::setVScrollBarMode(ScrollBarMode newMode)
{
    if (m_data->vScrollBarMode != newMode) {
        m_data->vScrollBarMode = newMode;
        updateScrollBars(m_data->scrollOffset);
    }
}

void ScrollView::setScrollBarsMode(ScrollBarMode newMode)
{
    m_data->hScrollBarMode = m_data->vScrollBarMode = newMode;
    updateScrollBars(m_data->scrollOffset);
}

void ScrollView::setStaticBackground(bool flag)
{
    m_data->hasStaticBackground = flag;
}

static int updateScrollInfo(ScrollView* view, short type, int current, int max, int pageSize)
{
    SCROLLINFO si;
    si.cbSize = sizeof(si);
    si.fMask  = SIF_RANGE | SIF_PAGE | SIF_POS;
    si.nMin   = 0;
    si.nMax   = max;
    si.nPage  = pageSize;
    si.nPos   = current;
    SetScrollInfo(view->containingWindow(), type, &si, TRUE);
    GetScrollInfo(view->containingWindow(), type, &si);
    return si.nPos;
}

void ScrollView::updateScrollBars(const IntPoint&)
{
    IntSize maxScrollPosition(contentsWidth(), contentsHeight());
    IntSize scroll = scrollOffset().shrunkTo(maxScrollPosition);
    scroll.clampNegativeToZero();

    m_data->scrollOffset =
        IntSize(updateScrollInfo(this, SB_HORZ, scroll.width(), contentsWidth() - 1, width()),
                updateScrollInfo(this, SB_VERT, scroll.height(), contentsHeight() - 1, height()));

    if (m_data->hScrollBarMode != ScrollBarAuto || m_data->suppressScrollBars)
        ShowScrollBar(containingWindow(), SB_HORZ, (m_data->hScrollBarMode != ScrollBarAlwaysOff) && !m_data->suppressScrollBars);
    if (m_data->vScrollBarMode != ScrollBarAuto || m_data->suppressScrollBars)
        ShowScrollBar(containingWindow(), SB_VERT, (m_data->vScrollBarMode != ScrollBarAlwaysOff) && !m_data->suppressScrollBars);
}

}
<commit_msg>Fix Win32 bustage.<commit_after>/*
 * Copyright (C) 2006 Apple Computer, Inc.  All rights reserved.
 * Copyright (C) 2006 Justin Haygood <jhaygood@spsu.edu>.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY APPLE COMPUTER, INC. ``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL APPLE COMPUTER, INC. OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
 * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "config.h"
#include "ScrollView.h"

#include <algorithm>
#include "FloatRect.h"
#include "IntRect.h"
#include <windows.h>

using namespace std;

namespace WebCore {

class ScrollView::ScrollViewPrivate {
public:
    ScrollViewPrivate()
        : hasStaticBackground(false)
        , suppressScrollBars(false)
        , vScrollBarMode(ScrollBarAuto)
        , hScrollBarMode(ScrollBarAuto)
    {
    }
    IntSize scrollOffset;
    IntSize contentsSize;
    bool hasStaticBackground;
    bool suppressScrollBars;
    ScrollBarMode vScrollBarMode;
    ScrollBarMode hScrollBarMode;
};

ScrollView::ScrollView()
    : m_data(new ScrollViewPrivate())
{
}

ScrollView::~ScrollView()
{
    delete m_data;
}

void ScrollView::updateContents(const IntRect& updateRect, bool now)
{
    IntRect adjustedDirtyRect(updateRect);
    adjustedDirtyRect.move(-m_data->scrollOffset);

    RECT dirtyRect = RECT(adjustedDirtyRect);
#if PAINT_FLASHING_DEBUG
    HDC dc = GetDC(containingWindow());
    FillRect(dc, &dirtyRect, (HBRUSH)GetStockObject(BLACK_BRUSH));
    ReleaseDC(containingWindow(), dc);
#endif

    InvalidateRect(containingWindow(), &dirtyRect, true);
    if (now)
        UpdateWindow(containingWindow());
}

int ScrollView::visibleWidth() const
{
    RECT bounds;
    GetClientRect(containingWindow(), &bounds);
    return (bounds.right - bounds.left);
}

int ScrollView::visibleHeight() const
{
    RECT bounds;
    GetClientRect(containingWindow(), &bounds);
    return (bounds.bottom - bounds.top);
}

FloatRect ScrollView::visibleContentRect() const
{
    RECT bounds;
    GetClientRect(containingWindow(), &bounds);
    FloatRect contentRect = bounds;
    contentRect.move(m_data->scrollOffset);
    return contentRect;
}

void ScrollView::setContentsPos(int newX, int newY)
{
    int dx = newX - contentsX();
    int dy = newY - contentsY();
    scrollBy(dx, dy);
}

void ScrollView::resizeContents(int w,int h)
{
    IntSize newSize(w,h);
    if (m_data->contentsSize != newSize) {
        m_data->contentsSize = newSize;
        updateScrollBars(m_data->scrollOffset);
    }
}

int ScrollView::contentsX() const
{
    return scrollOffset().width();
}

int ScrollView::contentsY() const
{
    return scrollOffset().height();
}

int ScrollView::contentsWidth() const
{
    return m_data->contentsSize.width();
}

int ScrollView::contentsHeight() const
{
    return m_data->contentsSize.height();
}

IntPoint ScrollView::contentsToWindow(const IntPoint& point) const
{
    return point - scrollOffset();
}

IntPoint ScrollView::windowToContents(const IntPoint& point) const
{
    return point + scrollOffset();
}

IntSize ScrollView::scrollOffset() const
{
    return m_data->scrollOffset;
}

IntSize ScrollView::maximumScroll() const
{
    IntSize delta = m_data->contentsSize - m_data->scrollOffset;
    delta.clampNegativeToZero();
    return delta;
}

void ScrollView::scrollBy(int dx, int dy)
{
    IntSize scrollOffset = m_data->scrollOffset;
    IntSize maxScroll = maximumScroll();
    IntSize newScrollOffset = scrollOffset + IntSize(dx, dy);
    newScrollOffset.clampNegativeToZero();

    if (newScrollOffset != scrollOffset) {
        m_data->scrollOffset = newScrollOffset;
        updateScrollBars(m_data->scrollOffset);
        // ScrollBar updates can fail, so we check the final delta before scrolling
        IntSize scrollDelta = m_data->scrollOffset - scrollOffset;
        if (scrollDelta == IntSize())
            return;
        if (!m_data->hasStaticBackground)
            // FIXME: This could be made more efficient by passing a valid clip rect for only the document content.
            ScrollWindowEx(containingWindow(), -scrollDelta.width(), -scrollDelta.height(), 0, 0, 0, 0, SW_INVALIDATE);
        else
            InvalidateRect(containingWindow(), 0, true);
    }
}

WebCore::ScrollBarMode ScrollView::hScrollBarMode() const
{
    return m_data->hScrollBarMode;
}

WebCore::ScrollBarMode ScrollView::vScrollBarMode() const
{
    return m_data->vScrollBarMode;
}

void ScrollView::suppressScrollBars(bool suppressed, bool repaintOnSuppress)
{
    m_data->suppressScrollBars = suppressed;
    if (repaintOnSuppress)
        updateScrollBars(m_data->scrollOffset);
}

void ScrollView::setHScrollBarMode(ScrollBarMode newMode)
{
    if (m_data->hScrollBarMode != newMode) {
        m_data->hScrollBarMode = newMode;
        updateScrollBars(m_data->scrollOffset);
    }
}

void ScrollView::setVScrollBarMode(ScrollBarMode newMode)
{
    if (m_data->vScrollBarMode != newMode) {
        m_data->vScrollBarMode = newMode;
        updateScrollBars(m_data->scrollOffset);
    }
}

void ScrollView::setScrollBarsMode(ScrollBarMode newMode)
{
    m_data->hScrollBarMode = m_data->vScrollBarMode = newMode;
    updateScrollBars(m_data->scrollOffset);
}

void ScrollView::setStaticBackground(bool flag)
{
    m_data->hasStaticBackground = flag;
}

static int updateScrollInfo(ScrollView* view, short type, int current, int max, int pageSize)
{
    SCROLLINFO si;
    si.cbSize = sizeof(si);
    si.fMask  = SIF_RANGE | SIF_PAGE | SIF_POS;
    si.nMin   = 0;
    si.nMax   = max;
    si.nPage  = pageSize;
    si.nPos   = current;
    SetScrollInfo(view->containingWindow(), type, &si, TRUE);
    GetScrollInfo(view->containingWindow(), type, &si);
    return si.nPos;
}

void ScrollView::updateScrollBars(const IntSize&)
{
    IntSize maxScrollPosition(contentsWidth(), contentsHeight());
    IntSize scroll = scrollOffset().shrunkTo(maxScrollPosition);
    scroll.clampNegativeToZero();

    m_data->scrollOffset =
        IntSize(updateScrollInfo(this, SB_HORZ, scroll.width(), contentsWidth() - 1, width()),
                updateScrollInfo(this, SB_VERT, scroll.height(), contentsHeight() - 1, height()));

    if (m_data->hScrollBarMode != ScrollBarAuto || m_data->suppressScrollBars)
        ShowScrollBar(containingWindow(), SB_HORZ, (m_data->hScrollBarMode != ScrollBarAlwaysOff) && !m_data->suppressScrollBars);
    if (m_data->vScrollBarMode != ScrollBarAuto || m_data->suppressScrollBars)
        ShowScrollBar(containingWindow(), SB_VERT, (m_data->vScrollBarMode != ScrollBarAlwaysOff) && !m_data->suppressScrollBars);
}

}
<|endoftext|>