<commit_before>#ifndef STOCHASTIC_NODE
#define STOCHASTIC_NODE
#include <memory>
#include <vector>
#include <future>
#include <numeric>
#include <map>
#include "error_handler.hpp"
#include "mcmc_traits.hpp"
#include "arms.hpp"
#include "slicer.hpp"
#include "base_urand.hpp"
#include "discrete_sample.hpp"
#include "continuous_sample.hpp"
#include "node.hpp"
#include "ptransform.hpp"
namespace mcmc_utilities
{
  template <typename T,template <typename TE> class T_vector>
  class stochastic_node
    :public node<T,T_vector>
  {
  private:
    T_vector<T> v;//store current value(s) of this node
    T_vector<int> observed;
    size_t current_idx;
  public:
    stochastic_node(size_t nparents,const T_vector<T>& v_)
      :node<T,T_vector>(nparents,get_size(v_)),
      v{v_},observed(get_size(v_)),current_idx(0)
    {
    }

    stochastic_node(size_t nparents,T v_)
      :node<T,T_vector>(nparents,1),
      v(1),observed(get_size(v)),current_idx(0)
    {
      //v[0]=v_;
      set_element(v,0,v_);
    }

    stochastic_node()=delete;
    stochastic_node(const stochastic_node<T,T_vector>& )=delete;
    stochastic_node<T,T_vector>& operator=(const stochastic_node<T,T_vector>&)=delete;
  public:
    bool is_observed(size_t n)const
    {
      //return observed[n]!=0;
      return get_element(observed,n)!=0;
    }

    void set_observed(size_t n,bool b)
    {
      //observed[n]=b?1:0;
      set_element(observed,n,b?1:0);
    }

    size_t num_of_observed()const
    {
      return std::accumulate(std::begin(observed),
			     std::end(observed),
			     0,
			     [](int x1,int x2){return x1+x2;});
    }

    size_t num_of_unobserved()const
    {
      return this->num_of_dims()-num_of_observed();
    }

    T log_posterior_prob()const
    {
      return log_prob()+this->log_likelihood();
    }

    T eval_log(const T& x)const
    {
      set_element(const_cast<stochastic_node*>(this)->v,current_idx,x);
      return log_posterior_prob();
    }

    void set_current_idx(size_t i)
    {
      current_idx=i;
    }

    size_t get_current_idx()const
    {
      return current_idx;
    }

    T log_likelihood()const
    {
      T result=static_cast<T>(0);
      auto& ss=this->get_all_stochastic_children();
      T_vector<T> results(get_size(ss));
      ////followings are the current implementation of computing the log likelihood
      //
#if defined USE_STD_TRANSFORM
      std::transform(std::begin(ss),std::end(ss),std::begin(results),[](const stochastic_node<T,T_vector>* const& p){return p->log_prob();});
#elif defined USE_TRANSFORM_THREAD
      pvec_transform_thread(ss,results,[](const stochastic_node<T,T_vector>* const& p){return p->log_prob();},10);
#else
      pvec_transform_omp(ss,results,[](const stochastic_node<T,T_vector>* const& p){return p->log_prob();},10);
#endif
      result=std::accumulate(std::begin(results),std::end(results),static_cast<T>(0));
      return result;
    }

    T log_prob()const
    {
      return do_log_prob();
    }

    void set_value(size_t idx,const T& v_)
    {
      this->set_initialized(idx,true);
      set_element(v,idx,this->regulate(idx,v_));
    }

    T regulate(size_t idx,const T& x)const
    {
      return do_regulate(idx,x);
    }

  public:
    void sample(base_urand<T>& urand)
    {
      do_sample(urand);
    }

  public:
    void default_sample(base_urand<T>& urand)
    {
      for(size_t i=0;i<this->num_of_dims();++i)
	{
	  if(is_observed(i))
	    {
	      continue;
	    }
	  this->set_current_idx(i);
	  T xprev=this->value(i);
	  std::pair<T,T> xrange(this->do_var_range());

	  //xprev=xprev<xrange.first?xrange.first:xprev;
	  //xprev=xprev>xrange.second?xrange.second:xprev;

	  if(xprev<xrange.first||xprev>xrange.second)
	    {
	      this->init_value(i);
	      xprev=this->value(i);
	    }

	  //arms_simple(*this,xprev,xsamp,dometrop(),rnd);

	  if(is_continuous(i))
	    {
	      T_vector<T> init_x(this->do_init_points());
	      xprev=continuous_sample([&](const T& x){return this->eval_log(x);},xrange,init_x, xprev,1,urand);
	    }
	  else
	    {
	      T_vector<T> candidate_points(this->do_candidate_points());
	      xprev=discrete_sample([&](const T& x){return this->eval_log(x);},xrange, candidate_points, xprev,10,urand);
	    }

	  this->set_value(i,xprev);
	}
    }

  private:
    virtual void do_sample(base_urand<T>& urand)
    {
      default_sample(urand);
    }

    virtual T do_log_prob()const=0;
    T do_value(size_t idx)const override final
    {
      //return v[idx];
      return get_element(v,idx);
    }

    void do_connect_to_parent(node<T,T_vector>*  rhs,size_t n,size_t idx) override
    {
      //set_element(this->parents,n,std::make_pair(rhs,idx));
      this->set_parent(n,std::make_pair(rhs,idx));
      rhs->add_stochastic_child(this);
    }

    virtual T do_regulate(size_t idx,const T& x)const
    {
      return x;
    }

    virtual bool is_continuous(size_t idx)const=0;

    virtual std::pair<T,T> do_var_range()const=0;

    virtual T_vector<T> do_init_points()const
    {
      T_vector<T> result(5);
      std::pair<T,T> xrange(do_var_range());
      T xl=xrange.first,xr=xrange.second;
      for(size_t n=0;n<get_size(result);++n)
	{
	  set_element(result,n,xl+(xr-xl)/(get_size(result)+1)*(n+1));
	}
      return result;
    }

    virtual T_vector<T> do_candidate_points()const
    {
      return T_vector<T>();
    }
  };
}

#endif
<commit_msg>	修改：     core/stochastic_node.hpp<commit_after>#ifndef STOCHASTIC_NODE
#define STOCHASTIC_NODE
#include <memory>
#include <vector>
#include <future>
#include <numeric>
#include <map>
#include "error_handler.hpp"
#include "mcmc_traits.hpp"
#include "arms.hpp"
#include "slicer.hpp"
#include "base_urand.hpp"
#include "discrete_sample.hpp"
#include "continuous_sample.hpp"
#include "node.hpp"
#include "ptransform.hpp"
namespace mcmc_utilities
{
  template <typename T,template <typename TE> class T_vector>
  class stochastic_node
    :public node<T,T_vector>
  {
  private:
    T_vector<T> v;//store current value(s) of this node
    T_vector<int> observed;
    size_t current_idx;
  public:
    stochastic_node(size_t nparents,const T_vector<T>& v_)
      :node<T,T_vector>(nparents,get_size(v_)),
      v{v_},observed(get_size(v_)),current_idx(0)
    {
    }

    stochastic_node(size_t nparents,T v_)
      :node<T,T_vector>(nparents,1),
      v(1),observed(get_size(v)),current_idx(0)
    {
      //v[0]=v_;
      set_element(v,0,v_);
    }

    stochastic_node()=delete;
    stochastic_node(const stochastic_node<T,T_vector>& )=delete;
    stochastic_node<T,T_vector>& operator=(const stochastic_node<T,T_vector>&)=delete;
  public:
    bool is_observed(size_t n)const
    {
      //return observed[n]!=0;
      return get_element(observed,n)!=0;
    }

    void set_observed(size_t n,bool b)
    {
      //observed[n]=b?1:0;
      set_element(observed,n,b?1:0);
    }

    size_t num_of_observed()const
    {
      return std::accumulate(std::begin(observed),
			     std::end(observed),
			     0,
			     [](int x1,int x2){return x1+x2;});
    }

    size_t num_of_unobserved()const
    {
      return this->num_of_dims()-num_of_observed();
    }

    T log_posterior_prob()const
    {
      return log_prob()+this->log_likelihood();
    }

    T eval_log(const T& x)const
    {
      set_element(const_cast<stochastic_node*>(this)->v,current_idx,x);
      return log_posterior_prob();
    }

    void set_current_idx(size_t i)
    {
      current_idx=i;
    }

    size_t get_current_idx()const
    {
      return current_idx;
    }

    T log_likelihood()const
    {
      T result=static_cast<T>(0);
      auto& ss=this->get_all_stochastic_children();
      T_vector<T> results(get_size(ss));
      ////followings are the current implementation of computing the log likelihood
      //
#if defined USE_OMP_TRANSFORM
      pvec_transform_omp(ss,results,[](const stochastic_node<T,T_vector>* const& p){return p->log_prob();},10);
#elif defined USE_THREAD_TRANSFORM
      pvec_transform_thread(ss,results,[](const stochastic_node<T,T_vector>* const& p){return p->log_prob();},10);
#else
      std::transform(std::begin(ss),std::end(ss),std::begin(results),[](const stochastic_node<T,T_vector>* const& p){return p->log_prob();});
#endif
      result=std::accumulate(std::begin(results),std::end(results),static_cast<T>(0));
      return result;
    }

    T log_prob()const
    {
      return do_log_prob();
    }

    void set_value(size_t idx,const T& v_)
    {
      this->set_initialized(idx,true);
      set_element(v,idx,this->regulate(idx,v_));
    }

    T regulate(size_t idx,const T& x)const
    {
      return do_regulate(idx,x);
    }

  public:
    void sample(base_urand<T>& urand)
    {
      do_sample(urand);
    }

  public:
    void default_sample(base_urand<T>& urand)
    {
      for(size_t i=0;i<this->num_of_dims();++i)
	{
	  if(is_observed(i))
	    {
	      continue;
	    }
	  this->set_current_idx(i);
	  T xprev=this->value(i);
	  std::pair<T,T> xrange(this->do_var_range());

	  //xprev=xprev<xrange.first?xrange.first:xprev;
	  //xprev=xprev>xrange.second?xrange.second:xprev;

	  if(xprev<xrange.first||xprev>xrange.second)
	    {
	      this->init_value(i);
	      xprev=this->value(i);
	    }

	  //arms_simple(*this,xprev,xsamp,dometrop(),rnd);

	  if(is_continuous(i))
	    {
	      T_vector<T> init_x(this->do_init_points());
	      xprev=continuous_sample([&](const T& x){return this->eval_log(x);},xrange,init_x, xprev,1,urand);
	    }
	  else
	    {
	      T_vector<T> candidate_points(this->do_candidate_points());
	      xprev=discrete_sample([&](const T& x){return this->eval_log(x);},xrange, candidate_points, xprev,10,urand);
	    }

	  this->set_value(i,xprev);
	}
    }

  private:
    virtual void do_sample(base_urand<T>& urand)
    {
      default_sample(urand);
    }

    virtual T do_log_prob()const=0;
    T do_value(size_t idx)const override final
    {
      //return v[idx];
      return get_element(v,idx);
    }

    void do_connect_to_parent(node<T,T_vector>*  rhs,size_t n,size_t idx) override
    {
      //set_element(this->parents,n,std::make_pair(rhs,idx));
      this->set_parent(n,std::make_pair(rhs,idx));
      rhs->add_stochastic_child(this);
    }

    virtual T do_regulate(size_t idx,const T& x)const
    {
      return x;
    }

    virtual bool is_continuous(size_t idx)const=0;

    virtual std::pair<T,T> do_var_range()const=0;

    virtual T_vector<T> do_init_points()const
    {
      T_vector<T> result(5);
      std::pair<T,T> xrange(do_var_range());
      T xl=xrange.first,xr=xrange.second;
      for(size_t n=0;n<get_size(result);++n)
	{
	  set_element(result,n,xl+(xr-xl)/(get_size(result)+1)*(n+1));
	}
      return result;
    }

    virtual T_vector<T> do_candidate_points()const
    {
      return T_vector<T>();
    }
  };
}

#endif
<|endoftext|>


<commit_before>/*
 * sample_nodelet_class.cpp
 *
 *  Created on: 2016/10/30
 *      Author: dandelion1124
 */
#include "sample_nodelet_class.h"
#include <pluginlib/class_list_macros.h>

namespace sample_nodelet_ns
{
SampleNodeletClass::SampleNodeletClass()
{
    ROS_INFO("SampleNodeletClass Constructor");
}

SampleNodeletClass::~SampleNodeletClass()
{
    ROS_INFO("SampleNodeletClass Destructor");
}

void SampleNodeletClass::onInit()
{
    nh = getNodeHandle();
    timer = nh.createTimer(ros::Duration(10.0), boost::bind(&SampleNodeletClass::timerCb, this));
    NODELET_INFO("SampleNodeletClass - %s", __FUNCTION__);
}

void SampleNodeletClass::timerCb()
{
    NODELET_INFO("SampleNodeletClass - %s", __FUNCTION__);
}
} // namespace sample_nodelet_ns

PLUGINLIB_EXPORT_CLASS(sample_nodelet_ns::SampleNodeletClass, nodelet::Nodelet)
<commit_msg>timerの時間を変更<commit_after>/*
 * sample_nodelet_class.cpp
 *
 *  Created on: 2016/10/30
 *      Author: dandelion1124
 */
#include "sample_nodelet_class.h"
#include <pluginlib/class_list_macros.h>

