<commit_before>#ifndef IO_HELPERS_HPP_
#define IO_HELPERS_HPP_

#include "itkImage.h"
#include "itkImageFileReader.h"
#include "itkImageFileWriter.h"
#include "itkTransformFileWriter.h"

using namespace std;

inline vector< string > getFileNames(const string& directory, const string& fileList) {
  vector< string > fileNames;
  fileNames.clear();
  ifstream infile(fileList.c_str(), ios_base::in);
  string fileName;
  while (getline(infile, fileName)) {
    fileNames.push_back(directory + fileName);
  }

  return fileNames;
}

// Reader helper
template<typename ImageType>
ImageType::Pointer readImage(const string& fileName) {
  typedef itk::ImageFileReader< ImageType > ReaderType;
  typename ReaderType::Pointer reader = ReaderType::New();

  reader->SetFileName( fileName.c_str() );

  try {
  	reader->Update();
  }
	catch( itk::ExceptionObject & err ) {
    cerr << "ExceptionObject caught !" << endl;
    cerr << err << endl;
		exit(EXIT_FAILURE);
	}

  return reader->GetOutput();
}

// Writer helpers
// const Data
template<typename WriterType, typename DataType>
void writeData(const typename DataType::ConstPointer data, const string& fileName) {
  typename WriterType::Pointer writer = WriterType::New();

	writer->SetInput( data );
  // writer->AddTransform( anotherTransform ); // only applies to writing transforms

  writer->SetFileName( fileName.c_str() );

  try {
  	writer->Update();
  }
	catch( itk::ExceptionObject & err ) {
    cerr << "ExceptionObject caught !" << endl;
    cerr << err << endl;
		exit(EXIT_FAILURE);
	}
}

// Data
template<typename WriterType, typename DataType>
void writeData(const typename DataType::Pointer data, const string& fileName) {
  writeData< WriterType, DataType >( (typename DataType::ConstPointer) data, fileName);
}

// Const Image
template<typename ImageType>
void writeImage(const typename ImageType::ConstPointer image, const string& fileName) {
  writeData< itk::ImageFileWriter< ImageType >, ImageType >( image, fileName );
}

// Image
template<typename ImageType>
void writeImage(const typename ImageType::Pointer image, const string& fileName) {
  writeData< itk::ImageFileWriter< ImageType >, ImageType >( image, fileName );
}

#endif<commit_msg>Fixed typename bug.<commit_after>#ifndef IO_HELPERS_HPP_
#define IO_HELPERS_HPP_

#include "itkImage.h"
#include "itkImageFileReader.h"
#include "itkImageFileWriter.h"
#include "itkTransformFileWriter.h"

using namespace std;

inline vector< string > getFileNames(const string& directory, const string& fileList) {
  vector< string > fileNames;
  fileNames.clear();
  ifstream infile(fileList.c_str(), ios_base::in);
  string fileName;
  while (getline(infile, fileName)) {
    fileNames.push_back(directory + fileName);
  }

  return fileNames;
}

// Reader helper
template<typename ImageType>
typename ImageType::Pointer readImage(const string& fileName) {
  typedef itk::ImageFileReader< ImageType > ReaderType;
  typename ReaderType::Pointer reader = ReaderType::New();

  reader->SetFileName( fileName.c_str() );

  try {
  	reader->Update();
  }
	catch( itk::ExceptionObject & err ) {
    cerr << "ExceptionObject caught !" << endl;
    cerr << err << endl;
		exit(EXIT_FAILURE);
	}

  return reader->GetOutput();
}

// Writer helpers
// const Data
template<typename WriterType, typename DataType>
void writeData(const typename DataType::ConstPointer data, const string& fileName) {
  typename WriterType::Pointer writer = WriterType::New();

	writer->SetInput( data );
  // writer->AddTransform( anotherTransform ); // only applies to writing transforms

  writer->SetFileName( fileName.c_str() );

  try {
  	writer->Update();
  }
	catch( itk::ExceptionObject & err ) {
    cerr << "ExceptionObject caught !" << endl;
    cerr << err << endl;
		exit(EXIT_FAILURE);
	}
}

// Data
template<typename WriterType, typename DataType>
void writeData(const typename DataType::Pointer data, const string& fileName) {
  writeData< WriterType, DataType >( (typename DataType::ConstPointer) data, fileName);
}

// Const Image
template<typename ImageType>
void writeImage(const typename ImageType::ConstPointer image, const string& fileName) {
  writeData< itk::ImageFileWriter< ImageType >, ImageType >( image, fileName );
}

// Image
template<typename ImageType>
void writeImage(const typename ImageType::Pointer image, const string& fileName) {
  writeData< itk::ImageFileWriter< ImageType >, ImageType >( image, fileName );
}

#endif<|endoftext|>


<commit_before>
/* -*-  mode: c++; c-default-style: "google"; indent-tabs-mode: nil -*- */
/* -------------------------------------------------------------------------
   ATS

   License: see $ATS_DIR/COPYRIGHT
   Author: Markus Berndt

   Interface for the Prescribed Deformation PK.

   <ParameterList name="prescribed deformation">
      <Parameter name="PK model" type="string" value="Prescibed Mesh Deformation"/>
      <Parameter name="Deformation method" type="string" value="method name"/>
   </ParameterList>

   ------------------------------------------------------------------------- */

#include "prescribed_deformation.hh"
#include "porosity_evaluator.hh"

namespace Amanzi {
namespace Deform {

using namespace Amanzi::AmanziMesh;

RegisteredPKFactory<PrescribedDeformation> PrescribedDeformation::reg_("prescribed deformation");

PrescribedDeformation::PrescribedDeformation(Teuchos::ParameterList& plist,
                                             const Teuchos::RCP<TreeVector>& solution):
    PKDefaultBase(plist,solution),
    PKPhysicalBase(plist,solution)
{

}

// -- Setup data
void PrescribedDeformation::setup(const Teuchos::Ptr<State>& S) {
  PKPhysicalBase::setup(S);

  Teuchos::ParameterList poro_plist = plist_.sublist("porosity evaluator");
  poro_plist.sublist("VerboseObject").set<std::string>("Verbosity Level","extreme");
  Teuchos::RCP<DeformRelations::PorosityEvaluator> porosity_evaluator
    = Teuchos::rcp(new DeformRelations::PorosityEvaluator(poro_plist));
  S->SetFieldEvaluator("porosity", porosity_evaluator);

  std::vector<AmanziMesh::Entity_kind> location(1);
  location[0] = AmanziMesh::CELL;
  std::vector<int> num_dofs(1);
  num_dofs[0] = 1;
  std::vector<std::string> name(1);
  name[0] = "cell";

  S->RequireField(key_, name_)->SetMesh(mesh_)->SetGhosted()
    ->SetComponents(name, location, num_dofs);
}

// -- Initialize owned (dependent) variables.
void PrescribedDeformation::initialize(const Teuchos::Ptr<State>& S) {
  PKPhysicalBase::initialize(S);

}

// -- advance via one of a few methods
bool PrescribedDeformation::advance(double dt) {
  std::cout << "ADVANCING prescribed deformation" << std:: endl;

  AmanziMesh::Mesh * write_access_mesh_ =  const_cast<AmanziMesh::Mesh*>(&*S_next_->GetMesh());

  Teuchos::RCP<CompositeVector> cell_volume = S_next_->GetFieldData("cell_volume", "cell_volume");
  // Teuchos::RCP<FieldEvaluator> cveval = S_next_->GetFieldEvaluator("cell_volume");

  double ss = S_next_->time();
  double ss0 = S_->time();

  double thickness = 0.8 + 0.2*cos( ss / (365.25*24*60*60) * 2.0*M_PI );
  double thickness0 = 0.8 + 0.2*cos( ss0 / (365.25*24*60*60) * 2.0*M_PI );

  double factor = thickness/thickness0;

  std::cout << "factor = " << factor << std::endl;

  // get space dimensions
  int dim = write_access_mesh_->space_dimension();

    // set the list of the new position coords
  AmanziGeometry::Point_List newpos, finpos;

  // move the mid point on the mesh top
  Entity_ID_List nodeids;

  // number of vertices
  int nV = write_access_mesh_->num_entities(Amanzi::AmanziMesh::NODE,
                               Amanzi::AmanziMesh::OWNED);

  std::cout << "number of nodes = " << nV << std::endl;

  Amanzi::AmanziGeometry::Point coords, new_coords;
  coords.init(dim);
  new_coords.init(dim);

  // search the id of the mid point on the top
  for (int iV=0; iV<nV; iV++) {

    // get the coords of the node
    write_access_mesh_->node_get_coordinates(iV,&coords);

    for ( int s=0; s<dim-1; ++s ) {
      new_coords[s] = coords[s];
    }
    // new_coords[dim-1] = coords[dim-1] * fac;
    new_coords[dim-1] = coords[dim-1] * factor;

    // puch back for deform method
    nodeids.push_back(iV);
    newpos.push_back( new_coords );
  }

  std::cout << "now modifying the mesh..." << std::endl;
  // compute the deformed mesh
  write_access_mesh_->deform( nodeids, newpos, true, &finpos);

  std::cout << "cell volume = " << write_access_mesh_->cell_volume(0) << std::endl;

  // now update cell volumes
  cell_volume->Scale(factor);

  solution_evaluator_->SetFieldAsChanged();

  std::cout << "DONE" << std::endl;
}

} // namespace
} // namespace

<commit_msg>intermediate commit<commit_after>
/* -*-  mode: c++; c-default-style: "google"; indent-tabs-mode: nil -*- */
/* -------------------------------------------------------------------------
   ATS

   License: see $ATS_DIR/COPYRIGHT
   Author: Markus Berndt

   Interface for the Prescribed Deformation PK.

   <ParameterList name="prescribed deformation">
      <Parameter name="PK model" type="string" value="Prescibed Mesh Deformation"/>
      <Parameter name="Deformation method" type="string" value="method name"/>
   </ParameterList>

   ------------------------------------------------------------------------- */

#include "prescribed_deformation.hh"
#include "porosity_evaluator.hh"

namespace Amanzi {
namespace Deform {

using namespace Amanzi::AmanziMesh;

RegisteredPKFactory<PrescribedDeformation> PrescribedDeformation::reg_("prescribed deformation");

PrescribedDeformation::PrescribedDeformation(Teuchos::ParameterList& plist,
                                             const Teuchos::RCP<TreeVector>& solution):
    PKDefaultBase(plist,solution),
    PKPhysicalBase(plist,solution)
{

}

// -- Setup data
void PrescribedDeformation::setup(const Teuchos::Ptr<State>& S) {
  PKPhysicalBase::setup(S);

  std::vector<AmanziMesh::Entity_kind> location(1);
  location[0] = AmanziMesh::CELL;
  std::vector<int> num_dofs(1);
  num_dofs[0] = 1;
  std::vector<std::string> name(1);
  name[0] = "cell";

  S->RequireField(key_, name_)->SetMesh(mesh_)->SetGhosted()
    ->SetComponents(name, location, num_dofs);

  std::cout << key_ << endl;
  std::cout << name_ << endl;

}

// -- Initialize owned (dependent) variables.
void PrescribedDeformation::initialize(const Teuchos::Ptr<State>& S) {
  PKPhysicalBase::initialize(S);

}

// -- advance via one of a few methods
bool PrescribedDeformation::advance(double dt) {
  std::cout << "ADVANCING prescribed deformation" << std:: endl;

  AmanziMesh::Mesh * write_access_mesh_ =  const_cast<AmanziMesh::Mesh*>(&*S_next_->GetMesh());

  Teuchos::RCP<CompositeVector> cell_volume = S_next_->GetFieldData("cell_volume", "cell_volume");
  // Teuchos::RCP<FieldEvaluator> cveval = S_next_->GetFieldEvaluator("cell_volume");

  double ss = S_next_->time();
  double ss0 = S_->time();

  double thickness = 0.8 + 0.2*cos( ss / (365.25*24*60*60) * 2.0*M_PI );
  double thickness0 = 0.8 + 0.2*cos( ss0 / (365.25*24*60*60) * 2.0*M_PI );

  double factor = thickness/thickness0;

  std::cout << "factor = " << factor << std::endl;

  // get space dimensions
  int dim = write_access_mesh_->space_dimension();

    // set the list of the new position coords
  AmanziGeometry::Point_List newpos, finpos;

  // move the mid point on the mesh top
  Entity_ID_List nodeids;

  // number of vertices
  int nV = write_access_mesh_->num_entities(Amanzi::AmanziMesh::NODE,
                               Amanzi::AmanziMesh::OWNED);

  std::cout << "number of nodes = " << nV << std::endl;

  Amanzi::AmanziGeometry::Point coords, new_coords;
  coords.init(dim);
  new_coords.init(dim);

  // search the id of the mid point on the top
  for (int iV=0; iV<nV; iV++) {

    // get the coords of the node
    write_access_mesh_->node_get_coordinates(iV,&coords);

    for ( int s=0; s<dim-1; ++s ) {
      new_coords[s] = coords[s];
    }
    // new_coords[dim-1] = coords[dim-1] * fac;
    new_coords[dim-1] = coords[dim-1] * factor;

    // puch back for deform method
    nodeids.push_back(iV);
    newpos.push_back( new_coords );
  }

  std::cout << "now modifying the mesh..." << std::endl;
  // compute the deformed mesh
  write_access_mesh_->deform( nodeids, newpos, true, &finpos);

  std::cout << "cell volume = " << write_access_mesh_->cell_volume(0) << std::endl;

  // now update cell volumes
  cell_volume->Scale(factor);

  solution_evaluator_->SetFieldAsChanged();

  std::cout << "DONE" << std::endl;
}

} // namespace
} // namespace

