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

/**
 * @file ecl_pitch_controller.cpp
 * Implementation of a simple orthogonal pitch PID controller.
 *
 * Authors and acknowledgements in header.
 */

#include "ecl_pitch_controller.h"
#include <math.h>
#include <stdint.h>
#include <float.h>
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
#include <systemlib/err.h>

ECL_PitchController::ECL_PitchController() :
	ECL_Controller("pitch"),
	_max_rate_neg(0.0f),
	_roll_ff(0.0f)
{
}

ECL_PitchController::~ECL_PitchController()
{
}

float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data)
{
	float roll = ctl_data.roll;

	/* Do not calculate control signal with bad inputs */
	if (!(isfinite(ctl_data.pitch_setpoint) &&
	      isfinite(roll) &&
	      isfinite(ctl_data.pitch) &&
	      isfinite(ctl_data.airspeed))) {
		perf_count(_nonfinite_input_perf);
		warnx("not controlling pitch");
		return _rate_setpoint;
	}

	/* flying inverted (wings upside down) ? */
	bool inverted = false;

	/* roll is used as feedforward term and inverted flight needs to be considered */
	if (fabsf(roll) < math::radians(90.0f)) {
		/* not inverted, but numerically still potentially close to infinity */
		roll = math::constrain(roll, math::radians(-80.0f), math::radians(80.0f));

	} else {
		/* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */

		/* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */
		if (roll > 0.0f) {
			/* right hemisphere */
			roll = math::constrain(roll, math::radians(100.0f), math::radians(180.0f));

		} else {
			/* left hemisphere */
			roll = math::constrain(roll, math::radians(-100.0f), math::radians(-180.0f));
		}
	}

	/* input conditioning */
	float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max);

	/* Calculate desired y-axis angular rate needed to compensate for roll angle.
	   For reference see Automatic Control of Aircraft and Missiles by John H. Blakelock, pg. 175
	   Availible on google books 8/11/2015:
	   https://books.google.com/books?id=ubcczZUDCsMC&pg=PA175#v=onepage&q&f=false*/
	float body_fixed_turn_offset = (fabsf((CONSTANTS_ONE_G / airspeed) *
				  		tanf(roll) * sinf(roll)));

	if (inverted) {
		body_fixed_turn_offset = -body_fixed_turn_offset;
	}

	/*Finally transform the body_fixed_turn offset to a change pitch angular rate */
	float turn_offset = cosf(roll)*body_fixed_turn_offset-sinf(roll)*ctl_data.yaw_rate;

	/* Calculate the error */
	float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch;

	/*  Apply P controller: rate setpoint from current error and time constant */
	_rate_setpoint =  pitch_error / _tc;

	/* add turn offset */
	_rate_setpoint += turn_offset;

	/* limit the rate */ //XXX: move to body angluar rates

	if (_max_rate > 0.01f && _max_rate_neg > 0.01f) {
		if (_rate_setpoint > 0.0f) {
			_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;

		} else {
			_rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint;
		}

	}

	return _rate_setpoint;
}

float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
	/* Do not calculate control signal with bad inputs */
	if (!(isfinite(ctl_data.roll) &&
	      isfinite(ctl_data.pitch) &&
	      isfinite(ctl_data.pitch_rate) &&
	      isfinite(ctl_data.yaw_rate) &&
	      isfinite(ctl_data.yaw_rate_setpoint) &&
	      isfinite(ctl_data.airspeed_min) &&
	      isfinite(ctl_data.airspeed_max) &&
	      isfinite(ctl_data.scaler))) {
		perf_count(_nonfinite_input_perf);
		return math::constrain(_last_output, -1.0f, 1.0f);
	}

	/* get the usual dt estimate */
	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
	_last_run = ecl_absolute_time();
	float dt = (float)dt_micros * 1e-6f;

	/* lock integral for long intervals */
	bool lock_integrator = ctl_data.lock_integrator;

	if (dt_micros > 500000) {
		lock_integrator = true;
	}

	/* Transform setpoint to body angular rates (jacobian) */
	_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
			     cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;

	_rate_error = _bodyrate_setpoint - ctl_data.pitch_rate;

	if (!lock_integrator && _k_i > 0.0f) {

		float id = _rate_error * dt * ctl_data.scaler;

		/*
		 * anti-windup: do not allow integrator to increase if actuator is at limit
		 */
		if (_last_output < -1.0f) {
			/* only allow motion to center: increase value */
			id = math::max(id, 0.0f);

		} else if (_last_output > 1.0f) {
			/* only allow motion to center: decrease value */
			id = math::min(id, 0.0f);
		}

		_integrator += id;
	}

	/* integrator limit */
	//xxx: until start detection is available: integral part in control signal is limited here
	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);

	/* Apply PI rate controller and store non-limited output */
	_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
		       _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
		       + integrator_constrained;  //scaler is proportional to 1/airspeed
//	warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
//	warnx("roll: _last_output %.4f", (double)_last_output);
	return math::constrain(_last_output, -1.0f, 1.0f);
}
<commit_msg>Handle turn pitch rate compensation in pitch rate controller instead of pitch controller.  Also set inverted flag to true if rolled more than 90 degrees<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name ECL nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file ecl_pitch_controller.cpp
 * Implementation of a simple orthogonal pitch PID controller.
 *
 * Authors and acknowledgements in header.
 */

#include "ecl_pitch_controller.h"
#include <math.h>
#include <stdint.h>
#include <float.h>
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
#include <systemlib/err.h>

ECL_PitchController::ECL_PitchController() :
	ECL_Controller("pitch"),
	_max_rate_neg(0.0f),
	_roll_ff(0.0f)
{
}

ECL_PitchController::~ECL_PitchController()
{
}

float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data)
{

	/* Do not calculate control signal with bad inputs */
	if (!(isfinite(ctl_data.pitch_setpoint) &&
	      isfinite(ctl_data.roll) &&
	      isfinite(ctl_data.pitch) &&
	      isfinite(ctl_data.airspeed))) {
		perf_count(_nonfinite_input_perf);
		warnx("not controlling pitch");
		return _rate_setpoint;
	}

	/* Calculate the error */
	float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch;

	/*  Apply P controller: rate setpoint from current error and time constant */
	_rate_setpoint =  pitch_error / _tc;

	/* limit the rate */
	if (_max_rate > 0.01f && _max_rate_neg > 0.01f) {
		if (_rate_setpoint > 0.0f) {
			_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;

		} else {
			_rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint;
		}

	}

	return _rate_setpoint;
}

float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
	/* Do not calculate control signal with bad inputs */
	if (!(isfinite(ctl_data.roll) &&
	      isfinite(ctl_data.pitch) &&
	      isfinite(ctl_data.pitch_rate) &&
	      isfinite(ctl_data.yaw_rate) &&
	      isfinite(ctl_data.yaw_rate_setpoint) &&
	      isfinite(ctl_data.airspeed_min) &&
	      isfinite(ctl_data.airspeed_max) &&
	      isfinite(ctl_data.scaler))) {
		perf_count(_nonfinite_input_perf);
		return math::constrain(_last_output, -1.0f, 1.0f);
	}

	/* get the usual dt estimate */
	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
	_last_run = ecl_absolute_time();
	float dt = (float)dt_micros * 1e-6f;

	/* lock integral for long intervals */
	bool lock_integrator = ctl_data.lock_integrator;

	if (dt_micros > 500000) {
		lock_integrator = true;
	}

	/* Transform setpoint to body angular rates (jacobian) */
	_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
			     cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;

	/* apply turning offset to desired bodyrate setpoint*/
	/* flying inverted (wings upside down)*/
	bool inverted = false;
	float constrained_roll;
	/* roll is used as feedforward term and inverted flight needs to be considered */
	if (fabsf(ctl_data.roll) < math::radians(90.0f)) {
		/* not inverted, but numerically still potentially close to infinity */
		constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f));

	} else {
		/* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */
		inverted = true;
		/* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */
		if (ctl_data.roll > 0.0f) {
			/* right hemisphere */
			constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f));

		} else {
			/* left hemisphere */
			constrained_roll = math::constrain(ctl_data.roll, math::radians(-100.0f), math::radians(-180.0f));
		}
	}

	/* input conditioning */
	float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max);

	/* Calculate desired body fixed y-axis angular rate needed to compensate for roll angle.
	   For reference see Automatic Control of Aircraft and Missiles by John H. Blakelock, pg. 175
	   Availible on google books 8/11/2015:
	   https://books.google.com/books?id=ubcczZUDCsMC&pg=PA175#v=onepage&q&f=false*/
	float body_fixed_turn_offset = (fabsf((CONSTANTS_ONE_G / airspeed) *
				  		tanf(constrained_roll) * sinf(constrained_roll)));

	if (inverted) {
		body_fixed_turn_offset = -body_fixed_turn_offset;
	}

	/* Finally add the turn offset to your bodyrate setpoint*/
	_bodyrate_setpoint += body_fixed_turn_offset;

	_rate_error = _bodyrate_setpoint - ctl_data.pitch_rate;

	if (!lock_integrator && _k_i > 0.0f) {

		float id = _rate_error * dt * ctl_data.scaler;

		/*
		 * anti-windup: do not allow integrator to increase if actuator is at limit
		 */
		if (_last_output < -1.0f) {
			/* only allow motion to center: increase value */
			id = math::max(id, 0.0f);

		} else if (_last_output > 1.0f) {
			/* only allow motion to center: decrease value */
			id = math::min(id, 0.0f);
		}

		_integrator += id;
	}

	/* integrator limit */
	//xxx: until start detection is available: integral part in control signal is limited here
	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);

	/* Apply PI rate controller and store non-limited output */
	_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
		       _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
		       + integrator_constrained;  //scaler is proportional to 1/airspeed
//	warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
//	warnx("roll: _last_output %.4f", (double)_last_output);
	return math::constrain(_last_output, -1.0f, 1.0f);
}
<|endoftext|>


<commit_before>/**
 * @file ToDartUtils.cpp
 * @brief Functions that convert ModelInterface objects to their Dart counterparts
 * @author A. Huaman Quispe
 * @date 2012 / 10 / 12
 */
#include "DartLoader.h"
#include <dynamics/SkeletonDynamics.h>
#include <kinematics/Joint.h>
#include <kinematics/Transformation.h>
#include <kinematics/TrfmTranslate.h>
#include <kinematics/TrfmRotateEuler.h>
#include <kinematics/Dof.h>
#include <kinematics/BodyNode.h>
#include <kinematics/Shape.h>
#include <dynamics/BodyNodeDynamics.h>
#include "../urdf_parser/urdf_parser.h"
#include "../urdfdom_headers/urdf_model/link.h"

// For continuous joint limit
#include <limits>

/**
 * @function createDartRootJoint
 * @brief Create defined rootJoint between world and rootNode
 */