namespace sample_nodelet_ns
{
SampleNodeletClass::SampleNodeletClass()
{
    ROS_INFO("SampleNodeletClass Constructor");
}

SampleNodeletClass::~SampleNodeletClass()
{
    ROS_INFO("SampleNodeletClass Destructor");
}

void SampleNodeletClass::onInit()
{
    nh = getNodeHandle();
    timer = nh.createTimer(ros::Duration(1.0), boost::bind(&SampleNodeletClass::timerCb, this));
    NODELET_INFO("SampleNodeletClass - %s", __FUNCTION__);
}

void SampleNodeletClass::timerCb()
{
    NODELET_INFO("SampleNodeletClass - %s", __FUNCTION__);
}
} // namespace sample_nodelet_ns

PLUGINLIB_EXPORT_CLASS(sample_nodelet_ns::SampleNodeletClass, nodelet::Nodelet)
<|endoftext|>


<commit_before>#include "GameScreenState.h"
#include <algorithm>
#include <SFML/Graphics.hpp>

//const int TOP = 1;
//const int BOTTOM = 2;
//const int LEFT = 4;
//const int RIGHT = 8;
const unsigned char TOP = 0;
const unsigned char BOTTOM = 1;
const unsigned char LEFT = 2;
const unsigned char RIGHT = 3;

inline float clamp(float x, float min, float max) {
	 return std::max(std::min(x, max), min);
}

GameScreenState::GameScreenState() {
	pos = sf::Vector2f(300, 100);
	velocity = sf::Vector2f(0, 0);
	acceleration = sf::Vector2f(0, 0);
	level_bbox = rectangle(0, 0, 3000, 600);
	for (unsigned char i = 0; i < 4; i++) touching_walls[i] = 0;
	level.push_back(rectangle(200, 400, 400, 450, 2));
	level.push_back(rectangle(400, 260, 450, 450, 3));
	level.push_back(rectangle(100, 200, 200, 450, 1));
	level.push_back(rectangle(95, 200, 100, 455, 2));
	level.push_back(rectangle(200, 200, 205, 455, 2));
	level.push_back(rectangle(100, 450, 200, 455, 2));
	level.push_back(rectangle(600, 450, 1000, 500, 2));
	level.push_back(rectangle(1200, 300, 2500, 350, 2));

	level.push_back(rectangle(0, 0, 3000, 2, 3));
	level.push_back(rectangle(0, 0, 2, 600, 3));
	level.push_back(rectangle(2998, 0, 3000, 600, 3));
	level.push_back(rectangle(0, 598, 3000, 600, 3));
	updateSprites();
}

void GameScreenState::event(const sf::Event& event) {

}

void GameScreenState::updateSprites() {
	sprites.clear();
	sprites.resize(level.size() + 1);
	static sf::Color colors[4] = {sf::Color::White, sf::Color::Blue, sf::Color::Black, sf::Color::Yellow};
	for (unsigned int i = 0; i < level.size(); i++) {
		sf::RectangleShape sprite;
		//sf::Sprite sprite;
		//sf::Texture tex;
		sprite.setSize(level[i].maxp - level[i].minp);
		sprite.setFillColor(colors[level[i].color]);
		//sprite.setTextureRect(sf::IntRect(0, 0,
		//	(int)(level[i].maxp.x - level[i].minp.x),
		//	(int)(level[i].maxp.y - level[i].minp.y)));
		//tex.create((int)(level[i].maxp.x - level[i].minp.x),
		//			(int)(level[i].maxp.y - level[i].minp.y));
		//sprite.setTexture(tex);
		sprite.setPosition(level[i].minp);
		sprites.push_back(sprite);
	}
	sf::RectangleShape sprite;
	sprite.setSize(sf::Vector2f(40, 40));
	sprite.setFillColor(sf::Color::Red);
	sprite.setPosition(pos - sf::Vector2f(20, 20));
	sprites.push_back(sprite);
}

void GameScreenState::render(sf::RenderTarget& target) {
	updateSprites();
	sf::View view = target.getDefaultView();
	sf::Vector2f size = view.getSize();
	sf::Vector2f center
		(clamp(pos.x, level_bbox.minp.x + size.x / 2,
			level_bbox.maxp.x - size.x / 2),
		clamp(pos.y, level_bbox.minp.y + size.y / 2,
			level_bbox.maxp.y - size.y / 2));
	view.setCenter(center);
	target.setView(view);
	target.clear(sf::Color::White);
	for (unsigned int i = 0; i < sprites.size(); i++) {
		target.draw(sprites[i]);
	}
}

void GameScreenState::update(const sf::Time& time) {
	float s = time.asSeconds();
	pos += velocity * s + ((float) 0.5) * s * s * acceleration;
	sf::Vector2f oldv = velocity + ((float) 0.5) * s * acceleration;
	velocity += s * acceleration;
	acceleration = sf::Vector2f(0, 240);
	if (sf::Keyboard::isKeyPressed(sf::Keyboard::Right)) {
		velocity.x = std::min(velocity.x + 400 * s, (float)200);
    } else if (sf::Keyboard::isKeyPressed(sf::Keyboard::Left)) {
		velocity.x = std::max(velocity.x - 400 * s, (float)-200);
	} else {
		//velocity.x = 0;
	}
	if (sf::Keyboard::isKeyPressed(sf::Keyboard::Up)) {
		switch (touching_walls[BOTTOM]) {
			case 1:
				velocity.y -= 500 * s;
				break;
			case 2:
			case 3:
				velocity.y -= 150;
		}
	}
	velocity.x = std::max(std::min(velocity.x, (float)1500), (float)-1500);
	velocity.y = std::max(std::min(velocity.y, (float)1500), (float)-1500);
	rectangle me_ex = rectangle(pos - sf::Vector2f(20, 19.9), pos + sf::Vector2f(20, 19.9));
	rectangle me_ey = rectangle(pos - sf::Vector2f(19.9, 20), pos + sf::Vector2f(19.9, 20));
	for (unsigned char i = 0; i < 4; i++) touching_walls[i] = 0;
	for (unsigned int i = 0; i < level.size(); i++) {
		if (level[i].color < 2) continue; // Not solid
		if (level[i].intersects(me_ex)) {
			if (me_ey.maxp.x >= level[i].maxp.x) {
				if (me_ey.minp.x >= level[i].maxp.x + oldv.x * s) {
					float d = std::max(level[i].maxp.x - me_ex.minp.x, (float)0);
					pos.x += d;
					me_ex.movex(d);
					me_ey.movex(d);
				}
			}
			if (me_ey.minp.x <= level[i].minp.x) {
				if (me_ey.maxp.x <= level[i].minp.x + oldv.x * s) {
					float d = std::min(level[i].minp.x - me_ex.maxp.x, (float)0);
					pos.x += d;
					me_ex.movex(d);
					me_ey.movex(d);
				}
			}
		}
		if (level[i].intersects(me_ey)) {
			if (me_ex.maxp.y >= level[i].maxp.y) {
				if (me_ex.minp.y >= level[i].maxp.y + oldv.y * s) {
					float d = std::max(level[i].maxp.y - me_ey.minp.y, (float)0);
					pos.y += d;
					me_ex.movey(d);
					me_ey.movey(d);
				}
			}
			if (me_ex.minp.y <= level[i].minp.y) {
				if (me_ex.maxp.y <= level[i].minp.y + oldv.y * s) {
					float d = std::min(level[i].minp.y - me_ey.maxp.y, (float)0);
					pos.y += d;
					me_ex.movey(d);
					me_ey.movey(d);
				}
			}
		}
	}

	for (unsigned int i = 0; i < level.size(); i++) {
		if (level[i].intersects(me_ex)) {
			if (me_ey.minp.x >= level[i].minp.x) {
				touching_walls[LEFT] = std::max(touching_walls[LEFT], level[i].color);
			}
			if (me_ey.maxp.x <= level[i].maxp.x) {
				touching_walls[RIGHT] = std::max(touching_walls[RIGHT], level[i].color);
			}
		}
		if (level[i].intersects(me_ey)) {
			if (me_ex.minp.y >= level[i].minp.y) {
				touching_walls[TOP] = std::max(touching_walls[TOP], level[i].color);
			}
			if (me_ex.maxp.y <= level[i].maxp.y) {
				touching_walls[BOTTOM] = std::max(touching_walls[BOTTOM], level[i].color);
			}
		}
	}
	float visc = 0.01;
	switch (touching_walls[LEFT]) {
		case 1:
			if (velocity.x < 0) velocity.x *= pow(visc, s);
			break;
		case 2:
			velocity.x = std::max(velocity.x, (float)0);
			acceleration.x = std::max(acceleration.x, (float)0);
			break;
		case 3:
			velocity.x = std::max(velocity.x, -velocity.x);
			acceleration.x = std::max(acceleration.x, (float)0);
			break;
		default: break;
	}
	switch (touching_walls[RIGHT]) {
		case 1:
			if (velocity.x > 0) velocity.x *= pow(visc, s);
			break;
		case 2:
			velocity.x = std::min(velocity.x, (float)0);
			acceleration.x = std::min(acceleration.x, (float)0);
			break;
		case 3:
			velocity.x = std::min(velocity.x, -velocity.x);
			acceleration.x = std::min(acceleration.x, (float)0);
			break;
		default: break;
	}
	switch (touching_walls[TOP]) {
		case 1:
			if (velocity.y < 0) velocity.y *= pow(visc, s);
			break;
		case 2:
			velocity.y = std::max(velocity.y, (float)0);
			acceleration.y = std::max(acceleration.y, (float)0);
			break;
		case 3:
			velocity.y = std::max(velocity.y, -velocity.y);
			acceleration.y = std::max(acceleration.y, (float)0);
			break;
		default: break;
	}
	switch (touching_walls[BOTTOM]) {
		case 1:
			if (velocity.y > 0) velocity.y *= pow(visc, s);
			break;
		case 2:
			velocity.y = std::min(velocity.y, (float)0);
			acceleration.y = std::min(acceleration.y, (float)0);
			break;
		case 3:
			velocity.y = std::min(velocity.y, -velocity.y);
			acceleration.y = std::min(acceleration.y, (float)0);
			break;
		default: break;
	}
}
<commit_msg>Physics part VII: add TIME_SPEED<commit_after>#include "GameScreenState.h"
#include <algorithm>
#include <SFML/Graphics.hpp>

//const int TOP = 1;
//const int BOTTOM = 2;
//const int LEFT = 4;
//const int RIGHT = 8;
const unsigned char TOP = 0;
const unsigned char BOTTOM = 1;
const unsigned char LEFT = 2;
const unsigned char RIGHT = 3;

inline float clamp(float x, float min, float max) {
	 return std::max(std::min(x, max), min);
}

GameScreenState::GameScreenState() {
	pos = sf::Vector2f(300, 100);
	velocity = sf::Vector2f(0, 0);
	acceleration = sf::Vector2f(0, 0);
	level_bbox = rectangle(0, 0, 3000, 600);
	for (unsigned char i = 0; i < 4; i++) touching_walls[i] = 0;
	level.push_back(rectangle(200, 400, 400, 450, 2));
	level.push_back(rectangle(400, 260, 450, 450, 3));
	level.push_back(rectangle(100, 200, 200, 450, 1));
	level.push_back(rectangle(95, 200, 100, 455, 2));
	level.push_back(rectangle(200, 200, 205, 455, 2));
	level.push_back(rectangle(100, 450, 200, 455, 2));
	level.push_back(rectangle(600, 450, 1000, 500, 2));
	level.push_back(rectangle(1200, 300, 2500, 350, 2));

	level.push_back(rectangle(0, 0, 3000, 2, 3));
	level.push_back(rectangle(0, 0, 2, 600, 3));
	level.push_back(rectangle(2998, 0, 3000, 600, 3));
	level.push_back(rectangle(0, 598, 3000, 600, 3));
	updateSprites();
}

void GameScreenState::event(const sf::Event& event) {

}

void GameScreenState::updateSprites() {
	sprites.clear();
	sprites.resize(level.size() + 1);
	static sf::Color colors[4] = {sf::Color::White, sf::Color::Blue, sf::Color::Black, sf::Color::Yellow};
	for (unsigned int i = 0; i < level.size(); i++) {
		sf::RectangleShape sprite;
		//sf::Sprite sprite;
		//sf::Texture tex;
		sprite.setSize(level[i].maxp - level[i].minp);
		sprite.setFillColor(colors[level[i].color]);
		//sprite.setTextureRect(sf::IntRect(0, 0,
		//	(int)(level[i].maxp.x - level[i].minp.x),
		//	(int)(level[i].maxp.y - level[i].minp.y)));
		//tex.create((int)(level[i].maxp.x - level[i].minp.x),
		//			(int)(level[i].maxp.y - level[i].minp.y));
		//sprite.setTexture(tex);
		sprite.setPosition(level[i].minp);
		sprites.push_back(sprite);
	}
	sf::RectangleShape sprite;
	sprite.setSize(sf::Vector2f(40, 40));
	sprite.setFillColor(sf::Color::Red);
	sprite.setPosition(pos - sf::Vector2f(20, 20));
	sprites.push_back(sprite);
}

void GameScreenState::render(sf::RenderTarget& target) {
	updateSprites();
	sf::View view = target.getDefaultView();
	sf::Vector2f size = view.getSize();
	sf::Vector2f center
		(clamp(pos.x, level_bbox.minp.x + size.x / 2,
			level_bbox.maxp.x - size.x / 2),
		clamp(pos.y, level_bbox.minp.y + size.y / 2,
			level_bbox.maxp.y - size.y / 2));
	view.setCenter(center);
	target.setView(view);
	target.clear(sf::Color::White);
	for (unsigned int i = 0; i < sprites.size(); i++) {
		target.draw(sprites[i]);
	}
}

