<commit_before>#include "stdafx.h"
#include "../stdafx.h"
#include "SnakeEntity.h"

// Determine priority for path node (in the priority queue)
bool operator<(const PathNode & a, const PathNode & b)
{
	return a.Priority > b.Priority;
}

SnakeEntity::SnakeEntity(std::map<std::pair<int, int>, LevelSegment>* _level, int _initial_size, int _level_width, int _level_height, std::pair<int, int>* _level_food_location)
{
	LevelGrid = _level;
	LevelWidth = _level_width;
	LevelHeight = _level_height;
	SnakeFoodLocation = _level_food_location;
	isDead = false;
	isFoodCollected = false;

	// Create starting snake
	int startPosX = LevelWidth / 2;
	int startPosY = LevelHeight / 2;

	SnakePart* tmpPart = new SnakePart();
	tmpPart->Location = std::make_pair(startPosX, startPosY);
	tmpPart->LastMovement = SNAKE_MOVE_LEFT;
	tmpPart->NewPart = false;
	SnakeParts.push_back(tmpPart);
	for (int i = 1; i < _initial_size; i++)
	{
		tmpPart = new SnakePart();
		tmpPart->Location = std::make_pair(startPosX + i, startPosY);
		tmpPart->LastMovement = SNAKE_MOVE_LEFT;
		tmpPart->NewPart = false;
		SnakeParts.push_back(tmpPart);
	}

	SnakeDirections[SNAKE_MOVE_UP] = std::make_pair(0, -1);
	SnakeDirections[SNAKE_MOVE_DOWN] = std::make_pair(0, 1);
	SnakeDirections[SNAKE_MOVE_LEFT] = std::make_pair(-1, 0);
	SnakeDirections[SNAKE_MOVE_RIGHT] = std::make_pair(1, 0);

	UpdateBody();
}

SnakeEntity::~SnakeEntity()
{
}

void SnakeEntity::ClearBody()
{
	for (std::vector<std::pair<int, int>>::iterator itr = SnakePartsOld.begin(); itr != SnakePartsOld.end(); ++itr)
	{
		(*LevelGrid)[(*itr)] = LEVEL_SEGMENT_BLANK;
	}
	SnakePartsOld.clear();
}

void SnakeEntity::UpdateBody()
{
	ClearBody(); // Clear the old body and add the new one

	for (std::vector<SnakePart*>::iterator itr = SnakeParts.begin(); itr != SnakeParts.end(); ++itr)
	{
		if (itr == SnakeParts.begin()) // First element should always be a head
		{
			(*LevelGrid)[(*itr)->Location] = LEVEL_SEGMENT_PLAYER_SNAKE_HEAD;
		}
		else
		{
			(*LevelGrid)[(*itr)->Location] = LEVEL_SEGMENT_PLAYER_SNAKE;
		}
	}
}

void SnakeEntity::Move(SnakeMovement _Direction)
{
	for (std::vector<SnakePart*>::iterator itr = SnakeParts.begin(); itr != SnakeParts.end(); ++itr)
	{
		SnakePartsOld.push_back((*itr)->Location);
	}

	SnakePart* snakeHead = SnakeParts.front();
	std::pair<int, int> newPosition = std::make_pair(SnakeDirections[_Direction].first + snakeHead->Location.first, SnakeDirections[_Direction].second + snakeHead->Location.second);

	if (!Collision(newPosition)) // Make sure we dont collide with anything
	{
		SnakeParts.front()->Location = newPosition; // Update snake head position
		SnakeMovement parentMovement = SnakeParts.front()->LastMovement;
		SnakeMovement oldMovement;
		std::pair<int, int> lastPosition;

		// Update body
		for (std::vector<SnakePart*>::iterator itr = SnakeParts.begin(); itr != SnakeParts.end(); ++itr)
		{
			if (itr == SnakeParts.begin()) // Ignore head
				continue;

			if ((*itr)->NewPart) // Expand snake to add new part
			{
				(*itr)->Location = lastPosition;
				(*itr)->LastMovement = oldMovement;
				(*itr)->NewPart = false;
			}
			else
			{
				lastPosition = (*itr)->Location;
				newPosition = std::make_pair(SnakeDirections[parentMovement].first + (*itr)->Location.first, SnakeDirections[parentMovement].second + (*itr)->Location.second);
				(*itr)->Location = newPosition;
				oldMovement = (*itr)->LastMovement;
				(*itr)->LastMovement = parentMovement;
				parentMovement = oldMovement;
			}
		}

		SnakeParts.front()->LastMovement = _Direction;
	}

	if (isDead)
	{
		printf("SNAKE: Dead!\n");
	}
}

bool SnakeEntity::CanMove(LevelSegment _SegmentType)
{
	switch (_SegmentType)
	{
	case LEVEL_SEGMENT_BLANK:
	case LEVEL_SEGMENT_PLAYER_FOOD:
		return true;
	default:
	case LEVEL_SEGMENT_WALL:
	case LEVEL_SEGMENT_PLAYER_SNAKE:
	case LEVEL_SEGMENT_PLAYER_SNAKE_HEAD: // Odd case check would never happen, never hurts to add it though
		return false;
	}
}

bool SnakeEntity::Collision(std::pair<int, int> _Location)
{
	LevelSegment segmentCheck = (*LevelGrid)[_Location];
	switch (segmentCheck)
	{
	case LEVEL_SEGMENT_PLAYER_FOOD:
		isFoodCollected = true;
		break;
	case LEVEL_SEGMENT_WALL:
	case LEVEL_SEGMENT_PLAYER_SNAKE:
	case LEVEL_SEGMENT_PLAYER_SNAKE_HEAD: // Odd case check would never happen, never hurts to add it though
		isDead = true;
		break;
	}

	if (isFoodCollected) // If food is collected increase snake length by 1
	{
		SnakePart* tmpPart = new SnakePart();
		tmpPart->NewPart = true;
		SnakeParts.push_back(tmpPart);
	}

	return !CanMove(segmentCheck);
}

SnakeMovement SnakeEntity::GetOppositeMovement(SnakeMovement _Movement)
{
	switch (_Movement)
	{
	case SNAKE_MOVE_UP:
		return SNAKE_MOVE_DOWN;
	case SNAKE_MOVE_DOWN:
		return SNAKE_MOVE_UP;
	case SNAKE_MOVE_LEFT:
		return SNAKE_MOVE_RIGHT;
	default:
	case SNAKE_MOVE_RIGHT:
		return SNAKE_MOVE_LEFT;
	}
}

// Path finding
bool SnakeEntity::CalculatePath_Try1(std::pair<int, int> _ToGridReference)
{
	std::priority_queue<PathNode> possibleOpenNodesQueue[2];
	int queueIndex = 0;
	PathNode* gridNode;
	PathNode* gridChildNode;
	int gridX, gridY;
	std::pair<int, int> gridLocation, startPosition;
	startPosition = SnakeParts.front()->Location;

	// Clear nodes
	PathClosedNodes.clear();
	PathOpenNodes.clear();

	gridNode = new PathNode(startPosition, 0, 0);
	gridNode->UpdatePriority(_ToGridReference);

	possibleOpenNodesQueue[queueIndex].push(*gridNode);
	PathOpenNodes[gridNode->Position] = gridNode->Priority; // Mark first point

	while (!possibleOpenNodesQueue[queueIndex].empty())
	{
		gridNode = new PathNode(possibleOpenNodesQueue[queueIndex].top().Position, possibleOpenNodesQueue[queueIndex].top().Depth, possibleOpenNodesQueue[queueIndex].top().Priority);
		gridX = gridNode->Position.first;
		gridY = gridNode->Position.second;
		gridLocation = std::make_pair(gridX, gridY);

		possibleOpenNodesQueue[queueIndex].pop();
		PathOpenNodes[gridLocation] = 0;
		PathClosedNodes[gridLocation] = 1;

		if (gridLocation == _ToGridReference)
		{
			while (gridLocation != startPosition)
			{
				// Fill Snake path
				SnakeMovement tmpMovement = PathDirections[gridLocation];
				SnakePath.push_back(GetOppositeMovement(tmpMovement));
				gridX += SnakeDirections[tmpMovement].first;
				gridY += SnakeDirections[tmpMovement].second;
				gridLocation = std::make_pair(gridX, gridY);
			}
			printf("SNAKE (FOOD FOUND): X: %i Y: %i\n", gridX, gridY);
			delete gridNode;
			// Empty unused nodes
			while (!possibleOpenNodesQueue[queueIndex].empty()) possibleOpenNodesQueue[queueIndex].pop();
			return true;
		}

		for (int i = 0; i < SnakeDirections.size(); i++)
		{
			int xDirection = gridX + SnakeDirections[(SnakeMovement)i].first;
			int yDirection = gridY + SnakeDirections[(SnakeMovement)i].second;
			std::pair<int, int> gridDirection = std::make_pair(xDirection, yDirection);

			// Check to see if snake can move there
			if (CanMove((*LevelGrid)[gridDirection]) || PathClosedNodes[gridDirection] != 1)
			{
				gridChildNode = new PathNode(gridDirection, gridNode->Depth, gridNode->Priority);
				gridChildNode->NextDepth((SnakeMovement)i);
				gridChildNode->UpdatePriority(_ToGridReference);

				if (PathOpenNodes.count(gridDirection) == 0)
				{
					PathOpenNodes[gridDirection] = gridChildNode->Priority;
					possibleOpenNodesQueue[queueIndex].push(*gridChildNode);
					PathDirections[gridDirection] = GetOppositeMovement((SnakeMovement)i);
				}
				else if (PathOpenNodes[gridDirection] > gridChildNode->Priority)
				{
					PathOpenNodes[gridDirection] = gridChildNode->Priority;
					PathDirections[gridDirection] = GetOppositeMovement((SnakeMovement)i);

					while (!(possibleOpenNodesQueue[queueIndex].top().Position == gridDirection))
					{
						possibleOpenNodesQueue[1 - queueIndex].push(possibleOpenNodesQueue[queueIndex].top());
						possibleOpenNodesQueue[queueIndex].pop();
					}
					possibleOpenNodesQueue[queueIndex].pop();

					if (possibleOpenNodesQueue[queueIndex].size() > possibleOpenNodesQueue[1 - queueIndex].size()) queueIndex = 1 - queueIndex;
					while (!possibleOpenNodesQueue[queueIndex].empty())
					{
						possibleOpenNodesQueue[1 - queueIndex].push(possibleOpenNodesQueue[queueIndex].top());
						possibleOpenNodesQueue[queueIndex].pop();
					}

					queueIndex = 1 - queueIndex;
					possibleOpenNodesQueue[queueIndex].push(*gridChildNode);
				}
				else
				{
					delete gridChildNode;
				}
			}
		}
		delete gridNode;
	}
	return false;
}

bool CalculatePath_Try2(std::pair<int, int> _ToGridReference)
{
	// Different algorithm for path finding. BFS?
	return false;
}