kinematics::Joint* DartLoader::createDartRootJoint( boost::shared_ptr<urdf::Joint> _jt,
						    dynamics::SkeletonDynamics* _skel ) {

  // Currently we support 2 root joints: Fixed and floating
  // other type of joints (revolute and prismatic) appear as fixed so don't use them by the time being
  // (joints located on nodes other than root can  be anything)

  double x, y, z;
  double roll, pitch, yaw;
  double val;
  double vmin, vmax;
  int axis;
  double xAxis, yAxis, zAxis;

  // Parent is NULL
  if( strcmp( (_jt->parent_link_name).c_str(), "world") != 0 ) {
    std::cout<< "Error, createDartRootJoint should receive a joint with world as parent. No creating joint" << std::endl;
    return NULL;
  }

  // Get child body Nodes
  kinematics::Joint* joint;
  std::string jointName = ( _jt->name);
  std::string childName =  (_jt->child_link_name);
  dynamics::BodyNodeDynamics* child = getNode( childName );

  joint = new kinematics::Joint( NULL, child, jointName.c_str() );

  // Add DOF if prismatic or revolute joint
  val = 0;
  if( _jt->type == urdf::Joint::FLOATING ) {

    // Here will go the rest of joints (revolute, prismatic, hence the next if)

    // Floating
    if( _jt->type == urdf::Joint::FLOATING ) {

      axis = GOLEM_FLOATING;

      // Define infinity limits
      vmin = -1*std::numeric_limits<double>::infinity();
      vmax = std::numeric_limits<double>::infinity();

      xAxis = 0; yAxis = 0; zAxis = 0;
    }

    add_DOF( _skel, joint, val, vmin, vmax, axis, xAxis, yAxis, zAxis );

    return joint;
  }

  // Fixed, do not add DOF
  else if( _jt->type == urdf::Joint::FIXED ) {
    if(debug) std::cout<<"[debug] Fixed Root joint: "<< jointName <<std::endl;

    // Add Rigid transform
    urdf::Pose p = _jt->parent_to_joint_origin_transform;
    x = p.position.x;
    y = p.position.y;
    z = p.position.z;
    p.rotation.getRPY( roll, pitch, yaw );
    add_XyzRpy( joint, x, y, z, roll, pitch, yaw );

    return joint;
  }

  // None of the above
  else {
    std::cout<<"[createDartRootJoint] ERROR: Parsing "<<  jointName <<" joint: No FLOATING or FIXED \n";
    return NULL;
  }

}

/**
 * @function createNewDartRootJoint
 * @brief Create a new floating joint if no joint is defined between world and rootNode
 */
kinematics::Joint* DartLoader::createNewDartRootJoint( dynamics::BodyNodeDynamics* _node,
						       dynamics::SkeletonDynamics* _skel ) {

  // Parent will be NULL.
  kinematics::Joint* rootJoint;
  if(debug) std::cout<<"[debug] Creating joint for root node: "<< _node->getName() <<std::endl;

  // This joint connects with the world
  rootJoint = new kinematics::Joint( NULL, _node, "worldJoint" );

  // This will be a FREEEULER joint type. We have 3 DOF for rotation and 3 DOF for translation
  add_DOF( _skel, rootJoint, 0, 0, 0, GOLEM_FLOATING );

  return rootJoint;
}

/**
 * @function createDartJoint
 */
kinematics::Joint* DartLoader::createDartJoint( boost::shared_ptr<urdf::Joint> _jt,
						dynamics::SkeletonDynamics* _skel ) {

  double x, y, z;
  double roll, pitch, yaw;
  double val;
  double vmin, vmax;
  int axis;
  double xAxis, yAxis, zAxis;

  // Get parent and child body Nodes
  std::string jointName = ( _jt->name);
  std::string parentName = (_jt->parent_link_name);
  std::string childName =  (_jt->child_link_name);
  dynamics::BodyNodeDynamics* parent = getNode( parentName );
  dynamics::BodyNodeDynamics* child = getNode( childName );

  // Create joint
  kinematics::Joint* joint;
  if( parent == NULL && parentName == "world" ) {
    joint = new kinematics::Joint( NULL, child, jointName.c_str() );
    std::cout<<"Creating joint between world and "<<childName<<std::endl;
  } else {
    joint = new kinematics::Joint( parent, child, jointName.c_str() );
  }

  // Add Rigid transform
  urdf::Pose p = _jt->parent_to_joint_origin_transform;
  x = p.position.x;
  y = p.position.y;
  z = p.position.z;
  p.rotation.getRPY( roll, pitch, yaw );
  add_XyzRpy( joint, x, y, z, roll, pitch, yaw );

  // Add DOF if prismatic or revolute joint
  val = 0;
  if( _jt->type == urdf::Joint::REVOLUTE ||
      _jt->type == urdf::Joint::PRISMATIC ||
      _jt->type == urdf::Joint::CONTINUOUS ||
      _jt->type == urdf::Joint::FLOATING ) {

    // Revolute
    if(   _jt->type == urdf::Joint::REVOLUTE ) {
      if( _jt->axis.x == 1 /*&& _jt->axis.y == 0 && _jt->axis.z == 0 */) {
	axis = GOLEM_ROLL;
      } else if( _jt->axis.y == 1  /* && _jt->axis.x == 0 && _jt->axis.z == 0 */ ) {
	axis = GOLEM_PITCH;
      } else if( _jt->axis.z == 1   /* && _jt->axis.x == 0 && _jt->axis.y == 0 */) {
	axis = GOLEM_YAW;
      } else {
	axis = GOLEM_ARBITRARY_ROTATION;
      }

      vmin = _jt->limits->lower;
      vmax = _jt->limits->upper;

      xAxis = _jt->axis.x; yAxis = _jt->axis.y; zAxis = _jt->axis.z;
    }

    // Prismatic
    else if( _jt->type == urdf::Joint::PRISMATIC ) {

      if( _jt->axis.x == 1 || _jt->axis.x == -1  ) { axis = GOLEM_X; }
      else if( _jt->axis.y == 1 || _jt->axis.y == -1  ) { axis = GOLEM_Y; }
      else if( _jt->axis.z == 1 || _jt->axis.z == -1 ) { axis = GOLEM_Z; }
      else { printf (" [ERROR] No axis defined for Prismatic DOF! \n"); }

      vmin = _jt->limits->lower;
      vmax = _jt->limits->upper;

      xAxis = _jt->axis.x; yAxis = _jt->axis.y; zAxis = _jt->axis.z;
    }

    // Continuous
    else if( _jt->type == urdf::Joint::CONTINUOUS ) {
      if( _jt->axis.x == 1 && _jt->axis.y == 0 && _jt->axis.z == 0 ) {
	axis = GOLEM_ROLL;
      } else if( _jt->axis.y == 1 && _jt->axis.x == 0 && _jt->axis.z == 0  ) {
	axis = GOLEM_PITCH;
      } else if( _jt->axis.z == 1 && _jt->axis.x == 0 && _jt->axis.y == 0 ) {
	axis = GOLEM_YAW;
      } else {
	axis = GOLEM_ARBITRARY_ROTATION;
      }
      // Define infinity limits
      vmin = -1*std::numeric_limits<double>::infinity();
      vmax = std::numeric_limits<double>::infinity();

      xAxis = _jt->axis.x; yAxis = _jt->axis.y; zAxis = _jt->axis.z;
    }

    // Floating
    else if( _jt->type == urdf::Joint::FLOATING ) {

      axis = GOLEM_FLOATING;

      // Define infinity limits
      vmin = 0;
      vmax = 0;

      xAxis = 0; yAxis = 0; zAxis = 0;
    }

    add_DOF( _skel, joint, val, vmin, vmax, axis, xAxis, yAxis, zAxis );

  }

  // Fixed, do not add DOF
  else if( _jt->type == urdf::Joint::FIXED ) {
    if(debug) std::cout<<"[debug] Fixed joint: "<< jointName <<std::endl;
  }

  // None of the above
  else {
    std::cout<<"[createDartJoint] ERROR: Parsing "<<  jointName <<" joint: No PRISMATIC or REVOLUTE or CONTINUOUS or FIXED \n";
  }

  return joint;
}

/**
 * @function createDartNode
 */
dynamics::BodyNodeDynamics* DartLoader::createDartNode( boost::shared_ptr<urdf::Link> _lk,
							dynamics::SkeletonDynamics* _skel,
							std::string _rootToSkelPath ) {

  std::string lk_name = _lk->name;

  if( lk_name == "world" ) {
    std::cout << "[info] world is not parsed as a link" << std::endl;
    return NULL;
  }

  if(debug) std::cout<<"[debug] Creating dart node:"<< lk_name <<std::endl;

  dynamics::BodyNodeDynamics* node =  (dynamics::BodyNodeDynamics*) _skel->createBodyNode( lk_name.c_str() );

  // Mesh Loading
  //FIXME: Shouldn't mass and inertia default to 0?
  double mass = 0.1;
  Eigen::Matrix3d inertia = Eigen::MatrixXd::Identity(3,3);
  inertia *= 0.1;

  // Load Inertial information
  if( _lk->inertial ) {
    boost::shared_ptr<urdf::Inertial>inert= (_lk->inertial);

    // Load mass
    mass = inert->mass;

    // Load Inertia matrix
    inertia(0,0) = inert->ixx;
    inertia(0,1) = -1*(inert->ixy);
    inertia(0,2) = -1*(inert->ixz);

    inertia(1,0) = -1*(inert->ixy);
    inertia(1,1) = (inert->iyy);
    inertia(1,2) = -1*(inert->iyz);

    inertia(2,0) = -1*(inert->ixz);
    inertia(2,1) = -1*(inert->iyz);
    inertia(2,2) = inert->izz;

    // Load local CoM
    Eigen::Vector3d localCOM;
    localCOM << inert->origin.position.x,
      inert->origin.position.y,
      inert->origin.position.z;
    // Set it to Node
    node->setLocalCOM( localCOM );

    if( debug ) { std::cout<< "[debug] Mass is: "<< mass << std::endl; }
    if( debug ) { std::cout<< "[debug] Inertia is: \n"<< inertia << std::endl; }
  }

  // Set inertial information
  node->setMass(mass);
  node->setLocalInertia(inertia);

  // Set visual information
  if( _lk->visual ) {
    if( add_VizShape( node, _lk->visual, _rootToSkelPath ) == false ) { std::cout<< "Error loading VizShape" <<std::endl; return NULL; }
  }
  else {
    // Add default
    kinematics::Shape* shape = new kinematics::Shape();
    node->setVisualizationShape( shape );
    if(debug) {  std::cout<< "No Visualization element defined for node "<<node->getName() <<". Using default empty VizShape"<<std::endl; }
  }
  // Set collision information
  if( _lk->collision ) {
    if( add_ColShape( node, _lk->collision, _rootToSkelPath ) == false ) {  std::cout<< "Error loading ColShape" <<std::endl; return NULL; }
  }
  else {
    // Add default
    kinematics::Shape* shape = new kinematics::Shape();
    node->setCollisionShape( shape );
    if(debug) { std::cout<< "No Collision element defined for node "<<node->getName() <<". Using default empty ColShape"<<std::endl; }
  }

  return node;
}
<commit_msg>Added the other joint types for root<commit_after>/**
 * @file ToDartUtils.cpp
 * @brief Functions that convert ModelInterface objects to their Dart counterparts
 * @author A. Huaman Quispe
 * @date 2012 / 10 / 12
 */
#include "DartLoader.h"
#include <dynamics/SkeletonDynamics.h>
#include <kinematics/Joint.h>
#include <kinematics/Transformation.h>
#include <kinematics/TrfmTranslate.h>
#include <kinematics/TrfmRotateEuler.h>
#include <kinematics/Dof.h>
#include <kinematics/BodyNode.h>
#include <kinematics/Shape.h>
#include <dynamics/BodyNodeDynamics.h>
#include "../urdf_parser/urdf_parser.h"
#include "../urdfdom_headers/urdf_model/link.h"

// For continuous joint limit
#include <limits>

/**
 * @function createDartRootJoint
 * @brief Create defined rootJoint between world and rootNode
 */
kinematics::Joint* DartLoader::createDartRootJoint( boost::shared_ptr<urdf::Joint> _jt,
						    dynamics::SkeletonDynamics* _skel ) {

    // Currently we support 2 root joints: Fixed and floating
    // other type of joints (revolute and prismatic) appear as fixed so don't use them by the time being
    // (joints located on nodes other than root can be anything)

    double x, y, z;
    double roll, pitch, yaw;
    double val;
    double vmin, vmax;
    int axis;
    double xAxis, yAxis, zAxis;

    // Parent is NULL (World).
    if( strcmp( (_jt->parent_link_name).c_str(), "world") != 0 ) {
        std::cout<< "Error, createDartRootJoint should receive a joint with world as parent. No creating joint" << std::endl;
        return NULL;
    }

    // Get child body Nodes
    kinematics::Joint* joint;
    std::string jointName = ( _jt->name);
    std::string childName = (_jt->child_link_name);
    dynamics::BodyNodeDynamics* child = getNode( childName );

    joint = new kinematics::Joint( NULL, child, jointName.c_str() );

    // For FLOATING root joint we do NOT use createDartJoint since it automatically adds a transform xyzRPY (fixed)
    // before the DOF. This would make the root joint to behave as a fixed joint with the current setup
    val = 0;
    if( _jt->type == urdf::Joint::FLOATING ) {

        axis = GOLEM_FLOATING;

        // Define infinity limits
        vmin = -1*std::numeric_limits<double>::infinity();
        vmax = std::numeric_limits<double>::infinity();
        xAxis = 0; yAxis = 0; zAxis = 0;

        add_DOF( _skel, joint, val, vmin, vmax, axis, xAxis, yAxis, zAxis );
        return joint;
    }

    else {
        return createDartJoint( _jt, _skel );
    }

}