const float TIME_SPEED = 3;
void GameScreenState::update(const sf::Time& time) {
	float s = time.asSeconds() * TIME_SPEED;
	pos += velocity * s + ((float) 0.5) * s * s * acceleration;
	sf::Vector2f oldv = velocity + ((float) 0.5) * s * acceleration;
	velocity += s * acceleration;
	acceleration = sf::Vector2f(0, 240);
	if (sf::Keyboard::isKeyPressed(sf::Keyboard::Right)) {
		velocity.x = std::min(velocity.x + (float)400 * s, (float)200);
    } else if (sf::Keyboard::isKeyPressed(sf::Keyboard::Left)) {
		velocity.x = std::max(velocity.x - (float)400 * s, (float)-200);
	} else {
		//velocity.x = 0;
	}
	if (sf::Keyboard::isKeyPressed(sf::Keyboard::Up)) {
		switch (touching_walls[BOTTOM]) {
			case 1:
				velocity.y -= 500 * s;
				break;
			case 2:
			case 3:
				velocity.y -= 150;
		}
	}
	velocity.x = std::max(std::min(velocity.x, (float)1500), (float)-1500);
	velocity.y = std::max(std::min(velocity.y, (float)1500), (float)-1500);
	rectangle me_ex = rectangle(pos - sf::Vector2f(20, 19.9), pos + sf::Vector2f(20, 19.9));
	rectangle me_ey = rectangle(pos - sf::Vector2f(19.9, 20), pos + sf::Vector2f(19.9, 20));
	for (unsigned char i = 0; i < 4; i++) touching_walls[i] = 0;
	for (unsigned int i = 0; i < level.size(); i++) {
		if (level[i].color < 2) continue; // Not solid
		if (level[i].intersects(me_ex)) {
			if (me_ey.maxp.x >= level[i].maxp.x) {
				if (me_ey.minp.x >= level[i].maxp.x + oldv.x * s) {
					float d = std::max(level[i].maxp.x - me_ex.minp.x, (float)0);
					pos.x += d;
					me_ex.movex(d);
					me_ey.movex(d);
				}
			}
			if (me_ey.minp.x <= level[i].minp.x) {
				if (me_ey.maxp.x <= level[i].minp.x + oldv.x * s) {
					float d = std::min(level[i].minp.x - me_ex.maxp.x, (float)0);
					pos.x += d;
					me_ex.movex(d);
					me_ey.movex(d);
				}
			}
		}
		if (level[i].intersects(me_ey)) {
			if (me_ex.maxp.y >= level[i].maxp.y) {
				if (me_ex.minp.y >= level[i].maxp.y + oldv.y * s) {
					float d = std::max(level[i].maxp.y - me_ey.minp.y, (float)0);
					pos.y += d;
					me_ex.movey(d);
					me_ey.movey(d);
				}
			}
			if (me_ex.minp.y <= level[i].minp.y) {
				if (me_ex.maxp.y <= level[i].minp.y + oldv.y * s) {
					float d = std::min(level[i].minp.y - me_ey.maxp.y, (float)0);
					pos.y += d;
					me_ex.movey(d);
					me_ey.movey(d);
				}
			}
		}
	}

	for (unsigned int i = 0; i < level.size(); i++) {
		if (level[i].intersects(me_ex)) {
			if (me_ey.minp.x >= level[i].minp.x) {
				touching_walls[LEFT] = std::max(touching_walls[LEFT], level[i].color);
			}
			if (me_ey.maxp.x <= level[i].maxp.x) {
				touching_walls[RIGHT] = std::max(touching_walls[RIGHT], level[i].color);
			}
		}
		if (level[i].intersects(me_ey)) {
			if (me_ex.minp.y >= level[i].minp.y) {
				touching_walls[TOP] = std::max(touching_walls[TOP], level[i].color);
			}
			if (me_ex.maxp.y <= level[i].maxp.y) {
				touching_walls[BOTTOM] = std::max(touching_walls[BOTTOM], level[i].color);
			}
		}
	}
	float visc = 0.01;
	switch (touching_walls[LEFT]) {
		case 1:
			if (velocity.x < 0) velocity.x *= pow(visc, s);
			break;
		case 2:
			velocity.x = std::max(velocity.x, (float)0);
			acceleration.x = std::max(acceleration.x, (float)0);
			break;
		case 3:
			velocity.x = std::max(velocity.x, -velocity.x);
			acceleration.x = std::max(acceleration.x, (float)0);
			break;
		default: break;
	}
	switch (touching_walls[RIGHT]) {
		case 1:
			if (velocity.x > 0) velocity.x *= pow(visc, s);
			break;
		case 2:
			velocity.x = std::min(velocity.x, (float)0);
			acceleration.x = std::min(acceleration.x, (float)0);
			break;
		case 3:
			velocity.x = std::min(velocity.x, -velocity.x);
			acceleration.x = std::min(acceleration.x, (float)0);
			break;
		default: break;
	}
	switch (touching_walls[TOP]) {
		case 1:
			if (velocity.y < 0) velocity.y *= pow(visc, s);
			break;
		case 2:
			velocity.y = std::max(velocity.y, (float)0);
			acceleration.y = std::max(acceleration.y, (float)0);
			break;
		case 3:
			velocity.y = std::max(velocity.y, -velocity.y);
			acceleration.y = std::max(acceleration.y, (float)0);
			break;
		default: break;
	}
	switch (touching_walls[BOTTOM]) {
		case 1:
			if (velocity.y > 0) velocity.y *= pow(visc, s);
			break;
		case 2:
			velocity.y = std::min(velocity.y, (float)0);
			acceleration.y = std::min(acceleration.y, (float)0);
			if (velocity.x > 0) {
				velocity.x = std::max((float)0, velocity.x - (float)100 * s);
			} else {
				velocity.x = std::min((float)0, velocity.x + (float)100 * s);
			}
			break;
		case 3:
			velocity.y = std::min(velocity.y, -velocity.y);
			acceleration.y = std::min(acceleration.y, (float)0);
			break;
		default: break;
	}
	velocity.x *= pow(0.2, s);
}
<|endoftext|>


<commit_before>/*
	This file is owned by Murtaza Alexandru and may not be distributed, edited or used without written permission of the owner
	When using this work you accept to keep this header
	E-mails from murtaza_alexandru73@yahoo.com with permissions can be seen as valid.
*/

#include "TextureMappedFont.hpp"
#include "StringEx.hpp"
#include "Render.hpp"
#include "Vertex.hpp"
#include "Image.hpp"
#include "Types.hpp"

using namespace std;
using namespace MPACK::Core;
using namespace MPACK::Math;

namespace MPACK
{
	namespace Graphics
	{
		TextureMappedFont::TextureMappedFont()
			: m_layer(1000.0f), m_fontSize(40.0f),
			  m_charSpacing(0.6f), m_charPadding(0.05f),
			  m_monospaced(false), m_caseType(NONE),
			  m_formatType(RGB_MAGNITUDE)
		{
		}

		void TextureMappedFont::SetMonospaced(bool monospaced)
		{
			m_monospaced = monospaced;
		}

		void TextureMappedFont::SendString(string str, GLfloat x, GLfloat y, AlignType alignType, vector<Math::Vector4f> *colorPattern)
		{
			switch(m_caseType)
			{
				case LOWERCASE:
					str=StringEx::ToLower(str);
				break;
				case UPPERCASE:
					str=StringEx::ToUpper(str);
				break;
			}

			if(alignType==ALIGN_CENTER)
			{
				GLfloat width=GetTextWidth(str);
				GLfloat height=m_fontSize;

				x-=width*0.5;
				y-=height*0.5;
			}

			GLfloat spacing(m_fontSize*m_charSpacing);

			vector<SpriteVertex> quadData;
			SpriteVertex vertex;
			if(m_formatType == RGB_MAGNITUDE)
			{
				vertex.stype=SpriteVertex::ALPHA_TEST;
			}
			else
			{
				vertex.stype=SpriteVertex::ALPHA_BLEND;
			}

			int ind=0;
			for(string::size_type i = 0; i < str.size(); ++i)
			{
				const GLfloat oneOverSixteen = 1.0f / 16.0f;

				GLuint ch = GLuint(str[i]);
				GLuint chX = ch / 16;
				GLuint chY = ch % 16;
				GLfloat xPos = GLfloat(ch % 16) * oneOverSixteen;
				GLfloat yPos = GLfloat(ch / 16) * oneOverSixteen;

				if(!m_monospaced)
				{
					x-=m_fontSize*m_cellSpacing[chX][chY].left;
				}

				////////////////////////////////////
				vertex.x=x;
				vertex.y=y+m_fontSize;
				vertex.s=xPos;
				vertex.t=1.0f-yPos-oneOverSixteen;
				if(!colorPattern)
				{
					vertex.r=vertex.g=vertex.b=vertex.a=1.0;
				}
				else
				{
					vertex.r=(*colorPattern)[ind].x;
					vertex.g=(*colorPattern)[ind].y;
					vertex.b=(*colorPattern)[ind].z;
					vertex.a=(*colorPattern)[ind].w;
					++ind;
					if(ind==colorPattern->size())
					{
						ind=0;
					}
				}
				quadData.push_back(vertex);

				vertex.x=x+m_fontSize;
				vertex.y=y+m_fontSize;
				vertex.s=xPos+oneOverSixteen;
				vertex.t=1.0f-yPos-oneOverSixteen;
				if(!colorPattern)
				{
					vertex.r=vertex.g=vertex.b=vertex.a=1.0;
				}
				else
				{
					vertex.r=(*colorPattern)[ind].x;
					vertex.g=(*colorPattern)[ind].y;
					vertex.b=(*colorPattern)[ind].z;
					vertex.a=(*colorPattern)[ind].w;
					++ind;
					if(ind==colorPattern->size())
					{
						ind=0;
					}
				}
				quadData.push_back(vertex);

				vertex.x=x+m_fontSize;
				vertex.y=y;
				vertex.s=xPos+oneOverSixteen;
				vertex.t=1.0f-yPos;
				if(!colorPattern)
				{
					vertex.r=vertex.g=vertex.b=vertex.a=1.0;
				}
				else
				{
					vertex.r=(*colorPattern)[ind].x;
					vertex.g=(*colorPattern)[ind].y;
					vertex.b=(*colorPattern)[ind].z;
					vertex.a=(*colorPattern)[ind].w;
					++ind;
					if(ind==colorPattern->size())
					{
						ind=0;
					}
				}
				quadData.push_back(vertex);

				vertex.x=x;
				vertex.y=y;
				vertex.s=xPos;
				vertex.t=1.0f-yPos;
				if(!colorPattern)
				{
					vertex.r=vertex.g=vertex.b=vertex.a=1.0;
				}
				else
				{
					vertex.r=(*colorPattern)[ind].x;
					vertex.g=(*colorPattern)[ind].y;
					vertex.b=(*colorPattern)[ind].z;
					vertex.a=(*colorPattern)[ind].w;
					++ind;
					if(ind==colorPattern->size())
					{
						ind=0;
					}
				}
				quadData.push_back(vertex);

				if(!m_monospaced)
				{
					x+=m_fontSize*(1-m_cellSpacing[chX][chY].right)+m_charPadding*m_fontSize;
				}
				else
				{
					x+=spacing;
				}
			}

			Batcher::SendSpriteVertexQuad(&quadData[0],quadData.size(),&m_texture,IndexData::TRIANGLES,m_layer);
		}

		void TextureMappedFont::SetCase(CaseType caseType)
		{
			m_caseType=caseType;
		}

		TextureMappedFont::CaseType TextureMappedFont::GetCase() const
		{
			return m_caseType;
		}

		TextureMappedFont::FormatType TextureMappedFont::GetFormat() const
		{
			return m_formatType;
		}

		void TextureMappedFont::SetCharPadding(GLfloat charPadding)
		{
			m_charPadding = charPadding;
		}

		GLfloat TextureMappedFont::GetCharPadding() const
		{
			return m_charPadding;
		}

		void TextureMappedFont::AutoCalibrate()
		{
			if(m_formatType == RGB_MAGNITUDE)
			{
				m_charPadding = 0.0f;
			}
			else
			{
				m_charPadding = 0.05f;
			}
		}

		void TextureMappedFont::SetFontSize(GLfloat fontSize)
		{
			m_fontSize=fontSize;
		}

		void TextureMappedFont::SetCharSpacing(GLfloat charSpacing)
		{
			m_charSpacing=charSpacing;
		}

		bool TextureMappedFont::Load(const string& textureName, FormatType format)
		{
			Image *pFontImage = LoadImage(textureName.c_str());
			if(pFontImage->Load(textureName.c_str())==RETURN_VALUE_KO)
			{
				LOGE("TextureMappedFont::Load() could not load the font texture: %s",textureName.c_str());
				delete pFontImage;
				return false;
			}

			if(format == ALPHA && !pFontImage->HaveAlphaChannel())
			{
				LOGE("TextureMappedFont::Load() format is set to ALPHA but font image does not have alpha channel!");
				delete pFontImage;
				return false;
			}

			m_formatType = format;

			pFontImage->FlipVertical();

			if(format == ALPHA)
			{
				BuildCellSpacing_ALPHA(pFontImage);
			}
			else if(format == RGB_MAGNITUDE)
			{
				BuildCellSpacing_RGB_MAGNITUDE(pFontImage);
			}

			pFontImage->FlipVertical();
			if (!m_texture.Load(pFontImage, Bilinear))
			{
				LOGE("Texture Mapped Font: Could not load font image to texture memory: %s",textureName.c_str());
				delete pFontImage;
				return false;
			}
			delete pFontImage;

			AutoCalibrate();
			return true;
		}

		Texture2D* TextureMappedFont::GetTexturePointer()
		{
			return &m_texture;
		}

