<commit_before>//
//  main.cpp
//  Euchre
//
//  Created by Dillon Hammond on 9/12/14.
//  Copyright (c) 2014 Learning. All rights reserved.
//

#define GLEW_STATIC
#include "GL/glew.h"
#include <SDL2/SDL.h>
#include "SOIL.h"
#define GLM_FORCE_RADIANS
#include "glm.hpp"
#include <iostream>
#include <fstream>
#include <vector>

GLuint LoadShaders(const char* vertex_file_path, const char* fragment_file_path) {

	// Create the shaders
	GLuint VertexShaderID = glCreateShader(GL_VERTEX_SHADER);
	GLuint FragmentShaderID = glCreateShader(GL_FRAGMENT_SHADER);

	// Read the Vertex Shader code from the file
	std::string VertexShaderCode;
	std::ifstream VertexShaderStream;
	VertexShaderStream.open(vertex_file_path);
	if(VertexShaderStream.is_open()) {
		std::string Line = "";
		while(getline(VertexShaderStream, Line)) {
			VertexShaderCode += "\n" + Line;
		}
		VertexShaderStream.close();
	}

	// Read the Fragment Shader code from the file
	std::string FragmentShaderCode;
	std::ifstream FragmentShaderStream;
	FragmentShaderStream.open(fragment_file_path);
	if(FragmentShaderStream.is_open()){
		std::string Line = "";
		while(getline(FragmentShaderStream, Line)) {
			FragmentShaderCode += "\n" + Line;
		}
		FragmentShaderStream.close();
	}
	GLint Result;
	// Compile Vertex Shader
	const char* VertexSourcePointer = VertexShaderCode.c_str();
	glShaderSource(VertexShaderID, 1, &VertexSourcePointer , NULL);
	glCompileShader(VertexShaderID);
	glGetShaderiv(VertexShaderID, GL_COMPILE_STATUS, &Result);
		if (Result == GL_FALSE) {
			std::cout << "NOOOV" << std::endl;
			std::cout << VertexSourcePointer << std::endl;
		}

	// Compile Fragment Shader
	const char* FragmentSourcePointer = FragmentShaderCode.c_str();
	glShaderSource(FragmentShaderID, 1, &FragmentSourcePointer , NULL);
	glCompileShader(FragmentShaderID);
	glGetShaderiv(FragmentShaderID, GL_COMPILE_STATUS, &Result);
		if (Result == GL_FALSE) {
			std::cout << "NOOOF" << std::endl;
			std::cout << FragmentSourcePointer << std::endl;
		}

	// Link the program
	GLuint ProgramID = glCreateProgram();
	glAttachShader(ProgramID, VertexShaderID);
	glAttachShader(ProgramID, FragmentShaderID);
	glLinkProgram(ProgramID);

	glDeleteShader(VertexShaderID);
	glDeleteShader(FragmentShaderID);

	return ProgramID;
}

SDL_Window* createWindow(const char* title, int x, int y, int w, int h, Uint32 flags) {
	SDL_Init(SDL_INIT_VIDEO);
	SDL_GL_SetAttribute(SDL_GL_CONTEXT_PROFILE_MASK, SDL_GL_CONTEXT_PROFILE_CORE);
	SDL_GL_SetAttribute(SDL_GL_CONTEXT_MAJOR_VERSION, 3);
	SDL_GL_SetAttribute(SDL_GL_CONTEXT_MINOR_VERSION, 2);
	SDL_GL_SetAttribute(SDL_GL_STENCIL_SIZE, 8);
	SDL_Window *window = SDL_CreateWindow(title, x, y, w, h, flags);
	return window;
}

void LoadTextures(std::vector<GLuint> textures, const char* filename, const char* texName, GLuint shaderProgram, int texNum) {
	int width, height;
	unsigned char* image;

	glActiveTexture(GL_TEXTURE0 + texNum);

	glBindTexture(GL_TEXTURE_2D, textures.at(texNum));
	image = SOIL_load_image(filename, &width, &height, 0, SOIL_LOAD_RGB);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_RGB, GL_UNSIGNED_BYTE, image);
	SOIL_free_image_data(image);
	glUniform1i(glGetUniformLocation(shaderProgram, "backGround"), texNum);

	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
}

template<class vecTYPE> // Allows for a vector of any type to be made and returned!
std::vector<vecTYPE> LoadData(const char* file_path) {
	// Read Vertices in from a file
	std::vector<vecTYPE> vertices;
	std::string fileCode;
	std::ifstream fileStream;
	fileStream.open(file_path);
	if(fileStream.is_open()) {
		std::string Line;
		while(getline(fileStream, Line, ',')) {
			fileCode = Line;
			float numRead = std::stof(fileCode); // std::stof converts a string to a float
			vertices.push_back(numRead); // When the value is pushed to the vertex, it is casted automatically
		}
		fileStream.close();
	}
	return vertices;
}

int main() {
	SDL_Window* window = createWindow("Euchre", SDL_WINDOWPOS_CENTERED, SDL_WINDOWPOS_CENTERED, 800, 600, SDL_WINDOW_OPENGL);
	SDL_GLContext context = SDL_GL_CreateContext(window);

	// Initialize GLEW
	glewExperimental = GL_TRUE;
	glewInit();

	// Create Vertex Array Object
	GLuint vao;
	glGenVertexArrays(1, &vao);
	glBindVertexArray(vao);

	// Create a Position Buffer Object and copy the vertex data to it
	GLuint positionBuffer;
	glGenBuffers(1, &positionBuffer);
	glBindBuffer(GL_ARRAY_BUFFER, positionBuffer);

	std::vector<float> bgPosition = LoadData<float>("./Resources/bgPosition.txt");

	glBufferData(GL_ARRAY_BUFFER, bgPosition.size() * sizeof(float), &bgPosition.at(0), GL_STATIC_DRAW);

	// Create a Color Buffer Object and copy the vertex data to it
	GLuint colorBuffer;
	glGenBuffers(1, &colorBuffer);
	glBindBuffer(GL_ARRAY_BUFFER, colorBuffer);

	std::vector<float> bgColor = LoadData<float>("./Resources/bgColor.txt");

	glBufferData(GL_ARRAY_BUFFER, bgColor.size() * sizeof(float), &bgColor.at(0), GL_STATIC_DRAW);

	// Create a Texture Buffer Object and copy the vertex data to it
	GLuint textureBuffer;
	glGenBuffers(1, &textureBuffer);
	glBindBuffer(GL_ARRAY_BUFFER, textureBuffer);

	std::vector<float> bgTexture = LoadData<float>("./Resources/bgTexture.txt");

	glBufferData(GL_ARRAY_BUFFER, bgTexture.size() * sizeof(float), &bgTexture.at(0), GL_STATIC_DRAW);

	// Create an element array
	GLuint ebo;
	glGenBuffers(1, &ebo);
	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, ebo);

	// Describes which set of points is drawn at each time
	std::vector<GLuint> drawOrder = LoadData<GLuint>("./Resources/drawOrder.txt");

	glBufferData(GL_ELEMENT_ARRAY_BUFFER, drawOrder.size() * sizeof(float), &drawOrder.at(0), GL_STATIC_DRAW);

	GLuint shaderProgram = LoadShaders("./Resources/vertexShader.txt", "./Resources/fragmentShader.txt");
	glUseProgram(shaderProgram);

	// Specify the layout of the vertex data
	GLint posAttrib = glGetAttribLocation(shaderProgram, "position");
	glEnableVertexAttribArray(posAttrib);
	glBindBuffer(GL_ARRAY_BUFFER, positionBuffer);
	glVertexAttribPointer(posAttrib, 2, GL_FLOAT, GL_FALSE, 0, 0);

	// Specify the color attributes
	GLint colAttrib = glGetAttribLocation(shaderProgram, "color");
	glEnableVertexAttribArray(colAttrib);
	glBindBuffer(GL_ARRAY_BUFFER, colorBuffer);
	glVertexAttribPointer(colAttrib, 3, GL_FLOAT, GL_FALSE, 0, 0);

	// Specifiy the texture usage
	GLint texAttrib = glGetAttribLocation(shaderProgram, "texcoord");
	glEnableVertexAttribArray(texAttrib);
	glBindBuffer(GL_ARRAY_BUFFER, textureBuffer);
	glVertexAttribPointer(texAttrib, 2, GL_FLOAT, GL_FALSE, 0, 0);

	std::vector<GLuint> textures(1,0);
	glGenTextures(1, &textures.at(0));

	LoadTextures(textures, "./Resources/Background.png", "backGround", shaderProgram, 0);

	SDL_Event windowEvent;
	while (true) {
		if (SDL_PollEvent(&windowEvent)) {
			if (windowEvent.type == SDL_QUIT) {
				break;
			}
		}

		// Clear the screen to black
		glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
		glClear(GL_COLOR_BUFFER_BIT);

		// Draw a rectangle from the 2 triangles using 6 indices
		glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_INT, 0);

		SDL_GL_SwapWindow(window);
	}
	glDeleteTextures(static_cast<GLsizei>(textures.size()), &textures.at(0)); // Casted to remove warning about precision loss (this doesn't matter)

	glDeleteProgram(shaderProgram);

	glDeleteBuffers(1, &ebo);
	glDeleteBuffers(1, &positionBuffer);

	glDeleteVertexArrays(1, &vao);

	SDL_GL_DeleteContext(context);
	return 0;
}<commit_msg>Position, color, texture are now made by a func<commit_after>//
//  main.cpp
//  Euchre
//
//  Created by Dillon Hammond on 9/12/14.
//  Copyright (c) 2014 Learning. All rights reserved.
//

#define GLEW_STATIC
#include "GL/glew.h"
#include <SDL2/SDL.h>
#include "SOIL.h"
#define GLM_FORCE_RADIANS
#include "glm.hpp"
#include <iostream>
#include <fstream>
#include <vector>

GLuint LoadShaders(const char* vertex_file_path, const char* fragment_file_path) {

	// Create the shaders
	GLuint VertexShaderID = glCreateShader(GL_VERTEX_SHADER);
	GLuint FragmentShaderID = glCreateShader(GL_FRAGMENT_SHADER);

	// Read the Vertex Shader code from the file
	std::string VertexShaderCode;
	std::ifstream VertexShaderStream;
	VertexShaderStream.open(vertex_file_path);
	if(VertexShaderStream.is_open()) {
		std::string Line = "";
		while(getline(VertexShaderStream, Line)) {
			VertexShaderCode += "\n" + Line;
		}
		VertexShaderStream.close();
	}

	// Read the Fragment Shader code from the file
	std::string FragmentShaderCode;
	std::ifstream FragmentShaderStream;
	FragmentShaderStream.open(fragment_file_path);
	if(FragmentShaderStream.is_open()){
		std::string Line = "";
		while(getline(FragmentShaderStream, Line)) {
			FragmentShaderCode += "\n" + Line;
		}
		FragmentShaderStream.close();
	}
	GLint Result;
	// Compile Vertex Shader
	const char* VertexSourcePointer = VertexShaderCode.c_str();
	glShaderSource(VertexShaderID, 1, &VertexSourcePointer , NULL);
	glCompileShader(VertexShaderID);
	glGetShaderiv(VertexShaderID, GL_COMPILE_STATUS, &Result);
		if (Result == GL_FALSE) {
			std::cout << "NOOOV" << std::endl;
			std::cout << VertexSourcePointer << std::endl;
		}

	// Compile Fragment Shader
	const char* FragmentSourcePointer = FragmentShaderCode.c_str();
	glShaderSource(FragmentShaderID, 1, &FragmentSourcePointer , NULL);
	glCompileShader(FragmentShaderID);
	glGetShaderiv(FragmentShaderID, GL_COMPILE_STATUS, &Result);
		if (Result == GL_FALSE) {
			std::cout << "NOOOF" << std::endl;
			std::cout << FragmentSourcePointer << std::endl;
		}

	// Link the program
	GLuint ProgramID = glCreateProgram();
	glAttachShader(ProgramID, VertexShaderID);
	glAttachShader(ProgramID, FragmentShaderID);
	glLinkProgram(ProgramID);

	glDeleteShader(VertexShaderID);
	glDeleteShader(FragmentShaderID);

	return ProgramID;
}

