<commit_before>/*!
 * \file calibration_from_carl.cpp
 * \brief External camera calibration from an AR tag with known position on the CARL robot
 *
 * \author David Kent, WPI - davidkent@wpi.edu
 * \date November 14, 2014
 */
#include <rail_ceiling/calibration_from_carl.h>

using namespace std;

CalibrationFromCarl::CalibrationFromCarl()
{
  // private node handle
  ros::NodeHandle private_nh("~");

  // get number of marker topics (i.e. number of overhead cameras), and the id of the calibration marker
  int numCameras;
  private_nh.param("num_cameras", numCameras, 5);
  private_nh.param("calibration_marker_id", markerID, 200);

  calibrationEnabled.resize(numCameras);
  transformSamples.resize(numCameras);
  finalTransforms.resize(numCameras);
  calibrated.resize(numCameras);
  markerSubscribers.resize(numCameras);
  for (unsigned int i = 0; i < numCameras; i ++)
  {
    transformSamples[i].clear();
    calibrationEnabled[i] = false;
    calibrated[i] = false;
    stringstream topicStream;
    topicStream << "ceiling_cam_tracker_" << i << "/ar_pose_marker";
    markerSubscribers[i] = n.subscribe(topicStream.str(), 1, &CalibrationFromCarl::markerCallback, this);
  }

  startCalibrationSubscriber = n.subscribe("start_calibration", 1, &CalibrationFromCarl::startCalibrationCallback, this);
  calibrationComplete = false;
}

void CalibrationFromCarl::startCalibrationCallback(const std_msgs::Int16::ConstPtr& msg)
{
  //enable calibration for the specified camera and clear out any previous samples
  calibrationEnabled[msg->data] = true;
  calibrated[msg->data] = false;
  transformSamples[msg->data].clear();
  calibrationComplete = false;
}

void CalibrationFromCarl::markerCallback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& msg)
{
  for (unsigned int i = 0; i < msg->markers.size(); i ++)
  {
    //check if the marker detected is the calibration marker
    if (msg->markers[i].id == markerID)
    {
      //get the marker index, this assumes the camera coordinate frames contain the string "cam_n", where n is an
      //integer denoting the camera number
      size_t pos = msg->markers[i].header.frame_id.find("cam_");
      int cameraid = atoi((msg->markers[i].header.frame_id.substr(pos + 4)).c_str());
      if (calibrationEnabled[cameraid])
      {
        geometry_msgs::PoseStamped sample = msg->markers[i].pose;
        sample.header.frame_id=msg->markers[i].header.frame_id;

        // transform pose to map frame
        tf::Transform tfSample;
        tf::StampedTransform finalTransform;
        tfSample.setOrigin(tf::Vector3(sample.pose.position.x, sample.pose.position.y, sample.pose.position.z));
        tfSample.setRotation(tf::Quaternion(sample.pose.orientation.x, sample.pose.orientation.y, sample.pose.orientation.z, sample.pose.orientation.w).normalize());
        // invert it
        tf::Transform tfSampleInverse = tfSample.inverse();
        ros::Time time = ros::Time::now();
        br.sendTransform(tf::StampedTransform(tfSampleInverse, time, "calibration_link", sample.header.frame_id.c_str()));
        tfListener.waitForTransform("map", "calibration_link", time, ros::Duration(1.0));
        tfListener.lookupTransform("map", sample.header.frame_id, time, finalTransform);

        transformSamples[cameraid].push_back(finalTransform);
        if (transformSamples[cameraid].size() >= REQUIRED_SAMPLES)
        {
          ROS_INFO("Finished calibration for camera %d", cameraid);
          calibrationEnabled[cameraid] = false;
        }
      }
    }
  }
}

void CalibrationFromCarl::publishTransforms()
{
  // go through each marker
  bool finished = true;
  for (unsigned int i = 0; i < transformSamples.size(); i ++)
  {
    // publish the average pose from the camera if it's received enough samples
    if (transformSamples[i].size() >= REQUIRED_SAMPLES)
    {
      if (!calibrated[i])
      {
        //calculate average pose
        //TODO: find a better way to approximate average rotation in 3D
        tf::StampedTransform avgTransform;
        avgTransform.frame_id_ = transformSamples[i][0].frame_id_;
        avgTransform.child_frame_id_ = transformSamples[i][0].child_frame_id_;
        avgTransform.stamp_ = ros::Time::now();
        float x = 0.0, y = 0.0, z = 0.0;
        //float qx = 0.0, qy = 0.0, qz = 0.0, qw = 0.0;
        tf::Quaternion avgQuat;
        for (unsigned int j = 0; j < transformSamples[i].size(); j++)
        {
          x += transformSamples[i][j].getOrigin().x();
          y += transformSamples[i][j].getOrigin().y();
          z += transformSamples[i][j].getOrigin().z();
          if (j == 0)
          {
            avgQuat = transformSamples[i][j].getRotation();
          }
          else
          {
            avgQuat.slerp(transformSamples[i][j].getRotation(), 1 - (1/(j + 1)));
          }
          /*
          qx += transformSamples[i][j].getRotation().getX();
          qy += transformSamples[i][j].getRotation().getY();
          qz += transformSamples[i][j].getRotation().getZ();
          qw += transformSamples[i][j].getRotation().getW();
          */
        }

        int numSamples = transformSamples[i].size();
        avgTransform.setOrigin(tf::Vector3(x/numSamples, y/numSamples, z/numSamples));
        //avgTransform.setRotation(tf::Quaternion(qx/numSamples, qy/numSamples, qz/numSamples, qw/numSamples).normalize());
        avgTransform.setRotation(avgQuat);

        finalTransforms[i] = avgTransform;
        calibrated[i] = true;
      }

      br.sendTransform(finalTransforms[i]);
    }
    else
    {
      finished = false;
    }
  }

  if (finished && !calibrationComplete)
  {
    //write calibration file
    ROS_INFO("Writing calibration...");

    stringstream ss;
    //TODO: update
    ss << getenv("HOME") << "/testCalibrationFile.urdf";
    string file_name = ss.str();

    // open the file for writing
    ofstream urdf;
    urdf.open(file_name.c_str());
    if (!urdf.is_open())
      ROS_ERROR("Failed to open '~/%s' for writing.", file_name.c_str());
    else
    {
      urdf << "<?xml version=\"1.0\"?>\n";
      urdf << "<robot xmlns:xacro=\"http://www.ros.org/wiki/xacro\" name=\"ceiling\">\n\n";

      urdf << "  <!-- Auto-Generated from rail_ceiling/calibration Node -->\n\n";

      urdf << "  <xacro:include filename=\"$(find rail_ceiling)/urdf/camera.urdf.xacro\" />\n\n";

      urdf << "  <xacro:property name=\"PARENT\" value=\"" << "map" << "\" />\n\n";

      urdf << "  <!-- fixed frame -->\n";
      urdf << "  <link name=\"${PARENT}\" />\n\n";

      urdf << "  <!-- " << finalTransforms.size() << " Camera(s) -->\n";
      for (int i = 0; i < finalTransforms.size(); i++)
      {
        // grab the TF info
        tf::StampedTransform &tf = finalTransforms.at(i);
        tf::Vector3 &pos = tf.getOrigin();
        double roll, pitch, yaw;
        tf.getBasis().getRPY(roll, pitch, yaw);
        urdf << "  <xacro:ceiling_cam parent=\"${PARENT}\" link=\"" << tf.child_frame_id_ << i << "\">\n";
        urdf << "    <origin xyz=\"" << pos.getX() << " " << pos.getY() << " " << pos.getZ() << "\" rpy=\"" << roll << " "
            << pitch << " " << yaw << "\" />\n";
        urdf << "  </xacro:ceiling_cam>\n";
      }
      urdf << "</robot>\n\n";

      urdf.close();
      ROS_INFO("Calibration written to '%s'.", file_name.c_str());
    }

    calibrationComplete = true;
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "calibration_from_carl");

  CalibrationFromCarl c;

  ros::Rate loop_rate(30);
  while (ros::ok())
  {
    c.publishTransforms();
    ros::spinOnce();
    loop_rate.sleep();
  }
}
<commit_msg>corrected the ratio for the slerp averaging; it was originally backwards<commit_after>/*!
 * \file calibration_from_carl.cpp
 * \brief External camera calibration from an AR tag with known position on the CARL robot
 *
 * \author David Kent, WPI - davidkent@wpi.edu
 * \date November 14, 2014
 */
#include <rail_ceiling/calibration_from_carl.h>

using namespace std;

CalibrationFromCarl::CalibrationFromCarl()
{
  // private node handle
  ros::NodeHandle private_nh("~");

  // get number of marker topics (i.e. number of overhead cameras), and the id of the calibration marker
  int numCameras;
  private_nh.param("num_cameras", numCameras, 5);
  private_nh.param("calibration_marker_id", markerID, 200);

  calibrationEnabled.resize(numCameras);
  transformSamples.resize(numCameras);
  finalTransforms.resize(numCameras);
  calibrated.resize(numCameras);
  markerSubscribers.resize(numCameras);
  for (unsigned int i = 0; i < numCameras; i ++)
  {
    transformSamples[i].clear();
    calibrationEnabled[i] = false;
    calibrated[i] = false;
    stringstream topicStream;
    topicStream << "ceiling_cam_tracker_" << i << "/ar_pose_marker";
    markerSubscribers[i] = n.subscribe(topicStream.str(), 1, &CalibrationFromCarl::markerCallback, this);
  }

  startCalibrationSubscriber = n.subscribe("start_calibration", 1, &CalibrationFromCarl::startCalibrationCallback, this);
  calibrationComplete = false;
}

void CalibrationFromCarl::startCalibrationCallback(const std_msgs::Int16::ConstPtr& msg)
{
  //enable calibration for the specified camera and clear out any previous samples
  calibrationEnabled[msg->data] = true;
  calibrated[msg->data] = false;
  transformSamples[msg->data].clear();
  calibrationComplete = false;
}

void CalibrationFromCarl::markerCallback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& msg)
{
  for (unsigned int i = 0; i < msg->markers.size(); i ++)
  {
    //check if the marker detected is the calibration marker
    if (msg->markers[i].id == markerID)
    {
      //get the marker index, this assumes the camera coordinate frames contain the string "cam_n", where n is an
      //integer denoting the camera number
      size_t pos = msg->markers[i].header.frame_id.find("cam_");
      int cameraid = atoi((msg->markers[i].header.frame_id.substr(pos + 4)).c_str());
      if (calibrationEnabled[cameraid])
      {
        geometry_msgs::PoseStamped sample = msg->markers[i].pose;
        sample.header.frame_id=msg->markers[i].header.frame_id;

        // transform pose to map frame
        tf::Transform tfSample;
        tf::StampedTransform finalTransform;
        tfSample.setOrigin(tf::Vector3(sample.pose.position.x, sample.pose.position.y, sample.pose.position.z));
        tfSample.setRotation(tf::Quaternion(sample.pose.orientation.x, sample.pose.orientation.y, sample.pose.orientation.z, sample.pose.orientation.w).normalize());
        // invert it
        tf::Transform tfSampleInverse = tfSample.inverse();
        ros::Time time = ros::Time::now();
        br.sendTransform(tf::StampedTransform(tfSampleInverse, time, "calibration_link", sample.header.frame_id.c_str()));
        tfListener.waitForTransform("map", "calibration_link", time, ros::Duration(1.0));
        tfListener.lookupTransform("map", sample.header.frame_id, time, finalTransform);

        transformSamples[cameraid].push_back(finalTransform);
        if (transformSamples[cameraid].size() >= REQUIRED_SAMPLES)
        {
          ROS_INFO("Finished calibration for camera %d", cameraid);
          calibrationEnabled[cameraid] = false;
        }
      }
    }
  }
}