<|endoftext|>


<commit_before>//============================================================================
// Name        : jaco_arm.cpp
// Author      : WPI, Clearpath Robotics
// Version     : 0.5
// Copyright   : BSD
// Description : A ROS driver for controlling the Kinova Jaco robotic manipulator arm
//============================================================================

#include "jaco_driver/jaco_arm.h"
#include <string>
#include <vector>

#define PI 3.14159265359

namespace jaco
{

JacoArm::JacoArm(JacoComm &arm, const ros::NodeHandle &nodeHandle)
    : jaco_comm_(arm), node_handle_(nodeHandle)
{
    /* Set up Services */
    stop_service_ = node_handle_.advertiseService("in/stop", &JacoArm::stopServiceCallback, this);
    start_service_ = node_handle_.advertiseService("in/start", &JacoArm::startServiceCallback, this);
    homing_service_ = node_handle_.advertiseService("in/home_arm", &JacoArm::homeArmServiceCallback, this);

    /* Set up Publishers */
    joint_angles_publisher_ = node_handle_.advertise<jaco_msgs::JointAngles>("out/joint_angles", 2);
    joint_state_publisher_ = node_handle_.advertise<sensor_msgs::JointState>("out/joint_state", 2);
    tool_position_publisher_ = node_handle_.advertise<geometry_msgs::PoseStamped>("out/tool_position", 2);
    tool_wrench_publisher_ = node_handle_.advertise<geometry_msgs::WrenchStamped>("out/tool_wrench", 2);
    finger_position_publisher_ = node_handle_.advertise<jaco_msgs::FingerPosition>("out/finger_position", 2);

    /* Set up Subscribers*/
    joint_velocity_subscriber_ = node_handle_.subscribe("in/joint_velocity", 1,
                                                      &JacoArm::jointVelocityCallback, this);
    cartesian_velocity_subscriber_ = node_handle_.subscribe("in/cartesian_velocity", 1,
                                                          &JacoArm::cartesianVelocityCallback, this);

    node_handle_.param<double>("status_interval_seconds", status_interval_seconds_, 0.1);
    node_handle_.param<double>("joint_angular_vel_timeout", joint_vel_timeout_seconds_, 0.25);
    node_handle_.param<double>("cartesian_vel_timeout", cartesian_vel_timeout_seconds_, 0.25);
    node_handle_.param<double>("joint_angular_vel_timeout", joint_vel_interval_seconds_, 0.1);
    node_handle_.param<double>("cartesian_vel_timeout", cartesian_vel_interval_seconds_, 0.01);

    node_handle_.param<std::string>("tf_prefix", tf_prefix_, "jaco_");

    // Approximative conversion ratio from finger position (0..6000) to joint angle
    // in radians (0..0.7).
    node_handle_.param("finger_angle_conv_ratio", finger_conv_ratio_, 0.7 / 5000.0);

    joint_names_.resize(JACO_JOINTS_COUNT);
    joint_names_[0] = tf_prefix_ + "joint_1";
    joint_names_[1] = tf_prefix_ + "joint_2";
    joint_names_[2] = tf_prefix_ + "joint_3";
    joint_names_[3] = tf_prefix_ + "joint_4";
    joint_names_[4] = tf_prefix_ + "joint_5";
    joint_names_[5] = tf_prefix_ + "joint_6";
    joint_names_[6] = tf_prefix_ + "joint_finger_1";
    joint_names_[7] = tf_prefix_ + "joint_finger_2";
    joint_names_[8] = tf_prefix_ + "joint_finger_3";

    status_timer_ = node_handle_.createTimer(ros::Duration(status_interval_seconds_),
                                           &JacoArm::statusTimer, this);

    joint_vel_timer_ = node_handle_.createTimer(ros::Duration(joint_vel_interval_seconds_),
                                              &JacoArm::jointVelocityTimer, this);
    joint_vel_timer_.stop();
    joint_vel_timer_flag_ = false;

    cartesian_vel_timer_ = node_handle_.createTimer(ros::Duration(cartesian_vel_interval_seconds_),
                                                  &JacoArm::cartesianVelocityTimer, this);
    cartesian_vel_timer_.stop();
    cartesian_vel_timer_flag_ = false;

    ROS_INFO("The arm is ready to use.");

}

JacoArm::~JacoArm()
{
}

bool JacoArm::homeArmServiceCallback(jaco_msgs::HomeArm::Request &req, jaco_msgs::HomeArm::Response &res)
{
    jaco_comm_.homeArm();
    jaco_comm_.initFingers();
    res.homearm_result = "JACO ARM HAS BEEN RETURNED HOME";
    return true;
}

void JacoArm::jointVelocityCallback(const jaco_msgs::JointVelocityConstPtr& joint_vel)
{
    if (!jaco_comm_.isStopped())
    {
        joint_velocities_.Actuator1 = joint_vel->joint1;
        joint_velocities_.Actuator2 = joint_vel->joint2;
        joint_velocities_.Actuator3 = joint_vel->joint3;
        joint_velocities_.Actuator4 = joint_vel->joint4;
        joint_velocities_.Actuator5 = joint_vel->joint5;
        joint_velocities_.Actuator6 = joint_vel->joint6;
        last_joint_vel_cmd_time_ = ros::Time().now();

        if (joint_vel_timer_flag_ == false)
        {
            joint_vel_timer_.start();
            joint_vel_timer_flag_ = true;
        }
    }
}

/*!
 * \brief Handler for "stop" service.
 *
 * Instantly stops the arm and prevents further movement until start service is
 * invoked.
 */
bool JacoArm::stopServiceCallback(jaco_msgs::Stop::Request &req, jaco_msgs::Stop::Response &res)
{
    jaco_comm_.stopAPI();
    res.stop_result = "Arm stopped";
    ROS_DEBUG("Arm stop requested");
    return true;
}

/*!
 * \brief Handler for "start" service.
 *
 * Re-enables control of the arm after a stop.
 */
bool JacoArm::startServiceCallback(jaco_msgs::Start::Request &req, jaco_msgs::Start::Response &res)
{
    jaco_comm_.startAPI();
    res.start_result = "Arm started";
    ROS_DEBUG("Arm start requested");
    return true;
}

void JacoArm::cartesianVelocityCallback(const geometry_msgs::TwistStampedConstPtr& cartesian_vel)
{
    if (!jaco_comm_.isStopped())
    {
        cartesian_velocities_.X = cartesian_vel->twist.linear.x;
        cartesian_velocities_.Y = cartesian_vel->twist.linear.y;
        cartesian_velocities_.Z = cartesian_vel->twist.linear.z;
        cartesian_velocities_.ThetaX = cartesian_vel->twist.angular.x;
        cartesian_velocities_.ThetaY = cartesian_vel->twist.angular.y;
        cartesian_velocities_.ThetaZ = cartesian_vel->twist.angular.z;

        last_cartesian_vel_cmd_time_ = ros::Time().now();

        if (cartesian_vel_timer_flag_ == false)
        {
            cartesian_vel_timer_.start();
            cartesian_vel_timer_flag_ = true;
        }
    }
}

void JacoArm::cartesianVelocityTimer(const ros::TimerEvent&)
{
    double elapsed_time_seconds = ros::Time().now().toSec() - last_cartesian_vel_cmd_time_.toSec();

    if (elapsed_time_seconds > cartesian_vel_timeout_seconds_)
    {
        ROS_DEBUG("Cartesian vel timed out: %f", elapsed_time_seconds);
        cartesian_vel_timer_.stop();
        cartesian_vel_timer_flag_ = false;
    }
    else
    {
        ROS_DEBUG("Cart vel timer (%f): %f, %f, %f, %f, %f, %f", elapsed_time_seconds,
                  cartesian_velocities_.X, cartesian_velocities_.Y, cartesian_velocities_.Z,
                  cartesian_velocities_.ThetaX, cartesian_velocities_.ThetaY, cartesian_velocities_.ThetaZ);
        jaco_comm_.setCartesianVelocities(cartesian_velocities_);
    }
}

void JacoArm::jointVelocityTimer(const ros::TimerEvent&)
{
    double elapsed_time_seconds = ros::Time().now().toSec() - last_joint_vel_cmd_time_.toSec();

    if (elapsed_time_seconds > joint_vel_timeout_seconds_)
    {
        ROS_DEBUG("Joint vel timed out: %f", elapsed_time_seconds);
        joint_vel_timer_.stop();
        joint_vel_timer_flag_ = false;
    }
    else
    {
        ROS_DEBUG("Joint vel timer (%f): %f, %f, %f, %f, %f, %f", elapsed_time_seconds,
                  joint_velocities_.Actuator1, joint_velocities_.Actuator2, joint_velocities_.Actuator3,
                  joint_velocities_.Actuator4, joint_velocities_.Actuator5, joint_velocities_.Actuator6);
        jaco_comm_.setJointVelocities(joint_velocities_);
    }
}

/*!
 * \brief Publishes the current joint angles.
 *
 * Joint angles are published in both their raw state as obtained from the arm
 * (JointAngles), and transformed & converted to radians (joint_state) as per
 * the Jaco Kinematics PDF.
 *
 * Velocities and torques (effort) are only published in the JointStates
 * message, only for the first 6 joints as these values are not available for
 * the fingers.
 */
void JacoArm::publishJointAngles(void)
{
    static const double PI_180 = (PI / 180.0);
    FingerAngles fingers;
    jaco_comm_.getFingerPositions(fingers);

    // Query arm for current joint angles
    JacoAngles current_angles;
    jaco_comm_.getJointAngles(current_angles);
    jaco_msgs::JointAngles jaco_angles = current_angles.constructAnglesMsg();

    jaco_angles.joint1 = current_angles.Actuator1;
    jaco_angles.joint2 = current_angles.Actuator2;
    jaco_angles.joint3 = current_angles.Actuator3;
    jaco_angles.joint4 = current_angles.Actuator4;
    jaco_angles.joint5 = current_angles.Actuator5;
    jaco_angles.joint6 = current_angles.Actuator6;

    sensor_msgs::JointState joint_state;
    joint_state.name = joint_names_;
    joint_state.header.stamp = ros::Time::now();

    // Transform from Kinova DH algorithm to physical angles in radians, then place into vector array
    joint_state.position.resize(9);

    joint_state.position[0] = (180- jaco_angles.joint1) * (PI / 180);
    joint_state.position[1] = (jaco_angles.joint2 - 270) * (PI / 180);
    joint_state.position[2] = (90-jaco_angles.joint3) * (PI / 180);
    joint_state.position[3] = (180-jaco_angles.joint4) * (PI / 180);
    joint_state.position[4] = (180-jaco_angles.joint5) * (PI / 180);
    joint_state.position[5] = (260-jaco_angles.joint6) * (PI / 180);
    joint_state.position[6] = finger_conv_ratio_ * fingers.Finger1;
    joint_state.position[7] = finger_conv_ratio_ * fingers.Finger2;
    joint_state.position[8] = finger_conv_ratio_ * fingers.Finger3;

    // Joint velocities
    JacoAngles current_vels;
    jaco_comm_.getJointVelocities(current_vels);
    joint_state.velocity.resize(9);
    joint_state.velocity[0] = current_vels.Actuator1;
    joint_state.velocity[1] = current_vels.Actuator2;
    joint_state.velocity[2] = current_vels.Actuator3;
    joint_state.velocity[3] = current_vels.Actuator4;
    joint_state.velocity[4] = current_vels.Actuator5;
    joint_state.velocity[5] = current_vels.Actuator6;
    for (int i = 0; i < 6; ++i) {
        // Angle velocities from the API are 0..180 for positive values,
        // and 360..181 for negative ones, in a kind of 2-complement setup.
        double& qd = joint_state.velocity[i];
        if (qd > 180.0) {
            qd -= 360.0;
        }
        qd *= PI_180;
    }
    // No velocity for the fingers:
    joint_state.velocity[6] = 0.0;
    joint_state.velocity[7] = 0.0;
    joint_state.velocity[8] = 0.0;

    // Joint torques (effort)
    JacoAngles joint_tqs;
    joint_state.effort.resize(9);
    joint_state.effort[0] = joint_tqs.Actuator1;
    joint_state.effort[1] = joint_tqs.Actuator2;
    joint_state.effort[2] = joint_tqs.Actuator3;
    joint_state.effort[3] = joint_tqs.Actuator4;
    joint_state.effort[4] = joint_tqs.Actuator5;
    joint_state.effort[5] = joint_tqs.Actuator6;
    joint_state.effort[6] = 0.0;
    joint_state.effort[6] = 0.0;
    joint_state.effort[8] = 0.0;

    joint_angles_publisher_.publish(jaco_angles);
    joint_state_publisher_.publish(joint_state);
}

/*!
 * \brief Publishes the current cartesian coordinates
 */
void JacoArm::publishToolPosition(void)
{
    JacoPose pose;
    geometry_msgs::PoseStamped current_position;

    jaco_comm_.getCartesianPosition(pose);

    current_position.pose            = pose.constructPoseMsg();
    current_position.header.stamp    = ros::Time::now();
    current_position.header.frame_id = tf_prefix_ + "link_base";

    tool_position_publisher_.publish(current_position);
}

/*!
 * \brief Publishes the current cartesian forces at the end effector.
 */
void JacoArm::publishToolWrench(void)
{
    JacoPose wrench;
    geometry_msgs::WrenchStamped current_wrench;

    jaco_comm_.getCartesianForce(wrench);

    current_wrench.wrench          = wrench.constructWrenchMsg();
    current_wrench.header.stamp    = ros::Time::now();
    // TODO: Rotate wrench to fit the end effector frame.
    // Right now, the orientation of the wrench is in the API's (base) frame.
    current_wrench.header.frame_id = tf_prefix_ + "api_origin";

    tool_wrench_publisher_.publish(current_wrench);
}

/*!
 * \brief Publishes the current finger positions.
 */
void JacoArm::publishFingerPosition(void)
{
    FingerAngles fingers;
    jaco_comm_.getFingerPositions(fingers);
    finger_position_publisher_.publish(fingers.constructFingersMsg());
}

void JacoArm::statusTimer(const ros::TimerEvent&)
{
    publishJointAngles();
    publishToolPosition();
    publishToolWrench();
    publishFingerPosition();
}

}  // namespace jaco
<commit_msg>Fix for type in effort output, J6 angle off by 10 degrees.<commit_after>//============================================================================
// Name        : jaco_arm.cpp
// Author      : WPI, Clearpath Robotics
// Version     : 0.5
// Copyright   : BSD
// Description : A ROS driver for controlling the Kinova Jaco robotic manipulator arm
//============================================================================