SDL_Window* createWindow(const char* title, int x, int y, int w, int h, Uint32 flags) {
	SDL_Init(SDL_INIT_VIDEO);
	SDL_GL_SetAttribute(SDL_GL_CONTEXT_PROFILE_MASK, SDL_GL_CONTEXT_PROFILE_CORE);
	SDL_GL_SetAttribute(SDL_GL_CONTEXT_MAJOR_VERSION, 3);
	SDL_GL_SetAttribute(SDL_GL_CONTEXT_MINOR_VERSION, 2);
	SDL_GL_SetAttribute(SDL_GL_STENCIL_SIZE, 8);
	SDL_Window *window = SDL_CreateWindow(title, x, y, w, h, flags);
	return window;
}

void LoadTextures(std::vector<GLuint> textures, const char* filename, const char* texName, GLuint shaderProgram, int texNum) {
	int width, height;
	unsigned char* image;

	glActiveTexture(GL_TEXTURE0 + texNum);

	glBindTexture(GL_TEXTURE_2D, textures.at(texNum));
	image = SOIL_load_image(filename, &width, &height, 0, SOIL_LOAD_RGB);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_RGB, GL_UNSIGNED_BYTE, image);
	SOIL_free_image_data(image);
	glUniform1i(glGetUniformLocation(shaderProgram, "backGround"), texNum);

	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
}

template<class vecTYPE> // Allows for a vector of any type to be made and returned!
std::vector<vecTYPE> LoadData(const char* file_path) {
	// Read Vertices in from a file
	std::vector<vecTYPE> vertices;
	std::string fileCode;
	std::ifstream fileStream;
	fileStream.open(file_path);
	if(fileStream.is_open()) {
		std::string Line;
		while(getline(fileStream, Line, ',')) {
			fileCode = Line;
			float numRead = std::stof(fileCode); // std::stof converts a string to a float
			vertices.push_back(numRead); // When the value is pushed to the vertex, it is casted automatically
		}
		fileStream.close();
	}
	return vertices;
}

template<class vecTYPE> // Redefine new vecTYPE
GLuint makeBufferObject(GLsizei numBuffers, GLenum target, GLenum usage, std::vector<vecTYPE> data) {
	GLuint buffer;
	glGenBuffers(numBuffers, &buffer);
	glBindBuffer(target, buffer);
	glBufferData(target, data.size() * sizeof(vecTYPE), &data.at(0), usage);

	return buffer;
}

int main() {
	SDL_Window* window = createWindow("Euchre", SDL_WINDOWPOS_CENTERED, SDL_WINDOWPOS_CENTERED, 800, 600, SDL_WINDOW_OPENGL);
	SDL_GLContext context = SDL_GL_CreateContext(window);

	// Initialize GLEW
	glewExperimental = GL_TRUE;
	glewInit();

	// Create Vertex Array Object
	GLuint vao;
	glGenVertexArrays(1, &vao);
	glBindVertexArray(vao);

	// Create a vector populated by text file data
	std::vector<float> bgPosition = LoadData<float>("./Resources/bgPosition.txt");

	// Create a Position Buffer Object and copy the vector data to it
	GLuint positionBuffer = makeBufferObject(1, GL_ARRAY_BUFFER, GL_STATIC_DRAW, bgPosition);

	// Create a vector populated by text file data
	std::vector<float> bgColor = LoadData<float>("./Resources/bgColor.txt");

	// Create a Color Buffer Object and copy the vector data to it
	GLuint colorBuffer = makeBufferObject(1, GL_ARRAY_BUFFER, GL_STATIC_DRAW, bgColor);

	// Create a vector populated by text file data
	std::vector<float> bgTexture = LoadData<float>("./Resources/bgTexture.txt");

	// Create a Texture Buffer Object and copy the vector data to it
	GLuint textureBuffer = makeBufferObject(1, GL_ARRAY_BUFFER, GL_STATIC_DRAW, bgTexture);

	// Create an element array
	GLuint ebo;
	glGenBuffers(1, &ebo);
	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, ebo);

	// Describes which set of points is drawn at each time
	std::vector<GLuint> drawOrder = LoadData<GLuint>("./Resources/drawOrder.txt");

	glBufferData(GL_ELEMENT_ARRAY_BUFFER, drawOrder.size() * sizeof(float), &drawOrder.at(0), GL_STATIC_DRAW);

	GLuint shaderProgram = LoadShaders("./Resources/vertexShader.txt", "./Resources/fragmentShader.txt");
	glUseProgram(shaderProgram);

	// Specify the layout of the vertex data
	GLint posAttrib = glGetAttribLocation(shaderProgram, "position");
	glEnableVertexAttribArray(posAttrib);
	glBindBuffer(GL_ARRAY_BUFFER, positionBuffer);
	glVertexAttribPointer(posAttrib, 2, GL_FLOAT, GL_FALSE, 0, 0);

	// Specify the color attributes
	GLint colAttrib = glGetAttribLocation(shaderProgram, "color");
	glEnableVertexAttribArray(colAttrib);
	glBindBuffer(GL_ARRAY_BUFFER, colorBuffer);
	glVertexAttribPointer(colAttrib, 3, GL_FLOAT, GL_FALSE, 0, 0);

	// Specifiy the texture usage
	GLint texAttrib = glGetAttribLocation(shaderProgram, "texcoord");
	glEnableVertexAttribArray(texAttrib);
	glBindBuffer(GL_ARRAY_BUFFER, textureBuffer);
	glVertexAttribPointer(texAttrib, 2, GL_FLOAT, GL_FALSE, 0, 0);

	std::vector<GLuint> textures(1,0);
	glGenTextures(1, &textures.at(0));

	LoadTextures(textures, "./Resources/Background.png", "backGround", shaderProgram, 0);

	SDL_Event windowEvent;
	while (true) {
		if (SDL_PollEvent(&windowEvent)) {
			if (windowEvent.type == SDL_QUIT) {
				break;
			}
		}

		// Clear the screen to black
		glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
		glClear(GL_COLOR_BUFFER_BIT);

		// Draw a rectangle from the 2 triangles using 6 indices
		glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_INT, 0);

		SDL_GL_SwapWindow(window);
	}
	glDeleteTextures(static_cast<GLsizei>(textures.size()), &textures.at(0)); // Casted to remove warning about precision loss (this doesn't matter)

	glDeleteProgram(shaderProgram);

	glDeleteBuffers(1, &ebo);
	glDeleteBuffers(1, &positionBuffer);

	glDeleteVertexArrays(1, &vao);

	SDL_GL_DeleteContext(context);
	return 0;
}<|endoftext|>


<commit_before>#include <PCU.h>
#include "phOutput.h"
#include "phLinks.h"
#include "phAdjacent.h"
#include "phBubble.h"

