<commit_before>/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  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 Willow Garage 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 OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Mrinal Kalakrishnan */

#include <ros/ros.h>
#include <chomp_motion_planner/chomp_trajectory.h>
#include <iostream>

namespace chomp
{
ChompTrajectory::ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, double duration,
                                 double discretization, const std::string& group_name)
  : ChompTrajectory(robot_model, static_cast<size_t>(duration / discretization) + 1, discretization, group_name)
{
}

ChompTrajectory::ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, size_t num_points,
                                 double discretization, const std::string& group_name)
  : planning_group_name_(group_name)
  , num_points_(num_points)
  , discretization_(discretization)
  , duration_((num_points - 1) * discretization)
  , start_index_(1)
  , end_index_(num_points_ - 2)
{
  const moveit::core::JointModelGroup* model_group = robot_model->getJointModelGroup(planning_group_name_);
  num_joints_ = model_group->getActiveJointModels().size();
  init();
}

ChompTrajectory::ChompTrajectory(const ChompTrajectory& source_traj, const std::string& group_name,
                                 int diff_rule_length)
  : planning_group_name_(group_name), discretization_(source_traj.discretization_)
{
  num_joints_ = source_traj.getNumJoints();

  // figure out the num_points_:
  // we need diff_rule_length-1 extra points on either side:
  int start_extra = (diff_rule_length - 1) - source_traj.start_index_;
  int end_extra = (diff_rule_length - 1) - ((source_traj.num_points_ - 1) - source_traj.end_index_);

  num_points_ = source_traj.num_points_ + start_extra + end_extra;
  start_index_ = diff_rule_length - 1;
  end_index_ = (num_points_ - 1) - (diff_rule_length - 1);
  duration_ = (num_points_ - 1) * discretization_;

  // allocate the memory:
  init();

  full_trajectory_index_.resize(num_points_);

  // now copy the trajectories over:
  for (size_t i = 0; i < num_points_; i++)
  {
    int source_traj_point = i - start_extra;
    if (source_traj_point < 0)
      source_traj_point = 0;
    if (static_cast<size_t>(source_traj_point) >= source_traj.num_points_)
      source_traj_point = source_traj.num_points_ - 1;
    full_trajectory_index_[i] = source_traj_point;
    getTrajectoryPoint(i) = const_cast<ChompTrajectory&>(source_traj).getTrajectoryPoint(source_traj_point);
  }
}

void ChompTrajectory::init()
{
  trajectory_.resize(num_points_, num_joints_);
}

void ChompTrajectory::updateFromGroupTrajectory(const ChompTrajectory& group_trajectory)
{
  size_t num_vars_free = end_index_ - start_index_ + 1;
  trajectory_.block(start_index_, 0, num_vars_free, num_joints_) =
      group_trajectory.trajectory_.block(group_trajectory.start_index_, 0, num_vars_free, num_joints_);
}

void ChompTrajectory::fillInLinearInterpolation()
{
  double start_index = start_index_ - 1;
  double end_index = end_index_ + 1;
  for (size_t i = 0; i < num_joints_; i++)
  {
    double theta = ((*this)(end_index, i) - (*this)(start_index, i)) / (end_index - 1);
    for (size_t j = start_index + 1; j < end_index; j++)
    {
      (*this)(j, i) = (*this)(start_index, i) + j * theta;
    }
  }
}

void ChompTrajectory::fillInCubicInterpolation()
{
  double start_index = start_index_ - 1;
  double end_index = end_index_ + 1;
  double dt = 0.001;
  std::vector<double> coeffs(4, 0);
  double total_time = (end_index - 1) * dt;
  for (size_t i = 0; i < num_joints_; i++)
  {
    coeffs[0] = (*this)(start_index, i);
    coeffs[2] = (3 / (pow(total_time, 2))) * ((*this)(end_index, i) - (*this)(start_index, i));
    coeffs[3] = (-2 / (pow(total_time, 3))) * ((*this)(end_index, i) - (*this)(start_index, i));

    double t;
    for (size_t j = start_index + 1; j < end_index; j++)
    {
      t = j * dt;
      (*this)(j, i) = coeffs[0] + coeffs[2] * pow(t, 2) + coeffs[3] * pow(t, 3);
    }
  }
}

void ChompTrajectory::fillInMinJerk()
{
  double start_index = start_index_ - 1;
  double end_index = end_index_ + 1;
  double td[6];  // powers of the time duration
  td[0] = 1.0;
  td[1] = (end_index - start_index) * discretization_;

  for (unsigned int i = 2; i <= 5; i++)
    td[i] = td[i - 1] * td[1];

  // calculate the spline coefficients for each joint:
  // (these are for the special case of zero start and end vel and acc)
  double coeff[num_joints_][6];
  for (size_t i = 0; i < num_joints_; i++)
  {
    double x0 = (*this)(start_index, i);
    double x1 = (*this)(end_index, i);
    coeff[i][0] = x0;
    coeff[i][1] = 0;
    coeff[i][2] = 0;
    coeff[i][3] = (-20 * x0 + 20 * x1) / (2 * td[3]);
    coeff[i][4] = (30 * x0 - 30 * x1) / (2 * td[4]);
    coeff[i][5] = (-12 * x0 + 12 * x1) / (2 * td[5]);
  }

  // now fill in the joint positions at each time step
  for (size_t i = start_index + 1; i < end_index; i++)
  {
    double ti[6];  // powers of the time index point
    ti[0] = 1.0;
    ti[1] = (i - start_index) * discretization_;
    for (unsigned int k = 2; k <= 5; k++)
      ti[k] = ti[k - 1] * ti[1];

    for (size_t j = 0; j < num_joints_; j++)
    {
      (*this)(i, j) = 0.0;
      for (unsigned int k = 0; k <= 5; k++)
      {
        (*this)(i, j) += ti[k] * coeff[j][k];
      }
    }
  }
}

bool ChompTrajectory::fillInFromTrajectory(const robot_trajectory::RobotTrajectory& trajectory)
{
  // check if input trajectory has less than two states (start and goal), function returns false if condition is true
  if (trajectory.getWayPointCount() < 2)
    return false;

  const size_t max_output_index = (*this).getNumPoints() - 1;
  const size_t max_input_index = trajectory.getWayPointCount() - 1;

  const robot_model::JointModelGroup* group = trajectory.getGroup();
  for (size_t i = 0; i <= max_output_index; i++)
    assignCHOMPTrajectoryPointFromRobotState(trajectory.getWayPoint(i * max_input_index / max_output_index), i, group);
  return true;
}

void ChompTrajectory::assignCHOMPTrajectoryPointFromRobotState(const robot_state::RobotState& source,
                                                               size_t chomp_trajectory_point_index,
                                                               const robot_model::JointModelGroup* group)
{
  Eigen::MatrixXd::RowXpr target = getTrajectoryPoint(chomp_trajectory_point_index);
  assert(group->getActiveJointModels().size() == static_cast<size_t>(target.cols()));
  size_t joint_index = 0;
  for (const robot_state::JointModel* jm : group->getActiveJointModels())
  {
    assert(jm->getVariableCount() == 1);
    target[joint_index++] = source.getVariablePosition(jm->getFirstVariableIndex());
  }
}

}  // namespace chomp
<commit_msg>Interpolate when initializing from trajectory<commit_after>/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  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 Willow Garage 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 OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Mrinal Kalakrishnan */

#include <ros/ros.h>
#include <chomp_motion_planner/chomp_trajectory.h>
#include <iostream>

namespace chomp
{
ChompTrajectory::ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, double duration,
                                 double discretization, const std::string& group_name)
  : ChompTrajectory(robot_model, static_cast<size_t>(duration / discretization) + 1, discretization, group_name)
{
}

ChompTrajectory::ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, size_t num_points,
                                 double discretization, const std::string& group_name)
  : planning_group_name_(group_name)
  , num_points_(num_points)
  , discretization_(discretization)
  , duration_((num_points - 1) * discretization)
  , start_index_(1)
  , end_index_(num_points_ - 2)
{
  const moveit::core::JointModelGroup* model_group = robot_model->getJointModelGroup(planning_group_name_);
  num_joints_ = model_group->getActiveJointModels().size();
  init();
}

ChompTrajectory::ChompTrajectory(const ChompTrajectory& source_traj, const std::string& group_name,
                                 int diff_rule_length)
  : planning_group_name_(group_name), discretization_(source_traj.discretization_)
{
  num_joints_ = source_traj.getNumJoints();

  // figure out the num_points_:
  // we need diff_rule_length-1 extra points on either side:
  int start_extra = (diff_rule_length - 1) - source_traj.start_index_;
  int end_extra = (diff_rule_length - 1) - ((source_traj.num_points_ - 1) - source_traj.end_index_);

  num_points_ = source_traj.num_points_ + start_extra + end_extra;
  start_index_ = diff_rule_length - 1;
  end_index_ = (num_points_ - 1) - (diff_rule_length - 1);
  duration_ = (num_points_ - 1) * discretization_;

  // allocate the memory:
  init();

  full_trajectory_index_.resize(num_points_);

  // now copy the trajectories over:
  for (size_t i = 0; i < num_points_; i++)
  {
    int source_traj_point = i - start_extra;
    if (source_traj_point < 0)
      source_traj_point = 0;
    if (static_cast<size_t>(source_traj_point) >= source_traj.num_points_)
      source_traj_point = source_traj.num_points_ - 1;
    full_trajectory_index_[i] = source_traj_point;
    getTrajectoryPoint(i) = const_cast<ChompTrajectory&>(source_traj).getTrajectoryPoint(source_traj_point);
  }
}

void ChompTrajectory::init()
{
  trajectory_.resize(num_points_, num_joints_);
}

void ChompTrajectory::updateFromGroupTrajectory(const ChompTrajectory& group_trajectory)
{
  size_t num_vars_free = end_index_ - start_index_ + 1;
  trajectory_.block(start_index_, 0, num_vars_free, num_joints_) =
      group_trajectory.trajectory_.block(group_trajectory.start_index_, 0, num_vars_free, num_joints_);
}

void ChompTrajectory::fillInLinearInterpolation()
{
  double start_index = start_index_ - 1;
  double end_index = end_index_ + 1;
  for (size_t i = 0; i < num_joints_; i++)
  {
    double theta = ((*this)(end_index, i) - (*this)(start_index, i)) / (end_index - 1);
    for (size_t j = start_index + 1; j < end_index; j++)
    {
      (*this)(j, i) = (*this)(start_index, i) + j * theta;
    }
  }
}

void ChompTrajectory::fillInCubicInterpolation()
{
  double start_index = start_index_ - 1;
  double end_index = end_index_ + 1;
  double dt = 0.001;
  std::vector<double> coeffs(4, 0);
  double total_time = (end_index - 1) * dt;
  for (size_t i = 0; i < num_joints_; i++)
  {
    coeffs[0] = (*this)(start_index, i);
    coeffs[2] = (3 / (pow(total_time, 2))) * ((*this)(end_index, i) - (*this)(start_index, i));
    coeffs[3] = (-2 / (pow(total_time, 3))) * ((*this)(end_index, i) - (*this)(start_index, i));

    double t;
    for (size_t j = start_index + 1; j < end_index; j++)
    {
      t = j * dt;
      (*this)(j, i) = coeffs[0] + coeffs[2] * pow(t, 2) + coeffs[3] * pow(t, 3);
    }
  }
}

void ChompTrajectory::fillInMinJerk()
{
  double start_index = start_index_ - 1;
  double end_index = end_index_ + 1;
  double td[6];  // powers of the time duration
  td[0] = 1.0;
  td[1] = (end_index - start_index) * discretization_;

  for (unsigned int i = 2; i <= 5; i++)
    td[i] = td[i - 1] * td[1];

  // calculate the spline coefficients for each joint:
  // (these are for the special case of zero start and end vel and acc)
  double coeff[num_joints_][6];
  for (size_t i = 0; i < num_joints_; i++)
  {
    double x0 = (*this)(start_index, i);
    double x1 = (*this)(end_index, i);
    coeff[i][0] = x0;
    coeff[i][1] = 0;
    coeff[i][2] = 0;
    coeff[i][3] = (-20 * x0 + 20 * x1) / (2 * td[3]);
    coeff[i][4] = (30 * x0 - 30 * x1) / (2 * td[4]);
    coeff[i][5] = (-12 * x0 + 12 * x1) / (2 * td[5]);
  }

  // now fill in the joint positions at each time step
  for (size_t i = start_index + 1; i < end_index; i++)
  {
    double ti[6];  // powers of the time index point
    ti[0] = 1.0;
    ti[1] = (i - start_index) * discretization_;
    for (unsigned int k = 2; k <= 5; k++)
      ti[k] = ti[k - 1] * ti[1];

    for (size_t j = 0; j < num_joints_; j++)
    {
      (*this)(i, j) = 0.0;
      for (unsigned int k = 0; k <= 5; k++)
      {
        (*this)(i, j) += ti[k] * coeff[j][k];
      }
    }
  }
}