void SnakeEntity::MoveOnPath()
{
	if (!SnakePath.empty()) // Move only if there is a next path
	{
		Move(SnakePath.front());
		SnakePath.erase(SnakePath.begin());
	}
	else
	{
		if (!CalculatePath_Try1(std::make_pair(SnakeFoodLocation->first, SnakeFoodLocation->second))) // Calculate route to food
		{
			Move(SNAKE_MOVE_LEFT); // TODO: More intelligent way of movement if no path is currently available
		}
	}
}

void SnakeEntity::Unload()
{
	LevelGrid = nullptr;
	SnakeParts.clear();
	SnakePath.clear();
	isDead = false;
	isFoodCollected = false;
}

void SnakeEntity::Update(float _DeltaTime)
{
	if (!isDead) // Dont update if dead
	{
		// Move the snake on the calculated path, or if no path exists calculate one.
		MoveOnPath();

		// Update snake body to map with new positions
		UpdateBody();
	}
}<commit_msg>Improve memory usage, more consistency<commit_after>#include "stdafx.h"
#include "../stdafx.h"
#include "SnakeEntity.h"

// Determine priority for path node (in the priority queue)
bool operator<(const PathNode & a, const PathNode & b)
{
	return a.Priority > b.Priority;
}

SnakeEntity::SnakeEntity(std::map<std::pair<int, int>, LevelSegment>* _level, int _initial_size, int _level_width, int _level_height, std::pair<int, int>* _level_food_location)
{
	LevelGrid = _level;
	LevelWidth = _level_width;
	LevelHeight = _level_height;
	SnakeFoodLocation = _level_food_location;
	isDead = false;
	isFoodCollected = false;

	// Create starting snake
	int startPosX = LevelWidth / 2;
	int startPosY = LevelHeight / 2;

	SnakePart* tmpPart = new SnakePart();
	tmpPart->Location = std::make_pair(startPosX, startPosY);
	tmpPart->LastMovement = SNAKE_MOVE_LEFT;
	tmpPart->NewPart = false;
	SnakeParts.push_back(tmpPart);
	for (int i = 1; i < _initial_size; i++)
	{
		tmpPart = new SnakePart();
		tmpPart->Location = std::make_pair(startPosX + i, startPosY);
		tmpPart->LastMovement = SNAKE_MOVE_LEFT;
		tmpPart->NewPart = false;
		SnakeParts.push_back(tmpPart);
	}

	SnakeDirections[SNAKE_MOVE_UP] = std::make_pair(0, -1);
	SnakeDirections[SNAKE_MOVE_DOWN] = std::make_pair(0, 1);
	SnakeDirections[SNAKE_MOVE_LEFT] = std::make_pair(-1, 0);
	SnakeDirections[SNAKE_MOVE_RIGHT] = std::make_pair(1, 0);

	UpdateBody();
}

SnakeEntity::~SnakeEntity()
{
}

void SnakeEntity::ClearBody()
{
	for (std::vector<std::pair<int, int>>::iterator itr = SnakePartsOld.begin(); itr != SnakePartsOld.end(); ++itr)
	{
		(*LevelGrid)[(*itr)] = LEVEL_SEGMENT_BLANK;
	}
	SnakePartsOld.clear();
}

void SnakeEntity::UpdateBody()
{
	ClearBody(); // Clear the old body and add the new one

	for (std::vector<SnakePart*>::iterator itr = SnakeParts.begin(); itr != SnakeParts.end(); ++itr)
	{
		if (itr == SnakeParts.begin()) // First element should always be a head
		{
			(*LevelGrid)[(*itr)->Location] = LEVEL_SEGMENT_PLAYER_SNAKE_HEAD;
		}
		else
		{
			(*LevelGrid)[(*itr)->Location] = LEVEL_SEGMENT_PLAYER_SNAKE;
		}
	}
}

void SnakeEntity::Move(SnakeMovement _Direction)
{
	for (std::vector<SnakePart*>::iterator itr = SnakeParts.begin(); itr != SnakeParts.end(); ++itr)
	{
		SnakePartsOld.push_back((*itr)->Location);
	}

	SnakePart* snakeHead = SnakeParts.front();
	std::pair<int, int> newPosition = std::make_pair(SnakeDirections[_Direction].first + snakeHead->Location.first, SnakeDirections[_Direction].second + snakeHead->Location.second);

	if (!Collision(newPosition)) // Make sure we dont collide with anything
	{
		SnakeParts.front()->Location = newPosition; // Update snake head position
		SnakeMovement parentMovement = SnakeParts.front()->LastMovement;
		SnakeMovement oldMovement;
		std::pair<int, int> lastPosition;

		// Update body
		for (std::vector<SnakePart*>::iterator itr = SnakeParts.begin(); itr != SnakeParts.end(); ++itr)
		{
			if (itr == SnakeParts.begin()) // Ignore head
				continue;

			if ((*itr)->NewPart) // Expand snake to add new part
			{
				(*itr)->Location = lastPosition;
				(*itr)->LastMovement = oldMovement;
				(*itr)->NewPart = false;
			}
			else
			{
				lastPosition = (*itr)->Location;
				newPosition = std::make_pair(SnakeDirections[parentMovement].first + (*itr)->Location.first, SnakeDirections[parentMovement].second + (*itr)->Location.second);
				(*itr)->Location = newPosition;
				oldMovement = (*itr)->LastMovement;
				(*itr)->LastMovement = parentMovement;
				parentMovement = oldMovement;
			}
		}

		SnakeParts.front()->LastMovement = _Direction;
	}

	if (isDead)
	{
		printf("SNAKE: Dead!\n");
	}
}

bool SnakeEntity::CanMove(LevelSegment _SegmentType)
{
	switch (_SegmentType)
	{
	case LEVEL_SEGMENT_BLANK:
	case LEVEL_SEGMENT_PLAYER_FOOD:
		return true;
	default:
	case LEVEL_SEGMENT_WALL:
	case LEVEL_SEGMENT_PLAYER_SNAKE:
	case LEVEL_SEGMENT_PLAYER_SNAKE_HEAD: // Odd case check would never happen, never hurts to add it though
		return false;
	}
}

bool SnakeEntity::Collision(std::pair<int, int> _Location)
{
	LevelSegment segmentCheck = (*LevelGrid)[_Location];
	switch (segmentCheck)
	{
	case LEVEL_SEGMENT_PLAYER_FOOD:
		isFoodCollected = true;
		break;
	case LEVEL_SEGMENT_WALL:
	case LEVEL_SEGMENT_PLAYER_SNAKE:
	case LEVEL_SEGMENT_PLAYER_SNAKE_HEAD: // Odd case check would never happen, never hurts to add it though
		isDead = true;
		break;
	}

	if (isFoodCollected) // If food is collected increase snake length by 1
	{
		SnakePart* tmpPart = new SnakePart();
		tmpPart->NewPart = true;
		SnakeParts.push_back(tmpPart);
	}

	return !CanMove(segmentCheck);
}

SnakeMovement SnakeEntity::GetOppositeMovement(SnakeMovement _Movement)
{
	switch (_Movement)
	{
	case SNAKE_MOVE_UP:
		return SNAKE_MOVE_DOWN;
	case SNAKE_MOVE_DOWN:
		return SNAKE_MOVE_UP;
	case SNAKE_MOVE_LEFT:
		return SNAKE_MOVE_RIGHT;
	default:
	case SNAKE_MOVE_RIGHT:
		return SNAKE_MOVE_LEFT;
	}
}

// Path finding
bool SnakeEntity::CalculatePath_Try1(std::pair<int, int> _ToGridReference)
{
	std::priority_queue<PathNode> possibleOpenNodesQueue[2];
	int queueIndex = 0;
	PathNode* gridNode;
	PathNode* gridChildNode;
	int gridX, gridY, xDirection, yDirection;
	std::pair<int, int> gridLocation, gridDirection, startPosition;
	startPosition = SnakeParts.front()->Location;

	// Clear nodes
	PathClosedNodes.clear();
	PathOpenNodes.clear();

	gridNode = new PathNode(startPosition, 0, 0);
	gridNode->UpdatePriority(_ToGridReference);

	possibleOpenNodesQueue[queueIndex].push(*gridNode);
	PathOpenNodes[startPosition] = gridNode->Priority; // Mark first point

	while (!possibleOpenNodesQueue[queueIndex].empty())
	{
		gridNode = new PathNode(possibleOpenNodesQueue[queueIndex].top().Position, possibleOpenNodesQueue[queueIndex].top().Depth, possibleOpenNodesQueue[queueIndex].top().Priority);
		gridX = gridNode->Position.first;
		gridY = gridNode->Position.second;
		gridLocation = std::make_pair(gridX, gridY);

		possibleOpenNodesQueue[queueIndex].pop();
		PathOpenNodes[gridLocation] = 0;
		PathClosedNodes[gridLocation] = 1;

		if (gridLocation == _ToGridReference)
		{
			while (gridLocation != startPosition)
			{
				// Fill Snake path
				SnakeMovement tmpMovement = PathDirections[gridLocation];
				gridX += SnakeDirections[tmpMovement].first;
				gridY += SnakeDirections[tmpMovement].second;
				gridLocation = std::make_pair(gridX, gridY);
				SnakePath.push_back(GetOppositeMovement(tmpMovement));
			}
			delete gridNode;
			// Empty unused nodes
			while (!possibleOpenNodesQueue[queueIndex].empty())
			{
				possibleOpenNodesQueue[queueIndex].pop();
			}
			printf("SNAKE (FOOD FOUND): X: %i Y: %i\n", gridX, gridY);
			return true;
		}

		for (int i = 0; i < SnakeDirections.size(); i++)
		{
			xDirection = gridX + SnakeDirections[(SnakeMovement)i].first;
			yDirection = gridY + SnakeDirections[(SnakeMovement)i].second;
			gridDirection = std::make_pair(xDirection, yDirection);

			// Check to see if snake can move there
			if (CanMove((*LevelGrid)[gridDirection]) || PathClosedNodes[gridDirection] != 1)
			{
				gridChildNode = new PathNode(gridDirection, gridNode->Depth, gridNode->Priority);
				gridChildNode->NextDepth((SnakeMovement)i);
				gridChildNode->UpdatePriority(_ToGridReference);

				if (PathOpenNodes.count(gridDirection) == 0)
				{
					PathOpenNodes[gridDirection] = gridChildNode->Priority;
					possibleOpenNodesQueue[queueIndex].push(*gridChildNode);
					PathDirections[gridDirection] = GetOppositeMovement((SnakeMovement)i);
				}
				else if (PathOpenNodes[gridDirection] > gridChildNode->Priority)
				{
					PathOpenNodes[gridDirection] = gridChildNode->Priority;
					PathDirections[gridDirection] = GetOppositeMovement((SnakeMovement)i);

					while (possibleOpenNodesQueue[queueIndex].top().Position != gridDirection)
					{
						possibleOpenNodesQueue[1 - queueIndex].push(possibleOpenNodesQueue[queueIndex].top());
						possibleOpenNodesQueue[queueIndex].pop();
					}
					possibleOpenNodesQueue[queueIndex].pop();

					if (possibleOpenNodesQueue[queueIndex].size() > possibleOpenNodesQueue[1 - queueIndex].size())
					{
						queueIndex = 1 - queueIndex;
					}

					while (!possibleOpenNodesQueue[queueIndex].empty())
					{
						possibleOpenNodesQueue[1 - queueIndex].push(possibleOpenNodesQueue[queueIndex].top());
						possibleOpenNodesQueue[queueIndex].pop();
					}

					queueIndex = 1 - queueIndex;
					possibleOpenNodesQueue[queueIndex].push(*gridChildNode);
				}
				else
				{
					delete gridChildNode;
				}
			}
		}
		delete gridNode;
	}
	return false;
}