#include "jaco_driver/jaco_arm.h"
#include <string>
#include <vector>

#define PI 3.14159265359

namespace jaco
{

JacoArm::JacoArm(JacoComm &arm, const ros::NodeHandle &nodeHandle)
    : jaco_comm_(arm), node_handle_(nodeHandle)
{
    /* Set up Services */
    stop_service_ = node_handle_.advertiseService("in/stop", &JacoArm::stopServiceCallback, this);
    start_service_ = node_handle_.advertiseService("in/start", &JacoArm::startServiceCallback, this);
    homing_service_ = node_handle_.advertiseService("in/home_arm", &JacoArm::homeArmServiceCallback, this);

    /* Set up Publishers */
    joint_angles_publisher_ = node_handle_.advertise<jaco_msgs::JointAngles>("out/joint_angles", 2);
    joint_state_publisher_ = node_handle_.advertise<sensor_msgs::JointState>("out/joint_state", 2);
    tool_position_publisher_ = node_handle_.advertise<geometry_msgs::PoseStamped>("out/tool_position", 2);
    tool_wrench_publisher_ = node_handle_.advertise<geometry_msgs::WrenchStamped>("out/tool_wrench", 2);
    finger_position_publisher_ = node_handle_.advertise<jaco_msgs::FingerPosition>("out/finger_position", 2);

    /* Set up Subscribers*/
    joint_velocity_subscriber_ = node_handle_.subscribe("in/joint_velocity", 1,
                                                      &JacoArm::jointVelocityCallback, this);
    cartesian_velocity_subscriber_ = node_handle_.subscribe("in/cartesian_velocity", 1,
                                                          &JacoArm::cartesianVelocityCallback, this);

    node_handle_.param<double>("status_interval_seconds", status_interval_seconds_, 0.1);
    node_handle_.param<double>("joint_angular_vel_timeout", joint_vel_timeout_seconds_, 0.25);
    node_handle_.param<double>("cartesian_vel_timeout", cartesian_vel_timeout_seconds_, 0.25);
    node_handle_.param<double>("joint_angular_vel_timeout", joint_vel_interval_seconds_, 0.1);
    node_handle_.param<double>("cartesian_vel_timeout", cartesian_vel_interval_seconds_, 0.01);

    node_handle_.param<std::string>("tf_prefix", tf_prefix_, "jaco_");

    // Approximative conversion ratio from finger position (0..6000) to joint angle
    // in radians (0..0.7).
    node_handle_.param("finger_angle_conv_ratio", finger_conv_ratio_, 0.7 / 5000.0);

    joint_names_.resize(JACO_JOINTS_COUNT);
    joint_names_[0] = tf_prefix_ + "joint_1";
    joint_names_[1] = tf_prefix_ + "joint_2";
    joint_names_[2] = tf_prefix_ + "joint_3";
    joint_names_[3] = tf_prefix_ + "joint_4";
    joint_names_[4] = tf_prefix_ + "joint_5";
    joint_names_[5] = tf_prefix_ + "joint_6";
    joint_names_[6] = tf_prefix_ + "joint_finger_1";
    joint_names_[7] = tf_prefix_ + "joint_finger_2";
    joint_names_[8] = tf_prefix_ + "joint_finger_3";

    status_timer_ = node_handle_.createTimer(ros::Duration(status_interval_seconds_),
                                           &JacoArm::statusTimer, this);

    joint_vel_timer_ = node_handle_.createTimer(ros::Duration(joint_vel_interval_seconds_),
                                              &JacoArm::jointVelocityTimer, this);
    joint_vel_timer_.stop();
    joint_vel_timer_flag_ = false;

    cartesian_vel_timer_ = node_handle_.createTimer(ros::Duration(cartesian_vel_interval_seconds_),
                                                  &JacoArm::cartesianVelocityTimer, this);
    cartesian_vel_timer_.stop();
    cartesian_vel_timer_flag_ = false;

    ROS_INFO("The arm is ready to use.");

}

JacoArm::~JacoArm()
{
}

bool JacoArm::homeArmServiceCallback(jaco_msgs::HomeArm::Request &req, jaco_msgs::HomeArm::Response &res)
{
    jaco_comm_.homeArm();
    jaco_comm_.initFingers();
    res.homearm_result = "JACO ARM HAS BEEN RETURNED HOME";
    return true;
}

void JacoArm::jointVelocityCallback(const jaco_msgs::JointVelocityConstPtr& joint_vel)
{
    if (!jaco_comm_.isStopped())
    {
        joint_velocities_.Actuator1 = joint_vel->joint1;
        joint_velocities_.Actuator2 = joint_vel->joint2;
        joint_velocities_.Actuator3 = joint_vel->joint3;
        joint_velocities_.Actuator4 = joint_vel->joint4;
        joint_velocities_.Actuator5 = joint_vel->joint5;
        joint_velocities_.Actuator6 = joint_vel->joint6;
        last_joint_vel_cmd_time_ = ros::Time().now();

        if (joint_vel_timer_flag_ == false)
        {
            joint_vel_timer_.start();
            joint_vel_timer_flag_ = true;
        }
    }
}

/*!
 * \brief Handler for "stop" service.
 *
 * Instantly stops the arm and prevents further movement until start service is
 * invoked.
 */
bool JacoArm::stopServiceCallback(jaco_msgs::Stop::Request &req, jaco_msgs::Stop::Response &res)
{
    jaco_comm_.stopAPI();
    res.stop_result = "Arm stopped";
    ROS_DEBUG("Arm stop requested");
    return true;
}

/*!
 * \brief Handler for "start" service.
 *
 * Re-enables control of the arm after a stop.
 */
bool JacoArm::startServiceCallback(jaco_msgs::Start::Request &req, jaco_msgs::Start::Response &res)
{
    jaco_comm_.startAPI();
    res.start_result = "Arm started";
    ROS_DEBUG("Arm start requested");
    return true;
}

void JacoArm::cartesianVelocityCallback(const geometry_msgs::TwistStampedConstPtr& cartesian_vel)
{
    if (!jaco_comm_.isStopped())
    {
        cartesian_velocities_.X = cartesian_vel->twist.linear.x;
        cartesian_velocities_.Y = cartesian_vel->twist.linear.y;
        cartesian_velocities_.Z = cartesian_vel->twist.linear.z;
        cartesian_velocities_.ThetaX = cartesian_vel->twist.angular.x;
        cartesian_velocities_.ThetaY = cartesian_vel->twist.angular.y;
        cartesian_velocities_.ThetaZ = cartesian_vel->twist.angular.z;

        last_cartesian_vel_cmd_time_ = ros::Time().now();

        if (cartesian_vel_timer_flag_ == false)
        {
            cartesian_vel_timer_.start();
            cartesian_vel_timer_flag_ = true;
        }
    }
}

void JacoArm::cartesianVelocityTimer(const ros::TimerEvent&)
{
    double elapsed_time_seconds = ros::Time().now().toSec() - last_cartesian_vel_cmd_time_.toSec();

    if (elapsed_time_seconds > cartesian_vel_timeout_seconds_)
    {
        ROS_DEBUG("Cartesian vel timed out: %f", elapsed_time_seconds);
        cartesian_vel_timer_.stop();
        cartesian_vel_timer_flag_ = false;
    }
    else
    {
        ROS_DEBUG("Cart vel timer (%f): %f, %f, %f, %f, %f, %f", elapsed_time_seconds,
                  cartesian_velocities_.X, cartesian_velocities_.Y, cartesian_velocities_.Z,
                  cartesian_velocities_.ThetaX, cartesian_velocities_.ThetaY, cartesian_velocities_.ThetaZ);
        jaco_comm_.setCartesianVelocities(cartesian_velocities_);
    }
}

void JacoArm::jointVelocityTimer(const ros::TimerEvent&)
{
    double elapsed_time_seconds = ros::Time().now().toSec() - last_joint_vel_cmd_time_.toSec();

    if (elapsed_time_seconds > joint_vel_timeout_seconds_)
    {
        ROS_DEBUG("Joint vel timed out: %f", elapsed_time_seconds);
        joint_vel_timer_.stop();
        joint_vel_timer_flag_ = false;
    }
    else
    {
        ROS_DEBUG("Joint vel timer (%f): %f, %f, %f, %f, %f, %f", elapsed_time_seconds,
                  joint_velocities_.Actuator1, joint_velocities_.Actuator2, joint_velocities_.Actuator3,
                  joint_velocities_.Actuator4, joint_velocities_.Actuator5, joint_velocities_.Actuator6);
        jaco_comm_.setJointVelocities(joint_velocities_);
    }
}

/*!
 * \brief Publishes the current joint angles.
 *
 * Joint angles are published in both their raw state as obtained from the arm
 * (JointAngles), and transformed & converted to radians (joint_state) as per
 * the Jaco Kinematics PDF.
 *
 * Velocities and torques (effort) are only published in the JointStates
 * message, only for the first 6 joints as these values are not available for
 * the fingers.
 */
void JacoArm::publishJointAngles(void)
{
    static const double PI_180 = (PI / 180.0);
    FingerAngles fingers;
    jaco_comm_.getFingerPositions(fingers);

    // Query arm for current joint angles
    JacoAngles current_angles;
    jaco_comm_.getJointAngles(current_angles);
    jaco_msgs::JointAngles jaco_angles = current_angles.constructAnglesMsg();

    jaco_angles.joint1 = current_angles.Actuator1;
    jaco_angles.joint2 = current_angles.Actuator2;
    jaco_angles.joint3 = current_angles.Actuator3;
    jaco_angles.joint4 = current_angles.Actuator4;
    jaco_angles.joint5 = current_angles.Actuator5;
    jaco_angles.joint6 = current_angles.Actuator6;

    sensor_msgs::JointState joint_state;
    joint_state.name = joint_names_;
    joint_state.header.stamp = ros::Time::now();

    // Transform from Kinova DH algorithm to physical angles in radians, then place into vector array
    joint_state.position.resize(9);

    joint_state.position[0] = (180- jaco_angles.joint1) * (PI / 180);
    joint_state.position[1] = (jaco_angles.joint2 - 270) * (PI / 180);
    joint_state.position[2] = (90-jaco_angles.joint3) * (PI / 180);
    joint_state.position[3] = (180-jaco_angles.joint4) * (PI / 180);
    joint_state.position[4] = (180-jaco_angles.joint5) * (PI / 180);
    joint_state.position[5] = (270-jaco_angles.joint6) * (PI / 180);
    joint_state.position[6] = finger_conv_ratio_ * fingers.Finger1;
    joint_state.position[7] = finger_conv_ratio_ * fingers.Finger2;
    joint_state.position[8] = finger_conv_ratio_ * fingers.Finger3;

    // Joint velocities
    JacoAngles current_vels;
    jaco_comm_.getJointVelocities(current_vels);
    joint_state.velocity.resize(9);
    joint_state.velocity[0] = current_vels.Actuator1;
    joint_state.velocity[1] = current_vels.Actuator2;
    joint_state.velocity[2] = current_vels.Actuator3;
    joint_state.velocity[3] = current_vels.Actuator4;
    joint_state.velocity[4] = current_vels.Actuator5;
    joint_state.velocity[5] = current_vels.Actuator6;
    for (int i = 0; i < 6; ++i) {
        // Angle velocities from the API are 0..180 for positive values,
        // and 360..181 for negative ones, in a kind of 2-complement setup.
        double& qd = joint_state.velocity[i];
        if (qd > 180.0) {
            qd -= 360.0;
        }
        qd *= PI_180;
    }
    // No velocity for the fingers:
    joint_state.velocity[6] = 0.0;
    joint_state.velocity[7] = 0.0;
    joint_state.velocity[8] = 0.0;

    // Joint torques (effort)
    JacoAngles joint_tqs;
    joint_state.effort.resize(9);
    joint_state.effort[0] = joint_tqs.Actuator1;
    joint_state.effort[1] = joint_tqs.Actuator2;
    joint_state.effort[2] = joint_tqs.Actuator3;
    joint_state.effort[3] = joint_tqs.Actuator4;
    joint_state.effort[4] = joint_tqs.Actuator5;
    joint_state.effort[5] = joint_tqs.Actuator6;
    joint_state.effort[6] = 0.0;
    joint_state.effort[7] = 0.0;
    joint_state.effort[8] = 0.0;

    joint_angles_publisher_.publish(jaco_angles);
    joint_state_publisher_.publish(joint_state);
}

/*!
 * \brief Publishes the current cartesian coordinates
 */
void JacoArm::publishToolPosition(void)
{
    JacoPose pose;
    geometry_msgs::PoseStamped current_position;

    jaco_comm_.getCartesianPosition(pose);

    current_position.pose            = pose.constructPoseMsg();
    current_position.header.stamp    = ros::Time::now();
    current_position.header.frame_id = tf_prefix_ + "link_base";

    tool_position_publisher_.publish(current_position);
}

/*!
 * \brief Publishes the current cartesian forces at the end effector.
 */
void JacoArm::publishToolWrench(void)
{
    JacoPose wrench;
    geometry_msgs::WrenchStamped current_wrench;

    jaco_comm_.getCartesianForce(wrench);

    current_wrench.wrench          = wrench.constructWrenchMsg();
    current_wrench.header.stamp    = ros::Time::now();
    // TODO: Rotate wrench to fit the end effector frame.
    // Right now, the orientation of the wrench is in the API's (base) frame.
    current_wrench.header.frame_id = tf_prefix_ + "api_origin";

    tool_wrench_publisher_.publish(current_wrench);
}

/*!
 * \brief Publishes the current finger positions.
 */
void JacoArm::publishFingerPosition(void)
{
    FingerAngles fingers;
    jaco_comm_.getFingerPositions(fingers);
    finger_position_publisher_.publish(fingers.constructFingersMsg());
}

void JacoArm::statusTimer(const ros::TimerEvent&)
{
    publishJointAngles();
    publishToolPosition();
    publishToolWrench();
    publishFingerPosition();
}

}  // namespace jaco
<|endoftext|>