bool ChompTrajectory::fillInFromTrajectory(const robot_trajectory::RobotTrajectory& trajectory)
{
  // check if input trajectory has less than two states (start and goal), function returns false if condition is true
  if (trajectory.getWayPointCount() < 2)
    return false;

  const size_t max_output_index = (*this).getNumPoints() - 1;
  const size_t max_input_index = trajectory.getWayPointCount() - 1;

  const robot_model::JointModelGroup* group = trajectory.getGroup();
  robot_state::RobotState interpolated(trajectory.getRobotModel());
  for (size_t i = 0; i <= max_output_index; i++)
  {
    double fraction = static_cast<double>(i * max_input_index) / max_output_index;
    size_t prev_idx = std::trunc(fraction);  // integer part
    fraction = fraction - prev_idx;          // fractional part
    size_t next_idx = prev_idx == max_input_index ? prev_idx : prev_idx + 1;
    trajectory.getWayPoint(prev_idx).interpolate(trajectory.getWayPoint(next_idx), fraction, interpolated, group);
    assignCHOMPTrajectoryPointFromRobotState(interpolated, i, group);
  }
  return true;
}

void ChompTrajectory::assignCHOMPTrajectoryPointFromRobotState(const robot_state::RobotState& source,
                                                               size_t chomp_trajectory_point_index,
                                                               const robot_model::JointModelGroup* group)
{
  Eigen::MatrixXd::RowXpr target = getTrajectoryPoint(chomp_trajectory_point_index);
  assert(group->getActiveJointModels().size() == static_cast<size_t>(target.cols()));
  size_t joint_index = 0;
  for (const robot_state::JointModel* jm : group->getActiveJointModels())
  {
    assert(jm->getVariableCount() == 1);
    target[joint_index++] = source.getVariablePosition(jm->getFirstVariableIndex());
  }
}

}  // namespace chomp
<|endoftext|>


<commit_before>/*
* /dev/random EntropySource
* (C) 1999-2009 Jack Lloyd
*
* Distributed under the terms of the Botan license
*/

#include <botan/es_dev.h>

#include <sys/types.h>
#include <sys/select.h>
#include <sys/stat.h>
#include <unistd.h>
#include <fcntl.h>

namespace Botan {

/**
Close the device, if open
*/
void Device_EntropySource::Device_Reader::close()
   {
   if(fd > 0) { ::close(fd); fd = -1; }
   }

/**
Read bytes from a device file
*/
u32bit Device_EntropySource::Device_Reader::get(byte out[], u32bit length,
                                                u32bit ms_wait_time)
   {
   if(fd < 0)
      return 0;

   if(fd >= FD_SETSIZE)
      return 0;

   fd_set read_set;
   FD_ZERO(&read_set);
   FD_SET(fd, &read_set);

   struct ::timeval timeout;
   timeout.tv_sec = 0;
   timeout.tv_usec = ms_wait_time * 1000;

   if(::select(fd + 1, &read_set, 0, 0, &timeout) < 0)
      return 0;

   if(!(FD_ISSET(fd, &read_set)))
      return 0;

   const ssize_t got = ::read(fd, out, length);
   if(got <= 0)
      return 0;

   return static_cast<u32bit>(got);
   }

/**
Attempt to open a device
*/
Device_EntropySource::Device_Reader::fd_type
Device_EntropySource::Device_Reader::open(const std::string& pathname)
   {
#ifndef O_NONBLOCK
  #define O_NONBLOCK 0
#endif

#ifndef O_NOCTTY
  #define O_NOCTTY 0
#endif

   const int flags = O_RDONLY | O_NONBLOCK | O_NOCTTY;
   return ::open(pathname.c_str(), flags);
   }

/**
Device_EntropySource constructor
Open a file descriptor to each (available) device in fsnames
*/
Device_EntropySource::Device_EntropySource(
   const std::vector<std::string>& fsnames)
   {
   for(u32bit i = 0; i != fsnames.size(); ++i)
      {
      Device_Reader::fd_type fd = Device_Reader::open(fsnames[i]);
      if(fd > 0)
         devices.push_back(Device_Reader(fd));
      }
   }

/**
Device_EntropySource destructor: close all open devices
*/
Device_EntropySource::~Device_EntropySource()
   {
   for(size_t i = 0; i != devices.size(); ++i)
      devices[i].close();
   }

/**
* Gather entropy from a RNG device
*/
void Device_EntropySource::poll(Entropy_Accumulator& accum)
   {
   u32bit go_get = std::min<u32bit>(accum.desired_remaining_bits() / 8, 16);

   u32bit read_wait_ms = go_get / 16;

   MemoryRegion<byte>& io_buffer = accum.get_io_buffer(go_get);

   for(size_t i = 0; i != devices.size(); ++i)
      {
      u32bit got = devices[i].get(io_buffer.begin(), io_buffer.size(),
                                  read_wait_ms);

      if(got)
         {
         accum.add(io_buffer.begin(), got, 8);
         break;
         }
      }
   }

}
<commit_msg>Changes to /dev/*random poller - read up to 48 bytes, and wait longer in select loop (up to a second)<commit_after>/*
* /dev/random EntropySource
* (C) 1999-2009 Jack Lloyd
*
* Distributed under the terms of the Botan license
*/

#include <botan/es_dev.h>

#include <sys/types.h>
#include <sys/select.h>
#include <sys/stat.h>
#include <unistd.h>
#include <fcntl.h>

namespace Botan {

/**
Close the device, if open
*/
void Device_EntropySource::Device_Reader::close()
   {
   if(fd > 0) { ::close(fd); fd = -1; }
   }

/**
Read bytes from a device file
*/
u32bit Device_EntropySource::Device_Reader::get(byte out[], u32bit length,
                                                u32bit ms_wait_time)
   {
   if(fd < 0)
      return 0;

   if(fd >= FD_SETSIZE)
      return 0;

   fd_set read_set;
   FD_ZERO(&read_set);
   FD_SET(fd, &read_set);

   struct ::timeval timeout;
   timeout.tv_sec = 0;
   timeout.tv_usec = ms_wait_time * 1000;

   if(::select(fd + 1, &read_set, 0, 0, &timeout) < 0)
      return 0;

   if(!(FD_ISSET(fd, &read_set)))
      return 0;

   const ssize_t got = ::read(fd, out, length);
   if(got <= 0)
      return 0;

   return static_cast<u32bit>(got);
   }

/**
Attempt to open a device
*/
Device_EntropySource::Device_Reader::fd_type
Device_EntropySource::Device_Reader::open(const std::string& pathname)
   {
#ifndef O_NONBLOCK
  #define O_NONBLOCK 0
#endif

#ifndef O_NOCTTY
  #define O_NOCTTY 0
#endif

   const int flags = O_RDONLY | O_NONBLOCK | O_NOCTTY;
   return ::open(pathname.c_str(), flags);
   }

/**
Device_EntropySource constructor
Open a file descriptor to each (available) device in fsnames
*/
Device_EntropySource::Device_EntropySource(
   const std::vector<std::string>& fsnames)
   {
   for(u32bit i = 0; i != fsnames.size(); ++i)
      {
      Device_Reader::fd_type fd = Device_Reader::open(fsnames[i]);
      if(fd > 0)
         devices.push_back(Device_Reader(fd));
      }
   }

/**
Device_EntropySource destructor: close all open devices
*/
Device_EntropySource::~Device_EntropySource()
   {
   for(size_t i = 0; i != devices.size(); ++i)
      devices[i].close();
   }

/**
* Gather entropy from a RNG device
*/
void Device_EntropySource::poll(Entropy_Accumulator& accum)
   {
   u32bit go_get = std::min<u32bit>(accum.desired_remaining_bits() / 8, 48);

   u32bit read_wait_ms = std::max<u32bit>(go_get, 1000);
   MemoryRegion<byte>& io_buffer = accum.get_io_buffer(go_get);

   for(size_t i = 0; i != devices.size(); ++i)
      {
      u32bit got = devices[i].get(io_buffer.begin(), io_buffer.size(),
                                  read_wait_ms);

      if(got)
         {
         accum.add(io_buffer.begin(), got, 8);
         break;
         }
      }
   }

}
<|endoftext|>


<commit_before>// Copyright (c) 2006-2008 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#if defined(OS_WIN)
#include <windows.h>
#include <process.h>
#endif

#include "base/simple_thread.h"
#include "base/thread_local_storage.h"
#include "testing/gtest/include/gtest/gtest.h"

#if defined(OS_WIN)
// Ignore warnings about ptr->int conversions that we use when
// storing ints into ThreadLocalStorage.
#pragma warning(disable : 4311 4312)
#endif

const int kInitialTlsValue = 0x5555;
static ThreadLocalStorage::Slot tls_slot(base::LINKER_INITIALIZED);

class ThreadLocalStorageRunner : public base::DelegateSimpleThread::Delegate {
 public:
  explicit ThreadLocalStorageRunner(int* tls_value_ptr)
      : tls_value_ptr_(tls_value_ptr) {}

  virtual ~ThreadLocalStorageRunner() {}

  virtual void Run() {
    *tls_value_ptr_ = kInitialTlsValue;
    tls_slot.Set(tls_value_ptr_);

    int *ptr = static_cast<int*>(tls_slot.Get());
    EXPECT_EQ(ptr, tls_value_ptr_);
    EXPECT_EQ(*ptr, kInitialTlsValue);
    *tls_value_ptr_ = 0;

    ptr = static_cast<int*>(tls_slot.Get());
    EXPECT_EQ(ptr, tls_value_ptr_);
    EXPECT_EQ(*ptr, 0);
  }

 private:
  int* tls_value_ptr_;
  DISALLOW_COPY_AND_ASSIGN(ThreadLocalStorageRunner);
};

void ThreadLocalStorageCleanup(void *value) {
  int *ptr = reinterpret_cast<int*>(value);
  if (ptr)
    *ptr = kInitialTlsValue;
}

TEST(ThreadLocalStorageTest, Basics) {
  ThreadLocalStorage::Slot slot;
  slot.Set(reinterpret_cast<void*>(123));
  int value = reinterpret_cast<int>(slot.Get());
  EXPECT_EQ(value, 123);
}

TEST(ThreadLocalStorageTest, TLSDestructors) {
  // Create a TLS index with a destructor.  Create a set of
  // threads that set the TLS, while the destructor cleans it up.
  // After the threads finish, verify that the value is cleaned up.
  const int kNumThreads = 5;
  int values[kNumThreads];
  ThreadLocalStorageRunner* thread_delegates[kNumThreads];
  base::DelegateSimpleThread* threads[kNumThreads];

  tls_slot.Initialize(ThreadLocalStorageCleanup);

  // Spawn the threads.
  for (int index = 0; index < kNumThreads; index++) {
    values[index] = kInitialTlsValue;
    thread_delegates[index] = new ThreadLocalStorageRunner(&values[index]);
    threads[index] = new base::DelegateSimpleThread(thread_delegates[index],
                                                    "tls thread");
    threads[index]->Start();
  }

  // Wait for the threads to finish.
  for (int index = 0; index < kNumThreads; index++) {
    threads[index]->Join();
    delete threads[index];
    delete thread_delegates[index];

    // Verify that the destructor was called and that we reset.
    EXPECT_EQ(values[index], kInitialTlsValue);
  }
}
<commit_msg>Use intptr_t for casting pointers for 64-bit.<commit_after>// Copyright (c) 2006-2008 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#if defined(OS_WIN)
#include <windows.h>
#include <process.h>
#endif

#include "base/simple_thread.h"
#include "base/thread_local_storage.h"
#include "testing/gtest/include/gtest/gtest.h"

#if defined(OS_WIN)
// Ignore warnings about ptr->int conversions that we use when
// storing ints into ThreadLocalStorage.
#pragma warning(disable : 4311 4312)
#endif

const int kInitialTlsValue = 0x5555;
static ThreadLocalStorage::Slot tls_slot(base::LINKER_INITIALIZED);

class ThreadLocalStorageRunner : public base::DelegateSimpleThread::Delegate {
 public:
  explicit ThreadLocalStorageRunner(int* tls_value_ptr)
      : tls_value_ptr_(tls_value_ptr) {}

  virtual ~ThreadLocalStorageRunner() {}

  virtual void Run() {
    *tls_value_ptr_ = kInitialTlsValue;
    tls_slot.Set(tls_value_ptr_);

    int *ptr = static_cast<int*>(tls_slot.Get());
    EXPECT_EQ(ptr, tls_value_ptr_);
    EXPECT_EQ(*ptr, kInitialTlsValue);
    *tls_value_ptr_ = 0;

    ptr = static_cast<int*>(tls_slot.Get());
    EXPECT_EQ(ptr, tls_value_ptr_);
    EXPECT_EQ(*ptr, 0);
  }

 private:
  int* tls_value_ptr_;
  DISALLOW_COPY_AND_ASSIGN(ThreadLocalStorageRunner);
};

void ThreadLocalStorageCleanup(void *value) {
  int *ptr = reinterpret_cast<int*>(value);
  if (ptr)
    *ptr = kInitialTlsValue;
}