bool CalculatePath_Try2(std::pair<int, int> _ToGridReference)
{
	// Different algorithm for path finding. BFS?
	return false;
}

void SnakeEntity::MoveOnPath()
{
	if (!SnakePath.empty() && !isFoodCollected) // Move only if there is a next path
	{
		Move(SnakePath.front());
		SnakePath.erase(SnakePath.begin());
	}
	else
	{
		// Clear any remaining paths
		SnakePath.clear();
		if (!CalculatePath_Try1(std::make_pair(SnakeFoodLocation->first, SnakeFoodLocation->second))) // Calculate route to food
		{
			//Move(SNAKE_MOVE_LEFT); // TODO: More intelligent way of movement if no path is currently available
		}
	}
}

void SnakeEntity::Unload()
{
	LevelGrid = nullptr;
	SnakeParts.clear();
	SnakePath.clear();
	isDead = false;
	isFoodCollected = false;
}

void SnakeEntity::Update(float _DeltaTime)
{
	if (!isDead) // Dont update if dead
	{
		// Move the snake on the calculated path, or if no path exists calculate one.
		MoveOnPath();

		// Update snake body to map with new positions
		UpdateBody();
	}
}<|endoftext|>


<commit_before>/*******************************************************************************
* Copyright 2016 ROBOTIS CO., LTD.
*
* 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.
*******************************************************************************/

/* Authors: Hye-Jong KIM */

#ifndef OMKINEMATICS_H_
#define OMKINEMATICS_H_

#include "../../include/open_manipulator/OMManager.hpp"
#include "../../include/open_manipulator/OMMath.hpp"
#include "../../include/open_manipulator/OMDebug.hpp"

#include <Eigen.h>        // Calls main Eigen matrix class library
#include <Eigen/LU>       // Calls inverse, determinant, LU decomp., etc.
#include <Eigen/Dense>
#include <math.h>

class OMKinematicsMethod
{
 private:
  OMMath math_;

 public:
  OMKinematicsMethod(){};
  ~OMKinematicsMethod(){};

  void getBasePose(Base &base, Eigen::Vector3f base_position, Eigen::Matrix3f base_orientation)
  {
    Pose temp;
    base.setPosition(base_position);
    base.setOrientaion(base_orientation);
    temp.position = base_position + base.inertia_.relative_center_position;
    base.setCenterPosition(temp.position);
  }

  void getBaseJointPose(Manipulator *manipulator, int8_t base_joint_number)
  {
    Pose temp;
    temp.position = manipulator.base_.getPosition() + manipulator.base_.getOrientation()*manipulator.base_.getRelativeBaseJointPosition(base_joint_number);
    temp.orientation = manipulator.base_.getOrientation() * manipulator.base_.getRelativeBaseJointOrientation(base_joint_number) * math_.rodriguesRotationMatrix(manipulator.joint_[base_joint_number].getAxis(), manipulator.joint_[base_joint_number].getAngle());
    manipulator.joint[base_joint_number].setJointPose(temp);
  }

  void getSinglejointPose(Manipulator *manipulator, int8_t joint_number, int8_t mather_joint_number, int8_t link_number)
  {
    Pose temp;
    temp.position = manipulator.joint_[mather_joint_number].getPosition() + manipulator.joint_[mather_joint_number].getOrientation()*manipulator.link_[link_number].getRelativeJointPosition(mather_joint_number,joint_number);
    temp.orientation = manipulator.joint_[mather_joint_number].getOrientation() * manipulator.link_[link_number].getRelativeJointOrientation(mather_joint_number,joint_number) * math_.rodriguesRotationMatrix(manipulator.joint_[joint_number].getAxis(), manipulator.joint[joint_number].getAngle());
    manipulator.joint_[joint_number].setPose(temp);
  }

  void getToolPose(Manipulator *manipulator, int8_t tool_number, int8_t mather_joint_number)
  {
    Pose temp;
    temp.position = manipulator.joint_[mather_joint_number].getPosition() + manipulator.joint_[mather_joint_number].getOrientation()*manipulator.tool_[tool_number].getRelativeToolPosition(mather_joint_number);
    temp.orientation = manipulator.joint_[mather_joint_number].getOrientation() * manipulator.tool_[tool_number].getRelativeToolOrientation(mather_joint_number);
    manipulator.tool_[tool_number].setPose(temp);
  }

  void jacobian(Manipulator *manipulator, int8_t tool_number)
  {
    Eigen::MatrixXf jacobian(6,manipulator.getDOF());
    Eigen::Vector3f position_changed    = Eigen::Vector3f::Zero();
    Eigen::Vector3f orientation_changed = Eigen::Vector3f::Zero();
    Eigen::VectorXf pose_changed(6);

    int8_t j = 0;
    for(int8_t i = 0; i < manipulator.getJointSize(); i++)
    {
      if(manipulator.joint_[i].getId() >= 0)
      {
        position_changed = math_.skewSymmetricMatrix(manipulator.joint_[i].getOrientation()*manipulator.joint_[i].getAxis()) * ( manipulator.tool_[tool_number].getPosition() - manipulator.joint_[i].getPosition());
        orientation_changed = manipulator.joint_[i].getOrientation()*manipulator.joint_[i].getAxis();

        pose_changed(6) << position_changed(0),
                          position_changed(1),
                          position_changed(2),
                          orientation_changed(0),
                          orientation_changed(1),
                          orientation_changed(2);

        jacobian.col(j) = pose_changed;
        j++;
      }
    }
  }
}

class OMChainKinematics
{
  private:
    OMMath math_;

  public:
    OMChainKinematics(){};
    ~OMChainKinematics(){};
};

class OMScaraKinematics
{
  private:
    OMMath math_;

  public:
    OMScaraKinematics(){};
    ~OMScaraKinematics(){};

};

class OMLinkKinematics
{
 private:
  OMMath math_;

  void getPassiveJointAngle(Joint *joint)
  {
    // joint[0].setAngle();
    // joint[1].setAngle(); // 0 < joint[1].setAngle < 3PI/4
    // joint[2].setAngle(); // PI/2 && joint[1].setAngle + 45? < joint[2].setAngle < PI
    joint[3].setAngle(joint[1].getAngle()-joint[2].getAngle());
    joint[4].setAngle(-M_PI-(joint[1].getAngle()-joint[2].getAngle()));
    joint[5].setAngle(-M_PI-(joint[1].getAngle()-joint[2].getAngle()));
    joint[6].setAngle((155 * DEG2RAD) - joint[2].getAngle());
    joint[7].setAngle(joint[1].getAngle());
    joint[8].setAngle((25 * DEG2RAD)-joint[1].getAngle());
    joint[9].setAngle(joint[2].getAngle() - (205 * DEG2RAD));
    joint[10].setAngle((90 * DEG2RAD) - joint[2].getAngle());
  }

 public:
  OMLinkKinematics(){};
  ~OMLinkKinematics(){};

  void forward(Manipulator *omlink, Eigen::Vector3f base_position, Eigen::Matrix3f base_orientation)
  {
    OMKinematicsMethod method_;
    method_.getBasePose(omlink.base_, base_position, base_orientation);
    forward(omlink);
  }

  void forward(Manipulator *omlink)
  {
    OMKinematicsMethod method_;
    getPassiveJointAngle(omlink.joint_);
    method_.getBaseJointPose(omlink,0);
    method_.getSinglejointPose(omlink, 1, 0, 0);
    method_.getSinglejointPose(omlink, 2, 0, 0);
    method_.getSinglejointPose(omlink, 3, 2, 2);
    method_.getSinglejointPose(omlink, 4, 3, 3);
    method_.getSinglejointPose(omlink, 5, 1, 1);
    method_.getSinglejointPose(omlink, 6, 5, 4);
    method_.getSinglejointPose(omlink, 7, 0, 0);
    method_.getSinglejointPose(omlink, 8, 7, 5);
    method_.getSinglejointPose(omlink, 9, 8, 6);
    method_.getSinglejointPose(omlink, 10, 9, 7);
    method_.getToolPose(omlink, 0, 6);
  }

  Eigen::VectorXf numericalInverse(Manipulator omlink, int8_t tool_number, Pose target_pose, float gain)
  {
    OMKinematicsMethod method_;
    OMMath math_;

    Eigen::VectorXf target_angle(omlink.getDOF());
    Eigen::MatrixXf jacobian(6,omlink.getDOF());
    Eigen::VectorXf differential_pose(6);
    Eigen::VectorXf previous_differential_pose(6);

    while(differential_pose.norm() < 1E-6)
    {
      forward(omlink);
      jacobian = method_.jacobian(omlink, tool_number);
      differential_pose = math_.differentialPose(target_pose.position, omlink.tool[tool_number].getPosition(), target_pose.orientation, omlink.tool[tool_number].getOrientation())

      Eigen::ColPivHouseholderQR<Eigen::MatrixXf> qrmethod(jacobian);
      target_angle = gain * qrmethod.solve(differential_pose);

      int8_t k = 0;
      for(int8_t j = 0; j < omlink.getJointSize(); j++)
      {
        if(omlink.joint_[j].getId() >= 0)
        {
          omlink.joint_[j].setAngle(target_angle(k));
          k++;
        }
      }

      if(differential_pose.norm()>previous_differential_pose.norm())
      {
        //ERROR
        break;
      }
      previous_differential_pose = differential_pose;
    }

    return target_angle;
  }

  Eigen::Vector3f geometricInverse(Manipulator *omlink, int8_t tool_number, Pose target_pose)
  {
    OMKinematicsMethod method_;
    OMMath math_;

    target_pose.position - omlink.tool[tool_number].

  }

};

class OMDeltaKinematics
{
  private:
    OMMath math_;

  public:
    OMDeltaKinematics(){};
    ~OMDeltaKinematics(){};

};

class MYKinematics
{
 private:
   OMMath math_;

 public:
   MYKinematics(){};
   ~MYKinematics(){};
};

#endif // OMKINEMATICS_H_
<commit_msg>update<commit_after>/*******************************************************************************
* Copyright 2016 ROBOTIS CO., LTD.
*
* 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.
*******************************************************************************/

/* Authors: Hye-Jong KIM */

#ifndef OMKINEMATICS_H_
#define OMKINEMATICS_H_