<commit_before>#ifndef __PreassembledMass_INL
#define __PreassembledMass_INL

#include "PreassembledMass.h"

#include <sofa/core/objectmodel/BaseNode.h>

#include <Compliant/assembly/AssemblyVisitor.h>
#include <sofa/simulation/MechanicalVisitor.h>

namespace sofa
{

namespace component
{

namespace mass
{

/// removing no longer necessary mapped masses
class RemoveChildMassVisitor : public simulation::Visitor
{
public:
    RemoveChildMassVisitor(const core::ExecParams* params)
        : simulation::Visitor(params)
    {
    }

    virtual Result processNodeTopDown(simulation::Node* node)
    {
        core::behavior::BaseMass* mass;
        node->get(mass);
        if( !dynamic_cast<BasePreassembledMass*>(mass) )
            node->removeObject(mass);
        return RESULT_CONTINUE;
    }

};

unsigned int BasePreassembledMass::s_instanciationCounter = 0;

template < class DataTypes >
void PreassembledMass< DataTypes >::init()
{
    core::behavior::Mass<DataTypes>::init();
}

template < class DataTypes >
void PreassembledMass< DataTypes >::bwdInit()
{

    MassMatrix& massMatrix = *d_massMatrix.beginEdit();

    // if the mass matrix is not given manually
    if( massMatrix.rows() != massMatrix.cols() || massMatrix.rows()!=(typename MassMatrix::Index)this->mstate->getMatrixSize() )
    {
        // perform assembly
        core::MechanicalParams mparams = *core::MechanicalParams::defaultInstance();
        mparams.setKFactor(0);
        mparams.setBFactor(0);
        mparams.setMFactor(1);
        mparams.setDt( this->getContext()->getDt() ); // should not be used but to be sure

        {
            // trigger masks computation as it is required by assembly
            simulation::MechanicalComputeForceVisitor visitor(&mparams, core::VecDerivId::force());
            this->getContext()->executeVisitor( &visitor );
        }

        simulation::AssemblyVisitor assemblyVisitor( &mparams );
        this->getContext()->executeVisitor( &assemblyVisitor );

        component::linearsolver::AssembledSystem sys;

        assemblyVisitor.assemble( sys );
        massMatrix.compressedMatrix = sys.H;

        if( massMatrix.rows()!=(typename MassMatrix::Index)this->mstate->getMatrixSize() )
        {
            serr<<"Are you sure that every independent dofs are in independent graph branches?\n";
            assert(false);
        }

        if( _instanciationNumber == 0 ) // only the first one (last bwdInit called) will call the mass removal
        {
    //        std::cerr<<SOFA_CLASS_METHOD<<"removing child masses"<<std::endl;

            // visitor to delete child mass
            RemoveChildMassVisitor removeChildMassVisitor( core::ExecParams::defaultInstance() );
            this->getContext()->executeVisitor( &removeChildMassVisitor );

            typename LinkMassNodes::Container massNodes = l_massNodes.getValue();
            for ( unsigned int i = 0; i < massNodes.size() ; i++)
            {
               if( massNodes[i]->isActive() ) massNodes[i]->setActive( false );
            }
        }
    }

    // for human debug
    Real totalmass = 0;
    for(typename MassMatrix::Index r=0;r<massMatrix.rows();++r)
        for(typename MassMatrix::Index c=0;c<massMatrix.cols();++c)
            totalmass+=massMatrix.element(r,c);
    sout<<"total mass: "<<totalmass/this->mstate->getMatrixBlockSize()<<sendl;

    d_massMatrix.endEdit();
}

////////////////////////////////////

// -- Mass interface
template < class DataTypes >
void PreassembledMass< DataTypes >::addMDx( const core::MechanicalParams*, DataVecDeriv& res, const DataVecDeriv& dx, double factor )
{
    if( factor == 1.0 )
    {
        d_massMatrix.getValue().addMult( res, dx );
    }
    else
    {
        d_massMatrix.getValue().addMult( res, dx, factor );
    }
}

template < class DataTypes >
void PreassembledMass< DataTypes >::accFromF( const core::MechanicalParams*, DataVecDeriv& /*acc*/, const DataVecDeriv& /*f*/ )
{
    serr<<"accFromF not yet implemented (the matrix inversion is needed)"<<sendl;
}

template < class DataTypes >
double PreassembledMass< DataTypes >::getKineticEnergy( const core::MechanicalParams*, const DataVecDeriv& v ) const
{
    const VecDeriv& _v = v.getValue();
    double e = 0;

    VecDeriv Mv;
    d_massMatrix.getValue().mult( Mv, _v );

    for( unsigned int i=0 ; i<_v.size() ; i++ )
        e += _v[i] * Mv[i];

    return e/2;
}

template < class DataTypes >
double PreassembledMass< DataTypes >::getPotentialEnergy( const core::MechanicalParams* mparams, const DataVecCoord& x ) const
{
    serr<<SOFA_CLASS_METHOD<<"not implemented!\n";
    return core::behavior::Mass< DataTypes >::getPotentialEnergy( mparams, x );

//    const VecCoord& _x = x.getValue();

//    VecCoord Mx/* = d_massMatrix * _x*/;
//    d_massMatrix.mult( Mx, _x );

//    SReal e = 0;
//    // gravity
//    Vec3d g ( this->getContext()->getGravity() );
//    Deriv theGravity;
//    DataTypes::set ( theGravity, g[0], g[1], g[2] );
//    for( unsigned int i=0 ; i<_x.size() ; i++ )
//    {
//        e -= theGravity*Mx[i];
//    }
//    return e;
}

template < class DataTypes >
void PreassembledMass< DataTypes >::addGravityToV(const core::MechanicalParams* mparams, DataVecDeriv& d_v)
{
    if(mparams)
    {
        VecDeriv& v = *d_v.beginEdit();

        // gravity
        Vec3 g ( this->getContext()->getGravity() * (mparams->dt()) );
        Deriv theGravity;
        DataTypes::set ( theGravity, g[0], g[1], g[2]);
        Deriv hg = theGravity * (mparams->dt());

        // add weight force
        for (unsigned int i=0; i<v.size(); i++)
        {
            v[i] += hg;
        }
        d_v.endEdit();
    }
}

template < class DataTypes >
void PreassembledMass< DataTypes >::addForce(const core::MechanicalParams* /*mparams*/, DataVecDeriv& f, const DataVecCoord& /*x*/, const DataVecDeriv& /*v*/)
{
    //if gravity was added separately (in solver's "solve" method), then nothing to do here
    if(this->m_separateGravity.getValue()) return;

    VecDeriv& _f = *f.beginEdit();

    // gravity
    Vec3 g ( this->getContext()->getGravity() );
//    Deriv theGravity;
//    DataTypes::set( theGravity, g[0], g[1], g[2] );
    // add weight
//    d_massMatrix.template addMul_by_line<Real,VecDeriv,Deriv>( _f, theGravity );

    //TODO optimize this!!!
    VecDeriv gravities(_f.size());
    for(size_t i=0 ; i<_f.size() ; ++i )
        DataTypes::set( gravities[i], g[0], g[1], g[2] );
    d_massMatrix.getValue().addMult( _f, gravities );

    f.endEdit();
}

template < class DataTypes >
void PreassembledMass< DataTypes >::addMToMatrix(const core::MechanicalParams *mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix)
{
    sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate);
    Real mFactor = (Real)mparams->mFactorIncludingRayleighDamping(this->rayleighMass.getValue());
    d_massMatrix.getValue().addToBaseMatrix( r.matrix, mFactor, r.offset );
}

} // namespace mass

} // namespace component

} // namespace sofa

#endif // __PreassembledMass_INL
<commit_msg>[PreassembledMass] fixed empty matrix multiplication<commit_after>#ifndef __PreassembledMass_INL
#define __PreassembledMass_INL

#include "PreassembledMass.h"

#include <sofa/core/objectmodel/BaseNode.h>

#include <Compliant/assembly/AssemblyVisitor.h>
#include <sofa/simulation/MechanicalVisitor.h>

namespace sofa
{

namespace component
{

namespace mass
{

/// removing no longer necessary mapped masses
class RemoveChildMassVisitor : public simulation::Visitor
{
public:
    RemoveChildMassVisitor(const core::ExecParams* params)
        : simulation::Visitor(params)
    {
    }