/**
 * @function createNewDartRootJoint
 * @brief Create a new floating joint if no joint is defined between world and rootNode
 */
kinematics::Joint* DartLoader::createNewDartRootJoint( dynamics::BodyNodeDynamics* _node,
						       dynamics::SkeletonDynamics* _skel ) {

  // Parent will be NULL.
  kinematics::Joint* rootJoint;
  if(debug) std::cout<<"[debug] Creating joint for root node: "<< _node->getName() <<std::endl;

  // This joint connects with the world
  rootJoint = new kinematics::Joint( NULL, _node, "worldJoint" );

  // This will be a FREEEULER joint type. We have 3 DOF for rotation and 3 DOF for translation
  add_DOF( _skel, rootJoint, 0, 0, 0, GOLEM_FLOATING );

  return rootJoint;
}

/**
 * @function createDartJoint
 */
kinematics::Joint* DartLoader::createDartJoint( boost::shared_ptr<urdf::Joint> _jt,
						dynamics::SkeletonDynamics* _skel ) {

  double x, y, z;
  double roll, pitch, yaw;
  double val;
  double vmin, vmax;
  int axis;
  double xAxis, yAxis, zAxis;

  // Get parent and child body Nodes
  std::string jointName = ( _jt->name);
  std::string parentName = (_jt->parent_link_name);
  std::string childName =  (_jt->child_link_name);
  dynamics::BodyNodeDynamics* parent = getNode( parentName );
  dynamics::BodyNodeDynamics* child = getNode( childName );

  // Create joint
  kinematics::Joint* joint;
  if( parent == NULL && parentName == "world" ) {
    joint = new kinematics::Joint( NULL, child, jointName.c_str() );
    std::cout<<"Creating joint between world and "<<childName<<std::endl;
  } else {
    joint = new kinematics::Joint( parent, child, jointName.c_str() );
  }

  // Add Rigid transform
  urdf::Pose p = _jt->parent_to_joint_origin_transform;
  x = p.position.x;
  y = p.position.y;
  z = p.position.z;
  p.rotation.getRPY( roll, pitch, yaw );
  add_XyzRpy( joint, x, y, z, roll, pitch, yaw );

  // Add DOF if prismatic or revolute joint
  val = 0;
  if( _jt->type == urdf::Joint::REVOLUTE ||
      _jt->type == urdf::Joint::PRISMATIC ||
      _jt->type == urdf::Joint::CONTINUOUS ||
      _jt->type == urdf::Joint::FLOATING ) {

    // Revolute
    if(   _jt->type == urdf::Joint::REVOLUTE ) {
      if( _jt->axis.x == 1 /*&& _jt->axis.y == 0 && _jt->axis.z == 0 */) {
	axis = GOLEM_ROLL;
      } else if( _jt->axis.y == 1  /* && _jt->axis.x == 0 && _jt->axis.z == 0 */ ) {
	axis = GOLEM_PITCH;
      } else if( _jt->axis.z == 1   /* && _jt->axis.x == 0 && _jt->axis.y == 0 */) {
	axis = GOLEM_YAW;
      } else {
	axis = GOLEM_ARBITRARY_ROTATION;
      }

      vmin = _jt->limits->lower;
      vmax = _jt->limits->upper;

      xAxis = _jt->axis.x; yAxis = _jt->axis.y; zAxis = _jt->axis.z;
    }

    // Prismatic
    else if( _jt->type == urdf::Joint::PRISMATIC ) {

      if( _jt->axis.x == 1 || _jt->axis.x == -1  ) { axis = GOLEM_X; }
      else if( _jt->axis.y == 1 || _jt->axis.y == -1  ) { axis = GOLEM_Y; }
      else if( _jt->axis.z == 1 || _jt->axis.z == -1 ) { axis = GOLEM_Z; }
      else { printf (" [ERROR] No axis defined for Prismatic DOF! \n"); }

      vmin = _jt->limits->lower;
      vmax = _jt->limits->upper;

      xAxis = _jt->axis.x; yAxis = _jt->axis.y; zAxis = _jt->axis.z;
    }

    // Continuous
    else if( _jt->type == urdf::Joint::CONTINUOUS ) {
      if( _jt->axis.x == 1 && _jt->axis.y == 0 && _jt->axis.z == 0 ) {
	axis = GOLEM_ROLL;
      } else if( _jt->axis.y == 1 && _jt->axis.x == 0 && _jt->axis.z == 0  ) {
	axis = GOLEM_PITCH;
      } else if( _jt->axis.z == 1 && _jt->axis.x == 0 && _jt->axis.y == 0 ) {
	axis = GOLEM_YAW;
      } else {
	axis = GOLEM_ARBITRARY_ROTATION;
      }
      // Define infinity limits
      vmin = -1*std::numeric_limits<double>::infinity();
      vmax = std::numeric_limits<double>::infinity();

      xAxis = _jt->axis.x; yAxis = _jt->axis.y; zAxis = _jt->axis.z;
    }

    // Floating
    else if( _jt->type == urdf::Joint::FLOATING ) {

      axis = GOLEM_FLOATING;

      // Define infinity limits
      vmin = 0;
      vmax = 0;

      xAxis = 0; yAxis = 0; zAxis = 0;
    }

    add_DOF( _skel, joint, val, vmin, vmax, axis, xAxis, yAxis, zAxis );

  }

  // Fixed, do not add DOF
  else if( _jt->type == urdf::Joint::FIXED ) {
    if(debug) std::cout<<"[debug] Fixed joint: "<< jointName <<std::endl;
  }

  // None of the above
  else {
    std::cout<<"[createDartJoint] ERROR: Parsing "<<  jointName <<" joint: No PRISMATIC or REVOLUTE or CONTINUOUS or FIXED \n";
  }

  return joint;
}

/**
 * @function createDartNode
 */
dynamics::BodyNodeDynamics* DartLoader::createDartNode( boost::shared_ptr<urdf::Link> _lk,
							dynamics::SkeletonDynamics* _skel,
							std::string _rootToSkelPath ) {

  std::string lk_name = _lk->name;

  if( lk_name == "world" ) {
    std::cout << "[info] world is not parsed as a link" << std::endl;
    return NULL;
  }

  if(debug) std::cout<<"[debug] Creating dart node:"<< lk_name <<std::endl;

  dynamics::BodyNodeDynamics* node =  (dynamics::BodyNodeDynamics*) _skel->createBodyNode( lk_name.c_str() );

  // Mesh Loading
  //FIXME: Shouldn't mass and inertia default to 0?
  double mass = 0.1;
  Eigen::Matrix3d inertia = Eigen::MatrixXd::Identity(3,3);
  inertia *= 0.1;

  // Load Inertial information
  if( _lk->inertial ) {
    boost::shared_ptr<urdf::Inertial>inert= (_lk->inertial);

    // Load mass
    mass = inert->mass;

    // Load Inertia matrix
    inertia(0,0) = inert->ixx;
    inertia(0,1) = -1*(inert->ixy);
    inertia(0,2) = -1*(inert->ixz);

    inertia(1,0) = -1*(inert->ixy);
    inertia(1,1) = (inert->iyy);
    inertia(1,2) = -1*(inert->iyz);

    inertia(2,0) = -1*(inert->ixz);
    inertia(2,1) = -1*(inert->iyz);
    inertia(2,2) = inert->izz;

    // Load local CoM
    Eigen::Vector3d localCOM;
    localCOM << inert->origin.position.x,
      inert->origin.position.y,
      inert->origin.position.z;
    // Set it to Node
    node->setLocalCOM( localCOM );

    if( debug ) { std::cout<< "[debug] Mass is: "<< mass << std::endl; }
    if( debug ) { std::cout<< "[debug] Inertia is: \n"<< inertia << std::endl; }
  }

  // Set inertial information
  node->setMass(mass);
  node->setLocalInertia(inertia);

  // Set visual information
  if( _lk->visual ) {
    if( add_VizShape( node, _lk->visual, _rootToSkelPath ) == false ) { std::cout<< "Error loading VizShape" <<std::endl; return NULL; }
  }
  else {
    // Add default
    kinematics::Shape* shape = new kinematics::Shape();
    node->setVisualizationShape( shape );
    if(debug) {  std::cout<< "No Visualization element defined for node "<<node->getName() <<". Using default empty VizShape"<<std::endl; }
  }
  // Set collision information
  if( _lk->collision ) {
    if( add_ColShape( node, _lk->collision, _rootToSkelPath ) == false ) {  std::cout<< "Error loading ColShape" <<std::endl; return NULL; }
  }
  else {
    // Add default
    kinematics::Shape* shape = new kinematics::Shape();
    node->setCollisionShape( shape );
    if(debug) { std::cout<< "No Collision element defined for node "<<node->getName() <<". Using default empty ColShape"<<std::endl; }
  }

  return node;
}
<|endoftext|>


<commit_before>// coding: utf-8
/* Copyright (c) 2015, Roboterclub Aachen e. V.
 * All Rights Reserved.
 *
 * The file is part of the xpcc library and is released under the 3-clause BSD
 * license. See the file `LICENSE` for the full license governing this code.
 */
// ----------------------------------------------------------------------------

#ifndef XPCC_GPIO_EXPANDER_HPP
#define XPCC_GPIO_EXPANDER_HPP

#include <xpcc/architecture/interface/gpio.hpp>
#include <xpcc/architecture/interface/register.hpp>
#include <xpcc/processing/resumable.hpp>

namespace xpcc
{

/**
 * Interface of an IO expander driver.
 *
 * All IO expander driver should implement this interface, so that they can be wrapped
 * into `xpcc::GpioExpanderPin` and `xpcc::GpioExpanderPort`.
 *
 * The interface is almost identical to an GPIO pin, except multiple pins can be set at once.
 *
 * @note The driver must buffer the expanders IO registers. This means at least the direction,
 *       output and input values should be buffered so that a read-modify-write can be performed
 *       on SRAM and then only one write is performed on the bus instead of a read and write!
 *
 * @warning The `read()` function does not perform a read bus access, but operates on the
 *          buffered result of `readInput()`. This allows a capture-and-evaluate approach, which
 *          is especially useful for reducing bus traffic and necessary for sampling all pins at
 *          the same time for `xpcc::GpioExpanderPort`.
 *
 * @see xpcc::GpioExpanderPin
 * @see xpcc::GpioExpanderPort
 * @see xpcc::GpioIO
 *
 * @author  Niklas Hauser
 * @ingroup gpio
 */
class GpioExpander
{
#ifdef __DOXYGEN__
public:
	/// This holds the width of the port as number of bits
	/// and can be used in drivers to assert the correct requirements
	static constexpr uint8_t width;

public:
	/// A type containing a unique identifier for each pin.
	/// This is mostly a bit mask, however, since this is implementation
	/// defined it can also be an index.
	/// The underlying type depends on the width of the expander!
	enum class
	Pin : uint16_t
	{
		P0,
		P1,
		P2,
		P3,
		P4,
		P5,
		P6,
		P7
	};
	/// Use type-safe flags container for the pins
	typedef xpcc::Flags8<Pin> Pins;

public:
	/// Sets one or more pins to output
	xpcc::ResumableResult<bool>
	setOutput(Pins pins);