		GLfloat TextureMappedFont::GetFontSize() const
		{
			return m_fontSize;
		}

		GLfloat TextureMappedFont::GetTextWidth(const std::string &str)
		{
			GLfloat width=0.0f;
			if(m_monospaced)
			{
				GLfloat spacing(m_fontSize*m_charSpacing);
				for(register int i=0;i<str.size();++i)
				{
					if(str[i]==' ')
					{
						width+=spacing;
					}
					else
					{
						width+=m_fontSize;
					}
				}
			}
			else
			{
				GLfloat x=0.0f;
				for(string::size_type i = 0; i < str.size(); ++i)
				{
					const GLfloat oneOverSixteen = 1.0f / 16.0f;

					GLuint ch = GLuint(str[i]);
					GLuint chX = ch / 16;
					GLuint chY = ch % 16;
					GLfloat xPos = GLfloat(ch % 16) * oneOverSixteen;
					GLfloat yPos = GLfloat(ch / 16) * oneOverSixteen;

					width-=m_fontSize*m_cellSpacing[chX][chY].left;
					width+=m_fontSize*(1-m_cellSpacing[chX][chY].right);
				}
				width+=m_fontSize*m_charPadding*(str.size()-1);
			}
			return width;
		}

		void TextureMappedFont::BuildCellSpacing_RGB_MAGNITUDE(Image *pFontImage)
		{
			GLuint width=pFontImage->GetWidth();
			GLuint height=pFontImage->GetHeight();
			GLuint cellWidth=width>>4;
			GLuint cellHeight=height>>4;
			const float OneOver255=1.0f/255.0f;
			float OneOverCellWidth=1.0f/(GLfloat)(cellWidth);
			float OneOverCellHeight=1.0f/(GLfloat)(cellHeight);
			AABB2Df cell;
			for(register GLuint i=0;i<16;++i)
			{
				for(register GLuint j=0;j<16;++j)
				{
					cell.Clear();
					for(register GLuint ci=0;ci<cellWidth;++ci)
					{
						for(register GLuint cj=0;cj<cellHeight;++cj)
						{
							GLuint ri=i*cellWidth+ci;
							GLuint rj=j*cellHeight+cj;
							Color c=pFontImage->GetPixel(ri,rj);
							BYTE b=c.b;
							BYTE g=c.g;
							BYTE r=c.r;
							Math::Vector3f color((GLfloat)(r)*OneOver255,(GLfloat)(g)*OneOver255,(GLfloat)(b)*OneOver255);

							if(color.Magnitude()>=FORMATTYPE_RGB_MAGNITUDE_THRESHOLD)
							{
								cell.AddPoint(Vector2f((GLfloat)(cj),(GLfloat)(ci)));
							}
						}
					}

					if(cell.m_xmin>cellWidth)
					{
						m_cellSpacing[i][j].left=OneOverCellWidth*(cellWidth>>1);
						m_cellSpacing[i][j].right=OneOverCellWidth*(cellWidth>>1);
						m_cellSpacing[i][j].top=OneOverCellWidth*(cellHeight>>1);
						m_cellSpacing[i][j].bottom=OneOverCellWidth*(cellHeight>>1);
					}
					else
					{
						m_cellSpacing[i][j].left=(GLfloat)(cell.m_xmin)*OneOverCellWidth;
						m_cellSpacing[i][j].right=(GLfloat)(cellWidth-cell.m_xmax)*OneOverCellWidth;
						m_cellSpacing[i][j].top=(GLfloat)(cell.m_ymin)*OneOverCellHeight;
						m_cellSpacing[i][j].bottom=(GLfloat)(cellHeight-cell.m_ymax)*OneOverCellHeight;
					}
				}
			}

			GLuint chX = GLuint(' ') / 16;
			GLuint chY = GLuint(' ') % 16;
			m_cellSpacing[chX][chY].right=m_cellSpacing[chX][chY].bottom=0.0f;

			chX = GLuint('\t') / 16;
			chY = GLuint('\t') % 16;
			m_cellSpacing[chX][chY].right=m_cellSpacing[chX][chY].bottom=0.0f;
		}

		void TextureMappedFont::BuildCellSpacing_ALPHA(Image *pFontImage)
		{
			GLuint width=pFontImage->GetWidth();
			GLuint height=pFontImage->GetHeight();
			GLuint cellWidth=width>>4;
			GLuint cellHeight=height>>4;
			const float OneOver255=1.0f/255.0f;
			float OneOverCellWidth=1.0f/(GLfloat)(cellWidth);
			float OneOverCellHeight=1.0f/(GLfloat)(cellHeight);
			AABB2Df cell;
			for(register GLuint i=0;i<16;++i)
			{
				for(register GLuint j=0;j<16;++j)
				{
					cell.Clear();
					for(register GLuint ci=0;ci<cellWidth;++ci)
					{
						for(register GLuint cj=0;cj<cellHeight;++cj)
						{
							GLuint ri=i*cellWidth+ci;
							GLuint rj=j*cellHeight+cj;
							Color c=pFontImage->GetPixel(ri,rj);
							BYTE alpha=c.a;

							if(alpha>=FORMATTYPE_ALPHA_THRESHOLD)
							{
								cell.AddPoint(Vector2f((GLfloat)(cj),(GLfloat)(ci)));
							}
						}
					}

					if(cell.m_xmin>cellWidth)
					{
						m_cellSpacing[i][j].left=OneOverCellWidth*(cellWidth>>1);
						m_cellSpacing[i][j].right=OneOverCellWidth*(cellWidth>>1);
						m_cellSpacing[i][j].top=OneOverCellWidth*(cellHeight>>1);
						m_cellSpacing[i][j].bottom=OneOverCellWidth*(cellHeight>>1);
					}
					else
					{
						m_cellSpacing[i][j].left=(GLfloat)(cell.m_xmin)*OneOverCellHeight;
						m_cellSpacing[i][j].right=(GLfloat)(cellWidth-cell.m_xmax)*OneOverCellHeight;
						m_cellSpacing[i][j].top=(GLfloat)(cell.m_ymin)*OneOverCellWidth;
						m_cellSpacing[i][j].bottom=(GLfloat)(cellHeight-cell.m_ymax)*OneOverCellWidth;
					}
				}
			}

			GLuint chX = GLuint(' ') / 16;
			GLuint chY = GLuint(' ') % 16;
			m_cellSpacing[chX][chY].right=m_cellSpacing[chX][chY].bottom=0.0f;

			chX = GLuint('\t') / 16;
			chY = GLuint('\t') % 16;
			m_cellSpacing[chX][chY].right=m_cellSpacing[chX][chY].bottom=0.0f;
		}
	}
}
<commit_msg>Fix TextureMappedFont memory leak<commit_after>/*
	This file is owned by Murtaza Alexandru and may not be distributed, edited or used without written permission of the owner
	When using this work you accept to keep this header
	E-mails from murtaza_alexandru73@yahoo.com with permissions can be seen as valid.
*/

#include "TextureMappedFont.hpp"
#include "StringEx.hpp"
#include "Render.hpp"
#include "Vertex.hpp"
#include "Image.hpp"
#include "Types.hpp"

using namespace std;
using namespace MPACK::Core;
using namespace MPACK::Math;

namespace MPACK
{
	namespace Graphics
	{
		TextureMappedFont::TextureMappedFont()
			: m_layer(1000.0f), m_fontSize(40.0f),
			  m_charSpacing(0.6f), m_charPadding(0.05f),
			  m_monospaced(false), m_caseType(NONE),
			  m_formatType(RGB_MAGNITUDE)
		{
		}

		void TextureMappedFont::SetMonospaced(bool monospaced)
		{
			m_monospaced = monospaced;
		}

		void TextureMappedFont::SendString(string str, GLfloat x, GLfloat y, AlignType alignType, vector<Math::Vector4f> *colorPattern)
		{
			switch(m_caseType)
			{
				case LOWERCASE:
					str=StringEx::ToLower(str);
				break;
				case UPPERCASE:
					str=StringEx::ToUpper(str);
				break;
			}

			if(alignType==ALIGN_CENTER)
			{
				GLfloat width=GetTextWidth(str);
				GLfloat height=m_fontSize;

				x-=width*0.5;
				y-=height*0.5;
			}

			GLfloat spacing(m_fontSize*m_charSpacing);

			vector<SpriteVertex> quadData;
			SpriteVertex vertex;
			if(m_formatType == RGB_MAGNITUDE)
			{
				vertex.stype=SpriteVertex::ALPHA_TEST;
			}
			else
			{
				vertex.stype=SpriteVertex::ALPHA_BLEND;
			}

			int ind=0;
			for(string::size_type i = 0; i < str.size(); ++i)
			{
				const GLfloat oneOverSixteen = 1.0f / 16.0f;

				GLuint ch = GLuint(str[i]);
				GLuint chX = ch / 16;
				GLuint chY = ch % 16;
				GLfloat xPos = GLfloat(ch % 16) * oneOverSixteen;
				GLfloat yPos = GLfloat(ch / 16) * oneOverSixteen;

				if(!m_monospaced)
				{
					x-=m_fontSize*m_cellSpacing[chX][chY].left;
				}

				////////////////////////////////////
				vertex.x=x;
				vertex.y=y+m_fontSize;
				vertex.s=xPos;
				vertex.t=1.0f-yPos-oneOverSixteen;
				if(!colorPattern)
				{
					vertex.r=vertex.g=vertex.b=vertex.a=1.0;
				}
				else
				{
					vertex.r=(*colorPattern)[ind].x;
					vertex.g=(*colorPattern)[ind].y;
					vertex.b=(*colorPattern)[ind].z;
					vertex.a=(*colorPattern)[ind].w;
					++ind;
					if(ind==colorPattern->size())
					{
						ind=0;
					}
				}
				quadData.push_back(vertex);

				vertex.x=x+m_fontSize;
				vertex.y=y+m_fontSize;
				vertex.s=xPos+oneOverSixteen;
				vertex.t=1.0f-yPos-oneOverSixteen;
				if(!colorPattern)
				{
					vertex.r=vertex.g=vertex.b=vertex.a=1.0;
				}
				else
				{
					vertex.r=(*colorPattern)[ind].x;
					vertex.g=(*colorPattern)[ind].y;
					vertex.b=(*colorPattern)[ind].z;
					vertex.a=(*colorPattern)[ind].w;
					++ind;
					if(ind==colorPattern->size())
					{
						ind=0;
					}
				}
				quadData.push_back(vertex);

				vertex.x=x+m_fontSize;
				vertex.y=y;
				vertex.s=xPos+oneOverSixteen;
				vertex.t=1.0f-yPos;
				if(!colorPattern)
				{
					vertex.r=vertex.g=vertex.b=vertex.a=1.0;
				}
				else
				{
					vertex.r=(*colorPattern)[ind].x;
					vertex.g=(*colorPattern)[ind].y;
					vertex.b=(*colorPattern)[ind].z;
					vertex.a=(*colorPattern)[ind].w;
					++ind;
					if(ind==colorPattern->size())
					{
						ind=0;
					}
				}
				quadData.push_back(vertex);

				vertex.x=x;
				vertex.y=y;
				vertex.s=xPos;
				vertex.t=1.0f-yPos;
				if(!colorPattern)
				{
					vertex.r=vertex.g=vertex.b=vertex.a=1.0;
				}
				else
				{
					vertex.r=(*colorPattern)[ind].x;
					vertex.g=(*colorPattern)[ind].y;
					vertex.b=(*colorPattern)[ind].z;
					vertex.a=(*colorPattern)[ind].w;
					++ind;
					if(ind==colorPattern->size())
					{
						ind=0;
					}
				}
				quadData.push_back(vertex);

				if(!m_monospaced)
				{
					x+=m_fontSize*(1-m_cellSpacing[chX][chY].right)+m_charPadding*m_fontSize;
				}
				else
				{
					x+=spacing;
				}
			}

			Batcher::SendSpriteVertexQuad(&quadData[0],quadData.size(),&m_texture,IndexData::TRIANGLES,m_layer);
		}

		void TextureMappedFont::SetCase(CaseType caseType)
		{
			m_caseType=caseType;
		}

		TextureMappedFont::CaseType TextureMappedFont::GetCase() const
		{
			return m_caseType;
		}

		TextureMappedFont::FormatType TextureMappedFont::GetFormat() const
		{
			return m_formatType;
		}

		void TextureMappedFont::SetCharPadding(GLfloat charPadding)
		{
			m_charPadding = charPadding;
		}

		GLfloat TextureMappedFont::GetCharPadding() const
		{
			return m_charPadding;
		}

		void TextureMappedFont::AutoCalibrate()
		{
			if(m_formatType == RGB_MAGNITUDE)
			{
				m_charPadding = 0.0f;
			}
			else
			{
				m_charPadding = 0.05f;
			}
		}

		void TextureMappedFont::SetFontSize(GLfloat fontSize)
		{
			m_fontSize=fontSize;
		}

		void TextureMappedFont::SetCharSpacing(GLfloat charSpacing)
		{
			m_charSpacing=charSpacing;
		}

