<commit_before>/** \file RobotCarVel.cc
    This domain is a simulation of velocity control for the Austin Robot
    Technology autonomous vehicle.
    This vehicle is described in:
    Beeson et al, "Multiagent Interactions in Urban Driving," Journal of Physical Agents, March 2008.
    The velocity control task is described in:
    Hester, Quinlan, and Stone, "A Real-Time Model-Based Reinforcement Learning Architecture for Robot Control", arXiv 1105.1749, 2011.
    \author Todd Hester
*/

#include <rl_env/RobotCarVel.hh>

// normal: true values of each
RobotCarVel::RobotCarVel(Random &rand, bool randomVel, bool upVel, bool tenToSix, bool lag):
  rng(rand),
  s(6),
  junk(0),
  targetVel(s[0]),
  currVel(s[1]),
  trueThrottle(s[2]),
  trueBrake(s[3]),
  throttleTarget(s[4]),
  brakeTarget(s[5]),
  randomVel(randomVel),
  upVel(upVel),
  tenToSix(tenToSix),
  lag(lag)
{
  reset();
}

RobotCarVel::~RobotCarVel() { }

const std::vector<float> &RobotCarVel::sensation() const {
  return s;
}

float RobotCarVel::apply(int action) {

  float HZ = 10.0;

  actNum++;

  // figure out reward based on target/curr vel
  float reward = -10.0 * fabs(currVel - targetVel);

  float throttleChangePct = 1.0;//0.9; //1.0;
  float brakeChangePct = 1.0;//0.9; //1.0;
  if (lag){
    brakeChangePct = brakePosVel / HZ;
    float brakeVelTarget = 3.0*(brakeTarget - trueBrake);
    brakePosVel += (brakeVelTarget - brakePosVel) * 3.0 / HZ;
    trueBrake += brakeChangePct;
  } else {
    trueBrake += (brakeTarget-trueBrake) * brakeChangePct;
  }
  trueBrake = bound(trueBrake, 0.0, 1.0);

  // figure out the change of true brake/throttle position based on last targets
  trueThrottle += (throttleTarget-trueThrottle) * throttleChangePct;
  trueThrottle = bound(trueThrottle, 0.0, 0.4);

  // figure out new velocity based on those positions
  // from the stage simulation
  float g = 9.81;         // acceleration due to gravity
  float throttle_accel = g;
  float brake_decel = g;
  float rolling_resistance = 0.01 * g;
  float drag_coeff = 0.01;
  float idle_accel = (rolling_resistance
                      + drag_coeff * 3.1 * 3.1);
  float wind_resistance = drag_coeff * currVel * currVel;
  float accel = (idle_accel
                  + trueThrottle * throttle_accel
                  - trueBrake * brake_decel
                  - rolling_resistance
                  - wind_resistance);
  currVel += (accel / HZ);
  currVel = bound(currVel, 0.0, 12.0);

  // figure out action's adjustment to throttle/brake targets
  if (action == THROTTLE_UP){
    brakeTarget = 0.0;
    if (throttleTarget < 0.4)
      throttleTarget += 0.1;
  }
  else if (action == THROTTLE_DOWN){
    brakeTarget = 0.0;
    if (throttleTarget > 0.0)
      throttleTarget -= 0.1;
  }
  else if (action == BRAKE_UP){
    throttleTarget = 0.0;
    if (brakeTarget < 1.0)
      brakeTarget += 0.1;
  }
  else if (action == BRAKE_DOWN){
    throttleTarget = 0.0;
    if (brakeTarget > 0.0)
      brakeTarget -= 0.1;
  }
  else if (action != NOTHING){
    cout << "invalid action " << action << endl;
  }
  throttleTarget = bound(throttleTarget, 0.0, 0.4);
  brakeTarget = bound(brakeTarget, 0.0, 1.0);
  throttleTarget = 0.1 * (float)((int)(throttleTarget*10.0));
  brakeTarget = 0.1 * (float)((int)(brakeTarget*10.0));

  /*
  cout << action << ", throt: " << throttleTarget << ", brake: " << brakeTarget
       << ", trueBrake: " << trueBrake
       << ", currVel: " << currVel << ", reward: " << reward << endl;
  */

  return reward;

}

bool RobotCarVel::terminal() const {
  return false;
}

void RobotCarVel::reset() {

  // for now
  if (randomVel){
    targetVel = rng.uniformDiscrete(0, 11);
    currVel = rng.uniformDiscrete(0, 11);
  } else {
    if (tenToSix){ // 10 to 6
      if (upVel){
        targetVel = 10.0;
        currVel = 6.0;
      } else {
        targetVel = 6.0;
        currVel = 10.0;
      }
    } else { // 7 to 2
      if (upVel){
        targetVel = 7.0;
        currVel = 2.0;
      } else {
        targetVel = 2.0;
        currVel = 7.0;
      }
    }
  }

  actNum = 0;
  throttleTarget = rng.uniformDiscrete(0,4) * 0.1;
  brakeTarget = 0.0;
  trueThrottle = throttleTarget;
  trueBrake = brakeTarget;
  brakePosVel = 0.0;

}

int RobotCarVel::getNumActions(){
  return 5;
}

void RobotCarVel::setSensation(std::vector<float> newS){
  if (s.size() != newS.size()){
    cerr << "Error in sensation sizes" << endl;
  }

  for (unsigned i = 0; i < newS.size(); i++){
    s[i] = newS[i];
  }
}

std::vector<experience> RobotCarVel::getSeedings() {

  // return seedings
  std::vector<experience> seeds;
  //return seeds;

  reset();

  /*
  // seeds of perfect velocity
  if (randomVel){
    for (int i = 0; i < 12; i++){
      // 3 random of each
      for (int j = 0; j < 3; j++)
        seeds.push_back(getRandomVelSeed(i));
    }
  } else {
    // just for target velocity
    // 5 random seeds
    for (int j = 0; j < 5; j++){
      seeds.push_back(getRandomVelSeed(targetVel));
    }
  }
  */

  // some completely random (non target)
  for (int i = 0; i < 25; i++){
    float vel = rng.uniform(0,11);

    float throt = 0;
    float brake = 0;
    if (rng.bernoulli(0.5)){
      throt = rng.uniformDiscrete(0,4)*0.1;
    } else {
      brake = rng.uniformDiscrete(0,9)*0.1;
    }
    int act = i%getNumActions();
    seeds.push_back(getExp(targetVel,vel,throt,brake,act));
  }

  reset();

  return seeds;
}

experience RobotCarVel::getRandomVelSeed(float target){
  float throt = 0;
  float brake = 0;
  if (rng.bernoulli(0.5)){
    throt = rng.uniformDiscrete(0,4)*0.1;
  } else {
    brake = rng.uniformDiscrete(0,4)*0.1;
  }

  return getExp(target, target, throt, brake, rng.uniformDiscrete(0,4));
}

experience RobotCarVel::getExp(float s0, float s1, float s2, float s3, int a){

  if (!randomVel) s0 = targetVel;

  experience e;

  e.s.resize(4, 0.0);
  e.next.resize(4, 0.0);

  targetVel = s0;
  currVel = s1;
  throttleTarget = s2;
  brakeTarget = s3;
  trueThrottle = throttleTarget;
  trueBrake = brakeTarget;
  brakePosVel = 0.0;

  e.act = a;
  e.s = sensation();
  e.reward = apply(e.act);

  e.terminal = terminal();
  e.next = sensation();

  /*
  cout << "seed from state: ";
  for (unsigned i = 0; i < e.s.size(); i++){
    cout << e.s[i] << ", ";
  }
  cout << "act: " << e.act << " reward: " << e.reward << " next: ";
  for (unsigned i = 0; i < e.next.size(); i++){
    cout << e.next[i] << ", ";
  }
  cout << endl;
  */

  reset();

  return e;
}

void RobotCarVel::getMinMaxFeatures(std::vector<float> *minFeat,
                                    std::vector<float> *maxFeat){

  minFeat->resize(s.size(), 0.0);
  maxFeat->resize(s.size(), 12.0);

  (*maxFeat)[2] = 0.4;
  (*maxFeat)[3] = 1.0;
  (*maxFeat)[4] = 0.4;
  (*maxFeat)[5] = 1.0;

}

void RobotCarVel::getMinMaxReward(float *minR,
                                  float *maxR){

  *minR = -120.0;
  *maxR = 0.0;

}

float RobotCarVel::bound(float val, float min, float max){
  if (val < min)
    return min;
  if (val > max)
    return max;
  return val;
}
<commit_msg>changed robot vel control simulation to give just brake/throttle targets<commit_after>/** \file RobotCarVel.cc
    This domain is a simulation of velocity control for the Austin Robot
    Technology autonomous vehicle.
    This vehicle is described in:
    Beeson et al, "Multiagent Interactions in Urban Driving," Journal of Physical Agents, March 2008.
    The velocity control task is described in:
    Hester, Quinlan, and Stone, "A Real-Time Model-Based Reinforcement Learning Architecture for Robot Control", arXiv 1105.1749, 2011.
    \author Todd Hester
*/

#include <rl_env/RobotCarVel.hh>

// normal: true values of each
RobotCarVel::RobotCarVel(Random &rand, bool randomVel, bool upVel, bool tenToSix, bool lag):
  rng(rand),
  s(4),
  junk(2),
  targetVel(s[0]),
  currVel(s[1]),
  trueThrottle(junk[0]),
  trueBrake(junk[1]),
  throttleTarget(s[2]),
  brakeTarget(s[3]),
  randomVel(randomVel),
  upVel(upVel),
  tenToSix(tenToSix),
  lag(lag)
{
  reset();
}

RobotCarVel::~RobotCarVel() { }

const std::vector<float> &RobotCarVel::sensation() const {
  return s;
}

float RobotCarVel::apply(int action) {

  float HZ = 10.0;

  actNum++;

  // figure out reward based on target/curr vel
  float reward = -10.0 * fabs(currVel - targetVel);

  float throttleChangePct = 1.0;//0.9; //1.0;
  float brakeChangePct = 1.0;//0.9; //1.0;
  if (lag){
    brakeChangePct = brakePosVel / HZ;
    float brakeVelTarget = 3.0*(brakeTarget - trueBrake);
    brakePosVel += (brakeVelTarget - brakePosVel) * 3.0 / HZ;
    trueBrake += brakeChangePct;
  } else {
    trueBrake += (brakeTarget-trueBrake) * brakeChangePct;
  }
  trueBrake = bound(trueBrake, 0.0, 1.0);

  // figure out the change of true brake/throttle position based on last targets
  trueThrottle += (throttleTarget-trueThrottle) * throttleChangePct;
  trueThrottle = bound(trueThrottle, 0.0, 0.4);

  // figure out new velocity based on those positions
  // from the stage simulation
  float g = 9.81;         // acceleration due to gravity
  float throttle_accel = g;
  float brake_decel = g;
  float rolling_resistance = 0.01 * g;
  float drag_coeff = 0.01;
  float idle_accel = (rolling_resistance
                      + drag_coeff * 3.1 * 3.1);
  float wind_resistance = drag_coeff * currVel * currVel;
  float accel = (idle_accel
                  + trueThrottle * throttle_accel
                  - trueBrake * brake_decel
                  - rolling_resistance
                  - wind_resistance);
  currVel += (accel / HZ);
  currVel = bound(currVel, 0.0, 12.0);

  // figure out action's adjustment to throttle/brake targets
  if (action == THROTTLE_UP){
    brakeTarget = 0.0;
    if (throttleTarget < 0.4)
      throttleTarget += 0.1;
  }
  else if (action == THROTTLE_DOWN){
    brakeTarget = 0.0;
    if (throttleTarget > 0.0)
      throttleTarget -= 0.1;
  }
  else if (action == BRAKE_UP){
    throttleTarget = 0.0;
    if (brakeTarget < 1.0)
      brakeTarget += 0.1;
  }
  else if (action == BRAKE_DOWN){
    throttleTarget = 0.0;
    if (brakeTarget > 0.0)
      brakeTarget -= 0.1;
  }
  else if (action != NOTHING){
    cout << "invalid action " << action << endl;
  }
  throttleTarget = bound(throttleTarget, 0.0, 0.4);
  brakeTarget = bound(brakeTarget, 0.0, 1.0);
  throttleTarget = 0.1 * (float)((int)(throttleTarget*10.0));
  brakeTarget = 0.1 * (float)((int)(brakeTarget*10.0));

  /*
  cout << action << ", throt: " << throttleTarget << ", brake: " << brakeTarget
       << ", trueBrake: " << trueBrake
       << ", currVel: " << currVel << ", reward: " << reward << endl;
  */

  return reward;

}