	/// Sets one or more pins to logic high
	/// @warning only modifies pins that have previously been set to output!
	xpcc::ResumableResult<bool>
	set(Pins pins);

	/// Resets one or more pins to logic low
	/// @warning only modifies pins that have previously been set to output!
	xpcc::ResumableResult<bool>
	reset(Pins pins);

	/// Toggles one or more pins
	/// @warning only modifies pins that have previously been set to output!
	xpcc::ResumableResult<bool>
	toggle(Pins pins);

	/// sets one or more pins to high or low level
	/// @warning only modifies pins that have previously been set to output!
	xpcc::ResumableResult<bool>
	set(Pins pins, bool value);

	/// Returns the set logical output state of the pin.
	bool
	isSet(Pin pin) const;

	/// returns direction of one pin at run-time
	xpcc::Gpio::Direction
	getDirection(Pin pin) const;

public:
	/// Sets one or more pins to input
	xpcc::ResumableResult<bool>
	setInput(Pins pins);

	/// Returns true if **all** pins have a high level
	/// @warning This function operates on the buffered input read.
	///          You have to call `readInput()` to update the result.
	bool
	read(Pins pins) const;

	/// Reads the inputs and buffers them
	xpcc::ResumableResult<bool>
	readInput();

public:
	/// Writes data to the entire port
	/// @warning only modifies pins that have previously been set to output!
	xpcc::ResumableResult<bool>
	writePort(uint16_t data);

	/// Reads the entire port, buffers them and outputs the result to data.
	xpcc::ResumableResult<bool>
	readPort(uint16_t &data);

public:
	/// Returns the direction bits: 0 for Input, 1 for Output
	Pins
	getDirections() const;

	/// Returns the output bits: 0 for low, 1 for high
	Pins
	getOutputs() const;

	/// Returns the input bits: 0 for low, 1 for high
	Pins
	getInputs() const;
#endif
};

/**
 * Create an xpcc GPIO compatible interface from any IO-expander
 * conforming to the xpcc::GpioExpander interface.
 *
 * This allows the transparent usage of IO Expander pins in classes that require
 * Gpios as template arguments.
 * @note For every pin access a blocking bus transfer is performed, therefore
 *       do not expect these gpios to be fast!
 *
 * Usage:
 * @code
 * typedef xpcc::Pca8574<MyI2cMaster> Expander;
 * Expander expander;
 *
 * typedef xpcc::GpioExpanderPin< Expander, expander, Expander::Pin::P0 > Rs;
 * @endcode
 *
 * However, instead of using this class like that, prefer using the alias-template
 * types provided by the IO-expander of your choice for better readability:
 *
 * @code
 * typedef xpcc::Pca8574<MyI2cMaster> Expander;
 * Expander expander;
 *
 * // use predefined pin types of the local IO-expander
 * typedef Expander::P0< expander > Rs;
 * typedef Expander::P1< expander > Rw;
 * typedef Expander::P2< expander > E;
 * typedef Expander::P3< expander > Backlight;
 *
 * Backlight::set();
 * @endcode
 *
 * @warning Access to the IO-expander is **blocking and can silently fail**!
 *          If this is undesirable for you application, you need to write your own wrapper!
 *
 * @see xpcc::GpioExpander
 * @see xpcc::GpioIO
 *
 * @tparam  GpioExpander    Type of class conforming to xpcc::GpioExpander interface
 * @tparam  expander        instance of the expander with the selected pin
 * @tparam  pin             pin identifier on expander
 *
 * @ingroup gpio
 * @author: strongly-typed
 * @author: Niklas Hauser
 */
template <
	typename GpioExpander,
	GpioExpander &expander,
	typename GpioExpander::Pin pin
	>
class GpioExpanderPin : public xpcc::GpioIO
{
public:
	static constexpr Direction direction = Direction::InOut;

public:
	static void
	setOutput()
	{
		RF_CALL_BLOCKING(expander.setOutput(pin));
	}

	static void inline
	setOutput(bool value)
	{
		set(value);
	}

	static void
	set()
	{
		RF_CALL_BLOCKING(expander.set(pin));
	}

	static void
	set(bool value)
	{
		RF_CALL_BLOCKING(expander.set(pin, value));
	}

	static void
	reset()
	{
		RF_CALL_BLOCKING(expander.reset(pin));
	}

	static void
	toggle()
	{
		RF_CALL_BLOCKING(expander.toggle(pin));
	}

	static bool inline
	isSet()
	{
		return expander.isSet(pin);
	}

	static void
	setInput()
	{
		RF_CALL_BLOCKING(expander.setInput(pin));
	}

	static bool
	read()
	{
		RF_CALL_BLOCKING(expander.readInput());

		return expander.read(pin);
	}

	static Direction
	getDirection()
	{
		return expander.getDirection(pin);
	}
};

} // xpcc namespace

#endif // XPCC_IO_EXPANDER_PIN_HPP
<commit_msg>Interface: Gpio Expander: Typos in documentation.<commit_after>// coding: utf-8
/* Copyright (c) 2015, Roboterclub Aachen e. V.
 * All Rights Reserved.
 *
 * The file is part of the xpcc library and is released under the 3-clause BSD
 * license. See the file `LICENSE` for the full license governing this code.
 */
// ----------------------------------------------------------------------------

#ifndef XPCC_GPIO_EXPANDER_HPP
#define XPCC_GPIO_EXPANDER_HPP

#include <xpcc/architecture/interface/gpio.hpp>
#include <xpcc/architecture/interface/register.hpp>
#include <xpcc/processing/resumable.hpp>

namespace xpcc
{

/**
 * Interface of an IO expander driver.
 *
 * All IO expander driver should implement this interface, so that they can be wrapped
 * into `xpcc::GpioExpanderPin` and `xpcc::GpioExpanderPort`.
 *
 * The interface is almost identical to an GPIO pin, except multiple pins can be set at once.
 *
 * @note The driver must buffer the expanders IO registers. This means at least the direction,
 *       output and input values should be buffered so that a read-modify-write can be performed
 *       on SRAM and then only one write is performed on the bus instead of a read and write!
 *
 * @warning The `read()` function does not perform a read bus access, but operates on the
 *          buffered result of `readInput()`. This allows a capture-and-evaluate approach, which
 *          is especially useful for reducing bus traffic and necessary for sampling all pins at
 *          the same time for `xpcc::GpioExpanderPort`.
 *
 * @see xpcc::GpioExpanderPin
 * @see xpcc::GpioExpanderPort
 * @see xpcc::GpioIO
 *
 * @author  Niklas Hauser
 * @ingroup gpio
 */
class GpioExpander
{
#ifdef __DOXYGEN__
public:
	/// This holds the width of the port as number of bits
	/// and can be used in drivers to assert the correct requirements
	static constexpr uint8_t width;

public:
	/// A type containing a unique identifier for each pin.
	/// This is mostly a bit mask, however, since this is implementation
	/// defined it can also be an index.
	/// The underlying type depends on the width of the expander!
	enum class
	Pin : uint16_t;
	/// Use type-safe flags container for the pins
	typedef xpcc::Flags8<Pin> Pins;

public:
	/// Sets one or more pins to output
	xpcc::ResumableResult<bool>
	setOutput(Pins pins);

	/// Sets one or more pins to logic high
	/// @warning only modifies pins that have previously been set to output!
	xpcc::ResumableResult<bool>
	set(Pins pins);

	/// Resets one or more pins to logic low
	/// @warning only modifies pins that have previously been set to output!
	xpcc::ResumableResult<bool>
	reset(Pins pins);

	/// Toggles one or more pins
	/// @warning only modifies pins that have previously been set to output!
	xpcc::ResumableResult<bool>
	toggle(Pins pins);

	/// sets one or more pins to high or low level
	/// @warning only modifies pins that have previously been set to output!
	xpcc::ResumableResult<bool>
	set(Pins pins, bool value);

	/// Returns the set logical output state of the pin.
	bool
	isSet(Pin pin) const;

	/// returns direction of one pin at run-time
	xpcc::Gpio::Direction
	getDirection(Pin pin) const;

public:
	/// Sets one or more pins to input
	xpcc::ResumableResult<bool>
	setInput(Pins pins);

	/// Returns true if **all** pins have a high level
	/// @warning This function operates on the buffered input read.
	///          You have to call `readInput()` to update the result.
	bool
	read(Pins pins) const;

	/// Reads the inputs and buffers them
	xpcc::ResumableResult<bool>
	readInput();

public:
	/// Writes data to the entire port
	/// @warning only modifies pins that have previously been set to output!
	xpcc::ResumableResult<bool>
	writePort(uint16_t data);

	/// Reads the entire port, buffers them and outputs the result to data.
	xpcc::ResumableResult<bool>
	readPort(uint16_t &data);

public:
	/// Returns the direction bits: 0 for Input, 1 for Output
	Pins
	getDirections() const;

	/// Returns the output bits: 0 for low, 1 for high
	Pins
	getOutputs() const;

	/// Returns the input bits: 0 for low, 1 for high
	Pins
	getInputs() const;
#endif
};

/**
 * Create an xpcc GPIO compatible interface from any IO-expander
 * conforming to the xpcc::GpioExpander interface.
 *
 * This allows the transparent usage of IO Expander pins in classes that require
 * Gpios as template arguments.
 * @note For every pin access a blocking bus transfer is performed, therefore
 *       do not expect these gpios to be fast!
 *
 * Usage:
 * @code
 * typedef xpcc::Pca8574<MyI2cMaster> Expander;
 * Expander expander;
 *
 * typedef xpcc::GpioExpanderPin< Expander, expander, Expander::Pin::P0 > Rs;
 * @endcode
 *
 * However, instead of using this class like that, prefer using the alias-template
 * types provided by the IO-expander of your choice for better readability:
 *
 * @code
 * typedef xpcc::Pca8574<MyI2cMaster> Expander;
 * Expander expander;
 *
 * // use predefined pin types of the local IO-expander
 * typedef Expander::P0< expander > Rs;
 * typedef Expander::P1< expander > Rw;
 * typedef Expander::P2< expander > E;
 * typedef Expander::P3< expander > Backlight;
 *
 * Backlight::set();
 * @endcode
 *
 * @warning Access to the IO-expander is **blocking and can silently fail**!
 *          If this is undesirable for you application, you need to write your own wrapper!
 *
 * @see xpcc::GpioExpander
 * @see xpcc::GpioIO
 *
 * @tparam  GpioExpander    Type of class conforming to xpcc::GpioExpander interface
 * @tparam  expander        instance of the expander with the selected pin
 * @tparam  pin             pin identifier on expander
 *
 * @ingroup gpio
 * @author  strongly-typed
 * @author  Niklas Hauser
 */
template <
	typename GpioExpander,
	GpioExpander &expander,
	typename GpioExpander::Pin pin
	>
class GpioExpanderPin : public xpcc::GpioIO
{
public:
	static constexpr Direction direction = Direction::InOut;

public:
	static void
	setOutput()
	{
		RF_CALL_BLOCKING(expander.setOutput(pin));
	}

	static void inline
	setOutput(bool value)
	{
		set(value);
	}

	static void
	set()
	{
		RF_CALL_BLOCKING(expander.set(pin));
	}

	static void
	set(bool value)
	{
		RF_CALL_BLOCKING(expander.set(pin, value));
	}

	static void
	reset()
	{
		RF_CALL_BLOCKING(expander.reset(pin));
	}

	static void
	toggle()
	{
		RF_CALL_BLOCKING(expander.toggle(pin));
	}

	static bool inline
	isSet()
	{
		return expander.isSet(pin);
	}

	static void
	setInput()
	{
		RF_CALL_BLOCKING(expander.setInput(pin));
	}

	static bool
	read()
	{
		RF_CALL_BLOCKING(expander.readInput());

		return expander.read(pin);
	}

	static Direction
	getDirection()
	{
		return expander.getDirection(pin);
	}
};

} // xpcc namespace

#endif // XPCC_IO_EXPANDER_PIN_HPP
<|endoftext|>


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

#include "ios/chrome/browser/prefs/browser_prefs.h"