    virtual Result processNodeTopDown(simulation::Node* node)
    {
        core::behavior::BaseMass* mass;
        node->get(mass);
        if( !dynamic_cast<BasePreassembledMass*>(mass) )
            node->removeObject(mass);
        return RESULT_CONTINUE;
    }

};

unsigned int BasePreassembledMass::s_instanciationCounter = 0;

template < class DataTypes >
void PreassembledMass< DataTypes >::init()
{
    core::behavior::Mass<DataTypes>::init();
}

template < class DataTypes >
void PreassembledMass< DataTypes >::bwdInit()
{

    MassMatrix& massMatrix = *d_massMatrix.beginEdit();

    // if the mass matrix is not given manually
    if( massMatrix.rows() != massMatrix.cols() || massMatrix.rows()!=(typename MassMatrix::Index)this->mstate->getMatrixSize() )
    {
        // perform assembly
        core::MechanicalParams mparams = *core::MechanicalParams::defaultInstance();
        mparams.setKFactor(0);
        mparams.setBFactor(0);
        mparams.setMFactor(1);
        mparams.setDt( this->getContext()->getDt() ); // should not be used but to be sure

        {
            // trigger masks computation as it is required by assembly
            simulation::MechanicalComputeForceVisitor visitor(&mparams, core::VecDerivId::force());

            // warning: calling this will execute addM and friends and we're not
            // yet initalized
            this->getContext()->executeVisitor( &visitor );
        }

        simulation::AssemblyVisitor assemblyVisitor( &mparams );
        this->getContext()->executeVisitor( &assemblyVisitor );

        component::linearsolver::AssembledSystem sys;

        assemblyVisitor.assemble( sys );
        massMatrix.compressedMatrix = sys.H;

        if( massMatrix.rows()!=(typename MassMatrix::Index)this->mstate->getMatrixSize() )
        {
            serr<<"Are you sure that every independent dofs are in independent graph branches?\n";
            assert(false);
        }

        if( _instanciationNumber == 0 ) // only the first one (last bwdInit called) will call the mass removal
        {
    //        std::cerr<<SOFA_CLASS_METHOD<<"removing child masses"<<std::endl;

            // visitor to delete child mass
            RemoveChildMassVisitor removeChildMassVisitor( core::ExecParams::defaultInstance() );
            this->getContext()->executeVisitor( &removeChildMassVisitor );

            typename LinkMassNodes::Container massNodes = l_massNodes.getValue();
            for ( unsigned int i = 0; i < massNodes.size() ; i++)
            {
               if( massNodes[i]->isActive() ) massNodes[i]->setActive( false );
            }
        }
    }

    // for human debug
    Real totalmass = 0;
    for(typename MassMatrix::Index r = 0; r < massMatrix.rows(); ++r) {
        for(typename MassMatrix::Index c = 0;c < massMatrix.cols(); ++c) {
            totalmass+=massMatrix.element(r, c);
        }
    }

    sout << "total mass: "<< totalmass / this->mstate->getMatrixBlockSize() << sendl;

    d_massMatrix.endEdit();
}

////////////////////////////////////

// -- Mass interface
template < class DataTypes >
void PreassembledMass< DataTypes >::addMDx( const core::MechanicalParams*, DataVecDeriv& res, const DataVecDeriv& dx, double factor )
{
    if( factor == 1.0 )
    {
        d_massMatrix.getValue().addMult( res, dx );
    }
    else
    {
        d_massMatrix.getValue().addMult( res, dx, factor );
    }
}

template < class DataTypes >
void PreassembledMass< DataTypes >::accFromF( const core::MechanicalParams*, DataVecDeriv& /*acc*/, const DataVecDeriv& /*f*/ )
{
    serr<<"accFromF not yet implemented (the matrix inversion is needed)"<<sendl;
}

template < class DataTypes >
double PreassembledMass< DataTypes >::getKineticEnergy( const core::MechanicalParams*, const DataVecDeriv& v ) const
{
    const VecDeriv& _v = v.getValue();
    double e = 0;

    VecDeriv Mv;
    d_massMatrix.getValue().mult( Mv, _v );

    for( unsigned int i=0 ; i<_v.size() ; i++ )
        e += _v[i] * Mv[i];

    return e/2;
}

template < class DataTypes >
double PreassembledMass< DataTypes >::getPotentialEnergy( const core::MechanicalParams* mparams, const DataVecCoord& x ) const
{
    serr<<SOFA_CLASS_METHOD<<"not implemented!\n";
    return core::behavior::Mass< DataTypes >::getPotentialEnergy( mparams, x );

//    const VecCoord& _x = x.getValue();

//    VecCoord Mx/* = d_massMatrix * _x*/;
//    d_massMatrix.mult( Mx, _x );

//    SReal e = 0;
//    // gravity
//    Vec3d g ( this->getContext()->getGravity() );
//    Deriv theGravity;
//    DataTypes::set ( theGravity, g[0], g[1], g[2] );
//    for( unsigned int i=0 ; i<_x.size() ; i++ )
//    {
//        e -= theGravity*Mx[i];
//    }
//    return e;
}

template < class DataTypes >
void PreassembledMass< DataTypes >::addGravityToV(const core::MechanicalParams* mparams, DataVecDeriv& d_v)
{
    if(mparams)
    {
        VecDeriv& v = *d_v.beginEdit();

        // gravity
        Vec3 g ( this->getContext()->getGravity() * (mparams->dt()) );
        Deriv theGravity;
        DataTypes::set ( theGravity, g[0], g[1], g[2]);
        Deriv hg = theGravity * (mparams->dt());

        // add weight force
        for (unsigned int i=0; i<v.size(); i++)
        {
            v[i] += hg;
        }
        d_v.endEdit();
    }
}

template < class DataTypes >
void PreassembledMass< DataTypes >::addForce(const core::MechanicalParams* /*mparams*/, DataVecDeriv& f, const DataVecCoord& /*x*/, const DataVecDeriv& /*v*/)
{
    const MassMatrix& M = d_massMatrix.getValue();

    // skip if we're not initialized yet (see bwdInit)
    if(!M.rows() || !M.cols()) return;

    //if gravity was added separately (in solver's "solve" method), then nothing
    //to do here
    if(this->m_separateGravity.getValue()) return;

    VecDeriv& _f = *f.beginEdit();

    // gravity
    Vec3 g ( this->getContext()->getGravity() );
//    Deriv theGravity;
//    DataTypes::set( theGravity, g[0], g[1], g[2] );
    // add weight
//    d_massMatrix.template addMul_by_line<Real,VecDeriv,Deriv>( _f, theGravity );

    //TODO optimize this!!!
    VecDeriv gravities(_f.size());
    for(size_t i=0 ; i<_f.size() ; ++i ) {
        DataTypes::set( gravities[i], g[0], g[1], g[2] );
    }

    M.addMult( _f, gravities );

    f.endEdit();
}

template < class DataTypes >
void PreassembledMass< DataTypes >::addMToMatrix(const core::MechanicalParams *mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix)
{
    sofa::core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate);
    Real mFactor = (Real)mparams->mFactorIncludingRayleighDamping(this->rayleighMass.getValue());
    d_massMatrix.getValue().addToBaseMatrix( r.matrix, mFactor, r.offset );
}

} // namespace mass

} // namespace component

} // namespace sofa

#endif // __PreassembledMass_INL
<|endoftext|>


<commit_before>/****************************************************************************
**
** Copyright (C) 2014 Digia Plc and/or its subsidiary(-ies).
** Contact: http://www.qt-project.org/legal
**
** This file is part of Qt Creator.
**
** Commercial License Usage
** Licensees holding valid commercial Qt licenses may use this file in
** accordance with the commercial license agreement provided with the
** Software or, alternatively, in accordance with the terms contained in
** a written agreement between you and Digia.  For licensing terms and
** conditions see http://qt.digia.com/licensing.  For further information
** use the contact form at http://qt.digia.com/contact-us.
**
** GNU Lesser General Public License Usage
** Alternatively, this file may be used under the terms of the GNU Lesser
** General Public License version 2.1 as published by the Free Software
** Foundation and appearing in the file LICENSE.LGPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU Lesser General Public License version 2.1 requirements
** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** In addition, as a special exception, Digia gives you certain additional
** rights.  These rights are described in the Digia Qt LGPL Exception
** version 1.1, included in the file LGPL_EXCEPTION.txt in this package.
**
****************************************************************************/

#include "qmltextgenerator.h"

#include <QVariant>
#include <QColor>

#include "bindingproperty.h"
#include "signalhandlerproperty.h"
#include "nodeproperty.h"
#include "nodelistproperty.h"
#include "variantproperty.h"
#include <nodemetainfo.h>
#include "model.h"

using namespace QmlDesigner;
using namespace QmlDesigner::Internal;

inline static QString properColorName(const QColor &color)
{
    QString s;
    if (color.alpha() == 255)
        s.sprintf("#%02x%02x%02x", color.red(), color.green(), color.blue());
    else
        s.sprintf("#%02x%02x%02x%02x", color.alpha(), color.red(), color.green(), color.blue());
    return s;
}

inline static QString doubleToString(double d)
{
    QString string = QString::number(d, 'f', 3);
    if (string.contains(QLatin1Char('.'))) {
        while (string.at(string.length()- 1) == QLatin1Char('0'))
            string.chop(1);
        if (string.at(string.length()- 1) == QLatin1Char('.'))
            string.chop(1);
    }
    return string;
}

QmlTextGenerator::QmlTextGenerator(const PropertyNameList &propertyOrder, int indentDepth):
        m_propertyOrder(propertyOrder),
        m_indentDepth(indentDepth)
{
}

QString QmlTextGenerator::toQml(const AbstractProperty &property, int indentDepth) const
{
    if (property.isBindingProperty()) {
        return property.toBindingProperty().expression();
    } else if (property.isSignalHandlerProperty()) {
        return property.toSignalHandlerProperty().source();
    } else if (property.isNodeProperty()) {
        return toQml(property.toNodeProperty().modelNode(), indentDepth);
    } else if (property.isNodeListProperty()) {
        const QList<ModelNode> nodes = property.toNodeListProperty().toModelNodeList();
        if (property.isDefaultProperty()) {
            QString result;
            for (int i = 0; i < nodes.length(); ++i) {
                if (i > 0)
                    result += QStringLiteral("\n\n");
                result += QString(indentDepth, QLatin1Char(' '));
                result += toQml(nodes.at(i), indentDepth);
            }
            return result;
        } else {
            QString result = QStringLiteral("[");
            const int arrayContentDepth = indentDepth + 4;
            const QString arrayContentIndentation(arrayContentDepth, QLatin1Char(' '));
            for (int i = 0; i < nodes.length(); ++i) {
                if (i > 0)
                    result += QLatin1Char(',');
                result += QLatin1Char('\n');
                result += arrayContentIndentation;
                result += toQml(nodes.at(i), arrayContentDepth);
            }
            return result + QLatin1Char(']');
        }
    } else if (property.isVariantProperty()) {
        const VariantProperty variantProperty = property.toVariantProperty();
        const QVariant value = variantProperty.value();
        const QString stringValue = value.toString();

        if (property.name() == "id")
            return stringValue;

          if (false) {
          }
        if (variantProperty.holdsEnumeration()) {
            return variantProperty.enumeration().toString();
        } else {

            switch (value.type()) {
            case QVariant::Bool:
                if (value.value<bool>())
                    return QStringLiteral("true");
                else
                    return QStringLiteral("false");

            case QVariant::Color:
                return QStringLiteral("\"%1\"").arg(properColorName(value.value<QColor>()));

            case static_cast<QVariant::Type>(QMetaType::Float):
            case QVariant::Double:
                return doubleToString(value.toDouble());
            case QVariant::Int:
            case QVariant::LongLong:
            case QVariant::UInt:
            case QVariant::ULongLong:
                return stringValue;

            default:
                return QStringLiteral("\"%1\"").arg(escape(stringValue));
            }
        }
    } else {
        Q_ASSERT("Unknown property type");
        return QString();
    }
}

QString QmlTextGenerator::toQml(const ModelNode &node, int indentDepth) const
{
    QString type = node.type();
    QString url;
    if (type.contains('.')) {
        QStringList nameComponents = type.split('.');
        url = nameComponents.first();
        type = nameComponents.last();
    }

    QString alias;
    if (!url.isEmpty()) {
        foreach (const Import &import, node.model()->imports()) {
            if (import.url() == url) {
                alias = import.alias();
                break;
            }
            if (import.file() == url) {
                alias = import.alias();
                break;
            }
        }
    }

    QString result;

    if (!alias.isEmpty())
        result = alias + '.';

    result += type;
    result += QStringLiteral(" {\n");

    const int propertyIndentDepth = indentDepth + 4;

    const QString properties = propertiesToQml(node, propertyIndentDepth);

    return result + properties + QString(indentDepth, QLatin1Char(' ')) + QLatin1Char('}');
}

QString QmlTextGenerator::propertiesToQml(const ModelNode &node, int indentDepth) const
{
    QString topPart;
    QString bottomPart;

    PropertyNameList nodePropertyNames = node.propertyNames();
    bool addToTop = true;

    foreach (const PropertyName &propertyName, m_propertyOrder) {
        if (propertyName == "id") {
            // the model handles the id property special, so:
            if (!node.id().isEmpty()) {
                QString idLine(indentDepth, QLatin1Char(' '));
                idLine += QStringLiteral("id: ");
                idLine += node.id();
                idLine += QLatin1Char('\n');

                if (addToTop)
                    topPart.append(idLine);
                else
                    bottomPart.append(idLine);
            }
        } else if (propertyName.isEmpty()) {
            addToTop = false;
        } else if (nodePropertyNames.removeAll(propertyName)) {
            const QString newContent = propertyToQml(node.property(propertyName), indentDepth);

            if (addToTop)
                topPart.append(newContent);
            else
                bottomPart.append(newContent);
        }
    }

    foreach (const PropertyName &propertyName, nodePropertyNames) {
        bottomPart.prepend(propertyToQml(node.property(propertyName), indentDepth));
    }

    return topPart + bottomPart;
}

QString QmlTextGenerator::propertyToQml(const AbstractProperty &property, int indentDepth) const
{
    QString result;

    if (property.isDefaultProperty()) {
        result = toQml(property, indentDepth);
    } else {
        if (property.isDynamic()) {
            result = QString(indentDepth, QLatin1Char(' '))
                    + QStringLiteral("property ")
                    + property.dynamicTypeName()
                    + QStringLiteral(" ")
                    + property.name()
                    + QStringLiteral(": ")
                    + toQml(property, indentDepth);
        } else {
            result = QString(indentDepth, QLatin1Char(' ')) + property.name() + QStringLiteral(": ") + toQml(property, indentDepth);
        }
    }

    result += QLatin1Char('\n');

    return result;
}

QString QmlTextGenerator::escape(const QString &value)
{
    QString result = value;

    result.replace(QStringLiteral("\\"), QStringLiteral("\\\\"));

    result.replace(QStringLiteral("\""), QStringLiteral("\\\""));
    result.replace(QStringLiteral("\t"), QStringLiteral("\\t"));
    result.replace(QStringLiteral("\r"), QStringLiteral("\\r"));
    result.replace(QStringLiteral("\n"), QStringLiteral("\\n"));

    return result;
}
<commit_msg>designer/qmltextgenerator: get rid of deprecated QVariant::Type<commit_after>/****************************************************************************
**
** Copyright (C) 2014 Digia Plc and/or its subsidiary(-ies).
** Contact: http://www.qt-project.org/legal
**
** This file is part of Qt Creator.
**
** Commercial License Usage
** Licensees holding valid commercial Qt licenses may use this file in
** accordance with the commercial license agreement provided with the
** Software or, alternatively, in accordance with the terms contained in
** a written agreement between you and Digia.  For licensing terms and
** conditions see http://qt.digia.com/licensing.  For further information
** use the contact form at http://qt.digia.com/contact-us.
**
** GNU Lesser General Public License Usage
** Alternatively, this file may be used under the terms of the GNU Lesser
** General Public License version 2.1 as published by the Free Software
** Foundation and appearing in the file LICENSE.LGPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU Lesser General Public License version 2.1 requirements
** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** In addition, as a special exception, Digia gives you certain additional
** rights.  These rights are described in the Digia Qt LGPL Exception
** version 1.1, included in the file LGPL_EXCEPTION.txt in this package.
**
****************************************************************************/

#include "qmltextgenerator.h"

#include <QVariant>
#include <QColor>

#include "bindingproperty.h"
#include "signalhandlerproperty.h"
#include "nodeproperty.h"
#include "nodelistproperty.h"
#include "variantproperty.h"
#include <nodemetainfo.h>
#include "model.h"

using namespace QmlDesigner;
using namespace QmlDesigner::Internal;