namespace ph {

static void getCounts(Output& o)
{
  for (int i = 0; i < 4; ++i)
    o.nGlobalEntities[i] = apf::countOwned(o.mesh, i);
  PCU_Add_Ints(o.nGlobalEntities, 4);
  o.nOwnedNodes = apf::countOwned(o.mesh, 0);
  o.nOverlapNodes = o.mesh->count(0);
  o.nGlobalNodes = o.nGlobalEntities[0];
}

static void getCoordinates(Output& o)
{
  apf::Mesh* m = o.mesh;
  int n = m->count(0);
  double* x = new double[n * 3];
  apf::MeshEntity* v;
  int i = 0;
  apf::MeshIterator* it = m->begin(0);
  while ((v = m->iterate(it))) {
    apf::Vector3 p;
    m->getPoint(v, 0, p);
    for (int j = 0; j < 3; ++j)
      x[j * n + i] = p[j]; /* FORTRAN indexing */
    ++i;
  }
  m->end(it);
  assert(i == n);
  o.arrays.coordinates = x;
}

/* so apparently old phParAdapt just used EN_id,
   and the id generator from pumi would do things
   like this. I guess PHASTA is ok with a unique
   number for each copy, regardless of part boundary
   sharing...

update: Michel says these global numbers are ignored
        by phasta. get rid of them when you can.
 */
static void getGlobal(Output& o)
{
  apf::Mesh* m = o.mesh;
  int n = m->count(0);
  int self = PCU_Comm_Self();
  int peers = PCU_Comm_Peers();
  int id = self + 1;
  o.arrays.globalNodeNumbers = new int[n];
  for (int i = 0; i < n; ++i) {
    o.arrays.globalNodeNumbers[i] = id;
    id += peers;
  }
}

static void getVertexLinks(Output& o, apf::Numbering* n)
{
  Links links;
  getLinks(o.mesh, 0, links);
  encodeILWORK(n, links, o.nlwork, o.arrays.ilwork);
}

static void getInterior(Output& o, apf::Numbering* n)
{
  apf::Mesh* m = o.mesh;
  Blocks& bs = o.blocks.interior;
  int*** ien = new int**[bs.getSize()];
  for (int i = 0; i < bs.getSize(); ++i) {
    ien[i] = new int*[bs.nElements[i]];
    int t = bs.keys[i].elementType;
    int nv = bs.keys[i].nElementVertices;
    apf::MeshEntity* e;
    int j = 0;
    apf::MeshIterator* it = m->begin(m->getDimension());
    while ((e = m->iterate(it))) {
      if (getPhastaType(m, e) != t)
        continue;
      ien[i][j] = new int[nv];
      apf::Downward v;
      getVertices(m, e, v);
      for (int k = 0; k < nv; ++k)
        ien[i][j][k] = apf::getNumber(n, v[k], 0, 0);
      ++j;
    }
    m->end(it);
  }
  o.arrays.ien = ien;
}

static void getBoundary(Output& o, BCs& bcs, apf::Numbering* n)
{
  apf::Mesh* m = o.mesh;
  ModelBounds modelFaces;
  int nbc = countNaturalBCs(*o.in);
  Blocks& bs = o.blocks.boundary;
  int*** ienb = new int**[bs.getSize()];
  int*** ibcb = new int**[bs.getSize()];
  double*** bcb = new double**[bs.getSize()];
  apf::NewArray<int> js(bs.getSize());
  for (int i = 0; i < bs.getSize(); ++i) {
    ienb[i] = new int*[bs.nElements[i]];
    ibcb[i] = new int*[bs.nElements[i]];
    bcb[i] = new double*[bs.nElements[i]];
    js[i] = 0;
  }
  gmi_model* gm = m->getModel();
  gmi_ent* gf;
  gmi_iter* git = gmi_begin(gm, m->getDimension() - 1);
  while ((gf = gmi_next(gm, git))) {
    apf::ModelEntity* mf = (apf::ModelEntity*)gf;
    int* ibcbMaster = new int[2]();
    double* bcbMaster = new double[nbc]();
    applyNaturalBCs(gm, gf, bcs, bcbMaster, ibcbMaster);
    apf::MeshEntity* f;
    apf::MeshIterator* it = m->begin(m->getDimension() - 1);
    while ((f = m->iterate(it))) {
      if (m->toModel(f) != mf)
        continue;
      BlockKey k;
      apf::MeshEntity* e = m->getUpward(f, 0);
      getBoundaryBlockKey(m, e, f, k);
      assert(bs.keyToIndex.count(k));
      int i = bs.keyToIndex[k];
      int j = js[i];
      int nv = k.nElementVertices;
      apf::Downward v;
      getBoundaryVertices(m, e, f, v);
      ienb[i][j] = new int[nv];
      for (int k = 0; k < nv; ++k)
        ienb[i][j][k] = apf::getNumber(n, v[k], 0, 0);
      bcb[i][j] = new double[nbc]();
      for (int k = 0; k < nbc; ++k)
        bcb[i][j][k] = bcbMaster[k];
      ibcb[i][j] = new int[2](); /* <- parens initialize to zero */
      for (int k = 0; k < 2; ++k)
        ibcb[i][j][k] = ibcbMaster[k];
      ++js[i];
    }
    delete [] ibcbMaster;
    delete [] bcbMaster;
    m->end(it);
  }
  gmi_end(gm, git);
  for (int i = 0; i < bs.getSize(); ++i)
    assert(js[i] == bs.nElements[i]);
  o.arrays.ienb = ienb;
  o.arrays.ibcb = ibcb;
  o.arrays.bcb = bcb;
}

static void getBoundaryElements(Output& o)
{
  Blocks& bs = o.blocks.boundary;
  int n = 0;
  for (int i = 0; i < bs.getSize(); ++i)
    n += bs.nElements[i];
  o.nBoundaryElements = n;
}

static void getMaxElementNodes(Output& o)
{
  int n = 0;
  Blocks& ibs = o.blocks.interior;
  for (int i = 0; i < ibs.getSize(); ++i)
    n = std::max(n, ibs.keys[i].nElementVertices);
  Blocks& bbs = o.blocks.boundary;
  for (int i = 0; i < bbs.getSize(); ++i)
    n = std::max(n, bbs.keys[i].nElementVertices);
  o.nMaxElementNodes = n;
}

static bool matchLess(apf::Copy& a, apf::Copy& b)
{
  if (a.peer != b.peer)
    return a.peer < b.peer;
  return a.entity < b.entity;
}

/* returns the global periodic master iff it is on this
   part, otherwise returns e */
static apf::MeshEntity* getPeriodicMaster(apf::Mesh* m, apf::MeshEntity* e)
{
  if ( ! m->hasMatching())
    return e;
  apf::Matches matches;
  m->getMatches(e, matches);
  if (!matches.getSize())
    return e;
  int self = PCU_Comm_Self();
  apf::Copy master(self, e);
  for (size_t i = 0; i < matches.getSize(); ++i)
    if (matchLess(matches[i], master))
      master = matches[i];
  if (master.peer == self)
    return master.entity;
  return e;
}

static void getPeriodicMasters(Output& o, apf::Numbering* n)
{
  apf::Mesh* m = o.mesh;
  int* iper = new int[m->count(0)];
  apf::MeshIterator* it = m->begin(0);
  apf::MeshEntity* e;
  int i = 0;
  while ((e = m->iterate(it))) {
    apf::MeshEntity* master = getPeriodicMaster(m, e);
    if (master == e)
      iper[i] = 0;
    else
      iper[i] = apf::getNumber(n, master, 0, 0) + 1;
    ++i;
  }
  m->end(it);
  o.arrays.iper = iper;
}

static void getEssentialBCsOn(BCs& bcs, Output& o, gmi_ent* ge)
{
  Input& in = *o.in;
  apf::Mesh* m = o.mesh;
  int ibcMaster = 0;
  int nec = countEssentialBCs(in);
  double* bcMaster = new double[nec]();
  gmi_model* gm = m->getModel();
  bool did = applyEssentialBCs(gm, ge, bcs, bcMaster, &ibcMaster);
  /* matching introduces an iper bit which in our system
     is really a per-entity thing, not specifically dictated
     by classification, so in that case we have to look
     at all the vertices anyway to see if they are periodic slaves */
  if (did || m->hasMatching()) {
    apf::MeshEntity* v;
    apf::MeshIterator* it = m->begin(0);
    int i = 0;
    int& ei = o.nEssentialBCNodes;
    while ((v = m->iterate(it))) {
      if (m->toModel(v) == (apf::ModelEntity*)ge) {
        apf::MeshEntity* master = getPeriodicMaster(m, v);
        if (did || (master != v)) {
          o.arrays.nbc[i] = ei + 1;
          o.arrays.ibc[ei] = ibcMaster;
          if (master != v)
            o.arrays.ibc[ei] |= (1<<10); //yes, hard coded...
          double* bc_ei = new double[nec]();
          for (int j = 0; j < nec; ++j)
            bc_ei[j] = bcMaster[j];
          o.arrays.bc[ei] = bc_ei;
          ++ei;
        }
      }
      ++i;
    }
    m->end(it);
  }
  delete [] bcMaster;
}

static void getEssentialBCs(BCs& bcs, Output& o)
{
  apf::Mesh* m = o.mesh;
  int nv = m->count(0);
  o.arrays.nbc = new int[nv];
  for (int i = 0; i < nv; ++i)
    o.arrays.nbc[i] = 0;
  o.arrays.ibc = new int[nv]();
  o.arrays.bc = new double*[nv];
  o.nEssentialBCNodes = 0;
  gmi_model* gm = m->getModel();
  for (int d = 3; d >= 0; --d) {
    gmi_iter* it = gmi_begin(gm, d);
    gmi_ent* ge;
    while ((ge = gmi_next(gm, it)))
      getEssentialBCsOn(bcs, o, ge);
    gmi_end(gm, it);
  }
}

static void getInitialConditions(BCs& bcs, Output& o)
{
  Input& in = *o.in;
  apf::Mesh* m = o.mesh;
  apf::MeshEntity* v;
  apf::NewArray<double> s(in.ensa_dof);
  apf::Field* f = m->findField("solution");
  apf::MeshIterator* it = m->begin(0);
  gmi_model* gm = m->getModel();
  while ((v = m->iterate(it))) {
    gmi_ent* ge = (gmi_ent*)m->toModel(v);
    apf::getComponents(f, v, 0, &s[0]);
/* unfortunately, there is no way to know which
   components this overwrites without creating
   bad coupling between pieces of code, so we
   call this for every vertex even though it
   only depends on classification */
    applySolutionBCs(gm, ge, bcs, &s[0]);
    apf::setComponents(f, v, 0, &s[0]);
  }
  m->end(it);
}

static void getElementGraph(Output& o)
{
  if (o.in->formElementGraph) {
    apf::Numbering* n = apf::numberElements(o.mesh, "ph::getElementGraph");
    o.arrays.ienneigh = formIENNEIGH(n);
    Links links;
    getLinks(o.mesh, o.mesh->getDimension() - 1, links);
    encodeILWORKF(n, links, o.nlworkf, o.arrays.ilworkf);
  } else {
    o.arrays.ilworkf = 0;
    o.arrays.ienneigh = 0;
  }
}

Output::~Output()
{
  delete [] arrays.coordinates;
  delete [] arrays.ilwork;
  delete [] arrays.ilworkf;
  delete [] arrays.iper;
  delete [] arrays.globalNodeNumbers;
  Blocks& ibs = blocks.interior;
  for (int i = 0; i < ibs.getSize(); ++i) {
    for (int j = 0; j < ibs.nElements[i]; ++j)
      delete [] arrays.ien[i][j];
    delete [] arrays.ien[i];
  }
  delete [] arrays.ien;
  Blocks& bbs = blocks.boundary;
  for (int i = 0; i < bbs.getSize(); ++i) {
    for (int j = 0; j < bbs.nElements[i]; ++j) {
      delete [] arrays.ienb[i][j];
      delete [] arrays.ibcb[i][j];
      delete [] arrays.bcb[i][j];
    }
    delete [] arrays.ienb[i];
    delete [] arrays.ibcb[i];
    delete [] arrays.bcb[i];
  }
  delete [] arrays.ienb;
  delete [] arrays.ibcb;
  delete [] arrays.bcb;
  delete [] arrays.nbc;
  delete [] arrays.ibc;
  for (int i = 0; i < nEssentialBCNodes; ++i)
    delete [] arrays.bc[i];
  delete [] arrays.bc;
  delete [] arrays.ienneigh;
}

void generateOutput(Input& in, BCs& bcs, apf::Mesh* mesh, Output& o)
{
  double t0 = PCU_Time();
  o.in = ∈
  o.mesh = mesh;
  getCounts(o);
  getCoordinates(o);
  getGlobal(o);
  getAllBlocks(o.mesh, o.blocks);
  apf::Numbering* n = apf::numberOverlapNodes(mesh, "ph_local");
  getVertexLinks(o, n);
  getInterior(o, n);
  getBoundary(o, bcs, n);
  getPeriodicMasters(o, n);
  apf::destroyNumbering(n);
  getBoundaryElements(o);
  getMaxElementNodes(o);
  getEssentialBCs(bcs, o);
  getInitialConditions(bcs, o);
  getElementGraph(o);
  if (in.initBubbles)
    initBubbles(o.mesh, in);
  double t1 = PCU_Time();
  if (!PCU_Comm_Self())
    printf("generated output structs in %f seconds\n",t1 - t0);
}

}
<commit_msg>evaluate essential BCs at all entities<commit_after>#include <PCU.h>
#include "phOutput.h"
#include "phLinks.h"
#include "phAdjacent.h"
#include "phBubble.h"