#include "base/prefs/pref_service.h"
#include "ios/chrome/browser/pref_names.h"

namespace ios {

// This method should be periodically pruned of year+ old migrations.
void MigrateObsoleteIOSProfilePrefs(PrefService* prefs) {
  // Added 08/2015.
  prefs->ClearPref(::prefs::kSigninSharedAuthenticationUserId);
}

}  // namespace ios
<commit_msg>Remove namespace from ios/chrome/browser/prefs/browser_prefs.cc<commit_after>// Copyright 2015 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "ios/chrome/browser/prefs/browser_prefs.h"

#include "base/prefs/pref_service.h"
#include "ios/chrome/browser/pref_names.h"

// This method should be periodically pruned of year+ old migrations.
void MigrateObsoleteIOSProfilePrefs(PrefService* prefs) {
  // Added 08/2015.
  prefs->ClearPref(::prefs::kSigninSharedAuthenticationUserId);
}
<|endoftext|>


<commit_before>#include <Halide.h>
#include <stdio.h>
#include "clock.h"

using namespace Halide;

int main(int argc, char **argv) {
    ImageParam src(UInt(8), 1);
    Func dst;
    Var x;
    dst(x) = src(x);

    Var xo;
    dst.split(x, xo, x, 8*4096);
    // dst.parallel(xo); speeds up halide's memcpy considerably, but doesn't seem sporting
    dst.vectorize(x, 16);

    dst.compile_to_assembly("memcpy.s", Internal::vec<Argument>(src), "memcpy");
    dst.compile_jit();

    Image<uint8_t> input(12345678);
    Image<uint8_t> output(12345678);

    src.set(input);

    double halide = 0, system = 0;
    for (int i = 0; i < 50; i++) {
        double t1 = currentTime();
        dst.realize(output);
        dst.realize(output);
        dst.realize(output);
        double t2 = currentTime();
        memcpy(output.data(), input.data(), input.width());
        memcpy(output.data(), input.data(), input.width());
        memcpy(output.data(), input.data(), input.width());
        double t3 = currentTime();
        system += t3-t2;
        halide += t2-t1;
    }

    printf("system memcpy: %f\n", system);
    printf("halide memcpy: %f\n", halide);

    // memcpy will win by a little bit for large inputs because it uses streaming stores
    if (halide > system * 1.2) {
        printf("Halide memcpy is slower than it should be.\n");
        return -1;
    }

    printf("Success!\n");
    return 0;
}
<commit_msg>Fixed memcpy test for ptx.<commit_after>#include <Halide.h>
#include <stdio.h>
#include "clock.h"

using namespace Halide;

int main(int argc, char **argv) {
    ImageParam src(UInt(8), 1);
    Func dst;
    Var x;
    dst(x) = src(x);

    Var xo;
    dst.split(x, xo, x, 8*4096);
    // dst.parallel(xo); speeds up halide's memcpy considerably, but doesn't seem sporting
    dst.vectorize(x, 16);

    dst.compile_to_assembly("memcpy.s", Internal::vec<Argument>(src), "memcpy");
    dst.compile_jit();

    Image<uint8_t> input(12345678);
    Image<uint8_t> output(12345678);

    src.set(input);

    // Get past one-time set-up issues for the ptx backend.
    dst.realize(output);

    double halide = 0, system = 0;
    for (int i = 0; i < 50; i++) {
        double t1 = currentTime();
        dst.realize(output);
        dst.realize(output);
        dst.realize(output);
        double t2 = currentTime();
        memcpy(output.data(), input.data(), input.width());
        memcpy(output.data(), input.data(), input.width());
        memcpy(output.data(), input.data(), input.width());
        double t3 = currentTime();
        system += t3-t2;
        halide += t2-t1;
    }

    printf("system memcpy: %f\n", system);
    printf("halide memcpy: %f\n", halide);

    // memcpy will win by a little bit for large inputs because it uses streaming stores
    if (halide > system * 1.2) {
        printf("Halide memcpy is slower than it should be.\n");
        return -1;
    }

    printf("Success!\n");
    return 0;
}
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2012-2018 Daniele Bartolini and individual contributors.
 * License: https://github.com/dbartolini/crown/blob/master/LICENSE
 */

#include "core/containers/array.h"
#include "core/containers/hash_map.h"
#include "core/containers/types.h"
#include "resource/expression_language.h"
#include "resource/resource_manager.h"
#include "resource/sprite_resource.h"
#include "resource/state_machine_resource.h"
#include "world/animation_state_machine.h"
#include "world/types.h"
#include "world/unit_manager.h"

namespace crown
{
static void unit_destroyed_callback_bridge(UnitId id, void* user_ptr)
{
	((AnimationStateMachine*)user_ptr)->unit_destroyed_callback(id);
}

AnimationStateMachine::AnimationStateMachine(Allocator& a, ResourceManager& rm, UnitManager& um)
	: _marker(ANIMATION_STATE_MACHINE_MARKER)
	, _resource_manager(&rm)
	, _unit_manager(&um)
	, _map(a)
	, _animations(a)
	, _events(a)
{
	_unit_destroy_callback.destroy = unit_destroyed_callback_bridge;
	_unit_destroy_callback.user_data = this;
	_unit_destroy_callback.node.next = NULL;
	_unit_destroy_callback.node.prev = NULL;
	um.register_destroy_callback(&_unit_destroy_callback);
}

AnimationStateMachine::~AnimationStateMachine()
{
	_unit_manager->unregister_destroy_callback(&_unit_destroy_callback);
	_marker = 0;
}

u32 AnimationStateMachine::create(UnitId unit, const AnimationStateMachineDesc& desc)
{
	CE_ASSERT(!hash_map::has(_map, unit), "Unit already has this component");

	const StateMachineResource* smr = (StateMachineResource*)_resource_manager->get(RESOURCE_TYPE_STATE_MACHINE, desc.state_machine_resource);

	Animation anim;
	anim.unit          = unit;
	anim.time          = 0.0f;
	anim.time_total    = 0.0f;
	anim.num_frames    = 0;
	anim.frames        = NULL;
	anim.resource      = NULL;
	anim.state         = state_machine::initial_state(smr);
	anim.state_next    = NULL;
	anim.state_machine = smr;
	anim.variables     = (f32*)default_allocator().allocate(sizeof(*anim.variables)*smr->num_variables);

	memcpy(anim.variables, state_machine::variables(smr), sizeof(*anim.variables)*smr->num_variables);

	u32 last = array::size(_animations);
	array::push_back(_animations, anim);
	hash_map::set(_map, unit, last);
	return 0;
}

void AnimationStateMachine::destroy(UnitId unit)
{
	const u32 i = hash_map::get(_map, unit, UINT32_MAX);
	const u32 last_i = array::size(_animations) - 1;
	const UnitId last_u = _animations[last_i].unit;

	default_allocator().deallocate(_animations[i].variables);
	_animations[i] = _animations[last_i];

	array::pop_back(_animations);
	hash_map::set(_map, last_u, i);
	hash_map::remove(_map, unit);
}

u32 AnimationStateMachine::instances(UnitId unit)
{
	return hash_map::get(_map, unit, UINT32_MAX);
}

bool AnimationStateMachine::has(UnitId unit)
{
	return hash_map::has(_map, unit);
}

u32 AnimationStateMachine::variable_id(UnitId unit, StringId32 name)
{
	const u32 i = hash_map::get(_map, unit, UINT32_MAX);
	const u32 index = state_machine::variable_index(_animations[i].state_machine, name);
	return index;
}

f32 AnimationStateMachine::variable(UnitId unit, u32 variable_id)
{
	const u32 i = hash_map::get(_map, unit, UINT32_MAX);
	CE_ENSURE(variable_id != UINT32_MAX);
	return _animations[i].variables[variable_id];
}

void AnimationStateMachine::set_variable(UnitId unit, u32 variable_id, f32 value)
{
	const u32 i = hash_map::get(_map, unit, UINT32_MAX);
	CE_ENSURE(variable_id != UINT32_MAX);
	_animations[i].variables[variable_id] = value;
}

void AnimationStateMachine::trigger(UnitId unit, StringId32 event)
{
	const u32 i = hash_map::get(_map, unit, UINT32_MAX);

	const Transition* transition;
	const State* s = state_machine::trigger(_animations[i].state_machine
		, _animations[i].state
		, event
		, &transition
		);

	if (!transition)
		return;

	if (transition->mode == TransitionMode::IMMEDIATE)
		_animations[i].state = s;
	else if (transition->mode == TransitionMode::WAIT_UNTIL_END)
		_animations[i].state_next = s;
	else
		CE_FATAL("Unknown transition mode");
}

void AnimationStateMachine::update(float dt)
{
	f32 stack_data[32];
	skinny::expression_language::Stack stack(stack_data, countof(stack_data));

	for (u32 i = 0; i < array::size(_animations); ++i)
	{
		Animation& anim_i = _animations[i];

		const f32* variables = anim_i.variables;
		const u32* byte_code = state_machine::byte_code(anim_i.state_machine);

		// Evaluate animation weights
		f32 max_v = 0.0f;
		u32 max_i = UINT32_MAX;
		StringId64 name;

		const AnimationArray* aa = state_machine::state_animations(anim_i.state);
		for (u32 i = 0; i < aa->num; ++i)
		{
			const crown::Animation* animation = state_machine::animation(aa, i);

			stack.size = 0;
			skinny::expression_language::run(&byte_code[animation->bytecode_entry], variables, stack);
			const f32 cur = stack.size > 0 ? stack_data[stack.size-1] : 0.0f;
			if (cur > max_v || max_i == UINT32_MAX)
			{
				max_v = cur;
				max_i = i;
				name = animation->name;
			}
		}

		// Evaluate animation speed
		stack.size = 0;
		skinny::expression_language::run(&byte_code[anim_i.state->speed_bytecode], variables, stack);
		const f32 speed = stack.size > 0 ? stack_data[stack.size-1] : 1.0f;

		// Advance animation
		const SpriteAnimationResource* sar = (SpriteAnimationResource*)_resource_manager->get(RESOURCE_TYPE_SPRITE_ANIMATION, name);
		if (anim_i.resource != sar)
		{
			anim_i.time       = 0.0f;
			anim_i.time_total = sar->total_time;
			anim_i.num_frames = sar->num_frames;
			anim_i.frames     = sprite_animation_resource::frames(sar);
			anim_i.resource   = sar;
		}

		if (!anim_i.resource)
			continue;

		const f32 frame_ratio     = anim_i.time / anim_i.time_total;
		const u32 frame_unclamped = frame_ratio * f32(anim_i.num_frames);
		const u32 frame_index     = min(frame_unclamped, anim_i.num_frames-1);

		anim_i.time += dt*speed;

		// If animation finished playing
		if (anim_i.time > anim_i.time_total)
		{
			if (anim_i.state_next)
			{
				anim_i.state = anim_i.state_next;
				anim_i.state_next = NULL;
				anim_i.time = 0.0f;
			}
			else
			{
				if (!!anim_i.state->loop)
				{
					anim_i.time = anim_i.time - anim_i.time_total;
				}
				else
				{
					const Transition* dummy;
					const State* s = state_machine::trigger(anim_i.state_machine
						, anim_i.state
						, StringId32("animation_end")
						, &dummy
						);
					anim_i.time = anim_i.state != s ? 0.0f : anim_i.time_total;
					anim_i.state = s;
				}
			}
		}

		// Emit events
		SpriteFrameChangeEvent ev;
		ev.unit      = anim_i.unit;
		ev.frame_num = anim_i.frames[frame_index];
		event_stream::write(_events, 0, ev);
	}
}

void AnimationStateMachine::unit_destroyed_callback(UnitId unit)
{
	if (has(unit))
		destroy(unit);
}

} // namespace crown
<commit_msg>world: fix warning<commit_after>/*
 * Copyright (c) 2012-2018 Daniele Bartolini and individual contributors.
 * License: https://github.com/dbartolini/crown/blob/master/LICENSE
 */