#include "../../include/open_manipulator/OMManager.hpp"
#include "../../include/open_manipulator/OMMath.hpp"
#include "../../include/open_manipulator/OMDebug.hpp"

#include <Eigen.h>        // Calls main Eigen matrix class library
#include <Eigen/LU>       // Calls inverse, determinant, LU decomp., etc.
#include <Eigen/Dense>
#include <math.h>

class OMKinematicsMethod
{
 private:
  OMMath math_;

 public:
  OMKinematicsMethod(){};
  ~OMKinematicsMethod(){};

  void getBasePose(Base &base, Eigen::Vector3f base_position, Eigen::Matrix3f base_orientation)
  {
    Pose temp;
    base.setPosition(base_position);
    base.setOrientaion(base_orientation);
    temp.position = base_position + base.inertia_.relative_center_position;
    base.setCenterPosition(temp.position);
  }

  void getBaseJointPose(Manipulator *manipulator, int8_t base_joint_number)
  {
    Pose temp;
    temp.position = manipulator.base_.getPosition() + manipulator.base_.getOrientation()*manipulator.base_.getRelativeBaseJointPosition(base_joint_number);
    temp.orientation = manipulator.base_.getOrientation() * manipulator.base_.getRelativeBaseJointOrientation(base_joint_number) * math_.rodriguesRotationMatrix(manipulator.joint_[base_joint_number].getAxis(), manipulator.joint_[base_joint_number].getAngle());
    manipulator.joint[base_joint_number].setJointPose(temp);
  }

  void getSinglejointPose(Manipulator *manipulator, int8_t joint_number, int8_t mather_joint_number, int8_t link_number)
  {
    Pose temp;
    temp.position = manipulator.joint_[mather_joint_number].getPosition() + manipulator.joint_[mather_joint_number].getOrientation()*manipulator.link_[link_number].getRelativeJointPosition(mather_joint_number,joint_number);
    temp.orientation = manipulator.joint_[mather_joint_number].getOrientation() * manipulator.link_[link_number].getRelativeJointOrientation(mather_joint_number,joint_number) * math_.rodriguesRotationMatrix(manipulator.joint_[joint_number].getAxis(), manipulator.joint[joint_number].getAngle());
    manipulator.joint_[joint_number].setPose(temp);
  }

  void getToolPose(Manipulator *manipulator, int8_t tool_number, int8_t mather_joint_number)
  {
    Pose temp;
    temp.position = manipulator.joint_[mather_joint_number].getPosition() + manipulator.joint_[mather_joint_number].getOrientation()*manipulator.tool_[tool_number].getRelativeToolPosition(mather_joint_number);
    temp.orientation = manipulator.joint_[mather_joint_number].getOrientation() * manipulator.tool_[tool_number].getRelativeToolOrientation(mather_joint_number);
    manipulator.tool_[tool_number].setPose(temp);
  }

  void jacobian(Manipulator *manipulator, int8_t tool_number)
  {
    Eigen::MatrixXf jacobian(6,manipulator.getDOF());
    Eigen::Vector3f position_changed    = Eigen::Vector3f::Zero();
    Eigen::Vector3f orientation_changed = Eigen::Vector3f::Zero();
    Eigen::VectorXf pose_changed(6);

    int8_t j = 0;
    for(int8_t i = 0; i < manipulator.getJointSize(); i++)
    {
      if(manipulator.joint_[i].getId() >= 0)
      {
        position_changed = math_.skewSymmetricMatrix(manipulator.joint_[i].getOrientation()*manipulator.joint_[i].getAxis()) * ( manipulator.tool_[tool_number].getPosition() - manipulator.joint_[i].getPosition());
        orientation_changed = manipulator.joint_[i].getOrientation()*manipulator.joint_[i].getAxis();

        pose_changed(6) << position_changed(0),
                          position_changed(1),
                          position_changed(2),
                          orientation_changed(0),
                          orientation_changed(1),
                          orientation_changed(2);

        jacobian.col(j) = pose_changed;
        j++;
      }
    }
  }
}

class OMChainKinematics
{
  private:
    OMMath math_;

  public:
    OMChainKinematics(){};
    ~OMChainKinematics(){};
};

class OMScaraKinematics
{
  private:
    OMMath math_;

  public:
    OMScaraKinematics(){};
    ~OMScaraKinematics(){};

};

class OMLinkKinematics
{
 private:
  OMMath math_;

  void getPassiveJointAngle(Joint *joint)
  {
    // joint[0].setAngle();
    // joint[1].setAngle(); // 0 < joint[1].setAngle < 3PI/4
    // joint[2].setAngle(); // PI/2 && joint[1].setAngle + 45? < joint[2].setAngle < PI
    joint[3].setAngle(joint[1].getAngle()-joint[2].getAngle());
    joint[4].setAngle(-M_PI-(joint[1].getAngle()-joint[2].getAngle()));
    joint[5].setAngle(-M_PI-(joint[1].getAngle()-joint[2].getAngle()));
    joint[6].setAngle((155 * DEG2RAD) - joint[2].getAngle());
    joint[7].setAngle(joint[1].getAngle());
    joint[8].setAngle((25 * DEG2RAD)-joint[1].getAngle());
    joint[9].setAngle(joint[2].getAngle() - (205 * DEG2RAD));
    joint[10].setAngle((90 * DEG2RAD) - joint[2].getAngle());
  }

 public:
  OMLinkKinematics(){};
  ~OMLinkKinematics(){};

  void forward(Manipulator *omlink, Eigen::Vector3f base_position, Eigen::Matrix3f base_orientation)
  {
    OMKinematicsMethod method_;
    method_.getBasePose(omlink.base_, base_position, base_orientation);
    forward(omlink);
  }

  void forward(Manipulator *omlink)
  {
    OMKinematicsMethod method_;
    getPassiveJointAngle(omlink.joint_);
    method_.getBaseJointPose(omlink,0);
    method_.getSinglejointPose(omlink, 1, 0, 0);
    method_.getSinglejointPose(omlink, 2, 0, 0);
    method_.getSinglejointPose(omlink, 3, 2, 2);
    method_.getSinglejointPose(omlink, 4, 3, 3);
    method_.getSinglejointPose(omlink, 5, 1, 1);
    method_.getSinglejointPose(omlink, 6, 5, 4);
    method_.getSinglejointPose(omlink, 7, 0, 0);
    method_.getSinglejointPose(omlink, 8, 7, 5);
    method_.getSinglejointPose(omlink, 9, 8, 6);
    method_.getSinglejointPose(omlink, 10, 9, 7);
    method_.getToolPose(omlink, 0, 6);
  }

  Eigen::VectorXf numericalInverse(Manipulator omlink, int8_t tool_number, Pose target_pose, float gain)
  {
    OMKinematicsMethod method_;
    OMMath math_;

    Eigen::VectorXf target_angle(omlink.getDOF());
    Eigen::MatrixXf jacobian(6,omlink.getDOF());
    Eigen::VectorXf differential_pose(6);
    Eigen::VectorXf previous_differential_pose(6);

    while(differential_pose.norm() < 1E-6)
    {
      forward(omlink);
      jacobian = method_.jacobian(omlink, tool_number);
      differential_pose = math_.differentialPose(target_pose.position, omlink.tool[tool_number].getPosition(), target_pose.orientation, omlink.tool[tool_number].getOrientation())

      Eigen::ColPivHouseholderQR<Eigen::MatrixXf> qrmethod(jacobian);
      target_angle = gain * qrmethod.solve(differential_pose);

      int8_t k = 0;
      for(int8_t j = 0; j < omlink.getJointSize(); j++)
      {
        if(omlink.joint_[j].getId() >= 0)
        {
          omlink.joint_[j].setAngle(target_angle(k));
          k++;
        }
      }

      if(differential_pose.norm()>previous_differential_pose.norm())
      {
        //ERROR
        break;
      }
      previous_differential_pose = differential_pose;
    }

    return target_angle;
  }

  Eigen::Vector3f geometricInverse(Manipulator *omlink, Pose target_pose)
  {
    OMKinematicsMethod method_;
    OMMath math_;

    target_pose.position - omlink.tool[0].getRelativeToolPosition[6]

  }

};

class OMDeltaKinematics
{
  private:
    OMMath math_;

  public:
    OMDeltaKinematics(){};
    ~OMDeltaKinematics(){};

};

class MYKinematics
{
 private:
   OMMath math_;

 public:
   MYKinematics(){};
   ~MYKinematics(){};
};

#endif // OMKINEMATICS_H_
<|endoftext|>


<commit_before>#include <iostream>
#include <iomanip>
#include <GL/glfw.h>
#include <sstream>
#include <omp.h>

#include "toefl.h"
#include "dft_dft_solver.h"
#include "drt_dft_solver.h"
#include "blueprint.h"

using namespace std;
using namespace toefl;

unsigned N; //initialized by init function
double amp, imp_amp; //
const double slit = 2./500.; //half distance between pictures in units of width
double field_ratio;
unsigned width = 960, height = 1080; //initial window width & height
stringstream window_str;  //window name

void GLFWCALL WindowResize( int w, int h)
{
    // map coordinates to the whole window
    double win_ratio = (double)w/(double)h;
    GLint ww = (win_ratio<field_ratio) ? w : h*field_ratio ;
    GLint hh = (win_ratio<field_ratio) ? w/field_ratio : h;
    glViewport( 0, 0, (GLsizei) ww, hh);
    width = w;
    height = h;
}

Blueprint read( char const * file)
{
    Blueprint bp;
    Physical& phys = bp.physical();
    Algorithmic& alg = bp.algorithmic();
    Boundary& bound = bp.boundary();
    vector<double> para;
    try{ para = read_input( file); }
    catch (Message& m)
    {
        m.display();
        throw m;
    }
    alg.nx = para[1];
    alg.ny = para[2];
    alg.dt = para[3];

    bound.ly = para[4];
    switch( (unsigned)para[5])
    {
        case( 0): bound.bc_x = TL_PERIODIC; break;
        case( 1): bound.bc_x = TL_DST10; break;
        case( 2): bound.bc_x = TL_DST01; break;
    }
    if( para[6])
        bp.enable( TL_MHW);

    phys.d = para[7];
    phys.nu = para[8];
    phys.kappa = para[9];

    amp = para[10];
    phys.g_e = phys.g[0] = para[11];
    phys.tau[0] = para[12];
    if( para[13])
        bp.enable( TL_IMPURITY);
    imp_amp = para[14];
    phys.g[1] = para[15];
    phys.a[1] = para[16];
    phys.mu[1] = para[17];
    phys.tau[1] = para[18];

    phys.a[0] = 1. -phys.a[1];
    phys.g[0] = (phys.g_e - phys.a[1] * phys.g[1])/(1.-phys.a[1]);
    phys.mu[0] = 1.0;//single charged ions

    N = para[19];
    omp_set_num_threads( para[20]);
    cout<< "With "<<omp_get_max_threads()<<" threads\n";

    alg.h = bound.ly / (double)alg.ny;
    bound.lx = (double)alg.nx * alg.h;
    return bp;
}