namespace ph {

static void getCounts(Output& o)
{
  for (int i = 0; i < 4; ++i)
    o.nGlobalEntities[i] = apf::countOwned(o.mesh, i);
  PCU_Add_Ints(o.nGlobalEntities, 4);
  o.nOwnedNodes = apf::countOwned(o.mesh, 0);
  o.nOverlapNodes = o.mesh->count(0);
  o.nGlobalNodes = o.nGlobalEntities[0];
}

static void getCoordinates(Output& o)
{
  apf::Mesh* m = o.mesh;
  int n = m->count(0);
  double* x = new double[n * 3];
  apf::MeshEntity* v;
  int i = 0;
  apf::MeshIterator* it = m->begin(0);
  while ((v = m->iterate(it))) {
    apf::Vector3 p;
    m->getPoint(v, 0, p);
    for (int j = 0; j < 3; ++j)
      x[j * n + i] = p[j]; /* FORTRAN indexing */
    ++i;
  }
  m->end(it);
  assert(i == n);
  o.arrays.coordinates = x;
}

/* so apparently old phParAdapt just used EN_id,
   and the id generator from pumi would do things
   like this. I guess PHASTA is ok with a unique
   number for each copy, regardless of part boundary
   sharing...

update: Michel says these global numbers are ignored
        by phasta. get rid of them when you can.
 */
static void getGlobal(Output& o)
{
  apf::Mesh* m = o.mesh;
  int n = m->count(0);
  int self = PCU_Comm_Self();
  int peers = PCU_Comm_Peers();
  int id = self + 1;
  o.arrays.globalNodeNumbers = new int[n];
  for (int i = 0; i < n; ++i) {
    o.arrays.globalNodeNumbers[i] = id;
    id += peers;
  }
}

static void getVertexLinks(Output& o, apf::Numbering* n)
{
  Links links;
  getLinks(o.mesh, 0, links);
  encodeILWORK(n, links, o.nlwork, o.arrays.ilwork);
}

static void getInterior(Output& o, apf::Numbering* n)
{
  apf::Mesh* m = o.mesh;
  Blocks& bs = o.blocks.interior;
  int*** ien = new int**[bs.getSize()];
  for (int i = 0; i < bs.getSize(); ++i) {
    ien[i] = new int*[bs.nElements[i]];
    int t = bs.keys[i].elementType;
    int nv = bs.keys[i].nElementVertices;
    apf::MeshEntity* e;
    int j = 0;
    apf::MeshIterator* it = m->begin(m->getDimension());
    while ((e = m->iterate(it))) {
      if (getPhastaType(m, e) != t)
        continue;
      ien[i][j] = new int[nv];
      apf::Downward v;
      getVertices(m, e, v);
      for (int k = 0; k < nv; ++k)
        ien[i][j][k] = apf::getNumber(n, v[k], 0, 0);
      ++j;
    }
    m->end(it);
  }
  o.arrays.ien = ien;
}

static void getBoundary(Output& o, BCs& bcs, apf::Numbering* n)
{
  apf::Mesh* m = o.mesh;
  ModelBounds modelFaces;
  int nbc = countNaturalBCs(*o.in);
  Blocks& bs = o.blocks.boundary;
  int*** ienb = new int**[bs.getSize()];
  int*** ibcb = new int**[bs.getSize()];
  double*** bcb = new double**[bs.getSize()];
  apf::NewArray<int> js(bs.getSize());
  for (int i = 0; i < bs.getSize(); ++i) {
    ienb[i] = new int*[bs.nElements[i]];
    ibcb[i] = new int*[bs.nElements[i]];
    bcb[i] = new double*[bs.nElements[i]];
    js[i] = 0;
  }
  gmi_model* gm = m->getModel();
  gmi_ent* gf;
  gmi_iter* git = gmi_begin(gm, m->getDimension() - 1);
  while ((gf = gmi_next(gm, git))) {
    apf::ModelEntity* mf = (apf::ModelEntity*)gf;
    int* ibcbMaster = new int[2]();
    double* bcbMaster = new double[nbc]();
    applyNaturalBCs(gm, gf, bcs, bcbMaster, ibcbMaster);
    apf::MeshEntity* f;
    apf::MeshIterator* it = m->begin(m->getDimension() - 1);
    while ((f = m->iterate(it))) {
      if (m->toModel(f) != mf)
        continue;
      BlockKey k;
      apf::MeshEntity* e = m->getUpward(f, 0);
      getBoundaryBlockKey(m, e, f, k);
      assert(bs.keyToIndex.count(k));
      int i = bs.keyToIndex[k];
      int j = js[i];
      int nv = k.nElementVertices;
      apf::Downward v;
      getBoundaryVertices(m, e, f, v);
      ienb[i][j] = new int[nv];
      for (int k = 0; k < nv; ++k)
        ienb[i][j][k] = apf::getNumber(n, v[k], 0, 0);
      bcb[i][j] = new double[nbc]();
      for (int k = 0; k < nbc; ++k)
        bcb[i][j][k] = bcbMaster[k];
      ibcb[i][j] = new int[2](); /* <- parens initialize to zero */
      for (int k = 0; k < 2; ++k)
        ibcb[i][j][k] = ibcbMaster[k];
      ++js[i];
    }
    delete [] ibcbMaster;
    delete [] bcbMaster;
    m->end(it);
  }
  gmi_end(gm, git);
  for (int i = 0; i < bs.getSize(); ++i)
    assert(js[i] == bs.nElements[i]);
  o.arrays.ienb = ienb;
  o.arrays.ibcb = ibcb;
  o.arrays.bcb = bcb;
}

static void getBoundaryElements(Output& o)
{
  Blocks& bs = o.blocks.boundary;
  int n = 0;
  for (int i = 0; i < bs.getSize(); ++i)
    n += bs.nElements[i];
  o.nBoundaryElements = n;
}

static void getMaxElementNodes(Output& o)
{
  int n = 0;
  Blocks& ibs = o.blocks.interior;
  for (int i = 0; i < ibs.getSize(); ++i)
    n = std::max(n, ibs.keys[i].nElementVertices);
  Blocks& bbs = o.blocks.boundary;
  for (int i = 0; i < bbs.getSize(); ++i)
    n = std::max(n, bbs.keys[i].nElementVertices);
  o.nMaxElementNodes = n;
}

static bool matchLess(apf::Copy& a, apf::Copy& b)
{
  if (a.peer != b.peer)
    return a.peer < b.peer;
  return a.entity < b.entity;
}

/* returns the global periodic master iff it is on this
   part, otherwise returns e */
static apf::MeshEntity* getPeriodicMaster(apf::Mesh* m, apf::MeshEntity* e)
{
  if ( ! m->hasMatching())
    return e;
  apf::Matches matches;
  m->getMatches(e, matches);
  if (!matches.getSize())
    return e;
  int self = PCU_Comm_Self();
  apf::Copy master(self, e);
  for (size_t i = 0; i < matches.getSize(); ++i)
    if (matchLess(matches[i], master))
      master = matches[i];
  if (master.peer == self)
    return master.entity;
  return e;
}

static void getPeriodicMasters(Output& o, apf::Numbering* n)
{
  apf::Mesh* m = o.mesh;
  int* iper = new int[m->count(0)];
  apf::MeshIterator* it = m->begin(0);
  apf::MeshEntity* e;
  int i = 0;
  while ((e = m->iterate(it))) {
    apf::MeshEntity* master = getPeriodicMaster(m, e);
    if (master == e)
      iper[i] = 0;
    else
      iper[i] = apf::getNumber(n, master, 0, 0) + 1;
    ++i;
  }
  m->end(it);
  o.arrays.iper = iper;
}

static void getEssentialBCsOn(BCs& bcs, Output& o, gmi_ent* ge)
{
  Input& in = *o.in;
  apf::Mesh* m = o.mesh;
  int ibcMaster = 0;
  int nec = countEssentialBCs(in);
  double* bcMaster = new double[nec]();
  gmi_model* gm = m->getModel();
  apf::MeshEntity* v;
  apf::MeshIterator* it = m->begin(0);
  int i = 0;
  int& ei = o.nEssentialBCNodes;
  /* this brute force reverse classification loop costs some time.
     its only here to get the same ordering as phParAdapt,
     but in theory we can do this whole thing through
     forward classification only. */
  while ((v = m->iterate(it))) {
    if (m->toModel(v) == (apf::ModelEntity*)ge) {
      apf::MeshEntity* master = getPeriodicMaster(m, v);
      bool did = applyEssentialBCs(gm, ge, bcs, bcMaster, &ibcMaster);
      /* matching introduces an iper bit which in our system
         is really a per-entity thing, not specifically dictated
         by classification, so in that case we have to look
         at all the vertices anyway to see if they are periodic slaves */
      if (did || (master != v)) {
        o.arrays.nbc[i] = ei + 1;
        o.arrays.ibc[ei] = ibcMaster;
        if (master != v)
          o.arrays.ibc[ei] |= (1<<10); //yes, hard coded...
        double* bc_ei = new double[nec]();
        for (int j = 0; j < nec; ++j)
          bc_ei[j] = bcMaster[j];
        o.arrays.bc[ei] = bc_ei;
        ++ei;
      }
    }
    ++i;
  }
  m->end(it);
  delete [] bcMaster;
}

static void getEssentialBCs(BCs& bcs, Output& o)
{
  apf::Mesh* m = o.mesh;
  int nv = m->count(0);
  o.arrays.nbc = new int[nv];
  for (int i = 0; i < nv; ++i)
    o.arrays.nbc[i] = 0;
  o.arrays.ibc = new int[nv]();
  o.arrays.bc = new double*[nv];
  o.nEssentialBCNodes = 0;
  gmi_model* gm = m->getModel();
  for (int d = 3; d >= 0; --d) {
    gmi_iter* it = gmi_begin(gm, d);
    gmi_ent* ge;
    while ((ge = gmi_next(gm, it)))
      getEssentialBCsOn(bcs, o, ge);
    gmi_end(gm, it);
  }
}

static void getInitialConditions(BCs& bcs, Output& o)
{
  Input& in = *o.in;
  apf::Mesh* m = o.mesh;
  apf::MeshEntity* v;
  apf::NewArray<double> s(in.ensa_dof);
  apf::Field* f = m->findField("solution");
  apf::MeshIterator* it = m->begin(0);
  gmi_model* gm = m->getModel();
  while ((v = m->iterate(it))) {
    gmi_ent* ge = (gmi_ent*)m->toModel(v);
    apf::getComponents(f, v, 0, &s[0]);
/* unfortunately, there is no way to know which
   components this overwrites without creating
   bad coupling between pieces of code, so we
   call this for every vertex even though it
   only depends on classification */
    applySolutionBCs(gm, ge, bcs, &s[0]);
    apf::setComponents(f, v, 0, &s[0]);
  }
  m->end(it);
}

static void getElementGraph(Output& o)
{
  if (o.in->formElementGraph) {
    apf::Numbering* n = apf::numberElements(o.mesh, "ph::getElementGraph");
    o.arrays.ienneigh = formIENNEIGH(n);
    Links links;
    getLinks(o.mesh, o.mesh->getDimension() - 1, links);
    encodeILWORKF(n, links, o.nlworkf, o.arrays.ilworkf);
  } else {
    o.arrays.ilworkf = 0;
    o.arrays.ienneigh = 0;
  }
}

Output::~Output()
{
  delete [] arrays.coordinates;
  delete [] arrays.ilwork;
  delete [] arrays.ilworkf;
  delete [] arrays.iper;
  delete [] arrays.globalNodeNumbers;
  Blocks& ibs = blocks.interior;
  for (int i = 0; i < ibs.getSize(); ++i) {
    for (int j = 0; j < ibs.nElements[i]; ++j)
      delete [] arrays.ien[i][j];
    delete [] arrays.ien[i];
  }
  delete [] arrays.ien;
  Blocks& bbs = blocks.boundary;
  for (int i = 0; i < bbs.getSize(); ++i) {
    for (int j = 0; j < bbs.nElements[i]; ++j) {
      delete [] arrays.ienb[i][j];
      delete [] arrays.ibcb[i][j];
      delete [] arrays.bcb[i][j];
    }
    delete [] arrays.ienb[i];
    delete [] arrays.ibcb[i];
    delete [] arrays.bcb[i];
  }
  delete [] arrays.ienb;
  delete [] arrays.ibcb;
  delete [] arrays.bcb;
  delete [] arrays.nbc;
  delete [] arrays.ibc;
  for (int i = 0; i < nEssentialBCNodes; ++i)
    delete [] arrays.bc[i];
  delete [] arrays.bc;
  delete [] arrays.ienneigh;
}

void generateOutput(Input& in, BCs& bcs, apf::Mesh* mesh, Output& o)
{
  double t0 = PCU_Time();
  o.in = ∈
  o.mesh = mesh;
  getCounts(o);
  getCoordinates(o);
  getGlobal(o);
  getAllBlocks(o.mesh, o.blocks);
  apf::Numbering* n = apf::numberOverlapNodes(mesh, "ph_local");
  getVertexLinks(o, n);
  getInterior(o, n);
  getBoundary(o, bcs, n);
  getPeriodicMasters(o, n);
  apf::destroyNumbering(n);
  getBoundaryElements(o);
  getMaxElementNodes(o);
  getEssentialBCs(bcs, o);
  getInitialConditions(bcs, o);
  getElementGraph(o);
  if (in.initBubbles)
    initBubbles(o.mesh, in);
  double t1 = PCU_Time();
  if (!PCU_Comm_Self())
    printf("generated output structs in %f seconds\n",t1 - t0);
}

}
<|endoftext|>


<commit_before>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    TestCellDistanceSelector.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
// .SECTION Thanks
// This test was written by Philippe Pebay, Kitware SAS 2012

#include "vtkCellArray.h"
#include "vtkCellData.h"
#include "vtkExtractSelection.h"
#include "vtkIdTypeArray.h"
#include "vtkInformation.h"
#include "vtkCellDistanceSelector.h"
#include "vtkMultiBlockDataSet.h"
#include "vtkPointData.h"
#include "vtkSelection.h"
#include "vtkSelectionNode.h"
#include "vtkSmartPointer.h"
#include "vtkTestUtilities.h"
#include "vtkUnstructuredGrid.h"
#include "vtkUnstructuredGridReader.h"
#include "vtkUnstructuredGridWriter.h"

#include <vtksys/ios/sstream>

// Reference values
vtkIdType cardCellDistanceSelection[] =
{
  125,
  54,
  108,
  45,
};

// ------------------------------------------------------------------------------------------------
static int CheckExtractedUGrid( vtkExtractSelection* extract,
                                const char* tag,
                                int testIdx,
                                bool writeGrid )
{
  // Output must be a multiblock dataset
  vtkMultiBlockDataSet* outputMB = vtkMultiBlockDataSet::SafeDownCast( extract->GetOutput() );
  if ( ! outputMB )
    {
    vtkGenericWarningMacro("Cannot downcast extracted selection to multiblock dataset.");

    return 1;
    }

  // First block must be an unstructured grid
  vtkUnstructuredGrid* ugrid = vtkUnstructuredGrid::SafeDownCast( outputMB->GetBlock( 0 ) );
  if ( ! ugrid )
    {
    vtkGenericWarningMacro("Cannot downcast extracted selection to unstructured grid.");

    return 1;
    }

  // Initialize test status
  int testStatus = 0;
  cerr << endl;

  // Verify selection cardinality
  vtkIdType nCells = ugrid->GetNumberOfCells();
  cout << tag
       << " contains "
       << nCells
       << " cells."
       << endl;

  if ( nCells != cardCellDistanceSelection[testIdx] )
    {
    vtkGenericWarningMacro( "Incorrect cardinality: "
                           << nCells
                           << " != "
                           << cardCellDistanceSelection[testIdx] );
    testStatus = 1;
    }

  // Verify selection cells
  cerr << "Original cell Ids (types): ";
  ugrid->GetCellData()->SetActiveScalars( "vtkOriginalCellIds" );
  vtkDataArray* oCellIds = ugrid->GetCellData()->GetScalars();
  for ( vtkIdType i = 0; i < oCellIds->GetNumberOfTuples(); ++ i )
    {
    cerr << oCellIds->GetTuple1( i )
         << " ";
    }
  cerr << endl;

  // If requested, write mesh
  if ( writeGrid )
    {
    vtksys_ios::ostringstream fileNameSS;
    fileNameSS << "./CellDistanceExtraction-"
               << testIdx
               << ".vtk";
    vtkSmartPointer<vtkUnstructuredGridWriter> writer = vtkSmartPointer<vtkUnstructuredGridWriter>::New();
    writer->SetFileName( fileNameSS.str().c_str() );
    writer->SetInputData( ugrid );
    writer->Write();
    cerr << "Wrote file "
         << fileNameSS.str()
         << endl;
    }

  return testStatus;
}