#include "core/containers/array.h"
#include "core/containers/hash_map.h"
#include "core/containers/types.h"
#include "resource/expression_language.h"
#include "resource/resource_manager.h"
#include "resource/sprite_resource.h"
#include "resource/state_machine_resource.h"
#include "world/animation_state_machine.h"
#include "world/types.h"
#include "world/unit_manager.h"

namespace crown
{
static void unit_destroyed_callback_bridge(UnitId id, void* user_ptr)
{
	((AnimationStateMachine*)user_ptr)->unit_destroyed_callback(id);
}

AnimationStateMachine::AnimationStateMachine(Allocator& a, ResourceManager& rm, UnitManager& um)
	: _marker(ANIMATION_STATE_MACHINE_MARKER)
	, _resource_manager(&rm)
	, _unit_manager(&um)
	, _map(a)
	, _animations(a)
	, _events(a)
{
	_unit_destroy_callback.destroy = unit_destroyed_callback_bridge;
	_unit_destroy_callback.user_data = this;
	_unit_destroy_callback.node.next = NULL;
	_unit_destroy_callback.node.prev = NULL;
	um.register_destroy_callback(&_unit_destroy_callback);
}

AnimationStateMachine::~AnimationStateMachine()
{
	_unit_manager->unregister_destroy_callback(&_unit_destroy_callback);
	_marker = 0;
}

u32 AnimationStateMachine::create(UnitId unit, const AnimationStateMachineDesc& desc)
{
	CE_ASSERT(!hash_map::has(_map, unit), "Unit already has this component");

	const StateMachineResource* smr = (StateMachineResource*)_resource_manager->get(RESOURCE_TYPE_STATE_MACHINE, desc.state_machine_resource);

	Animation anim;
	anim.unit          = unit;
	anim.time          = 0.0f;
	anim.time_total    = 0.0f;
	anim.num_frames    = 0;
	anim.frames        = NULL;
	anim.resource      = NULL;
	anim.state         = state_machine::initial_state(smr);
	anim.state_next    = NULL;
	anim.state_machine = smr;
	anim.variables     = (f32*)default_allocator().allocate(sizeof(*anim.variables)*smr->num_variables);

	memcpy(anim.variables, state_machine::variables(smr), sizeof(*anim.variables)*smr->num_variables);

	u32 last = array::size(_animations);
	array::push_back(_animations, anim);
	hash_map::set(_map, unit, last);
	return 0;
}

void AnimationStateMachine::destroy(UnitId unit)
{
	const u32 i = hash_map::get(_map, unit, UINT32_MAX);
	const u32 last_i = array::size(_animations) - 1;
	const UnitId last_u = _animations[last_i].unit;

	default_allocator().deallocate(_animations[i].variables);
	_animations[i] = _animations[last_i];

	array::pop_back(_animations);
	hash_map::set(_map, last_u, i);
	hash_map::remove(_map, unit);
}

u32 AnimationStateMachine::instances(UnitId unit)
{
	return hash_map::get(_map, unit, UINT32_MAX);
}

bool AnimationStateMachine::has(UnitId unit)
{
	return hash_map::has(_map, unit);
}

u32 AnimationStateMachine::variable_id(UnitId unit, StringId32 name)
{
	const u32 i = hash_map::get(_map, unit, UINT32_MAX);
	const u32 index = state_machine::variable_index(_animations[i].state_machine, name);
	return index;
}

f32 AnimationStateMachine::variable(UnitId unit, u32 variable_id)
{
	const u32 i = hash_map::get(_map, unit, UINT32_MAX);
	CE_ENSURE(variable_id != UINT32_MAX);
	return _animations[i].variables[variable_id];
}

void AnimationStateMachine::set_variable(UnitId unit, u32 variable_id, f32 value)
{
	const u32 i = hash_map::get(_map, unit, UINT32_MAX);
	CE_ENSURE(variable_id != UINT32_MAX);
	_animations[i].variables[variable_id] = value;
}

void AnimationStateMachine::trigger(UnitId unit, StringId32 event)
{
	const u32 i = hash_map::get(_map, unit, UINT32_MAX);

	const Transition* transition;
	const State* s = state_machine::trigger(_animations[i].state_machine
		, _animations[i].state
		, event
		, &transition
		);

	if (!transition)
		return;

	if (transition->mode == TransitionMode::IMMEDIATE)
		_animations[i].state = s;
	else if (transition->mode == TransitionMode::WAIT_UNTIL_END)
		_animations[i].state_next = s;
	else
		CE_FATAL("Unknown transition mode");
}

void AnimationStateMachine::update(float dt)
{
	f32 stack_data[32];
	skinny::expression_language::Stack stack(stack_data, countof(stack_data));

	for (u32 i = 0; i < array::size(_animations); ++i)
	{
		Animation& anim_i = _animations[i];

		const f32* variables = anim_i.variables;
		const u32* byte_code = state_machine::byte_code(anim_i.state_machine);

		// Evaluate animation weights
		f32 max_v = 0.0f;
		u32 max_i = UINT32_MAX;
		StringId64 name;

		const AnimationArray* aa = state_machine::state_animations(anim_i.state);
		for (u32 i = 0; i < aa->num; ++i)
		{
			const crown::Animation* animation = state_machine::animation(aa, i);

			stack.size = 0;
			skinny::expression_language::run(&byte_code[animation->bytecode_entry], variables, stack);
			const f32 cur = stack.size > 0 ? stack_data[stack.size-1] : 0.0f;
			if (cur > max_v || max_i == UINT32_MAX)
			{
				max_v = cur;
				max_i = i;
				name = animation->name;
			}
		}

		// Evaluate animation speed
		stack.size = 0;
		skinny::expression_language::run(&byte_code[anim_i.state->speed_bytecode], variables, stack);
		const f32 speed = stack.size > 0 ? stack_data[stack.size-1] : 1.0f;

		// Advance animation
		const SpriteAnimationResource* sar = (SpriteAnimationResource*)_resource_manager->get(RESOURCE_TYPE_SPRITE_ANIMATION, name);
		if (anim_i.resource != sar)
		{
			anim_i.time       = 0.0f;
			anim_i.time_total = sar->total_time;
			anim_i.num_frames = sar->num_frames;
			anim_i.frames     = sprite_animation_resource::frames(sar);
			anim_i.resource   = sar;
		}

		if (!anim_i.resource)
			continue;

		const f32 frame_ratio     = anim_i.time / anim_i.time_total;
		const u32 frame_unclamped = u32(frame_ratio * f32(anim_i.num_frames));
		const u32 frame_index     = min(frame_unclamped, anim_i.num_frames-1);

		anim_i.time += dt*speed;

		// If animation finished playing
		if (anim_i.time > anim_i.time_total)
		{
			if (anim_i.state_next)
			{
				anim_i.state = anim_i.state_next;
				anim_i.state_next = NULL;
				anim_i.time = 0.0f;
			}
			else
			{
				if (!!anim_i.state->loop)
				{
					anim_i.time = anim_i.time - anim_i.time_total;
				}
				else
				{
					const Transition* dummy;
					const State* s = state_machine::trigger(anim_i.state_machine
						, anim_i.state
						, StringId32("animation_end")
						, &dummy
						);
					anim_i.time = anim_i.state != s ? 0.0f : anim_i.time_total;
					anim_i.state = s;
				}
			}
		}

		// Emit events
		SpriteFrameChangeEvent ev;
		ev.unit      = anim_i.unit;
		ev.frame_num = anim_i.frames[frame_index];
		event_stream::write(_events, 0, ev);
	}
}

void AnimationStateMachine::unit_destroyed_callback(UnitId unit)
{
	if (has(unit))
		destroy(unit);
}

} // namespace crown
<|endoftext|>


<commit_before>#include "writer/verilog/verilog_writer.h"

#include "design/design_util.h"
#include "iroha/i_design.h"
#include "iroha/logging.h"
#include "iroha/resource_params.h"
#include "iroha/stl_util.h"
#include "writer/names.h"
#include "writer/verilog/embedded_modules.h"
#include "writer/verilog/indent.h"
#include "writer/verilog/internal_sram.h"
#include "writer/verilog/module.h"
#include "writer/verilog/port.h"
#include "writer/verilog/port_set.h"
#include "writer/verilog/self_shell.h"