void CalibrationFromCarl::publishTransforms()
{
  // go through each marker
  bool finished = true;
  for (unsigned int i = 0; i < transformSamples.size(); i ++)
  {
    // publish the average pose from the camera if it's received enough samples
    if (transformSamples[i].size() >= REQUIRED_SAMPLES)
    {
      if (!calibrated[i])
      {
        //calculate average pose
        //TODO: find a better way to approximate average rotation in 3D
        tf::StampedTransform avgTransform;
        avgTransform.frame_id_ = transformSamples[i][0].frame_id_;
        avgTransform.child_frame_id_ = transformSamples[i][0].child_frame_id_;
        avgTransform.stamp_ = ros::Time::now();
        float x = 0.0, y = 0.0, z = 0.0;
        //float qx = 0.0, qy = 0.0, qz = 0.0, qw = 0.0;
        tf::Quaternion avgQuat;
        for (unsigned int j = 0; j < transformSamples[i].size(); j++)
        {
          x += transformSamples[i][j].getOrigin().x();
          y += transformSamples[i][j].getOrigin().y();
          z += transformSamples[i][j].getOrigin().z();
          if (j == 0)
          {
            avgQuat = transformSamples[i][j].getRotation();
          }
          else
          {
            avgQuat.slerp(transformSamples[i][j].getRotation(), 1.0/((float)(j + 1)));
          }
          /*
          qx += transformSamples[i][j].getRotation().getX();
          qy += transformSamples[i][j].getRotation().getY();
          qz += transformSamples[i][j].getRotation().getZ();
          qw += transformSamples[i][j].getRotation().getW();
          */
        }

        int numSamples = transformSamples[i].size();
        avgTransform.setOrigin(tf::Vector3(x/numSamples, y/numSamples, z/numSamples));
        //avgTransform.setRotation(tf::Quaternion(qx/numSamples, qy/numSamples, qz/numSamples, qw/numSamples).normalize());
        avgTransform.setRotation(avgQuat);

        finalTransforms[i] = avgTransform;
        calibrated[i] = true;
      }

      br.sendTransform(finalTransforms[i]);
    }
    else
    {
      finished = false;
    }
  }

  if (finished && !calibrationComplete)
  {
    //write calibration file
    ROS_INFO("Writing calibration...");

    stringstream ss;
    //TODO: update
    ss << getenv("HOME") << "/testCalibrationFile.urdf";
    string file_name = ss.str();

    // open the file for writing
    ofstream urdf;
    urdf.open(file_name.c_str());
    if (!urdf.is_open())
      ROS_ERROR("Failed to open '~/%s' for writing.", file_name.c_str());
    else
    {
      urdf << "<?xml version=\"1.0\"?>\n";
      urdf << "<robot xmlns:xacro=\"http://www.ros.org/wiki/xacro\" name=\"ceiling\">\n\n";

      urdf << "  <!-- Auto-Generated from rail_ceiling/calibration Node -->\n\n";

      urdf << "  <xacro:include filename=\"$(find rail_ceiling)/urdf/camera.urdf.xacro\" />\n\n";

      urdf << "  <xacro:property name=\"PARENT\" value=\"" << "map" << "\" />\n\n";

      urdf << "  <!-- fixed frame -->\n";
      urdf << "  <link name=\"${PARENT}\" />\n\n";

      urdf << "  <!-- " << finalTransforms.size() << " Camera(s) -->\n";
      for (int i = 0; i < finalTransforms.size(); i++)
      {
        // grab the TF info
        tf::StampedTransform &tf = finalTransforms.at(i);
        tf::Vector3 &pos = tf.getOrigin();
        double roll, pitch, yaw;
        tf.getBasis().getRPY(roll, pitch, yaw);
        urdf << "  <xacro:ceiling_cam parent=\"${PARENT}\" link=\"" << tf.child_frame_id_ << i << "\">\n";
        urdf << "    <origin xyz=\"" << pos.getX() << " " << pos.getY() << " " << pos.getZ() << "\" rpy=\"" << roll << " "
            << pitch << " " << yaw << "\" />\n";
        urdf << "  </xacro:ceiling_cam>\n";
      }
      urdf << "</robot>\n\n";

      urdf.close();
      ROS_INFO("Calibration written to '%s'.", file_name.c_str());
    }

    calibrationComplete = true;
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "calibration_from_carl");

  CalibrationFromCarl c;

  ros::Rate loop_rate(30);
  while (ros::ok())
  {
    c.publishTransforms();
    ros::spinOnce();
    loop_rate.sleep();
  }
}
<|endoftext|>


<commit_before>/**
 * @file PathPlanner.cpp
 *
 * @author Yigit Can Akcay
 * Implementation of class PathPlanner
 */

#include "PathPlanner.h"

PathPlanner::PathPlanner()
:
step_buffer({}),
foot_to_use(Foot::RIGHT),
last_stepcontrol_stepID(0),
kick_planned(false)
{
  DEBUG_REQUEST_REGISTER("PathPlanner:walk_forward", "Walks forward with y=0 and x=40", false);
}