//----------------------------------------------------------------------------
int TestCellDistanceSelector( int argc, char * argv [] )
{
  // Initialize test value
  int testIntValue = 0;

  // Read 3D unstructured input mesh
  char* fileName = vtkTestUtilities::ExpandDataFileName( argc, argv, "Data/AngularSector.vtk");
  vtkSmartPointer<vtkUnstructuredGridReader> reader = vtkSmartPointer<vtkUnstructuredGridReader>::New();
  reader->SetFileName( fileName );
  reader->Update();
  delete [] fileName;

  // Create multi-block mesh for linear selector
  vtkSmartPointer<vtkMultiBlockDataSet> mesh = vtkSmartPointer<vtkMultiBlockDataSet>::New();
  mesh->SetNumberOfBlocks( 1 );
  mesh->GetMetaData( static_cast<unsigned>( 0 ) )->Set( vtkCompositeDataSet::NAME(), "Mesh" );
  mesh->SetBlock( 0, reader->GetOutput() );

  // *****************************************************************************
  // 0. Selection with distance of 2 from cell 7000
  // *****************************************************************************

  // Create a selection, sel0, of cell with index 7000
  vtkSmartPointer<vtkIdTypeArray> selArr0 = vtkSmartPointer<vtkIdTypeArray>::New();
  selArr0->InsertNextValue( 7000 );
  vtkSmartPointer<vtkSelectionNode> selNode0 = vtkSmartPointer<vtkSelectionNode>::New();
  selNode0->SetContentType( vtkSelectionNode::INDICES );
  selNode0->SetFieldType( vtkSelectionNode::CELL );
  selNode0->GetProperties()->Set( vtkSelectionNode::COMPOSITE_INDEX(), 1 );
  selNode0->SetSelectionList( selArr0 );
  vtkSmartPointer<vtkSelection> sel0 = vtkSmartPointer<vtkSelection>::New();
  sel0->AddNode( selNode0 );

  // Create selection up to topological distance of 2
  vtkSmartPointer<vtkCellDistanceSelector> ls0 = vtkSmartPointer<vtkCellDistanceSelector>::New();
  ls0->SetInputData( 0, sel0 );
  ls0->SetInputData( 1, mesh );
  ls0->SetDistance( 2 );

  // Extract selection from mesh
  vtkSmartPointer<vtkExtractSelection> es0 =  vtkSmartPointer<vtkExtractSelection>::New();
  es0->SetInputData( 0, mesh );
  es0->SetInputConnection( 1, ls0->GetOutputPort() );
  es0->Update();
  testIntValue += CheckExtractedUGrid( es0, "Selection d({7000})<3", 0, true );

  return testIntValue;
}
<commit_msg>Testing more parameters of cell-distance selector<commit_after>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    TestCellDistanceSelector.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
// .SECTION Thanks
// This test was written by Philippe Pebay, Kitware SAS 2012

#include "vtkCellArray.h"
#include "vtkCellData.h"
#include "vtkExtractSelection.h"
#include "vtkIdTypeArray.h"
#include "vtkInformation.h"
#include "vtkCellDistanceSelector.h"
#include "vtkMultiBlockDataSet.h"
#include "vtkPointData.h"
#include "vtkSelection.h"
#include "vtkSelectionNode.h"
#include "vtkSmartPointer.h"
#include "vtkTestUtilities.h"
#include "vtkUnstructuredGrid.h"
#include "vtkUnstructuredGridReader.h"
#include "vtkUnstructuredGridWriter.h"

#include <vtksys/ios/sstream>

// Reference values
vtkIdType cardCellDistanceSelection[] =
{
  125,
  16,
  108,
  45,
};

// ------------------------------------------------------------------------------------------------
static int CheckExtractedUGrid( vtkExtractSelection* extract,
                                const char* tag,
                                int testIdx,
                                bool writeGrid )
{
  // Output must be a multiblock dataset
  vtkMultiBlockDataSet* outputMB = vtkMultiBlockDataSet::SafeDownCast( extract->GetOutput() );
  if ( ! outputMB )
    {
    vtkGenericWarningMacro("Cannot downcast extracted selection to multiblock dataset.");

    return 1;
    }

  // First block must be an unstructured grid
  vtkUnstructuredGrid* ugrid = vtkUnstructuredGrid::SafeDownCast( outputMB->GetBlock( 0 ) );
  if ( ! ugrid )
    {
    vtkGenericWarningMacro("Cannot downcast extracted selection to unstructured grid.");

    return 1;
    }

  // Initialize test status
  int testStatus = 0;
  cerr << endl;

  // Verify selection cardinality
  vtkIdType nCells = ugrid->GetNumberOfCells();
  cout << tag
       << " contains "
       << nCells
       << " cells."
       << endl;

  if ( nCells != cardCellDistanceSelection[testIdx] )
    {
    vtkGenericWarningMacro( "Incorrect cardinality: "
                           << nCells
                           << " != "
                           << cardCellDistanceSelection[testIdx] );
    testStatus = 1;
    }

  // Verify selection cells
  cerr << "Original cell Ids (types): ";
  ugrid->GetCellData()->SetActiveScalars( "vtkOriginalCellIds" );
  vtkDataArray* oCellIds = ugrid->GetCellData()->GetScalars();
  for ( vtkIdType i = 0; i < oCellIds->GetNumberOfTuples(); ++ i )
    {
    cerr << oCellIds->GetTuple1( i )
         << " ";
    }
  cerr << endl;

  // If requested, write mesh
  if ( writeGrid )
    {
    vtksys_ios::ostringstream fileNameSS;
    fileNameSS << "./CellDistanceExtraction-"
               << testIdx
               << ".vtk";
    vtkSmartPointer<vtkUnstructuredGridWriter> writer = vtkSmartPointer<vtkUnstructuredGridWriter>::New();
    writer->SetFileName( fileNameSS.str().c_str() );
    writer->SetInputData( ugrid );
    writer->Write();
    cerr << "Wrote file "
         << fileNameSS.str()
         << endl;
    }

  return testStatus;
}

//----------------------------------------------------------------------------
int TestCellDistanceSelector( int argc, char * argv [] )
{
  // Initialize test value
  int testIntValue = 0;

  // Read 3D unstructured input mesh
  char* fileName = vtkTestUtilities::ExpandDataFileName( argc, argv, "Data/AngularSector.vtk");
  vtkSmartPointer<vtkUnstructuredGridReader> reader = vtkSmartPointer<vtkUnstructuredGridReader>::New();
  reader->SetFileName( fileName );
  reader->Update();
  delete [] fileName;

  // Create multi-block mesh for linear selector
  vtkSmartPointer<vtkMultiBlockDataSet> mesh = vtkSmartPointer<vtkMultiBlockDataSet>::New();
  mesh->SetNumberOfBlocks( 1 );
  mesh->GetMetaData( static_cast<unsigned>( 0 ) )->Set( vtkCompositeDataSet::NAME(), "Mesh" );
  mesh->SetBlock( 0, reader->GetOutput() );

  // *****************************************************************************
  // 0. Selection within distance of 2 from cell 7000
  // *****************************************************************************

  // Create a selection, sel0, of cell with index 7000
  vtkSmartPointer<vtkIdTypeArray> selArr0 = vtkSmartPointer<vtkIdTypeArray>::New();
  selArr0->InsertNextValue( 7000 );
  vtkSmartPointer<vtkSelectionNode> selNode0 = vtkSmartPointer<vtkSelectionNode>::New();
  selNode0->SetContentType( vtkSelectionNode::INDICES );
  selNode0->SetFieldType( vtkSelectionNode::CELL );
  selNode0->GetProperties()->Set( vtkSelectionNode::COMPOSITE_INDEX(), 1 );
  selNode0->SetSelectionList( selArr0 );
  vtkSmartPointer<vtkSelection> sel0 = vtkSmartPointer<vtkSelection>::New();
  sel0->AddNode( selNode0 );

  // Create selection up to topological distance of 2
  vtkSmartPointer<vtkCellDistanceSelector> ls0 = vtkSmartPointer<vtkCellDistanceSelector>::New();
  ls0->SetInputData( 0, sel0 );
  ls0->SetInputData( 1, mesh );
  ls0->SetDistance( 2 );

  // Extract selection from mesh
  vtkSmartPointer<vtkExtractSelection> es0 =  vtkSmartPointer<vtkExtractSelection>::New();
  es0->SetInputData( 0, mesh );
  es0->SetInputConnection( 1, ls0->GetOutputPort() );
  es0->Update();
  testIntValue += CheckExtractedUGrid( es0, "Selection d({7000})<3", 0, true );

  // *****************************************************************************
  // 1. Selection at distance of 1 from ridge 7643-7499-7355-7211, excluding it
  // *****************************************************************************

  // Create a selection, sel1, of cell with index 7643
  vtkSmartPointer<vtkIdTypeArray> selArr1 = vtkSmartPointer<vtkIdTypeArray>::New();
  selArr1->InsertNextValue( 7643 );
  selArr1->InsertNextValue( 7499 );
  selArr1->InsertNextValue( 7355 );
  selArr1->InsertNextValue( 7211 );
  vtkSmartPointer<vtkSelectionNode> selNode1 = vtkSmartPointer<vtkSelectionNode>::New();
  selNode1->SetContentType( vtkSelectionNode::INDICES );
  selNode1->SetFieldType( vtkSelectionNode::CELL );
  selNode1->GetProperties()->Set( vtkSelectionNode::COMPOSITE_INDEX(), 1 );
  selNode1->SetSelectionList( selArr1 );
  vtkSmartPointer<vtkSelection> sel1 = vtkSmartPointer<vtkSelection>::New();
  sel1->AddNode( selNode1 );

  // Create selection at distance of 1
  vtkSmartPointer<vtkCellDistanceSelector> ls1 = vtkSmartPointer<vtkCellDistanceSelector>::New();
  ls1->SetInputData( 0, sel1 );
  ls1->SetInputData( 1, mesh );
  ls1->SetDistance( 1 );
  ls1->IncludeSeedOff();

  // Extract selection from mesh
  vtkSmartPointer<vtkExtractSelection> es1 =  vtkSmartPointer<vtkExtractSelection>::New();
  es1->SetInputData( 0, mesh );
  es1->SetInputConnection( 1, ls1->GetOutputPort() );
  es1->Update();
  testIntValue += CheckExtractedUGrid( es1, "Selection d({7643-7499-7355-7211})=1", 1, true );

  return testIntValue;
}
<|endoftext|>


<commit_before>// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2014 Benoit Steiner <benoit.steiner.goog@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"

#include <Eigen/CXX11/Tensor>

using Eigen::Tensor;

static void test_dynamic_size()
{
  Eigen::DSizes<int, 3> dimensions(2,3,7);

  VERIFY_IS_EQUAL((int)Eigen::internal::array_get<0>(dimensions), 2);
  VERIFY_IS_EQUAL((int)Eigen::internal::array_get<1>(dimensions), 3);
  VERIFY_IS_EQUAL((int)Eigen::internal::array_get<2>(dimensions), 7);
  VERIFY_IS_EQUAL(dimensions.TotalSize(), 2*3*7);
  VERIFY_IS_EQUAL((int)dimensions[0], 2);
  VERIFY_IS_EQUAL((int)dimensions[1], 3);
  VERIFY_IS_EQUAL((int)dimensions[2], 7);
}

static void test_fixed_size()
{
  Eigen::Sizes<2,3,7> dimensions;

  VERIFY_IS_EQUAL((int)Eigen::internal::array_get<0>(dimensions), 2);
  VERIFY_IS_EQUAL((int)Eigen::internal::array_get<1>(dimensions), 3);
  VERIFY_IS_EQUAL((int)Eigen::internal::array_get<2>(dimensions), 7);
  VERIFY_IS_EQUAL(dimensions.TotalSize(), 2*3*7);
}

static void test_match()
{
  Eigen::DSizes<int, 3> dyn(2,3,7);
  Eigen::Sizes<2,3,7> stat;
  VERIFY_IS_EQUAL(Eigen::dimensions_match(dyn, stat), true);

  Eigen::DSizes<int, 3> dyn1(2,3,7);
  Eigen::DSizes<int, 2> dyn2(2,3);
  VERIFY_IS_EQUAL(Eigen::dimensions_match(dyn1, dyn2), false);
}

static void test_rank_zero()
{
  Eigen::Sizes<> scalar;
  VERIFY_IS_EQUAL(scalar.TotalSize(), 1);
  VERIFY_IS_EQUAL(scalar.rank(), 0);
  VERIFY_IS_EQUAL(internal::array_prod(scalar), 1);

  Eigen::DSizes<ptrdiff_t, 0> dscalar;
  VERIFY_IS_EQUAL(dscalar.TotalSize(), 1);
  VERIFY_IS_EQUAL(dscalar.rank(), 0u);
}

void test_cxx11_tensor_dimension()
{
  CALL_SUBTEST(test_dynamic_size());
  CALL_SUBTEST(test_fixed_size());
  CALL_SUBTEST(test_match());
  CALL_SUBTEST(test_rank_zero());
}
<commit_msg>Fix warnings<commit_after>// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2014 Benoit Steiner <benoit.steiner.goog@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"