namespace iroha {
namespace writer {
namespace verilog {

VerilogWriter::VerilogWriter(const IDesign *design, const Connection &conn,
                             const string &flavor, bool debug, ostream &os)
    : design_(design),
      conn_(conn),
      flavor_(flavor),
      debug_(debug),
      final_os_(os),
      embedded_modules_(new EmbeddedModules),
      with_self_contained_(false),
      output_vcd_(false),
      names_root_(new Names(nullptr)),
      reset_polarity_(false) {}

VerilogWriter::~VerilogWriter() {}

bool VerilogWriter::Write() {
  names_root_->ReservePrefix("insn");
  names_root_->ReservePrefix("st");
  names_root_->ReservePrefix("task");
  names_root_->ReservePrefix("S");
  // Resource related.
  names_root_->ReservePrefix("fifo");
  names_root_->ReservePrefix("mem");
  // e.g. shared_reg
  names_root_->ReservePrefix("shared");
  // SystemVerilog keyword.
  names_root_->ReservePrefix("always");

  DebugMarker::SetEnable(debug_);

  os_ << "// Generated from " << PACKAGE << "-" << VERSION << ".\n";

  IModule *root = DesignUtil::GetRootModule(design_);
  if (root == nullptr) {
    LOG(ERROR) << "Failed to determine a root module";
    return false;
  }
  PrepareModulesRec(root);
  ResolveResetPolarity(root);
  BuildModules(root);
  // * MODULE-NAME-PREFIX is specified.
  //  prefix_rawName
  // * Not specifed.
  //  shellModuleName_rawName
  string prefix = design_->GetParams()->GetModuleNamePrefix();
  if (prefix.empty()) {
    prefix = shell_module_name_ + "_";
  }
  for (auto *mod : ordered_modules_) {
    mod->Write(prefix, os_);
  }
  if (!shell_module_name_.empty()) {
    Module *root_mod = modules_[root];
    WriteShellModule(root_mod);
  }
  if (!embedded_modules_->Write(GetResetPolarity(), os_)) {
    LOG(ERROR) << "Failed to write embedded modules.";
    return false;
  }
  STLDeleteSecondElements(&modules_);
  Indent indent(os_.str());
  final_os_ << indent.DoIndent();
  return true;
}

void VerilogWriter::SetShellModuleName(const string &n,
                                       bool with_self_contained,
                                       bool output_vcd) {
  shell_module_name_ = n;
  with_self_contained_ = with_self_contained;
  output_vcd_ = output_vcd;
}

Module *VerilogWriter::GetByIModule(const IModule *mod) const {
  auto it = modules_.find(mod);
  if (it != modules_.end()) {
    return it->second;
  }
  return nullptr;
}

bool VerilogWriter::GetResetPolarity() const { return reset_polarity_; }

void VerilogWriter::ResolveResetPolarity(const IModule *root) {
  if (root->GetParams()->HasResetPolarity()) {
    reset_polarity_ = root->GetParams()->GetResetPolarity();
    return;
  }
  // This may return the default value.
  reset_polarity_ = design_->GetParams()->GetResetPolarity();
}

void VerilogWriter::PrepareModulesRec(const IModule *imod) {
  vector<IModule *> children = DesignUtil::GetChildModules(imod);
  for (IModule *child : children) {
    PrepareModulesRec(child);
  }
  Module *mod = new Module(imod, this, conn_, embedded_modules_.get(),
                           names_root_->NewChildNames());
  modules_[imod] = mod;
  ordered_modules_.push_back(mod);
}

void VerilogWriter::BuildModules(const IModule *imod) {
  for (auto *mod : ordered_modules_) {
    const IModule *imod = mod->GetIModule();
    IModule *iparent = imod->GetParentModule();
    if (iparent != nullptr) {
      Module *parent = modules_[iparent];
      mod->SetParentModule(parent);
    }
  }
  for (auto *mod : ordered_modules_) {
    mod->PrepareTables();
  }
  for (auto *mod : ordered_modules_) {
    mod->Build();
  }
  BuildChildModuleSection();
}

void VerilogWriter::BuildChildModuleSection() {
  for (auto *mod : ordered_modules_) {
    vector<IModule *> child_imods =
        DesignUtil::GetChildModules(mod->GetIModule());
    vector<Module *> mods;
    for (auto *imod : child_imods) {
      mods.push_back(modules_[imod]);
    }
    mod->BuildChildModuleInstSection(mods);
  }
}

void VerilogWriter::WriteShellModule(const Module *mod) {
  const PortSet *ports = mod->GetPortSet();
  os_ << "`ifndef STRIP_SHELL\n";
  os_ << "module " << shell_module_name_ << "(";
  if (!with_self_contained_) {
    ports->Output(PortSet::PORT_MODULE_HEAD_DIRECTION, os_);
  }
  os_ << ");\n";
  if (with_self_contained_) {
    WriteSelfClockGenerator(mod);
  }
  std::unique_ptr<SelfShell> shell;
  if (with_self_contained_) {
    shell.reset(new SelfShell(design_, ports, reset_polarity_,
                              embedded_modules_.get()));
    shell->WriteWireDecl(os_);
  }
  string name = mod->GetName();
  if (design_->GetParams()->GetModuleNamePrefix().empty()) {
    name = shell_module_name_ + "_" + name;
  } else {
    name = design_->GetParams()->GetModuleNamePrefix() + name;
  }
  os_ << "  " << name << " " << name << "_inst(";
  if (with_self_contained_) {
    WriteSelfClockConnection(mod);
    shell->WritePortConnection(os_);
  } else {
    ports->Output(PortSet::PORT_CONNECTION, os_);
  }
  os_ << ");\n";
  if (with_self_contained_) {
    shell->WriteShellFSM(os_);
  }
  if (output_vcd_) {
    os_ << "\n"
        << "initial begin\n"
        << "    $dumpfile(\"/tmp/" << shell_module_name_ << ".vcd\");\n"
        << "    $dumpvars(0, " << name << "_inst);\n"
        << "  end\n";
  }
  os_ << "\nendmodule  // " << shell_module_name_ << "\n";
  os_ << "`endif  // STRIP_SHELL\n";

  os_ << "\n// NOTE: Please copy the follwoing line to your design.\n"
      << "// " << shell_module_name_ << " " << shell_module_name_ << "_inst(";
  ports->OutputWithFlavor(PortSet::PORT_CONNECTION_TEMPLATE, flavor_, os_);
  os_ << ");\n";
  os_ << "//";
  ports->OutputWithFlavor(PortSet::AXI_USER_ASSIGN_TEMPLATE, flavor_, os_);
  os_ << "\n";
  os_ << "// NOTE: This can be used by your script to auto generate the "
         "instantiation and connections.\n"
      << "// :connection: " << shell_module_name_ << ":" << shell_module_name_
      << "_inst ";
  ports->Output(PortSet::PORT_CONNECTION_DATA, os_);
  os_ << "\n";
}

void VerilogWriter::WriteSelfClockGenerator(const Module *mod) {
  const PortSet *ports = mod->GetPortSet();
  const string &clk = ports->GetClk();
  const string &rst = ports->GetReset();
  os_ << "  reg " << clk << ", " << rst << ";\n\n"
      << "  initial begin\n"
      << "    " << clk << " <= 0;\n"
      << "    " << rst << " <= " << (reset_polarity_ ? "1" : "0") << ";\n"
      << "    #15\n"
      << "    " << rst << " <= ~" << rst << ";\n"
      << "    #10000\n"
      << "    $finish;\n"
      << "  end\n\n";
  os_ << "  always begin\n"
      << "    #10 " << clk << " = ~" << clk << ";\n"
      << "  end\n\n";
}

void VerilogWriter::WriteSelfClockConnection(const Module *mod) {
  const PortSet *ports = mod->GetPortSet();
  const string &clk = ports->GetClk();
  const string &rst = ports->GetReset();
  os_ << "." << clk << "(" << clk << "), ." << rst << "(" << rst << ")";
}

}  // namespace verilog
}  // namespace writer
}  // namespace iroha
<commit_msg>Use a copy of stringstream.str().<commit_after>#include "writer/verilog/verilog_writer.h"

#include "design/design_util.h"
#include "iroha/i_design.h"
#include "iroha/logging.h"
#include "iroha/resource_params.h"
#include "iroha/stl_util.h"
#include "writer/names.h"
#include "writer/verilog/embedded_modules.h"
#include "writer/verilog/indent.h"
#include "writer/verilog/internal_sram.h"
#include "writer/verilog/module.h"
#include "writer/verilog/port.h"
#include "writer/verilog/port_set.h"
#include "writer/verilog/self_shell.h"

namespace iroha {
namespace writer {
namespace verilog {

VerilogWriter::VerilogWriter(const IDesign *design, const Connection &conn,
                             const string &flavor, bool debug, ostream &os)
    : design_(design),
      conn_(conn),
      flavor_(flavor),
      debug_(debug),
      final_os_(os),
      embedded_modules_(new EmbeddedModules),
      with_self_contained_(false),
      output_vcd_(false),
      names_root_(new Names(nullptr)),
      reset_polarity_(false) {}

VerilogWriter::~VerilogWriter() {}

bool VerilogWriter::Write() {
  names_root_->ReservePrefix("insn");
  names_root_->ReservePrefix("st");
  names_root_->ReservePrefix("task");
  names_root_->ReservePrefix("S");
  // Resource related.
  names_root_->ReservePrefix("fifo");
  names_root_->ReservePrefix("mem");
  // e.g. shared_reg
  names_root_->ReservePrefix("shared");
  // SystemVerilog keyword.
  names_root_->ReservePrefix("always");

  DebugMarker::SetEnable(debug_);

  os_ << "// Generated from " << PACKAGE << "-" << VERSION << ".\n";

  IModule *root = DesignUtil::GetRootModule(design_);
  if (root == nullptr) {
    LOG(ERROR) << "Failed to determine a root module";
    return false;
  }
  PrepareModulesRec(root);
  ResolveResetPolarity(root);
  BuildModules(root);
  // * MODULE-NAME-PREFIX is specified.
  //  prefix_rawName
  // * Not specifed.
  //  shellModuleName_rawName
  string prefix = design_->GetParams()->GetModuleNamePrefix();
  if (prefix.empty()) {
    prefix = shell_module_name_ + "_";
  }
  for (auto *mod : ordered_modules_) {
    mod->Write(prefix, os_);
  }
  if (!shell_module_name_.empty()) {
    Module *root_mod = modules_[root];
    WriteShellModule(root_mod);
  }
  if (!embedded_modules_->Write(GetResetPolarity(), os_)) {
    LOG(ERROR) << "Failed to write embedded modules.";
    return false;
  }
  STLDeleteSecondElements(&modules_);
  string s = os_.str();
  Indent indent(s);
  final_os_ << indent.DoIndent();
  return true;
}

void VerilogWriter::SetShellModuleName(const string &n,
                                       bool with_self_contained,
                                       bool output_vcd) {
  shell_module_name_ = n;
  with_self_contained_ = with_self_contained;
  output_vcd_ = output_vcd;
}

Module *VerilogWriter::GetByIModule(const IModule *mod) const {
  auto it = modules_.find(mod);
  if (it != modules_.end()) {
    return it->second;
  }
  return nullptr;
}

bool VerilogWriter::GetResetPolarity() const { return reset_polarity_; }

void VerilogWriter::ResolveResetPolarity(const IModule *root) {
  if (root->GetParams()->HasResetPolarity()) {
    reset_polarity_ = root->GetParams()->GetResetPolarity();
    return;
  }
  // This may return the default value.
  reset_polarity_ = design_->GetParams()->GetResetPolarity();
}

void VerilogWriter::PrepareModulesRec(const IModule *imod) {
  vector<IModule *> children = DesignUtil::GetChildModules(imod);
  for (IModule *child : children) {
    PrepareModulesRec(child);
  }
  Module *mod = new Module(imod, this, conn_, embedded_modules_.get(),
                           names_root_->NewChildNames());
  modules_[imod] = mod;
  ordered_modules_.push_back(mod);
}

void VerilogWriter::BuildModules(const IModule *imod) {
  for (auto *mod : ordered_modules_) {
    const IModule *imod = mod->GetIModule();
    IModule *iparent = imod->GetParentModule();
    if (iparent != nullptr) {
      Module *parent = modules_[iparent];
      mod->SetParentModule(parent);
    }
  }
  for (auto *mod : ordered_modules_) {
    mod->PrepareTables();
  }
  for (auto *mod : ordered_modules_) {
    mod->Build();
  }
  BuildChildModuleSection();
}

void VerilogWriter::BuildChildModuleSection() {
  for (auto *mod : ordered_modules_) {
    vector<IModule *> child_imods =
        DesignUtil::GetChildModules(mod->GetIModule());
    vector<Module *> mods;
    for (auto *imod : child_imods) {
      mods.push_back(modules_[imod]);
    }
    mod->BuildChildModuleInstSection(mods);
  }
}

void VerilogWriter::WriteShellModule(const Module *mod) {
  const PortSet *ports = mod->GetPortSet();
  os_ << "`ifndef STRIP_SHELL\n";
  os_ << "module " << shell_module_name_ << "(";
  if (!with_self_contained_) {
    ports->Output(PortSet::PORT_MODULE_HEAD_DIRECTION, os_);
  }
  os_ << ");\n";
  if (with_self_contained_) {
    WriteSelfClockGenerator(mod);
  }
  std::unique_ptr<SelfShell> shell;
  if (with_self_contained_) {
    shell.reset(new SelfShell(design_, ports, reset_polarity_,
                              embedded_modules_.get()));
    shell->WriteWireDecl(os_);
  }
  string name = mod->GetName();
  if (design_->GetParams()->GetModuleNamePrefix().empty()) {
    name = shell_module_name_ + "_" + name;
  } else {
    name = design_->GetParams()->GetModuleNamePrefix() + name;
  }
  os_ << "  " << name << " " << name << "_inst(";
  if (with_self_contained_) {
    WriteSelfClockConnection(mod);
    shell->WritePortConnection(os_);
  } else {
    ports->Output(PortSet::PORT_CONNECTION, os_);
  }
  os_ << ");\n";
  if (with_self_contained_) {
    shell->WriteShellFSM(os_);
  }
  if (output_vcd_) {
    os_ << "\n"
        << "initial begin\n"
        << "    $dumpfile(\"/tmp/" << shell_module_name_ << ".vcd\");\n"
        << "    $dumpvars(0, " << name << "_inst);\n"
        << "  end\n";
  }
  os_ << "\nendmodule  // " << shell_module_name_ << "\n";
  os_ << "`endif  // STRIP_SHELL\n";

  os_ << "\n// NOTE: Please copy the follwoing line to your design.\n"
      << "// " << shell_module_name_ << " " << shell_module_name_ << "_inst(";
  ports->OutputWithFlavor(PortSet::PORT_CONNECTION_TEMPLATE, flavor_, os_);
  os_ << ");\n";
  os_ << "//";
  ports->OutputWithFlavor(PortSet::AXI_USER_ASSIGN_TEMPLATE, flavor_, os_);
  os_ << "\n";
  os_ << "// NOTE: This can be used by your script to auto generate the "
         "instantiation and connections.\n"
      << "// :connection: " << shell_module_name_ << ":" << shell_module_name_
      << "_inst ";
  ports->Output(PortSet::PORT_CONNECTION_DATA, os_);
  os_ << "\n";
}

void VerilogWriter::WriteSelfClockGenerator(const Module *mod) {
  const PortSet *ports = mod->GetPortSet();
  const string &clk = ports->GetClk();
  const string &rst = ports->GetReset();
  os_ << "  reg " << clk << ", " << rst << ";\n\n"
      << "  initial begin\n"
      << "    " << clk << " <= 0;\n"
      << "    " << rst << " <= " << (reset_polarity_ ? "1" : "0") << ";\n"
      << "    #15\n"
      << "    " << rst << " <= ~" << rst << ";\n"
      << "    #10000\n"
      << "    $finish;\n"
      << "  end\n\n";
  os_ << "  always begin\n"
      << "    #10 " << clk << " = ~" << clk << ";\n"
      << "  end\n\n";
}

void VerilogWriter::WriteSelfClockConnection(const Module *mod) {
  const PortSet *ports = mod->GetPortSet();
  const string &clk = ports->GetClk();
  const string &rst = ports->GetReset();
  os_ << "." << clk << "(" << clk << "), ." << rst << "(" << rst << ")";
}

}  // namespace verilog
}  // namespace writer
}  // namespace iroha
<|endoftext|>