inline static QString properColorName(const QColor &color)
{
    QString s;
    if (color.alpha() == 255)
        s.sprintf("#%02x%02x%02x", color.red(), color.green(), color.blue());
    else
        s.sprintf("#%02x%02x%02x%02x", color.alpha(), color.red(), color.green(), color.blue());
    return s;
}

inline static QString doubleToString(double d)
{
    QString string = QString::number(d, 'f', 3);
    if (string.contains(QLatin1Char('.'))) {
        while (string.at(string.length()- 1) == QLatin1Char('0'))
            string.chop(1);
        if (string.at(string.length()- 1) == QLatin1Char('.'))
            string.chop(1);
    }
    return string;
}

QmlTextGenerator::QmlTextGenerator(const PropertyNameList &propertyOrder, int indentDepth):
        m_propertyOrder(propertyOrder),
        m_indentDepth(indentDepth)
{
}

QString QmlTextGenerator::toQml(const AbstractProperty &property, int indentDepth) const
{
    if (property.isBindingProperty()) {
        return property.toBindingProperty().expression();
    } else if (property.isSignalHandlerProperty()) {
        return property.toSignalHandlerProperty().source();
    } else if (property.isNodeProperty()) {
        return toQml(property.toNodeProperty().modelNode(), indentDepth);
    } else if (property.isNodeListProperty()) {
        const QList<ModelNode> nodes = property.toNodeListProperty().toModelNodeList();
        if (property.isDefaultProperty()) {
            QString result;
            for (int i = 0; i < nodes.length(); ++i) {
                if (i > 0)
                    result += QStringLiteral("\n\n");
                result += QString(indentDepth, QLatin1Char(' '));
                result += toQml(nodes.at(i), indentDepth);
            }
            return result;
        } else {
            QString result = QStringLiteral("[");
            const int arrayContentDepth = indentDepth + 4;
            const QString arrayContentIndentation(arrayContentDepth, QLatin1Char(' '));
            for (int i = 0; i < nodes.length(); ++i) {
                if (i > 0)
                    result += QLatin1Char(',');
                result += QLatin1Char('\n');
                result += arrayContentIndentation;
                result += toQml(nodes.at(i), arrayContentDepth);
            }
            return result + QLatin1Char(']');
        }
    } else if (property.isVariantProperty()) {
        const VariantProperty variantProperty = property.toVariantProperty();
        const QVariant value = variantProperty.value();
        const QString stringValue = value.toString();

        if (property.name() == "id")
            return stringValue;

          if (false) {
          }
        if (variantProperty.holdsEnumeration()) {
            return variantProperty.enumeration().toString();
        } else {

            switch (value.userType()) {
            case QMetaType::Bool:
                if (value.value<bool>())
                    return QStringLiteral("true");
                else
                    return QStringLiteral("false");

            case QMetaType::QColor:
                return QStringLiteral("\"%1\"").arg(properColorName(value.value<QColor>()));

            case QMetaType::Float:
            case QMetaType::Double:
                return doubleToString(value.toDouble());
            case QMetaType::Int:
            case QMetaType::LongLong:
            case QMetaType::UInt:
            case QMetaType::ULongLong:
                return stringValue;

            default:
                return QStringLiteral("\"%1\"").arg(escape(stringValue));
            }
        }
    } else {
        Q_ASSERT("Unknown property type");
        return QString();
    }
}

QString QmlTextGenerator::toQml(const ModelNode &node, int indentDepth) const
{
    QString type = node.type();
    QString url;
    if (type.contains('.')) {
        QStringList nameComponents = type.split('.');
        url = nameComponents.first();
        type = nameComponents.last();
    }

    QString alias;
    if (!url.isEmpty()) {
        foreach (const Import &import, node.model()->imports()) {
            if (import.url() == url) {
                alias = import.alias();
                break;
            }
            if (import.file() == url) {
                alias = import.alias();
                break;
            }
        }
    }

    QString result;

    if (!alias.isEmpty())
        result = alias + '.';

    result += type;
    result += QStringLiteral(" {\n");

    const int propertyIndentDepth = indentDepth + 4;

    const QString properties = propertiesToQml(node, propertyIndentDepth);

    return result + properties + QString(indentDepth, QLatin1Char(' ')) + QLatin1Char('}');
}

QString QmlTextGenerator::propertiesToQml(const ModelNode &node, int indentDepth) const
{
    QString topPart;
    QString bottomPart;

    PropertyNameList nodePropertyNames = node.propertyNames();
    bool addToTop = true;

    foreach (const PropertyName &propertyName, m_propertyOrder) {
        if (propertyName == "id") {
            // the model handles the id property special, so:
            if (!node.id().isEmpty()) {
                QString idLine(indentDepth, QLatin1Char(' '));
                idLine += QStringLiteral("id: ");
                idLine += node.id();
                idLine += QLatin1Char('\n');

                if (addToTop)
                    topPart.append(idLine);
                else
                    bottomPart.append(idLine);
            }
        } else if (propertyName.isEmpty()) {
            addToTop = false;
        } else if (nodePropertyNames.removeAll(propertyName)) {
            const QString newContent = propertyToQml(node.property(propertyName), indentDepth);

            if (addToTop)
                topPart.append(newContent);
            else
                bottomPart.append(newContent);
        }
    }

    foreach (const PropertyName &propertyName, nodePropertyNames) {
        bottomPart.prepend(propertyToQml(node.property(propertyName), indentDepth));
    }

    return topPart + bottomPart;
}

QString QmlTextGenerator::propertyToQml(const AbstractProperty &property, int indentDepth) const
{
    QString result;

    if (property.isDefaultProperty()) {
        result = toQml(property, indentDepth);
    } else {
        if (property.isDynamic()) {
            result = QString(indentDepth, QLatin1Char(' '))
                    + QStringLiteral("property ")
                    + property.dynamicTypeName()
                    + QStringLiteral(" ")
                    + property.name()
                    + QStringLiteral(": ")
                    + toQml(property, indentDepth);
        } else {
            result = QString(indentDepth, QLatin1Char(' ')) + property.name() + QStringLiteral(": ") + toQml(property, indentDepth);
        }
    }

    result += QLatin1Char('\n');

    return result;
}

QString QmlTextGenerator::escape(const QString &value)
{
    QString result = value;

    result.replace(QStringLiteral("\\"), QStringLiteral("\\\\"));

    result.replace(QStringLiteral("\""), QStringLiteral("\\\""));
    result.replace(QStringLiteral("\t"), QStringLiteral("\\t"));
    result.replace(QStringLiteral("\r"), QStringLiteral("\\r"));
    result.replace(QStringLiteral("\n"), QStringLiteral("\\n"));

    return result;
}
<|endoftext|>


<commit_before>//===--- BackendUtil.cpp - LLVM Backend Utilities -------------------------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//

#include "clang/CodeGen/BackendUtil.h"
#include "clang/Basic/Diagnostic.h"
#include "clang/Basic/TargetOptions.h"
#include "clang/Basic/LangOptions.h"
#include "clang/Frontend/CodeGenOptions.h"
#include "clang/Frontend/FrontendDiagnostic.h"
#include "llvm/Module.h"
#include "llvm/PassManager.h"
#include "llvm/Analysis/Verifier.h"
#include "llvm/Assembly/PrintModulePass.h"
#include "llvm/Bitcode/ReaderWriter.h"
#include "llvm/CodeGen/RegAllocRegistry.h"
#include "llvm/CodeGen/SchedulerRegistry.h"
#include "llvm/MC/SubtargetFeature.h"
#include "llvm/Support/CommandLine.h"
#include "llvm/Support/FormattedStream.h"
#include "llvm/Support/PrettyStackTrace.h"
#include "llvm/Support/TargetRegistry.h"
#include "llvm/Support/Timer.h"
#include "llvm/Support/raw_ostream.h"
#include "llvm/Target/TargetData.h"
#include "llvm/Target/TargetLibraryInfo.h"
#include "llvm/Target/TargetMachine.h"
#include "llvm/Target/TargetOptions.h"
#include "llvm/Transforms/Instrumentation.h"
#include "llvm/Transforms/IPO.h"
#include "llvm/Transforms/IPO/PassManagerBuilder.h"
#include "llvm/Transforms/Scalar.h"
using namespace clang;
using namespace llvm;

namespace {

class EmitAssemblyHelper {
  DiagnosticsEngine &Diags;
  const CodeGenOptions &CodeGenOpts;
  const TargetOptions &TargetOpts;
  const LangOptions &LangOpts;
  Module *TheModule;

  Timer CodeGenerationTime;

  mutable PassManager *CodeGenPasses;
  mutable PassManager *PerModulePasses;
  mutable FunctionPassManager *PerFunctionPasses;

private:
  PassManager *getCodeGenPasses() const {
    if (!CodeGenPasses) {
      CodeGenPasses = new PassManager();
      CodeGenPasses->add(new TargetData(TheModule));
    }
    return CodeGenPasses;
  }

  PassManager *getPerModulePasses() const {
    if (!PerModulePasses) {
      PerModulePasses = new PassManager();
      PerModulePasses->add(new TargetData(TheModule));
    }
    return PerModulePasses;
  }

  FunctionPassManager *getPerFunctionPasses() const {
    if (!PerFunctionPasses) {
      PerFunctionPasses = new FunctionPassManager(TheModule);
      PerFunctionPasses->add(new TargetData(TheModule));
    }
    return PerFunctionPasses;
  }

  void CreatePasses();

  /// AddEmitPasses - Add passes necessary to emit assembly or LLVM IR.
  ///
  /// \return True on success.
  bool AddEmitPasses(BackendAction Action, formatted_raw_ostream &OS);

public:
  EmitAssemblyHelper(DiagnosticsEngine &_Diags,
                     const CodeGenOptions &CGOpts, const TargetOptions &TOpts,
                     const LangOptions &LOpts,
                     Module *M)
    : Diags(_Diags), CodeGenOpts(CGOpts), TargetOpts(TOpts), LangOpts(LOpts),
      TheModule(M), CodeGenerationTime("Code Generation Time"),
      CodeGenPasses(0), PerModulePasses(0), PerFunctionPasses(0) {}

  ~EmitAssemblyHelper() {
    delete CodeGenPasses;
    delete PerModulePasses;
    delete PerFunctionPasses;
  }

  void EmitAssembly(BackendAction Action, raw_ostream *OS);
};

}

static void addObjCARCExpandPass(const PassManagerBuilder &Builder, PassManagerBase &PM) {
  if (Builder.OptLevel > 0)
    PM.add(createObjCARCExpandPass());
}

static void addObjCARCOptPass(const PassManagerBuilder &Builder, PassManagerBase &PM) {
  if (Builder.OptLevel > 0)
    PM.add(createObjCARCOptPass());
}

static void addAddressSanitizerPass(const PassManagerBuilder &Builder,
                                    PassManagerBase &PM) {
  PM.add(createAddressSanitizerPass());
}

void EmitAssemblyHelper::CreatePasses() {
  unsigned OptLevel = CodeGenOpts.OptimizationLevel;
  CodeGenOptions::InliningMethod Inlining = CodeGenOpts.Inlining;

  // Handle disabling of LLVM optimization, where we want to preserve the
  // internal module before any optimization.
  if (CodeGenOpts.DisableLLVMOpts) {
    OptLevel = 0;
    Inlining = CodeGenOpts.NoInlining;
  }

  PassManagerBuilder PMBuilder;
  PMBuilder.OptLevel = OptLevel;
  PMBuilder.SizeLevel = CodeGenOpts.OptimizeSize;

  PMBuilder.DisableSimplifyLibCalls = !CodeGenOpts.SimplifyLibCalls;
  PMBuilder.DisableUnitAtATime = !CodeGenOpts.UnitAtATime;
  PMBuilder.DisableUnrollLoops = !CodeGenOpts.UnrollLoops;

  // In ObjC ARC mode, add the main ARC optimization passes.
  if (LangOpts.ObjCAutoRefCount) {
    PMBuilder.addExtension(PassManagerBuilder::EP_EarlyAsPossible,
                           addObjCARCExpandPass);
    PMBuilder.addExtension(PassManagerBuilder::EP_ScalarOptimizerLate,
                           addObjCARCOptPass);
  }

  if (LangOpts.AddressSanitizer) {
    PMBuilder.addExtension(PassManagerBuilder::EP_ScalarOptimizerLate,
                           addAddressSanitizerPass);
    PMBuilder.addExtension(PassManagerBuilder::EP_EnabledOnOptLevel0,
                           addAddressSanitizerPass);
  }

  // Figure out TargetLibraryInfo.
  Triple TargetTriple(TheModule->getTargetTriple());
  PMBuilder.LibraryInfo = new TargetLibraryInfo(TargetTriple);
  if (!CodeGenOpts.SimplifyLibCalls)
    PMBuilder.LibraryInfo->disableAllFunctions();

  switch (Inlining) {
  case CodeGenOptions::NoInlining: break;
  case CodeGenOptions::NormalInlining: {
    // FIXME: Derive these constants in a principled fashion.
    unsigned Threshold = 225;
    if (CodeGenOpts.OptimizeSize == 1)      // -Os
      Threshold = 75;
    else if (CodeGenOpts.OptimizeSize == 2) // -Oz
      Threshold = 25;
    else if (OptLevel > 2)
      Threshold = 275;
    PMBuilder.Inliner = createFunctionInliningPass(Threshold);
    break;
  }
  case CodeGenOptions::OnlyAlwaysInlining:
    // Respect always_inline.
    PMBuilder.Inliner = createAlwaysInlinerPass();
    break;
  }

  // Set up the per-function pass manager.
  FunctionPassManager *FPM = getPerFunctionPasses();
  if (CodeGenOpts.VerifyModule)
    FPM->add(createVerifierPass());
  PMBuilder.populateFunctionPassManager(*FPM);

  // Set up the per-module pass manager.
  PassManager *MPM = getPerModulePasses();

  if (CodeGenOpts.EmitGcovArcs || CodeGenOpts.EmitGcovNotes) {
    MPM->add(createGCOVProfilerPass(CodeGenOpts.EmitGcovNotes,
                                    CodeGenOpts.EmitGcovArcs,
                                    TargetTriple.isMacOSX()));

    if (!CodeGenOpts.DebugInfo)
      MPM->add(createStripSymbolsPass(true));
  }

  PMBuilder.populateModulePassManager(*MPM);
}