#include <Eigen/CXX11/Tensor>

using Eigen::Tensor;

static void test_dynamic_size()
{
  Eigen::DSizes<int, 3> dimensions(2,3,7);

  VERIFY_IS_EQUAL((int)Eigen::internal::array_get<0>(dimensions), 2);
  VERIFY_IS_EQUAL((int)Eigen::internal::array_get<1>(dimensions), 3);
  VERIFY_IS_EQUAL((int)Eigen::internal::array_get<2>(dimensions), 7);
  VERIFY_IS_EQUAL((int)dimensions.TotalSize(), 2*3*7);
  VERIFY_IS_EQUAL((int)dimensions[0], 2);
  VERIFY_IS_EQUAL((int)dimensions[1], 3);
  VERIFY_IS_EQUAL((int)dimensions[2], 7);
}

static void test_fixed_size()
{
  Eigen::Sizes<2,3,7> dimensions;

  VERIFY_IS_EQUAL((int)Eigen::internal::array_get<0>(dimensions), 2);
  VERIFY_IS_EQUAL((int)Eigen::internal::array_get<1>(dimensions), 3);
  VERIFY_IS_EQUAL((int)Eigen::internal::array_get<2>(dimensions), 7);
  VERIFY_IS_EQUAL((int)dimensions.TotalSize(), 2*3*7);
}

static void test_match()
{
  Eigen::DSizes<unsigned int, 3> dyn(2,3,7);
  Eigen::Sizes<2,3,7> stat;
  VERIFY_IS_EQUAL(Eigen::dimensions_match(dyn, stat), true);

  Eigen::DSizes<int, 3> dyn1(2,3,7);
  Eigen::DSizes<int, 2> dyn2(2,3);
  VERIFY_IS_EQUAL(Eigen::dimensions_match(dyn1, dyn2), false);
}

static void test_rank_zero()
{
  Eigen::Sizes<> scalar;
  VERIFY_IS_EQUAL((int)scalar.TotalSize(), 1);
  VERIFY_IS_EQUAL((int)scalar.rank(), 0);
  VERIFY_IS_EQUAL((int)internal::array_prod(scalar), 1);

  Eigen::DSizes<ptrdiff_t, 0> dscalar;
  VERIFY_IS_EQUAL((int)dscalar.TotalSize(), 1);
  VERIFY_IS_EQUAL((int)dscalar.rank(), 0);
}

void test_cxx11_tensor_dimension()
{
  CALL_SUBTEST(test_dynamic_size());
  CALL_SUBTEST(test_fixed_size());
  CALL_SUBTEST(test_match());
  CALL_SUBTEST(test_rank_zero());
}
<|endoftext|>


<commit_before>#include "controller_ros.h"

#include "uranus_dp/eigen_helper.h"
#include "uranus_dp/vortex_helper.h"
#include <tf/transform_datatypes.h>
#include <eigen_conversions/eigen_msg.h>

#include "std_msgs/String.h"

#include <math.h>

Controller::Controller(ros::NodeHandle nh) : nh(nh)
{
  command_sub = nh.subscribe("propulsion_command", 10, &Controller::commandCallback, this);
  state_sub   = nh.subscribe("state_estimate", 10, &Controller::stateCallback, this);
  wrench_pub  = nh.advertise<geometry_msgs::Wrench>("rov_forces", 10);
  mode_pub    = nh.advertise<std_msgs::String>("controller/mode", 10);

  control_mode = ControlModes::OPEN_LOOP;

  if (!nh.getParam("/controller/frequency", frequency))
  {
    ROS_WARN("Failed to read parameter controller frequency, defaulting to 10 Hz.");
    frequency = 10;
  }

  state = new State();
  initSetpoints();
  initPositionHoldController();

  // Set up a dynamic reconfigure server
  dynamic_reconfigure::Server<uranus_dp::ControllerConfig>::CallbackType cb;
  cb = boost::bind(&Controller::configCallback, this, _1, _2);
  dr_srv.setCallback(cb);

  ROS_INFO("Controller: Initialized with dynamic reconfigure.");
}

void Controller::commandCallback(const vortex_msgs::PropulsionCommand& msg)
{
  if (!healthyMessage(msg))
  {
    ROS_WARN("Controller: Propulsion command message out of range, ignoring...");
    return;
  }

  ControlMode new_control_mode;
  {
    int i;
    for (i = 0; i < msg.control_mode.size(); ++i)
      if (msg.control_mode[i])
        break;
    new_control_mode = static_cast<ControlMode>(i);
  }

  if (new_control_mode != control_mode)
  {
    control_mode = new_control_mode;

    Eigen::Vector3d    position;
    Eigen::Quaterniond orientation;
    Eigen::Vector6d    velocity;

    // Reset setpoints to be equal to state
    state->get(position, orientation, velocity);
    setpoints->set(position, orientation);

    ROS_INFO_STREAM("Controller: Changing mode to control mode " << control_mode << ".");
  }
  publishControlMode();

  double time = msg.header.stamp.toSec();
  Eigen::Vector6d command;
  for (int i = 0; i < 6; ++i)
    command(i) = msg.motion[i];
  setpoints->update(time, command);
}

void Controller::stateCallback(const nav_msgs::Odometry &msg)
{
  Eigen::Vector3d    position;
  Eigen::Quaterniond orientation;
  Eigen::Vector6d    velocity;

  tf::pointMsgToEigen(msg.pose.pose.position, position);
  tf::quaternionMsgToEigen(msg.pose.pose.orientation, orientation);
  tf::twistMsgToEigen(msg.twist.twist, velocity);

  const double MAX_QUAT_NORM_DEVIATION = 0.1;
  bool orientation_invalid = (abs(orientation.norm() - 1) > MAX_QUAT_NORM_DEVIATION);
  if (isFucked(position) || isFucked(velocity) || orientation_invalid)
  {
    ROS_WARN("Controller: State not valid, ignoring...");
    return;
  }

  state->set(position, orientation, velocity);
}

void Controller::configCallback(uranus_dp::ControllerConfig &config, uint32_t level)
{
  ROS_INFO_STREAM("Setting quat pd gains:   a = " << config.a << ",   b = " << config.b << ",   c = " << config.c);
  position_hold_controller->setGains(config.a, config.b, config.c);
}

void Controller::spin()
{
  Eigen::Vector6d    tau_command          = Eigen::VectorXd::Zero(6);
  Eigen::Vector6d    tau_openloop         = Eigen::VectorXd::Zero(6);
  Eigen::Vector6d    tau_sixdof           = Eigen::VectorXd::Zero(6);

  Eigen::Vector3d    position_state       = Eigen::Vector3d::Zero();
  Eigen::Quaterniond orientation_state    = Eigen::Quaterniond::Identity();
  Eigen::Vector6d    velocity_state       = Eigen::VectorXd::Zero(6);

  Eigen::Vector3d    position_setpoint    = Eigen::Vector3d::Zero();
  Eigen::Quaterniond orientation_setpoint = Eigen::Quaterniond::Identity();

  ros::Rate rate(frequency);
  while (ros::ok())
  {
    // TODO: check value of bool return from getters
    state->get(position_state, orientation_state, velocity_state);
    setpoints->get(position_setpoint, orientation_setpoint);

    switch (control_mode)
    {
      case ControlModes::OPEN_LOOP:
      {
        setpoints->get(tau_command);
        break;
      }

      case ControlModes::SIXDOF:
      {
        tau_command = position_hold_controller->compute(position_state,
                                                        orientation_state,
                                                        velocity_state,
                                                        position_setpoint,
                                                        orientation_setpoint);
        break;
      }

      case ControlModes::RPY_DEPTH:
      {
        // TODO: make this similar to depth hold
        tau_command(0) = tau_openloop(0);
        tau_command(1) = tau_openloop(1);
        tau_command(2) = tau_sixdof(2);
        tau_command(3) = tau_sixdof(3);
        tau_command(4) = tau_sixdof(4);
        tau_command(5) = tau_sixdof(5);
        break;
      }

      case ControlModes::DEPTH_HOLD:
      {
        setpoints->get(tau_openloop);

        bool depth_change_commanded = abs(tau_openloop(2) > FORCE_DEADZONE_LIMIT);
        if (depth_change_commanded)
        {
          tau_command = tau_openloop;
        }
        else
        {
          position_setpoint(0) = position_state(0);
          position_setpoint(1) = position_state(1);
          orientation_setpoint = orientation_state;
          tau_sixdof = position_hold_controller->compute(position_state,
                                                         orientation_state,
                                                         velocity_state,
                                                         position_setpoint,
                                                         orientation_setpoint);
          tau_command = tau_sixdof + tau_openloop;
        }
        break;
      }

      default:
      {
        ROS_ERROR("Default control mode reached in Controller::spin().");
        break;
      }
    }

    geometry_msgs::Wrench msg;
    tf::wrenchEigenToMsg(tau_command, msg);
    wrench_pub.publish(msg);

    ros::spinOnce();
    rate.sleep();
  }
}

void Controller::initSetpoints()
{
  std::vector<double> v;

  if (!nh.getParam("/propulsion/command/wrench/max", v))
    ROS_FATAL("Failed to read parameter max wrench command.");
  Eigen::Vector6d wrench_command_max = Eigen::Vector6d::Map(v.data(), v.size());

  if (!nh.getParam("/propulsion/command/wrench/scaling", v))
    ROS_FATAL("Failed to read parameter scaling wrench command.");
  Eigen::Vector6d wrench_command_scaling = Eigen::Vector6d::Map(v.data(), v.size());

  if (!nh.getParam("/propulsion/command/pose/rate", v))
    ROS_FATAL("Failed to read parameter pose command rate.");
  Eigen::Vector6d pose_command_rate = Eigen::Vector6d::Map(v.data(), v.size());

  setpoints = new Setpoints(wrench_command_scaling,
                            wrench_command_max,
                            pose_command_rate);
}

void Controller::initPositionHoldController()
{
  // Read controller gains from parameter server
  std::map<std::string,double> gains;
  if (!nh.getParam("/controller/gains", gains))
    ROS_ERROR("Failed to read parameter controller gains.");

  // Read center of gravity and buoyancy vectors
  std::vector<double> r_G_vec, r_B_vec;
  if (!nh.getParam("/physical/center_of_mass", r_G_vec))
    ROS_ERROR("Failed to read robot center of mass parameter.");
  if (!nh.getParam("/physical/center_of_buoyancy", r_B_vec))
    ROS_ERROR("Failed to read robot center of buoyancy parameter.");
  Eigen::Vector3d r_G(r_G_vec.data());
  Eigen::Vector3d r_B(r_B_vec.data());

  // Read and calculate ROV weight and buoyancy
  double mass, displacement, acceleration_of_gravity, density_of_water;
  if (!nh.getParam("/physical/mass_kg", mass))
    ROS_ERROR("Failed to read parameter mass.");
  if (!nh.getParam("/physical/displacement_m3", displacement))
    ROS_ERROR("Failed to read parameter displacement.");
  if (!nh.getParam("/gravity/acceleration", acceleration_of_gravity))
    ROS_ERROR("Failed to read parameter acceleration of gravity");
  if (!nh.getParam("/water/density", density_of_water))
    ROS_ERROR("Failed to read parameter density of water");
  double W = mass * acceleration_of_gravity;
  double B = density_of_water * displacement * acceleration_of_gravity;

  position_hold_controller = new QuaternionPdController(gains["a"],
                                                        gains["b"],
                                                        gains["c"],
                                                        W, B, r_G, r_B);
}

bool Controller::healthyMessage(const vortex_msgs::PropulsionCommand& msg)
{
  // Check that motion commands are in range
  for (int i = 0; i < msg.motion.size(); ++i)
  {
    if (msg.motion[i] > 1 || msg.motion[i] < -1)
    {
      ROS_WARN("Controller: Motion command out of range.");
      return false;
    }
  }

  // Check that exactly one control mode is requested
  int num_requested_modes = 0;
  for (int i = 0; i < msg.control_mode.size(); ++i)
    if (msg.control_mode[i])
      num_requested_modes++;

  if (num_requested_modes != 1)
  {
    ROS_WARN_STREAM("Controller: Invalid control mode. Attempt to set "
                    << num_requested_modes << " control modes at once.");
    return false;
  }

  return true;
}

void Controller::publishControlMode()
{
  std::string s = controlModeString(control_mode);
  std_msgs::String msg;
  msg.data = s;
  mode_pub.publish(msg);
}
<commit_msg>Improve control mode console output<commit_after>#include "controller_ros.h"