TEST(ThreadLocalStorageTest, Basics) {
  ThreadLocalStorage::Slot slot;
  slot.Set(reinterpret_cast<void*>(123));
  int value = reinterpret_cast<intptr_t>(slot.Get());
  EXPECT_EQ(value, 123);
}

TEST(ThreadLocalStorageTest, TLSDestructors) {
  // Create a TLS index with a destructor.  Create a set of
  // threads that set the TLS, while the destructor cleans it up.
  // After the threads finish, verify that the value is cleaned up.
  const int kNumThreads = 5;
  int values[kNumThreads];
  ThreadLocalStorageRunner* thread_delegates[kNumThreads];
  base::DelegateSimpleThread* threads[kNumThreads];

  tls_slot.Initialize(ThreadLocalStorageCleanup);

  // Spawn the threads.
  for (int index = 0; index < kNumThreads; index++) {
    values[index] = kInitialTlsValue;
    thread_delegates[index] = new ThreadLocalStorageRunner(&values[index]);
    threads[index] = new base::DelegateSimpleThread(thread_delegates[index],
                                                    "tls thread");
    threads[index]->Start();
  }

  // Wait for the threads to finish.
  for (int index = 0; index < kNumThreads; index++) {
    threads[index]->Join();
    delete threads[index];
    delete thread_delegates[index];

    // Verify that the destructor was called and that we reset.
    EXPECT_EQ(values[index], kInitialTlsValue);
  }
}
<|endoftext|>


<commit_before>// @(#)root/rootx:$Name:  $:$Id: rootx.cxx,v 1.2 2000/06/16 15:23:01 rdm Exp $
// Author: Fons Rademakers   19/02/98

//////////////////////////////////////////////////////////////////////////
//                                                                      //
// Rootx                                                                //
//                                                                      //
// Rootx is a small front-end program that starts the main ROOT module. //
// This program is called "root" in the $ROOTSYS/bin directory and the  //
// real ROOT executable is now called "root.exe" (formerly "root").     //
// Rootx puts up a splash screen giving some info about the current     //
// version of ROOT and, more importanly, it sets up the required        //
// LD_LIBRARY_PATH, SHLIB_PATH and LIBPATH environment variables        //
// (depending on the platform).                                         //
//                                                                      //
//////////////////////////////////////////////////////////////////////////

#ifdef HAVE_CONFIG
#include "config.h"
#endif

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <signal.h>
#include <sys/wait.h>
#include <sys/stat.h>
#include <errno.h>

#include <netdb.h>
#include <sys/socket.h>

#if defined(__sgi) || defined(__sun)
#define HAVE_UTMPX_H
#define UTMP_NO_ADDR
#endif
#if (defined(__alpha) && !defined(__linux)) || defined(_AIX) || \
    defined(__FreeBSD__) || defined(__Lynx__)
#define UTMP_NO_ADDR
#endif

#ifdef __sun
#   if __SUNPRO_CC > 0x420
#      define GLOBAL_ERRNO
#   endif
#endif