bool RobotCarVel::terminal() const {
  return false;
}

void RobotCarVel::reset() {

  // for now
  if (randomVel){
    targetVel = rng.uniformDiscrete(0, 11);
    currVel = rng.uniformDiscrete(0, 11);
  } else {
    if (tenToSix){ // 10 to 6
      if (upVel){
        targetVel = 10.0;
        currVel = 6.0;
      } else {
        targetVel = 6.0;
        currVel = 10.0;
      }
    } else { // 7 to 2
      if (upVel){
        targetVel = 7.0;
        currVel = 2.0;
      } else {
        targetVel = 2.0;
        currVel = 7.0;
      }
    }
  }

  actNum = 0;
  throttleTarget = rng.uniformDiscrete(0,4) * 0.1;
  brakeTarget = 0.0;
  trueThrottle = throttleTarget;
  trueBrake = brakeTarget;
  brakePosVel = 0.0;

}

int RobotCarVel::getNumActions(){
  return 5;
}

void RobotCarVel::setSensation(std::vector<float> newS){
  if (s.size() != newS.size()){
    cerr << "Error in sensation sizes" << endl;
  }

  for (unsigned i = 0; i < newS.size(); i++){
    s[i] = newS[i];
  }
}

std::vector<experience> RobotCarVel::getSeedings() {

  // return seedings
  std::vector<experience> seeds;
  //return seeds;

  reset();

  /*
  // seeds of perfect velocity
  if (randomVel){
    for (int i = 0; i < 12; i++){
      // 3 random of each
      for (int j = 0; j < 3; j++)
        seeds.push_back(getRandomVelSeed(i));
    }
  } else {
    // just for target velocity
    // 5 random seeds
    for (int j = 0; j < 5; j++){
      seeds.push_back(getRandomVelSeed(targetVel));
    }
  }
  */

  // some completely random (non target)
  for (int i = 0; i < 25; i++){
    float vel = rng.uniform(0,11);

    float throt = 0;
    float brake = 0;
    if (rng.bernoulli(0.5)){
      throt = rng.uniformDiscrete(0,4)*0.1;
    } else {
      brake = rng.uniformDiscrete(0,9)*0.1;
    }
    int act = i%getNumActions();
    seeds.push_back(getExp(targetVel,vel,throt,brake,act));
  }

  reset();

  return seeds;
}

experience RobotCarVel::getRandomVelSeed(float target){
  float throt = 0;
  float brake = 0;
  if (rng.bernoulli(0.5)){
    throt = rng.uniformDiscrete(0,4)*0.1;
  } else {
    brake = rng.uniformDiscrete(0,4)*0.1;
  }

  return getExp(target, target, throt, brake, rng.uniformDiscrete(0,4));
}

experience RobotCarVel::getExp(float s0, float s1, float s2, float s3, int a){

  if (!randomVel) s0 = targetVel;

  experience e;

  e.s.resize(4, 0.0);
  e.next.resize(4, 0.0);

  targetVel = s0;
  currVel = s1;
  throttleTarget = s2;
  brakeTarget = s3;
  trueThrottle = throttleTarget;
  trueBrake = brakeTarget;
  brakePosVel = 0.0;

  e.act = a;
  e.s = sensation();
  e.reward = apply(e.act);

  e.terminal = terminal();
  e.next = sensation();

  /*
  cout << "seed from state: ";
  for (unsigned i = 0; i < e.s.size(); i++){
    cout << e.s[i] << ", ";
  }
  cout << "act: " << e.act << " reward: " << e.reward << " next: ";
  for (unsigned i = 0; i < e.next.size(); i++){
    cout << e.next[i] << ", ";
  }
  cout << endl;
  */

  reset();

  return e;
}

void RobotCarVel::getMinMaxFeatures(std::vector<float> *minFeat,
                                    std::vector<float> *maxFeat){

  minFeat->resize(s.size(), 0.0);
  maxFeat->resize(s.size(), 12.0);

  (*maxFeat)[2] = 0.4;
  (*maxFeat)[3] = 1.0;

}

void RobotCarVel::getMinMaxReward(float *minR,
                                  float *maxR){

  *minR = -120.0;
  *maxR = 0.0;

}

float RobotCarVel::bound(float val, float min, float max){
  if (val < min)
    return min;
  if (val > max)
    return max;
  return val;
}
<|endoftext|>


<commit_before>/**
 *  @file
 *  @copyright defined in eos/LICENSE.txt
 */
#pragma once
#include <eosio/chain/transaction.hpp>
#include <eosio/chain/block.hpp>
#include <eosio/chain/trace.hpp>
#include <future>

namespace eosio { namespace chain {

/**
 *  This data structure should store context-free cached data about a transaction such as
 *  packed/unpacked/compressed and recovered keys
 */
class transaction_metadata {
   public:
      transaction_id_type                                        id;
      transaction_id_type                                        signed_id;
      signed_transaction                                         trx;
      packed_transaction                                         packed_trx;
      optional<pair<chain_id_type, flat_set<public_key_type>>>   signing_keys;
      std::future<flat_set<public_key_type>>                     signing_keys_future;
      bool                                                       accepted = false;
      bool                                                       implicit = false;
      bool                                                       scheduled = false;

      explicit transaction_metadata( const signed_transaction& t, packed_transaction::compression_type c = packed_transaction::none )
      :trx(t),packed_trx(t, c) {
         id = trx.id();
         //raw_packed = fc::raw::pack( static_cast<const transaction&>(trx) );
         signed_id = digest_type::hash(packed_trx);
      }

      explicit transaction_metadata( const packed_transaction& ptrx )
      :trx( ptrx.get_signed_transaction() ), packed_trx(ptrx) {
         id = trx.id();
         //raw_packed = fc::raw::pack( static_cast<const transaction&>(trx) );
         signed_id = digest_type::hash(packed_trx);
      }

      const flat_set<public_key_type>& recover_keys( const chain_id_type& chain_id ) {
         if( !signing_keys || signing_keys->first != chain_id ) {// Unlikely for more than one chain_id to be used in one nodeos instance
            if( signing_keys_future.valid() ) {
               signing_keys = std::make_pair( chain_id, signing_keys_future.get());
            } else {
               signing_keys = std::make_pair( chain_id, trx.get_signature_keys( chain_id ));
            }
         }
         return signing_keys->second;
      }

      uint32_t total_actions()const { return trx.context_free_actions.size() + trx.actions.size(); }
};

using transaction_metadata_ptr = std::shared_ptr<transaction_metadata>;

} } // eosio::chain
<commit_msg>Verify chain_id even though there is no way for it to be different<commit_after>/**
 *  @file
 *  @copyright defined in eos/LICENSE.txt
 */
#pragma once
#include <eosio/chain/transaction.hpp>
#include <eosio/chain/block.hpp>
#include <eosio/chain/trace.hpp>
#include <future>

namespace eosio { namespace chain {

/**
 *  This data structure should store context-free cached data about a transaction such as
 *  packed/unpacked/compressed and recovered keys
 */
class transaction_metadata {
   public:
      transaction_id_type                                        id;
      transaction_id_type                                        signed_id;
      signed_transaction                                         trx;
      packed_transaction                                         packed_trx;
      optional<pair<chain_id_type, flat_set<public_key_type>>>   signing_keys;
      std::future<pair<chain_id_type,flat_set<public_key_type>>> signing_keys_future;
      bool                                                       accepted = false;
      bool                                                       implicit = false;
      bool                                                       scheduled = false;

      explicit transaction_metadata( const signed_transaction& t, packed_transaction::compression_type c = packed_transaction::none )
      :trx(t),packed_trx(t, c) {
         id = trx.id();
         //raw_packed = fc::raw::pack( static_cast<const transaction&>(trx) );
         signed_id = digest_type::hash(packed_trx);
      }

      explicit transaction_metadata( const packed_transaction& ptrx )
      :trx( ptrx.get_signed_transaction() ), packed_trx(ptrx) {
         id = trx.id();
         //raw_packed = fc::raw::pack( static_cast<const transaction&>(trx) );
         signed_id = digest_type::hash(packed_trx);
      }

      const flat_set<public_key_type>& recover_keys( const chain_id_type& chain_id ) {
         // Unlikely for more than one chain_id to be used in one nodeos instance
         if( !signing_keys || signing_keys->first != chain_id ) {
            if( signing_keys_future.valid() ) {
               signing_keys = signing_keys_future.get();
               if( signing_keys->first == chain_id ) {
                  return signing_keys->second;
               }
            }
            signing_keys = std::make_pair( chain_id, trx.get_signature_keys( chain_id ));
         }
         return signing_keys->second;
      }

      uint32_t total_actions()const { return trx.context_free_actions.size() + trx.actions.size(); }
};

using transaction_metadata_ptr = std::shared_ptr<transaction_metadata>;

} } // eosio::chain
<|endoftext|>


<commit_before>/*
 * test_imu.cpp
 *
 *  Created on: May 24, 2013
 *      Author: drc
 */
#include <iostream>
#include <csignal>
#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/assign/std/vector.hpp>
#include <lcm/lcm-cpp.hpp>
#include <lcm/lcm.h>
#include "lcmtypes/drc_lcmtypes.hpp"
#include <model-client/model-client.hpp>
#include <lcmtypes/bot_core.hpp>
#include <Eigen/Dense>
#include "QuaternionLib.h"

#define PI  3.14159265358979323

using namespace std;
using namespace InertialOdometry;

void signalHandler( int signum ){
  cout << "Interrupt signal (" << signum << ") received.\n";

  // cleanup and close up
  try{
    cout << "Exiting.\n";
  } catch (std::exception &e){
    std::cout << "Exception occurred during close out\n";
  }
  // terminate program

  exit(signum);

}

class HandleIMU {
private:
	boost::shared_ptr<lcm::LCM> _lcm;

	Eigen::Quaterniond imu_orientation;
	double max;
	unsigned long long prev_msg_utime;
	unsigned long long fall_utime;

public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