#include "uranus_dp/eigen_helper.h"
#include "uranus_dp/vortex_helper.h"
#include <tf/transform_datatypes.h>
#include <eigen_conversions/eigen_msg.h>

#include "std_msgs/String.h"

#include <math.h>

Controller::Controller(ros::NodeHandle nh) : nh(nh)
{
  command_sub = nh.subscribe("propulsion_command", 10, &Controller::commandCallback, this);
  state_sub   = nh.subscribe("state_estimate", 10, &Controller::stateCallback, this);
  wrench_pub  = nh.advertise<geometry_msgs::Wrench>("rov_forces", 10);
  mode_pub    = nh.advertise<std_msgs::String>("controller/mode", 10);

  control_mode = ControlModes::OPEN_LOOP;

  if (!nh.getParam("/controller/frequency", frequency))
  {
    ROS_WARN("Failed to read parameter controller frequency, defaulting to 10 Hz.");
    frequency = 10;
  }

  state = new State();
  initSetpoints();
  initPositionHoldController();

  // Set up a dynamic reconfigure server
  dynamic_reconfigure::Server<uranus_dp::ControllerConfig>::CallbackType cb;
  cb = boost::bind(&Controller::configCallback, this, _1, _2);
  dr_srv.setCallback(cb);

  ROS_INFO("Controller: Initialized with dynamic reconfigure.");
}

void Controller::commandCallback(const vortex_msgs::PropulsionCommand& msg)
{
  if (!healthyMessage(msg))
  {
    ROS_WARN("Controller: Propulsion command message out of range, ignoring...");
    return;
  }

  ControlMode new_control_mode;
  {
    int i;
    for (i = 0; i < msg.control_mode.size(); ++i)
      if (msg.control_mode[i])
        break;
    new_control_mode = static_cast<ControlMode>(i);
  }

  if (new_control_mode != control_mode)
  {
    control_mode = new_control_mode;

    Eigen::Vector3d    position;
    Eigen::Quaterniond orientation;
    Eigen::Vector6d    velocity;

    // Reset setpoints to be equal to state
    state->get(position, orientation, velocity);
    setpoints->set(position, orientation);

    ROS_INFO_STREAM("Controller: Changing mode to " << controlModeString(control_mode) << ".");
  }
  publishControlMode();

  double time = msg.header.stamp.toSec();
  Eigen::Vector6d command;
  for (int i = 0; i < 6; ++i)
    command(i) = msg.motion[i];
  setpoints->update(time, command);
}

void Controller::stateCallback(const nav_msgs::Odometry &msg)
{
  Eigen::Vector3d    position;
  Eigen::Quaterniond orientation;
  Eigen::Vector6d    velocity;

  tf::pointMsgToEigen(msg.pose.pose.position, position);
  tf::quaternionMsgToEigen(msg.pose.pose.orientation, orientation);
  tf::twistMsgToEigen(msg.twist.twist, velocity);

  const double MAX_QUAT_NORM_DEVIATION = 0.1;
  bool orientation_invalid = (abs(orientation.norm() - 1) > MAX_QUAT_NORM_DEVIATION);
  if (isFucked(position) || isFucked(velocity) || orientation_invalid)
  {
    ROS_WARN("Controller: State not valid, ignoring...");
    return;
  }

  state->set(position, orientation, velocity);
}

void Controller::configCallback(uranus_dp::ControllerConfig &config, uint32_t level)
{
  ROS_INFO_STREAM("Setting quat pd gains:   a = " << config.a << ",   b = " << config.b << ",   c = " << config.c);
  position_hold_controller->setGains(config.a, config.b, config.c);
}

void Controller::spin()
{
  Eigen::Vector6d    tau_command          = Eigen::VectorXd::Zero(6);
  Eigen::Vector6d    tau_openloop         = Eigen::VectorXd::Zero(6);
  Eigen::Vector6d    tau_sixdof           = Eigen::VectorXd::Zero(6);

  Eigen::Vector3d    position_state       = Eigen::Vector3d::Zero();
  Eigen::Quaterniond orientation_state    = Eigen::Quaterniond::Identity();
  Eigen::Vector6d    velocity_state       = Eigen::VectorXd::Zero(6);

  Eigen::Vector3d    position_setpoint    = Eigen::Vector3d::Zero();
  Eigen::Quaterniond orientation_setpoint = Eigen::Quaterniond::Identity();

  ros::Rate rate(frequency);
  while (ros::ok())
  {
    // TODO: check value of bool return from getters
    state->get(position_state, orientation_state, velocity_state);
    setpoints->get(position_setpoint, orientation_setpoint);

    switch (control_mode)
    {
      case ControlModes::OPEN_LOOP:
      {
        setpoints->get(tau_command);
        break;
      }

      case ControlModes::SIXDOF:
      {
        tau_command = position_hold_controller->compute(position_state,
                                                        orientation_state,
                                                        velocity_state,
                                                        position_setpoint,
                                                        orientation_setpoint);
        break;
      }

      case ControlModes::RPY_DEPTH:
      {
        // TODO: make this similar to depth hold
        tau_command(0) = tau_openloop(0);
        tau_command(1) = tau_openloop(1);
        tau_command(2) = tau_sixdof(2);
        tau_command(3) = tau_sixdof(3);
        tau_command(4) = tau_sixdof(4);
        tau_command(5) = tau_sixdof(5);
        break;
      }

      case ControlModes::DEPTH_HOLD:
      {
        setpoints->get(tau_openloop);

        bool depth_change_commanded = abs(tau_openloop(2) > FORCE_DEADZONE_LIMIT);
        if (depth_change_commanded)
        {
          tau_command = tau_openloop;
        }
        else
        {
          position_setpoint(0) = position_state(0);
          position_setpoint(1) = position_state(1);
          orientation_setpoint = orientation_state;
          tau_sixdof = position_hold_controller->compute(position_state,
                                                         orientation_state,
                                                         velocity_state,
                                                         position_setpoint,
                                                         orientation_setpoint);
          tau_command = tau_sixdof + tau_openloop;
        }
        break;
      }

      default:
      {
        ROS_ERROR("Default control mode reached in Controller::spin().");
        break;
      }
    }

    geometry_msgs::Wrench msg;
    tf::wrenchEigenToMsg(tau_command, msg);
    wrench_pub.publish(msg);

    ros::spinOnce();
    rate.sleep();
  }
}

void Controller::initSetpoints()
{
  std::vector<double> v;

  if (!nh.getParam("/propulsion/command/wrench/max", v))
    ROS_FATAL("Failed to read parameter max wrench command.");
  Eigen::Vector6d wrench_command_max = Eigen::Vector6d::Map(v.data(), v.size());

  if (!nh.getParam("/propulsion/command/wrench/scaling", v))
    ROS_FATAL("Failed to read parameter scaling wrench command.");
  Eigen::Vector6d wrench_command_scaling = Eigen::Vector6d::Map(v.data(), v.size());

  if (!nh.getParam("/propulsion/command/pose/rate", v))
    ROS_FATAL("Failed to read parameter pose command rate.");
  Eigen::Vector6d pose_command_rate = Eigen::Vector6d::Map(v.data(), v.size());

  setpoints = new Setpoints(wrench_command_scaling,
                            wrench_command_max,
                            pose_command_rate);
}

void Controller::initPositionHoldController()
{
  // Read controller gains from parameter server
  std::map<std::string,double> gains;
  if (!nh.getParam("/controller/gains", gains))
    ROS_ERROR("Failed to read parameter controller gains.");

  // Read center of gravity and buoyancy vectors
  std::vector<double> r_G_vec, r_B_vec;
  if (!nh.getParam("/physical/center_of_mass", r_G_vec))
    ROS_ERROR("Failed to read robot center of mass parameter.");
  if (!nh.getParam("/physical/center_of_buoyancy", r_B_vec))
    ROS_ERROR("Failed to read robot center of buoyancy parameter.");
  Eigen::Vector3d r_G(r_G_vec.data());
  Eigen::Vector3d r_B(r_B_vec.data());

  // Read and calculate ROV weight and buoyancy
  double mass, displacement, acceleration_of_gravity, density_of_water;
  if (!nh.getParam("/physical/mass_kg", mass))
    ROS_ERROR("Failed to read parameter mass.");
  if (!nh.getParam("/physical/displacement_m3", displacement))
    ROS_ERROR("Failed to read parameter displacement.");
  if (!nh.getParam("/gravity/acceleration", acceleration_of_gravity))
    ROS_ERROR("Failed to read parameter acceleration of gravity");
  if (!nh.getParam("/water/density", density_of_water))
    ROS_ERROR("Failed to read parameter density of water");
  double W = mass * acceleration_of_gravity;
  double B = density_of_water * displacement * acceleration_of_gravity;

  position_hold_controller = new QuaternionPdController(gains["a"],
                                                        gains["b"],
                                                        gains["c"],
                                                        W, B, r_G, r_B);
}

bool Controller::healthyMessage(const vortex_msgs::PropulsionCommand& msg)
{
  // Check that motion commands are in range
  for (int i = 0; i < msg.motion.size(); ++i)
  {
    if (msg.motion[i] > 1 || msg.motion[i] < -1)
    {
      ROS_WARN("Controller: Motion command out of range.");
      return false;
    }
  }

  // Check that exactly one control mode is requested
  int num_requested_modes = 0;
  for (int i = 0; i < msg.control_mode.size(); ++i)
    if (msg.control_mode[i])
      num_requested_modes++;

  if (num_requested_modes != 1)
  {
    ROS_WARN_STREAM("Controller: Invalid control mode. Attempt to set "
                    << num_requested_modes << " control modes at once.");
    return false;
  }

  return true;
}

void Controller::publishControlMode()
{
  std::string s = controlModeString(control_mode);
  std_msgs::String msg;
  msg.data = s;
  mode_pub.publish(msg);
}
<|endoftext|>


<commit_before>#include "writer/verilog/axi/slave_controller.h"

#include "writer/verilog/axi/slave_port.h"
#include "writer/verilog/module.h"
#include "writer/verilog/ports.h"