template< class M>
double abs_max( const M& field)
{
    double temp = 0;
    for( unsigned i=0; i<field.rows(); i++)
        for( unsigned j=0; j<field.cols(); j++)
            if( fabs(field(i,j)) > temp) temp = fabs(field(i,j));
    return temp;
}

template<class M>
void loadTexture( const M& field, double max)
{
    static Texture_RGBf tex( field.rows(), field.cols());
    gentexture_RGBf( tex, field, max);
    // image comes from texarray on host
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, tex.cols(), tex.rows(), 0, GL_RGB, GL_FLOAT, tex.getPtr());
}

// The solver has to have the getField( target) function returing M
// and the blueprint() function
template<class M, class Solver>
void drawScene( const Solver& solver)
{
    glClear(GL_COLOR_BUFFER_BIT);
    double max;
    M const * field;

    { //draw electrons
    field = &solver.getField( TL_ELECTRONS);
    max = abs_max( *field);
    glLoadIdentity();
    loadTexture( *field, max);
    window_str << scientific;
    window_str <<"ne / "<<max<<"\t";
    //Draw a textured quad
    //upper left
    glBegin(GL_QUADS);
        glTexCoord2f(0.0f, 0.0f); glVertex2f( -1.0, +slit);
        glTexCoord2f(1.0f, 0.0f); glVertex2f( -slit, +slit);
        glTexCoord2f(1.0f, 1.0f); glVertex2f( -slit, 1.0);
        glTexCoord2f(0.0f, 1.0f); glVertex2f( -1.0, 1.0);
    glEnd();
    }

    { //draw Ions
    field = &solver.getField( TL_IONS);
    loadTexture( *field, max);
    window_str <<" ni / "<<max<<"\t";
    glLoadIdentity();
    //upper right
    glBegin(GL_QUADS);
        glTexCoord2f(0.0f, 0.0f); glVertex2f( +slit, +slit);
        glTexCoord2f(1.0f, 0.0f); glVertex2f( 1.0, +slit);
        glTexCoord2f(1.0f, 1.0f); glVertex2f( 1.0, 1.0);
        glTexCoord2f(0.0f, 1.0f); glVertex2f( +slit, 1.0);
    glEnd();
    }

    if( solver.blueprint().isEnabled( TL_IMPURITY))
    {
        field = &solver.getField( TL_IMPURITIES);
        max = abs_max(*field);
        loadTexture( *field, max);
        window_str <<" nz / "<<max<<"\t";
        glLoadIdentity();
        //lower left
        glBegin(GL_QUADS);
            glTexCoord2f(0.0f, 0.0f); glVertex2f(-1.0, -1.0);
            glTexCoord2f(1.0f, 0.0f); glVertex2f( -slit, -1.0);
            glTexCoord2f(1.0f, 1.0f); glVertex2f( -slit, -slit);
            glTexCoord2f(0.0f, 1.0f); glVertex2f(-1.0, -slit);
        glEnd();
    }

    { //draw potential
    field = &solver.getField( TL_POTENTIAL);
    max = abs_max(*field);
    loadTexture( *field, max);
    window_str <<" phi / "<<max<<"\t";
    glLoadIdentity();
    //lower right
    glBegin(GL_QUADS);
        glTexCoord2f(0.0f, 0.0f); glVertex2f( +slit, -1.0);
        glTexCoord2f(1.0f, 0.0f); glVertex2f( 1.0, -1.0);
        glTexCoord2f(1.0f, 1.0f); glVertex2f( 1.0, 0);
        glTexCoord2f(0.0f, 1.0f); glVertex2f(+slit, -slit);
    glEnd();
    }

}

int main( int argc, char* argv[])
{
    //Parameter initialisation
    Blueprint bp_mod;
    if( argc == 1)
    {
        bp_mod = read("input.txt");
    }
    else if( argc == 2)
    {
        bp_mod = read( argv[1]);
    }
    else
    {
        cerr << "ERROR: Too many arguments!\nUsage: "<< argv[0]<<" [filename]\n";
        return -1;
    }
    const Blueprint bp = bp_mod;
    field_ratio = bp.boundary().lx/bp.boundary().ly;

    bp.display(cout);
    //construct solvers
    DFT_DFT_Solver<2> solver2( bp);
    DFT_DFT_Solver<3> solver3( bp);
    if( bp.boundary().bc_x == TL_PERIODIC)
        bp_mod.boundary().bc_x = TL_DST10;
    DRT_DFT_Solver<2> drt_solver2( bp_mod);
    DRT_DFT_Solver<3> drt_solver3( bp_mod);

    const Algorithmic& alg = bp.algorithmic();
    Matrix<double, TL_DFT> ne{ alg.ny, alg.nx, 0.}, nz{ ne}, phi{ ne};
    // place some gaussian blobs in the field
    try{
        //init_gaussian( ne, 0.1,0.2, 10./128./field_ratio, 10./128., amp);
        //init_gaussian( ne, 0.1,0.4, 10./128./field_ratio, 10./128., -amp);
        init_gaussian( ne, 0.8,0.4, 10./128./field_ratio, 10./128., amp);
        //init_gaussian( ne, 0.1,0.8, 10./128./field_ratio, 10./128., -amp);
        //init_gaussian( ni, 0.1,0.5, 0.05/field_ratio, 0.05, amp);
        if( bp.isEnabled( TL_IMPURITY))
        {
            init_gaussian( nz, 0.8,0.4, 0.05/field_ratio, 0.05, imp_amp);
            //init_gaussian_column( nz, 0.2, 0.05/field_ratio, imp_amp);
        }
        std::array< Matrix<double, TL_DFT>,2> arr2{{ ne, phi}};
        std::array< Matrix<double, TL_DFT>,3> arr3{{ ne, nz, phi}};
        Matrix<double, TL_DRT_DFT> ne_{ alg.ny, alg.nx, 0.}, nz_{ ne_}, phi_{ ne_};
        init_gaussian( ne_, 0.5,0.5, 0.05/field_ratio, 0.05, amp);
        //init_gaussian( ne_, 0.2,0.2, 0.05/field_ratio, 0.05, -amp);
        //init_gaussian( ne_, 0.6,0.6, 0.05/field_ratio, 0.05, -amp);
        //init_gaussian( ni_, 0.5,0.5, 0.05/field_ratio, 0.05, amp);
        if( bp.isEnabled( TL_IMPURITY))
        {
            init_gaussian( nz_, 0.5,0.5, 0.05/field_ratio, 0.05, -imp_amp);
            //init_gaussian( nz_, 0.2,0.2, 0.05/field_ratio, 0.05, -imp_amp);
            //init_gaussian( nz_, 0.6,0.6, 0.05/field_ratio, 0.05, -imp_amp);
        }
        std::array< Matrix<double, TL_DRT_DFT>,2> arr2_{{ ne_, phi_}};
        std::array< Matrix<double, TL_DRT_DFT>,3> arr3_{{ ne_, nz_, phi_}};
        //now set the field to be computed
        if( !bp.isEnabled( TL_IMPURITY))
        {
            if( bp.boundary().bc_x == TL_PERIODIC)
                solver2.init( arr2, TL_IONS);
            else
                drt_solver2.init( arr2_, TL_IONS);
        }
        else
        {
            if( bp.boundary().bc_x == TL_PERIODIC)
                solver3.init( arr3, TL_IONS);
            else
                drt_solver3.init( arr3_, TL_IONS);
        }
    }catch( Message& m){m.display();}

    ////////////////////////////////glfw//////////////////////////////
    {
    int running = GL_TRUE;
    if( !glfwInit()) { cerr << "ERROR: glfw couldn't initialize.\n";}

    height = width/field_ratio;
    if( !glfwOpenWindow( width, height,  0,0,0,  0,0,0, GLFW_WINDOW))
    {
        cerr << "ERROR: glfw couldn't open window!\n";
    }
    glfwSetWindowSizeCallback( WindowResize);

    glEnable( GL_TEXTURE_2D);
    glfwEnable( GLFW_STICKY_KEYS);
    glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
    glClearColor(0.f, 0.f, 0.f, 0.f);

    double t = 3*alg.dt;
    Timer timer;
    Timer overhead;
    cout<< "HIT ESC to terminate program \n"
        << "HIT S   to stop simulation \n"
        << "HIT R   to continue simulation!\n";
    while( running)
    {
        overhead.tic();
        //ask if simulation shall be stopped
        glfwPollEvents();
        if( glfwGetKey( 'S'))
        {
            do
            {
                glfwWaitEvents();
            } while( !glfwGetKey('R') &&
                     !glfwGetKey( GLFW_KEY_ESC) &&
                      glfwGetWindowParam( GLFW_OPENED) );
        }

        //draw scene
        if( !bp.isEnabled( TL_IMPURITY))
        {
            if( bp.boundary().bc_x == TL_PERIODIC)
                drawScene<Matrix<double, TL_DFT>>( solver2);
            else
                drawScene<Matrix<double, TL_DRT_DFT>>( drt_solver2);
        }
        else
        {
            if( bp.boundary().bc_x == TL_PERIODIC)
                drawScene<Matrix<double, TL_DFT>>( solver3);
            else
                drawScene<Matrix<double, TL_DRT_DFT>>( drt_solver3);
        }
        window_str << setprecision(2) << fixed;
        window_str << " &&   time = "<<t;
        glfwSetWindowTitle( (window_str.str()).c_str() );
        window_str.str("");
        glfwSwapBuffers();
#ifdef TL_DEBUG
        glfwWaitEvents();
        if( glfwGetKey('N'))
        {
#endif
        timer.tic();
        for(unsigned i=0; i<N; i++)
        {
            if( !bp.isEnabled( TL_IMPURITY))
            {
                if( bp.boundary().bc_x == TL_PERIODIC)
                    solver2.step( );
                else
                    drt_solver2.step();
            }
            else
            {
                if( bp.boundary().bc_x == TL_PERIODIC)
                    solver3.step( );
                else
                    drt_solver3.step( );
            }
            t+= alg.dt;
        }
        timer.toc();
#ifdef TL_DEBUG
            cout << "Next "<<N<<" Steps\n";
        }
#endif
        running = !glfwGetKey( GLFW_KEY_ESC) &&
                    glfwGetWindowParam( GLFW_OPENED);
        overhead.toc();
    }
    glfwTerminate();
    cout << "Average time for one step = "<<timer.diff()/(double)N<<"s\n";
    cout << "Overhead for visualisation, etc. per step = "<<(overhead.diff()-timer.diff())/(double)N<<"s\n";
    }
    //////////////////////////////////////////////////////////////////
    fftw_cleanup();
    return 0;

}
<commit_msg>last change<commit_after>#include <iostream>
#include <iomanip>
#include <GL/glfw.h>
#include <sstream>
#include <omp.h>

#include "toefl.h"
#include "dft_dft_solver.h"
#include "drt_dft_solver.h"
#include "blueprint.h"

using namespace std;
using namespace toefl;