		bool TextureMappedFont::Load(const string& textureName, FormatType format)
		{
			Image *pFontImage = LoadImage(textureName.c_str());
			if(!pFontImage)
			{
				LOGE("TextureMappedFont::Load() could not load the font texture: %s",textureName.c_str());
				delete pFontImage;
				return false;
			}

			if(format == ALPHA && !pFontImage->HaveAlphaChannel())
			{
				LOGE("TextureMappedFont::Load() format is set to ALPHA but font image does not have alpha channel!");
				delete pFontImage;
				return false;
			}

			m_formatType = format;

			pFontImage->FlipVertical();

			if(format == ALPHA)
			{
				BuildCellSpacing_ALPHA(pFontImage);
			}
			else if(format == RGB_MAGNITUDE)
			{
				BuildCellSpacing_RGB_MAGNITUDE(pFontImage);
			}

			pFontImage->FlipVertical();
			if (!m_texture.Load(pFontImage, Bilinear))
			{
				LOGE("Texture Mapped Font: Could not load font image to texture memory: %s",textureName.c_str());
				delete pFontImage;
				return false;
			}
			delete pFontImage;

			AutoCalibrate();
			return true;
		}

		Texture2D* TextureMappedFont::GetTexturePointer()
		{
			return &m_texture;
		}

		GLfloat TextureMappedFont::GetFontSize() const
		{
			return m_fontSize;
		}

		GLfloat TextureMappedFont::GetTextWidth(const std::string &str)
		{
			GLfloat width=0.0f;
			if(m_monospaced)
			{
				GLfloat spacing(m_fontSize*m_charSpacing);
				for(register int i=0;i<str.size();++i)
				{
					if(str[i]==' ')
					{
						width+=spacing;
					}
					else
					{
						width+=m_fontSize;
					}
				}
			}
			else
			{
				GLfloat x=0.0f;
				for(string::size_type i = 0; i < str.size(); ++i)
				{
					const GLfloat oneOverSixteen = 1.0f / 16.0f;

					GLuint ch = GLuint(str[i]);
					GLuint chX = ch / 16;
					GLuint chY = ch % 16;
					GLfloat xPos = GLfloat(ch % 16) * oneOverSixteen;
					GLfloat yPos = GLfloat(ch / 16) * oneOverSixteen;

					width-=m_fontSize*m_cellSpacing[chX][chY].left;
					width+=m_fontSize*(1-m_cellSpacing[chX][chY].right);
				}
				width+=m_fontSize*m_charPadding*(str.size()-1);
			}
			return width;
		}

		void TextureMappedFont::BuildCellSpacing_RGB_MAGNITUDE(Image *pFontImage)
		{
			GLuint width=pFontImage->GetWidth();
			GLuint height=pFontImage->GetHeight();
			GLuint cellWidth=width>>4;
			GLuint cellHeight=height>>4;
			const float OneOver255=1.0f/255.0f;
			float OneOverCellWidth=1.0f/(GLfloat)(cellWidth);
			float OneOverCellHeight=1.0f/(GLfloat)(cellHeight);
			AABB2Df cell;
			for(register GLuint i=0;i<16;++i)
			{
				for(register GLuint j=0;j<16;++j)
				{
					cell.Clear();
					for(register GLuint ci=0;ci<cellWidth;++ci)
					{
						for(register GLuint cj=0;cj<cellHeight;++cj)
						{
							GLuint ri=i*cellWidth+ci;
							GLuint rj=j*cellHeight+cj;
							Color c=pFontImage->GetPixel(ri,rj);
							BYTE b=c.b;
							BYTE g=c.g;
							BYTE r=c.r;
							Math::Vector3f color((GLfloat)(r)*OneOver255,(GLfloat)(g)*OneOver255,(GLfloat)(b)*OneOver255);

							if(color.Magnitude()>=FORMATTYPE_RGB_MAGNITUDE_THRESHOLD)
							{
								cell.AddPoint(Vector2f((GLfloat)(cj),(GLfloat)(ci)));
							}
						}
					}

					if(cell.m_xmin>cellWidth)
					{
						m_cellSpacing[i][j].left=OneOverCellWidth*(cellWidth>>1);
						m_cellSpacing[i][j].right=OneOverCellWidth*(cellWidth>>1);
						m_cellSpacing[i][j].top=OneOverCellWidth*(cellHeight>>1);
						m_cellSpacing[i][j].bottom=OneOverCellWidth*(cellHeight>>1);
					}
					else
					{
						m_cellSpacing[i][j].left=(GLfloat)(cell.m_xmin)*OneOverCellWidth;
						m_cellSpacing[i][j].right=(GLfloat)(cellWidth-cell.m_xmax)*OneOverCellWidth;
						m_cellSpacing[i][j].top=(GLfloat)(cell.m_ymin)*OneOverCellHeight;
						m_cellSpacing[i][j].bottom=(GLfloat)(cellHeight-cell.m_ymax)*OneOverCellHeight;
					}
				}
			}

			GLuint chX = GLuint(' ') / 16;
			GLuint chY = GLuint(' ') % 16;
			m_cellSpacing[chX][chY].right=m_cellSpacing[chX][chY].bottom=0.0f;

			chX = GLuint('\t') / 16;
			chY = GLuint('\t') % 16;
			m_cellSpacing[chX][chY].right=m_cellSpacing[chX][chY].bottom=0.0f;
		}

		void TextureMappedFont::BuildCellSpacing_ALPHA(Image *pFontImage)
		{
			GLuint width=pFontImage->GetWidth();
			GLuint height=pFontImage->GetHeight();
			GLuint cellWidth=width>>4;
			GLuint cellHeight=height>>4;
			const float OneOver255=1.0f/255.0f;
			float OneOverCellWidth=1.0f/(GLfloat)(cellWidth);
			float OneOverCellHeight=1.0f/(GLfloat)(cellHeight);
			AABB2Df cell;
			for(register GLuint i=0;i<16;++i)
			{
				for(register GLuint j=0;j<16;++j)
				{
					cell.Clear();
					for(register GLuint ci=0;ci<cellWidth;++ci)
					{
						for(register GLuint cj=0;cj<cellHeight;++cj)
						{
							GLuint ri=i*cellWidth+ci;
							GLuint rj=j*cellHeight+cj;
							Color c=pFontImage->GetPixel(ri,rj);
							BYTE alpha=c.a;

							if(alpha>=FORMATTYPE_ALPHA_THRESHOLD)
							{
								cell.AddPoint(Vector2f((GLfloat)(cj),(GLfloat)(ci)));
							}
						}
					}

					if(cell.m_xmin>cellWidth)
					{
						m_cellSpacing[i][j].left=OneOverCellWidth*(cellWidth>>1);
						m_cellSpacing[i][j].right=OneOverCellWidth*(cellWidth>>1);
						m_cellSpacing[i][j].top=OneOverCellWidth*(cellHeight>>1);
						m_cellSpacing[i][j].bottom=OneOverCellWidth*(cellHeight>>1);
					}
					else
					{
						m_cellSpacing[i][j].left=(GLfloat)(cell.m_xmin)*OneOverCellHeight;
						m_cellSpacing[i][j].right=(GLfloat)(cellWidth-cell.m_xmax)*OneOverCellHeight;
						m_cellSpacing[i][j].top=(GLfloat)(cell.m_ymin)*OneOverCellWidth;
						m_cellSpacing[i][j].bottom=(GLfloat)(cellHeight-cell.m_ymax)*OneOverCellWidth;
					}
				}
			}

			GLuint chX = GLuint(' ') / 16;
			GLuint chY = GLuint(' ') % 16;
			m_cellSpacing[chX][chY].right=m_cellSpacing[chX][chY].bottom=0.0f;

			chX = GLuint('\t') / 16;
			chY = GLuint('\t') % 16;
			m_cellSpacing[chX][chY].right=m_cellSpacing[chX][chY].bottom=0.0f;
		}
	}
}
<|endoftext|>


<commit_before>/*
 *  despot1_main.cpp
 *
 *  Created by Tobias Wood on 23/01/2012.
 *  Copyright 2012 Tobias Wood. All rights reserved.
 *
 */

#include <time.h>
#include <getopt.h>
#include <iostream>
#include <atomic>
#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>

#include "NiftiImage.h"
#include "DESPOT.h"
#include "DESPOT_Functors.h"
#include "RegionContraction.h"

#define USE_PROCPAR
#ifdef USE_PROCPAR
	#include "procpar.h"
	using namespace Recon;
#endif

using namespace std;
using namespace Eigen;

//******************************************************************************
// Arguments / Usage
//******************************************************************************
const string credit {
"despot2 - Written by tobias.wood@kcl.ac.uk, based on work by Sean Deoni. \n\
Acknowledgements greatfully received, grant discussions welcome."
};

const string usage {
"Usage is: despot2 [options] output_prefix T1_map ssfp_files\n\
\
Options:\n\
	--help, -h        : Print this message.\n\
	--mask, -m file   : Mask input with specified file.\n\
	--B0 file         : B0 Map file.\n\
	--B1 file         : B1 Map file.\n\
	--M0 file         : Proton density file.\n\
	--verbose, -v     : Print slice processing times.\n\
	--start_slice N   : Start processing from slice N.\n\
	--end_slice   N   : Finish processing at slice N.\n\
	--tesla, -t 3     : Enables DESPOT-FM with boundaries suitable for 3T\n\
	            7     : Boundaries suitable for 7T (default)\n\
	            u     : User specified boundaries from stdin.\n"

};

// tesla == 0 means NO DESPOT-FM
static int tesla = 0, fitB0 = false, verbose = false, start_slice = -1, end_slice = -1;
static string outPrefix;
static struct option long_options[] =
{
	{"B0", required_argument, 0, '0'},
	{"B1", required_argument, 0, '1'},
	{"M0", required_argument, 0, 'M'},
	{"help", no_argument, 0, 'h'},
	{"mask", required_argument, 0, 'm'},
	{"tesla", optional_argument, 0, 'f'},
	{"verbose", no_argument, 0, 'v'},
	{"start_slice", required_argument, 0, 'S'},
	{"end_slice", required_argument, 0, 'E'},
	{0, 0, 0, 0}
};