namespace iroha {
namespace writer {
namespace verilog {
namespace axi {

SlaveController::SlaveController(const IResource &res,
				 bool reset_polarity)
  : AxiController(res, reset_polarity) {
  ports_.reset(new Ports);
}

SlaveController::~SlaveController() {
}

void SlaveController::Write(ostream &os) {
  string name = SlavePort::ControllerName(res_, reset_polarity_);
  os << "// slave controller: "
     << name << "\n";
  AddSramPorts();
  string initials;
  GenReadChannel(cfg_, false, nullptr, ports_.get(), &initials);
  GenWriteChannel(cfg_, false, nullptr, ports_.get(), &initials);
  os << "module " << name << "(";
  ports_->Output(Ports::PORT_NAME, os);
  os << ");\n";
  ports_->Output(Ports::PORT_TYPE, os);

  os << "  `define S_IDLE 0\n"
     << "  `define S_WRITE_DONE 7\n"
     << "  // single cycle mode (sram_EXCLUSIVE)\n"
     << "  `define S_READ 1\n"
     << "  `define S_WRITE 2\n"
     << "  // multi cycle mode (!sram_EXCLUSIVE)\n"
     << "  `define S_READ_AXI 3\n"
     << "  `define S_READ_SRAM 4\n"
     << "  `define S_WRITE_AXI 5\n"
     << "  `define S_WRITE_SRAM 6\n";
  os << "  reg [3:0] st;\n\n";
  os << "  reg [" << sram_addr_width_ << ":0] idx;\n\n";
  os << "  reg first_addr;\n"
     << "  reg last_write;\n"
     << "  reg [7:0] rlen;\n\n";

  os << "  always @(posedge clk) begin\n"
     << "    if (" << (reset_polarity_ ? "" : "!")
     << ResetName(reset_polarity_) << ") begin\n"
     << "      st <= `S_IDLE;\n"
     << "      first_addr <= 0;\n"
     << "      last_write <= 0;\n"
     << "      rlen <= 0;\n";
  os << initials
     << "    end else begin\n";
  OutputFSM(os);
  os << "    end\n"
     << "  end\n"
     << "endmodule\n";
}

void SlaveController::AddPorts(const PortConfig &cfg, Module *mod, string *s) {
  Ports *ports = mod->GetPorts();
  GenWriteChannel(cfg, false, mod, ports, s);
  GenReadChannel(cfg, false, mod, ports, s);
}

void SlaveController::OutputFSM(ostream &os) {
  os << "      sram_wen <= (st == `S_WRITE && WVALID);\n"
     << "      case (st)\n"
     << "        `S_IDLE: begin\n"
     << "          if (ARVALID) begin\n"
     << "            if (ARREADY) begin\n"
     << "              ARREADY <= 0;\n"
     << "              sram_addr <= ARADDR[" << (sram_addr_width_ - 1) << ":0];\n"
     << "              rlen <= ARLEN;\n"
     << "              if (sram_EXCLUSIVE) begin\n"
     << "                st <= `S_READ;\n"
     << "              end else begin\n"
     << "                st <= `S_READ_SRAM;\n"
     << "                sram_req <= 1;\n"
     << "              end\n"
     << "            end else begin\n"
     << "              ARREADY <= 1;\n"
     << "            end\n"
     << "          end else if (AWVALID) begin\n"
     << "            if (AWREADY) begin\n"
     << "              AWREADY <= 0;\n"
     << "              first_addr <= 1;\n"
     << "              sram_addr <= AWADDR[" << (sram_addr_width_ - 1) << ":0];\n"
     << "              if (sram_EXCLUSIVE) begin\n"
     << "                st <= `S_WRITE;\n"
     << "              end else begin\n"
     << "                st <= `S_WRITE_AXI;\n"
     << "              end\n"
     << "              WREADY <= 1;\n"
     << "            end else begin\n"
     << "              AWREADY <= 1;\n"
     << "            end\n"
     << "          end\n"
     << "        end\n"
     << "        `S_READ: begin\n"
     << "          if (RREADY && RVALID) begin\n"
     << "            rlen <= rlen - 1;\n"
     << "            if (rlen == 0) begin\n"
     << "              st <= `S_IDLE;\n"
     << "              RLAST <= 0;\n"
     << "              RVALID <= 0;\n"
     << "            end else if (rlen == 1) begin\n"
     << "              RLAST <= 1;\n"
     << "            end\n"
     << "          end else begin\n"
     << "            RVALID <= 1;\n"
     << "          end\n"
     << "          if (RREADY) begin\n"
     << "            sram_addr <= sram_addr + 1;\n"
     << "          end\n"
     << "          RDATA <= sram_rdata;\n"
     << "        end\n"
     << "        `S_WRITE: begin\n"
     << "          if (WVALID) begin\n"
     << "            sram_wdata <= WDATA;\n"
     << "            if (first_addr) begin\n"
     << "              first_addr <= 0;\n"
     << "            end else begin\n"
     << "              sram_addr <= sram_addr + 1;\n"
     << "            end\n"
     << "          end\n"
     << "          if (WLAST && WVALID) begin\n"
     << "            st <= `S_WRITE_DONE;\n"
     << "            BVALID <= 1;\n"
     << "            WREADY <= 0;\n"
     << "          end\n"
     << "        end\n"
     << "        `S_READ_AXI: begin\n"
     << "          if (RREADY) begin\n"
     << "            RVALID <= 0;\n"
     << "            RLAST <= 0;\n"
     << "            rlen <= rlen - 1;\n"
     << "            if (rlen == 0) begin\n"
     << "              st <= `S_IDLE;\n"
     << "            end else begin\n"
     << "              st <= `S_READ_SRAM;\n"
     << "              sram_addr <= sram_addr + 1;\n"
     << "              sram_req <= 1;\n"
     << "            end\n"
     << "          end\n"
     << "        end\n"
     << "        `S_READ_SRAM: begin\n"
     << "          if (sram_ack) begin\n"
     << "            sram_req <= 0;\n"
     << "            st <= `S_READ_AXI;\n"
     << "            RDATA <= sram_rdata;\n"
     << "            RVALID <= 1;\n"
     << "            if (rlen == 0) begin\n"
     << "              RLAST <= 1;\n"
     << "            end\n"
     << "          end\n"
     << "        end\n"
     << "        `S_WRITE_AXI: begin\n"
     << "          if (WVALID) begin\n"
     << "            WREADY <= 0;\n"
     << "            sram_req <= 1;\n"
     << "            sram_wdata <= WDATA;\n"
     << "            last_write <= WLAST;\n"
     << "            st <= `S_WRITE_SRAM;\n"
     << "          end\n"
     << "        end\n"
     << "        `S_WRITE_SRAM: begin\n"
     << "          if (sram_ack) begin\n"
     << "            sram_req <= 0;\n"
     << "            if (last_write) begin\n"
     << "              st <= `S_IDLE;\n"
     << "            end else begin\n"
     << "              sram_addr <= sram_addr + 1;\n"
     << "              WREADY <= 1;\n"
     << "            end\n"
     << "          end\n"
     << "        end\n"
     << "        `S_WRITE_DONE: begin\n"
     << "          st <= `S_IDLE;\n"
     << "          if (BREADY) begin\n"
     << "            BVALID <= 0;\n"
     << "          end\n"
     << "        end\n";
  os << "      endcase\n";
}

}  // namespace axi
}  // namespace verilog
}  // namespace writer
}  // namespace iroha
<commit_msg>Fix axi slave state transition.<commit_after>#include "writer/verilog/axi/slave_controller.h"

#include "writer/verilog/axi/slave_port.h"
#include "writer/verilog/module.h"
#include "writer/verilog/ports.h"

namespace iroha {
namespace writer {
namespace verilog {
namespace axi {

SlaveController::SlaveController(const IResource &res,
				 bool reset_polarity)
  : AxiController(res, reset_polarity) {
  ports_.reset(new Ports);
}

SlaveController::~SlaveController() {
}

void SlaveController::Write(ostream &os) {
  string name = SlavePort::ControllerName(res_, reset_polarity_);
  os << "// slave controller: "
     << name << "\n";
  AddSramPorts();
  string initials;
  GenReadChannel(cfg_, false, nullptr, ports_.get(), &initials);
  GenWriteChannel(cfg_, false, nullptr, ports_.get(), &initials);
  os << "module " << name << "(";
  ports_->Output(Ports::PORT_NAME, os);
  os << ");\n";
  ports_->Output(Ports::PORT_TYPE, os);

  os << "  `define S_IDLE 0\n"
     << "  `define S_WRITE_DONE 7\n"
     << "  // single cycle mode (sram_EXCLUSIVE)\n"
     << "  `define S_READ 1\n"
     << "  `define S_WRITE 2\n"
     << "  // multi cycle mode (!sram_EXCLUSIVE)\n"
     << "  `define S_READ_AXI 3\n"
     << "  `define S_READ_SRAM 4\n"
     << "  `define S_WRITE_AXI 5\n"
     << "  `define S_WRITE_SRAM 6\n";
  os << "  reg [3:0] st;\n\n";
  os << "  reg [" << sram_addr_width_ << ":0] idx;\n\n";
  os << "  reg first_addr;\n"
     << "  reg last_write;\n"
     << "  reg [7:0] rlen;\n\n";

  os << "  always @(posedge clk) begin\n"
     << "    if (" << (reset_polarity_ ? "" : "!")
     << ResetName(reset_polarity_) << ") begin\n"
     << "      st <= `S_IDLE;\n"
     << "      first_addr <= 0;\n"
     << "      last_write <= 0;\n"
     << "      rlen <= 0;\n"
     << "      sram_req <= 0;\n";
  os << initials
     << "    end else begin\n";
  OutputFSM(os);
  os << "    end\n"
     << "  end\n"
     << "endmodule\n";
}

void SlaveController::AddPorts(const PortConfig &cfg, Module *mod, string *s) {
  Ports *ports = mod->GetPorts();
  GenWriteChannel(cfg, false, mod, ports, s);
  GenReadChannel(cfg, false, mod, ports, s);
}

void SlaveController::OutputFSM(ostream &os) {
  os << "      sram_wen <= (st == `S_WRITE && WVALID);\n"
     << "      case (st)\n"
     << "        `S_IDLE: begin\n"
     << "          if (ARVALID) begin\n"
     << "            if (ARREADY) begin\n"
     << "              ARREADY <= 0;\n"
     << "              sram_addr <= ARADDR[" << (sram_addr_width_ - 1) << ":0];\n"
     << "              rlen <= ARLEN;\n"
     << "              if (sram_EXCLUSIVE) begin\n"
     << "                st <= `S_READ;\n"
     << "              end else begin\n"
     << "                st <= `S_READ_SRAM;\n"
     << "                sram_req <= 1;\n"
     << "              end\n"
     << "            end else begin\n"
     << "              ARREADY <= 1;\n"
     << "            end\n"
     << "          end else if (AWVALID) begin\n"
     << "            if (AWREADY) begin\n"
     << "              AWREADY <= 0;\n"
     << "              first_addr <= 1;\n"
     << "              sram_addr <= AWADDR[" << (sram_addr_width_ - 1) << ":0];\n"
     << "              if (sram_EXCLUSIVE) begin\n"
     << "                st <= `S_WRITE;\n"
     << "              end else begin\n"
     << "                st <= `S_WRITE_AXI;\n"
     << "              end\n"
     << "              WREADY <= 1;\n"
     << "            end else begin\n"
     << "              AWREADY <= 1;\n"
     << "            end\n"
     << "          end\n"
     << "        end\n"
     << "        `S_READ: begin\n"
     << "          if (RREADY && RVALID) begin\n"
     << "            rlen <= rlen - 1;\n"
     << "            if (rlen == 0) begin\n"
     << "              st <= `S_IDLE;\n"
     << "              RLAST <= 0;\n"
     << "              RVALID <= 0;\n"
     << "            end else if (rlen == 1) begin\n"
     << "              RLAST <= 1;\n"
     << "            end\n"
     << "          end else begin\n"
     << "            RVALID <= 1;\n"
     << "          end\n"
     << "          if (RREADY) begin\n"
     << "            sram_addr <= sram_addr + 1;\n"
     << "          end\n"
     << "          RDATA <= sram_rdata;\n"
     << "        end\n"
     << "        `S_WRITE: begin\n"
     << "          if (WVALID) begin\n"
     << "            sram_wdata <= WDATA;\n"
     << "            if (first_addr) begin\n"
     << "              first_addr <= 0;\n"
     << "            end else begin\n"
     << "              sram_addr <= sram_addr + 1;\n"
     << "            end\n"
     << "          end\n"
     << "          if (WLAST && WVALID) begin\n"
     << "            st <= `S_WRITE_DONE;\n"
     << "            BVALID <= 1;\n"
     << "            WREADY <= 0;\n"
     << "          end\n"
     << "        end\n"
     << "        `S_READ_AXI: begin\n"
     << "          if (RREADY) begin\n"
     << "            RVALID <= 0;\n"
     << "            RLAST <= 0;\n"
     << "            rlen <= rlen - 1;\n"
     << "            if (rlen == 0) begin\n"
     << "              st <= `S_IDLE;\n"
     << "            end else begin\n"
     << "              st <= `S_READ_SRAM;\n"
     << "              sram_addr <= sram_addr + 1;\n"
     << "              sram_req <= 1;\n"
     << "            end\n"
     << "          end\n"
     << "        end\n"
     << "        `S_READ_SRAM: begin\n"
     << "          if (sram_ack) begin\n"
     << "            sram_req <= 0;\n"
     << "            st <= `S_READ_AXI;\n"
     << "            RDATA <= sram_rdata;\n"
     << "            RVALID <= 1;\n"
     << "            if (rlen == 0) begin\n"
     << "              RLAST <= 1;\n"
     << "            end\n"
     << "          end\n"
     << "        end\n"
     << "        `S_WRITE_AXI: begin\n"
     << "          if (WVALID) begin\n"
     << "            WREADY <= 0;\n"
     << "            sram_req <= 1;\n"
     << "            sram_wdata <= WDATA;\n"
     << "            last_write <= WLAST;\n"
     << "            st <= `S_WRITE_SRAM;\n"
     << "          end\n"
     << "        end\n"
     << "        `S_WRITE_SRAM: begin\n"
     << "          if (sram_ack) begin\n"
     << "            sram_req <= 0;\n"
     << "            if (last_write) begin\n"
     << "              st <= `S_IDLE;\n"
     << "            end else begin\n"
     << "              sram_addr <= sram_addr + 1;\n"
     << "              WREADY <= 1;\n"
     << "              st <= `S_WRITE_AXI;\n"
     << "            end\n"
     << "          end\n"
     << "        end\n"
     << "        `S_WRITE_DONE: begin\n"
     << "          st <= `S_IDLE;\n"
     << "          if (BREADY) begin\n"
     << "            BVALID <= 0;\n"
     << "          end\n"
     << "        end\n";
  os << "      endcase\n";
}

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