#ifndef __VMS
# ifdef HAVE_UTMPX_H
# include <utmpx.h>
# define STRUCT_UTMP struct utmpx
# else
# if defined(__linux) && defined(__powerpc) && (__GNUC_MINOR__ < 90)
   extern "C" {
# endif
# include <utmp.h>
# define STRUCT_UTMP struct utmp
# endif
#endif

#if !defined(UTMP_FILE) && defined(_PATH_UTMP)      // 4.4BSD
#define UTMP_FILE _PATH_UTMP
#endif
#if defined(UTMPX_FILE)                             // Solaris, SysVr4
#undef  UTMP_FILE
#define UTMP_FILE UTMPX_FILE
#endif
#ifndef UTMP_FILE
#define UTMP_FILE "/etc/utmp"
#endif

extern void PopupLogo();
extern void PopdownLogo();
extern void CloseDisplay();

static STRUCT_UTMP *gUtmpContents;
static int          gBatch  = 0;
static int          gNoLogo = 0;

static int GetErrno()
{
#ifdef GLOBAL_ERRNO
   return ::errno;
#else
   return errno;
#endif
}

static void ResetErrno()
{
#ifdef GLOBAL_ERRNO
   ::errno = 0;
#else
   errno = 0;
#endif
}

static int ReadUtmp()
{
   FILE  *utmp;
   struct stat file_stats;
   size_t n_read, size;

   gUtmpContents = 0;

   utmp = fopen(UTMP_FILE, "r");
   if (!utmp)
      return 0;

   fstat(fileno(utmp), &file_stats);
   size = file_stats.st_size;
   if (size <= 0) {
      fclose(utmp);
      return 0;
   }

   gUtmpContents = (STRUCT_UTMP *) malloc(size);
   if (!gUtmpContents) return 0;

   n_read = fread(gUtmpContents, 1, size, utmp);
   if (ferror(utmp) || fclose(utmp) == EOF || n_read < size) {
      free(gUtmpContents);
      gUtmpContents = 0;
      return 0;
   }

   return size / sizeof(STRUCT_UTMP);
}

static STRUCT_UTMP *SearchEntry(int n, const char *tty)
{
   STRUCT_UTMP *ue = gUtmpContents;

   while (n--) {
      if (ue->ut_name[0] && !strncmp(tty, ue->ut_line, sizeof(ue->ut_line)))
         return ue;
      ue++;
   }
   return 0;
}

static void SetDisplay()
{
   // Set DISPLAY environment variable.

   if (!getenv("DISPLAY")) {
      char *tty = ttyname(0);  // device user is logged in on
      if (tty) {
         tty += 5;             // remove "/dev/"
         STRUCT_UTMP *utmp_entry = SearchEntry(ReadUtmp(), tty);
         if (utmp_entry) {
            static char display[64];
            if (utmp_entry->ut_host[0]) {
               if (strchr(utmp_entry->ut_host, ':')) {
                  sprintf(display, "DISPLAY=%s", utmp_entry->ut_host);
                  fprintf(stderr, "*** DISPLAY not set, setting it to %s\n",
                          utmp_entry->ut_host);
	       } else {
                  sprintf(display, "DISPLAY=%s:0.0", utmp_entry->ut_host);
                  fprintf(stderr, "*** DISPLAY not set, setting it to %s:0.0\n",
                          utmp_entry->ut_host);
               }
               putenv((char *)display);
#ifndef UTMP_NO_ADDR
            } else if (utmp_entry->ut_addr) {
               struct hostent *he;
               if ((he = gethostbyaddr((const char*)&utmp_entry->ut_addr,
                                       sizeof(utmp_entry->ut_addr), AF_INET))) {
                  sprintf(display, "DISPLAY=%s:0.0", he->h_name);
                  fprintf(stderr, "*** DISPLAY not set, setting it to %s:0.0\n",
                          he->h_name);
                  putenv((char *)display);
               }
#endif
            }
         }
         free(gUtmpContents);
      }
   }
}

static void SetLibraryPath()
{
#ifndef ROOTLIBDIR
   // Set library path for the different platforms.

   static char msg[512];

#  if defined(__linux) || defined(__alpha) || defined(__sgi) || \
      defined(__sun) || defined(__FreeBSD__)
   if (getenv("LD_LIBRARY_PATH"))
      sprintf(msg, "LD_LIBRARY_PATH=%s/lib:%s", getenv("ROOTSYS"),
                                                getenv("LD_LIBRARY_PATH"));
   else
#  if defined(__sun)
      sprintf(msg, "LD_LIBRARY_PATH=%s/lib:/usr/dt/lib", getenv("ROOTSYS"));
#  else
      sprintf(msg, "LD_LIBRARY_PATH=%s/lib", getenv("ROOTSYS"));
#  endif
#  elif defined(__hpux)  || defined(_HIUX_SOURCE)
   if (getenv("SHLIB_PATH"))
      sprintf(msg, "SHLIB_PATH=%s/lib:%s", getenv("ROOTSYS"),
                                           getenv("SHLIB_PATH"));
   else
      sprintf(msg, "SHLIB_PATH=%s/lib", getenv("ROOTSYS"));
#  elif defined(_AIX)
   if (getenv("LIBPATH"))
      sprintf(msg, "LIBPATH=%s/lib:%s", getenv("ROOTSYS"),
                                        getenv("LIBPATH"));
   else
      sprintf(msg, "LIBPATH=%s/lib:/lib:/usr/lib", getenv("ROOTSYS"));
#  endif
   putenv((char *)msg);
#endif
}

static void SigUsr1(int)
{
   // When we get SIGUSR1 from child (i.e. ROOT) then pop down logo.

   if (!gBatch)
      PopdownLogo();
}

static void WaitChild(int childpid)
{
   // Wait till child (i.e. ROOT) is finished.

   int status;

   do {
      while (waitpid(childpid, &status, WUNTRACED) < 0) {
         if (GetErrno() != EINTR)
            break;
         ResetErrno();
      }
      if (WIFSTOPPED(status)) {         // child got ctlr-Z
         raise(SIGTSTP);                // stop also parent
         kill(childpid, SIGCONT);       // if parent wakes up, wake up child
      }
   } while (WIFSTOPPED(status));

   exit(0);
}

static void PrintUsage(char *pname)
{
   // This is a copy of the text in TApplication::GetOptions().

   fprintf(stderr, "Usage: %s [-l] [-b] [-n] [-q] [dir] [file1.C ... fileN.C]\n", pname);
   fprintf(stderr, "Options:\n");
   fprintf(stderr, "  -b : run in batch mode without graphics\n");
   fprintf(stderr, "  -n : do not execute logon and logoff macros as specified in .rootrc\n");
   fprintf(stderr, "  -q : exit after processing command line macro files\n");
   fprintf(stderr, "  -l : do not show splash screen\n");
   fprintf(stderr, " dir : if dir is a valid directory cd to it before executing\n");
   fprintf(stderr, "\n");
}

int main(int argc, char **argv)
{
   const int kMAXARGS = 256;
   char *argvv[kMAXARGS];
   char  arg0[256];

#ifndef ROOTPREFIX
   if (!getenv("ROOTSYS")) {
      fprintf(stderr, "%s: ROOTSYS not set. Set it before trying to run %s.\n",
              argv[0], argv[0]);
      return 1;
   }
#endif

   // In batch mode don't show splash screen
   int i;
   for (i = 1; i < argc; i++) {
      if (!strcmp(argv[i], "-?") || !strncmp(argv[i], "-h", 2)) {
         PrintUsage(argv[0]);
         return 1;
      }
      if (!strcmp(argv[i], "-b")) gBatch  = 1;
      if (!strcmp(argv[i], "-l")) gNoLogo = 1;
   }

   if (!gBatch) {
      SetDisplay();
      if (!getenv("DISPLAY")) {
         fprintf(stderr, "%s: can't figure out DISPLAY, set it manually\n", argv[0]);
         return 1;
      }
      if (!gNoLogo) PopupLogo();
   }

   // Ignore SIGINT and SIGQUIT. Install handler for SIGUSR1.

   struct sigaction ignore, handle, saveintr, savequit, saveusr1;

#if defined(__sun) && !defined(__i386) && !defined(__SVR4)
   ignore.sa_handler = (void (*)())SIG_IGN;
#elif defined(__sun) && defined(__SVR4)
   ignore.sa_handler = (void (*)(int))SIG_IGN;
#else
   ignore.sa_handler = SIG_IGN;
#endif
   sigemptyset(&ignore.sa_mask);
   ignore.sa_flags = 0;
   handle = ignore;
#if defined(__sun) && !defined(__i386) && !defined(__SVR4)
   handle.sa_handler = (void (*)())SigUsr1;
#elif defined(__sun) && defined(__SVR4)
   handle.sa_handler = (void (*)(int))SigUsr1;
#elif (defined(__sgi) && !defined(__KCC)) || defined(__Lynx__)
#   if defined(IRIX64)
   handle.sa_handler = (void (*)(int))SigUsr1;
#   else
   handle.sa_handler = (void (*)(...))SigUsr1;
#   endif
#else
   handle.sa_handler = SigUsr1;
#endif
   sigaction(SIGINT,  &ignore, &saveintr);
   sigaction(SIGQUIT, &ignore, &savequit);
   sigaction(SIGUSR1, &handle, &saveusr1);

   // Create child...

   int childpid;
   if ((childpid = fork()) < 0) {
      fprintf(stderr, "%s: error forking child\n", argv[0]);
      return 1;
   } else if (childpid > 0)
      WaitChild(childpid);

   // Continue with child...

   // Restore original signal actions
   sigaction(SIGINT,  &saveintr, 0);
   sigaction(SIGQUIT, &savequit, 0);
   sigaction(SIGUSR1, &saveusr1, 0);

   // Close X display connection
   CloseDisplay();

   // Child is going to overlay itself with the actual ROOT module...

   // Build argv vector
#ifdef ROOTBINDIR
   sprintf(arg0, "%s/root.exe", ROOTBINDIR);
#else
   sprintf(arg0, "%s/bin/root.exe", getenv("ROOTSYS"));
#endif
   argvv[0] = arg0;
   argvv[1] = (char *) "-splash";

   int iargc = argc;
   if (iargc > kMAXARGS-2) iargc = kMAXARGS-2;
   for (i = 1; i < iargc; i++)
      argvv[1+i] = argv[i];
   argvv[1+i] = 0;

   // Make sure library path is set
   SetLibraryPath();

   // Execute actual ROOT module
   execv(arg0, argvv);

   // Exec failed
#ifndef ROOTBINDIR
   fprintf(stderr,
	   "%s: can't start ROOT -- check that %s/bin/root.exe exists!\n",
           argv[0], getenv("ROOTSYS"));
#else
   fprintf(stderr, "%s: can't start ROOT -- check that %s/root.exe exists!\n",
           argv[0], ROOTBINDIR);
#endif

   return 1;
}

<commit_msg>correctly report exit status of child, i.e. root.exe. Now doing: gApplication->Terminate(1); will correctly return 1 as exit status to the shell.<commit_after>// @(#)root/rootx:$Name:  $:$Id: rootx.cxx,v 1.3 2000/12/10 16:02:49 rdm Exp $
// Author: Fons Rademakers   19/02/98

//////////////////////////////////////////////////////////////////////////
//                                                                      //
// Rootx                                                                //
//                                                                      //
// Rootx is a small front-end program that starts the main ROOT module. //
// This program is called "root" in the $ROOTSYS/bin directory and the  //
// real ROOT executable is now called "root.exe" (formerly "root").     //
// Rootx puts up a splash screen giving some info about the current     //
// version of ROOT and, more importanly, it sets up the required        //
// LD_LIBRARY_PATH, SHLIB_PATH and LIBPATH environment variables        //
// (depending on the platform).                                         //
//                                                                      //
//////////////////////////////////////////////////////////////////////////

#ifdef HAVE_CONFIG
#include "config.h"
#endif

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <signal.h>
#include <sys/wait.h>
#include <sys/stat.h>
#include <errno.h>

#include <netdb.h>
#include <sys/socket.h>

#if defined(__sgi) || defined(__sun)
#define HAVE_UTMPX_H
#define UTMP_NO_ADDR
#endif
#if (defined(__alpha) && !defined(__linux)) || defined(_AIX) || \
    defined(__FreeBSD__) || defined(__Lynx__)
#define UTMP_NO_ADDR
#endif

#ifdef __sun
#   if __SUNPRO_CC > 0x420
#      define GLOBAL_ERRNO
#   endif
#endif

#ifndef __VMS
# ifdef HAVE_UTMPX_H
# include <utmpx.h>
# define STRUCT_UTMP struct utmpx
# else
# if defined(__linux) && defined(__powerpc) && (__GNUC_MINOR__ < 90)
   extern "C" {
# endif
# include <utmp.h>
# define STRUCT_UTMP struct utmp
# endif
#endif

#if !defined(UTMP_FILE) && defined(_PATH_UTMP)      // 4.4BSD
#define UTMP_FILE _PATH_UTMP
#endif
#if defined(UTMPX_FILE)                             // Solaris, SysVr4
#undef  UTMP_FILE
#define UTMP_FILE UTMPX_FILE
#endif
#ifndef UTMP_FILE
#define UTMP_FILE "/etc/utmp"
#endif

extern void PopupLogo();
extern void PopdownLogo();
extern void CloseDisplay();

static STRUCT_UTMP *gUtmpContents;
static int          gBatch  = 0;
static int          gNoLogo = 0;

static int GetErrno()
{
#ifdef GLOBAL_ERRNO
   return ::errno;
#else
   return errno;
#endif
}

static void ResetErrno()
{
#ifdef GLOBAL_ERRNO
   ::errno = 0;
#else
   errno = 0;
#endif
}

static int ReadUtmp()
{
   FILE  *utmp;
   struct stat file_stats;
   size_t n_read, size;

   gUtmpContents = 0;

   utmp = fopen(UTMP_FILE, "r");
   if (!utmp)
      return 0;

   fstat(fileno(utmp), &file_stats);
   size = file_stats.st_size;
   if (size <= 0) {
      fclose(utmp);
      return 0;
   }

   gUtmpContents = (STRUCT_UTMP *) malloc(size);
   if (!gUtmpContents) return 0;

   n_read = fread(gUtmpContents, 1, size, utmp);
   if (ferror(utmp) || fclose(utmp) == EOF || n_read < size) {
      free(gUtmpContents);
      gUtmpContents = 0;
      return 0;
   }

   return size / sizeof(STRUCT_UTMP);
}

static STRUCT_UTMP *SearchEntry(int n, const char *tty)
{
   STRUCT_UTMP *ue = gUtmpContents;

   while (n--) {
      if (ue->ut_name[0] && !strncmp(tty, ue->ut_line, sizeof(ue->ut_line)))
         return ue;
      ue++;
   }
   return 0;
}

static void SetDisplay()
{
   // Set DISPLAY environment variable.

   if (!getenv("DISPLAY")) {
      char *tty = ttyname(0);  // device user is logged in on
      if (tty) {
         tty += 5;             // remove "/dev/"
         STRUCT_UTMP *utmp_entry = SearchEntry(ReadUtmp(), tty);
         if (utmp_entry) {
            static char display[64];
            if (utmp_entry->ut_host[0]) {
               if (strchr(utmp_entry->ut_host, ':')) {
                  sprintf(display, "DISPLAY=%s", utmp_entry->ut_host);
                  fprintf(stderr, "*** DISPLAY not set, setting it to %s\n",
                          utmp_entry->ut_host);
	       } else {
                  sprintf(display, "DISPLAY=%s:0.0", utmp_entry->ut_host);
                  fprintf(stderr, "*** DISPLAY not set, setting it to %s:0.0\n",
                          utmp_entry->ut_host);
               }
               putenv((char *)display);
#ifndef UTMP_NO_ADDR
            } else if (utmp_entry->ut_addr) {
               struct hostent *he;
               if ((he = gethostbyaddr((const char*)&utmp_entry->ut_addr,
                                       sizeof(utmp_entry->ut_addr), AF_INET))) {
                  sprintf(display, "DISPLAY=%s:0.0", he->h_name);
                  fprintf(stderr, "*** DISPLAY not set, setting it to %s:0.0\n",
                          he->h_name);
                  putenv((char *)display);
               }
#endif
            }
         }
         free(gUtmpContents);
      }
   }
}

static void SetLibraryPath()
{
#ifndef ROOTLIBDIR
   // Set library path for the different platforms.

   static char msg[512];

#  if defined(__linux) || defined(__alpha) || defined(__sgi) || \
      defined(__sun) || defined(__FreeBSD__)
   if (getenv("LD_LIBRARY_PATH"))
      sprintf(msg, "LD_LIBRARY_PATH=%s/lib:%s", getenv("ROOTSYS"),
                                                getenv("LD_LIBRARY_PATH"));
   else
#  if defined(__sun)
      sprintf(msg, "LD_LIBRARY_PATH=%s/lib:/usr/dt/lib", getenv("ROOTSYS"));
#  else
      sprintf(msg, "LD_LIBRARY_PATH=%s/lib", getenv("ROOTSYS"));
#  endif
#  elif defined(__hpux)  || defined(_HIUX_SOURCE)
   if (getenv("SHLIB_PATH"))
      sprintf(msg, "SHLIB_PATH=%s/lib:%s", getenv("ROOTSYS"),
                                           getenv("SHLIB_PATH"));
   else
      sprintf(msg, "SHLIB_PATH=%s/lib", getenv("ROOTSYS"));
#  elif defined(_AIX)
   if (getenv("LIBPATH"))
      sprintf(msg, "LIBPATH=%s/lib:%s", getenv("ROOTSYS"),
                                        getenv("LIBPATH"));
   else
      sprintf(msg, "LIBPATH=%s/lib:/lib:/usr/lib", getenv("ROOTSYS"));
#  endif
   putenv((char *)msg);
#endif
}

static void SigUsr1(int)
{
   // When we get SIGUSR1 from child (i.e. ROOT) then pop down logo.

   if (!gBatch)
      PopdownLogo();
}

static void WaitChild(int childpid)
{
   // Wait till child (i.e. ROOT) is finished.

   int status;

   do {
      while (waitpid(childpid, &status, WUNTRACED) < 0) {
         if (GetErrno() != EINTR)
            break;
         ResetErrno();
      }

      if (WIFEXITED(status))
         exit(WEXITSTATUS(status));

      if (WIFSTOPPED(status)) {         // child got ctlr-Z
         raise(SIGTSTP);                // stop also parent
         kill(childpid, SIGCONT);       // if parent wakes up, wake up child
      }
   } while (WIFSTOPPED(status));

   exit(0);
}

static void PrintUsage(char *pname)
{
   // This is a copy of the text in TApplication::GetOptions().

   fprintf(stderr, "Usage: %s [-l] [-b] [-n] [-q] [dir] [file1.C ... fileN.C]\n", pname);
   fprintf(stderr, "Options:\n");
   fprintf(stderr, "  -b : run in batch mode without graphics\n");
   fprintf(stderr, "  -n : do not execute logon and logoff macros as specified in .rootrc\n");
   fprintf(stderr, "  -q : exit after processing command line macro files\n");
   fprintf(stderr, "  -l : do not show splash screen\n");
   fprintf(stderr, " dir : if dir is a valid directory cd to it before executing\n");
   fprintf(stderr, "\n");
}

int main(int argc, char **argv)
{
   const int kMAXARGS = 256;
   char *argvv[kMAXARGS];
   char  arg0[256];

#ifndef ROOTPREFIX
   if (!getenv("ROOTSYS")) {
      fprintf(stderr, "%s: ROOTSYS not set. Set it before trying to run %s.\n",
              argv[0], argv[0]);
      return 1;
   }
#endif

   // In batch mode don't show splash screen
   int i;
   for (i = 1; i < argc; i++) {
      if (!strcmp(argv[i], "-?") || !strncmp(argv[i], "-h", 2)) {
         PrintUsage(argv[0]);
         return 1;
      }
      if (!strcmp(argv[i], "-b")) gBatch  = 1;
      if (!strcmp(argv[i], "-l")) gNoLogo = 1;
   }

   if (!gBatch) {
      SetDisplay();
      if (!getenv("DISPLAY")) {
         fprintf(stderr, "%s: can't figure out DISPLAY, set it manually\n", argv[0]);
         return 1;
      }
      if (!gNoLogo) PopupLogo();
   }

   // Ignore SIGINT and SIGQUIT. Install handler for SIGUSR1.

   struct sigaction ignore, handle, saveintr, savequit, saveusr1;

#if defined(__sun) && !defined(__i386) && !defined(__SVR4)
   ignore.sa_handler = (void (*)())SIG_IGN;
#elif defined(__sun) && defined(__SVR4)
   ignore.sa_handler = (void (*)(int))SIG_IGN;
#else
   ignore.sa_handler = SIG_IGN;
#endif
   sigemptyset(&ignore.sa_mask);
   ignore.sa_flags = 0;
   handle = ignore;
#if defined(__sun) && !defined(__i386) && !defined(__SVR4)
   handle.sa_handler = (void (*)())SigUsr1;
#elif defined(__sun) && defined(__SVR4)
   handle.sa_handler = (void (*)(int))SigUsr1;
#elif (defined(__sgi) && !defined(__KCC)) || defined(__Lynx__)
#   if defined(IRIX64)
   handle.sa_handler = (void (*)(int))SigUsr1;
#   else
   handle.sa_handler = (void (*)(...))SigUsr1;
#   endif
#else
   handle.sa_handler = SigUsr1;
#endif
   sigaction(SIGINT,  &ignore, &saveintr);
   sigaction(SIGQUIT, &ignore, &savequit);
   sigaction(SIGUSR1, &handle, &saveusr1);

   // Create child...

   int childpid;
   if ((childpid = fork()) < 0) {
      fprintf(stderr, "%s: error forking child\n", argv[0]);
      return 1;
   } else if (childpid > 0)
      WaitChild(childpid);

   // Continue with child...

   // Restore original signal actions
   sigaction(SIGINT,  &saveintr, 0);
   sigaction(SIGQUIT, &savequit, 0);
   sigaction(SIGUSR1, &saveusr1, 0);

   // Close X display connection
   CloseDisplay();

   // Child is going to overlay itself with the actual ROOT module...

   // Build argv vector
#ifdef ROOTBINDIR
   sprintf(arg0, "%s/root.exe", ROOTBINDIR);
#else
   sprintf(arg0, "%s/bin/root.exe", getenv("ROOTSYS"));
#endif
   argvv[0] = arg0;
   argvv[1] = (char *) "-splash";

   int iargc = argc;
   if (iargc > kMAXARGS-2) iargc = kMAXARGS-2;
   for (i = 1; i < iargc; i++)
      argvv[1+i] = argv[i];
   argvv[1+i] = 0;

   // Make sure library path is set
   SetLibraryPath();

   // Execute actual ROOT module
   execv(arg0, argvv);

   // Exec failed
#ifndef ROOTBINDIR
   fprintf(stderr,
	   "%s: can't start ROOT -- check that %s/bin/root.exe exists!\n",
           argv[0], getenv("ROOTSYS"));
#else
   fprintf(stderr, "%s: can't start ROOT -- check that %s/root.exe exists!\n",
           argv[0], ROOTBINDIR);
#endif

   return 1;
}

<|endoftext|>


<commit_before>// ========================================================================== //
// This file is part of Sara, a basic set of libraries in C++ for computer
// vision.
//
// Copyright (C) 2017 David Ok <david.ok8@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License v. 2.0. If a copy of the MPL was not distributed with this file,
// you can obtain one at http://mozilla.org/MPL/2.0/.
// ========================================================================== //

//! @file

#pragma once

#include <DO/Sara/Core/Tensor.hpp>

namespace DO { namespace Sara {

  //! @brief Image iterator trait class.
  template <typename Iterator>
  struct ImageIteratorTraits
  {
    using iterator = Iterator;
    using value_type = typename Iterator::value_type;
    using pixel_type = typename value_type::pixel_type;
    using channel_type = typename PixelTraits<pixel_type>::channel_type;

    static const int num_channels = PixelTraits<pixel_type>::num_channels;

    using color_mean_type = Matrix<double, num_channels, 1>;
    using color_covariance_matrix_type =
        Matrix<double, num_channels, num_channels>;
  };

  inline namespace naive {

    //! @brief Compute the sample color mean vector.
    //! The implementation is the naive textbook algorithm.
    template <typename ImageIterator>
    inline auto color_sample_mean_vector(ImageIterator first,
                                         ImageIterator last)
        -> Matrix<double, ImageIteratorTraits<ImageIterator>::num_channels, 1>
    {
      constexpr auto num_channels =
          ImageIteratorTraits<ImageIterator>::num_channels;
      using out_vector_type = Matrix<double, num_channels, 1>;

      auto sum = out_vector_type::Zero().eval();
      auto count = 0ull;

      for (; first != last; ++first)
      {
        const auto tensor = to_cwh_tensor(*first);
        for (auto c = 0; c < num_channels; ++c)
          sum[c] += tensor[c].flat_array().template cast<double>().sum();
        count += first->size();
      };

      return sum / count;
    }

    //! @brief Compute the color sample covariance matrix.
    /*!
     * Read wikipedia article:
     * https://en.wikipedia.org/wiki/Sample_mean_and_covariance regarding
     * Bessel's
     * correction.
     *
     * This function is implemented with the naive two-pass algorithm.
     */
    template <typename ImageIterator>
    inline auto color_sample_covariance_matrix(
        ImageIterator first, ImageIterator last,
        const Matrix<double, ImageIteratorTraits<ImageIterator>::num_channels,
                     1>& mean) -> Matrix3d
    {
      constexpr auto num_channels =
          ImageIteratorTraits<ImageIterator>::num_channels;
      using out_matrix_type = Matrix<double, num_channels, num_channels>;

      auto cov = out_matrix_type::Zero().eval();
      auto count = 0ull;

      for (; first != last; ++first)
      {
        const auto tensor = to_cwh_tensor(*first);
        count += first->size();

        for (int i = 0; i < num_channels; ++i)
          for (int j = i; j < num_channels; ++j)
          {
            auto t_i = tensor[i].flat_array().template cast<double>() - mean(i);
            auto t_j = tensor[j].flat_array().template cast<double>() - mean(j);

            cov(i, j) += (t_i * t_j).sum();
          }
      }

      for (int i = 1; i < num_channels; ++i)
        for (int j = 0; j < i; ++j)
          cov(i, j) = cov(j, i);

      // Divide by (N-1), cf. Bessel's correction.
      return cov / (count - 1);
    }

  } /* namespace naive */

  inline namespace chan_golub_leveque {

    //! @brief Perform online covariance matrix update
    /*!
     * This function implements the method presented in:
     *
     * Updating Formulae and a Pairwise Algorithm for Computing Sample Variances
     * Tony F. Chan, Gene H. Golub, Randall J. Leveque, November 1979
     * STAN-CS-79-773
     *
     * N.B. we don't use the pairwise algorithm scheme yet.
     */
    template <typename ImageIterator>
    inline auto online_color_covariance(ImageIterator first, ImageIterator last)
        -> std::pair<
            typename ImageIteratorTraits<ImageIterator>::color_mean_type,
            typename ImageIteratorTraits<
                ImageIterator>::color_covariance_matrix_type>
    {
      using vector_type =
          typename ImageIteratorTraits<ImageIterator>::color_mean_type;
      using matrix_type = typename ImageIteratorTraits<
          ImageIterator>::color_covariance_matrix_type;

      constexpr auto num_channels =
          ImageIteratorTraits<ImageIterator>::num_channels;

      auto count = 0ull;
      auto sum = vector_type{};
      auto cov = matrix_type{};
      auto correction = matrix_type{};

      // Initialize the variables with the first image.
      {
        // Read the image as a tensor.
        const auto tensor = to_cwh_tensor(*first);

        // Initialize the number of colors count so far.
        count = first->size();

        // Initialize the sum.
        for (auto i = 0; i < num_channels; ++i)
          sum[i] = tensor[i].flat_array().template cast<double>().sum();

        // Initialize the covariance matrix

        // 1. Calculate the mean expression which is used in the covariance
        //    matrix.
        const auto mean = sum / count;

        // 2. For each entry of the covariance matrix, compute the necessary
        //    algebraic expressions used for the sum of products.
        for (auto i = 0; i < num_channels; ++i)
        {
          for (auto j = i; j < num_channels; ++j)
          {
            // 2a. Compute the expressions.
            const auto t_i =
                (tensor[i].flat_array().template cast<double>() - mean[i]);
            const auto t_j =
                (tensor[j].flat_array().template cast<double>() - mean[j]);

            // 2b. Compute the two residual expressions, which are zero in exact
            //     arithmetic computation but they increase the numerical
            //     stability.
            //
            //     This is Professor Ake Bjorck's trick (cf. paper for details).
            const auto t_i_res =
                (tensor[i].flat_array().template cast<double>() - mean[i])
                    .sum() /
                count;
            const auto t_j_res =
                (tensor[j].flat_array().template cast<double>() - mean[j])
                    .sum() /
                count;

            cov(i, j) = (t_i * t_j).sum() - t_i_res * t_j_res;
          }
        }

        for (int i = 1; i < num_channels; ++i)
          for (int j = 0; j < i; ++j)
            cov(i, j) = cov(j, i);
      }

      for (++first; first != last; ++first)
      {
        const auto tensor = to_cwh_tensor(*first);

        const auto count_i = first->size();

        auto sum_i = vector_type{};
        for (auto i = 0; i < num_channels; ++i)
          sum_i[i] = tensor[i].flat_array().template cast<double>().sum();

        const auto mean_i = sum_i / count_i;

        auto cov_i = matrix_type{};
        for (auto i = 0; i < num_channels; ++i)
        {
          for (auto j = i; j < num_channels; ++j)
          {
            const auto t_i =
                (tensor[i].flat_array().template cast<double>() - mean_i[i]);
            const auto t_j =
                (tensor[j].flat_array().template cast<double>() - mean_i[j]);

            // Bjorck's trick again.
            const auto t_i_res =
                (tensor[i].flat_array().template cast<double>() - mean_i[i])
                    .sum() /
                count_i;
            const auto t_j_res =
                (tensor[j].flat_array().template cast<double>() - mean_i[j])
                    .sum() /
                count_i;

            cov_i(i, j) = (t_i * t_j).sum() - t_i_res * t_j_res;

            correction(i, j) = (sum(i) * count_i / count - sum_i(i)) *
                               (sum(j) * count_i / count - sum_i(j)) * count /
                               count_i / (count + count_i);
          }
        }

        for (int i = 1; i < num_channels; ++i)
        {
          for (int j = 0; j < i; ++j)
          {
            cov_i(i, j) = cov_i(j, i);
            correction(i, j) = correction(j, i);
          }
        }

        count += count_i;
        sum += sum_i;
        cov += cov_i + correction;
      }

      return std::make_pair((sum / count).eval(), (cov / (count - 1)).eval());
    }

  } /* namespace chan_golub_leveque */

  //! @brief Perform PCA on the color data.
  template <typename _Matrix>
  inline auto color_pca(const _Matrix& covariance_matrix)
      -> std::pair<typename Eigen::JacobiSVD<_Matrix>::MatrixUType,
                   typename Eigen::JacobiSVD<_Matrix>::SingularValuesType>
  {
    auto svd = Eigen::JacobiSVD<_Matrix>{covariance_matrix, ComputeFullU};
    return std::make_pair(svd.matrixU(), svd.singularValues());
  }

} /* namespace Sara */
} /* namespace DO */
<commit_msg>MAINT: change inference of return type.<commit_after>// ========================================================================== //
// This file is part of Sara, a basic set of libraries in C++ for computer
// vision.
//
// Copyright (C) 2017 David Ok <david.ok8@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License v. 2.0. If a copy of the MPL was not distributed with this file,
// you can obtain one at http://mozilla.org/MPL/2.0/.
// ========================================================================== //

//! @file

#pragma once

#include <DO/Sara/Core/Tensor.hpp>

namespace DO { namespace Sara {

  //! @brief Image iterator trait class.
  template <typename Iterator>
  struct ImageIteratorTraits
  {
    using iterator = Iterator;
    using value_type = typename Iterator::value_type;
    using pixel_type = typename value_type::pixel_type;
    using channel_type = typename PixelTraits<pixel_type>::channel_type;

    static const int num_channels = PixelTraits<pixel_type>::num_channels;

    using color_mean_type = Matrix<double, num_channels, 1>;
    using color_covariance_matrix_type =
        Matrix<double, num_channels, num_channels>;
  };

  inline namespace naive {

    //! @brief Compute the sample color mean vector.
    //! The implementation is the naive textbook algorithm.
    template <typename ImageIterator>
    inline auto color_sample_mean_vector(ImageIterator first,
                                         ImageIterator last)
        -> typename ImageIteratorTraits<ImageIterator>::color_mean_type
    {
      constexpr auto num_channels =
          ImageIteratorTraits<ImageIterator>::num_channels;

      using out_vector_type =
          typename ImageIteratorTraits<ImageIterator>::color_mean_type;

      auto sum = out_vector_type::Zero().eval();
      auto count = 0ull;

      for (; first != last; ++first)
      {
        const auto tensor = to_cwh_tensor(*first);
        for (auto c = 0; c < num_channels; ++c)
          sum[c] += tensor[c].flat_array().template cast<double>().sum();
        count += first->size();
      };

      return sum / count;
    }

    //! @brief Compute the color sample covariance matrix.
    /*!
     * Read wikipedia article:
     * https://en.wikipedia.org/wiki/Sample_mean_and_covariance regarding
     * Bessel's
     * correction.
     *
     * This function is implemented with the naive two-pass algorithm.
     */
    template <typename ImageIterator>
    inline auto color_sample_covariance_matrix(
        ImageIterator first, ImageIterator last,
        const typename ImageIteratorTraits<ImageIterator>::color_mean_type&
            mean) ->
        typename ImageIteratorTraits<
            ImageIterator>::color_covariance_matrix_type
    {
      constexpr auto num_channels =
          ImageIteratorTraits<ImageIterator>::num_channels;

      using out_matrix_type = typename ImageIteratorTraits<
          ImageIterator>::color_covariance_matrix_type;

      auto cov = out_matrix_type::Zero().eval();
      auto count = 0ull;

      for (; first != last; ++first)
      {
        const auto tensor = to_cwh_tensor(*first);
        count += first->size();

        for (int i = 0; i < num_channels; ++i)
          for (int j = i; j < num_channels; ++j)
          {
            auto t_i = tensor[i].flat_array().template cast<double>() - mean(i);
            auto t_j = tensor[j].flat_array().template cast<double>() - mean(j);

            cov(i, j) += (t_i * t_j).sum();
          }
      }

      for (int i = 1; i < num_channels; ++i)
        for (int j = 0; j < i; ++j)
          cov(i, j) = cov(j, i);

      // Divide by (N-1), cf. Bessel's correction.
      return cov / (count - 1);
    }

  } /* namespace naive */

  inline namespace chan_golub_leveque {

    //! @brief Perform online covariance matrix update
    /*!
     * This function implements the method presented in:
     *
     * Updating Formulae and a Pairwise Algorithm for Computing Sample Variances
     * Tony F. Chan, Gene H. Golub, Randall J. Leveque, November 1979
     * STAN-CS-79-773
     *
     * N.B. we don't use the pairwise algorithm scheme yet.
     */
    template <typename ImageIterator>
    inline auto online_color_covariance(ImageIterator first, ImageIterator last)
        -> std::pair<
            typename ImageIteratorTraits<ImageIterator>::color_mean_type,
            typename ImageIteratorTraits<
                ImageIterator>::color_covariance_matrix_type>
    {
      using vector_type =
          typename ImageIteratorTraits<ImageIterator>::color_mean_type;
      using matrix_type = typename ImageIteratorTraits<
          ImageIterator>::color_covariance_matrix_type;

      constexpr auto num_channels =
          ImageIteratorTraits<ImageIterator>::num_channels;

      auto count = 0ull;
      auto sum = vector_type{};
      auto cov = matrix_type{};
      auto correction = matrix_type{};

      // Initialize the variables with the first image.
      {
        // Read the image as a tensor.
        const auto tensor = to_cwh_tensor(*first);

        // Initialize the number of colors count so far.
        count = first->size();

        // Initialize the sum.
        for (auto i = 0; i < num_channels; ++i)
          sum[i] = tensor[i].flat_array().template cast<double>().sum();

        // Initialize the covariance matrix

        // 1. Calculate the mean expression which is used in the covariance
        //    matrix.
        const auto mean = sum / count;

        // 2. For each entry of the covariance matrix, compute the necessary
        //    algebraic expressions used for the sum of products.
        for (auto i = 0; i < num_channels; ++i)
        {
          for (auto j = i; j < num_channels; ++j)
          {
            // 2a. Compute the expressions.
            const auto t_i =
                (tensor[i].flat_array().template cast<double>() - mean[i]);
            const auto t_j =
                (tensor[j].flat_array().template cast<double>() - mean[j]);

            // 2b. Compute the two residual expressions, which are zero in exact
            //     arithmetic computation but they increase the numerical
            //     stability.
            //
            //     This is Professor Ake Bjorck's trick (cf. paper for details).
            const auto t_i_res =
                (tensor[i].flat_array().template cast<double>() - mean[i])
                    .sum() /
                count;
            const auto t_j_res =
                (tensor[j].flat_array().template cast<double>() - mean[j])
                    .sum() /
                count;

            cov(i, j) = (t_i * t_j).sum() - t_i_res * t_j_res;
          }
        }

        for (int i = 1; i < num_channels; ++i)
          for (int j = 0; j < i; ++j)
            cov(i, j) = cov(j, i);
      }

      for (++first; first != last; ++first)
      {
        const auto tensor = to_cwh_tensor(*first);

        const auto count_i = first->size();

        auto sum_i = vector_type{};
        for (auto i = 0; i < num_channels; ++i)
          sum_i[i] = tensor[i].flat_array().template cast<double>().sum();

        const auto mean_i = sum_i / count_i;

        auto cov_i = matrix_type{};
        for (auto i = 0; i < num_channels; ++i)
        {
          for (auto j = i; j < num_channels; ++j)
          {
            const auto t_i =
                (tensor[i].flat_array().template cast<double>() - mean_i[i]);
            const auto t_j =
                (tensor[j].flat_array().template cast<double>() - mean_i[j]);

            // Bjorck's trick again.
            const auto t_i_res =
                (tensor[i].flat_array().template cast<double>() - mean_i[i])
                    .sum() /
                count_i;
            const auto t_j_res =
                (tensor[j].flat_array().template cast<double>() - mean_i[j])
                    .sum() /
                count_i;

            cov_i(i, j) = (t_i * t_j).sum() - t_i_res * t_j_res;

            correction(i, j) = (sum(i) * count_i / count - sum_i(i)) *
                               (sum(j) * count_i / count - sum_i(j)) * count /
                               count_i / (count + count_i);
          }
        }

        for (int i = 1; i < num_channels; ++i)
        {
          for (int j = 0; j < i; ++j)
          {
            cov_i(i, j) = cov_i(j, i);
            correction(i, j) = correction(j, i);
          }
        }

        count += count_i;
        sum += sum_i;
        cov += cov_i + correction;
      }

      return std::make_pair((sum / count).eval(), (cov / (count - 1)).eval());
    }

  } /* namespace chan_golub_leveque */

  //! @brief Perform PCA on the color data.
  template <typename _Matrix>
  inline auto color_pca(const _Matrix& covariance_matrix)
      -> std::pair<typename Eigen::JacobiSVD<_Matrix>::MatrixUType,
                   typename Eigen::JacobiSVD<_Matrix>::SingularValuesType>
  {
    auto svd = Eigen::JacobiSVD<_Matrix>{covariance_matrix, ComputeFullU};
    return std::make_pair(svd.matrixU(), svd.singularValues());
  }

} /* namespace Sara */
} /* namespace DO */
<|endoftext|>


<commit_before>// This file is part of the "x0" project, http://xzero.io/
//   (c) 2009-2014 Christian Parpart <trapni@gmail.com>
//
// Licensed under the MIT License (the "License"); you may not use this
// file except in compliance with the License. You may obtain a copy of
// the License at: http://opensource.org/licenses/MIT

#include <xzero/executor/ThreadedExecutor.h>
#include <xzero/sysconfig.h>
#include <memory>
#include <algorithm>
#include <limits>
#include <pthread.h>

namespace xzero {

#if 0 //!defined(NDEBUG)
static std::mutex m;
#define TRACE(msg...)  do { \
    m.lock(); \
    printf("ThreadedExecutor: " msg); \
    printf("\n"); \
    m.unlock(); \
  } while (0);
#else
#define TRACE(msg...) do { } while (0)
#endif

ThreadedExecutor::ThreadedExecutor() : threads_() {
}

ThreadedExecutor::~ThreadedExecutor() {
  joinAll();
}

static std::string getThreadName(pthread_t tid) {
  char name[16];
  name[0] = '\0';
  pthread_getname_np(tid, name, sizeof(name));
  return name;
}

void ThreadedExecutor::joinAll() {
  for (;;) {
    pthread_t tid = 0;
    {
      TRACE("joinAll: getting lock for getting TID");
      std::lock_guard<std::mutex> lock(mutex_);
      if (threads_.empty())
        break;

      tid = threads_.front();
      threads_.pop_front();
    }
    TRACE("joinAll: join(%lu) %s", tid, getThreadName(tid).c_str());
    pthread_join(tid, nullptr);
  }
  TRACE("joinAll: done");
}

void* ThreadedExecutor::launchme(void* ptr) {
  TRACE("launchme[%d](%p) enter", pthread_self(), ptr);
  std::unique_ptr<Executor::Task> task(reinterpret_cast<Executor::Task*>(ptr));
  (*task)();
  TRACE("launchme[%d](%p) leave", pthread_self(), ptr);
  return nullptr;
}

void ThreadedExecutor::execute(const std::string& name, Task&& task) {
  pthread_t tid;
  pthread_create(&tid, NULL, &launchme, new Task{std::move(task)});

#if !defined(__APPLE__)
  // OS/x doesn't support setting thread names for other threads
  // TODO: pass thread name to target thread and set it inside
  pthread_setname_np(tid, name.c_str());
#endif

  std::lock_guard<std::mutex> lock(mutex_);
  threads_.push_back(tid);
}

void ThreadedExecutor::execute(Task&& task) {
  pthread_t tid = 0;
  //pthread_create(&tid, NULL, &launchme, new Task{std::move(task)});
  pthread_create(&tid, NULL, &launchme, new Task([this, tid, task]{
    task();
    {
      TRACE("task %s finished. getting lock for cleanup", getThreadName(pthread_self()).c_str());
      std::lock_guard<std::mutex> lock(mutex_);
      pthread_detach(tid);
      auto i = std::find(threads_.begin(), threads_.end(), tid);
      if (i != threads_.end()) {
        threads_.erase(i);
      }
    }
  }));
  std::lock_guard<std::mutex> lock(mutex_);
  threads_.push_back(tid);
}

size_t ThreadedExecutor::maxConcurrency() const noexcept {
  return std::numeric_limits<size_t>::max();
}

std::string ThreadedExecutor::toString() const {
  char buf[32];
  snprintf(buf, sizeof(buf), "ThreadedExecutor@%p", this);
  return buf;
}

} // namespace xzero
<commit_msg>ThreadedExecutor: fixes a bug; how could that have ever worked, d'oh.<commit_after>// This file is part of the "x0" project, http://xzero.io/
//   (c) 2009-2014 Christian Parpart <trapni@gmail.com>
//
// Licensed under the MIT License (the "License"); you may not use this
// file except in compliance with the License. You may obtain a copy of
// the License at: http://opensource.org/licenses/MIT

#include <xzero/executor/ThreadedExecutor.h>
#include <xzero/sysconfig.h>
#include <memory>
#include <algorithm>
#include <limits>
#include <pthread.h>

namespace xzero {

#if 0 //!defined(NDEBUG)
static std::mutex m;
#define TRACE(msg...)  do { \
    m.lock(); \
    printf("ThreadedExecutor: " msg); \
    printf("\n"); \
    m.unlock(); \
  } while (0);
#else
#define TRACE(msg...) do { } while (0)
#endif

ThreadedExecutor::ThreadedExecutor() : threads_() {
}

ThreadedExecutor::~ThreadedExecutor() {
  joinAll();
}

static std::string getThreadName(pthread_t tid) {
  char name[16];
  name[0] = '\0';
  pthread_getname_np(tid, name, sizeof(name));
  return name;
}

void ThreadedExecutor::joinAll() {
  for (;;) {
    pthread_t tid = 0;
    {
      TRACE("joinAll: getting lock for getting TID");
      std::lock_guard<std::mutex> lock(mutex_);
      if (threads_.empty())
        break;

      tid = threads_.front();
      threads_.pop_front();
    }
    TRACE("joinAll: join(%lu) %s", tid, getThreadName(tid).c_str());
    pthread_join(tid, nullptr);
  }
  TRACE("joinAll: done");
}

void* ThreadedExecutor::launchme(void* ptr) {
  TRACE("launchme[%d](%p) enter", pthread_self(), ptr);
  std::unique_ptr<Executor::Task> task(reinterpret_cast<Executor::Task*>(ptr));
  (*task)();
  TRACE("launchme[%d](%p) leave", pthread_self(), ptr);
  return nullptr;
}

void ThreadedExecutor::execute(const std::string& name, Task&& task) {
  pthread_t tid;
  pthread_create(&tid, NULL, &launchme, new Task{std::move(task)});

#if !defined(__APPLE__)
  // OS/x doesn't support setting thread names for other threads
  // TODO: pass thread name to target thread and set it inside
  pthread_setname_np(tid, name.c_str());
#endif

  std::lock_guard<std::mutex> lock(mutex_);
  threads_.push_back(tid);
}

void ThreadedExecutor::execute(Task&& task) {
  pthread_t tid = 0;
  //pthread_create(&tid, NULL, &launchme, new Task{std::move(task)});
  pthread_create(&tid, NULL, &launchme, new Task([this, task]{
    pthread_t tid = pthread_self();
    task();
    {
      TRACE("task %s finished. getting lock for cleanup", getThreadName(tid).c_str());
      std::lock_guard<std::mutex> lock(mutex_);
      pthread_detach(tid);
      auto i = std::find(threads_.begin(), threads_.end(), tid);
      if (i != threads_.end()) {
        threads_.erase(i);
      }
    }
  }));
  std::lock_guard<std::mutex> lock(mutex_);
  threads_.push_back(tid);
}

size_t ThreadedExecutor::maxConcurrency() const noexcept {
  return std::numeric_limits<size_t>::max();
}

std::string ThreadedExecutor::toString() const {
  char buf[32];
  snprintf(buf, sizeof(buf), "ThreadedExecutor@%p", this);
  return buf;
}

} // namespace xzero
<|endoftext|>