	HandleIMU(boost::shared_ptr<lcm::LCM> &_lcm) : _lcm(_lcm) {
		cout << "New handler\n";
		max = 0;
		_lcm->subscribe("TRUE_ROBOT_STATE",&HandleIMU::true_state_handler,this);
		_lcm->subscribe("TORSO_IMU",&HandleIMU::torso_state_handler,this);

		prev_msg_utime = 0;
		fall_utime = 0;

	}

	void true_state_handler(const lcm::ReceiveBuffer* rbuf,
							const std::string& channel,
							const  drc::robot_state_t* msg) {

		Eigen::Quaterniond tp_q(msg->origin_position.rotation.w,
								msg->origin_position.rotation.x,
								msg->origin_position.rotation.y,
								msg->origin_position.rotation.z);

		//cout << "" << (msg->utime - prev_msg_utime)/1000. << endl;

		prev_msg_utime = msg->utime;

		Eigen::Vector3d err_angles;

		Eigen::Matrix3d C;
		C = q2C(tp_q);

		err_angles = C2e(C) - q2e_new(imu_orientation);

		if (err_angles.norm()>max) {
			max = err_angles.norm();
		}

		if (err_angles.norm() > 1E-2) {
			//cout << fixed << max*57.29 << " | " << err_angles.norm()*57.29 << endl;
		}
		//cout << "Receiving state\n";

		//cout << "num_joints: " << msg->num_joints << endl;

	}

	void torso_state_handler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  drc::imu_t* msg) {

		Eigen::Quaterniond q(msg->orientation[0],msg->orientation[1],msg->orientation[2],msg->orientation[3]);

		imu_orientation = q;

		Eigen::Vector4d check_conv;
		Eigen::Quaterniond __q;

		__q = e2q(q2e_new(q));

		//std::cout <<  "IMU_ANGLES: " << q2e_new(q).transpose() << std::endl;

		Eigen::Vector3d E;
		E = q2e_new(q);

		// fall detector
		if ((E(1) > 20*PI/180. || E(0) > 20*PI/180.) && (msg->utime - fall_utime) > 30000000) {
			fall_utime = msg->utime;
			system("notify-send ROBOT_FELL_OVER");
		}

		check_conv(0) = __q.w() - q.w();
		check_conv(1) = __q.x() - q.x();
		check_conv(2) = __q.y() - q.y();
		check_conv(3) = __q.z() - q.z();

		//cout << "Checking additive norm~: " << check_conv.norm() << endl;

		//cout << "Receiving IMU\n";

	}

};

int main() {

  //Eigen::Matrix3d test;

  //test << 1,2,3,4,5,6,7,8,9;

  //cout << test(2,1) << endl;

  //cout << "Got here\n";

  // register signal SIGINT and signal handler
  signal(SIGINT, signalHandler);

  system("notify-send FallDetectorStarted");

  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM);
  if(!lcm->good())
    return 1;

  HandleIMU handler(lcm);

  while(0 == lcm->handle());

  return 0;
}

<commit_msg>updated a timeout flag for fall notification<commit_after>/*
 * test_imu.cpp
 *
 *  Created on: May 24, 2013
 *      Author: drc
 */
#include <iostream>
#include <csignal>
#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/assign/std/vector.hpp>
#include <lcm/lcm-cpp.hpp>
#include <lcm/lcm.h>
#include "lcmtypes/drc_lcmtypes.hpp"
#include <model-client/model-client.hpp>
#include <lcmtypes/bot_core.hpp>
#include <Eigen/Dense>
#include "QuaternionLib.h"

#define PI  3.14159265358979323

using namespace std;
using namespace InertialOdometry;

void signalHandler( int signum ){
  cout << "Interrupt signal (" << signum << ") received.\n";

  // cleanup and close up
  try{
    cout << "Exiting.\n";
  } catch (std::exception &e){
    std::cout << "Exception occurred during close out\n";
  }
  // terminate program

  exit(signum);

}

class HandleIMU {
private:
	boost::shared_ptr<lcm::LCM> _lcm;

	Eigen::Quaterniond imu_orientation;
	double max;
	unsigned long long prev_msg_utime;
	unsigned long long fall_utime;

public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

	HandleIMU(boost::shared_ptr<lcm::LCM> &_lcm) : _lcm(_lcm) {
		cout << "New handler\n";
		max = 0;
		_lcm->subscribe("TRUE_ROBOT_STATE",&HandleIMU::true_state_handler,this);
		_lcm->subscribe("TORSO_IMU",&HandleIMU::torso_state_handler,this);

		prev_msg_utime = 0;
		fall_utime = 0;

	}

	void true_state_handler(const lcm::ReceiveBuffer* rbuf,
							const std::string& channel,
							const  drc::robot_state_t* msg) {

		Eigen::Quaterniond tp_q(msg->origin_position.rotation.w,
								msg->origin_position.rotation.x,
								msg->origin_position.rotation.y,
								msg->origin_position.rotation.z);

		//cout << "" << (msg->utime - prev_msg_utime)/1000. << endl;

		prev_msg_utime = msg->utime;

		Eigen::Vector3d err_angles;

		Eigen::Matrix3d C;
		C = q2C(tp_q);

		err_angles = C2e(C) - q2e_new(imu_orientation);

		if (err_angles.norm()>max) {
			max = err_angles.norm();
		}

		if (err_angles.norm() > 1E-2) {
			//cout << fixed << max*57.29 << " | " << err_angles.norm()*57.29 << endl;
		}
		//cout << "Receiving state\n";

		//cout << "num_joints: " << msg->num_joints << endl;

	}

	void torso_state_handler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  drc::imu_t* msg) {

		Eigen::Quaterniond q(msg->orientation[0],msg->orientation[1],msg->orientation[2],msg->orientation[3]);

		imu_orientation = q;

		Eigen::Vector4d check_conv;
		Eigen::Quaterniond __q;

		__q = e2q(q2e_new(q));

		//std::cout <<  "IMU_ANGLES: " << q2e_new(q).transpose() << std::endl;

		Eigen::Vector3d E;
		E = q2e_new(q);

		// fall detector

		if (E(1) > 20*PI/180. || E(0) > 20*PI/180.) {
			//std::cout << "Falling\n";
			if ((msg->utime - fall_utime) > 1200000) {
				std::cout << "Timeout.\n";
				fall_utime = msg->utime;
				system("notify-send ROBOT_FELL_OVER");
			}
		}

		check_conv(0) = __q.w() - q.w();
		check_conv(1) = __q.x() - q.x();
		check_conv(2) = __q.y() - q.y();
		check_conv(3) = __q.z() - q.z();

		//cout << "Checking additive norm~: " << check_conv.norm() << endl;

		//cout << "Receiving IMU\n";

	}

};

int main() {

  //Eigen::Matrix3d test;

  //test << 1,2,3,4,5,6,7,8,9;

  //cout << test(2,1) << endl;

  //cout << "Got here\n";

  // register signal SIGINT and signal handler
  signal(SIGINT, signalHandler);

  system("notify-send FallDetectorStarted");

  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM);
  if(!lcm->good())
    return 1;

  HandleIMU handler(lcm);

  while(0 == lcm->handle());

  return 0;
}

<|endoftext|>


<commit_before><commit_msg>FIX: Add softmax to MLP multi-class output<commit_after><|endoftext|>