unsigned N; //initialized by init function
double amp, imp_amp; //
const double slit = 2./500.; //half distance between pictures in units of width
double field_ratio;
unsigned width = 960, height = 1080; //initial window width & height
stringstream window_str;  //window name

void GLFWCALL WindowResize( int w, int h)
{
    // map coordinates to the whole window
    double win_ratio = (double)w/(double)h;
    GLint ww = (win_ratio<field_ratio) ? w : h*field_ratio ;
    GLint hh = (win_ratio<field_ratio) ? w/field_ratio : h;
    glViewport( 0, 0, (GLsizei) ww, hh);
    width = w;
    height = h;
}

Blueprint read( char const * file)
{
    Blueprint bp;
    Physical& phys = bp.physical();
    Algorithmic& alg = bp.algorithmic();
    Boundary& bound = bp.boundary();
    vector<double> para;
    try{ para = read_input( file); }
    catch (Message& m)
    {
        m.display();
        throw m;
    }
    alg.nx = para[1];
    alg.ny = para[2];
    alg.dt = para[3];

    bound.ly = para[4];
    switch( (unsigned)para[5])
    {
        case( 0): bound.bc_x = TL_PERIODIC; break;
        case( 1): bound.bc_x = TL_DST10; break;
        case( 2): bound.bc_x = TL_DST01; break;
    }
    if( para[6])
        bp.enable( TL_MHW);

    phys.d = para[7];
    phys.nu = para[8];
    phys.kappa = para[9];

    amp = para[10];
    phys.g_e = phys.g[0] = para[11];
    phys.tau[0] = para[12];
    if( para[13])
        bp.enable( TL_IMPURITY);
    imp_amp = para[14];
    phys.g[1] = para[15];
    phys.a[1] = para[16];
    phys.mu[1] = para[17];
    phys.tau[1] = para[18];

    phys.a[0] = 1. -phys.a[1];
    phys.g[0] = (phys.g_e - phys.a[1] * phys.g[1])/(1.-phys.a[1]);
    phys.mu[0] = 1.0;//single charged ions

    N = para[19];
    omp_set_num_threads( para[20]);
    cout<< "With "<<omp_get_max_threads()<<" threads\n";

    alg.h = bound.ly / (double)alg.ny;
    bound.lx = (double)alg.nx * alg.h;
    return bp;
}

template< class M>
double abs_max( const M& field)
{
    double temp = 0;
    for( unsigned i=0; i<field.rows(); i++)
        for( unsigned j=0; j<field.cols(); j++)
            if( fabs(field(i,j)) > temp) temp = fabs(field(i,j));
    return temp;
}

template<class M>
void loadTexture( const M& field, double max)
{
    static Texture_RGBf tex( field.rows(), field.cols());
    gentexture_RGBf( tex, field, max);
    // image comes from texarray on host
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, tex.cols(), tex.rows(), 0, GL_RGB, GL_FLOAT, tex.getPtr());
}

// The solver has to have the getField( target) function returing M
// and the blueprint() function
template<class M, class Solver>
void drawScene( const Solver& solver)
{
    glClear(GL_COLOR_BUFFER_BIT);
    double max;
    M const * field;

    { //draw electrons
    field = &solver.getField( TL_ELECTRONS);
    max = abs_max( *field);
    glLoadIdentity();
    loadTexture( *field, max);
    window_str << scientific;
    window_str <<"ne / "<<max<<"\t";
    //Draw a textured quad
    //upper left
    glBegin(GL_QUADS);
        glTexCoord2f(0.0f, 0.0f); glVertex2f( -1.0, +slit);
        glTexCoord2f(1.0f, 0.0f); glVertex2f( -slit, +slit);
        glTexCoord2f(1.0f, 1.0f); glVertex2f( -slit, 1.0);
        glTexCoord2f(0.0f, 1.0f); glVertex2f( -1.0, 1.0);
    glEnd();
    }

    { //draw Ions
    field = &solver.getField( TL_IONS);
    loadTexture( *field, max);
    window_str <<" ni / "<<max<<"\t";
    glLoadIdentity();
    //upper right
    glBegin(GL_QUADS);
        glTexCoord2f(0.0f, 0.0f); glVertex2f( +slit, +slit);
        glTexCoord2f(1.0f, 0.0f); glVertex2f( 1.0, +slit);
        glTexCoord2f(1.0f, 1.0f); glVertex2f( 1.0, 1.0);
        glTexCoord2f(0.0f, 1.0f); glVertex2f( +slit, 1.0);
    glEnd();
    }

    if( solver.blueprint().isEnabled( TL_IMPURITY))
    {
        field = &solver.getField( TL_IMPURITIES);
        max = abs_max(*field);
        loadTexture( *field, max);
        window_str <<" nz / "<<max<<"\t";
        glLoadIdentity();
        //lower left
        glBegin(GL_QUADS);
            glTexCoord2f(0.0f, 0.0f); glVertex2f(-1.0, -1.0);
            glTexCoord2f(1.0f, 0.0f); glVertex2f( -slit, -1.0);
            glTexCoord2f(1.0f, 1.0f); glVertex2f( -slit, -slit);
            glTexCoord2f(0.0f, 1.0f); glVertex2f(-1.0, -slit);
        glEnd();
    }

    { //draw potential
    field = &solver.getField( TL_POTENTIAL);
    max = abs_max(*field);
    loadTexture( *field, max);
    window_str <<" phi / "<<max<<"\t";
    glLoadIdentity();
    //lower right
    glBegin(GL_QUADS);
        glTexCoord2f(0.0f, 0.0f); glVertex2f( +slit, -1.0);
        glTexCoord2f(1.0f, 0.0f); glVertex2f( 1.0, -1.0);
        glTexCoord2f(1.0f, 1.0f); glVertex2f( 1.0, 0);
        glTexCoord2f(0.0f, 1.0f); glVertex2f(+slit, -slit);
    glEnd();
    }

}

int main( int argc, char* argv[])
{
    //Parameter initialisation
    Blueprint bp_mod;
    if( argc == 1)
    {
        bp_mod = read("input.txt");
    }
    else if( argc == 2)
    {
        bp_mod = read( argv[1]);
    }
    else
    {
        cerr << "ERROR: Too many arguments!\nUsage: "<< argv[0]<<" [filename]\n";
        return -1;
    }
    const Blueprint bp = bp_mod;
    field_ratio = bp.boundary().lx/bp.boundary().ly;

    bp.display(cout);
    //construct solvers
    DFT_DFT_Solver<2> solver2( bp);
    DFT_DFT_Solver<3> solver3( bp);
    if( bp.boundary().bc_x == TL_PERIODIC)
        bp_mod.boundary().bc_x = TL_DST10;
    DRT_DFT_Solver<2> drt_solver2( bp_mod);
    DRT_DFT_Solver<3> drt_solver3( bp_mod);

    const Algorithmic& alg = bp.algorithmic();
    Matrix<double, TL_DFT> ne{ alg.ny, alg.nx, 0.}, nz{ ne}, phi{ ne};
    // place some gaussian blobs in the field
    try{
        //init_gaussian( ne, 0.1,0.2, 10./128./field_ratio, 10./128., amp);
        //init_gaussian( ne, 0.1,0.4, 10./128./field_ratio, 10./128., -amp);
        init_gaussian( ne, 0.8,0.4, 10./128./field_ratio, 10./128., amp);
        //init_gaussian( ne, 0.1,0.8, 10./128./field_ratio, 10./128., -amp);
        //init_gaussian( ni, 0.1,0.5, 0.05/field_ratio, 0.05, amp);
        if( bp.isEnabled( TL_IMPURITY))
        {
            init_gaussian( nz, 0.8,0.4, 0.05/field_ratio, 0.05, -imp_amp);
            //init_gaussian_column( nz, 0.2, 0.05/field_ratio, imp_amp);
        }
        std::array< Matrix<double, TL_DFT>,2> arr2{{ ne, phi}};
        std::array< Matrix<double, TL_DFT>,3> arr3{{ ne, nz, phi}};
        Matrix<double, TL_DRT_DFT> ne_{ alg.ny, alg.nx, 0.}, nz_{ ne_}, phi_{ ne_};
        init_gaussian( ne_, 0.5,0.5, 0.05/field_ratio, 0.05, amp);
        //init_gaussian( ne_, 0.2,0.2, 0.05/field_ratio, 0.05, -amp);
        //init_gaussian( ne_, 0.6,0.6, 0.05/field_ratio, 0.05, -amp);
        //init_gaussian( ni_, 0.5,0.5, 0.05/field_ratio, 0.05, amp);
        if( bp.isEnabled( TL_IMPURITY))
        {
            init_gaussian( nz_, 0.5,0.5, 0.05/field_ratio, 0.05, -imp_amp);
            //init_gaussian( nz_, 0.2,0.2, 0.05/field_ratio, 0.05, -imp_amp);
            //init_gaussian( nz_, 0.6,0.6, 0.05/field_ratio, 0.05, -imp_amp);
        }
        std::array< Matrix<double, TL_DRT_DFT>,2> arr2_{{ ne_, phi_}};
        std::array< Matrix<double, TL_DRT_DFT>,3> arr3_{{ ne_, nz_, phi_}};
        //now set the field to be computed
        if( !bp.isEnabled( TL_IMPURITY))
        {
            if( bp.boundary().bc_x == TL_PERIODIC)
                solver2.init( arr2, TL_IONS);
            else
                drt_solver2.init( arr2_, TL_IONS);
        }
        else
        {
            if( bp.boundary().bc_x == TL_PERIODIC)
                solver3.init( arr3, TL_IONS);
            else
                drt_solver3.init( arr3_, TL_IONS);
        }
    }catch( Message& m){m.display();}

    ////////////////////////////////glfw//////////////////////////////
    {
    int running = GL_TRUE;
    if( !glfwInit()) { cerr << "ERROR: glfw couldn't initialize.\n";}

    height = width/field_ratio;
    if( !glfwOpenWindow( width, height,  0,0,0,  0,0,0, GLFW_WINDOW))
    {
        cerr << "ERROR: glfw couldn't open window!\n";
    }
    glfwSetWindowSizeCallback( WindowResize);

    glEnable( GL_TEXTURE_2D);
    glfwEnable( GLFW_STICKY_KEYS);
    glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
    glClearColor(0.f, 0.f, 0.f, 0.f);

    double t = 3*alg.dt;
    Timer timer;
    Timer overhead;
    cout<< "HIT ESC to terminate program \n"
        << "HIT S   to stop simulation \n"
        << "HIT R   to continue simulation!\n";
    while( running)
    {
        overhead.tic();
        //ask if simulation shall be stopped
        glfwPollEvents();
        if( glfwGetKey( 'S'))
        {
            do
            {
                glfwWaitEvents();
            } while( !glfwGetKey('R') &&
                     !glfwGetKey( GLFW_KEY_ESC) &&
                      glfwGetWindowParam( GLFW_OPENED) );
        }

        //draw scene
        if( !bp.isEnabled( TL_IMPURITY))
        {
            if( bp.boundary().bc_x == TL_PERIODIC)
                drawScene<Matrix<double, TL_DFT>>( solver2);
            else
                drawScene<Matrix<double, TL_DRT_DFT>>( drt_solver2);
        }
        else
        {
            if( bp.boundary().bc_x == TL_PERIODIC)
                drawScene<Matrix<double, TL_DFT>>( solver3);
            else
                drawScene<Matrix<double, TL_DRT_DFT>>( drt_solver3);
        }
        window_str << setprecision(2) << fixed;
        window_str << " &&   time = "<<t;
        glfwSetWindowTitle( (window_str.str()).c_str() );
        window_str.str("");
        glfwSwapBuffers();
#ifdef TL_DEBUG
        glfwWaitEvents();
        if( glfwGetKey('N'))
        {
#endif
        timer.tic();
        for(unsigned i=0; i<N; i++)
        {
            if( !bp.isEnabled( TL_IMPURITY))
            {
                if( bp.boundary().bc_x == TL_PERIODIC)
                    solver2.step( );
                else
                    drt_solver2.step();
            }
            else
            {
                if( bp.boundary().bc_x == TL_PERIODIC)
                    solver3.step( );
                else
                    drt_solver3.step( );
            }
            t+= alg.dt;
        }
        timer.toc();
#ifdef TL_DEBUG
            cout << "Next "<<N<<" Steps\n";
        }
#endif
        running = !glfwGetKey( GLFW_KEY_ESC) &&
                    glfwGetWindowParam( GLFW_OPENED);
        overhead.toc();
    }
    glfwTerminate();
    cout << "Average time for one step = "<<timer.diff()/(double)N<<"s\n";
    cout << "Overhead for visualisation, etc. per step = "<<(overhead.diff()-timer.diff())/(double)N<<"s\n";
    }
    //////////////////////////////////////////////////////////////////
    fftw_cleanup();
    return 0;

}
<|endoftext|>