//******************************************************************************
// Main
//******************************************************************************
int main(int argc, char **argv)
{
	//**************************************************************************
	// Argument Processing
	//**************************************************************************
	cout << credit << endl;
	if (argc < 4) {
		cout << usage << endl;
		return EXIT_FAILURE;
	}
	Eigen::initParallel();
	NiftiImage inFile, savedHeader;
	double *maskData = NULL, *B0Data = NULL, *B1Data = NULL, *T1Data = NULL,
	       *M0Data = NULL;
	string procPath;

	int indexptr = 0, c;
	while ((c = getopt_long(argc, argv, "hm:vz", long_options, &indexptr)) != -1) {
		switch (c) {
			case 'm':
				cout << "Reading mask file " << optarg << endl;
				inFile.open(optarg, 'r');
				maskData = inFile.readVolume<double>(0);
				inFile.close();
				break;
			case '0':
				cout << "Reading B0 file " << optarg << endl;
				inFile.open(optarg, 'r');
				B0Data = inFile.readVolume<double>(0);
				inFile.close();
				break;
			case '1':
				cout << "Reading B1 file " << optarg;
				inFile.open(optarg, 'r');
				B1Data = inFile.readVolume<double>(0);
				inFile.close();
				break;
			case 'M':
				cout << "Reading M0 file " << optarg;
				inFile.open(optarg, 'r');
				M0Data = inFile.readVolume<double>(0);
				inFile.close();
				break;
			case 't':
				switch (*optarg) {
					case '3': tesla = 3; break;
					case '7': tesla = 7; break;
					case 'u': tesla = -1; break;
					default:
						cout << "Unknown boundaries type " << optarg << endl;
						abort();
						break;
				}
				cout << "Using " << tesla << "T boundaries." << endl;
				break;
			case 'v':
				verbose = true;
				break;
			case 'S':
				start_slice = atoi(optarg);
				break;
			case 'E':
				end_slice = atoi(optarg);
				break;
			case 0:
				// Just a flag
				break;
			case '?': // getopt will print an error message
			case 'h':
				cout << usage << endl;
				return EXIT_FAILURE;
		}
	}
	if ((tesla != 0) && !B0Data)
		fitB0 = true;
	cout << "Output prefix will be: " << argv[optind] << endl;
	outPrefix = argv[optind++];
	cout << "Reading T1 Map from: " << argv[optind] << endl;
	inFile.open(argv[optind++], 'r');
	T1Data = inFile.readVolume<double>(0);
	inFile.close();
	savedHeader = inFile;
	//**************************************************************************
	// Gather SSFP Data
	//**************************************************************************
	int nFlip, nPhases;
	double ssfpTR;
	nPhases = argc - optind;
	vector<double> ssfpPhases(nPhases);
	VectorXd ssfpAngles;
	int voxelsPerSlice, voxelsPerVolume;
	double **ssfpData = (double **)malloc(nPhases * sizeof(double *));
	for (size_t p = 0; p < nPhases; p++)
	{
		cout << "Reading SSFP header from " << argv[optind] << endl;
		inFile.open(argv[optind], 'r');
		if (p == 0)
		{	// Read nFlip, TR and flip angles from first file
			nFlip = inFile.dim(4);
			voxelsPerSlice = inFile.voxelsPerSlice();
			voxelsPerVolume = inFile.voxelsPerVolume();
			ssfpAngles.resize(nFlip, 1);

			#ifdef USE_PROCPAR
			ParameterList pars;
			if (ReadProcpar(inFile.basename() + ".procpar", pars)) {
				ssfpTR = RealValue(pars, "tr");
				for (int i = 0; i < nFlip; i++)
					ssfpAngles[i] = RealValue(pars, "flip1", i);
			} else
			#endif
			{
				cout << "Enter SSFP TR (s): " << flush;
				cin >> ssfpTR;
				cout << "Enter " << nFlip << " flip angles (degrees): " << flush;
				for (int i = 0; i < ssfpAngles.size(); i++)
					cin >> ssfpAngles[i];
			}
		}
		#ifdef USE_PROCPAR
		ParameterList pars;
		if (ReadProcpar(inFile.basename() + ".procpar", pars)) {
			ssfpPhases[p] = RealValue(pars, "rfphase") * M_PI / 180.;
		} else
		#endif
		{
			cout << "Enter phase-cycling (degrees): " << flush;
			cin >> ssfpPhases[p]; ssfpPhases[p] *= M_PI / 180.;
		}
		cout << "Reading SSFP data..." << endl;
		ssfpData[p] = inFile.readAllVolumes<double>();
		// Don't close the first header because we've saved it to write the
		// results, and FSLIO gets fussy about cloning closed headers
		inFile.close();
		optind++;
	}
	ssfpAngles *= M_PI / 180.;

	if (optind != argc) {
		cerr << "Unprocessed arguments supplied.\n" << usage;
		exit(EXIT_FAILURE);
	}

	// Set up boundaries for DESPOT-FM if needed
	DESPOT2FM::ParamType loBounds, hiBounds;
	if (tesla != 0) {
		if (tesla > 0) {
			loBounds = DESPOT2FM::loBounds(tesla);
			hiBounds = DESPOT2FM::hiBounds(tesla);
		} else if (tesla < 0) {
			cout << "Enter " << DESPOT2FM::nP << " parameter pairs (low then high): " << flush;
			for (int i = 0; i < DESPOT2FM::nP; i++) cin >> loBounds[i] >> hiBounds[i];
		}
		// If fitting, give a suitable range and allocate results memory
		if (fitB0) {
			loBounds[DESPOT2FM::nP - 1] = -0.5 / ssfpTR;
			hiBounds[DESPOT2FM::nP - 1] =  0.5 / ssfpTR;
			B0Data = new double[voxelsPerVolume];
		} else { // Otherwise fix and let functors pick up the specified value
			loBounds[DESPOT2FM::nP - 1] = 0.;
			hiBounds[DESPOT2FM::nP - 1] = 0.;
		}
	}

	if (verbose) {
		cout << "SSFP Angles (deg): " << ssfpAngles.transpose() * 180 / M_PI << endl;
		if (tesla != 0)
			cout << "Low bounds: " << loBounds.transpose() << endl
				 << "Hi bounds:  " << hiBounds.transpose() << endl;
	}
	//**************************************************************************
	// Set up results data
	//**************************************************************************
	double *residualData = new double[voxelsPerVolume];
	double *PDData = new double[voxelsPerVolume];
	double *T2Data = new double[voxelsPerVolume];
	//**************************************************************************
	// Do the fitting
	//**************************************************************************
    time_t procStart = time(NULL);
	if ((start_slice < 0) || (start_slice >= inFile.dim(4)))
		start_slice = 0;
	if ((end_slice < 0) || (end_slice > inFile.dim(4)))
		end_slice = inFile.dim(4);
	for (int slice = start_slice; slice < end_slice; slice++) {
		// Read in data
		if (verbose)
			cout << "Starting slice " << slice << "..." << flush;

		atomic<int> voxCount{0};
		const int sliceOffset = slice * voxelsPerSlice;
		clock_t loopStart = clock();
		function<void (const int&)> processVox = [&] (const int &vox) {
			// Set up parameters and constants
			double M0 = 0., T1 = 0., T2 = 0., B0 = 0, B1 = 1., residual = 0.;
			if (!maskData || ((maskData[sliceOffset + vox] > 0.) && (T1Data[sliceOffset + vox] > 0.)))
			{	// Zero T1 causes zero-pivot error.
				voxCount++;
				T1 = T1Data[sliceOffset + vox];
				if (B0Data) B0 = B0Data[sliceOffset + vox];
				if (B1Data)	B1 = B1Data[sliceOffset + vox];
				// Gather signals.
				vector<VectorXd> signals;
				for (int p = 0; p < nPhases; p++)
				{
					VectorXd temp(nFlip);
					for (int i = 0; i < nFlip; i++)
						temp(i) = ssfpData[p][i*voxelsPerVolume + sliceOffset + vox];
					signals.push_back(temp);
				}

				if (tesla == 0) {
					// Choose phase with accumulated phase closest to 180 and then classic DESPOT2
					int index = 0;
					double bestPhase = DBL_MAX;
					for (int p = 0; p < nPhases; p++) {
						double thisPhase = (B0 * ssfpTR * 2 * M_PI) + ssfpPhases[p];
						if (fabs(fmod(thisPhase - M_PI, 2 * M_PI)) < bestPhase) {
							bestPhase = fabs(fmod(thisPhase - M_PI, 2 * M_PI));
							index = p;
						}
					}
					residual = classicDESPOT2(ssfpAngles, signals[index], ssfpTR, T1, B1, M0, T2);
				} else {
					// DESPOT2-FM
					VectorXd allB0(nFlip), allB1(nFlip);
					allB0.setConstant(B0);
					allB1.setConstant(B1);
					DESPOT2FM tc(ssfpAngles, ssfpPhases, signals,
				                 allB0, allB1, ssfpTR, false, fitB0);
					DESPOT2FM::ParamType params(DESPOT2FM::nP);
					residual = regionContraction<DESPOT2FM>(params, tc, loBounds, hiBounds);
					M0 = params[0];
					T2 = params[1];
					if (fitB0)
						B0 = params[2];

				}
			}
			PDData[sliceOffset + vox] = clamp(M0, 0., 5.e3);
			T2Data[sliceOffset + vox] = clamp(T2, 0., 0.5);
			if (fitB0)
				B0Data[sliceOffset + vox] = B0;
			residualData[sliceOffset + vox] = residual;
		};
		apply_for(voxelsPerSlice, processVox);

		if (verbose) {
			clock_t loopEnd = clock();
			if (voxCount > 0)
				cout << voxCount << " unmasked voxels, CPU time per voxel was "
				          << ((loopEnd - loopStart) / ((float)voxCount * CLOCKS_PER_SEC)) << " s, ";
			cout << "finished." << endl;
		}
	}
    time_t procEnd = time(NULL);
    struct tm *localEnd = localtime(&procEnd);
	char theTime[512];
    strftime(theTime, 512, "%H:%M:%S", localEnd);
	cout << "Finished processing at " << theTime << ". Run-time was "
	          << difftime(procEnd, procStart) << " s." << endl;
	savedHeader.setDim(4, 1);
	savedHeader.setDatatype(NIFTI_TYPE_FLOAT32);
	savedHeader.open(outPrefix + "_T2.nii.gz", NiftiImage::NIFTI_WRITE);
	savedHeader.writeVolume(0, T2Data);
	savedHeader.close();
	savedHeader.open(outPrefix + "_PD.nii.gz", NiftiImage::NIFTI_WRITE);
	savedHeader.writeVolume(0, PDData);
	savedHeader.close();
	if (fitB0) {
		savedHeader.open(outPrefix + "_B0.nii.gz", NiftiImage::NIFTI_WRITE);
		savedHeader.writeVolume(0, B0Data);
		savedHeader.close();
	}
	// Clean up memory
	for (int p = 0; p < nPhases; p++)
		free(ssfpData[p]);
	if (B0Data)
		free(B0Data);
	if (B1Data)
		free(B1Data);
	if (maskData)
		free(maskData);
	return EXIT_SUCCESS;
}
<commit_msg>Start and end slices in despot2 were being incorrectly taken from 4th dimension instead of 3rd.<commit_after>/*
 *  despot1_main.cpp
 *
 *  Created by Tobias Wood on 23/01/2012.
 *  Copyright 2012 Tobias Wood. All rights reserved.
 *
 */

#include <time.h>
#include <getopt.h>
#include <iostream>
#include <atomic>
#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>

#include "NiftiImage.h"
#include "DESPOT.h"
#include "DESPOT_Functors.h"
#include "RegionContraction.h"

#define USE_PROCPAR
#ifdef USE_PROCPAR
	#include "procpar.h"
	using namespace Recon;
#endif

using namespace std;
using namespace Eigen;

//******************************************************************************
// Arguments / Usage
//******************************************************************************
const string credit {
"despot2 - Written by tobias.wood@kcl.ac.uk, based on work by Sean Deoni. \n\
Acknowledgements greatfully received, grant discussions welcome."
};

const string usage {
"Usage is: despot2 [options] output_prefix T1_map ssfp_files\n\
\
Options:\n\
	--help, -h        : Print this message.\n\
	--mask, -m file   : Mask input with specified file.\n\
	--B0 file         : B0 Map file.\n\
	--B1 file         : B1 Map file.\n\
	--M0 file         : Proton density file.\n\
	--verbose, -v     : Print slice processing times.\n\
	--start_slice N   : Start processing from slice N.\n\
	--end_slice   N   : Finish processing at slice N.\n\
	--tesla, -t 3     : Enables DESPOT-FM with boundaries suitable for 3T\n\
	            7     : Boundaries suitable for 7T (default)\n\
	            u     : User specified boundaries from stdin.\n"

};

// tesla == 0 means NO DESPOT-FM
static int tesla = 0, fitB0 = false, verbose = false, start_slice = -1, end_slice = -1;
static string outPrefix;
static struct option long_options[] =
{
	{"B0", required_argument, 0, '0'},
	{"B1", required_argument, 0, '1'},
	{"M0", required_argument, 0, 'M'},
	{"help", no_argument, 0, 'h'},
	{"mask", required_argument, 0, 'm'},
	{"tesla", optional_argument, 0, 'f'},
	{"verbose", no_argument, 0, 'v'},
	{"start_slice", required_argument, 0, 'S'},
	{"end_slice", required_argument, 0, 'E'},
	{0, 0, 0, 0}
};