<commit_before>#include "log_writer.h"
#include <fcntl.h>
#include <string.h>

#include <mathlib/mathlib.h>

namespace px4
{
namespace logger
{
constexpr size_t LogWriter::_min_write_chunk;

LogWriter::LogWriter(size_t buffer_size) :
	_buffer_size(math::max(buffer_size, _min_write_chunk))
{
	pthread_mutex_init(&_mtx, nullptr);
	pthread_cond_init(&_cv, nullptr);
	/* allocate write performance counters */
	perf_write = perf_alloc(PC_ELAPSED, "sd write");
	perf_fsync = perf_alloc(PC_ELAPSED, "sd fsync");
}

bool LogWriter::init()
{
	if (_buffer) {
		return true;
	}

	_buffer = new uint8_t[_buffer_size];

	return _buffer;
}

LogWriter::~LogWriter()
{
	if (_buffer) {
		delete[] _buffer;
	}
}

void LogWriter::start_log(const char *filename)
{
	::strncpy(_filename, filename, sizeof(_filename));
	_fd = ::open(_filename, O_CREAT | O_WRONLY, PX4_O_MODE_666);

	if (_fd < 0) {
		PX4_WARN("can't open log file %s", _filename);
		_should_run = false;
		return;

	} else {
		PX4_WARN("opened log file: %s", _filename);
		_should_run = true;
		_running = true;
	}

	// Clear buffer and counters
	_head = 0;
	_count = 0;
	_total_written = 0;
	notify();
}

void LogWriter::stop_log()
{
	_should_run = false;
	notify();
}

int LogWriter::thread_start(pthread_t &thread)
{
	pthread_attr_t thr_attr;
	pthread_attr_init(&thr_attr);

	sched_param param;
	/* low priority, as this is expensive disk I/O */
	param.sched_priority = SCHED_PRIORITY_DEFAULT - 40;
	(void)pthread_attr_setschedparam(&thr_attr, &param);

	pthread_attr_setstacksize(&thr_attr, 1024);

	int ret = pthread_create(&thread, &thr_attr, &LogWriter::run_helper, this);
	pthread_attr_destroy(&thr_attr);

	return ret;
}

void LogWriter::thread_stop()
{
	// this will terminate the main loop of the writer thread
	_exit_thread = true;
	_should_run = false;
}

void *LogWriter::run_helper(void *context)
{
	px4_prctl(PR_SET_NAME, "log_writer", px4_getpid());

	reinterpret_cast<LogWriter *>(context)->run();
	return nullptr;
}

void LogWriter::run()
{
	while (!_exit_thread) {
		// Outer endless loop
		// Wait for _should_run flag
		while (!_exit_thread) {
			bool start = false;
			pthread_mutex_lock(&_mtx);
			pthread_cond_wait(&_cv, &_mtx);
			start = _should_run;
			pthread_mutex_unlock(&_mtx);

			if (start) {
				break;
			}
		}

		int poll_count = 0;
		int written = 0;

		while (true) {
			size_t available = 0;
			void *read_ptr = nullptr;
			bool is_part = false;

			/* lock _buffer
			 * wait for sufficient data, cycle on notify()
			 */
			pthread_mutex_lock(&_mtx);

			while (true) {
				available = get_read_ptr(&read_ptr, &is_part);

				/* if sufficient data available or partial read or terminating, exit this wait loop */
				if ((available >= _min_write_chunk) || is_part || !_should_run) {
					/* GOTO end of block */
					break;
				}

				/* wait for a call to notify()
				 * this call unlocks the mutex while waiting, and returns with the mutex locked
				 */
				pthread_cond_wait(&_cv, &_mtx);
			}

			pthread_mutex_unlock(&_mtx);

			if (available > 0) {
				perf_begin(perf_write);
				written = ::write(_fd, read_ptr, available);
				perf_end(perf_write);

				/* call fsync periodically to minimize potential loss of data */
				if (++poll_count >= 100) {
					perf_begin(perf_fsync);
					::fsync(_fd);
					perf_end(perf_fsync);
					poll_count = 0;
				}

				if (written < 0) {
					PX4_WARN("error writing log file");
					_should_run = false;
					/* GOTO end of block */
					break;
				}

				pthread_mutex_lock(&_mtx);
				/* subtract bytes written from number in _buffer (_count -= written) */
				mark_read(written);
				pthread_mutex_unlock(&_mtx);

				_total_written += written;
			}

			if (_running && !_should_run && written == static_cast<int>(available) && !is_part) {
				// Stop only when all data written
				_running = false;
				_head = 0;
				_count = 0;

				int res = ::close(_fd);
				_fd = -1;

				if (res) {
					PX4_WARN("error closing log file");

				} else {
					PX4_WARN("closed logfile: %s, bytes written: %zu", _filename, _total_written);
				}

				break;
			}
		}
	}
}

bool LogWriter::write(void *ptr, size_t size)
{

	// Bytes available to write
	size_t available = _buffer_size - _count;

	if (size > available) {
		// buffer overflow
		return false;
	}

	size_t n = _buffer_size - _head;	// bytes to end of the buffer

	uint8_t *buffer_c = reinterpret_cast<uint8_t *>(ptr);

	if (size > n) {
		// Message goes over the end of the buffer
		memcpy(&(_buffer[_head]), buffer_c, n);
		_head = 0;

	} else {
		n = 0;
	}

	// now: n = bytes already written
	size_t p = size - n;	// number of bytes to write
	memcpy(&(_buffer[_head]), &(buffer_c[n]), p);
	_head = (_head + p) % _buffer_size;
	_count += size;
	return true;
}

size_t LogWriter::get_read_ptr(void **ptr, bool *is_part)
{
	// bytes available to read
	int read_ptr = _head - _count;

	if (read_ptr < 0) {
		read_ptr += _buffer_size;
		*ptr = &_buffer[read_ptr];
		*is_part = true;
		return _buffer_size - read_ptr;

	} else {
		*ptr = &_buffer[read_ptr];
		*is_part = false;
		return _count;
	}
}

}
}
<commit_msg>logger: make sure the buffer is at least 300B larger than _min_write_chunk<commit_after>#include "log_writer.h"
#include <fcntl.h>
#include <string.h>