bool EmitAssemblyHelper::AddEmitPasses(BackendAction Action,
                                       formatted_raw_ostream &OS) {
  // Create the TargetMachine for generating code.
  std::string Error;
  std::string Triple = TheModule->getTargetTriple();
  const llvm::Target *TheTarget = TargetRegistry::lookupTarget(Triple, Error);
  if (!TheTarget) {
    Diags.Report(diag::err_fe_unable_to_create_target) << Error;
    return false;
  }

  // FIXME: Expose these capabilities via actual APIs!!!! Aside from just
  // being gross, this is also totally broken if we ever care about
  // concurrency.

  // Set frame pointer elimination mode.
  if (!CodeGenOpts.DisableFPElim) {
    llvm::NoFramePointerElim = false;
    llvm::NoFramePointerElimNonLeaf = false;
  } else if (CodeGenOpts.OmitLeafFramePointer) {
    llvm::NoFramePointerElim = false;
    llvm::NoFramePointerElimNonLeaf = true;
  } else {
    llvm::NoFramePointerElim = true;
    llvm::NoFramePointerElimNonLeaf = true;
  }

  // Set float ABI type.
  if (CodeGenOpts.FloatABI == "soft" || CodeGenOpts.FloatABI == "softfp")
    llvm::FloatABIType = llvm::FloatABI::Soft;
  else if (CodeGenOpts.FloatABI == "hard")
    llvm::FloatABIType = llvm::FloatABI::Hard;
  else {
    assert(CodeGenOpts.FloatABI.empty() && "Invalid float abi!");
    llvm::FloatABIType = llvm::FloatABI::Default;
  }

  llvm::LessPreciseFPMADOption = CodeGenOpts.LessPreciseFPMAD;
  llvm::NoInfsFPMath = CodeGenOpts.NoInfsFPMath;
  llvm::NoNaNsFPMath = CodeGenOpts.NoNaNsFPMath;
  NoZerosInBSS = CodeGenOpts.NoZeroInitializedInBSS;
  llvm::UnsafeFPMath = CodeGenOpts.UnsafeFPMath;
  llvm::UseSoftFloat = CodeGenOpts.SoftFloat;

  TargetMachine::setAsmVerbosityDefault(CodeGenOpts.AsmVerbose);

  TargetMachine::setFunctionSections(CodeGenOpts.FunctionSections);
  TargetMachine::setDataSections    (CodeGenOpts.DataSections);

  // FIXME: Parse this earlier.
  llvm::CodeModel::Model CM;
  if (CodeGenOpts.CodeModel == "small") {
    CM = llvm::CodeModel::Small;
  } else if (CodeGenOpts.CodeModel == "kernel") {
    CM = llvm::CodeModel::Kernel;
  } else if (CodeGenOpts.CodeModel == "medium") {
    CM = llvm::CodeModel::Medium;
  } else if (CodeGenOpts.CodeModel == "large") {
    CM = llvm::CodeModel::Large;
  } else {
    assert(CodeGenOpts.CodeModel.empty() && "Invalid code model!");
    CM = llvm::CodeModel::Default;
  }

  std::vector<const char *> BackendArgs;
  BackendArgs.push_back("clang"); // Fake program name.
  if (!CodeGenOpts.DebugPass.empty()) {
    BackendArgs.push_back("-debug-pass");
    BackendArgs.push_back(CodeGenOpts.DebugPass.c_str());
  }
  if (!CodeGenOpts.LimitFloatPrecision.empty()) {
    BackendArgs.push_back("-limit-float-precision");
    BackendArgs.push_back(CodeGenOpts.LimitFloatPrecision.c_str());
  }
  if (llvm::TimePassesIsEnabled)
    BackendArgs.push_back("-time-passes");
  for (unsigned i = 0, e = CodeGenOpts.BackendOptions.size(); i != e; ++i)
    BackendArgs.push_back(CodeGenOpts.BackendOptions[i].c_str());
  if (CodeGenOpts.NoGlobalMerge)
    BackendArgs.push_back("-global-merge=false");
  BackendArgs.push_back(0);
  llvm::cl::ParseCommandLineOptions(BackendArgs.size() - 1,
                                    const_cast<char **>(&BackendArgs[0]));

  std::string FeaturesStr;
  if (TargetOpts.Features.size()) {
    SubtargetFeatures Features;
    for (std::vector<std::string>::const_iterator
           it = TargetOpts.Features.begin(),
           ie = TargetOpts.Features.end(); it != ie; ++it)
      Features.AddFeature(*it);
    FeaturesStr = Features.getString();
  }

  llvm::Reloc::Model RM = llvm::Reloc::Default;
  if (CodeGenOpts.RelocationModel == "static") {
    RM = llvm::Reloc::Static;
  } else if (CodeGenOpts.RelocationModel == "pic") {
    RM = llvm::Reloc::PIC_;
  } else {
    assert(CodeGenOpts.RelocationModel == "dynamic-no-pic" &&
           "Invalid PIC model!");
    RM = llvm::Reloc::DynamicNoPIC;
  }

  CodeGenOpt::Level OptLevel = CodeGenOpt::Default;
  switch (CodeGenOpts.OptimizationLevel) {
  default: break;
  case 0: OptLevel = CodeGenOpt::None; break;
  case 3: OptLevel = CodeGenOpt::Aggressive; break;
  }

  TargetMachine *TM = TheTarget->createTargetMachine(Triple, TargetOpts.CPU,
                                                     FeaturesStr, RM, CM,
                                                     OptLevel);

  if (CodeGenOpts.RelaxAll)
    TM->setMCRelaxAll(true);
  if (CodeGenOpts.SaveTempLabels)
    TM->setMCSaveTempLabels(true);
  if (CodeGenOpts.NoDwarf2CFIAsm)
    TM->setMCUseCFI(false);
  if (!CodeGenOpts.NoDwarfDirectoryAsm)
    TM->setMCUseDwarfDirectory(true);
  if (CodeGenOpts.NoExecStack)
    TM->setMCNoExecStack(true);

  // Create the code generator passes.
  PassManager *PM = getCodeGenPasses();

  // Normal mode, emit a .s or .o file by running the code generator. Note,
  // this also adds codegenerator level optimization passes.
  TargetMachine::CodeGenFileType CGFT = TargetMachine::CGFT_AssemblyFile;
  if (Action == Backend_EmitObj)
    CGFT = TargetMachine::CGFT_ObjectFile;
  else if (Action == Backend_EmitMCNull)
    CGFT = TargetMachine::CGFT_Null;
  else
    assert(Action == Backend_EmitAssembly && "Invalid action!");

  // Add ObjC ARC final-cleanup optimizations. This is done as part of the
  // "codegen" passes so that it isn't run multiple times when there is
  // inlining happening.
  if (LangOpts.ObjCAutoRefCount)
    PM->add(createObjCARCContractPass());

  if (TM->addPassesToEmitFile(*PM, OS, CGFT,
                              /*DisableVerify=*/!CodeGenOpts.VerifyModule)) {
    Diags.Report(diag::err_fe_unable_to_interface_with_target);
    return false;
  }

  return true;
}

void EmitAssemblyHelper::EmitAssembly(BackendAction Action, raw_ostream *OS) {
  TimeRegion Region(llvm::TimePassesIsEnabled ? &CodeGenerationTime : 0);
  llvm::formatted_raw_ostream FormattedOS;

  CreatePasses();
  switch (Action) {
  case Backend_EmitNothing:
    break;

  case Backend_EmitBC:
    getPerModulePasses()->add(createBitcodeWriterPass(*OS));
    break;

  case Backend_EmitLL:
    FormattedOS.setStream(*OS, formatted_raw_ostream::PRESERVE_STREAM);
    getPerModulePasses()->add(createPrintModulePass(&FormattedOS));
    break;

  default:
    FormattedOS.setStream(*OS, formatted_raw_ostream::PRESERVE_STREAM);
    if (!AddEmitPasses(Action, FormattedOS))
      return;
  }

  // Before executing passes, print the final values of the LLVM options.
  cl::PrintOptionValues();

  // Run passes. For now we do all passes at once, but eventually we
  // would like to have the option of streaming code generation.

  if (PerFunctionPasses) {
    PrettyStackTraceString CrashInfo("Per-function optimization");

    PerFunctionPasses->doInitialization();
    for (Module::iterator I = TheModule->begin(),
           E = TheModule->end(); I != E; ++I)
      if (!I->isDeclaration())
        PerFunctionPasses->run(*I);
    PerFunctionPasses->doFinalization();
  }

  if (PerModulePasses) {
    PrettyStackTraceString CrashInfo("Per-module optimization passes");
    PerModulePasses->run(*TheModule);
  }

  if (CodeGenPasses) {
    PrettyStackTraceString CrashInfo("Code generation");
    CodeGenPasses->run(*TheModule);
  }
}

void clang::EmitBackendOutput(DiagnosticsEngine &Diags,
                              const CodeGenOptions &CGOpts,
                              const TargetOptions &TOpts,
                              const LangOptions &LOpts,
                              Module *M,
                              BackendAction Action, raw_ostream *OS) {
  EmitAssemblyHelper AsmHelper(Diags, CGOpts, TOpts, LOpts, M);

  AsmHelper.EmitAssembly(Action, OS);
}
<commit_msg>Update for change to LLVM TargetMachine API in r145714.<commit_after>//===--- BackendUtil.cpp - LLVM Backend Utilities -------------------------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//

#include "clang/CodeGen/BackendUtil.h"
#include "clang/Basic/Diagnostic.h"
#include "clang/Basic/TargetOptions.h"
#include "clang/Basic/LangOptions.h"
#include "clang/Frontend/CodeGenOptions.h"
#include "clang/Frontend/FrontendDiagnostic.h"
#include "llvm/Module.h"
#include "llvm/PassManager.h"
#include "llvm/Analysis/Verifier.h"
#include "llvm/Assembly/PrintModulePass.h"
#include "llvm/Bitcode/ReaderWriter.h"
#include "llvm/CodeGen/RegAllocRegistry.h"
#include "llvm/CodeGen/SchedulerRegistry.h"
#include "llvm/MC/SubtargetFeature.h"
#include "llvm/Support/CommandLine.h"
#include "llvm/Support/FormattedStream.h"
#include "llvm/Support/PrettyStackTrace.h"
#include "llvm/Support/TargetRegistry.h"
#include "llvm/Support/Timer.h"
#include "llvm/Support/raw_ostream.h"
#include "llvm/Target/TargetData.h"
#include "llvm/Target/TargetLibraryInfo.h"
#include "llvm/Target/TargetMachine.h"
#include "llvm/Target/TargetOptions.h"
#include "llvm/Transforms/Instrumentation.h"
#include "llvm/Transforms/IPO.h"
#include "llvm/Transforms/IPO/PassManagerBuilder.h"
#include "llvm/Transforms/Scalar.h"
using namespace clang;
using namespace llvm;

namespace {

class EmitAssemblyHelper {
  DiagnosticsEngine &Diags;
  const CodeGenOptions &CodeGenOpts;
  const clang::TargetOptions &TargetOpts;
  const LangOptions &LangOpts;
  Module *TheModule;

  Timer CodeGenerationTime;

  mutable PassManager *CodeGenPasses;
  mutable PassManager *PerModulePasses;
  mutable FunctionPassManager *PerFunctionPasses;

private:
  PassManager *getCodeGenPasses() const {
    if (!CodeGenPasses) {
      CodeGenPasses = new PassManager();
      CodeGenPasses->add(new TargetData(TheModule));
    }
    return CodeGenPasses;
  }

  PassManager *getPerModulePasses() const {
    if (!PerModulePasses) {
      PerModulePasses = new PassManager();
      PerModulePasses->add(new TargetData(TheModule));
    }
    return PerModulePasses;
  }

  FunctionPassManager *getPerFunctionPasses() const {
    if (!PerFunctionPasses) {
      PerFunctionPasses = new FunctionPassManager(TheModule);
      PerFunctionPasses->add(new TargetData(TheModule));
    }
    return PerFunctionPasses;
  }

  void CreatePasses();

  /// AddEmitPasses - Add passes necessary to emit assembly or LLVM IR.
  ///
  /// \return True on success.
  bool AddEmitPasses(BackendAction Action, formatted_raw_ostream &OS);

public:
  EmitAssemblyHelper(DiagnosticsEngine &_Diags,
                     const CodeGenOptions &CGOpts,
                     const clang::TargetOptions &TOpts,
                     const LangOptions &LOpts,
                     Module *M)
    : Diags(_Diags), CodeGenOpts(CGOpts), TargetOpts(TOpts), LangOpts(LOpts),
      TheModule(M), CodeGenerationTime("Code Generation Time"),
      CodeGenPasses(0), PerModulePasses(0), PerFunctionPasses(0) {}