//******************************************************************************
// Main
//******************************************************************************
int main(int argc, char **argv)
{
	//**************************************************************************
	// Argument Processing
	//**************************************************************************
	cout << credit << endl;
	if (argc < 4) {
		cout << usage << endl;
		return EXIT_FAILURE;
	}
	Eigen::initParallel();
	NiftiImage inFile, savedHeader;
	double *maskData = NULL, *B0Data = NULL, *B1Data = NULL, *T1Data = NULL,
	       *M0Data = NULL;
	string procPath;

	int indexptr = 0, c;
	while ((c = getopt_long(argc, argv, "hm:vz", long_options, &indexptr)) != -1) {
		switch (c) {
			case 'm':
				cout << "Reading mask file " << optarg << endl;
				inFile.open(optarg, 'r');
				maskData = inFile.readVolume<double>(0);
				inFile.close();
				break;
			case '0':
				cout << "Reading B0 file " << optarg << endl;
				inFile.open(optarg, 'r');
				B0Data = inFile.readVolume<double>(0);
				inFile.close();
				break;
			case '1':
				cout << "Reading B1 file " << optarg;
				inFile.open(optarg, 'r');
				B1Data = inFile.readVolume<double>(0);
				inFile.close();
				break;
			case 'M':
				cout << "Reading M0 file " << optarg;
				inFile.open(optarg, 'r');
				M0Data = inFile.readVolume<double>(0);
				inFile.close();
				break;
			case 't':
				switch (*optarg) {
					case '3': tesla = 3; break;
					case '7': tesla = 7; break;
					case 'u': tesla = -1; break;
					default:
						cout << "Unknown boundaries type " << optarg << endl;
						abort();
						break;
				}
				cout << "Using " << tesla << "T boundaries." << endl;
				break;
			case 'v':
				verbose = true;
				break;
			case 'S':
				start_slice = atoi(optarg);
				break;
			case 'E':
				end_slice = atoi(optarg);
				break;
			case 0:
				// Just a flag
				break;
			case '?': // getopt will print an error message
			case 'h':
				cout << usage << endl;
				return EXIT_FAILURE;
		}
	}
	if ((tesla != 0) && !B0Data)
		fitB0 = true;
	cout << "Output prefix will be: " << argv[optind] << endl;
	outPrefix = argv[optind++];
	cout << "Reading T1 Map from: " << argv[optind] << endl;
	inFile.open(argv[optind++], 'r');
	T1Data = inFile.readVolume<double>(0);
	inFile.close();
	savedHeader = inFile;
	//**************************************************************************
	// Gather SSFP Data
	//**************************************************************************
	int nFlip, nPhases;
	double ssfpTR;
	nPhases = argc - optind;
	vector<double> ssfpPhases(nPhases);
	VectorXd ssfpAngles;
	int voxelsPerSlice, voxelsPerVolume;
	double **ssfpData = (double **)malloc(nPhases * sizeof(double *));
	for (size_t p = 0; p < nPhases; p++)
	{
		cout << "Reading SSFP header from " << argv[optind] << endl;
		inFile.open(argv[optind], 'r');
		if (p == 0)
		{	// Read nFlip, TR and flip angles from first file
			nFlip = inFile.dim(4);
			voxelsPerSlice = inFile.voxelsPerSlice();
			voxelsPerVolume = inFile.voxelsPerVolume();
			ssfpAngles.resize(nFlip, 1);

			#ifdef USE_PROCPAR
			ParameterList pars;
			if (ReadProcpar(inFile.basename() + ".procpar", pars)) {
				ssfpTR = RealValue(pars, "tr");
				for (int i = 0; i < nFlip; i++)
					ssfpAngles[i] = RealValue(pars, "flip1", i);
			} else
			#endif
			{
				cout << "Enter SSFP TR (s): " << flush;
				cin >> ssfpTR;
				cout << "Enter " << nFlip << " flip angles (degrees): " << flush;
				for (int i = 0; i < ssfpAngles.size(); i++)
					cin >> ssfpAngles[i];
			}
		}
		#ifdef USE_PROCPAR
		ParameterList pars;
		if (ReadProcpar(inFile.basename() + ".procpar", pars)) {
			ssfpPhases[p] = RealValue(pars, "rfphase") * M_PI / 180.;
		} else
		#endif
		{
			cout << "Enter phase-cycling (degrees): " << flush;
			cin >> ssfpPhases[p]; ssfpPhases[p] *= M_PI / 180.;
		}
		cout << "Reading SSFP data..." << endl;
		ssfpData[p] = inFile.readAllVolumes<double>();
		// Don't close the first header because we've saved it to write the
		// results, and FSLIO gets fussy about cloning closed headers
		inFile.close();
		optind++;
	}
	ssfpAngles *= M_PI / 180.;

	if (optind != argc) {
		cerr << "Unprocessed arguments supplied.\n" << usage;
		exit(EXIT_FAILURE);
	}

	// Set up boundaries for DESPOT-FM if needed
	DESPOT2FM::ParamType loBounds, hiBounds;
	if (tesla != 0) {
		if (tesla > 0) {
			loBounds = DESPOT2FM::loBounds(tesla);
			hiBounds = DESPOT2FM::hiBounds(tesla);
		} else if (tesla < 0) {
			cout << "Enter " << DESPOT2FM::nP << " parameter pairs (low then high): " << flush;
			for (int i = 0; i < DESPOT2FM::nP; i++) cin >> loBounds[i] >> hiBounds[i];
		}
		// If fitting, give a suitable range and allocate results memory
		if (fitB0) {
			loBounds[DESPOT2FM::nP - 1] = -0.5 / ssfpTR;
			hiBounds[DESPOT2FM::nP - 1] =  0.5 / ssfpTR;
			B0Data = new double[voxelsPerVolume];
		} else { // Otherwise fix and let functors pick up the specified value
			loBounds[DESPOT2FM::nP - 1] = 0.;
			hiBounds[DESPOT2FM::nP - 1] = 0.;
		}
	}

	if (verbose) {
		cout << "SSFP Angles (deg): " << ssfpAngles.transpose() * 180 / M_PI << endl;
		if (tesla != 0)
			cout << "Low bounds: " << loBounds.transpose() << endl
				 << "Hi bounds:  " << hiBounds.transpose() << endl;
	}
	//**************************************************************************
	// Set up results data
	//**************************************************************************
	double *residualData = new double[voxelsPerVolume];
	double *PDData = new double[voxelsPerVolume];
	double *T2Data = new double[voxelsPerVolume];
	//**************************************************************************
	// Do the fitting
	//**************************************************************************
    time_t procStart = time(NULL);
	if ((start_slice < 0) || (start_slice >= inFile.dim(3)))
		start_slice = 0;
	if ((end_slice < 0) || (end_slice > inFile.dim(3)))
		end_slice = inFile.dim(4);
	for (int slice = start_slice; slice < end_slice; slice++) {
		// Read in data
		if (verbose)
			cout << "Starting slice " << slice << "..." << flush;

		atomic<int> voxCount{0};
		const int sliceOffset = slice * voxelsPerSlice;
		clock_t loopStart = clock();
		function<void (const int&)> processVox = [&] (const int &vox) {
			// Set up parameters and constants
			double M0 = 0., T1 = 0., T2 = 0., B0 = 0, B1 = 1., residual = 0.;
			if (!maskData || ((maskData[sliceOffset + vox] > 0.) && (T1Data[sliceOffset + vox] > 0.)))
			{	// Zero T1 causes zero-pivot error.
				voxCount++;
				T1 = T1Data[sliceOffset + vox];
				if (B0Data) B0 = B0Data[sliceOffset + vox];
				if (B1Data)	B1 = B1Data[sliceOffset + vox];
				// Gather signals.
				vector<VectorXd> signals;
				for (int p = 0; p < nPhases; p++)
				{
					VectorXd temp(nFlip);
					for (int i = 0; i < nFlip; i++)
						temp(i) = ssfpData[p][i*voxelsPerVolume + sliceOffset + vox];
					signals.push_back(temp);
				}

				if (tesla == 0) {
					// Choose phase with accumulated phase closest to 180 and then classic DESPOT2
					int index = 0;
					double bestPhase = DBL_MAX;
					for (int p = 0; p < nPhases; p++) {
						double thisPhase = (B0 * ssfpTR * 2 * M_PI) + ssfpPhases[p];
						if (fabs(fmod(thisPhase - M_PI, 2 * M_PI)) < bestPhase) {
							bestPhase = fabs(fmod(thisPhase - M_PI, 2 * M_PI));
							index = p;
						}
					}
					residual = classicDESPOT2(ssfpAngles, signals[index], ssfpTR, T1, B1, M0, T2);
				} else {
					// DESPOT2-FM
					VectorXd allB0(nFlip), allB1(nFlip);
					allB0.setConstant(B0);
					allB1.setConstant(B1);
					DESPOT2FM tc(ssfpAngles, ssfpPhases, signals,
				                 allB0, allB1, ssfpTR, false, fitB0);
					DESPOT2FM::ParamType params(DESPOT2FM::nP);
					residual = regionContraction<DESPOT2FM>(params, tc, loBounds, hiBounds);
					M0 = params[0];
					T2 = params[1];
					if (fitB0)
						B0 = params[2];

				}
			}
			PDData[sliceOffset + vox] = clamp(M0, 0., 5.e3);
			T2Data[sliceOffset + vox] = clamp(T2, 0., 0.5);
			if (fitB0)
				B0Data[sliceOffset + vox] = B0;
			residualData[sliceOffset + vox] = residual;
		};
		apply_for(voxelsPerSlice, processVox);

		if (verbose) {
			clock_t loopEnd = clock();
			if (voxCount > 0)
				cout << voxCount << " unmasked voxels, CPU time per voxel was "
				          << ((loopEnd - loopStart) / ((float)voxCount * CLOCKS_PER_SEC)) << " s, ";
			cout << "finished." << endl;
		}
	}
    time_t procEnd = time(NULL);
    struct tm *localEnd = localtime(&procEnd);
	char theTime[512];
    strftime(theTime, 512, "%H:%M:%S", localEnd);
	cout << "Finished processing at " << theTime << ". Run-time was "
	          << difftime(procEnd, procStart) << " s." << endl;
	savedHeader.setDim(4, 1);
	savedHeader.setDatatype(NIFTI_TYPE_FLOAT32);
	savedHeader.open(outPrefix + "_T2.nii.gz", NiftiImage::NIFTI_WRITE);
	savedHeader.writeVolume(0, T2Data);
	savedHeader.close();
	savedHeader.open(outPrefix + "_PD.nii.gz", NiftiImage::NIFTI_WRITE);
	savedHeader.writeVolume(0, PDData);
	savedHeader.close();
	if (fitB0) {
		savedHeader.open(outPrefix + "_B0.nii.gz", NiftiImage::NIFTI_WRITE);
		savedHeader.writeVolume(0, B0Data);
		savedHeader.close();
	}
	// Clean up memory
	for (int p = 0; p < nPhases; p++)
		free(ssfpData[p]);
	if (B0Data)
		free(B0Data);
	if (B1Data)
		free(B1Data);
	if (maskData)
		free(maskData);
	return EXIT_SUCCESS;
}
<|endoftext|>


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

#include "RenderingAlgorithm.hh"
#include "Point4.hh"
#include "Varying.hh"

namespace clockwork {
namespace detail {
/**
 * The VertexShaderOutput is the result of a per-vertex operation (vertex shader)
 * on a set of vertex attributes.
 */
template<RenderingAlgorithm algorithm>
struct VertexShaderOutput {
	/**
	 * The vertex position in clip space.
	 */
	Point4 position;
	/**
	 * The vertex's varying variables.
	 */
	Varying<algorithm> varying;
};
} // namespace detail
} // namespace clockwork

#endif // CLOCKWORK_VERTEX_SHADER_OUTPUT_HH
<commit_msg>Implement the GenericVertexShaderOutput struct<commit_after>/*
 * This file is part of Clockwork.
 *
 * Copyright (c) 2014-2016 Jeremy Othieno.
 *
 * The MIT License (MIT)
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */
#ifndef CLOCKWORK_VERTEX_SHADER_OUTPUT_HH
#define CLOCKWORK_VERTEX_SHADER_OUTPUT_HH

#include "RenderingAlgorithm.hh"
#include "Point4.hh"
#include "Varying.hh"

namespace clockwork {
namespace detail {
namespace {
/**
 * The GenericVertexShaderOutput contains the minimum set of results obtained from
 * a per-vertex operation (vertex shader) applied to a set of vertex attributes.
 */
struct GenericVertexShaderOutput {
	/**
	 * The vertex position in clipping space.
	 */
	Point4 position;
};
} // namespace
/**
 *
 */
template<RenderingAlgorithm algorithm>
struct VertexShaderOutput : GenericVertexShaderOutput {
	/**
	 * The vertex's varying variables.
	 */
	Varying<algorithm> varying;
};
} // namespace detail
} // namespace clockwork

#endif // CLOCKWORK_VERTEX_SHADER_OUTPUT_HH
<|endoftext|>


<commit_before>// Copyright 2013 Sean McKenna
//
//    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.
//

// a ray tracer in C++

// libraries, namespace
#include <thread>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <cmath>
#include <random>
#include "library/loadXML.cpp"
#include "library/scene.cpp"
using namespace std;

// scene to load (project #) + all ray tracing options & settings
string xml = "scenes/prj11-1.xml";
bool printXML = false;
bool zBuffer = false;
bool sampleCount = false;
int bounceCount = 5;
int sampleMin = 4;
int sampleMax = 32;
float sampleThreshold = 0.001;
int shadowMin = 8;
int shadowMax = 32;
bool gammaCorr = true;
bool globalIllum = true;
bool irradMap = true;
int samplesGI = 16;

// variables for ray tracing
int w;
int h;
int size;
Color24* img;
float* zImg;
float* sampleImg;
IrradianceMap im;

// variables for anti-aliasing brightness calculations (XYZ, Lab)
float perR = 0.2126;
float perG = 0.7152;
float perB = 0.0722;
float Ycutoff = pow(6.0 / 29.0, 3.0);
float Yprecalc = (1.0 / 3.0) * pow(29.0 / 6.0, 2.0);

// setup threading
static const int numThreads = 8;
void rayTracing(int i);

// for camera ray generation
void cameraRayVars();
float imageDistance = 1.0;
Point *imageTopLeftV;
Point *dXV;
Point *dYV;
Point *dVx;
Point *dVy;
Point firstPixel;
Transformation* c;
Point cameraRay(float pX, float pY, Point offset);

// ray tracer
int main(){

  // load scene: root node, camera, image (and set shadow casting variables)
  loadScene(xml, printXML, shadowMin, shadowMax, globalIllum, samplesGI);

  // set the scene as the root node
  setScene(rootNode);

  // set variables for ray tracing
  w = render.getWidth();
  h = render.getHeight();
  size = render.getSize();
  img = render.getRender();
  zImg = render.getZBuffer();
  sampleImg = render.getSample();
  im.Initialize(w, h);

  // set variables for generating camera rays
  cameraRayVars();

  // start ray tracing loop (in parallel with threads)
  thread t[numThreads];
  for(int i = 0; i < numThreads; i++)
    t[i] = thread(rayTracing, i);

  // when finished, join all threads back to main
  for(int i = 0; i < numThreads; i++)
    t[i].join();

  // output ray-traced image & z-buffer & sample count image (if set)
  render.save("images/image.ppm");
  if(zBuffer){
    render.computeZImage();
    render.saveZImage("images/imageZ.ppm");
  }
  if(sampleCount){
    render.computeSampleImage();
    render.saveSampleImage("images/imageSample.ppm");
  }
}