#include <mathlib/mathlib.h>

namespace px4
{
namespace logger
{
constexpr size_t LogWriter::_min_write_chunk;

LogWriter::LogWriter(size_t buffer_size) :
	//We always write larger chunks (orb messages) to the buffer, so the buffer
	//needs to be larger than the minimum write chunk (300 is somewhat arbitrary)
	_buffer_size(math::max(buffer_size, _min_write_chunk + 300))
{
	pthread_mutex_init(&_mtx, nullptr);
	pthread_cond_init(&_cv, nullptr);
	/* allocate write performance counters */
	perf_write = perf_alloc(PC_ELAPSED, "sd write");
	perf_fsync = perf_alloc(PC_ELAPSED, "sd fsync");
}

bool LogWriter::init()
{
	if (_buffer) {
		return true;
	}

	_buffer = new uint8_t[_buffer_size];

	return _buffer;
}

LogWriter::~LogWriter()
{
	if (_buffer) {
		delete[] _buffer;
	}
}

void LogWriter::start_log(const char *filename)
{
	::strncpy(_filename, filename, sizeof(_filename));
	_fd = ::open(_filename, O_CREAT | O_WRONLY, PX4_O_MODE_666);

	if (_fd < 0) {
		PX4_WARN("can't open log file %s", _filename);
		_should_run = false;
		return;

	} else {
		PX4_WARN("opened log file: %s", _filename);
		_should_run = true;
		_running = true;
	}

	// Clear buffer and counters
	_head = 0;
	_count = 0;
	_total_written = 0;
	notify();
}

void LogWriter::stop_log()
{
	_should_run = false;
	notify();
}

int LogWriter::thread_start(pthread_t &thread)
{
	pthread_attr_t thr_attr;
	pthread_attr_init(&thr_attr);

	sched_param param;
	/* low priority, as this is expensive disk I/O */
	param.sched_priority = SCHED_PRIORITY_DEFAULT - 40;
	(void)pthread_attr_setschedparam(&thr_attr, &param);

	pthread_attr_setstacksize(&thr_attr, 1024);

	int ret = pthread_create(&thread, &thr_attr, &LogWriter::run_helper, this);
	pthread_attr_destroy(&thr_attr);

	return ret;
}

void LogWriter::thread_stop()
{
	// this will terminate the main loop of the writer thread
	_exit_thread = true;
	_should_run = false;
}

void *LogWriter::run_helper(void *context)
{
	px4_prctl(PR_SET_NAME, "log_writer", px4_getpid());

	reinterpret_cast<LogWriter *>(context)->run();
	return nullptr;
}

void LogWriter::run()
{
	while (!_exit_thread) {
		// Outer endless loop
		// Wait for _should_run flag
		while (!_exit_thread) {
			bool start = false;
			pthread_mutex_lock(&_mtx);
			pthread_cond_wait(&_cv, &_mtx);
			start = _should_run;
			pthread_mutex_unlock(&_mtx);

			if (start) {
				break;
			}
		}

		int poll_count = 0;
		int written = 0;

		while (true) {
			size_t available = 0;
			void *read_ptr = nullptr;
			bool is_part = false;

			/* lock _buffer
			 * wait for sufficient data, cycle on notify()
			 */
			pthread_mutex_lock(&_mtx);

			while (true) {
				available = get_read_ptr(&read_ptr, &is_part);

				/* if sufficient data available or partial read or terminating, exit this wait loop */
				if ((available >= _min_write_chunk) || is_part || !_should_run) {
					/* GOTO end of block */
					break;
				}

				/* wait for a call to notify()
				 * this call unlocks the mutex while waiting, and returns with the mutex locked
				 */
				pthread_cond_wait(&_cv, &_mtx);
			}

			pthread_mutex_unlock(&_mtx);

			if (available > 0) {
				perf_begin(perf_write);
				written = ::write(_fd, read_ptr, available);
				perf_end(perf_write);

				/* call fsync periodically to minimize potential loss of data */
				if (++poll_count >= 100) {
					perf_begin(perf_fsync);
					::fsync(_fd);
					perf_end(perf_fsync);
					poll_count = 0;
				}

				if (written < 0) {
					PX4_WARN("error writing log file");
					_should_run = false;
					/* GOTO end of block */
					break;
				}

				pthread_mutex_lock(&_mtx);
				/* subtract bytes written from number in _buffer (_count -= written) */
				mark_read(written);
				pthread_mutex_unlock(&_mtx);

				_total_written += written;
			}

			if (_running && !_should_run && written == static_cast<int>(available) && !is_part) {
				// Stop only when all data written
				_running = false;
				_head = 0;
				_count = 0;

				int res = ::close(_fd);
				_fd = -1;

				if (res) {
					PX4_WARN("error closing log file");

				} else {
					PX4_WARN("closed logfile: %s, bytes written: %zu", _filename, _total_written);
				}

				break;
			}
		}
	}
}

bool LogWriter::write(void *ptr, size_t size)
{

	// Bytes available to write
	size_t available = _buffer_size - _count;

	if (size > available) {
		// buffer overflow
		return false;
	}

	size_t n = _buffer_size - _head;	// bytes to end of the buffer

	uint8_t *buffer_c = reinterpret_cast<uint8_t *>(ptr);

	if (size > n) {
		// Message goes over the end of the buffer
		memcpy(&(_buffer[_head]), buffer_c, n);
		_head = 0;

	} else {
		n = 0;
	}

	// now: n = bytes already written
	size_t p = size - n;	// number of bytes to write
	memcpy(&(_buffer[_head]), &(buffer_c[n]), p);
	_head = (_head + p) % _buffer_size;
	_count += size;
	return true;
}

size_t LogWriter::get_read_ptr(void **ptr, bool *is_part)
{
	// bytes available to read
	int read_ptr = _head - _count;

	if (read_ptr < 0) {
		read_ptr += _buffer_size;
		*ptr = &_buffer[read_ptr];
		*is_part = true;
		return _buffer_size - read_ptr;

	} else {
		*ptr = &_buffer[read_ptr];
		*is_part = false;
		return _count;
	}
}

}
}
<|endoftext|>