<commit_before>#ifndef STAN_MATH_PRIM_FUN_QUAD_FORM_DIAG_HPP
#define STAN_MATH_PRIM_FUN_QUAD_FORM_DIAG_HPP

#include <stan/math/prim/err.hpp>
#include <stan/math/prim/fun/Eigen.hpp>

namespace stan {
namespace math {

template <typename T1, typename T2, int R, int C>
inline Eigen::Matrix<return_type_t<T1, T2>, Eigen::Dynamic, Eigen::Dynamic>
quad_form_diag(const Eigen::Matrix<T1, Eigen::Dynamic, Eigen::Dynamic>& mat,
               const Eigen::Matrix<T2, R, C>& vec) {
  check_vector("quad_form_diag", "vec", vec);
  check_square("quad_form_diag", "mat", mat);
  check_multiplicable("quad_form_diag", "mat", mat, "vec", vec);
  return vec.asDiagonal() * mat * vec.asDiagonal();
}

}  // namespace math
}  // namespace stan

#endif
<commit_msg>Restore check_size_match<commit_after>#ifndef STAN_MATH_PRIM_FUN_QUAD_FORM_DIAG_HPP
#define STAN_MATH_PRIM_FUN_QUAD_FORM_DIAG_HPP

#include <stan/math/prim/err.hpp>
#include <stan/math/prim/fun/Eigen.hpp>

namespace stan {
namespace math {

template <typename T1, typename T2, int R, int C>
inline Eigen::Matrix<return_type_t<T1, T2>, Eigen::Dynamic, Eigen::Dynamic>
quad_form_diag(const Eigen::Matrix<T1, Eigen::Dynamic, Eigen::Dynamic>& mat,
               const Eigen::Matrix<T2, R, C>& vec) {
  check_vector("quad_form_diag", "vec", vec);
  check_square("quad_form_diag", "mat", mat);
  check_size_match("quad_form_diag", "rows of mat", mat.rows(), "size of vec",
                   vec.size());
  return vec.asDiagonal() * mat * vec.asDiagonal();
}

}  // namespace math
}  // namespace stan

#endif
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: XMLClipPropertyHandler.cxx,v $
 *
 *  $Revision: 1.3 $
 *
 *  last change: $Author: rt $ $Date: 2005-09-09 14:32:17 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

#ifndef _XMLOFF_XMLCLIPPROPERTYHANDLER_HXX
#include "XMLClipPropertyHandler.hxx"
#endif

#ifndef _COM_SUN_STAR_UNO_ANY_HXX_
#include <com/sun/star/uno/Any.hxx>
#endif

#ifndef _RTL_USTRBUF_HXX_
#include <rtl/ustrbuf.hxx>
#endif

#ifndef _COM_SUN_STAR_TEXT_GRAPHICCROP_HPP_
#include <com/sun/star/text/GraphicCrop.hpp>
#endif

#ifndef _XMLOFF_XMLUCONV_HXX
#include "xmluconv.hxx"
#endif

#ifndef _XMLOFF_XMLKYWD_HXX
#include <xmlkywd.hxx>
#endif
#ifndef _XMLOFF_XMLTOKEN_HXX
#include <xmltoken.hxx>
#endif

using namespace ::rtl;
using namespace ::com::sun::star;
using namespace ::com::sun::star::uno;
using namespace ::com::sun::star::text;
using namespace ::xmloff::token;

///////////////////////////////////////////////////////////////////////////////
//
// class XMLMeasurePropHdl
//

XMLClipPropertyHandler::~XMLClipPropertyHandler()
{
    // nothing to do
}

sal_Bool XMLClipPropertyHandler::equals(
        const Any& r1,
        const Any& r2 ) const
{
    GraphicCrop aCrop1, aCrop2;
    r1 >>= aCrop1;
    r2 >>= aCrop2;

    return aCrop1.Top == aCrop2.Top &&
           aCrop1.Bottom == aCrop2.Bottom &&
           aCrop1.Left == aCrop2.Left &&
           aCrop1.Right == aCrop2.Right;
}

sal_Bool XMLClipPropertyHandler::importXML( const OUString& rStrImpValue, uno::Any& rValue, const SvXMLUnitConverter& rUnitConverter ) const
{
    sal_Bool bRet = sal_False;
    sal_Int32 nLen = rStrImpValue.getLength();
    if( nLen > 6 &&
        0 == rStrImpValue.compareToAscii( sXML_rect, 4 ) &&
        rStrImpValue[4] == '(' &&
        rStrImpValue[nLen-1] == ')' )
    {
        GraphicCrop aCrop;
        OUString sTmp( rStrImpValue.copy( 5, nLen-6 ) );
        SvXMLTokenEnumerator aTokenEnum( sTmp );

        sal_uInt16 nPos = 0;
        OUString aToken;
        while( aTokenEnum.getNextToken( aToken ) )
        {
            sal_Int32 nVal = 0;
            if( !IsXMLToken(aToken, XML_AUTO) &&
                 !rUnitConverter.convertMeasure( nVal, aToken ) )
                break;

            switch( nPos )
            {
            case 0: aCrop.Top = nVal;   break;
            case 1: aCrop.Right = nVal; break;
            case 2: aCrop.Bottom = nVal;    break;
            case 3: aCrop.Left = nVal;  break;
            }
            nPos++;
        }

        bRet = (4 == nPos );
        if( bRet )
            rValue <<= aCrop;
    }

    return bRet;
}

sal_Bool XMLClipPropertyHandler::exportXML( OUString& rStrExpValue, const uno::Any& rValue, const SvXMLUnitConverter& rUnitConverter ) const
{
    sal_Bool bRet = sal_False;
      OUStringBuffer aOut(30);
    GraphicCrop aCrop;

    if( rValue >>= aCrop )
    {
        aOut.append( GetXMLToken(XML_RECT) );
        aOut.append( (sal_Unicode)'(' );
        rUnitConverter.convertMeasure( aOut, aCrop.Top );
        aOut.append( (sal_Unicode)' ' );
        rUnitConverter.convertMeasure( aOut, aCrop.Right );
        aOut.append( (sal_Unicode)' ' );
        rUnitConverter.convertMeasure( aOut, aCrop.Bottom );
        aOut.append( (sal_Unicode)' ' );
        rUnitConverter.convertMeasure( aOut, aCrop.Left );
        aOut.append( (sal_Unicode)')' );
        rStrExpValue = aOut.makeStringAndClear();

        bRet = sal_True;
    }

    return bRet;
}
<commit_msg>INTEGRATION: CWS pchfix02 (1.3.160); FILE MERGED 2006/09/01 17:59:50 kaib 1.3.160.1: #i68856# Added header markers and pch files<commit_after>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: XMLClipPropertyHandler.cxx,v $
 *
 *  $Revision: 1.4 $
 *
 *  last change: $Author: obo $ $Date: 2006-09-17 10:48:50 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

// MARKER(update_precomp.py): autogen include statement, do not remove
#include "precompiled_xmloff.hxx"

#ifndef _XMLOFF_XMLCLIPPROPERTYHANDLER_HXX
#include "XMLClipPropertyHandler.hxx"
#endif

#ifndef _COM_SUN_STAR_UNO_ANY_HXX_
#include <com/sun/star/uno/Any.hxx>
#endif

#ifndef _RTL_USTRBUF_HXX_
#include <rtl/ustrbuf.hxx>
#endif

#ifndef _COM_SUN_STAR_TEXT_GRAPHICCROP_HPP_
#include <com/sun/star/text/GraphicCrop.hpp>
#endif

#ifndef _XMLOFF_XMLUCONV_HXX
#include "xmluconv.hxx"
#endif

#ifndef _XMLOFF_XMLKYWD_HXX
#include <xmlkywd.hxx>
#endif
#ifndef _XMLOFF_XMLTOKEN_HXX
#include <xmltoken.hxx>
#endif

using namespace ::rtl;
using namespace ::com::sun::star;
using namespace ::com::sun::star::uno;
using namespace ::com::sun::star::text;
using namespace ::xmloff::token;

///////////////////////////////////////////////////////////////////////////////
//
// class XMLMeasurePropHdl
//

XMLClipPropertyHandler::~XMLClipPropertyHandler()
{
    // nothing to do
}

sal_Bool XMLClipPropertyHandler::equals(
        const Any& r1,
        const Any& r2 ) const
{
    GraphicCrop aCrop1, aCrop2;
    r1 >>= aCrop1;
    r2 >>= aCrop2;

    return aCrop1.Top == aCrop2.Top &&
           aCrop1.Bottom == aCrop2.Bottom &&
           aCrop1.Left == aCrop2.Left &&
           aCrop1.Right == aCrop2.Right;
}

sal_Bool XMLClipPropertyHandler::importXML( const OUString& rStrImpValue, uno::Any& rValue, const SvXMLUnitConverter& rUnitConverter ) const
{
    sal_Bool bRet = sal_False;
    sal_Int32 nLen = rStrImpValue.getLength();
    if( nLen > 6 &&
        0 == rStrImpValue.compareToAscii( sXML_rect, 4 ) &&
        rStrImpValue[4] == '(' &&
        rStrImpValue[nLen-1] == ')' )
    {
        GraphicCrop aCrop;
        OUString sTmp( rStrImpValue.copy( 5, nLen-6 ) );
        SvXMLTokenEnumerator aTokenEnum( sTmp );

        sal_uInt16 nPos = 0;
        OUString aToken;
        while( aTokenEnum.getNextToken( aToken ) )
        {
            sal_Int32 nVal = 0;
            if( !IsXMLToken(aToken, XML_AUTO) &&
                 !rUnitConverter.convertMeasure( nVal, aToken ) )
                break;

            switch( nPos )
            {
            case 0: aCrop.Top = nVal;   break;
            case 1: aCrop.Right = nVal; break;
            case 2: aCrop.Bottom = nVal;    break;
            case 3: aCrop.Left = nVal;  break;
            }
            nPos++;
        }

        bRet = (4 == nPos );
        if( bRet )
            rValue <<= aCrop;
    }

    return bRet;
}

sal_Bool XMLClipPropertyHandler::exportXML( OUString& rStrExpValue, const uno::Any& rValue, const SvXMLUnitConverter& rUnitConverter ) const
{
    sal_Bool bRet = sal_False;
      OUStringBuffer aOut(30);
    GraphicCrop aCrop;

    if( rValue >>= aCrop )
    {
        aOut.append( GetXMLToken(XML_RECT) );
        aOut.append( (sal_Unicode)'(' );
        rUnitConverter.convertMeasure( aOut, aCrop.Top );
        aOut.append( (sal_Unicode)' ' );
        rUnitConverter.convertMeasure( aOut, aCrop.Right );
        aOut.append( (sal_Unicode)' ' );
        rUnitConverter.convertMeasure( aOut, aCrop.Bottom );
        aOut.append( (sal_Unicode)' ' );
        rUnitConverter.convertMeasure( aOut, aCrop.Left );
        aOut.append( (sal_Unicode)')' );
        rStrExpValue = aOut.makeStringAndClear();

        bRet = sal_True;
    }

    return bRet;
}
<|endoftext|>