// ray tracing loop (for an individual pixel)
void rayTracing(int i){

  // initial starting pixel
  int pixel = i;

  // setup random generator for anti-aliasing & depth-of-field
  mt19937 rnd;
  uniform_real_distribution<float> dist{0.0, 1.0};

  // thread continuation condition
  while(pixel < size){

    // number of samples
    int s = 0;

    // establish pixel location (center)
    float pX = pixel % w;
    float pY = pixel / w;

    // color values to store across samples
    Color col;
    Color colAvg;
    float zAvg = 0.0;
    float rVar = 0.0;
    float gVar = 0.0;
    float bVar = 0.0;
    float var = sampleThreshold;
    float brightness = 0.0;

    // random rotation of Halton sequence on circle of confusion
    float dcR = dist(rnd) * 2.0 * M_PI;

    // compute multi-adaptive sampling for each pixel (anti-aliasing)
    while(s < sampleMin || (s != sampleMax && (rVar * perR > var + brightness * var || gVar * perG > var + brightness * var || bVar * perB > var + brightness * var))){

      // grab Halton sequence to shift point by on image plane
      float dpX = centerHalton(Halton(s, 3));
      float dpY = centerHalton(Halton(s, 2));

      // grab Halton sequence to shift point along circle of confusion
      float dcS = sqrt(Halton(s, 2)) * camera.dof;

      // grab Halton sequence to shift point around circle of confusion
      float dcT = Halton(s, 3) * 2.0 * M_PI;

      // compute the offset for depth of field sampling
      Point posOffset = (*dVx * cos(dcR + dcT) + *dVy * sin(dcR + dcT)) * dcS;

      // transform ray into world space (offset by Halton seqeunce for sampling)
      Point rayDir = cameraRay(pX + dpX, pY + dpY, posOffset);
      Cone *ray = new Cone();
      ray->pos = camera.pos + c->transformFrom(posOffset);
      ray->dir = c->transformFrom(rayDir);
      ray->radius = 0.0;
      ray->tan = dXV->x / (2.0 * imageDistance);

      // traverse through scene DOM
      // transform rays into model space
      // detect ray intersections and get back HitInfo
      HitInfo hi = HitInfo();
      bool hit = traceRay(*ray, hi);

      // update z-buffer, if necessary
      if(zBuffer)
        zAvg = (zAvg * s + hi.z) / (float) (s + 1);

      // if hit, get the node's material
      if(hit){
        Node *n = hi.node;
        Material *m;
        if(n)
          m = n->getMaterial();

        // if there is a material, shade the pixel
        // 5-passes for reflections and refractions
        if(m)
          col = m->shade(*ray, hi, lights, bounceCount);

        // otherwise color it white (as a hit)
        else
          col.Set(0.929, 0.929, 0.929);

      // if we hit nothing, draw the background
      }else{
        Point p = Point((float) pX / w, (float) pY / h, 0.0);
        Color b = background.sample(p);
        col = b;
      }

      // compute average color
      float rAvg = (colAvg.r * s + col.r) / (float) (s + 1);
      float gAvg = (colAvg.g * s + col.g) / (float) (s + 1);
      float bAvg = (colAvg.b * s + col.b) / (float) (s + 1);
      colAvg.Set(rAvg, gAvg, bAvg);

      // compute color variances
      rVar = (rVar * s + (col.r - rAvg) * (col.r - rAvg)) / (float) (s + 1);
      gVar = (gVar * s + (col.g - gAvg) * (col.g - gAvg)) / (float) (s + 1);
      bVar = (bVar * s + (col.b - bAvg) * (col.b - bAvg)) / (float) (s + 1);

      // calculate and update brightness average using XYZ and Lab space
      float Y = perR * rAvg + perG * gAvg + perB * bAvg;
      float Y13 = Y;
      if(Y13 > Ycutoff)
        Y13 = pow(Y13, 1.0 / 3.0);
      else
        Y13 = Yprecalc * Y13 + (4.0 / 29.0);
      brightness = (116.0 * Y13 - 16.0) / 100.0;

      // increment sample count
      s++;

      // watch for errors at any individual sample, terminate thread if so
      if(colAvg[0] != colAvg[0] || colAvg[1] != colAvg[1] || colAvg[2] != colAvg[2]){
        cout << "ERROR - pixel " << pixel << " & sample " << s << endl;
        s = sampleMax;
        pixel = size;
      }
    }

    // gamma correction
    if(gammaCorr){
      colAvg.r = pow(colAvg.r, 1.0 / 2.2);
      colAvg.g = pow(colAvg.g, 1.0 / 2.2);
      colAvg.b = pow(colAvg.b, 1.0 / 2.2);
    }

    // color the pixel image
    img[pixel] = Color24(colAvg);

    // update the z-buffer image, if necessary
    if(zBuffer)
      zImg[pixel] = zAvg;

    // update the sample count image, if necessary
    if(sampleCount)
      sampleImg[pixel] = s;

    // re-assign next pixel (naive, but works)
    pixel += numThreads;
  }
}

// create variables for camera ray generation
void cameraRayVars(){
  float fov = camera.fov * M_PI / 180.0;
  float aspectRatio = (float) w / (float) h;
  imageDistance = camera.focalDist;
  float imageTipY = imageDistance * tan(fov / 2.0);
  float imageTipX = imageTipY * aspectRatio;
  float dX = (2.0 * imageTipX) / (float) w;
  float dY = (2.0 * imageTipY) / (float) h;
  imageTopLeftV = new Point(-imageTipX, imageTipY, -imageDistance);
  dXV = new Point(dX, 0.0, 0.0);
  dYV = new Point(0.0, -dY, 0.0);
  firstPixel = *imageTopLeftV + (*dXV * 0.5) + (*dYV * 0.5);

  // set up camera transformation (only need to rotate coordinates)
  c = new Transformation();
  Matrix *rotate = new cyMatrix3f();
  rotate->Set(camera.cross, camera.up, -camera.dir);
  c->transform(*rotate);

  // get normalized rays on the focal plane
  dVx = new Point(1.0, 0.0, 0.0);
  dVy = new Point(0.0, 1.0, 0.0);
}

// compute camera ray direction
Point cameraRay(float pX, float pY, Point offset){
  Point ray = firstPixel + (*dXV * pX) + (*dYV * pY) - offset;
  ray.Normalize();
  return ray;
}
<commit_msg>begin adding initialization and subdivision of the irradiance map<commit_after>// Copyright 2013 Sean McKenna
//
//    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.
//

// a ray tracer in C++

// libraries, namespace
#include <thread>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <cmath>
#include <random>
#include "library/loadXML.cpp"
#include "library/scene.cpp"
using namespace std;

// scene to load (project #) + all ray tracing options & settings
string xml = "scenes/prj11-1.xml";
bool printXML = false;
bool zBuffer = false;
bool sampleCount = false;
int bounceCount = 5;
int sampleMin = 4;
int sampleMax = 32;
float sampleThreshold = 0.001;
int shadowMin = 8;
int shadowMax = 32;
bool gammaCorr = true;
bool globalIllum = true;
bool irradMap = true;
int samplesGI = 16;

// variables for ray tracing
int w;
int h;
int size;
Color24* img;
float* zImg;
float* sampleImg;
IrradianceMap im;

// variables for anti-aliasing brightness calculations (XYZ, Lab)
float perR = 0.2126;
float perG = 0.7152;
float perB = 0.0722;
float Ycutoff = pow(6.0 / 29.0, 3.0);
float Yprecalc = (1.0 / 3.0) * pow(29.0 / 6.0, 2.0);

// setup threading
static const int numThreads = 8;
void rayTracing(int i);

// for camera ray generation
void cameraRayVars();
float imageDistance = 1.0;
Point *imageTopLeftV;
Point *dXV;
Point *dYV;
Point *dVx;
Point *dVy;
Point firstPixel;
Transformation* c;
Point cameraRay(float pX, float pY, Point offset);

// ray tracer
int main(){

  // load scene: root node, camera, image (and set shadow casting variables)
  loadScene(xml, printXML, shadowMin, shadowMax, globalIllum, samplesGI);

  // set the scene as the root node
  setScene(rootNode);

  // set variables for ray tracing
  w = render.getWidth();
  h = render.getHeight();
  size = render.getSize();
  img = render.getRender();
  zImg = render.getZBuffer();
  sampleImg = render.getSample();
  if(irradMap)
    im.Initialize(w, h);

  // set variables for generating camera rays
  cameraRayVars();

  // start irradiance map computation
  if(irradMap){

    // subdivide our image to compute indirect illumination
    bool subdivide = true;
    while(subdivide){

      // check if we are on final subdivide
      if(im.GetSubdivLevel() == 0)
        subdivide = false;

      // calculate indirect illumination

      // subdivide (if necessary)
      if(subdivide)
        im.Subdivide();
    }
  }

  // sample test output (know irrad map done)
  cout << "onto ray tracing!" << endl;

  // start ray tracing loop (in parallel with threads)
  thread t[numThreads];
  for(int i = 0; i < numThreads; i++)
    t[i] = thread(rayTracing, i);

  // when finished, join all threads back to main
  for(int i = 0; i < numThreads; i++)
    t[i].join();

  // output ray-traced image & z-buffer & sample count image (if set)
  render.save("images/image.ppm");
  if(zBuffer){
    render.computeZImage();
    render.saveZImage("images/imageZ.ppm");
  }
  if(sampleCount){
    render.computeSampleImage();
    render.saveSampleImage("images/imageSample.ppm");
  }
}

// ray tracing loop (for an individual pixel)
void rayTracing(int i){

  // initial starting pixel
  int pixel = i;

  // setup random generator for anti-aliasing & depth-of-field
  mt19937 rnd;
  uniform_real_distribution<float> dist{0.0, 1.0};

  // thread continuation condition
  while(pixel < size){

    // number of samples
    int s = 0;

    // establish pixel location (center)
    float pX = pixel % w;
    float pY = pixel / w;

    // color values to store across samples
    Color col;
    Color colAvg;
    float zAvg = 0.0;
    float rVar = 0.0;
    float gVar = 0.0;
    float bVar = 0.0;
    float var = sampleThreshold;
    float brightness = 0.0;

    // random rotation of Halton sequence on circle of confusion
    float dcR = dist(rnd) * 2.0 * M_PI;

    // compute multi-adaptive sampling for each pixel (anti-aliasing)
    while(s < sampleMin || (s != sampleMax && (rVar * perR > var + brightness * var || gVar * perG > var + brightness * var || bVar * perB > var + brightness * var))){

      // grab Halton sequence to shift point by on image plane
      float dpX = centerHalton(Halton(s, 3));
      float dpY = centerHalton(Halton(s, 2));

      // grab Halton sequence to shift point along circle of confusion
      float dcS = sqrt(Halton(s, 2)) * camera.dof;

      // grab Halton sequence to shift point around circle of confusion
      float dcT = Halton(s, 3) * 2.0 * M_PI;

      // compute the offset for depth of field sampling
      Point posOffset = (*dVx * cos(dcR + dcT) + *dVy * sin(dcR + dcT)) * dcS;

      // transform ray into world space (offset by Halton seqeunce for sampling)
      Point rayDir = cameraRay(pX + dpX, pY + dpY, posOffset);
      Cone *ray = new Cone();
      ray->pos = camera.pos + c->transformFrom(posOffset);
      ray->dir = c->transformFrom(rayDir);
      ray->radius = 0.0;
      ray->tan = dXV->x / (2.0 * imageDistance);

      // traverse through scene DOM
      // transform rays into model space
      // detect ray intersections and get back HitInfo
      HitInfo hi = HitInfo();
      bool hit = traceRay(*ray, hi);

      // update z-buffer, if necessary
      if(zBuffer)
        zAvg = (zAvg * s + hi.z) / (float) (s + 1);

      // if hit, get the node's material
      if(hit){
        Node *n = hi.node;
        Material *m;
        if(n)
          m = n->getMaterial();

        // if there is a material, shade the pixel
        // 5-passes for reflections and refractions
        if(m)
          col = m->shade(*ray, hi, lights, bounceCount);

        // otherwise color it white (as a hit)
        else
          col.Set(0.929, 0.929, 0.929);

      // if we hit nothing, draw the background
      }else{
        Point p = Point((float) pX / w, (float) pY / h, 0.0);
        Color b = background.sample(p);
        col = b;
      }

      // compute average color
      float rAvg = (colAvg.r * s + col.r) / (float) (s + 1);
      float gAvg = (colAvg.g * s + col.g) / (float) (s + 1);
      float bAvg = (colAvg.b * s + col.b) / (float) (s + 1);
      colAvg.Set(rAvg, gAvg, bAvg);

      // compute color variances
      rVar = (rVar * s + (col.r - rAvg) * (col.r - rAvg)) / (float) (s + 1);
      gVar = (gVar * s + (col.g - gAvg) * (col.g - gAvg)) / (float) (s + 1);
      bVar = (bVar * s + (col.b - bAvg) * (col.b - bAvg)) / (float) (s + 1);

      // calculate and update brightness average using XYZ and Lab space
      float Y = perR * rAvg + perG * gAvg + perB * bAvg;
      float Y13 = Y;
      if(Y13 > Ycutoff)
        Y13 = pow(Y13, 1.0 / 3.0);
      else
        Y13 = Yprecalc * Y13 + (4.0 / 29.0);
      brightness = (116.0 * Y13 - 16.0) / 100.0;

      // increment sample count
      s++;

      // watch for errors at any individual sample, terminate thread if so
      if(colAvg[0] != colAvg[0] || colAvg[1] != colAvg[1] || colAvg[2] != colAvg[2]){
        cout << "ERROR - pixel " << pixel << " & sample " << s << endl;
        s = sampleMax;
        pixel = size;
      }
    }

    // gamma correction
    if(gammaCorr){
      colAvg.r = pow(colAvg.r, 1.0 / 2.2);
      colAvg.g = pow(colAvg.g, 1.0 / 2.2);
      colAvg.b = pow(colAvg.b, 1.0 / 2.2);
    }

    // color the pixel image
    img[pixel] = Color24(colAvg);

    // update the z-buffer image, if necessary
    if(zBuffer)
      zImg[pixel] = zAvg;

    // update the sample count image, if necessary
    if(sampleCount)
      sampleImg[pixel] = s;

    // re-assign next pixel (naive, but works)
    pixel += numThreads;
  }
}

// create variables for camera ray generation
void cameraRayVars(){
  float fov = camera.fov * M_PI / 180.0;
  float aspectRatio = (float) w / (float) h;
  imageDistance = camera.focalDist;
  float imageTipY = imageDistance * tan(fov / 2.0);
  float imageTipX = imageTipY * aspectRatio;
  float dX = (2.0 * imageTipX) / (float) w;
  float dY = (2.0 * imageTipY) / (float) h;
  imageTopLeftV = new Point(-imageTipX, imageTipY, -imageDistance);
  dXV = new Point(dX, 0.0, 0.0);
  dYV = new Point(0.0, -dY, 0.0);
  firstPixel = *imageTopLeftV + (*dXV * 0.5) + (*dYV * 0.5);

  // set up camera transformation (only need to rotate coordinates)
  c = new Transformation();
  Matrix *rotate = new cyMatrix3f();
  rotate->Set(camera.cross, camera.up, -camera.dir);
  c->transform(*rotate);

  // get normalized rays on the focal plane
  dVx = new Point(1.0, 0.0, 0.0);
  dVy = new Point(0.0, 1.0, 0.0);
}

// compute camera ray direction
Point cameraRay(float pX, float pY, Point offset){
  Point ray = firstPixel + (*dXV * pX) + (*dYV * pY) - offset;
  ray.Normalize();
  return ray;
}
<|endoftext|>