<commit_before>/* Copyright 2018 The TensorFlow Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#include "tensorflow/compiler/xla/service/gpu/cublas_pad_for_gemms.h"

#include "tensorflow/compiler/xla/literal_util.h"
#include "tensorflow/compiler/xla/service/gpu/ir_emission_utils.h"
#include "tensorflow/compiler/xla/service/hlo_casting_utils.h"
#include "tensorflow/compiler/xla/service/hlo_instruction.h"
#include "tensorflow/compiler/xla/service/hlo_instructions.h"
#include "tensorflow/compiler/xla/util.h"
#include "tensorflow/compiler/xla/window_util.h"

namespace xla {
namespace gpu {

static StatusOr<bool> PadForGemm(HloDotInstruction* dot, PrimitiveType datatype,
                                 int pad_to_multiple_of) {
  auto* lhs = dot->mutable_operand(0);
  auto* rhs = dot->mutable_operand(1);

  Shape lshape = lhs->shape();
  Shape rshape = rhs->shape();
  Shape result_shape = dot->shape();

  if (lshape.element_type() != datatype || rshape.element_type() != datatype) {
    return false;
  }

  auto pad_dim = [&](Shape& s, int dim) {
    s.set_dimensions(dim,
                     RoundUpTo<int64_t>(s.dimensions(dim), pad_to_multiple_of));
  };

  auto pad_matrix_dims = [&pad_dim](Shape s) {
    // Since the dot instruction is canonicalized, the last two dimensions for
    // each operand represent non-batch dimensions, and the others are the same
    // for both operands and correspond to batch dimensions.
    pad_dim(s, s.rank() - 2);
    pad_dim(s, s.rank() - 1);
    return s;
  };

  Shape new_lshape = pad_matrix_dims(lshape);
  Shape new_rshape = pad_matrix_dims(rshape);
  Shape new_result_shape = pad_matrix_dims(result_shape);

  if (new_lshape == lshape && new_rshape == rshape) {
    return false;
  }

  VLOG(3) << "old shape: " << lshape << " " << rshape << " " << result_shape;
  VLOG(3) << "new shape: " << new_lshape << " " << new_rshape << " "
          << new_result_shape;

  auto create_padding_config = [](Shape& shape, Shape& new_shape) {
    PaddingConfig padding_config;
    for (int i = 0; i < shape.rank(); ++i) {
      auto dimension = padding_config.add_dimensions();
      dimension->set_edge_padding_high(new_shape.dimensions()[i] -
                                       shape.dimensions()[i]);
      dimension->set_edge_padding_low(0);
      dimension->set_interior_padding(0);
    }
    return padding_config;
  };

  auto l_padding_config = create_padding_config(lshape, new_lshape);
  auto r_padding_config = create_padding_config(rshape, new_rshape);

  HloComputation* parent = dot->parent();

  HloInstruction* zero_float = parent->AddInstruction(
      HloInstruction::CreateConstant(LiteralUtil::Zero(datatype)));
  zero_float->set_metadata(dot->metadata());

  HloInstruction* lpad = parent->AddInstruction(
      HloInstruction::CreatePad(new_lshape, lhs, zero_float, l_padding_config));
  lpad->set_metadata(dot->metadata());

  HloInstruction* rpad = parent->AddInstruction(
      HloInstruction::CreatePad(new_rshape, rhs, zero_float, r_padding_config));
  rpad->set_metadata(dot->metadata());

  HloInstruction* new_dot = parent->AddInstruction(
      dot->CloneWithNewOperands(new_result_shape, {lpad, rpad}));

  std::vector<int64_t> start_indices(result_shape.rank(), 0);
  std::vector<int64_t> strides(result_shape.rank(), 1);
  HloInstruction* slice = parent->AddInstruction(
      HloInstruction::CreateSlice(result_shape, new_dot, start_indices,
                                  result_shape.dimensions(), strides));
  slice->set_metadata(dot->metadata());

  bool is_root = dot->user_count() == 0;

  TF_CHECK_OK(parent->ReplaceInstruction(dot, slice));

  if (is_root) {
    parent->set_root_instruction(slice);
  }

  return true;
}

namespace {

// We need this check because PadForGemm works in the assumption that
// the dot instruction is canonicalized.
bool CheckCanonical(HloDotInstruction* dot) {
  auto dimension_numbers = dot->dot_dimension_numbers();

  if (dimension_numbers.lhs_batch_dimensions_size() + 2 !=
          dot->operand(0)->shape().rank() ||
      dimension_numbers.rhs_batch_dimensions_size() + 2 !=
          dot->operand(1)->shape().rank()) {
    LOG(ERROR) << "Dot is not canonical: Expected all dimensions but 2 to be "
                  "batch_dimensions.";
    return false;
  }

  std::vector<int64_t> canonical_batch_dims(
      dimension_numbers.lhs_batch_dimensions_size());
  absl::c_iota(canonical_batch_dims, 0);
  if (!absl::c_equal(dimension_numbers.lhs_batch_dimensions(),
                     canonical_batch_dims) ||
      !absl::c_equal(dimension_numbers.rhs_batch_dimensions(),
                     canonical_batch_dims)) {
    LOG(ERROR) << "Dot is not canonical: Expected batch dimensions to be all "
                  "dimensions except for the last 2 ones.";
    return false;
  }

  return true;
}

}  // namespace

static std::vector<HloDotInstruction*> GetRelevantDots(HloComputation* comp,
                                                       PrimitiveType datatype) {
  std::vector<HloDotInstruction*> gemms;

  for (HloInstruction* instr : comp->instructions()) {
    if (IsMatrixMultiplication(*instr)) {
      HloDotInstruction* dot = Cast<HloDotInstruction>(instr);
      if (instr->operand(0)->shape().element_type() == datatype &&
          CheckCanonical(dot)) {
        gemms.push_back(dot);
      }
    }
  }
  return gemms;
}

StatusOr<bool> CublasPadForGemms::Run(
    HloModule* module,
    const absl::flat_hash_set<absl::string_view>& execution_threads) {
  bool changed = false;
  for (HloComputation* comp :
       module->MakeNonfusionComputations(execution_threads)) {
    for (HloDotInstruction* dot : GetRelevantDots(comp, datatype_)) {
      TF_ASSIGN_OR_RETURN(bool result,
                          PadForGemm(dot, datatype_, pad_to_multiple_of_));
      changed |= result;
    }
  }
  return changed;
}

}  // namespace gpu
}  // namespace xla
<commit_msg>Replace error with VLOG<commit_after>/* Copyright 2018 The TensorFlow Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#include "tensorflow/compiler/xla/service/gpu/cublas_pad_for_gemms.h"

#include "tensorflow/compiler/xla/literal_util.h"
#include "tensorflow/compiler/xla/service/gpu/ir_emission_utils.h"
#include "tensorflow/compiler/xla/service/hlo_casting_utils.h"
#include "tensorflow/compiler/xla/service/hlo_instruction.h"
#include "tensorflow/compiler/xla/service/hlo_instructions.h"
#include "tensorflow/compiler/xla/util.h"
#include "tensorflow/compiler/xla/window_util.h"

namespace xla {
namespace gpu {

static StatusOr<bool> PadForGemm(HloDotInstruction* dot, PrimitiveType datatype,
                                 int pad_to_multiple_of) {
  auto* lhs = dot->mutable_operand(0);
  auto* rhs = dot->mutable_operand(1);

  Shape lshape = lhs->shape();
  Shape rshape = rhs->shape();
  Shape result_shape = dot->shape();

  if (lshape.element_type() != datatype || rshape.element_type() != datatype) {
    return false;
  }

  auto pad_dim = [&](Shape& s, int dim) {
    s.set_dimensions(dim,
                     RoundUpTo<int64_t>(s.dimensions(dim), pad_to_multiple_of));
  };

  auto pad_matrix_dims = [&pad_dim](Shape s) {
    // Since the dot instruction is canonicalized, the last two dimensions for
    // each operand represent non-batch dimensions, and the others are the same
    // for both operands and correspond to batch dimensions.
    pad_dim(s, s.rank() - 2);
    pad_dim(s, s.rank() - 1);
    return s;
  };

  Shape new_lshape = pad_matrix_dims(lshape);
  Shape new_rshape = pad_matrix_dims(rshape);
  Shape new_result_shape = pad_matrix_dims(result_shape);

  if (new_lshape == lshape && new_rshape == rshape) {
    return false;
  }

  VLOG(3) << "old shape: " << lshape << " " << rshape << " " << result_shape;
  VLOG(3) << "new shape: " << new_lshape << " " << new_rshape << " "
          << new_result_shape;

  auto create_padding_config = [](Shape& shape, Shape& new_shape) {
    PaddingConfig padding_config;
    for (int i = 0; i < shape.rank(); ++i) {
      auto dimension = padding_config.add_dimensions();
      dimension->set_edge_padding_high(new_shape.dimensions()[i] -
                                       shape.dimensions()[i]);
      dimension->set_edge_padding_low(0);
      dimension->set_interior_padding(0);
    }
    return padding_config;
  };

  auto l_padding_config = create_padding_config(lshape, new_lshape);
  auto r_padding_config = create_padding_config(rshape, new_rshape);

  HloComputation* parent = dot->parent();

  HloInstruction* zero_float = parent->AddInstruction(
      HloInstruction::CreateConstant(LiteralUtil::Zero(datatype)));
  zero_float->set_metadata(dot->metadata());

  HloInstruction* lpad = parent->AddInstruction(
      HloInstruction::CreatePad(new_lshape, lhs, zero_float, l_padding_config));
  lpad->set_metadata(dot->metadata());

  HloInstruction* rpad = parent->AddInstruction(
      HloInstruction::CreatePad(new_rshape, rhs, zero_float, r_padding_config));
  rpad->set_metadata(dot->metadata());

  HloInstruction* new_dot = parent->AddInstruction(
      dot->CloneWithNewOperands(new_result_shape, {lpad, rpad}));

  std::vector<int64_t> start_indices(result_shape.rank(), 0);
  std::vector<int64_t> strides(result_shape.rank(), 1);
  HloInstruction* slice = parent->AddInstruction(
      HloInstruction::CreateSlice(result_shape, new_dot, start_indices,
                                  result_shape.dimensions(), strides));
  slice->set_metadata(dot->metadata());

  bool is_root = dot->user_count() == 0;

  TF_CHECK_OK(parent->ReplaceInstruction(dot, slice));

  if (is_root) {
    parent->set_root_instruction(slice);
  }

  return true;
}

namespace {

// We need this check because PadForGemm works in the assumption that
// the dot instruction is canonicalized.
bool CheckCanonical(HloDotInstruction* dot) {
  auto dimension_numbers = dot->dot_dimension_numbers();

  if (dimension_numbers.lhs_batch_dimensions_size() + 2 !=
          dot->operand(0)->shape().rank() ||
      dimension_numbers.rhs_batch_dimensions_size() + 2 !=
          dot->operand(1)->shape().rank()) {
    VLOG(2)
        << dot->ToString()
        << " is not canonical: Expected all dimensions but 2 to be "
           "batch_dimensions. Hence, this dot is not a candidate for padding.";
    return false;
  }

  std::vector<int64_t> canonical_batch_dims(
      dimension_numbers.lhs_batch_dimensions_size());
  absl::c_iota(canonical_batch_dims, 0);
  if (!absl::c_equal(dimension_numbers.lhs_batch_dimensions(),
                     canonical_batch_dims) ||
      !absl::c_equal(dimension_numbers.rhs_batch_dimensions(),
                     canonical_batch_dims)) {
    VLOG(2)
        dot->ToString()
        << " is not canonical: Expected batch dimensions to be all "
           "dimensions except for the last 2 ones. Hence, this dot is not a "
           "candidate for padding.";
    return false;
  }

  return true;
}

}  // namespace

static std::vector<HloDotInstruction*> GetRelevantDots(HloComputation* comp,
                                                       PrimitiveType datatype) {
  std::vector<HloDotInstruction*> gemms;

  for (HloInstruction* instr : comp->instructions()) {
    if (IsMatrixMultiplication(*instr)) {
      HloDotInstruction* dot = Cast<HloDotInstruction>(instr);
      if (instr->operand(0)->shape().element_type() == datatype &&
          CheckCanonical(dot)) {
        gemms.push_back(dot);
      }
    }
  }
  return gemms;
}

StatusOr<bool> CublasPadForGemms::Run(
    HloModule* module,
    const absl::flat_hash_set<absl::string_view>& execution_threads) {
  bool changed = false;
  for (HloComputation* comp :
       module->MakeNonfusionComputations(execution_threads)) {
    for (HloDotInstruction* dot : GetRelevantDots(comp, datatype_)) {
      TF_ASSIGN_OR_RETURN(bool result,
                          PadForGemm(dot, datatype_, pad_to_multiple_of_));
      changed |= result;
    }
  }
  return changed;
}

}  // namespace gpu
}  // namespace xla
<|endoftext|>


<commit_before>//===----------------------------------------------------------------------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is dual licensed under the MIT and the University of Illinois Open
// Source Licenses. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//

// This test verifies behavior specified by [atomics.types.operations.req]/21:
//
//     When only one memory_order argument is supplied, the value of success is
//     order, and the value of failure is order except that a value of
//     memory_order_acq_rel shall be replaced by the value memory_order_acquire
//     and a value of memory_order_release shall be replaced by the value
//     memory_order_relaxed.
//
// Clang's atomic intrinsics do this for us, but GCC's do not. We don't actually
// have visibility to see what these memory orders are lowered to, but we can at
// least check that they are lowered at all (otherwise there is a compile
// failure with GCC).

#include <atomic>

int main() {
    std::atomic<int> i;
    volatile std::atomic<int> v;
    int exp;

    i.compare_exchange_weak(exp, 0, std::memory_order_acq_rel);
    i.compare_exchange_weak(exp, 0, std::memory_order_release);
    i.compare_exchange_strong(exp, 0, std::memory_order_acq_rel);
    i.compare_exchange_strong(exp, 0, std::memory_order_release);

    v.compare_exchange_weak(exp, 0, std::memory_order_acq_rel);
    v.compare_exchange_weak(exp, 0, std::memory_order_release);
    v.compare_exchange_strong(exp, 0, std::memory_order_acq_rel);
    v.compare_exchange_strong(exp, 0, std::memory_order_release);

    return 0;
}
<commit_msg>Appease MSAN buildbots.<commit_after>//===----------------------------------------------------------------------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is dual licensed under the MIT and the University of Illinois Open
// Source Licenses. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//

// This test verifies behavior specified by [atomics.types.operations.req]/21:
//
//     When only one memory_order argument is supplied, the value of success is
//     order, and the value of failure is order except that a value of
//     memory_order_acq_rel shall be replaced by the value memory_order_acquire
//     and a value of memory_order_release shall be replaced by the value
//     memory_order_relaxed.
//
// Clang's atomic intrinsics do this for us, but GCC's do not. We don't actually
// have visibility to see what these memory orders are lowered to, but we can at
// least check that they are lowered at all (otherwise there is a compile
// failure with GCC).

#include <atomic>

int main() {
    std::atomic<int> i;
    volatile std::atomic<int> v;
    int exp = 0;

    i.compare_exchange_weak(exp, 0, std::memory_order_acq_rel);
    i.compare_exchange_weak(exp, 0, std::memory_order_release);
    i.compare_exchange_strong(exp, 0, std::memory_order_acq_rel);
    i.compare_exchange_strong(exp, 0, std::memory_order_release);

    v.compare_exchange_weak(exp, 0, std::memory_order_acq_rel);
    v.compare_exchange_weak(exp, 0, std::memory_order_release);
    v.compare_exchange_strong(exp, 0, std::memory_order_acq_rel);
    v.compare_exchange_strong(exp, 0, std::memory_order_release);

    return 0;
}
<|endoftext|>


<commit_before>
#pragma once

#include "integrators/embedded_explicit_runge_kutta_nyström_integrator.hpp"

#include <algorithm>
#include <cmath>
#include <ctime>
#include <experimental/optional>
#include <vector>

#include "geometry/sign.hpp"
#include "glog/logging.h"
#include "quantities/quantities.hpp"

namespace principia {
namespace integrators {
namespace internal_embedded_explicit_runge_kutta_nyström_integrator {

using base::make_not_null_unique;
using geometry::Sign;
using numerics::DoublePrecision;
using quantities::DebugString;
using quantities::Difference;
using quantities::Quotient;

template<typename Method, typename Position>
EmbeddedExplicitRungeKuttaNyströmIntegrator<Method, Position>::
EmbeddedExplicitRungeKuttaNyströmIntegrator()
    : AdaptiveStepSizeIntegrator<
          SpecialSecondOrderDifferentialEquation<Position>>(Method::kind) {
  // the first node is always 0 in an explicit method.
  CHECK_EQ(0.0, c_[0]);
  if (first_same_as_last) {
    // Check that the conditions for the FSAL property are satisfied, see for
    // instance Dormand, El-Mikkawy and Prince (1986),
    // Families of Runge-Kutta-Nyström formulae, equation 3.1.
    CHECK_EQ(1.0, c_[stages_ - 1]);
    CHECK_EQ(0.0, b_hat_[stages_ - 1]);
    for (int j = 0; j < stages_ - 1; ++j) {
      CHECK_EQ(b_hat_[j], a_[stages_ - 1][j]);
    }
  }
}

template<typename Method, typename Position>
Status EmbeddedExplicitRungeKuttaNyströmIntegrator<Method, Position>::
Instance::Solve(Instant const& t_final) {
  using Displacement = typename ODE::Displacement;
  using Velocity = typename ODE::Velocity;
  using Acceleration = typename ODE::Acceleration;

  auto const& a = integrator_.a_;
  auto const& b_hat = integrator_.b_hat_;
  auto const& b_prime_hat = integrator_.b_prime_hat_;
  auto const& b = integrator_.b_;
  auto const& b_prime = integrator_.b_prime_;
  auto const& c = integrator_.c_;

  auto& append_state = this->append_state_;
  auto& current_state = this->current_state_;
  auto& first_use = this->first_use_;
  auto& parameters = this->parameters_;
  auto const& equation = this->equation_;

  // |current_state| gets updated as the integration progresses to allow
  // restartability.

  // State before the last, truncated step.
  std::experimental::optional<typename ODE::SystemState> final_state;

  // Argument checks.
  int const dimension = current_state.positions.size();
  Sign const integration_direction = Sign(parameters.first_time_step);
  if (integration_direction.Positive()) {
    // Integrating forward.
    CHECK_LT(current_state.time.value, t_final);
  } else {
    // Integrating backward.
    CHECK_GT(current_state.time.value, t_final);
  }
  CHECK(first_use || !parameters.last_step_is_exact)
      << "Cannot reuse an instance where the last step is exact";
  first_use = false;

  // Time step.  Updated as the integration progresses to allow restartability.
  Time& h = this->time_step_;
  // Current time.  This is a non-const reference whose purpose is to make the
  // equations more readable.
  DoublePrecision<Instant>& t = current_state.time;

  // Position increment (high-order).
  std::vector<Displacement> Δq_hat(dimension);
  // Velocity increment (high-order).
  std::vector<Velocity> Δv_hat(dimension);
  // Current position.  This is a non-const reference whose purpose is to make
  // the equations more readable.
  std::vector<DoublePrecision<Position>>& q_hat = current_state.positions;
  // Current velocity.  This is a non-const reference whose purpose is to make
  // the equations more readable.
  std::vector<DoublePrecision<Velocity>>& v_hat = current_state.velocities;

  // Difference between the low- and high-order approximations.
  typename ODE::SystemStateError error_estimate;
  error_estimate.position_error.resize(dimension);
  error_estimate.velocity_error.resize(dimension);

  // Current Runge-Kutta-Nyström stage.
  std::vector<Position> q_stage(dimension);
  // Accelerations at each stage.
  // TODO(egg): this is a rectangular container, use something more appropriate.
  std::vector<std::vector<Acceleration>> g(stages_);
  for (auto& g_stage : g) {
    g_stage.resize(dimension);
  }

  bool at_end = false;
  double tolerance_to_error_ratio;

  // The first stage of the Runge-Kutta-Nyström iteration.  In the FSAL case,
  // |first_stage == 1| after the first step, since the first RHS evaluation has
  // already occured in the previous step.  In the non-FSAL case and in the
  // first step of the FSAL case, |first_stage == 0|.
  int first_stage = 0;

  // The number of steps already performed.
  std::int64_t step_count = 0;

  Status status;

  // No step size control on the first step.  If this instance is being
  // restarted we already have a value of |h| suitable for the next step, based
  // on the computation of |tolerance_to_error_ratio_| during the last
  // invocation.
  goto runge_kutta_nyström_step;

  while (!at_end) {
    // Compute the next step with decreasing step sizes until the error is
    // tolerable.
    do {
      // Adapt step size.
      // TODO(egg): find out whether there's a smarter way to compute that root,
      // especially since we make the order compile-time.
      h *= parameters.safety_factor *
               std::pow(tolerance_to_error_ratio, 1.0 / (lower_order + 1));
      // TODO(egg): should we check whether it vanishes in double precision
      // instead?
      if (t.value + (t.error + h) == t.value) {
        return Status(termination_condition::VanishingStepSize,
                      "At time " + DebugString(t.value) +
                          ", step size is effectively zero.  Singularity or "
                          "stiff system suspected.");
      }

    runge_kutta_nyström_step:
      // Termination condition.
      if (parameters.last_step_is_exact) {
        Time const time_to_end = (t_final - t.value) - t.error;
        at_end = integration_direction * h >=
                 integration_direction * time_to_end;
        if (at_end) {
          // The chosen step size will overshoot.  Clip it to just reach the
          // end, and terminate if the step is accepted.
          h = time_to_end;
          final_state = current_state;
        }
      }

      // Runge-Kutta-Nyström iteration; fills |g|.
      for (int i = first_stage; i < stages_; ++i) {
        Instant const t_stage = t.value + (t.error + c[i] * h);
        for (int k = 0; k < dimension; ++k) {
          Acceleration Σj_a_ij_g_jk{};
          for (int j = 0; j < i; ++j) {
            Σj_a_ij_g_jk += a[i][j] * g[j][k];
          }
          q_stage[k] = q_hat[k].value +
                           h * (c[i] * v_hat[k].value + h * Σj_a_ij_g_jk);
        }
        status.Update(equation.compute_acceleration(t_stage, q_stage, g[i]));
      }

      // Increment computation and step size control.
      for (int k = 0; k < dimension; ++k) {
        Acceleration Σi_b_hat_i_g_ik{};
        Acceleration Σi_b_i_g_ik{};
        Acceleration Σi_b_prime_hat_i_g_ik{};
        Acceleration Σi_b_prime_i_g_ik{};
        // Please keep the eight assigments below aligned, they become illegible
        // otherwise.
        for (int i = 0; i < stages_; ++i) {
          Σi_b_hat_i_g_ik       += b_hat[i] * g[i][k];
          Σi_b_i_g_ik           += b[i] * g[i][k];
          Σi_b_prime_hat_i_g_ik += b_prime_hat[i] * g[i][k];
          Σi_b_prime_i_g_ik     += b_prime[i] * g[i][k];
        }
        // The hat-less Δq and Δv are the low-order increments.
        Δq_hat[k]               = h * (h * (Σi_b_hat_i_g_ik) + v_hat[k].value);
        Displacement const Δq_k = h * (h * (Σi_b_i_g_ik) + v_hat[k].value);
        Δv_hat[k]               = h * Σi_b_prime_hat_i_g_ik;
        Velocity const Δv_k     = h * Σi_b_prime_i_g_ik;

        error_estimate.position_error[k] = Δq_k - Δq_hat[k];
        error_estimate.velocity_error[k] = Δv_k - Δv_hat[k];
      }
      tolerance_to_error_ratio =
          this->tolerance_to_error_ratio_(h, error_estimate);
    } while (tolerance_to_error_ratio < 1.0);

    if (!parameters.last_step_is_exact && t.value + (t.error + h) > t_final) {
      // We did overshoot.  Drop the point that we just computed and exit.
      final_state = current_state;
      break;
    }

    if (first_same_as_last) {
      using std::swap;
      swap(g.front(), g.back());
      first_stage = 1;
    }

    // Increment the solution with the high-order approximation.
    t.Increment(h);
    for (int k = 0; k < dimension; ++k) {
      q_hat[k].Increment(Δq_hat[k]);
      v_hat[k].Increment(Δv_hat[k]);
    }
    append_state(current_state);
    ++step_count;
    if (step_count == parameters.max_steps && !at_end) {
      return Status(termination_condition::ReachedMaximalStepCount,
                    "Reached maximum step count " +
                        std::to_string(parameters.max_steps) +
                        " at time " + DebugString(t.value) +
                        "; requested t_final is " + DebugString(t_final) +
                        ".");
    }
  }
  // The resolution is restartable from the last non-truncated state.
  CHECK(final_state);
  current_state = *final_state;
  return status;
}

template<typename Method, typename Position>
EmbeddedExplicitRungeKuttaNyströmIntegrator<Method, Position> const&
EmbeddedExplicitRungeKuttaNyströmIntegrator<Method, Position>::
Instance::integrator() const {
  return integrator_;
}

template<typename Method, typename Position>
not_null<std::unique_ptr<typename Integrator<
    SpecialSecondOrderDifferentialEquation<Position>>::Instance>>
EmbeddedExplicitRungeKuttaNyströmIntegrator<Method, Position>::
Instance::Clone() const {
  return std::unique_ptr<Instance>(new Instance(*this));
}

template<typename Method, typename Position>
void EmbeddedExplicitRungeKuttaNyströmIntegrator<Method, Position>::
Instance::WriteToMessage(
    not_null<serialization::IntegratorInstance*> message) const {
  AdaptiveStepSizeIntegrator<ODE>::Instance::WriteToMessage(message);
  auto* const extension =
      message
          ->MutableExtension(
              serialization::AdaptiveStepSizeIntegratorInstance::extension)
          ->MutableExtension(
              serialization::
                  EmbeddedExplicitRungeKuttaNystromIntegratorInstance::
                      extension);
}

template<typename Method, typename Position>
EmbeddedExplicitRungeKuttaNyströmIntegrator<Method, Position>::
Instance::Instance(
    IntegrationProblem<ODE> const& problem,
    AppendState const& append_state,
    ToleranceToErrorRatio const& tolerance_to_error_ratio,
    Parameters const& parameters,
    EmbeddedExplicitRungeKuttaNyströmIntegrator const& integrator)
    : AdaptiveStepSizeIntegrator<ODE>::Instance(
          problem, append_state, tolerance_to_error_ratio, parameters),
      integrator_(integrator) {}

template<typename Method, typename Position>
not_null<std::unique_ptr<typename Integrator<
    SpecialSecondOrderDifferentialEquation<Position>>::Instance>>
EmbeddedExplicitRungeKuttaNyströmIntegrator<Method, Position>::
NewInstance(IntegrationProblem<ODE> const& problem,
            AppendState const& append_state,
            ToleranceToErrorRatio const& tolerance_to_error_ratio,
            Parameters const& parameters) const {
  // Cannot use |make_not_null_unique| because the constructor of |Instance| is
  // private.
  return std::unique_ptr<Instance>(new Instance(problem,
                                                append_state,
                                                tolerance_to_error_ratio,
                                                parameters,
                                                *this));
}

template<typename Method, typename Position>
not_null<std::unique_ptr<typename Integrator<
    SpecialSecondOrderDifferentialEquation<Position>>::Instance>>
EmbeddedExplicitRungeKuttaNyströmIntegrator<Method, Position>::
ReadFromMessage(
    serialization::AdaptiveStepSizeIntegratorInstance const& message,
    IntegrationProblem<ODE> const& problem,
    AppendState const& append_state,
    ToleranceToErrorRatio const& tolerance_to_error_ratio,
    Parameters const& parameters) const {
  CHECK(message.HasExtension(
      serialization::EmbeddedExplicitRungeKuttaNystromIntegratorInstance::
          extension))
      << message.DebugString();

  // Cannot use |make_not_null_unique| because the constructor of |Instance| is
  // private.
  return std::unique_ptr<typename AdaptiveStepSizeIntegrator<ODE>::Instance>(
      new Instance(problem,
                   append_state,
                   tolerance_to_error_ratio,
                   parameters,
                   *this));
}

}  // namespace internal_embedded_explicit_runge_kutta_nyström_integrator

template<typename Method, typename Position>
internal_embedded_explicit_runge_kutta_nyström_integrator::
    EmbeddedExplicitRungeKuttaNyströmIntegrator<Method, Position> const&
EmbeddedExplicitRungeKuttaNyströmIntegrator() {
  static_assert(
      std::is_base_of<methods::EmbeddedExplicitRungeKuttaNyström, Method>::value,
      "Method must be derived from EmbeddedExplicitRungeKuttaNyström");
  static internal_embedded_explicit_runge_kutta_nyström_integrator::
      EmbeddedExplicitRungeKuttaNyströmIntegrator<Method, Position> const
          integrator;
  return integrator;
}

}  // namespace integrators
}  // namespace principia
<commit_msg>Lint.<commit_after>
#pragma once

#include "integrators/embedded_explicit_runge_kutta_nyström_integrator.hpp"

#include <algorithm>
#include <cmath>
#include <ctime>
#include <experimental/optional>
#include <vector>

#include "geometry/sign.hpp"
#include "glog/logging.h"
#include "quantities/quantities.hpp"

namespace principia {
namespace integrators {
namespace internal_embedded_explicit_runge_kutta_nyström_integrator {

using base::make_not_null_unique;
using geometry::Sign;
using numerics::DoublePrecision;
using quantities::DebugString;
using quantities::Difference;
using quantities::Quotient;

template<typename Method, typename Position>
EmbeddedExplicitRungeKuttaNyströmIntegrator<Method, Position>::
EmbeddedExplicitRungeKuttaNyströmIntegrator()
    : AdaptiveStepSizeIntegrator<
          SpecialSecondOrderDifferentialEquation<Position>>(Method::kind) {
  // the first node is always 0 in an explicit method.
  CHECK_EQ(0.0, c_[0]);
  if (first_same_as_last) {
    // Check that the conditions for the FSAL property are satisfied, see for
    // instance Dormand, El-Mikkawy and Prince (1986),
    // Families of Runge-Kutta-Nyström formulae, equation 3.1.
    CHECK_EQ(1.0, c_[stages_ - 1]);
    CHECK_EQ(0.0, b_hat_[stages_ - 1]);
    for (int j = 0; j < stages_ - 1; ++j) {
      CHECK_EQ(b_hat_[j], a_[stages_ - 1][j]);
    }
  }
}

template<typename Method, typename Position>
Status EmbeddedExplicitRungeKuttaNyströmIntegrator<Method, Position>::
Instance::Solve(Instant const& t_final) {
  using Displacement = typename ODE::Displacement;
  using Velocity = typename ODE::Velocity;
  using Acceleration = typename ODE::Acceleration;

  auto const& a = integrator_.a_;
  auto const& b_hat = integrator_.b_hat_;
  auto const& b_prime_hat = integrator_.b_prime_hat_;
  auto const& b = integrator_.b_;
  auto const& b_prime = integrator_.b_prime_;
  auto const& c = integrator_.c_;

  auto& append_state = this->append_state_;
  auto& current_state = this->current_state_;
  auto& first_use = this->first_use_;
  auto& parameters = this->parameters_;
  auto const& equation = this->equation_;

  // |current_state| gets updated as the integration progresses to allow
  // restartability.

  // State before the last, truncated step.
  std::experimental::optional<typename ODE::SystemState> final_state;

  // Argument checks.
  int const dimension = current_state.positions.size();
  Sign const integration_direction = Sign(parameters.first_time_step);
  if (integration_direction.Positive()) {
    // Integrating forward.
    CHECK_LT(current_state.time.value, t_final);
  } else {
    // Integrating backward.
    CHECK_GT(current_state.time.value, t_final);
  }
  CHECK(first_use || !parameters.last_step_is_exact)
      << "Cannot reuse an instance where the last step is exact";
  first_use = false;

  // Time step.  Updated as the integration progresses to allow restartability.
  Time& h = this->time_step_;
  // Current time.  This is a non-const reference whose purpose is to make the
  // equations more readable.
  DoublePrecision<Instant>& t = current_state.time;

  // Position increment (high-order).
  std::vector<Displacement> Δq_hat(dimension);
  // Velocity increment (high-order).
  std::vector<Velocity> Δv_hat(dimension);
  // Current position.  This is a non-const reference whose purpose is to make
  // the equations more readable.
  std::vector<DoublePrecision<Position>>& q_hat = current_state.positions;
  // Current velocity.  This is a non-const reference whose purpose is to make
  // the equations more readable.
  std::vector<DoublePrecision<Velocity>>& v_hat = current_state.velocities;

  // Difference between the low- and high-order approximations.
  typename ODE::SystemStateError error_estimate;
  error_estimate.position_error.resize(dimension);
  error_estimate.velocity_error.resize(dimension);

  // Current Runge-Kutta-Nyström stage.
  std::vector<Position> q_stage(dimension);
  // Accelerations at each stage.
  // TODO(egg): this is a rectangular container, use something more appropriate.
  std::vector<std::vector<Acceleration>> g(stages_);
  for (auto& g_stage : g) {
    g_stage.resize(dimension);
  }

  bool at_end = false;
  double tolerance_to_error_ratio;

  // The first stage of the Runge-Kutta-Nyström iteration.  In the FSAL case,
  // |first_stage == 1| after the first step, since the first RHS evaluation has
  // already occured in the previous step.  In the non-FSAL case and in the
  // first step of the FSAL case, |first_stage == 0|.
  int first_stage = 0;

  // The number of steps already performed.
  std::int64_t step_count = 0;

  Status status;

  // No step size control on the first step.  If this instance is being
  // restarted we already have a value of |h| suitable for the next step, based
  // on the computation of |tolerance_to_error_ratio_| during the last
  // invocation.
  goto runge_kutta_nyström_step;

  while (!at_end) {
    // Compute the next step with decreasing step sizes until the error is
    // tolerable.
    do {
      // Adapt step size.
      // TODO(egg): find out whether there's a smarter way to compute that root,
      // especially since we make the order compile-time.
      h *= parameters.safety_factor *
               std::pow(tolerance_to_error_ratio, 1.0 / (lower_order + 1));
      // TODO(egg): should we check whether it vanishes in double precision
      // instead?
      if (t.value + (t.error + h) == t.value) {
        return Status(termination_condition::VanishingStepSize,
                      "At time " + DebugString(t.value) +
                          ", step size is effectively zero.  Singularity or "
                          "stiff system suspected.");
      }

    runge_kutta_nyström_step:
      // Termination condition.
      if (parameters.last_step_is_exact) {
        Time const time_to_end = (t_final - t.value) - t.error;
        at_end = integration_direction * h >=
                 integration_direction * time_to_end;
        if (at_end) {
          // The chosen step size will overshoot.  Clip it to just reach the
          // end, and terminate if the step is accepted.
          h = time_to_end;
          final_state = current_state;
        }
      }

      // Runge-Kutta-Nyström iteration; fills |g|.
      for (int i = first_stage; i < stages_; ++i) {
        Instant const t_stage = t.value + (t.error + c[i] * h);
        for (int k = 0; k < dimension; ++k) {
          Acceleration Σj_a_ij_g_jk{};
          for (int j = 0; j < i; ++j) {
            Σj_a_ij_g_jk += a[i][j] * g[j][k];
          }
          q_stage[k] = q_hat[k].value +
                           h * (c[i] * v_hat[k].value + h * Σj_a_ij_g_jk);
        }
        status.Update(equation.compute_acceleration(t_stage, q_stage, g[i]));
      }

      // Increment computation and step size control.
      for (int k = 0; k < dimension; ++k) {
        Acceleration Σi_b_hat_i_g_ik{};
        Acceleration Σi_b_i_g_ik{};
        Acceleration Σi_b_prime_hat_i_g_ik{};
        Acceleration Σi_b_prime_i_g_ik{};
        // Please keep the eight assigments below aligned, they become illegible
        // otherwise.
        for (int i = 0; i < stages_; ++i) {
          Σi_b_hat_i_g_ik       += b_hat[i] * g[i][k];
          Σi_b_i_g_ik           += b[i] * g[i][k];
          Σi_b_prime_hat_i_g_ik += b_prime_hat[i] * g[i][k];
          Σi_b_prime_i_g_ik     += b_prime[i] * g[i][k];
        }
        // The hat-less Δq and Δv are the low-order increments.
        Δq_hat[k]               = h * (h * (Σi_b_hat_i_g_ik) + v_hat[k].value);
        Displacement const Δq_k = h * (h * (Σi_b_i_g_ik) + v_hat[k].value);
        Δv_hat[k]               = h * Σi_b_prime_hat_i_g_ik;
        Velocity const Δv_k     = h * Σi_b_prime_i_g_ik;

        error_estimate.position_error[k] = Δq_k - Δq_hat[k];
        error_estimate.velocity_error[k] = Δv_k - Δv_hat[k];
      }
      tolerance_to_error_ratio =
          this->tolerance_to_error_ratio_(h, error_estimate);
    } while (tolerance_to_error_ratio < 1.0);

    if (!parameters.last_step_is_exact && t.value + (t.error + h) > t_final) {
      // We did overshoot.  Drop the point that we just computed and exit.
      final_state = current_state;
      break;
    }

    if (first_same_as_last) {
      using std::swap;
      swap(g.front(), g.back());
      first_stage = 1;
    }

    // Increment the solution with the high-order approximation.
    t.Increment(h);
    for (int k = 0; k < dimension; ++k) {
      q_hat[k].Increment(Δq_hat[k]);
      v_hat[k].Increment(Δv_hat[k]);
    }
    append_state(current_state);
    ++step_count;
    if (step_count == parameters.max_steps && !at_end) {
      return Status(termination_condition::ReachedMaximalStepCount,
                    "Reached maximum step count " +
                        std::to_string(parameters.max_steps) +
                        " at time " + DebugString(t.value) +
                        "; requested t_final is " + DebugString(t_final) +
                        ".");
    }
  }
  // The resolution is restartable from the last non-truncated state.
  CHECK(final_state);
  current_state = *final_state;
  return status;
}

template<typename Method, typename Position>
EmbeddedExplicitRungeKuttaNyströmIntegrator<Method, Position> const&
EmbeddedExplicitRungeKuttaNyströmIntegrator<Method, Position>::
Instance::integrator() const {
  return integrator_;
}

template<typename Method, typename Position>
not_null<std::unique_ptr<typename Integrator<
    SpecialSecondOrderDifferentialEquation<Position>>::Instance>>
EmbeddedExplicitRungeKuttaNyströmIntegrator<Method, Position>::
Instance::Clone() const {
  return std::unique_ptr<Instance>(new Instance(*this));
}

template<typename Method, typename Position>
void EmbeddedExplicitRungeKuttaNyströmIntegrator<Method, Position>::
Instance::WriteToMessage(
    not_null<serialization::IntegratorInstance*> message) const {
  AdaptiveStepSizeIntegrator<ODE>::Instance::WriteToMessage(message);
  auto* const extension =
      message
          ->MutableExtension(
              serialization::AdaptiveStepSizeIntegratorInstance::extension)
          ->MutableExtension(
              serialization::
                  EmbeddedExplicitRungeKuttaNystromIntegratorInstance::
                      extension);
}

template<typename Method, typename Position>
EmbeddedExplicitRungeKuttaNyströmIntegrator<Method, Position>::
Instance::Instance(
    IntegrationProblem<ODE> const& problem,
    AppendState const& append_state,
    ToleranceToErrorRatio const& tolerance_to_error_ratio,
    Parameters const& parameters,
    EmbeddedExplicitRungeKuttaNyströmIntegrator const& integrator)
    : AdaptiveStepSizeIntegrator<ODE>::Instance(
          problem, append_state, tolerance_to_error_ratio, parameters),
      integrator_(integrator) {}

template<typename Method, typename Position>
not_null<std::unique_ptr<typename Integrator<
    SpecialSecondOrderDifferentialEquation<Position>>::Instance>>
EmbeddedExplicitRungeKuttaNyströmIntegrator<Method, Position>::
NewInstance(IntegrationProblem<ODE> const& problem,
            AppendState const& append_state,
            ToleranceToErrorRatio const& tolerance_to_error_ratio,
            Parameters const& parameters) const {
  // Cannot use |make_not_null_unique| because the constructor of |Instance| is
  // private.
  return std::unique_ptr<Instance>(new Instance(problem,
                                                append_state,
                                                tolerance_to_error_ratio,
                                                parameters,
                                                *this));
}

template<typename Method, typename Position>
not_null<std::unique_ptr<typename Integrator<
    SpecialSecondOrderDifferentialEquation<Position>>::Instance>>
EmbeddedExplicitRungeKuttaNyströmIntegrator<Method, Position>::
ReadFromMessage(
    serialization::AdaptiveStepSizeIntegratorInstance const& message,
    IntegrationProblem<ODE> const& problem,
    AppendState const& append_state,
    ToleranceToErrorRatio const& tolerance_to_error_ratio,
    Parameters const& parameters) const {
  CHECK(message.HasExtension(
      serialization::EmbeddedExplicitRungeKuttaNystromIntegratorInstance::
          extension))
      << message.DebugString();

  // Cannot use |make_not_null_unique| because the constructor of |Instance| is
  // private.
  return std::unique_ptr<typename AdaptiveStepSizeIntegrator<ODE>::Instance>(
      new Instance(problem,
                   append_state,
                   tolerance_to_error_ratio,
                   parameters,
                   *this));
}

}  // namespace internal_embedded_explicit_runge_kutta_nyström_integrator

template<typename Method, typename Position>
internal_embedded_explicit_runge_kutta_nyström_integrator::
    EmbeddedExplicitRungeKuttaNyströmIntegrator<Method, Position> const&
EmbeddedExplicitRungeKuttaNyströmIntegrator() {
  static_assert(
      std::is_base_of<methods::EmbeddedExplicitRungeKuttaNyström,
                      Method>::value,
      "Method must be derived from EmbeddedExplicitRungeKuttaNyström");
  static internal_embedded_explicit_runge_kutta_nyström_integrator::
      EmbeddedExplicitRungeKuttaNyströmIntegrator<Method, Position> const
          integrator;
  return integrator;
}

}  // namespace integrators
}  // namespace principia
<|endoftext|>


<commit_before>/*
    Copyright 2005-2007 Adobe Systems Incorporated
    Distributed under the MIT License (see accompanying file LICENSE_1_0_0.txt
    or a copy at http://stlab.adobe.com/licenses.html)
*/