<commit_before><commit_msg>Broken DofObject redistribution triggers these asserts - let's define an intermediate variable to make them easier to probe in gdb<commit_after><|endoftext|>


<commit_before>/*
   Copyright (c) 2009-2014, Jack Poulson
   All rights reserved.

   This file is part of Elemental and is under the BSD 2-Clause License,
   which can be found in the LICENSE file in the root directory, or at
   http://opensource.org/licenses/BSD-2-Clause
*/
#pragma once
#ifndef ELEM_MULTISHIFTHESSSOLVE_HPP
#define ELEM_MULTISHIFTHESSSOLVE_HPP

// NOTE: These algorithms are adaptations and/or extensions of Alg. 2 from
//       Greg Henry's "The shifted Hessenberg system solve computation".
//       It is important to note that the Given's rotation definition in
//       said paper is the adjoint of the LAPACK definition (as well as
//       leaving out a conjugation necessary for the complex case).

namespace elem {
namespace mshs {

template<typename F>
inline void
UN( F alpha, const Matrix<F>& H, const Matrix<F>& shifts, Matrix<F>& X )
{
    DEBUG_ONLY(CallStackEntry cse("mshs::UN"))
    Scale( alpha, X );

    const Int m = X.Height();
    const Int n = X.Width();
    if( m == 0 )
        return;

    // Initialize storage for Givens rotations
    typedef Base<F> Real;
    Matrix<Real> C(m,n);
    Matrix<F> S(m,n);

    // Initialize the workspace for shifted columns of H
    Matrix<F> W(m,n);
    for( Int j=0; j<n; ++j )
    {
        MemCopy( W.Buffer(0,j), H.LockedBuffer(0,m-1), m );
        W.Update( m-1, j, -shifts.Get(j,0) );
    }

    // Simulataneously factor and solve against R
    for( Int k=m-1; k>0; --k )
    {
        auto hT = LockedView( H, 0, k-1, k-1, 1 );
        const F etakkm1 = H.Get(k,k-1);
        const F etakm1km1 = H.Get(k-1,k-1);
        for( Int j=0; j<n; ++j )
        {
            Real c;
            F s, rho;
            lapack::ComputeGivens( W.Get(k,j), etakkm1, &c, &s, &rho );
            C.Set( k, j, c );
            S.Set( k, j, s );

            X.Set( k, j, X.Get(k,j)/(c*W.Get(k,j)+s*etakkm1) );

            auto xT = View( X, 0, j, k-1, 1 );
            auto wT = View( W, 0, j, k-1, 1 );

            const F xc = X.Get(k,j)*c;
            const F xs  = X.Get(k,j)*s;
            Axpy( -xc, wT, xT );
            Axpy( -xs, hT, xT );

            Scale( -Conj(s), wT );
            Axpy( c, hT, wT );

            const F mu = shifts.Get( j, 0 );
            X.Update( k-1, j, -xc*W.Get(k-1,j)-xs*(etakm1km1-mu) );
            W.Set(    k-1, j, c*(etakm1km1-mu)-Conj(s)*W.Get(k-1,j) );
        }
    }
    for( Int j=0; j<n; ++j )
        X.Set( 0, j, X.Get(0,j)/W.Get(0,j) );

    // Solve against Q
    Matrix<F> t1(n,1), t2(n,1);
    for( Int j=0; j<n; ++j )
        t1.Set( j, 0, X.Get(0,j) );
    for( Int k=1; k<m; ++k )
    {
        for( Int j=0; j<n; ++j )
        {
            t2.Set( j,   0, X.Get(k,j)                                    );
            X.Set(  k-1, j, C.Get(k,j)*t1.Get(j,0)+S.Get(k,j)*t2.Get(j,0) );
            t1.Set( j,   0, C.Get(k,j)*t2.Get(j,0)-
                            Conj(S.Get(k,j))*t1.Get(j,0) );
        }
    }
    for( Int j=0; j<n; ++j )
        X.Set( m-1, j, t1.Get(j,0) );
}

// NOTE: A [VC,* ] distribution might be most appropriate for the
//       upper-Hessenberg matrix H since whole columns will need to be formed
//       on every process and this distribution will keep the communication
//       balanced.
template<typename F,Dist UH,Dist VH,Dist UX,Dist VX>
inline void
UN
( F alpha, const DistMatrix<F,UH,VH>& H, const DistMatrix<F,VX,STAR>& shifts,
  DistMatrix<F,UX,VX>& X )
{
    DEBUG_ONLY(CallStackEntry cse("mshs::UN"))
    Scale( alpha, X );

    const Grid& g = H.Grid();

    const Int m = X.Height();
    const Int n = X.Width();
    for( Int k=m-1; k>0; --k )
    {
        LogicError("This routine is not yet written");
    }
}

// TODO: UT, LN, and LT

} // namespace mshs

template<typename F>
inline void
MultiShiftHessSolve
( UpperOrLower uplo, Orientation orientation,
  F alpha, const Matrix<F>& H, const Matrix<F>& shifts, Matrix<F>& X )
{
    DEBUG_ONLY(CallStackEntry cse("MultiShiftHessSolve"))
    if( uplo == UPPER )
    {
        if( orientation == NORMAL )
            mshs::UN( alpha, H, shifts, X );
        else
            LogicError("This option is not yet supported");
    }
    else
        LogicError("This option is not yet supported");
}

template<typename F>
inline void
MultiShiftHessSolve
( UpperOrLower uplo, Orientation orientation,
  F alpha, const DistMatrix<F,VC,STAR>& H, const DistMatrix<F,VR,STAR>& shifts,
  DistMatrix<F>& X )
{
    DEBUG_ONLY(CallStackEntry cse("MultiShiftHessSolve"))
    if( uplo == UPPER )
    {
        if( orientation == NORMAL )
            mshs::UN( alpha, H, shifts, X );
        else
            LogicError("This option is not yet supported");
    }
    else
        LogicError("This option is not yet supported");
}

} // namespace elem