  ~EmitAssemblyHelper() {
    delete CodeGenPasses;
    delete PerModulePasses;
    delete PerFunctionPasses;
  }

  void EmitAssembly(BackendAction Action, raw_ostream *OS);
};

}

static void addObjCARCExpandPass(const PassManagerBuilder &Builder, PassManagerBase &PM) {
  if (Builder.OptLevel > 0)
    PM.add(createObjCARCExpandPass());
}

static void addObjCARCOptPass(const PassManagerBuilder &Builder, PassManagerBase &PM) {
  if (Builder.OptLevel > 0)
    PM.add(createObjCARCOptPass());
}

static void addAddressSanitizerPass(const PassManagerBuilder &Builder,
                                    PassManagerBase &PM) {
  PM.add(createAddressSanitizerPass());
}

void EmitAssemblyHelper::CreatePasses() {
  unsigned OptLevel = CodeGenOpts.OptimizationLevel;
  CodeGenOptions::InliningMethod Inlining = CodeGenOpts.Inlining;

  // Handle disabling of LLVM optimization, where we want to preserve the
  // internal module before any optimization.
  if (CodeGenOpts.DisableLLVMOpts) {
    OptLevel = 0;
    Inlining = CodeGenOpts.NoInlining;
  }

  PassManagerBuilder PMBuilder;
  PMBuilder.OptLevel = OptLevel;
  PMBuilder.SizeLevel = CodeGenOpts.OptimizeSize;

  PMBuilder.DisableSimplifyLibCalls = !CodeGenOpts.SimplifyLibCalls;
  PMBuilder.DisableUnitAtATime = !CodeGenOpts.UnitAtATime;
  PMBuilder.DisableUnrollLoops = !CodeGenOpts.UnrollLoops;

  // In ObjC ARC mode, add the main ARC optimization passes.
  if (LangOpts.ObjCAutoRefCount) {
    PMBuilder.addExtension(PassManagerBuilder::EP_EarlyAsPossible,
                           addObjCARCExpandPass);
    PMBuilder.addExtension(PassManagerBuilder::EP_ScalarOptimizerLate,
                           addObjCARCOptPass);
  }

  if (LangOpts.AddressSanitizer) {
    PMBuilder.addExtension(PassManagerBuilder::EP_ScalarOptimizerLate,
                           addAddressSanitizerPass);
    PMBuilder.addExtension(PassManagerBuilder::EP_EnabledOnOptLevel0,
                           addAddressSanitizerPass);
  }

  // Figure out TargetLibraryInfo.
  Triple TargetTriple(TheModule->getTargetTriple());
  PMBuilder.LibraryInfo = new TargetLibraryInfo(TargetTriple);
  if (!CodeGenOpts.SimplifyLibCalls)
    PMBuilder.LibraryInfo->disableAllFunctions();

  switch (Inlining) {
  case CodeGenOptions::NoInlining: break;
  case CodeGenOptions::NormalInlining: {
    // FIXME: Derive these constants in a principled fashion.
    unsigned Threshold = 225;
    if (CodeGenOpts.OptimizeSize == 1)      // -Os
      Threshold = 75;
    else if (CodeGenOpts.OptimizeSize == 2) // -Oz
      Threshold = 25;
    else if (OptLevel > 2)
      Threshold = 275;
    PMBuilder.Inliner = createFunctionInliningPass(Threshold);
    break;
  }
  case CodeGenOptions::OnlyAlwaysInlining:
    // Respect always_inline.
    PMBuilder.Inliner = createAlwaysInlinerPass();
    break;
  }

  // Set up the per-function pass manager.
  FunctionPassManager *FPM = getPerFunctionPasses();
  if (CodeGenOpts.VerifyModule)
    FPM->add(createVerifierPass());
  PMBuilder.populateFunctionPassManager(*FPM);

  // Set up the per-module pass manager.
  PassManager *MPM = getPerModulePasses();

  if (CodeGenOpts.EmitGcovArcs || CodeGenOpts.EmitGcovNotes) {
    MPM->add(createGCOVProfilerPass(CodeGenOpts.EmitGcovNotes,
                                    CodeGenOpts.EmitGcovArcs,
                                    TargetTriple.isMacOSX()));

    if (!CodeGenOpts.DebugInfo)
      MPM->add(createStripSymbolsPass(true));
  }

  PMBuilder.populateModulePassManager(*MPM);
}

bool EmitAssemblyHelper::AddEmitPasses(BackendAction Action,
                                       formatted_raw_ostream &OS) {
  // Create the TargetMachine for generating code.
  std::string Error;
  std::string Triple = TheModule->getTargetTriple();
  const llvm::Target *TheTarget = TargetRegistry::lookupTarget(Triple, Error);
  if (!TheTarget) {
    Diags.Report(diag::err_fe_unable_to_create_target) << Error;
    return false;
  }

  // FIXME: Expose these capabilities via actual APIs!!!! Aside from just
  // being gross, this is also totally broken if we ever care about
  // concurrency.

  TargetMachine::setAsmVerbosityDefault(CodeGenOpts.AsmVerbose);

  TargetMachine::setFunctionSections(CodeGenOpts.FunctionSections);
  TargetMachine::setDataSections    (CodeGenOpts.DataSections);

  // FIXME: Parse this earlier.
  llvm::CodeModel::Model CM;
  if (CodeGenOpts.CodeModel == "small") {
    CM = llvm::CodeModel::Small;
  } else if (CodeGenOpts.CodeModel == "kernel") {
    CM = llvm::CodeModel::Kernel;
  } else if (CodeGenOpts.CodeModel == "medium") {
    CM = llvm::CodeModel::Medium;
  } else if (CodeGenOpts.CodeModel == "large") {
    CM = llvm::CodeModel::Large;
  } else {
    assert(CodeGenOpts.CodeModel.empty() && "Invalid code model!");
    CM = llvm::CodeModel::Default;
  }

  std::vector<const char *> BackendArgs;
  BackendArgs.push_back("clang"); // Fake program name.
  if (!CodeGenOpts.DebugPass.empty()) {
    BackendArgs.push_back("-debug-pass");
    BackendArgs.push_back(CodeGenOpts.DebugPass.c_str());
  }
  if (!CodeGenOpts.LimitFloatPrecision.empty()) {
    BackendArgs.push_back("-limit-float-precision");
    BackendArgs.push_back(CodeGenOpts.LimitFloatPrecision.c_str());
  }
  if (llvm::TimePassesIsEnabled)
    BackendArgs.push_back("-time-passes");
  for (unsigned i = 0, e = CodeGenOpts.BackendOptions.size(); i != e; ++i)
    BackendArgs.push_back(CodeGenOpts.BackendOptions[i].c_str());
  if (CodeGenOpts.NoGlobalMerge)
    BackendArgs.push_back("-global-merge=false");
  BackendArgs.push_back(0);
  llvm::cl::ParseCommandLineOptions(BackendArgs.size() - 1,
                                    const_cast<char **>(&BackendArgs[0]));

  std::string FeaturesStr;
  if (TargetOpts.Features.size()) {
    SubtargetFeatures Features;
    for (std::vector<std::string>::const_iterator
           it = TargetOpts.Features.begin(),
           ie = TargetOpts.Features.end(); it != ie; ++it)
      Features.AddFeature(*it);
    FeaturesStr = Features.getString();
  }

  llvm::Reloc::Model RM = llvm::Reloc::Default;
  if (CodeGenOpts.RelocationModel == "static") {
    RM = llvm::Reloc::Static;
  } else if (CodeGenOpts.RelocationModel == "pic") {
    RM = llvm::Reloc::PIC_;
  } else {
    assert(CodeGenOpts.RelocationModel == "dynamic-no-pic" &&
           "Invalid PIC model!");
    RM = llvm::Reloc::DynamicNoPIC;
  }

  CodeGenOpt::Level OptLevel = CodeGenOpt::Default;
  switch (CodeGenOpts.OptimizationLevel) {
  default: break;
  case 0: OptLevel = CodeGenOpt::None; break;
  case 3: OptLevel = CodeGenOpt::Aggressive; break;
  }

  llvm::TargetOptions Options;

  // Set frame pointer elimination mode.
  if (!CodeGenOpts.DisableFPElim) {
    Options.NoFramePointerElim = false;
    Options.NoFramePointerElimNonLeaf = false;
  } else if (CodeGenOpts.OmitLeafFramePointer) {
    Options.NoFramePointerElim = false;
    Options.NoFramePointerElimNonLeaf = true;
  } else {
    Options.NoFramePointerElim = true;
    Options.NoFramePointerElimNonLeaf = true;
  }

  // Set float ABI type.
  if (CodeGenOpts.FloatABI == "soft" || CodeGenOpts.FloatABI == "softfp")
    Options.FloatABIType = llvm::FloatABI::Soft;
  else if (CodeGenOpts.FloatABI == "hard")
    Options.FloatABIType = llvm::FloatABI::Hard;
  else {
    assert(CodeGenOpts.FloatABI.empty() && "Invalid float abi!");
    Options.FloatABIType = llvm::FloatABI::Default;
  }

  Options.LessPreciseFPMADOption = CodeGenOpts.LessPreciseFPMAD;
  Options.NoInfsFPMath = CodeGenOpts.NoInfsFPMath;
  Options.NoNaNsFPMath = CodeGenOpts.NoNaNsFPMath;
  Options.NoZerosInBSS = CodeGenOpts.NoZeroInitializedInBSS;
  Options.UnsafeFPMath = CodeGenOpts.UnsafeFPMath;
  Options.UseSoftFloat = CodeGenOpts.SoftFloat;

  TargetMachine *TM = TheTarget->createTargetMachine(Triple, TargetOpts.CPU,
                                                     FeaturesStr, Options,
                                                     RM, CM, OptLevel);

  if (CodeGenOpts.RelaxAll)
    TM->setMCRelaxAll(true);
  if (CodeGenOpts.SaveTempLabels)
    TM->setMCSaveTempLabels(true);
  if (CodeGenOpts.NoDwarf2CFIAsm)
    TM->setMCUseCFI(false);
  if (!CodeGenOpts.NoDwarfDirectoryAsm)
    TM->setMCUseDwarfDirectory(true);
  if (CodeGenOpts.NoExecStack)
    TM->setMCNoExecStack(true);

  // Create the code generator passes.
  PassManager *PM = getCodeGenPasses();

  // Normal mode, emit a .s or .o file by running the code generator. Note,
  // this also adds codegenerator level optimization passes.
  TargetMachine::CodeGenFileType CGFT = TargetMachine::CGFT_AssemblyFile;
  if (Action == Backend_EmitObj)
    CGFT = TargetMachine::CGFT_ObjectFile;
  else if (Action == Backend_EmitMCNull)
    CGFT = TargetMachine::CGFT_Null;
  else
    assert(Action == Backend_EmitAssembly && "Invalid action!");

  // Add ObjC ARC final-cleanup optimizations. This is done as part of the
  // "codegen" passes so that it isn't run multiple times when there is
  // inlining happening.
  if (LangOpts.ObjCAutoRefCount)
    PM->add(createObjCARCContractPass());

  if (TM->addPassesToEmitFile(*PM, OS, CGFT,
                              /*DisableVerify=*/!CodeGenOpts.VerifyModule)) {
    Diags.Report(diag::err_fe_unable_to_interface_with_target);
    return false;
  }

  return true;
}

void EmitAssemblyHelper::EmitAssembly(BackendAction Action, raw_ostream *OS) {
  TimeRegion Region(llvm::TimePassesIsEnabled ? &CodeGenerationTime : 0);
  llvm::formatted_raw_ostream FormattedOS;

  CreatePasses();
  switch (Action) {
  case Backend_EmitNothing:
    break;

  case Backend_EmitBC:
    getPerModulePasses()->add(createBitcodeWriterPass(*OS));
    break;

  case Backend_EmitLL:
    FormattedOS.setStream(*OS, formatted_raw_ostream::PRESERVE_STREAM);
    getPerModulePasses()->add(createPrintModulePass(&FormattedOS));
    break;

  default:
    FormattedOS.setStream(*OS, formatted_raw_ostream::PRESERVE_STREAM);
    if (!AddEmitPasses(Action, FormattedOS))
      return;
  }

  // Before executing passes, print the final values of the LLVM options.
  cl::PrintOptionValues();

  // Run passes. For now we do all passes at once, but eventually we
  // would like to have the option of streaming code generation.

  if (PerFunctionPasses) {
    PrettyStackTraceString CrashInfo("Per-function optimization");

    PerFunctionPasses->doInitialization();
    for (Module::iterator I = TheModule->begin(),
           E = TheModule->end(); I != E; ++I)
      if (!I->isDeclaration())
        PerFunctionPasses->run(*I);
    PerFunctionPasses->doFinalization();
  }

  if (PerModulePasses) {
    PrettyStackTraceString CrashInfo("Per-module optimization passes");
    PerModulePasses->run(*TheModule);
  }

  if (CodeGenPasses) {
    PrettyStackTraceString CrashInfo("Code generation");
    CodeGenPasses->run(*TheModule);
  }
}

void clang::EmitBackendOutput(DiagnosticsEngine &Diags,
                              const CodeGenOptions &CGOpts,
                              const clang::TargetOptions &TOpts,
                              const LangOptions &LOpts,
                              Module *M,
                              BackendAction Action, raw_ostream *OS) {
  EmitAssemblyHelper AsmHelper(Diags, CGOpts, TOpts, LOpts, M);

  AsmHelper.EmitAssembly(Action, OS);
}
<|endoftext|>