<commit_before>// Copyright (c) 2016 Steinwurf ApS
// All Rights Reserved
//
// Distributed under the "BSD License". See the accompanying LICENSE.rst file.

#pragma once
#include "big_endian.hpp"

namespace endian
{
using network = big_endian;
}<commit_msg>comment on the network alias<commit_after>// Copyright (c) 2016 Steinwurf ApS
// All Rights Reserved
//
// Distributed under the "BSD License". See the accompanying LICENSE.rst file.

#pragma once
#include "big_endian.hpp"

namespace endian
{
// https://en.wikipedia.org/wiki/Endianness#Networking
// "Many IETF RFCs use the term network order, meaning the order of transmission
// for bits and bytes over the wire in network protocols. Among others, the
// historic RFC 1700 (also known as Internet standard STD 2) has defined the
// network order for protocols in the Internet protocol suite to be big-endian,
// hence the use of the term "network byte order" for big-endian byte order."
//
// https://datatracker.ietf.org/doc/html/rfc1700#page-3
using network = big_endian;
}<|endoftext|>


<commit_before>#include <assert.h>
#include <cstdio>

#include <llvm/IR/LLVMContext.h>
#include <llvm/IR/Module.h>
#include <llvm/Support/SourceMgr.h>
#include <llvm/Support/raw_ostream.h>
#include <llvm/IRReader/IRReader.h>

#include <iostream>
#include <string>

#include "../src/LLVMDependenceGraph.h"

using namespace dg;
using llvm::errs;

static struct {
#if ENABLE_CFG
    bool printCFG;
    bool printRevCFG;
    bool printBB;
#endif

    bool printControlDep;
    bool printDataDep;
} OPTIONS;

static std::string& getValueName(const llvm::Value *val, std::string &str)
{
    llvm::raw_string_ostream s(str);

    str.clear();

    if (!val) {
        s << "[NULL]";
        return s.str();
    }

    if (llvm::isa<llvm::Function>(val))
        s << "ENTRY " << val->getName();
    else
        s << *val;

    return s.str();
}

static void dump_to_dot(LLVMDGNode *n, std::ostream& out)
{
    const llvm::Value *val;

    /* do not draw multiple edges */
    if (n->getDFSrun() == 1)
        return;
    else
        n->setDFSrun(1);

    if (OPTIONS.printControlDep)
        for (auto I = n->control_begin(), E = n->control_end();
             I != E; ++I)
            out << "\tNODE" << n << " -> NODE" <<  *I << "\n";

    if (OPTIONS.printDataDep)
        for (auto I = n->dependence_begin(), E = n->dependence_end();
             I != E; ++I)
            out << "\tNODE" << n << " -> NODE" <<  *I
                << " [color=red]\n";

#if ENABLE_CFG
    if (OPTIONS.printCFG) {
        if (n->hasSuccessor()) {
            LLVMDGNode *succ = n->getSuccessor();
            if (!succ)
                errs() << "n hasSuccessor NULL!\n";

            out << "\tNODE" << n << " -> NODE" << succ
                << " [style=dotted]\n";
        }
    }

    if (OPTIONS.printRevCFG) {
        if (n->hasPredcessor()) {
            out << "\tNODE" << n << " -> NODE" <<  n->getPredcessor()
                << " [style=dotted color=gray]\n";
        }
    }

    // print edges between blocks
    if (OPTIONS.printCFG) {
        auto BB = n->getBasicBlock();
        // if this is BB header, print the edges
        if (BB && BB->getFirstNode() == n) {
            auto fn = BB->getFirstNode();
            for (auto pred : BB->predcessors()) {
                auto ln = pred->getLastNode();
                if (!ln) {
                    std::string valName;
                    getValueName(n->getValue(), valName);
                    errs() << "WARN: No last node for "
                           << valName << "\n";
                } else {
                    out << "\tNODE" << ln << " -> NODE" << fn
                        << " [style=dotted color=purple]\n";
                }
            }

            for (auto pred : BB->successors()) {
                auto ln = BB->getLastNode();
                auto fn = pred->getFirstNode();
                if (!ln) {
                    std::string valName;
                    getValueName(n->getValue(), valName);
                    errs() << "WARN: No last node for "
                           << valName << "\n";
                } else {
                    out << "\tNODE" << ln << " -> NODE" << fn
                        << " [style=dotted color=red]\n";
                }
            }
        }
    }
#endif // ENABLE_CFG

    if (n->getSubgraph()) {
        out << "\tNODE" << n << " -> NODE" <<  n->getSubgraph()->getEntry()
            << " [style=dashed label=\"call\"]\n";
    }
}

void print_to_dot(LLVMDependenceGraph *dg,
                  bool issubgraph = false,
                  const char *description = NULL)
{
    static unsigned subgraph_no = 0;
    const llvm::Value *val;
    std::string valName;
    std::ostream& out = std::cout;

    if (!dg)
        return;

    if (issubgraph) {
        out << "subgraph \"cluster_" << subgraph_no++;
    } else {
        out << "digraph \""<< description ? description : "DependencyGraph";
    }

    out << "\" {\n";

    for (auto I = dg->begin(), E = dg->end(); I != E; ++I)
    {
        auto n = I->second;
        if (!n) {
                getValueName(dg->getEntry()->getValue(), valName);
                errs() << "WARN [" << valName
                       << "]: Node is NULL for value: "
                       << *I->first << "\n";

            continue;
        }

        val = n->getValue();

        print_to_dot(n->getSubgraph(), true);
        LLVMDependenceGraph *params = n->getParameters();
        if (params) {
            print_to_dot(params, true);

            // add control edge from call-site to the parameters subgraph
            out << "\tNODE" << n << " -> NODE" <<  params->getEntry()
                << "[label=\"params\"]\n";
        }

        getValueName(val, valName);

        out << "\tNODE" << n << " [label=\"" << valName;

#ifdef ENABLE_CFG
        if (OPTIONS.printBB) {
            auto BB = n->getBasicBlock();
            if (!BB) {
                errs() << "WARN: No basic block for " << valName << "\n";
            } else {
                auto fn = BB->getFirstNode();
                if (!fn) {
                    errs() << "CRITICAL: No first value for "
                           << valName << "\n";
                } else {
                auto fval = fn->getValue();
                    if (!fval) {
                        errs() << "WARN: No value in first value for "
                               << valName << "\n";
                    } else {
                        getValueName(fval, valName);
                        out << "\\nBB: " << valName;
                        if (n == fn)
                            out << "\\nBB head (preds "
                                << BB->predcessorsNum() << ")";
                        if (BB->getLastNode() == n)
                            out << "\\nBB tail (succs "
                                << BB->successorsNum() << ")";
                    }
                }
            }
        }
#endif  // ENABLE_CFG

        for (auto d : n->getDefs()) {
            getValueName(d->getValue(), valName);
            out << "\\nDEF " << valName;
        }
        for (auto d : n->getPtrs()) {
            getValueName(d->getValue(), valName);
            out << "\\nPTR " << valName;
        }

        out << "\"];\n";
            //<<" (runid=" << n->getDFSrun() << ")\"];\n";
    }

    for (auto I = dg->begin(), E = dg->end(); I != E; ++I) {
        auto n = I->second;
        if (!n) {
                errs() << "WARN [" << dg
                       << "]: Node is NULL for value: "
                       << *I->first << "\n";

            continue;
        }

        dump_to_dot(I->second, out);
    }

    out << "}\n";
}

int main(int argc, char *argv[])
{
    llvm::LLVMContext context;
    llvm::SMDiagnostic SMD;
    std::unique_ptr<llvm::Module> M;
    const char *module = NULL;

    // default
    OPTIONS.printControlDep = true;
    OPTIONS.printDataDep = true;

    // parse options
    for (int i = 1; i < argc; ++i) {
        if (strcmp(argv[i], "-no-control") == 0) {
            OPTIONS.printControlDep = false;
        } else if (strcmp(argv[i], "-no-data") == 0) {
            OPTIONS.printDataDep = false;

#if ENABLE_CFG
        } else if (strcmp(argv[i], "-bb") == 0) {
            OPTIONS.printBB = true;
        } else if (strcmp(argv[i], "-cfg") == 0) {
            OPTIONS.printCFG = true;
        } else if (strcmp(argv[i], "-cfgall") == 0) {
            OPTIONS.printCFG = true;
            OPTIONS.printRevCFG = true;
#endif // ENABLE_CFG

        } else {
            module = argv[i];
        }
    }

    if (!module) {
        errs() << "Usage: % IR_module [output_file]\n";
        return 1;
    }

    M = llvm::parseIRFile(module, SMD, context);
    if (!M) {
        SMD.print(argv[0], errs());
        return 1;
    }

    LLVMDependenceGraph d;
    d.build(&*M);

    print_to_dot(&d, false, "LLVM Dependence Graph");

    return 0;
}
<commit_msg>llvm-dg-dump: fix printing CFG branches<commit_after>#include <assert.h>
#include <cstdio>

#include <llvm/IR/LLVMContext.h>
#include <llvm/IR/Module.h>
#include <llvm/Support/SourceMgr.h>
#include <llvm/Support/raw_ostream.h>
#include <llvm/IRReader/IRReader.h>