/****************************************************************************************************/

#include <GG/adobe/future/widgets/headers/platform_window.hpp>

#include <GG/adobe/future/widgets/headers/display.hpp>
#include <GG/adobe/future/widgets/headers/widget_utils.hpp>
#include <GG/adobe/future/widgets/headers/platform_label.hpp>
#include <GG/adobe/future/widgets/headers/platform_metrics.hpp>
#include <GG/adobe/future/widgets/headers/platform_widget_utils.hpp>
#include <GG/adobe/keyboard.hpp>

#include <GG/DrawUtil.h>
#include <GG/GUI.h>
#include <GG/StyleFactory.h>
#include <GG/TextControl.h>
#include <GG/Wnd.h>

/****************************************************************************************************/

namespace {

/****************************************************************************************************/

// TODO: Put this into StyleFactory.
class Window :
    public GG::Wnd
{
public:
    static const unsigned int BEVEL = 2;

    Window(adobe::window_t& imp) :
        Wnd(GG::X0, GG::Y0, GG::X1, GG::Y1, imp.flags_m),
        m_imp(imp),
        m_title(0)
        {
            if (!m_imp.name_m.empty()) {
                m_title = adobe::implementation::Factory().NewTextControl(
                    BEVEL_OFFSET.x, BEVEL_OFFSET.y - adobe::implementation::CharHeight(),
                    m_imp.name_m, adobe::implementation::DefaultFont()
                );
                AttachChild(m_title);
            }
        }

    virtual GG::Pt ClientUpperLeft() const
        { return UpperLeft() + BEVEL_OFFSET + GG::Pt(GG::X0, m_title ? m_title->Height() : GG::Y0); }

    virtual GG::Pt ClientLowerRight() const
        { return LowerRight() - BEVEL_OFFSET; }

    virtual GG::WndRegion WindowRegion(const GG::Pt& pt) const
        {
            enum {LEFT = 0, MIDDLE = 1, RIGHT = 2};
            enum {TOP = 0, BOTTOM = 2};

            // window regions look like this:
            // 0111112
            // 3444445   // 4 is client area, 0,2,6,8 are corners
            // 3444445
            // 6777778

            int x_pos = MIDDLE;   // default & typical case is that the mouse is over the (non-border) client area
            int y_pos = MIDDLE;

            GG::Pt ul = UpperLeft() + BEVEL_OFFSET, lr = LowerRight() - BEVEL_OFFSET;

            if (pt.x < ul.x)
                x_pos = LEFT;
            else if (pt.x > lr.x)
                x_pos = RIGHT;

            if (pt.y < ul.y)
                y_pos = TOP;
            else if (pt.y > lr.y)
                y_pos = BOTTOM;

            return (Resizable() ? GG::WndRegion(x_pos + 3 * y_pos) : GG::WR_NONE);
        }

    virtual void SizeMove(const GG::Pt& ul, const GG::Pt& lr)
        {
            GG::Pt client_size = ClientSize();

            if (!m_imp.debounce_m && !m_imp.resize_proc_m.empty()) {
                m_imp.debounce_m = true;

                if (adobe::width(m_imp.place_data_m) != Value(client_size.x) ||
                    adobe::height(m_imp.place_data_m) != Value(client_size.y)) {
                    m_imp.resize_proc_m(Value(client_size.x), Value(client_size.y));

                    adobe::width(m_imp.place_data_m) = Value(client_size.x);
                    adobe::height(m_imp.place_data_m) = Value(client_size.y);
                }

                m_imp.debounce_m = false;
            }

            GG::Wnd::SizeMove(ul, lr);

            GG::Pt new_title_size((LowerRight() - UpperLeft()).x - BEVEL_OFFSET.x * 2, m_title->Height());
            m_title->Resize(new_title_size);
        }

    virtual void Render()
        { GG::BeveledRectangle(UpperLeft(), LowerRight(), GG::CLR_GRAY, GG::CLR_GRAY, true, BEVEL); }

    virtual void KeyPress(GG::Key key, boost::uint32_t key_code_point,
                          GG::Flags<GG::ModKey> mod_keys)
        {
            adobe::keyboard_t::get().dispatch(adobe::key_type(key, key_code_point),
                                              true,
                                              adobe::modifier_state(),
                                              adobe::any_regular_t(this));
        }

    virtual void KeyRelease(GG::Key key, boost::uint32_t key_code_point,
                            GG::Flags<GG::ModKey> mod_keys)
        {
            adobe::keyboard_t::get().dispatch(adobe::key_type(key, key_code_point),
                                              false,
                                              adobe::modifier_state(),
                                              adobe::any_regular_t(this));
        }

private:
    adobe::window_t& m_imp;
    GG::TextControl* m_title;

    static const int FRAME_WIDTH;
    static const GG::Pt BEVEL_OFFSET;
};

const int Window::FRAME_WIDTH = 2;
const GG::Pt Window::BEVEL_OFFSET(GG::X(Window::FRAME_WIDTH), GG::Y(Window::FRAME_WIDTH));

/****************************************************************************************************/

} // namespace