#endif // ifndef ELEM_MULTISHIFTHESSSOLVE_HPP
<commit_msg>Adding preliminary implementation of a distributed multi-shift Hessenberg solver.<commit_after>/*
   Copyright (c) 2009-2014, Jack Poulson
   All rights reserved.

   This file is part of Elemental and is under the BSD 2-Clause License,
   which can be found in the LICENSE file in the root directory, or at
   http://opensource.org/licenses/BSD-2-Clause
*/
#pragma once
#ifndef ELEM_MULTISHIFTHESSSOLVE_HPP
#define ELEM_MULTISHIFTHESSSOLVE_HPP

// NOTE: These algorithms are adaptations and/or extensions of Alg. 2 from
//       Greg Henry's "The shifted Hessenberg system solve computation".
//       It is important to note that the Given's rotation definition in
//       said paper is the adjoint of the LAPACK definition (as well as
//       leaving out a conjugation necessary for the complex case).

namespace elem {
namespace mshs {

template<typename F>
inline void
UN( F alpha, const Matrix<F>& H, const Matrix<F>& shifts, Matrix<F>& X )
{
    DEBUG_ONLY(CallStackEntry cse("mshs::UN"))
    Scale( alpha, X );

    const Int m = X.Height();
    const Int n = X.Width();
    if( m == 0 )
        return;

    // Initialize storage for Givens rotations
    typedef Base<F> Real;
    Matrix<Real> C(m,n);
    Matrix<F> S(m,n);

    // Initialize the workspace for shifted columns of H
    Matrix<F> W(m,n);
    for( Int j=0; j<n; ++j )
    {
        MemCopy( W.Buffer(0,j), H.LockedBuffer(0,m-1), m );
        W.Update( m-1, j, -shifts.Get(j,0) );
    }

    // Simulataneously factor and solve against R
    for( Int k=m-1; k>0; --k )
    {
        auto hT = LockedView( H, 0, k-1, k-1, 1 );
        const F etakkm1 = H.Get(k,k-1);
        const F etakm1km1 = H.Get(k-1,k-1);
        for( Int j=0; j<n; ++j )
        {
            Real c;
            F s, rho;
            lapack::ComputeGivens( W.Get(k,j), etakkm1, &c, &s, &rho );
            C.Set( k, j, c );
            S.Set( k, j, s );

            X.Set( k, j, X.Get(k,j)/(c*W.Get(k,j)+s*etakkm1) );

            auto xT = View( X, 0, j, k-1, 1 );
            auto wT = View( W, 0, j, k-1, 1 );

            const F xc = X.Get(k,j)*c;
            const F xs  = X.Get(k,j)*s;
            Axpy( -xc, wT, xT );
            Axpy( -xs, hT, xT );

            Scale( -Conj(s), wT );
            Axpy( c, hT, wT );

            const F mu = shifts.Get( j, 0 );
            X.Update( k-1, j, -xc*W.Get(k-1,j)-xs*(etakm1km1-mu) );
            W.Set(    k-1, j, c*(etakm1km1-mu)-Conj(s)*W.Get(k-1,j) );
        }
    }
    for( Int j=0; j<n; ++j )
        X.Set( 0, j, X.Get(0,j)/W.Get(0,j) );

    // Solve against Q
    Matrix<F> t1(n,1), t2(n,1);
    for( Int j=0; j<n; ++j )
        t1.Set( j, 0, X.Get(0,j) );
    for( Int k=1; k<m; ++k )
    {
        for( Int j=0; j<n; ++j )
        {
            t2.Set( j,   0, X.Get(k,j)                                    );
            X.Set(  k-1, j, C.Get(k,j)*t1.Get(j,0)+S.Get(k,j)*t2.Get(j,0) );
            t1.Set( j,   0, C.Get(k,j)*t2.Get(j,0)-
                            Conj(S.Get(k,j))*t1.Get(j,0) );
        }
    }
    for( Int j=0; j<n; ++j )
        X.Set( m-1, j, t1.Get(j,0) );
}

// NOTE: A [VC,* ] distribution might be most appropriate for the
//       upper-Hessenberg matrix H since whole columns will need to be formed
//       on every process and this distribution will keep the communication
//       balanced.
template<typename F,Dist UH,Dist VH,Dist VX>
inline void
UN
( F alpha, const DistMatrix<F,UH,VH>& H, const DistMatrix<F,VX,STAR>& shifts,
  DistMatrix<F,STAR,VX>& X )
{
    DEBUG_ONLY(
        CallStackEntry cse("mshs::UN");
        if( shifts.ColAlign() != X.RowAlign() )
            LogicError("shifts and X are not aligned");
    )
    Scale( alpha, X );

    const Int m = X.Height();
    const Int n = X.Width();
    const Int nLoc = X.LocalWidth();
    if( m == 0 )
        return;

    // Initialize storage for Givens rotations
    typedef Base<F> Real;
    const Grid& g = H.Grid();
    DistMatrix<Real,STAR,VX> C(g);
    DistMatrix<F,STAR,VX> S(g);
    C.AlignWith( X );
    S.AlignWith( X );
    C.Resize( m, n );
    S.Resize( m, n );

    // Initialize the workspace for shifted columns of H
    DistMatrix<F,STAR,VX> W(m,n,g);
    {
        auto hLast = LockedView( H, 0, m-1, m, 1 );
        DistMatrix<F,STAR,STAR> hLast_STAR_STAR( hLast );
        for( Int jLoc=0; jLoc<nLoc; ++jLoc )
        {
            MemCopy( W.Buffer(0,jLoc), hLast_STAR_STAR.LockedBuffer(), m );
            W.UpdateLocal( m-1, jLoc, -shifts.GetLocal(jLoc,0) );
        }
    }

    // Simulataneously factor and solve against R
    DistMatrix<F,STAR,STAR> hT_STAR_STAR(g);
    for( Int k=m-1; k>0; --k )
    {
        auto hT = LockedView( H, 0, k-1, k-1, 1 );
        hT_STAR_STAR = hT;
        const F etakkm1 = H.Get(k,k-1);
        const F etakm1km1 = H.Get(k-1,k-1);
        for( Int jLoc=0; jLoc<nLoc; ++jLoc )
        {
            Real c;
            F s, rho;
            lapack::ComputeGivens( W.GetLocal(k,jLoc), etakkm1, &c, &s, &rho );
            C.SetLocal( k, jLoc, c );
            S.SetLocal( k, jLoc, s );

            X.SetLocal
            ( k, jLoc, X.GetLocal(k,jLoc)/(c*W.GetLocal(k,jLoc)+s*etakkm1) );

            auto xT = View( X.Matrix(), 0, jLoc, k-1, 1 );
            auto wT = View( W.Matrix(), 0, jLoc, k-1, 1 );

            const F xc = X.GetLocal(k,jLoc)*c;
            const F xs  = X.GetLocal(k,jLoc)*s;
            Axpy( -xc, wT, xT );
            Axpy( -xs, hT_STAR_STAR.LockedMatrix(), xT );

            Scale( -Conj(s), wT );
            Axpy( c, hT_STAR_STAR.LockedMatrix(), wT );

            const F mu = shifts.GetLocal( jLoc, 0 );
            X.UpdateLocal
            ( k-1, jLoc, -xc*W.GetLocal(k-1,jLoc)-xs*(etakm1km1-mu) );
            W.SetLocal
            ( k-1, jLoc, c*(etakm1km1-mu)-Conj(s)*W.GetLocal(k-1,jLoc) );
        }
    }
    for( Int jLoc=0; jLoc<nLoc; ++jLoc )
        X.SetLocal( 0, jLoc, X.GetLocal(0,jLoc)/W.GetLocal(0,jLoc) );

    // Solve against Q
    Matrix<F> t1(nLoc,1), t2(nLoc,1);
    for( Int jLoc=0; jLoc<nLoc; ++jLoc )
        t1.Set( jLoc, 0, X.GetLocal(0,jLoc) );
    for( Int k=1; k<m; ++k )
    {
        for( Int jLoc=0; jLoc<nLoc; ++jLoc )
        {
            t2.Set( jLoc, 0, X.GetLocal(k,jLoc) );
            X.SetLocal( k-1, jLoc, C.GetLocal(k,jLoc)*t1.Get(jLoc,0)+
                                   S.GetLocal(k,jLoc)*t2.Get(jLoc,0) );
            t1.Set( jLoc, 0, C.GetLocal(k,jLoc)*t2.Get(jLoc,0)-
                             Conj(S.GetLocal(k,jLoc))*t1.Get(jLoc,0) );
        }
    }
    for( Int jLoc=0; jLoc<nLoc; ++jLoc )
        X.SetLocal( m-1, jLoc, t1.Get(jLoc,0) );
}

// TODO: UT, LN, and LT

} // namespace mshs

template<typename F>
inline void
MultiShiftHessSolve
( UpperOrLower uplo, Orientation orientation,
  F alpha, const Matrix<F>& H, const Matrix<F>& shifts, Matrix<F>& X )
{
    DEBUG_ONLY(CallStackEntry cse("MultiShiftHessSolve"))
    if( uplo == UPPER )
    {
        if( orientation == NORMAL )
            mshs::UN( alpha, H, shifts, X );
        else
            LogicError("This option is not yet supported");
    }
    else
        LogicError("This option is not yet supported");
}

template<typename F>
inline void
MultiShiftHessSolve
( UpperOrLower uplo, Orientation orientation,
  F alpha, const DistMatrix<F,VC,STAR>& H, const DistMatrix<F,VR,STAR>& shifts,
  DistMatrix<F>& X )
{
    DEBUG_ONLY(CallStackEntry cse("MultiShiftHessSolve"))
    if( uplo == UPPER )
    {
        if( orientation == NORMAL )
            mshs::UN( alpha, H, shifts, X );
        else
            LogicError("This option is not yet supported");
    }
    else
        LogicError("This option is not yet supported");
}

} // namespace elem

#endif // ifndef ELEM_MULTISHIFTHESSSOLVE_HPP
<|endoftext|>