#include <iostream>
#include <string>

#include "../src/LLVMDependenceGraph.h"

using namespace dg;
using llvm::errs;

static struct {
#if ENABLE_CFG
    bool printCFG;
    bool printRevCFG;
    bool printBB;
#endif

    bool printControlDep;
    bool printDataDep;
} OPTIONS;

static std::string& getValueName(const llvm::Value *val, std::string &str)
{
    llvm::raw_string_ostream s(str);

    str.clear();

    if (!val) {
        s << "[NULL]";
        return s.str();
    }

    if (llvm::isa<llvm::Function>(val))
        s << "ENTRY " << val->getName();
    else
        s << *val;

    return s.str();
}

static void dump_to_dot(LLVMDGNode *n, std::ostream& out)
{
    const llvm::Value *val;

    /* do not draw multiple edges */
    if (n->getDFSrun() == 1)
        return;
    else
        n->setDFSrun(1);

    if (OPTIONS.printControlDep)
        for (auto I = n->control_begin(), E = n->control_end();
             I != E; ++I)
            out << "\tNODE" << n << " -> NODE" <<  *I << "\n";

    if (OPTIONS.printDataDep)
        for (auto I = n->dependence_begin(), E = n->dependence_end();
             I != E; ++I)
            out << "\tNODE" << n << " -> NODE" <<  *I
                << " [color=red]\n";

#if ENABLE_CFG
    if (OPTIONS.printCFG) {
        if (n->hasSuccessor()) {
            LLVMDGNode *succ = n->getSuccessor();
            if (!succ)
                errs() << "n hasSuccessor NULL!\n";

            out << "\tNODE" << n << " -> NODE" << succ
                << " [style=dotted]\n";
        }
    }

    if (OPTIONS.printRevCFG) {
        if (n->hasPredcessor()) {
            out << "\tNODE" << n << " -> NODE" <<  n->getPredcessor()
                << " [style=dotted color=gray]\n";
        }
    }

    // print edges between blocks
    if (OPTIONS.printCFG) {
        auto BB = n->getBasicBlock();
        if (BB && BB->getLastNode() == n) {
            for (auto pred : BB->successors()) {
                auto fn = pred->getFirstNode();
                out << "\tNODE" << n << " -> NODE" << fn
                    << " [style=dotted color=red]\n";
            }
        }
    }

    if (OPTIONS.printRevCFG) {
        auto BB = n->getBasicBlock();
        // if this is BB header, print the edges
        if (BB && BB->getFirstNode() == n) {
            auto fn = BB->getFirstNode();
            for (auto pred : BB->predcessors()) {
                auto ln = pred->getLastNode();
                out << "\tNODE" << fn << " -> NODE" << ln
                    << " [style=dotted color=purple]\n";
            }
        }
    }

#endif // ENABLE_CFG

    if (n->getSubgraph()) {
        out << "\tNODE" << n << " -> NODE" <<  n->getSubgraph()->getEntry()
            << " [style=dashed label=\"call\"]\n";
    }
}

void print_to_dot(LLVMDependenceGraph *dg,
                  bool issubgraph = false,
                  const char *description = NULL)
{
    static unsigned subgraph_no = 0;
    const llvm::Value *val;
    std::string valName;
    std::ostream& out = std::cout;

    if (!dg)
        return;

    if (issubgraph) {
        out << "subgraph \"cluster_" << subgraph_no++;
    } else {
        out << "digraph \""<< description ? description : "DependencyGraph";
    }

    out << "\" {\n";

    for (auto I = dg->begin(), E = dg->end(); I != E; ++I)
    {
        auto n = I->second;
        if (!n) {
                getValueName(dg->getEntry()->getValue(), valName);
                errs() << "WARN [" << valName
                       << "]: Node is NULL for value: "
                       << *I->first << "\n";

            continue;
        }

        val = n->getValue();

        print_to_dot(n->getSubgraph(), true);
        LLVMDependenceGraph *params = n->getParameters();
        if (params) {
            print_to_dot(params, true);

            // add control edge from call-site to the parameters subgraph
            out << "\tNODE" << n << " -> NODE" <<  params->getEntry()
                << "[label=\"params\"]\n";
        }

        getValueName(val, valName);

        out << "\tNODE" << n << " [label=\"" << valName;

#ifdef ENABLE_CFG
        if (OPTIONS.printBB) {
            auto BB = n->getBasicBlock();
            if (!BB) {
                errs() << "WARN: No basic block for " << valName << "\n";
            } else {
                auto fn = BB->getFirstNode();
                if (!fn) {
                    errs() << "CRITICAL: No first value for "
                           << valName << "\n";
                } else {
                auto fval = fn->getValue();
                    if (!fval) {
                        errs() << "WARN: No value in first value for "
                               << valName << "\n";
                    } else {
                        getValueName(fval, valName);
                        out << "\\nBB: " << valName;
                        if (n == fn)
                            out << "\\nBB head (preds "
                                << BB->predcessorsNum() << ")";
                        if (BB->getLastNode() == n)
                            out << "\\nBB tail (succs "
                                << BB->successorsNum() << ")";
                    }
                }
            }
        }
#endif  // ENABLE_CFG

        for (auto d : n->getDefs()) {
            getValueName(d->getValue(), valName);
            out << "\\nDEF " << valName;
        }
        for (auto d : n->getPtrs()) {
            getValueName(d->getValue(), valName);
            out << "\\nPTR " << valName;
        }

        out << "\"];\n";
            //<<" (runid=" << n->getDFSrun() << ")\"];\n";
    }

    for (auto I = dg->begin(), E = dg->end(); I != E; ++I) {
        auto n = I->second;
        if (!n) {
                errs() << "WARN [" << dg
                       << "]: Node is NULL for value: "
                       << *I->first << "\n";

            continue;
        }

        dump_to_dot(I->second, out);
    }

    out << "}\n";
}

int main(int argc, char *argv[])
{
    llvm::LLVMContext context;
    llvm::SMDiagnostic SMD;
    std::unique_ptr<llvm::Module> M;
    const char *module = NULL;

    // default
    OPTIONS.printControlDep = true;
    OPTIONS.printDataDep = true;

    // parse options
    for (int i = 1; i < argc; ++i) {
        if (strcmp(argv[i], "-no-control") == 0) {
            OPTIONS.printControlDep = false;
        } else if (strcmp(argv[i], "-no-data") == 0) {
            OPTIONS.printDataDep = false;

#if ENABLE_CFG
        } else if (strcmp(argv[i], "-bb") == 0) {
            OPTIONS.printBB = true;
        } else if (strcmp(argv[i], "-cfg") == 0) {
            OPTIONS.printCFG = true;
        } else if (strcmp(argv[i], "-cfgall") == 0) {
            OPTIONS.printCFG = true;
            OPTIONS.printRevCFG = true;
#endif // ENABLE_CFG

        } else {
            module = argv[i];
        }
    }

    if (!module) {
        errs() << "Usage: % IR_module [output_file]\n";
        return 1;
    }

    M = llvm::parseIRFile(module, SMD, context);
    if (!M) {
        SMD.print(argv[0], errs());
        return 1;
    }

    LLVMDependenceGraph d;
    d.build(&*M);

    print_to_dot(&d, false, "LLVM Dependence Graph");

    return 0;
}
<|endoftext|>


<commit_before>//Standard Libraries
#include <iostream>
#include <string>
#include <fstream>

//Header Files
#include "../include/Solver.hpp"
#include "../include/Matrix.hpp"

int main(int argc, char* argv[])
{
    if (argc < 2)
    {
        std::cout << "I need a input file " << argc;
        return -1;
    }

    //Opening the input;
    std::ifstream input (argv[1]);

    if(!input.is_open())
    {
        std::cout << "Error while trying to open " << argv[1] << "\n";
        return -1;
    }

    std::string buffer;
    std::string line;
    std::string answer;

    //Reading the input
    while(std::getline(input, line))
    {
        buffer.insert(buffer.size(), line);
        buffer.insert(buffer.size(), " ");
    }

    input.close();

    bool v = false;
    if(argc == 3 && argv[2][1] == 'v')
        v = true;

    Matrix coef(buffer);

    std::vector<double>& rV = coef.getVector();
    int n = coef.getN();
    int w = coef.getW();
    //Static method that creates the CVC input file.

    std::cout << "INEQUATION:\n\n";

    std::cout << rV[0] << "\n\n";

    for(int i = 0; i < w; i++)
    {
        for(int j = 0; j < n; j++)
        {
            std::cout << " + x[" << i+1 << "," << j+1 << "]*("<< rV[i*n + j + 1] << ")";
        }
        std::cout << "\n\n";
    }

    std::cout << "<= 0\n\n";

    std::vector<int> sol = Solver::find_x(rV, n, w, v);

    std::cout << "SOLUTION:\n\n";

    if(sol[0] != -1)
        for(int i = 0; i < w; i++)
        {
            for(int j = 0; j < n; j++)
            {
                std::cout << "x[" << i+1 << "," << j+1 << "] = "<< sol[i*n + j] << " ";
            }
            std::cout << "\n\n";
        }
    else
        std::cout <<"There are no solutions.";

    return 1;
}
<commit_msg>made some changes in the output printing<commit_after>//Standard Libraries
#include <iostream>
#include <string>
#include <fstream>

//Header Files
#include "../include/Solver.hpp"
#include "../include/Matrix.hpp"

int main(int argc, char* argv[])
{
    if (argc < 2)
    {
        std::cout << "I need a input file " << argc;
        return -1;
    }

    //Opening the input;
    std::ifstream input (argv[1]);

    if(!input.is_open())
    {
        std::cout << "Error while trying to open " << argv[1] << "\n";
        return -1;
    }

    std::string buffer;
    std::string line;
    std::string answer;

    //Reading the input
    while(std::getline(input, line))
    {
        buffer.insert(buffer.size(), line);
        buffer.insert(buffer.size(), " ");
    }

    input.close();

    bool v = false;
    if(argc == 3 && argv[2][1] == 'v')
        v = true;

    Matrix coef(buffer);

    std::vector<double>& rV = coef.getVector();
    int n = coef.getN();
    int w = coef.getW();

    std::cout << "INEQUATION:\n\n";

    std::cout << rV[0] << "\n\n";

    for(int i = 0; i < w; i++)
    {
        for(int j = 0; j < n; j++)
        {
            std::cout << " + x[" << i+1 << "," << j+1 << "]*("<< rV[i*n + j + 1] << ")";
        }
        std::cout << "\n\n";
    }

    std::cout << "<= 0\n\n";

    std::vector<int> sol = Solver::find_x(rV, n, w);

    std::cout << "SOLUTION:\n\n";

    if(sol[0] != -1)
        for(int i = 0; i < w; i++)
        {
            for(int j = 0; j < n; j++)
            {
                std::cout << "x[" << i+1 << "," << j+1 << "] = "<< sol[i*n + j] << " ";
            }
            std::cout << "\n\n";
        }
    else
        std::cout <<"There are no solutions.";

    return 1;
}
<|endoftext|>