/****************************************************************************************************/

namespace adobe {

/****************************************************************************************************/

window_t::window_t(const std::string&     name,
                   GG::Flags<GG::WndFlag> flags,
                   theme_t                theme) :
    window_m(0),
    flags_m(flags),
    name_m(name),
    theme_m(theme),
    debounce_m(false),
    placed_once_m(false)
{ }

/****************************************************************************************************/

window_t::~window_t()
{ delete window_m; }

/****************************************************************************************************/

void window_t::measure(extents_t& result)
{
    assert(window_m);

    if (name_m.empty())
    {
        result.height() = 15;
        result.width() = 15;

        return;
    }

    // REVISIT (fbrereto) : A lot of static metrics values added here

    boost::shared_ptr<GG::StyleFactory> style = GG::GUI::GetGUI()->GetStyleFactory();
    result = metrics::measure_text(name_m, style->DefaultFont());

    result.width() = static_cast<long>(result.width() * 1.5);
}

/****************************************************************************************************/

void window_t::place(const place_data_t& place_data)
{
    assert(window_m);

    if (placed_once_m)
    {
        set_size(point_2d_t(width(place_data), height(place_data)));
    }
    else
    {
        placed_once_m = true;

        place_data_m = place_data;

        GG::Pt ul(GG::X(left(place_data)), GG::Y(top(place_data)));
        GG::Pt size(
            GG::Pt(GG::X(width(place_data)), GG::Y(height(place_data))) +
            implementation::NonClientSize(*window_m)
        );

        window_m->SetMinSize(size);

        window_m->SizeMove(ul, ul + size);
    }
}

/****************************************************************************************************/

void window_t::set_size(const point_2d_t& size)
{
    assert(window_m);

    if (debounce_m)
        return;

    debounce_m = true;

    width(place_data_m) = size.x_m;
    height(place_data_m) = size.y_m;

    window_m->Resize(
        GG::Pt(GG::X(width(place_data_m)), GG::Y(height(place_data_m))) +
        implementation::NonClientSize(*window_m)
    );

    debounce_m = false;
}

/****************************************************************************************************/

void window_t::reposition()
{
    assert(window_m);

    const GG::X width(window_m->Width());
    const GG::Y height(window_m->Height());

    GG::X app_width(GG::GUI::GetGUI()->AppWidth());
    GG::Y app_height(GG::GUI::GetGUI()->AppHeight());

    GG::X left(std::max(GG::X(10), (app_width - width) / 2));
    GG::Y top(std::max(GG::Y(10), (app_height - height) / 2));

    window_m->MoveTo(GG::Pt(left, top));
}

/****************************************************************************************************/

void window_t::set_visible(bool make_visible)
{
    assert(window_m);

    if (!window_m->Visible())
        reposition();

    set_control_visible(window_m, make_visible);
}

/****************************************************************************************************/

void window_t::monitor_resize(const window_resize_proc_t& proc)
{ resize_proc_m = proc; }

/****************************************************************************************************/

any_regular_t window_t::underlying_handler()
{ return any_regular_t(window_m); }

/****************************************************************************************************/

bool window_t::handle_key(key_type key, bool pressed, modifiers_t modifiers)
{ return false; }

/****************************************************************************************************/

template <>
platform_display_type insert<window_t>(display_t&             display,
                                       platform_display_type& parent,
                                       window_t&              element)
{
    assert(!element.window_m);
    assert(!parent);

    element.window_m = new Window(element);

    return display.insert(parent, element.window_m);
}

/****************************************************************************************************/

} // namespace adobe