void PathPlanner::execute()
{
  getPathModel().kick_executed = false;

  DEBUG_REQUEST("PathPlanner:walk_forward",
    Pose2D pose = Pose2D(0.0, 40.0, 0.0);
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;
    if (step_buffer.empty())
    {
      add_step(pose, StepType::WALKSTEP, coordinate);
    }
  );

  STOPWATCH_START("PathPlanner");
  // Always executed first
  manage_step_buffer();

  if (kick_planned && step_buffer.empty()) {
    getPathModel().kick_executed = true;
  }

  switch (getPathModel().path_routine)
  {
  case PathModel::PathRoutine::NONE:
    // Reset routine_executed, so that XABSL
    // can jump out of option (PathRoutine) that is
    // being executed
    //kick_planned = false;
    if (kick_planned) {
      kick_planned = false;
    }

    if (step_buffer.empty()) {
      return;
    }
    break;
  case PathModel::PathRoutine::GO_TO_BALL_FAST:
    walk_to_ball(Foot::NONE , true);
    break;
  case PathModel::PathRoutine::GO_TO_BALL_SLOW:
    walk_to_ball(Foot::NONE);
    break;
  case PathModel::PathRoutine::MOVE_AROUND_BALL:
    move_around_ball(getPathModel().direction, getPathModel().radius);
    break;
  case PathModel::PathRoutine::APPROACH_BALL_LEFT:
    approach_ball(Foot::LEFT);
    break;
  case PathModel::PathRoutine::APPROACH_BALL_RIGHT:
    approach_ball(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::SHORT_KICK_LEFT:
    short_kick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::SHORT_KICK_RIGHT:
    short_kick(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::LONG_KICK_LEFT:
    long_kick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::LONG_KICK_RIGHT:
    long_kick(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::SIDEKICK_LEFT:
    sidekick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::SIDEKICK_RIGHT:
    sidekick(Foot::RIGHT);
    break;
  }

  // Always executed last
  execute_step_buffer();

  STOPWATCH_STOP("PathPlanner");
}

// Primitive Maneuvers
void PathPlanner::walk_to_ball(const Foot foot, const bool go_fast)
{
  Vector2d ballPos                   = Vector2d();
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;
  switch (foot) {
    case Foot::LEFT:
      ballPos    = getBallModel().positionPreviewInLFoot;
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::RIGHT:
      ballPos    = getBallModel().positionPreviewInRFoot;
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::NONE:
      ballPos    = getBallModel().positionPreview;
      coordinate = WalkRequest::Hip;
      break;
  }
  double ballRotation = ballPos.angle();

  Pose2D pose = { ballRotation, 0.7*(ballPos.x - getPathModel().distance), ballPos.y };
  if (step_buffer.empty())
  {
    if (go_fast)
    {
      add_step(pose, StepType::WALKSTEP, coordinate, 1.0);
    }
    else
    {
      add_step(pose, StepType::WALKSTEP, coordinate, 0.3);
    }
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::move_around_ball(const double direction, const double radius)
{
  Vector2d ballPos                   = getBallModel().positionPreview;
  double ballRotation                = ballPos.angle();
  double ballDistance                = ballPos.abs();
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;

  double min1;
  double min2;
  double max1;
  double max2;
  if (direction <= 0)
  {
    min1 = 0.0;
    min2 = 0.0;
    max1 = 45.0;
    max2 = 100.0;
  }
  else {
    min1 = -45;
    min2 = -100;
    max1 = 0;
    max2 = 0;
  }

  double stepX = (ballDistance - radius) * std::cos(ballRotation);
  double stepY = Math::clamp(radius * std::tan(Math::clamp(Math::toDegrees(-direction), min1, max1)), min2, max2) * std::cos(ballRotation);
  Pose2D pose = { ballRotation, stepX, stepY };

  if (step_buffer.empty())
  {
    add_step(pose, StepType::WALKSTEP, coordinate, 0.7);
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::approach_ball(const Foot foot)
{
  Vector2d ballPos                   = Vector2d();
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;
  double stepX                       = 0.0;
  double stepY                       = 0.0;
  double ballRadius                  = getFieldInfo().ballRadius;
  double stepRotation                = ballPos.abs() > 250 ? ballPos.angle() : 0;

  switch (foot) {
  case Foot::LEFT:
    ballPos    = getBallModel().positionPreviewInLFoot;
    coordinate = WalkRequest::LFoot;
    stepX      = ballPos.x - std::abs(ballPos.y - getPathModel().yOffset) - getPathModel().distance - ballRadius;
    stepY      = ballPos.y - getPathModel().yOffset;
    break;
  case Foot::RIGHT:
    ballPos    = getBallModel().positionPreviewInRFoot;
    coordinate = WalkRequest::RFoot;
    stepX      = ballPos.x - std::abs(ballPos.y + getPathModel().yOffset) - getPathModel().distance - ballRadius;
    stepY      = ballPos.y + getPathModel().yOffset;
    break;
  case Foot::NONE:
    ASSERT(false);
  }

  Pose2D pose = { stepRotation, 0.7 * stepX, 0.7 * stepY };

  if (step_buffer.empty())
  {
    add_step(pose, StepType::WALKSTEP, coordinate, 0.7);
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::short_kick(const Foot foot)
{
  if (!kick_planned)
  {
    Vector2d ballPos                   = Vector2d();
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      ballPos    = getBallModel().positionPreviewInLFoot;
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::RIGHT:
      ballPos    = getBallModel().positionPreviewInRFoot;
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose = { 0.0, ballPos.x + 500 , 0.0 };
      add_step(pose, StepType::KICKSTEP, coordinate, 1.0, foot, 0.7);

      add_step(pose, StepType::ZEROSTEP, coordinate, 1.0, foot);

      pose = { 0.0, 0.0, 0.0 };
      add_step(pose, StepType::WALKSTEP, coordinate, 1.0, foot);

      kick_planned = true;
    }
  }
}

void PathPlanner::long_kick(const Foot foot)
{
  if (!kick_planned)
  {
    Vector2d ballPos                   = Vector2d();
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      ballPos    = getBallModel().positionPreviewInLFoot;
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::RIGHT:
      ballPos    = getBallModel().positionPreviewInRFoot;
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose = { 0.0, ballPos.x + 500, 0.0 };
      add_step(pose, StepType::KICKSTEP, coordinate, 1.0, foot, 0.7);

      add_step(pose, StepType::ZEROSTEP, coordinate, 1.0, foot);

      pose = { 0.0, 0.0, 0.0 };
      add_step(pose, StepType::WALKSTEP, coordinate, 1.0, foot);

      kick_planned = true;
    }
  }
}

void PathPlanner::sidekick(const Foot foot)
{
  if (!kick_planned)
  {
    double speedDirection              = 0.0;
    double stepY                       = 0.0;
    Vector2d ballPos                   = Vector2d();
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      ballPos        = getBallModel().positionPreviewInLFoot;
      coordinate     = WalkRequest::LFoot;
      speedDirection = 90;
      stepY          = 100;
      break;
    case Foot::RIGHT:
      ballPos        = getBallModel().positionPreviewInRFoot;
      coordinate     = WalkRequest::RFoot;
      speedDirection = -90;
      stepY          = -100;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose = { 0.0, 500, stepY };
      add_step(pose, StepType::KICKSTEP, coordinate, 1.0, foot == Foot::RIGHT ? Foot::LEFT : Foot::RIGHT, 1.0, speedDirection);

      add_step(pose, StepType::ZEROSTEP, coordinate, 1.0, foot == Foot::RIGHT ? Foot::LEFT : Foot::RIGHT);

      pose = { 0.0, 0.0, 0.0 };
      add_step(pose, StepType::WALKSTEP, coordinate, 1.0, foot == Foot::RIGHT ? Foot::LEFT : Foot::RIGHT);

      kick_planned = true;
    }
  }
}

// Stepcontrol
void PathPlanner::add_step(Pose2D &pose, const StepType &type, const WalkRequest::Coordinate &coordinate, const double character, const Foot foot, const double scale, const double speedDirection)
{
  // taken out of the stepplanner
  // limiting the steps if walksteps
  // 0.75 because 0.5 * character(usually 0.5) + 0.5 (taken from stepplanner)
  /*
  if (type == StepType::WALKSTEP)
  {
    double maxStepTurn = Math::fromDegrees(30) * 0.75;
    double maxStep     = 40.0f;
    pose.rotation      = Math::clamp(pose.rotation, -maxStepTurn, maxStepTurn);
    pose.translation.x = Math::clamp(pose.translation.x, -maxStep, maxStep) * cos(pose.rotation / maxStepTurn * Math::pi / 2);
    pose.translation.y = Math::clamp(pose.translation.y, -maxStep, maxStep) * cos(pose.rotation / maxStepTurn * Math::pi / 2);
  }
  */
  step_buffer.push_back(Step_Buffer_Element({ pose,
                                              speedDirection,
                                              type,
                                              type == StepType::KICKSTEP ? 300 : 250,
                                              character,
                                              scale,
                                              foot,
                                              coordinate}));
}
void PathPlanner::update_step(Pose2D &pose)
{
  ASSERT(step_buffer.size() > 0);
  // taken out of the stepplanner
  // limiting the steps if walksteps
  // 0.75 because 0.5 * character(usually 0.5) + 0.5 (taken from stepplanner)

  /*
  if (step_buffer.front().type == StepType::WALKSTEP)
  {
    double maxStepTurn = Math::fromDegrees(30) * 0.75;
    double maxStep     = 40.0f;
    pose.rotation      = Math::clamp(pose.rotation, -maxStepTurn, maxStepTurn);
    pose.translation.x = Math::clamp(pose.translation.x, -maxStep, maxStep) * cos(pose.rotation / maxStepTurn * Math::pi / 2);
    pose.translation.y = Math::clamp(pose.translation.y, -maxStep, maxStep) * cos(pose.rotation / maxStepTurn * Math::pi / 2);
  }*/

  Step_Buffer_Element updatedElement = step_buffer.front();
  updatedElement.pose                = pose;
  step_buffer.front()                = updatedElement;
}

void PathPlanner::manage_step_buffer()
{
  if (step_buffer.empty()) {
    return;
  }

  // stepID higher than the last one means, stepControl request with the
  // last_stepcontrol_stepID has been accepted
  if (last_stepcontrol_stepID < getMotionStatus().stepControl.stepID)
  {
    step_buffer.erase(step_buffer.begin());
    last_stepcontrol_stepID = getMotionStatus().stepControl.stepID;
  }

  // Correct last_stepcontrol_stepID if neccessary
  if (getMotionStatus().stepControl.stepID < last_stepcontrol_stepID) {
    last_stepcontrol_stepID = getMotionStatus().stepControl.stepID;
  }
}

void PathPlanner::execute_step_buffer()
{
  if (step_buffer.empty()) {
    return;
  }

  STOPWATCH_START("PathPlanner:execute_steplist");
  getMotionRequest().id = motion::walk;
  getMotionRequest().standardStand = false;
  getMotionRequest().walkRequest.coordinate                 = step_buffer.front().coordinate;
  getMotionRequest().walkRequest.character                  = step_buffer.front().character;
  getMotionRequest().walkRequest.stepControl.scale          = step_buffer.front().scale;
  getMotionRequest().walkRequest.stepControl.stepID         = getMotionStatus().stepControl.stepID;
  getMotionRequest().walkRequest.stepControl.type           = step_buffer.front().type;
  getMotionRequest().walkRequest.stepControl.time           = step_buffer.front().time;
  getMotionRequest().walkRequest.stepControl.speedDirection = step_buffer.front().speedDirection;
  getMotionRequest().walkRequest.stepControl.target         = step_buffer.front().pose;

  // normal walking WALKSTEPs use Foot::NONE, for KICKSTEPs the foot to use has to be specified
  if (step_buffer.front().foot == Foot::NONE)
  {
    switch (getMotionStatus().stepControl.moveableFoot)
    {
    case MotionStatus::StepControlStatus::LEFT:
      foot_to_use = Foot::LEFT;
      break;
    case MotionStatus::StepControlStatus::RIGHT:
      foot_to_use = Foot::RIGHT;
      break;
      // TODO: choose foot more intelligently when both feet are usable
    case MotionStatus::StepControlStatus::BOTH:
      if (step_buffer.front().pose.translation.y > 0.0f || step_buffer.front().pose.rotation > 0.0f)
      {
        foot_to_use = Foot::LEFT;
      }
      else
      {
        foot_to_use = Foot::RIGHT;
      }
      break;
    case MotionStatus::StepControlStatus::NONE:
      foot_to_use = Foot::RIGHT;
      break;
    }
  }
  else
  {
    foot_to_use = step_buffer.front().foot;
  }
  // false means right foot
  getMotionRequest().walkRequest.stepControl.moveLeftFoot = foot_to_use == Foot::RIGHT ? false : true;
  STOPWATCH_STOP("PathPlanner:execute_steplist");
}<commit_msg>clean up (default function parameters are bad and some comments made more clear/corrected). got rid of two useless functions.<commit_after>/**
 * @file PathPlanner.cpp
 *
 * @author Yigit Can Akcay
 * Implementation of class PathPlanner
 */

#include "PathPlanner.h"

PathPlanner::PathPlanner()
:
step_buffer({}),
foot_to_use(Foot::RIGHT),
last_stepcontrol_stepID(0),
kick_planned(false)
{}

void PathPlanner::execute()
{
  getPathModel().kick_executed = false;

  STOPWATCH_START("PathPlanner");
  // Always executed first
  manage_step_buffer();

  // The kick has been executed
  // Tells XABSL to jump into next state
  if (kick_planned && step_buffer.empty()) {
    getPathModel().kick_executed = true;
  }

  switch (getPathModel().path_routine)
  {
  case PathModel::PathRoutine::NONE:
    // There is no kick planned, since the kick has been executed
    // and XABSL is in a different state now
    if (kick_planned) {
      kick_planned = false;
    }

    if (step_buffer.empty()) {
      return;
    }
    break;
  case PathModel::PathRoutine::GO_TO_BALL_FAST:
    walk_to_ball(Foot::NONE , true);
    break;
  case PathModel::PathRoutine::GO_TO_BALL_SLOW:
    walk_to_ball(Foot::NONE);
    break;
  case PathModel::PathRoutine::MOVE_AROUND_BALL:
    move_around_ball(getPathModel().direction, getPathModel().radius);
    break;
  case PathModel::PathRoutine::APPROACH_BALL_LEFT:
    approach_ball(Foot::LEFT);
    break;
  case PathModel::PathRoutine::APPROACH_BALL_RIGHT:
    approach_ball(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::SHORT_KICK_LEFT:
    short_kick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::SHORT_KICK_RIGHT:
    short_kick(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::LONG_KICK_LEFT:
    long_kick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::LONG_KICK_RIGHT:
    long_kick(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::SIDEKICK_LEFT:
    sidekick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::SIDEKICK_RIGHT:
    sidekick(Foot::RIGHT);
    break;
  }

  // Always executed last
  execute_step_buffer();

  STOPWATCH_STOP("PathPlanner");
}

// Primitive Maneuvers
void PathPlanner::walk_to_ball(const Foot foot, const bool go_fast)
{
  Vector2d ballPos                   = Vector2d();
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;
  switch (foot) {
    case Foot::LEFT:
      ballPos    = getBallModel().positionPreviewInLFoot;
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::RIGHT:
      ballPos    = getBallModel().positionPreviewInRFoot;
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::NONE:
      ballPos    = getBallModel().positionPreview;
      coordinate = WalkRequest::Hip;
      break;
  }
  double ballRotation = ballPos.angle();

  Pose2D pose           = { ballRotation, 0.7*(ballPos.x - getPathModel().distance), ballPos.y };

  if (step_buffer.empty())
  {
    StepType type         = StepType::WALKSTEP;
    double scale          = 1.0;
    double speed_direction = 0.0;

    if (go_fast)
    {
      double character = 1.0;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction);
    }
    else
    {
      double character = 0.3;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction);
    }
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::move_around_ball(const double direction, const double radius)
{
  Vector2d ballPos                   = getBallModel().positionPreview;
  double ballRotation                = ballPos.angle();
  double ballDistance                = ballPos.abs();
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;

  double min1;
  double min2;
  double max1;
  double max2;
  if (direction <= 0)
  {
    min1 = 0.0;
    min2 = 0.0;
    max1 = 45.0;
    max2 = 100.0;
  }
  else {
    min1 = -45;
    min2 = -100;
    max1 = 0;
    max2 = 0;
  }

  double stepX = (ballDistance - radius) * std::cos(ballRotation);
  double stepY = Math::clamp(radius * std::tan(Math::clamp(Math::toDegrees(-direction), min1, max1)), min2, max2) * std::cos(ballRotation);

  Pose2D pose           = { ballRotation, stepX, stepY };

  if (step_buffer.empty())
  {
    StepType type          = StepType::WALKSTEP;
    double character       = 0.7;
    Foot foot              = Foot::NONE;
    double scale           = 1.0;
    double speed_direction = 0.0;
    add_step(pose, type, coordinate, character, foot, scale, speed_direction);
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::approach_ball(const Foot foot)
{
  Vector2d ballPos                   = Vector2d();
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;
  double stepX                       = 0.0;
  double stepY                       = 0.0;
  double ballRadius                  = getFieldInfo().ballRadius;
  double stepRotation                = ballPos.abs() > 250 ? ballPos.angle() : 0;

  switch (foot) {
  case Foot::LEFT:
    ballPos    = getBallModel().positionPreviewInLFoot;
    coordinate = WalkRequest::LFoot;
    stepX      = ballPos.x - std::abs(ballPos.y - getPathModel().yOffset) - getPathModel().distance - ballRadius;
    stepY      = ballPos.y - getPathModel().yOffset;
    break;
  case Foot::RIGHT:
    ballPos    = getBallModel().positionPreviewInRFoot;
    coordinate = WalkRequest::RFoot;
    stepX      = ballPos.x - std::abs(ballPos.y + getPathModel().yOffset) - getPathModel().distance - ballRadius;
    stepY      = ballPos.y + getPathModel().yOffset;
    break;
  case Foot::NONE:
    ASSERT(false);
  }

  Pose2D pose = { stepRotation, 0.7 * stepX, 0.7 * stepY };

  if (step_buffer.empty())
  {
    StepType type          = StepType::WALKSTEP;
    double character       = 0.7;
    double scale           = 1.0;
    double speed_direction = 0.0;
    add_step(pose, type, coordinate, character, foot, scale, speed_direction);
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::short_kick(const Foot foot)
{
  if (!kick_planned)
  {
    Vector2d ballPos                   = Vector2d();
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      ballPos    = getBallModel().positionPreviewInLFoot;
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::RIGHT:
      ballPos    = getBallModel().positionPreviewInRFoot;
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose            = { 0.0, ballPos.x + 500 , 0.0 };
      StepType type          = StepType::KICKSTEP;
      double character       = 1.0;
      double scale           = 0.7;
      double speed_direction = 0.0;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction);

      type = StepType::ZEROSTEP;

      add_step(pose, type, coordinate, character, foot, scale, speed_direction);

      pose = { 0.0, 0.0, 0.0 };
      type = StepType::WALKSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction);

      kick_planned = true;
    }
  }
}

void PathPlanner::long_kick(const Foot foot)
{
  if (!kick_planned)
  {
    Vector2d ballPos                   = Vector2d();
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      ballPos    = getBallModel().positionPreviewInLFoot;
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::RIGHT:
      ballPos    = getBallModel().positionPreviewInRFoot;
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose            = { 0.0, ballPos.x + 500, 0.0 };
      StepType type          = StepType::KICKSTEP;
      double character       = 1.0;
      double scale           = 0.7;
      double speed_direction = 0.0;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction);

      type = StepType::ZEROSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction);

      pose = { 0.0, 0.0, 0.0 };
      type = StepType::WALKSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction);

      kick_planned = true;
    }
  }
}

void PathPlanner::sidekick(const Foot foot)
{
  if (!kick_planned)
  {
    double speed_direction             = 0.0;
    double stepY                       = 0.0;
    Vector2d ballPos                   = Vector2d();
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      ballPos        = getBallModel().positionPreviewInLFoot;
      coordinate     = WalkRequest::LFoot;
      speed_direction = 90;
      stepY          = 100;
      break;
    case Foot::RIGHT:
      ballPos        = getBallModel().positionPreviewInRFoot;
      coordinate     = WalkRequest::RFoot;
      speed_direction = -90;
      stepY          = -100;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose = { 0.0, 500, stepY };
      StepType type = StepType::KICKSTEP;
      double character = 1.0;
      Foot step_foot = foot == Foot::RIGHT ? Foot::LEFT : Foot::RIGHT;
      double scale = 1.0;
      add_step(pose, type, coordinate, character, step_foot, scale, speed_direction);

      type = StepType::ZEROSTEP;
      add_step(pose, type, coordinate, character, step_foot, scale, speed_direction);

      pose = { 0.0, 0.0, 0.0 };
      type = StepType::WALKSTEP;
      add_step(pose, type, coordinate, character, step_foot, scale, speed_direction);

      kick_planned = true;
    }
  }
}

// Stepcontrol
void PathPlanner::add_step(Pose2D &pose, const StepType &type, const WalkRequest::Coordinate &coordinate, const double character, const Foot foot, const double scale, const double speedDirection)
{
  step_buffer.push_back(Step_Buffer_Element({ pose,
                                              speedDirection,
                                              type,
                                              type == StepType::KICKSTEP ? 300 : 250,
                                              character,
                                              scale,
                                              foot,
                                              coordinate}));
}
void PathPlanner::update_step(Pose2D &pose)
{
  ASSERT(step_buffer.size() > 0);

  Step_Buffer_Element updatedElement = step_buffer.front();
  updatedElement.pose                = pose;
  step_buffer.front()                = updatedElement;
}

void PathPlanner::manage_step_buffer()
{
  if (step_buffer.empty()) {
    return;
  }

  // stepID higher than the last one means, stepControl request with the
  // last_stepcontrol_stepID has been accepted
  if (last_stepcontrol_stepID < getMotionStatus().stepControl.stepID)
  {
    step_buffer.erase(step_buffer.begin());
    last_stepcontrol_stepID = getMotionStatus().stepControl.stepID;
  }

  // Correct last_stepcontrol_stepID if neccessary
  if (getMotionStatus().stepControl.stepID < last_stepcontrol_stepID) {
    last_stepcontrol_stepID = getMotionStatus().stepControl.stepID;
  }
}

void PathPlanner::execute_step_buffer()
{
  if (step_buffer.empty()) {
    return;
  }

  STOPWATCH_START("PathPlanner:execute_steplist");
  getMotionRequest().id = motion::walk;
  getMotionRequest().standardStand = false;
  getMotionRequest().walkRequest.coordinate                 = step_buffer.front().coordinate;
  getMotionRequest().walkRequest.character                  = step_buffer.front().character;
  getMotionRequest().walkRequest.stepControl.scale          = step_buffer.front().scale;
  getMotionRequest().walkRequest.stepControl.stepID         = getMotionStatus().stepControl.stepID;
  getMotionRequest().walkRequest.stepControl.type           = step_buffer.front().type;
  getMotionRequest().walkRequest.stepControl.time           = step_buffer.front().time;
  getMotionRequest().walkRequest.stepControl.speedDirection = step_buffer.front().speedDirection;
  getMotionRequest().walkRequest.stepControl.target         = step_buffer.front().pose;

  // normal walking WALKSTEPs use Foot::NONE, for KICKSTEPs the foot to use has to be specified
  if (step_buffer.front().foot == Foot::NONE)
  {
    switch (getMotionStatus().stepControl.moveableFoot)
    {
    case MotionStatus::StepControlStatus::LEFT:
      foot_to_use = Foot::LEFT;
      break;
    case MotionStatus::StepControlStatus::RIGHT:
      foot_to_use = Foot::RIGHT;
      break;
    case MotionStatus::StepControlStatus::BOTH:
      if (step_buffer.front().pose.translation.y > 0.0f || step_buffer.front().pose.rotation > 0.0f)
      {
        foot_to_use = Foot::LEFT;
      }
      else
      {
        foot_to_use = Foot::RIGHT;
      }
      break;
    case MotionStatus::StepControlStatus::NONE:
      foot_to_use = Foot::RIGHT;
      break;
    }
  }
  else
  {
    foot_to_use = step_buffer.front().foot;
  }
  // false means right foot
  getMotionRequest().walkRequest.stepControl.moveLeftFoot = foot_to_use == Foot::RIGHT ? false : true;
  STOPWATCH_STOP("PathPlanner:execute_steplist");
}
<|endoftext|>


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

 */
#ifndef NEWTONEULERIMPACT_H
#define NEWTONEULERIMPACT_H

#include "NewtonEulerR.hpp"
#include "NewtonEulerDS.hpp"

/** NewtonEulerFrom1DLocalFrameR
 *
 * \author O. Bonnefon
 *  \version 3.0.0.
 *  \date Dec, 2010
 *
 * This class is an interface for a relation with impact.  It
 * implements the computation of the jacoboian of h from the points of
 * contacts and the normal.  Use this class consists in overloading
 * the method computeh, by setting the member pc1, pc2, nc and y.  The
 * matrix jachq is used both for the building of the OSNSP (with T)
 * and for the predictor of activation of deactivation of the Interaction.
 *
 */

class NewtonEulerFrom1DLocalFrameR : public NewtonEulerR
{
protected:
  /** serialization hooks
  */
  ACCEPT_SERIALIZATION(NewtonEulerFrom1DLocalFrameR);

  /* Current Contact Points, may be updated within Newton loop based
   * on _relPc1, _relPc2. */
  SP::SiconosVector _Pc1;
  SP::SiconosVector _Pc2;

  /* Contact Points in coordinates relative to attached DS->q.  Set
   * these if _Pc1/_Pc2 are not calculated within the Newton loop. */
  SP::SiconosVector _relPc1;
  SP::SiconosVector _relPc2;

  /* Inward Normal at the contact.
   * \todo The meaning of "Inward" has to be explained carefully.
   */
  SP::SiconosVector _Nc;

  /* _Nc must be calculated relative to q2 */
  SP::SiconosVector _relNc;

  /* Rotation matrix converting the absolute coordinate to the contact frame coordinate.
   * This matrix contains the unit vector(s)of the contact frame in row.
   */
  SP::SimpleMatrix _RotationAbsToContactFrame;

  /* Matrix converting */
  SP::SimpleMatrix _rotationMatrixAbsToBody;

  /* Cross product matrices that correspond the lever arm from
   * contact point to center of mass*/
  SP::SimpleMatrix _NPG1;
  SP::SimpleMatrix _NPG2;

  /*buffer matrices*/
  SP::SimpleMatrix _AUX1;
  SP::SimpleMatrix _AUX2;
private:
  void NIcomputeJachqTFromContacts(SP::SiconosVector q1);
  void NIcomputeJachqTFromContacts(SP::SiconosVector q1, SP::SiconosVector q2);
public:

  /** V.A. boolean _isOnCOntact ?? Why is it public members ?
  *  seems parametrize the projection algorithm
  *  the projection is done on the surface \f$y=0\f$ or on \f$y \geq 0\f$
  */
  bool _isOnContact;

  /** constructorx
  */
  NewtonEulerFrom1DLocalFrameR():
    NewtonEulerR(), _Pc1(new SiconosVector(3)), _Pc2(new SiconosVector(3)),
    _relPc1(new SiconosVector(3)), _relPc2(new SiconosVector(3)),
    _Nc(new SiconosVector(3)), _relNc(new SiconosVector(3))
  {
    /*_ds1=NULL;_ds2=NULL;*/
  }

  /** destructor
  */
  virtual ~NewtonEulerFrom1DLocalFrameR() {};

  virtual void computeJachq(double time, Interaction& inter, SP::BlockVector q0);

  virtual void initializeWorkVectorsAndMatrices(Interaction& inter, VectorOfBlockVectors& DSlink, VectorOfVectors& workV, VectorOfSMatrices& workM);

  virtual void initialize(Interaction& inter);

  /* Default implementation consists in multiplying jachq and T (see NewtonEulerR::computeJachqT)
   * but here we compute the operator from the the contact point locations
   * and the local frame at contact
   *  \param inter interaction that owns the relation
   *  \param q0  the block vector to the dynamical system position
   */
  virtual void computeJachqT(Interaction& inter, SP::BlockVector q0);

  /* Default implementation of computeh updates contact points and
   * distance for q if different than qold. */
  virtual void computeh(double time, BlockVector& q0, SiconosVector &y);

  /** Return the distance between pc1 and pc, with sign according to normal */
  double distance() const;

  inline SP::SiconosVector pc1() const
  {
    return _Pc1;
  }
  inline SP::SiconosVector pc2() const
  {
    return _Pc2;
  }
  inline SP::SiconosVector nc() const
  {
    return _Nc;
  }

  inline SP::SiconosVector relPc1() const
  {
    return _relPc1;
  }
  inline SP::SiconosVector relPc2() const
  {
    return _relPc2;
  }
  inline SP::SiconosVector relNc() const
  {
    return _relNc;
  }

  /** set the coordinates of first contact point
  * \param npc new coordinates
  */
  void setpc1(SP::SiconosVector npc)
  {
    _Pc1 = npc;
  };

  /** set the coordinates of second contact point
  * \param npc new coordinates
  */
  void setpc2(SP::SiconosVector npc)
  {
    _Pc2 = npc;
  };

  /** set the coordinates of inside normal vector at the contact point
  * \param nnc new coordinates
  */
  void setnc(SP::SiconosVector nnc)
  {
    _Nc = nnc;
  };

  /** Set the coordinates of first contact point in ds1 frame.
   * It will be used to compute _Pc1 during computeh().
  * \param npc new coordinates
  */
  void setRelPc1(SP::SiconosVector npc)
  {
    _relPc1 = npc;
  };

  /** Set the coordinates of second contact point in ds2 frame
   * It will be used to compute _Pc2 during computeh().
  * \param npc new coordinates
  */
  void setRelPc2(SP::SiconosVector npc)
  {
    _relPc2 = npc;
  };

  /** Set the coordinates of inside normal vector at the contact point in ds2 frame.
   * It will be used to compute _Nc during computeh().
  * \param nnc new coordinates
  */
  void setRelNc(SP::SiconosVector nnc)
  {
    _relNc = nnc;
  };

  // visitors hook
  ACCEPT_STD_VISITORS();

};
#endif // NEWTONEULERRIMPACT_H
<commit_msg>[kernel] move setpc1,setpc2,setnc to protected<commit_after>/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2016 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/
/*! \file NewtonEulerFrom1DLocalFrameR.hpp

 */
#ifndef NEWTONEULERIMPACT_H
#define NEWTONEULERIMPACT_H

#include "NewtonEulerR.hpp"
#include "NewtonEulerDS.hpp"

/** NewtonEulerFrom1DLocalFrameR
 *
 * \author O. Bonnefon
 *  \version 3.0.0.
 *  \date Dec, 2010
 *
 * This class is an interface for a relation with impact.  It
 * implements the computation of the jacoboian of h from the points of
 * contacts and the normal.  Use this class consists in overloading
 * the method computeh, by setting the member pc1, pc2, nc and y.  The
 * matrix jachq is used both for the building of the OSNSP (with T)
 * and for the predictor of activation of deactivation of the Interaction.
 *
 */

class NewtonEulerFrom1DLocalFrameR : public NewtonEulerR
{
protected:
  /** serialization hooks
  */
  ACCEPT_SERIALIZATION(NewtonEulerFrom1DLocalFrameR);

  /* Current Contact Points, may be updated within Newton loop based
   * on _relPc1, _relPc2. */
  SP::SiconosVector _Pc1;
  SP::SiconosVector _Pc2;

  /* Contact Points in coordinates relative to attached DS->q.  Set
   * these if _Pc1/_Pc2 are not calculated within the Newton loop. */
  SP::SiconosVector _relPc1;
  SP::SiconosVector _relPc2;

  /* Inward Normal at the contact.
   * \todo The meaning of "Inward" has to be explained carefully.
   */
  SP::SiconosVector _Nc;

  /* _Nc must be calculated relative to q2 */
  SP::SiconosVector _relNc;

  /* Rotation matrix converting the absolute coordinate to the contact frame coordinate.
   * This matrix contains the unit vector(s)of the contact frame in row.
   */
  SP::SimpleMatrix _RotationAbsToContactFrame;

  /* Matrix converting */
  SP::SimpleMatrix _rotationMatrixAbsToBody;

  /* Cross product matrices that correspond the lever arm from
   * contact point to center of mass*/
  SP::SimpleMatrix _NPG1;
  SP::SimpleMatrix _NPG2;

  /*buffer matrices*/
  SP::SimpleMatrix _AUX1;
  SP::SimpleMatrix _AUX2;

  /** Set the coordinates of first contact point.  Must only be done
  * in a computeh() override.
  * \param npc new coordinates
  */
  void setpc1(SP::SiconosVector npc)
  {
    _Pc1 = npc;
  };

  /** Set the coordinates of second contact point.  Must only be done
  * in a computeh() override.
  * \param npc new coordinates
  */
  void setpc2(SP::SiconosVector npc)
  {
    _Pc2 = npc;
  };

  /** Set the coordinates of inside normal vector at the contact point.
   * Must only be done in a computeh() override.
  * \param nnc new coordinates
  */
  void setnc(SP::SiconosVector nnc)
  {
    _Nc = nnc;
  };

private:
  void NIcomputeJachqTFromContacts(SP::SiconosVector q1);
  void NIcomputeJachqTFromContacts(SP::SiconosVector q1, SP::SiconosVector q2);
public:

  /** V.A. boolean _isOnCOntact ?? Why is it public members ?
  *  seems parametrize the projection algorithm
  *  the projection is done on the surface \f$y=0\f$ or on \f$y \geq 0\f$
  */
  bool _isOnContact;

  /** constructorx
  */
  NewtonEulerFrom1DLocalFrameR():
    NewtonEulerR(), _Pc1(new SiconosVector(3)), _Pc2(new SiconosVector(3)),
    _relPc1(new SiconosVector(3)), _relPc2(new SiconosVector(3)),
    _Nc(new SiconosVector(3)), _relNc(new SiconosVector(3))
  {
    /*_ds1=NULL;_ds2=NULL;*/
  }

  /** destructor
  */
  virtual ~NewtonEulerFrom1DLocalFrameR() {};

  virtual void computeJachq(double time, Interaction& inter, SP::BlockVector q0);

  virtual void initializeWorkVectorsAndMatrices(Interaction& inter, VectorOfBlockVectors& DSlink, VectorOfVectors& workV, VectorOfSMatrices& workM);

  virtual void initialize(Interaction& inter);

  /* Default implementation consists in multiplying jachq and T (see NewtonEulerR::computeJachqT)
   * but here we compute the operator from the the contact point locations
   * and the local frame at contact
   *  \param inter interaction that owns the relation
   *  \param q0  the block vector to the dynamical system position
   */
  virtual void computeJachqT(Interaction& inter, SP::BlockVector q0);

  /* Default implementation of computeh updates contact points and
   * distance for q if different than qold. */
  virtual void computeh(double time, BlockVector& q0, SiconosVector &y);

  /** Return the distance between pc1 and pc, with sign according to normal */
  double distance() const;

  inline SP::SiconosVector pc1() const
  {
    return _Pc1;
  }
  inline SP::SiconosVector pc2() const
  {
    return _Pc2;
  }
  inline SP::SiconosVector nc() const
  {
    return _Nc;
  }

  inline SP::SiconosVector relPc1() const
  {
    return _relPc1;
  }
  inline SP::SiconosVector relPc2() const
  {
    return _relPc2;
  }
  inline SP::SiconosVector relNc() const
  {
    return _relNc;
  }

  /** Set the coordinates of first contact point in ds1 frame.
   * It will be used to compute _Pc1 during computeh().
  * \param npc new coordinates
  */
  void setRelPc1(SP::SiconosVector npc)
  {
    _relPc1 = npc;
  };

  /** Set the coordinates of second contact point in ds2 frame
   * It will be used to compute _Pc2 during computeh().
  * \param npc new coordinates
  */
  void setRelPc2(SP::SiconosVector npc)
  {
    _relPc2 = npc;
  };

  /** Set the coordinates of inside normal vector at the contact point in ds2 frame.
   * It will be used to compute _Nc during computeh().
  * \param nnc new coordinates
  */
  void setRelNc(SP::SiconosVector nnc)
  {
    _relNc = nnc;
  };

  // visitors hook
  ACCEPT_STD_VISITORS();

};
#endif // NEWTONEULERRIMPACT_H
<|endoftext|>


<commit_before>
/////////////////////////////////////////////////////////////////////////////
// Copyright (c) 1996, California Institute of Technology.
// ALL RIGHTS RESERVED.   U.S. Government Sponsorship acknowledged.
//
// Please read the full copyright notice in the file COPYRIGH
// in this directory.
//
// Author: Todd Karakashian, NASA/Jet Propulsion Laboratory
//         Todd.K.Karakashian@jpl.nasa.gov
//
// $RCSfile: escaping.cc,v $ - Miscellaneous routines for DODS HDF server
//
// $Log: escaping.cc,v $
// Revision 1.3  1997/02/14 02:24:44  jimg
// Removed reliance on the dods-hdf code.
// Introduced a const int MAXSTR with value 256. This matches the length of
// ID_MAX in the parser.h header (which I did not include since it defines
// a lot of software that is irrelevant to this module).
//
// Revision 1.2  1997/02/14 02:18:16  jimg
// Added to DODS core
//
// Revision 1.2  1996/10/07 21:15:17  todd
// Changes escape character to % from _.
//
// Revision 1.1  1996/09/24 22:38:16  todd
// Initial revision
//
//
/////////////////////////////////////////////////////////////////////////////

// These two routines are for escaping/unescaping strings that are identifiers
// in DODS
// id2dods() -- escape (using WWW hex codes) non-allowable characters in a
// DODS identifier
// dods2id() -- given an WWW hexcode escaped identifier, restore it
//
// These two routines are for escaping/unescaping strings storing attribute
// values.  They use traditional octal escapes (\nnn) because they are
// intended to be viewed by a user
// escattr() -- escape (using traditional octal backslash) non-allowable
// characters in the value of a DODS attribute
// unescattr() -- given an octally escaped string, restore it
//
// These are routines used by the above, not intended to be called directly:
//
// hexstring()
// unhexstring()
// octstring()
// unoctstring()
//
// -Todd

#include <ctype.h>
#include <strstream.h>
#include <iomanip.h>
#include <String.h>

#if 0
#include <mfhdf.h>
#include <hdfclass.h>
#include "HDFStructure.h"
#include "HDFArray.h"
#include "hdfutil.h"
#include "dhdferr.h"
#endif

const int MAXSTR = 256;

String hexstring(int val) {
    static char buf[MAXSTR];

    ostrstream(buf,MAXSTR) << hex << setw(2) << setfill('0') <<
	val << ends;

    return (String)buf;
}

char unhexstring(String s) {
    int val;
    static char buf[MAXSTR];

    strcpy(buf,(const char *)s);
    istrstream(buf,MAXSTR) >> hex >> val;

    return (char)val;
}

String octstring(int val) {
    static char buf[MAXSTR];

    ostrstream(buf,MAXSTR) << oct << setw(3) << setfill('0') <<
	val << ends;

    return (String)buf;
}

char unoctstring(String s) {
    int val;
    static char buf[MAXSTR];

    strcpy(buf,(const char *)s);
    istrstream(buf,MAXSTR) >> oct >> val;

    return (char)val;
}

// replace characters that are not allowed in DODS identifiers
String id2dods(String s) {
    static Regex badregx = "[^0-9a-zA-Z_%]";
    const char ESC = '%';

    int index;
    while ( (index = s.index(badregx)) >= 0)
	s.at(index,1) = ESC + hexstring(toascii(s[index]));
    if (isdigit(s[0]))
	s.before(0) = '_';
    return s;
}

String dods2id(String s) {
    static Regex escregx = "%[0-7][0-9a-fA-F]";

    int index;
    while ( (index = s.index(escregx)) >= 0)
	s.at(index,3) = unhexstring(s.at(index+1,2));

    return s;
}

// Escape non-printable characters and quotes from an HDF attribute
String escattr(String s) {
    static Regex nonprintable = "[^ !-~]";
    const char ESC = '\\';
    const char QUOTE = '\"';
    const String ESCQUOTE = (String)ESC + (String)QUOTE;

    // escape non-printing characters with octal escape
    int index = 0;
    while ( (index = s.index(nonprintable)) >= 0)
	s.at(index,1) = ESC + octstring(toascii(s[index]));

    // escape " with backslash
    index = 0;
    while ( (index = s.index(QUOTE,index)) >= 0) {
	s.at(index,1) = ESCQUOTE;
	index += ESCQUOTE.length();
    }

    return s;
}

// Un-escape special characters, quotes and backslashes from an HDF attribute.
// Note: A regex to match one \ must be defined as
//          Regex foo = "\\\\";
//       because both C++ strings and libg++ regex's also employ \ as
//       an escape character!
String unescattr(String s) {
    static Regex escregx = "\\\\[01][0-7][0-7]";  // matches 4 characters
    static Regex escquoteregex = "[^\\\\]\\\\\"";  // matches 3 characters
    static Regex escescregex = "\\\\\\\\";      // matches 2 characters
    const char ESC = '\\';
    const char QUOTE = '\"';
    const String ESCQUOTE = (String)ESC + (String)QUOTE;

    // unescape any octal-escaped ASCII characters
    int index = 0;
    while ( (index = s.index(escregx,index)) >= 0) {
	s.at(index,4) = unoctstring(s.at(index+1,3));
	index++;
    }

    // unescape any escaped quotes
    index = 0;
    while ( (index = s.index(escquoteregex,index)) >= 0) {
	s.at(index+1,2) = QUOTE;
	index++;
    }

    // unescape any escaped backslashes
    index = 0;
    while ( (index = s.index(escescregex,index)) >= 0) {
	s.at(index,2) = ESC;
	index++;
    }

    return s;
}
<commit_msg>Added allowable and escape parameters to id2dods and dods2id so that the builtin regexs can be overridden if needed. Switched to the `fast compile' mode for the Regex objects.<commit_after>
/////////////////////////////////////////////////////////////////////////////
// Copyright (c) 1996, California Institute of Technology.
// ALL RIGHTS RESERVED.   U.S. Government Sponsorship acknowledged.
//
// Please read the full copyright notice in the file COPYRIGH
// in this directory.
//
// Author: Todd Karakashian, NASA/Jet Propulsion Laboratory
//         Todd.K.Karakashian@jpl.nasa.gov
//
// $RCSfile: escaping.cc,v $ - Miscellaneous routines for DODS HDF server
//
// $Log: escaping.cc,v $
// Revision 1.4  1997/02/14 04:18:10  jimg
// Added allowable and escape parameters to id2dods and dods2id so that the
// builtin regexs can be overridden if needed.
// Switched to the `fast compile' mode for the Regex objects.
//
// Revision 1.3  1997/02/14 02:24:44  jimg
// Removed reliance on the dods-hdf code.
// Introduced a const int MAXSTR with value 256. This matches the length of
// ID_MAX in the parser.h header (which I did not include since it defines
// a lot of software that is irrelevant to this module).
//
// Revision 1.2  1997/02/14 02:18:16  jimg
// Added to DODS core
//
// Revision 1.2  1996/10/07 21:15:17  todd
// Changes escape character to % from _.
//
// Revision 1.1  1996/09/24 22:38:16  todd
// Initial revision
//
//
/////////////////////////////////////////////////////////////////////////////

// These two routines are for escaping/unescaping strings that are identifiers
// in DODS
// id2dods() -- escape (using WWW hex codes) non-allowable characters in a
// DODS identifier
// dods2id() -- given an WWW hexcode escaped identifier, restore it
//
// These two routines are for escaping/unescaping strings storing attribute
// values.  They use traditional octal escapes (\nnn) because they are
// intended to be viewed by a user
// escattr() -- escape (using traditional octal backslash) non-allowable
// characters in the value of a DODS attribute
// unescattr() -- given an octally escaped string, restore it
//
// These are routines used by the above, not intended to be called directly:
//
// hexstring()
// unhexstring()
// octstring()
// unoctstring()
//
// -Todd

#include <ctype.h>
#include <strstream.h>
#include <iomanip.h>
#include <String.h>

#if 0
#include <mfhdf.h>
#include <hdfclass.h>
#include "HDFStructure.h"
#include "HDFArray.h"
#include "hdfutil.h"
#include "dhdferr.h"
#endif

const int MAXSTR = 256;

String hexstring(int val) {
    static char buf[MAXSTR];

    ostrstream(buf,MAXSTR) << hex << setw(2) << setfill('0') <<
	val << ends;

    return (String)buf;
}

char unhexstring(String s) {
    int val;
    static char buf[MAXSTR];

    strcpy(buf,(const char *)s);
    istrstream(buf,MAXSTR) >> hex >> val;

    return (char)val;
}

String octstring(int val) {
    static char buf[MAXSTR];

    ostrstream(buf,MAXSTR) << oct << setw(3) << setfill('0') <<
	val << ends;

    return (String)buf;
}

char unoctstring(String s) {
    int val;
    static char buf[MAXSTR];

    strcpy(buf,(const char *)s);
    istrstream(buf,MAXSTR) >> oct >> val;

    return (char)val;
}

// replace characters that are not allowed in DODS identifiers
String id2dods(String s, const String allowable = (char *)0) {
    static Regex badregx("[^0-9a-zA-Z_%]", 1);
    static const char ESC = '%';

    if (allowable) {
	Regex badregx2(allowable, 1);

	int index;
	while ((index = s.index(badregx2)) >= 0)
	    s.at(index,1) = ESC + hexstring(toascii(s[index]));
    }
    else {
	int index;
	while ((index = s.index(badregx)) >= 0)
	    s.at(index,1) = ESC + hexstring(toascii(s[index]));
    }

    if (isdigit(s[0]))
	s.before(0) = '_';

    return s;
}

String dods2id(String s, const String escape = (char *)0) {
    static Regex escregx("%[0-7][0-9a-fA-F]", 1);

    if (escape) {
	Regex escregx2(escape, 1);
	int index;
	while ((index = s.index(escregx2)) >= 0)
	    s.at(index,3) = unhexstring(s.at(index+1,2));
    }
    else {
	int index;
	while ((index = s.index(escregx)) >= 0)
	    s.at(index,3) = unhexstring(s.at(index+1,2));
    }
    return s;
}

// Escape non-printable characters and quotes from an HDF attribute
String escattr(String s) {
    static Regex nonprintable = "[^ !-~]";
    const char ESC = '\\';
    const char QUOTE = '\"';
    const String ESCQUOTE = (String)ESC + (String)QUOTE;

    // escape non-printing characters with octal escape
    int index = 0;
    while ( (index = s.index(nonprintable)) >= 0)
	s.at(index,1) = ESC + octstring(toascii(s[index]));

    // escape " with backslash
    index = 0;
    while ( (index = s.index(QUOTE,index)) >= 0) {
	s.at(index,1) = ESCQUOTE;
	index += ESCQUOTE.length();
    }

    return s;
}

// Un-escape special characters, quotes and backslashes from an HDF attribute.
// Note: A regex to match one \ must be defined as
//          Regex foo = "\\\\";
//       because both C++ strings and libg++ regex's also employ \ as
//       an escape character!
String unescattr(String s) {
    static Regex escregx = "\\\\[01][0-7][0-7]";  // matches 4 characters
    static Regex escquoteregex = "[^\\\\]\\\\\"";  // matches 3 characters
    static Regex escescregex = "\\\\\\\\";      // matches 2 characters
    const char ESC = '\\';
    const char QUOTE = '\"';
    const String ESCQUOTE = (String)ESC + (String)QUOTE;

    // unescape any octal-escaped ASCII characters
    int index = 0;
    while ( (index = s.index(escregx,index)) >= 0) {
	s.at(index,4) = unoctstring(s.at(index+1,3));
	index++;
    }

    // unescape any escaped quotes
    index = 0;
    while ( (index = s.index(escquoteregex,index)) >= 0) {
	s.at(index+1,2) = QUOTE;
	index++;
    }

    // unescape any escaped backslashes
    index = 0;
    while ( (index = s.index(escescregex,index)) >= 0) {
	s.at(index,2) = ESC;
	index++;
    }

    return s;
}
<|endoftext|>


<commit_before>//
// C++ exception wrappers
//
//   These will allow you to detect where an exception is thrown,
//   and where it is caught.
//

#include <core/System.h>
#include <core/Interpose.h>
#include <stack/ThrowHandler.h>
#include <stack/CatchHandler.h>

#include <cxxabi.h>
#include <exception>

using namespace exray;

typedef void  (*CxaThrowFuncPtr)(void *, std::type_info *, void(*)(void *));
typedef void* (*CxaBeginCatchFuncPtr)(void *);

namespace exray {
    namespace CXX {
        CxaThrowFuncPtr      throwFunc = NULL;
        CxaBeginCatchFuncPtr catchFunc = NULL;
    }
}

extern "C"
{
    void __cxa_throw(void *thrownException,
                     std::type_info *tinfo,
                     void (*dest) (void *))
    {
        if (exray::CXX::throwFunc == NULL) {
            exray::CXX::throwFunc = (CxaThrowFuncPtr)exray::Interpose::loadFuncPtr("__cxa_throw");
        }
        if (System::isThreadUnwinding() == false) {
            ThrowHandler *handler = System::getThrowHandler();
            handler->captureFrames(*tinfo, "__cxa_throw");
            System::setThreadUnwinding();
        }
        exray::CXX::throwFunc(thrownException, tinfo, dest);
    }

    void *__cxa_begin_catch(void *exceptionObject) throw()
    {
        if (exray::CXX::catchFunc == NULL) {
            exray::CXX::catchFunc = (CxaBeginCatchFuncPtr)exray::Interpose::loadFuncPtr("__cxa_begin_catch");
        }
        System::clearThreadUnwinding();
        CatchHandler *handler = System::getCatchHandler();
        handler->captureFrames("__cxa_begin_catch");
        exray::CXX::catchFunc(exceptionObject);
    }
}

#if defined(CPPMODE) && (CPPMODE == CPP11)
typedef void  (*CxaRethrowExceptionFuncPtr)(std::__exception_ptr::exception_ptr);

namespace exray {
    namespace CXX {
        CxaRethrowExceptionFuncPtr rethrowExceptionFunc __attribute__ ((__noreturn__)) = NULL;
    }
}

namespace std
{
    static const char *RETHROW_FUNC_MANGLED = "_ZSt17rethrow_exceptionNSt15__exception_ptr13exception_ptrE";

    void rethrow_exception(__exception_ptr::exception_ptr p)
    {
        if (CXX::rethrowExceptionFunc == NULL) {
            CXX::rethrowExceptionFunc =
                (CxaRethrowExceptionFuncPtr)exray::Interpose::loadFuncPtr(RETHROW_FUNC_MANGLED);
        }
        if (System::isThreadUnwinding() == false) {
            ThrowHandler *handler = System::getThrowHandler();
            handler->captureFrames(*p.__cxa_exception_type(), "__cxa_rethrow");
            System::setThreadUnwinding();
        }
        CXX::rethrowExceptionFunc(p);
    }
}
#endif
<commit_msg>Fixing signature for std::rethrow_exception()<commit_after>//
// C++ exception wrappers
//
//   These will allow you to detect where an exception is thrown,
//   and where it is caught.
//

#include <core/System.h>
#include <core/Interpose.h>
#include <stack/ThrowHandler.h>
#include <stack/CatchHandler.h>

#include <cxxabi.h>
#include <exception>

using namespace exray;

typedef void  (*CxaThrowFuncPtr)(void *, std::type_info *, void(*)(void *));
typedef void* (*CxaBeginCatchFuncPtr)(void *);

namespace exray {
    namespace CXX {
        CxaThrowFuncPtr      throwFunc = NULL;
        CxaBeginCatchFuncPtr catchFunc = NULL;
    }
}

extern "C"
{
    void __cxa_throw(void *thrownException,
                     std::type_info *tinfo,
                     void (*dest) (void *))
    {
        if (exray::CXX::throwFunc == NULL) {
            exray::CXX::throwFunc = (CxaThrowFuncPtr)exray::Interpose::loadFuncPtr("__cxa_throw");
        }
        if (System::isThreadUnwinding() == false) {
            ThrowHandler *handler = System::getThrowHandler();
            handler->captureFrames(*tinfo, "__cxa_throw");
            System::setThreadUnwinding();
        }
        exray::CXX::throwFunc(thrownException, tinfo, dest);
    }

    void *__cxa_begin_catch(void *exceptionObject) throw()
    {
        if (exray::CXX::catchFunc == NULL) {
            exray::CXX::catchFunc = (CxaBeginCatchFuncPtr)exray::Interpose::loadFuncPtr("__cxa_begin_catch");
        }
        System::clearThreadUnwinding();
        CatchHandler *handler = System::getCatchHandler();
        handler->captureFrames("__cxa_begin_catch");
        exray::CXX::catchFunc(exceptionObject);
    }
}

#if defined(CPPMODE) && (CPPMODE == CPP11)
typedef void  (*CxaRethrowExceptionFuncPtr)(std::__exception_ptr::exception_ptr);

namespace exray {
    namespace CXX {
        CxaRethrowExceptionFuncPtr rethrowExceptionFunc __attribute__ ((__noreturn__)) = NULL;
    }
}

namespace std
{
    static const char *RETHROW_FUNC_MANGLED = "_ZSt17rethrow_exceptionNSt15__exception_ptr13exception_ptrE";

    void rethrow_exception(__exception_ptr::exception_ptr p)
    {
        if (CXX::rethrowExceptionFunc == NULL) {
            CXX::rethrowExceptionFunc =
                (CxaRethrowExceptionFuncPtr)exray::Interpose::loadFuncPtr(RETHROW_FUNC_MANGLED);
        }
        if (System::isThreadUnwinding() == false) {
            ThrowHandler *handler = System::getThrowHandler();
            handler->captureFrames(*p.__cxa_exception_type(), "std::rethrow_exception()");
            System::setThreadUnwinding();
        }
        CXX::rethrowExceptionFunc(p);
    }
}
#endif
<|endoftext|>


<commit_before>// Copyright NVIDIA Corporation 2007 -- Ignacio Castano <icastano@nvidia.com>
//
// Permission is hereby granted, free of charge, to any person
// obtaining a copy of this software and associated documentation
// files (the "Software"), to deal in the Software without
// restriction, including without limitation the rights to use,
// copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following
// conditions:
//
// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
// OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
// WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
// OTHER DEALINGS IN THE SOFTWARE.

#include <nvcore/Debug.h>
#include "CudaUtils.h"

#if defined HAVE_CUDA
#include <cuda_runtime.h>
#endif

using namespace nv;
using namespace cuda;

#if NV_OS_WIN32

#define WINDOWS_LEAN_AND_MEAN
#include <windows.h>

static bool isWindowsVista()
{
	OSVERSIONINFO osvi;
	osvi.dwOSVersionInfoSize = sizeof(OSVERSIONINFO);

	::GetVersionEx(&osvi);
	return osvi.dwMajorVersion >= 6;
}

typedef BOOL (WINAPI *LPFN_ISWOW64PROCESS) (HANDLE, PBOOL);

static bool isWow32()
{
	LPFN_ISWOW64PROCESS fnIsWow64Process = (LPFN_ISWOW64PROCESS)GetProcAddress(GetModuleHandle("kernel32"), "IsWow64Process");

    BOOL bIsWow64 = FALSE;

    if (NULL != fnIsWow64Process)
    {
        if (!fnIsWow64Process(GetCurrentProcess(), &bIsWow64))
        {
			// Assume 32 bits.
            return true;
        }
    }

    return !bIsWow64;
}

#endif

/// Determine if CUDA is available.
bool nv::cuda::isHardwarePresent()
{
#if defined HAVE_CUDA
#if NV_OS_WIN32
	//if (isWindowsVista()) return false;
	//if (isWindowsVista() || !isWow32()) return false;
#endif
	int count = deviceCount();
	if (count == 1)
	{
		// Make sure it's not an emulation device.
		cudaDeviceProp deviceProp;
		cudaGetDeviceProperties(&deviceProp, 0);

		// deviceProp.name != Device Emulation (CPU)
		if (deviceProp.major == -1 || deviceProp.minor == -1)
		{
			return false;
		}

		// @@ Make sure that warp size == 32
	}

	return count > 0;
#else
	return false;
#endif
}

/// Get number of CUDA enabled devices.
int nv::cuda::deviceCount()
{
#if defined HAVE_CUDA
	int gpuCount = 0;

	cudaError_t result = cudaGetDeviceCount(&gpuCount);

	if (result == cudaSuccess)
	{
		return gpuCount;
	}
#endif
	return 0;
}

/// Activate the given devices.
bool nv::cuda::setDevice(int i)
{
	nvCheck(i < deviceCount());
#if defined HAVE_CUDA
	cudaError_t result = cudaSetDevice(i);
	return result == cudaSuccess;
#else
	return false;
#endif
}
<commit_msg>Workaround bug in CUDA runtime. When using CUDA 2.0, it's required to use a driver that supports CUDA 2.0.<commit_after>// Copyright NVIDIA Corporation 2007 -- Ignacio Castano <icastano@nvidia.com>
//
// Permission is hereby granted, free of charge, to any person
// obtaining a copy of this software and associated documentation
// files (the "Software"), to deal in the Software without
// restriction, including without limitation the rights to use,
// copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following
// conditions:
//
// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
// OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
// WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
// OTHER DEALINGS IN THE SOFTWARE.

#include <nvcore/Debug.h>
#include "CudaUtils.h"

#if defined HAVE_CUDA
#include <cuda_runtime.h>
#endif

using namespace nv;
using namespace cuda;

#if NV_OS_WIN32

#define WINDOWS_LEAN_AND_MEAN
#include <windows.h>

static bool isWindowsVista()
{
	OSVERSIONINFO osvi;
	osvi.dwOSVersionInfoSize = sizeof(OSVERSIONINFO);

	::GetVersionEx(&osvi);
	return osvi.dwMajorVersion >= 6;
}

typedef BOOL (WINAPI *LPFN_ISWOW64PROCESS) (HANDLE, PBOOL);

static bool isWow32()
{
	LPFN_ISWOW64PROCESS fnIsWow64Process = (LPFN_ISWOW64PROCESS)GetProcAddress(GetModuleHandle("kernel32"), "IsWow64Process");

	BOOL bIsWow64 = FALSE;

	if (NULL != fnIsWow64Process)
	{
		if (!fnIsWow64Process(GetCurrentProcess(), &bIsWow64))
		{
			// Assume 32 bits.
			return true;
		}
	}

	return !bIsWow64;
}

#endif

#if	NV_OS_WIN32

static bool isCuda20DriverAvailable()
{
	bool result = false;

	HINSTANCE nvcuda = LoadLibraryExA( "nvcuda.dll", NULL, 0 );
	if (nvcuda != NULL)
	{
		FARPROC lcuStreamCreate = GetProcAddress( nvcuda, "cuStreamCreate" );

		result = lcuStreamCreate != NULL;

		FreeLibrary( nvcuda );
	}

	return result;
}

#endif

/// Determine if CUDA is available.
bool nv::cuda::isHardwarePresent()
{
#if defined HAVE_CUDA
#if NV_OS_WIN32
	//if (isWindowsVista()) return false;
	//if (isWindowsVista() || !isWow32()) return false;
#endif
	int count = deviceCount();
	if (count == 1)
	{
		// Make sure it's not an emulation device.
		cudaDeviceProp deviceProp;
		cudaGetDeviceProperties(&deviceProp, 0);

		// deviceProp.name != Device Emulation (CPU)
		if (deviceProp.major == -1 || deviceProp.minor == -1)
		{
			return false;
		}

		// Make sure that CUDA driver matches CUDA runtime.
#if NV_OS_WIN32 && CUDART_VERSION >= 2000
		if (!isCuda20DriverAvailable())
		{
			return false;
		}
#endif

		// @@ Make sure that warp size == 32
	}

	return count > 0;
#else
	return false;
#endif
}

/// Get number of CUDA enabled devices.
int nv::cuda::deviceCount()
{
#if defined HAVE_CUDA
	int gpuCount = 0;

	cudaError_t result = cudaGetDeviceCount(&gpuCount);

	if (result == cudaSuccess)
	{
		return gpuCount;
	}
#endif
	return 0;
}

/// Activate the given devices.
bool nv::cuda::setDevice(int i)
{
	nvCheck(i < deviceCount());
#if defined HAVE_CUDA
	cudaError_t result = cudaSetDevice(i);
	return result == cudaSuccess;
#else
	return false;
#endif
}
<|endoftext|>


<commit_before>/**
 * Copyright (c) 2011-2015 libbitcoin developers (see AUTHORS)
 *
 * This file is part of libbitcoin-explorer.
 *
 * libbitcoin-explorer is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Affero General Public License with
 * additional permissions to the one published by the Free Software
 * Foundation, either version 3 of the License, or (at your option)
 * any later version. For more information see LICENSE.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */

#include <bitcoin/explorer/commands/send-tx-node.hpp>

#include <iostream>
#include <boost/format.hpp>
#include <bitcoin/bitcoin.hpp>
#include <bitcoin/explorer/async_client.hpp>
#include <bitcoin/explorer/callback_state.hpp>
#include <bitcoin/explorer/define.hpp>
#include <bitcoin/explorer/primitives/transaction.hpp>
#include <bitcoin/explorer/utility.hpp>

using namespace bc;
using namespace bc::explorer;
using namespace bc::explorer::commands;
using namespace bc::explorer::primitives;

console_result send_tx_node::invoke(std::ostream& output, std::ostream& error)
{
    // Bound parameters.
    const auto& node = get_host_option();
    const auto& port = get_port_option();
    const tx_type& transaction = get_transaction_argument();
    const auto& debug_file = get_logging_debug_file_setting();
    const auto& error_file = get_logging_error_file_setting();

    bc::ofstream debug_log(debug_file.string(), log_open_mode);
    bc::ofstream error_log(error_file.string(), log_open_mode);
    bind_debug_log(debug_log);
    bind_error_log(error_log);

    const static auto headline = "================= Startup =================";
    log_fatal(LOG_NETWORK) << headline;
    log_error(LOG_NETWORK) << headline;
    log_warning(LOG_NETWORK) << headline;
    log_info(LOG_NETWORK) << headline;
    log_debug(LOG_NETWORK) << headline;

    constexpr size_t threads = 4;
    constexpr size_t poll_period_ms = 2000;

    async_client client(threads);
    auto& pool = client.get_threadpool();
    bc::network::hosts host(pool);
    bc::network::handshake shake(pool);
    bc::network::network net(pool);
    bc::network::protocol proto(pool, host, shake, net);

    callback_state state(error, output);

    const auto start_handler = [&state](const std::error_code& code)
    {
        state.handle_error(code);
    };

    const auto send_handler = [&state](const std::error_code& code)
    {
        if (state.handle_error(code))
            state.output(format(BX_SEND_TX_OUTPUT) % now());

        --state;
    };

    const auto channel_handler = [&state, &send_handler](
        const std::error_code& code, bc::network::channel_ptr node,
        bc::network::protocol&, const tx_type& tx)
    {
        if (state.handle_error(code))
            node->send(tx, send_handler);
    };

    bool stopped = false;
    const auto stop_handler = [&state, &stopped](const std::error_code& code)
    {
        state.handle_error(code);
        stopped = true;
    };

    ++state;

    // Handle each successful connection.
    proto.subscribe_channel(
        std::bind(channel_handler,
            ph::_1, ph::_2, std::ref(proto), std::ref(transaction)));

    // Connect to the explicitly-specified host.
    proto.maintain_connection(node, port);

    client.poll(state.stopped(), poll_period_ms);
    proto.stop(stop_handler);

    // Delay until the protocol stop handler has been called.
    while (!stopped)
        sleep_ms(1);

    client.stop();

    // BUGBUG: The server may not have time to process the message before the
    // connection is dropped.
    return state.get_result();
}
<commit_msg>Handle ctrl-c, fix constant, remove redundant loop, disable tx relays.<commit_after>/**
 * Copyright (c) 2011-2015 libbitcoin developers (see AUTHORS)
 *
 * This file is part of libbitcoin-explorer.
 *
 * libbitcoin-explorer is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Affero General Public License with
 * additional permissions to the one published by the Free Software
 * Foundation, either version 3 of the License, or (at your option)
 * any later version. For more information see LICENSE.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */

#include <bitcoin/explorer/commands/send-tx-node.hpp>

#include <csignal>
#include <iostream>
#include <boost/format.hpp>
#include <bitcoin/bitcoin.hpp>
#include <bitcoin/explorer/async_client.hpp>
#include <bitcoin/explorer/callback_state.hpp>
#include <bitcoin/explorer/define.hpp>
#include <bitcoin/explorer/primitives/transaction.hpp>
#include <bitcoin/explorer/utility.hpp>

using namespace bc;
using namespace bc::explorer;
using namespace bc::explorer::commands;
using namespace bc::explorer::primitives;

static void handle_signal(int signal)
{
    // Can't pass args using lambda capture for a simple function pointer.
    // This means there's no way to terminate without using a global variable
    // or process termination. Since the variable would screw with testing all
    // other methods we opt for process termination here.
    exit(console_result::failure);
}

console_result send_tx_node::invoke(std::ostream& output, std::ostream& error)
{
    // Bound parameters.
    const auto& node = get_host_option();
    const auto& port = get_port_option();
    const tx_type& transaction = get_transaction_argument();
    const auto& debug_file = get_logging_debug_file_setting();
    const auto& error_file = get_logging_error_file_setting();

    // These haven't been used for node (or p2p) connections.
    const auto retries = get_general_retries_setting();
    const auto wait = get_general_wait_setting();

    bc::ofstream debug_log(debug_file.string(), log_open_mode);
    bc::ofstream error_log(error_file.string(), log_open_mode);
    bind_debug_log(debug_log);
    bind_error_log(error_log);

    const static auto headline = "================= Startup =================";
    log_fatal(LOG_NETWORK) << headline;
    log_error(LOG_NETWORK) << headline;
    log_warning(LOG_NETWORK) << headline;
    log_info(LOG_NETWORK) << headline;
    log_debug(LOG_NETWORK) << headline;

    constexpr size_t threads = 4;
    constexpr size_t poll_period_ms = 2000;

    async_client client(threads);
    auto& pool = client.get_threadpool();
    bc::network::hosts host(pool, 0);
    bc::network::handshake shake(pool);
    bc::network::network net(pool);
    bc::network::protocol proto(pool, host, shake, net);

    callback_state state(error, output);

    const auto start_handler = [&state](const std::error_code& code)
    {
        state.handle_error(code);
    };

    const auto send_handler = [&state](const std::error_code& code)
    {
        if (state.handle_error(code))
            state.output(format(BX_SEND_TX_NODE_OUTPUT) % now());

        --state;
    };

    const auto channel_handler = [&state, &send_handler](
        const std::error_code& code, bc::network::channel_ptr node,
        bc::network::protocol&, const tx_type& tx)
    {
        if (state.handle_error(code))
            node->send(tx, send_handler);
    };

    bool stopped = false;
    const auto stop_handler = [&state, &stopped](const std::error_code& code)
    {
        state.handle_error(code);
        stopped = true;
    };

    ++state;

    // Handle each successful connection.
    proto.subscribe_channel(
        std::bind(channel_handler,
            ph::_1, ph::_2, std::ref(proto), std::ref(transaction)));

    // Connect to the explicitly-specified host.
    proto.maintain_connection(node, port, false);

    // Catch C signals for aborting the program.
    signal(SIGABRT, handle_signal);
    signal(SIGTERM, handle_signal);
    signal(SIGINT, handle_signal);

    client.poll(state.stopped(), poll_period_ms);
    proto.stop(stop_handler);
    client.stop();

    return state.get_result();
}
<|endoftext|>


<commit_before>#include "tgatexture.h"
#include "detail/TextureImpl.h"
#include "ifile.h"
#include <boost/assert.hpp>
#include <algorithm>

namespace platform {
namespace {
struct Header
{
    uint8_t mData[18];
    // don't wanna pragma pack
    uint8_t IdLength()const
    {
        return mData[0];
    }
    uint8_t ColorMapType()const
    {
        return mData[1];
    }
    uint8_t ImageType()const
    {
        return mData[2];
    }
    uint16_t ColMapStart()const
    {
        return mData[3] | ( ( uint16_t )mData[4] << 8 );
    }
    uint16_t ColMapLength()const
    {
        return mData[5] | ( ( uint16_t )mData[6] << 8 );
    }
    uint8_t ColMapDepth()const
    {
        return mData[7];
    }
    uint16_t Origin()const
    {
        return mData[8] | ( ( uint16_t )mData[9] << 8 );
    }
    uint16_t OriginY()const
    {
        return mData[10] | ( ( uint16_t )mData[11] << 8 );
    }
    uint16_t Width()const
    {
        return mData[12] | ( ( uint16_t )mData[13] << 8 );
    }
    uint16_t Height()const
    {
        return mData[14] | ( ( uint16_t )mData[15] << 8 );
    }
    uint8_t PixelDepth()const
    {
        return mData[16];
    }
    uint8_t ImageDescriptor()const
    {
        return mData[17];
    }
};

void Reorder( uint8_t* Data, size_t Size );

detail::TextureImpl* Load( File& F )
{
    Header Head;
    std::auto_ptr<detail::TextureImpl> impl( new detail::TextureImpl );
    if( !F.Read( Head.mData, sizeof( Header ) ) )
    {
        return NULL;
    }
    static uint16_t const MaxSupportedImageSize = 1 << 15;
    assert( Head.Width() <= MaxSupportedImageSize && Head.Height() < MaxSupportedImageSize );
    impl->mWidth = Head.Width();
    impl->mHeight = Head.Height();
    assert( Head.PixelDepth() == 24 || Head.PixelDepth() == 32 );
    if( Head.PixelDepth() != 24 && Head.PixelDepth() != 32 )
    {
        return NULL;
    }
    size_t const ImgSize = impl->mWidth * impl->mHeight;
    size_t const DataSize = detail::TextureImpl::mChannels * ImgSize;
    impl->mData.resize( DataSize );
    bool Succ;
    if( Head.PixelDepth() == 32 )
    {
        assert( ( Head.ImageDescriptor() & 15 ) == 8 );
        Succ = F.Read( static_cast< void* >( &impl->mData.at( 0 ) ), DataSize );
    }
    else
    {
        std::string Buf;
        size_t const InSize = ImgSize * 3;
        Succ = F.Read( Buf, InSize );
        if( Succ )
        {
            detail::TextureImpl::ConvertRGBtoRGBA( ( uint8_t* )( void* )Buf.c_str(), ImgSize, &impl->mData.at( 0 ) );
        }
    }
    if( !Succ )
    {
        impl->mData.clear();
        return NULL;
    }
    else
    {
        Reorder( &impl->mData.at( 0 ), ImgSize );
    }
    return impl.release();
}

void Reorder( uint8_t* Data, size_t Size )
{
    for( size_t x = 0; x < Size; x++ )
    {
        unsigned char* pixel_out = &Data[x * 4];
        std::swap( pixel_out[0], pixel_out[2] );
    }
}
} // namespace anonymous

TgaTexture::TgaTexture( File& F )
{
    if( F.IsValid() )
    {
        mImpl.reset( Load( F ) );
    }
}

} // namespace platform

<commit_msg>B: < vs <= typo<commit_after>#include "tgatexture.h"
#include "detail/TextureImpl.h"
#include "ifile.h"
#include <boost/assert.hpp>
#include <algorithm>

namespace platform {
namespace {
struct Header
{
    uint8_t mData[18];
    // don't wanna pragma pack
    uint8_t IdLength()const
    {
        return mData[0];
    }
    uint8_t ColorMapType()const
    {
        return mData[1];
    }
    uint8_t ImageType()const
    {
        return mData[2];
    }
    uint16_t ColMapStart()const
    {
        return mData[3] | ( ( uint16_t )mData[4] << 8 );
    }
    uint16_t ColMapLength()const
    {
        return mData[5] | ( ( uint16_t )mData[6] << 8 );
    }
    uint8_t ColMapDepth()const
    {
        return mData[7];
    }
    uint16_t Origin()const
    {
        return mData[8] | ( ( uint16_t )mData[9] << 8 );
    }
    uint16_t OriginY()const
    {
        return mData[10] | ( ( uint16_t )mData[11] << 8 );
    }
    uint16_t Width()const
    {
        return mData[12] | ( ( uint16_t )mData[13] << 8 );
    }
    uint16_t Height()const
    {
        return mData[14] | ( ( uint16_t )mData[15] << 8 );
    }
    uint8_t PixelDepth()const
    {
        return mData[16];
    }
    uint8_t ImageDescriptor()const
    {
        return mData[17];
    }
};

void Reorder( uint8_t* Data, size_t Size );

detail::TextureImpl* Load( File& F )
{
    Header Head;
    std::auto_ptr<detail::TextureImpl> impl( new detail::TextureImpl );
    if( !F.Read( Head.mData, sizeof( Header ) ) )
    {
        return NULL;
    }
    static uint16_t const MaxSupportedImageSize = 1 << 15;
    assert( Head.Width() <= MaxSupportedImageSize && Head.Height() <= MaxSupportedImageSize );
    impl->mWidth = Head.Width();
    impl->mHeight = Head.Height();
    assert( Head.PixelDepth() == 24 || Head.PixelDepth() == 32 );
    if( Head.PixelDepth() != 24 && Head.PixelDepth() != 32 )
    {
        return NULL;
    }
    size_t const ImgSize = impl->mWidth * impl->mHeight;
    size_t const DataSize = detail::TextureImpl::mChannels * ImgSize;
    impl->mData.resize( DataSize );
    bool Succ;
    if( Head.PixelDepth() == 32 )
    {
        assert( ( Head.ImageDescriptor() & 15 ) == 8 );
        Succ = F.Read( static_cast< void* >( &impl->mData.at( 0 ) ), DataSize );
    }
    else
    {
        std::string Buf;
        size_t const InSize = ImgSize * 3;
        Succ = F.Read( Buf, InSize );
        if( Succ )
        {
            detail::TextureImpl::ConvertRGBtoRGBA( ( uint8_t* )( void* )Buf.c_str(), ImgSize, &impl->mData.at( 0 ) );
        }
    }
    if( !Succ )
    {
        impl->mData.clear();
        return NULL;
    }
    else
    {
        Reorder( &impl->mData.at( 0 ), ImgSize );
    }
    return impl.release();
}

void Reorder( uint8_t* Data, size_t Size )
{
    for( size_t x = 0; x < Size; x++ )
    {
        unsigned char* pixel_out = &Data[x * 4];
        std::swap( pixel_out[0], pixel_out[2] );
    }
}
} // namespace anonymous

TgaTexture::TgaTexture( File& F )
{
    if( F.IsValid() )
    {
        mImpl.reset( Load( F ) );
    }
}

} // namespace platform

<|endoftext|>