/****************************************************************************************************/
<commit_msg>Fixed platform_window.cpp's Window::SizeMove().<commit_after>/*
    Copyright 2005-2007 Adobe Systems Incorporated
    Distributed under the MIT License (see accompanying file LICENSE_1_0_0.txt
    or a copy at http://stlab.adobe.com/licenses.html)
*/

/****************************************************************************************************/

#include <GG/adobe/future/widgets/headers/platform_window.hpp>

#include <GG/adobe/future/widgets/headers/display.hpp>
#include <GG/adobe/future/widgets/headers/widget_utils.hpp>
#include <GG/adobe/future/widgets/headers/platform_label.hpp>
#include <GG/adobe/future/widgets/headers/platform_metrics.hpp>
#include <GG/adobe/future/widgets/headers/platform_widget_utils.hpp>
#include <GG/adobe/keyboard.hpp>

#include <GG/DrawUtil.h>
#include <GG/GUI.h>
#include <GG/StyleFactory.h>
#include <GG/TextControl.h>
#include <GG/Wnd.h>

/****************************************************************************************************/

namespace {

/****************************************************************************************************/

// TODO: Put this into StyleFactory.
class Window :
    public GG::Wnd
{
public:
    static const unsigned int BEVEL = 2;

    Window(adobe::window_t& imp) :
        Wnd(GG::X0, GG::Y0, GG::X1, GG::Y1, imp.flags_m),
        m_imp(imp),
        m_title(0)
        {
            if (!m_imp.name_m.empty()) {
                m_title = adobe::implementation::Factory().NewTextControl(
                    BEVEL_OFFSET.x, BEVEL_OFFSET.y - adobe::implementation::CharHeight(),
                    m_imp.name_m, adobe::implementation::DefaultFont()
                );
                AttachChild(m_title);
            }
        }

    virtual GG::Pt ClientUpperLeft() const
        { return UpperLeft() + BEVEL_OFFSET + GG::Pt(GG::X0, m_title ? m_title->Height() : GG::Y0); }

    virtual GG::Pt ClientLowerRight() const
        { return LowerRight() - BEVEL_OFFSET; }

    virtual GG::WndRegion WindowRegion(const GG::Pt& pt) const
        {
            enum {LEFT = 0, MIDDLE = 1, RIGHT = 2};
            enum {TOP = 0, BOTTOM = 2};

            // window regions look like this:
            // 0111112
            // 3444445   // 4 is client area, 0,2,6,8 are corners
            // 3444445
            // 6777778

            int x_pos = MIDDLE;   // default & typical case is that the mouse is over the (non-border) client area
            int y_pos = MIDDLE;

            GG::Pt ul = UpperLeft() + BEVEL_OFFSET, lr = LowerRight() - BEVEL_OFFSET;

            if (pt.x < ul.x)
                x_pos = LEFT;
            else if (pt.x > lr.x)
                x_pos = RIGHT;

            if (pt.y < ul.y)
                y_pos = TOP;
            else if (pt.y > lr.y)
                y_pos = BOTTOM;

            return (Resizable() ? GG::WndRegion(x_pos + 3 * y_pos) : GG::WR_NONE);
        }

    virtual void SizeMove(const GG::Pt& ul, const GG::Pt& lr)
        {
            GG::Wnd::SizeMove(ul, lr);

            GG::Pt client_size = ClientSize();

            if (!m_imp.debounce_m && !m_imp.resize_proc_m.empty()) {
                m_imp.debounce_m = true;

                if (adobe::width(m_imp.place_data_m) != Value(client_size.x) ||
                    adobe::height(m_imp.place_data_m) != Value(client_size.y)) {
                    m_imp.resize_proc_m(Value(client_size.x), Value(client_size.y));

                    adobe::width(m_imp.place_data_m) = Value(client_size.x);
                    adobe::height(m_imp.place_data_m) = Value(client_size.y);
                }

                m_imp.debounce_m = false;
            }

            GG::Pt new_title_size((LowerRight() - UpperLeft()).x - BEVEL_OFFSET.x * 2, m_title->Height());
            m_title->Resize(new_title_size);
        }

    virtual void Render()
        { GG::BeveledRectangle(UpperLeft(), LowerRight(), GG::CLR_GRAY, GG::CLR_GRAY, true, BEVEL); }

    virtual void KeyPress(GG::Key key, boost::uint32_t key_code_point,
                          GG::Flags<GG::ModKey> mod_keys)
        {
            adobe::keyboard_t::get().dispatch(adobe::key_type(key, key_code_point),
                                              true,
                                              adobe::modifier_state(),
                                              adobe::any_regular_t(this));
        }

    virtual void KeyRelease(GG::Key key, boost::uint32_t key_code_point,
                            GG::Flags<GG::ModKey> mod_keys)
        {
            adobe::keyboard_t::get().dispatch(adobe::key_type(key, key_code_point),
                                              false,
                                              adobe::modifier_state(),
                                              adobe::any_regular_t(this));
        }

private:
    adobe::window_t& m_imp;
    GG::TextControl* m_title;

    static const int FRAME_WIDTH;
    static const GG::Pt BEVEL_OFFSET;
};

const int Window::FRAME_WIDTH = 2;
const GG::Pt Window::BEVEL_OFFSET(GG::X(Window::FRAME_WIDTH), GG::Y(Window::FRAME_WIDTH));

/****************************************************************************************************/

} // namespace

/****************************************************************************************************/

namespace adobe {

/****************************************************************************************************/

window_t::window_t(const std::string&     name,
                   GG::Flags<GG::WndFlag> flags,
                   theme_t                theme) :
    window_m(0),
    flags_m(flags),
    name_m(name),
    theme_m(theme),
    debounce_m(false),
    placed_once_m(false)
{ }

/****************************************************************************************************/

window_t::~window_t()
{ delete window_m; }

/****************************************************************************************************/

void window_t::measure(extents_t& result)
{
    assert(window_m);

    if (name_m.empty())
    {
        result.height() = 15;
        result.width() = 15;

        return;
    }

    // REVISIT (fbrereto) : A lot of static metrics values added here

    boost::shared_ptr<GG::StyleFactory> style = GG::GUI::GetGUI()->GetStyleFactory();
    result = metrics::measure_text(name_m, style->DefaultFont());

    result.width() = static_cast<long>(result.width() * 1.5);
}

/****************************************************************************************************/

void window_t::place(const place_data_t& place_data)
{
    assert(window_m);

    if (placed_once_m)
    {
        set_size(point_2d_t(width(place_data), height(place_data)));
    }
    else
    {
        placed_once_m = true;

        place_data_m = place_data;

        GG::Pt ul(GG::X(left(place_data)), GG::Y(top(place_data)));
        GG::Pt size(
            GG::Pt(GG::X(width(place_data)), GG::Y(height(place_data))) +
            implementation::NonClientSize(*window_m)
        );

        window_m->SetMinSize(size);

        window_m->SizeMove(ul, ul + size);
    }
}

/****************************************************************************************************/

void window_t::set_size(const point_2d_t& size)
{
    assert(window_m);

    if (debounce_m)
        return;

    debounce_m = true;

    width(place_data_m) = size.x_m;
    height(place_data_m) = size.y_m;

    window_m->Resize(
        GG::Pt(GG::X(width(place_data_m)), GG::Y(height(place_data_m))) +
        implementation::NonClientSize(*window_m)
    );

    debounce_m = false;
}

/****************************************************************************************************/

void window_t::reposition()
{
    assert(window_m);

    const GG::X width(window_m->Width());
    const GG::Y height(window_m->Height());

    GG::X app_width(GG::GUI::GetGUI()->AppWidth());
    GG::Y app_height(GG::GUI::GetGUI()->AppHeight());

    GG::X left(std::max(GG::X(10), (app_width - width) / 2));
    GG::Y top(std::max(GG::Y(10), (app_height - height) / 2));

    window_m->MoveTo(GG::Pt(left, top));
}

/****************************************************************************************************/

void window_t::set_visible(bool make_visible)
{
    assert(window_m);

    if (!window_m->Visible())
        reposition();

    set_control_visible(window_m, make_visible);
}

/****************************************************************************************************/

void window_t::monitor_resize(const window_resize_proc_t& proc)
{ resize_proc_m = proc; }

/****************************************************************************************************/

any_regular_t window_t::underlying_handler()
{ return any_regular_t(window_m); }

/****************************************************************************************************/

bool window_t::handle_key(key_type key, bool pressed, modifiers_t modifiers)
{ return false; }

/****************************************************************************************************/

template <>
platform_display_type insert<window_t>(display_t&             display,
                                       platform_display_type& parent,
                                       window_t&              element)
{
    assert(!element.window_m);
    assert(!parent);

    element.window_m = new Window(element);

    return display.insert(parent, element.window_m);
}

/****************************************************************************************************/

} // namespace adobe

/****************************************************************************************************/
<|endoftext|>