text:
/*
---------------------------------------------------------------------------
Open Asset Import Library (assimp)
---------------------------------------------------------------------------

Copyright (c) 2006-2012, assimp team

All rights reserved.

Redistribution and use of this software in source and binary forms,
with or without modification, are permitted provided that the following
conditions are met:

* Redistributions of source code must retain the above
  copyright notice, this list of conditions and the
  following disclaimer.

* Redistributions in binary form must reproduce the above
  copyright notice, this list of conditions and the
  following disclaimer in the documentation and/or other
  materials provided with the distribution.

* Neither the name of the assimp team, nor the names of its
  contributors may be used to endorse or promote products
  derived from this software without specific prior
  written permission of the assimp team.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
---------------------------------------------------------------------------
*/

/** @file  MD5Loader.cpp
 *  @brief Implementation of the MD5 importer class
 */

#ifndef ASSIMP_BUILD_NO_MD5_IMPORTER

// internal headers
#include "RemoveComments.h"
#include "MD5Loader.h"
#include "StringComparison.h"
#include "fast_atof.h"
#include "SkeletonMeshBuilder.h"
#include "../include/assimp/Importer.hpp"
#include "../include/assimp/scene.h"
#include <boost/scoped_ptr.hpp>
#include "../include/assimp/IOSystem.hpp"
#include "../include/assimp/DefaultLogger.hpp"

using namespace Assimp;

// Minimum weight value. Weights inside [-n ... n] are ignored
#define AI_MD5_WEIGHT_EPSILON 1e-5f

static const aiImporterDesc desc = {
	"Doom 3 / MD5 Mesh Importer",
	"",
	"",
	"",
	aiImporterFlags_SupportBinaryFlavour,
	0,
	0,
	0,
	0,
	"md5mesh md5camera md5anim"
};

// ------------------------------------------------------------------------------------------------
// Constructor to be privately used by Importer
MD5Importer::MD5Importer()
: mBuffer()
, configNoAutoLoad (false)
{}

// ------------------------------------------------------------------------------------------------
// Destructor, private as well
MD5Importer::~MD5Importer()
{}

// ------------------------------------------------------------------------------------------------
// Returns whether the class can handle the format of the given file.
bool MD5Importer::CanRead( const std::string& pFile, IOSystem* pIOHandler, bool checkSig) const
{
	const std::string extension = GetExtension(pFile);

	if (extension == "md5anim" || extension == "md5mesh" || extension == "md5camera")
		return true;
	else if (!extension.length() || checkSig)	{
		if (!pIOHandler) {
			return true;
		}
		const char* tokens[] = {"MD5Version"};
		return SearchFileHeaderForToken(pIOHandler,pFile,tokens,1);
	}
	return false;
}

// ------------------------------------------------------------------------------------------------
// Get list of all supported extensions
const aiImporterDesc* MD5Importer::GetInfo () const
{
	return &desc;
}

// ------------------------------------------------------------------------------------------------
// Setup import properties
void MD5Importer::SetupProperties(const Importer* pImp)
{
	// AI_CONFIG_IMPORT_MD5_NO_ANIM_AUTOLOAD
	configNoAutoLoad = (0 !=  pImp->GetPropertyInteger(AI_CONFIG_IMPORT_MD5_NO_ANIM_AUTOLOAD,0));
}

// ------------------------------------------------------------------------------------------------
// Imports the given file into the given scene structure.
void MD5Importer::InternReadFile( const std::string& pFile,
								 aiScene* _pScene, IOSystem* _pIOHandler)
{
	pIOHandler = _pIOHandler;
	pScene     = _pScene;
	bHadMD5Mesh = bHadMD5Anim = bHadMD5Camera = false;

	// remove the file extension
	const std::string::size_type pos = pFile.find_last_of('.');
	mFile = (std::string::npos == pos ? pFile : pFile.substr(0,pos+1));

	const std::string extension = GetExtension(pFile);
	try {
		if (extension == "md5camera") {
			LoadMD5CameraFile();
		}
		else if (configNoAutoLoad || extension == "md5anim") {
			// determine file extension and process just *one* file
			if (extension.length() == 0) {
				throw DeadlyImportError("Failure, need file extension to determine MD5 part type");
			}
			if (extension == "md5anim") {
				LoadMD5AnimFile();
			}
			else if (extension == "md5mesh") {
				LoadMD5MeshFile();
			}
		}
		else {
			LoadMD5MeshFile();
			LoadMD5AnimFile();
		}
	}
	catch ( ... ) { // std::exception, Assimp::DeadlyImportError
		UnloadFileFromMemory();
		throw;
	}

	// make sure we have at least one file
	if (!bHadMD5Mesh && !bHadMD5Anim && !bHadMD5Camera) {
		throw DeadlyImportError("Failed to read valid contents out of this MD5* file");
	}

	// Now rotate the whole scene 90 degrees around the x axis to match our internal coordinate system
	pScene->mRootNode->mTransformation = aiMatrix4x4(1.f,0.f,0.f,0.f,
		0.f,0.f,1.f,0.f,0.f,-1.f,0.f,0.f,0.f,0.f,0.f,1.f);

	// the output scene wouldn't pass the validation without this flag
	if (!bHadMD5Mesh) {
		pScene->mFlags |= AI_SCENE_FLAGS_INCOMPLETE;
	}

	// clean the instance -- the BaseImporter instance may be reused later.
	UnloadFileFromMemory();
}

// ------------------------------------------------------------------------------------------------
// Load a file into a memory buffer
void MD5Importer::LoadFileIntoMemory (IOStream* file)
{
	// unload the previous buffer, if any
	UnloadFileFromMemory();

	ai_assert(NULL != file);
	fileSize = (unsigned int)file->FileSize();
	ai_assert(fileSize);

	// allocate storage and copy the contents of the file to a memory buffer
	mBuffer = new char[fileSize+1];
	file->Read( (void*)mBuffer, 1, fileSize);
	iLineNumber = 1;

	// append a terminal 0
	mBuffer[fileSize] = '\0';

	// now remove all line comments from the file
	CommentRemover::RemoveLineComments("//",mBuffer,' ');
}

// ------------------------------------------------------------------------------------------------
// Unload the current memory buffer
void MD5Importer::UnloadFileFromMemory ()
{
	// delete the file buffer
	delete[] mBuffer;
	mBuffer = NULL;
	fileSize = 0;
}

// ------------------------------------------------------------------------------------------------
// Build unique vertices
void MD5Importer::MakeDataUnique (MD5::MeshDesc& meshSrc)
{
	std::vector<bool> abHad(meshSrc.mVertices.size(),false);

	// allocate enough storage to keep the output structures
	const unsigned int iNewNum = meshSrc.mFaces.size()*3;
	unsigned int iNewIndex = meshSrc.mVertices.size();
	meshSrc.mVertices.resize(iNewNum);

	// try to guess how much storage we'll need for new weights
	const float fWeightsPerVert = meshSrc.mWeights.size() / (float)iNewIndex;
	const unsigned int guess = (unsigned int)(fWeightsPerVert*iNewNum);
	meshSrc.mWeights.reserve(guess + (guess >> 3)); // + 12.5% as buffer

	for (FaceList::const_iterator iter = meshSrc.mFaces.begin(),iterEnd = meshSrc.mFaces.end();iter != iterEnd;++iter){
		const aiFace& face = *iter;
		for (unsigned int i = 0; i < 3;++i) {
			if (face.mIndices[0] >= meshSrc.mVertices.size()) {
				throw DeadlyImportError("MD5MESH: Invalid vertex index");
			}

			if (abHad[face.mIndices[i]])	{
				// generate a new vertex
				meshSrc.mVertices[iNewIndex] = meshSrc.mVertices[face.mIndices[i]];
				face.mIndices[i] = iNewIndex++;
			}
			else abHad[face.mIndices[i]] = true;
		}
		// swap face order
		std::swap(face.mIndices[0],face.mIndices[2]);
	}
}

// ------------------------------------------------------------------------------------------------
// Recursive node graph construction from a MD5MESH
void MD5Importer::AttachChilds_Mesh(int iParentID,aiNode* piParent, BoneList& bones)
{
	ai_assert(NULL != piParent && !piParent->mNumChildren);

	// First find out how many children we'll have
	for (int i = 0; i < (int)bones.size();++i)	{
		if (iParentID != i && bones[i].mParentIndex == iParentID)	{
			++piParent->mNumChildren;
		}
	}
	if (piParent->mNumChildren)	{
		piParent->mChildren = new aiNode*[piParent->mNumChildren];
		for (int i = 0; i < (int)bones.size();++i)	{
			// (avoid infinite recursion)
			if (iParentID != i && bones[i].mParentIndex == iParentID)	{
				aiNode* pc;
				// setup a new node
				*piParent->mChildren++ = pc = new aiNode();
				pc->mName = aiString(bones[i].mName);
				pc->mParent = piParent;

				// get the transformation matrix from rotation and translational components
				aiQuaternion quat;
				MD5::ConvertQuaternion ( bones[i].mRotationQuat, quat );

				// FIX to get to Assimp's quaternion conventions
				quat.w *= -1.f;

				bones[i].mTransform = aiMatrix4x4 ( quat.GetMatrix());
				bones[i].mTransform.a4 = bones[i].mPositionXYZ.x;
				bones[i].mTransform.b4 = bones[i].mPositionXYZ.y;
				bones[i].mTransform.c4 = bones[i].mPositionXYZ.z;

				// store it for later use
				pc->mTransformation = bones[i].mInvTransform = bones[i].mTransform;
				bones[i].mInvTransform.Inverse();

				// the transformations for each bone are absolute, so we need to multiply them
				// with the inverse of the absolute matrix of the parent joint
				if (-1 != iParentID)	{
					pc->mTransformation = bones[iParentID].mInvTransform * pc->mTransformation;
				}

				// add children to this node, too
				AttachChilds_Mesh( i, pc, bones);
			}
		}
		// undo offset computations
		piParent->mChildren -= piParent->mNumChildren;
	}
}

// ------------------------------------------------------------------------------------------------
// Recursive node graph construction from a MD5ANIM
void MD5Importer::AttachChilds_Anim(int iParentID,aiNode* piParent, AnimBoneList& bones,const aiNodeAnim** node_anims)
{
	ai_assert(NULL != piParent && !piParent->mNumChildren);

	// First find out how many children we'll have
	for (int i = 0; i < (int)bones.size();++i)	{
		if (iParentID != i && bones[i].mParentIndex == iParentID)	{
			++piParent->mNumChildren;
		}
	}
	if (piParent->mNumChildren)	{
		piParent->mChildren = new aiNode*[piParent->mNumChildren];
		for (int i = 0; i < (int)bones.size();++i)	{
			// (avoid infinite recursion)
			if (iParentID != i && bones[i].mParentIndex == iParentID)
			{
				aiNode* pc;
				// setup a new node
				*piParent->mChildren++ = pc = new aiNode();
				pc->mName = aiString(bones[i].mName);
				pc->mParent = piParent;

				// get the corresponding animation channel and its first frame
				const aiNodeAnim** cur = node_anims;
				while ((**cur).mNodeName != pc->mName)++cur;

				aiMatrix4x4::Translation((**cur).mPositionKeys[0].mValue,pc->mTransformation);
				pc->mTransformation = pc->mTransformation * aiMatrix4x4((**cur).mRotationKeys[0].mValue.GetMatrix()) ;

				// add children to this node, too
				AttachChilds_Anim( i, pc, bones,node_anims);
			}
		}
		// undo offset computations
		piParent->mChildren -= piParent->mNumChildren;
	}
}

// ------------------------------------------------------------------------------------------------
// Load a MD5MESH file
void MD5Importer::LoadMD5MeshFile ()
{
	std::string pFile = mFile + "md5mesh";
	boost::scoped_ptr<IOStream> file( pIOHandler->Open( pFile, "rb"));

	// Check whether we can read from the file
	if( file.get() == NULL || !file->FileSize())	{
		DefaultLogger::get()->warn("Failed to access MD5MESH file: " + pFile);
		return;
	}
	bHadMD5Mesh = true;
	LoadFileIntoMemory(file.get());

	// now construct a parser and parse the file
	MD5::MD5Parser parser(mBuffer,fileSize);

	// load the mesh information from it
	MD5::MD5MeshParser meshParser(parser.mSections);

	// create the bone hierarchy - first the root node and dummy nodes for all meshes
	pScene->mRootNode = new aiNode("<MD5_Root>");
	pScene->mRootNode->mNumChildren = 2;
	pScene->mRootNode->mChildren = new aiNode*[2];

	// build the hierarchy from the MD5MESH file
	aiNode* pcNode = pScene->mRootNode->mChildren[1] = new aiNode();
	pcNode->mName.Set("<MD5_Hierarchy>");
	pcNode->mParent = pScene->mRootNode;
	AttachChilds_Mesh(-1,pcNode,meshParser.mJoints);

	pcNode = pScene->mRootNode->mChildren[0] = new aiNode();
	pcNode->mName.Set("<MD5_Mesh>");
	pcNode->mParent = pScene->mRootNode;

#if 0
	if (pScene->mRootNode->mChildren[1]->mNumChildren) /* start at the right hierarchy level */
		SkeletonMeshBuilder skeleton_maker(pScene,pScene->mRootNode->mChildren[1]->mChildren[0]);
#else

	// FIX: MD5 files exported from Blender can have empty meshes
	for (std::vector<MD5::MeshDesc>::const_iterator it  = meshParser.mMeshes.begin(),end = meshParser.mMeshes.end(); it != end;++it) {
		if (!(*it).mFaces.empty() && !(*it).mVertices.empty())
			++pScene->mNumMaterials;
	}

	// generate all meshes
	pScene->mNumMeshes = pScene->mNumMaterials;
	pScene->mMeshes = new aiMesh*[pScene->mNumMeshes];
	pScene->mMaterials = new aiMaterial*[pScene->mNumMeshes];

	//  storage for node mesh indices
	pcNode->mNumMeshes = pScene->mNumMeshes;
	pcNode->mMeshes = new unsigned int[pcNode->mNumMeshes];
	for (unsigned int m = 0; m < pcNode->mNumMeshes;++m)
		pcNode->mMeshes[m] = m;

	unsigned int n = 0;
	for (std::vector<MD5::MeshDesc>::iterator it  = meshParser.mMeshes.begin(),end = meshParser.mMeshes.end(); it != end;++it) {
		MD5::MeshDesc& meshSrc = *it;
		if (meshSrc.mFaces.empty() || meshSrc.mVertices.empty())
			continue;

		aiMesh* mesh = pScene->mMeshes[n] = new aiMesh();
		mesh->mPrimitiveTypes = aiPrimitiveType_TRIANGLE;

		// generate unique vertices in our internal verbose format
		MakeDataUnique(meshSrc);

		mesh->mNumVertices = (unsigned int) meshSrc.mVertices.size();
		mesh->mVertices = new aiVector3D[mesh->mNumVertices];
		mesh->mTextureCoords[0] = new aiVector3D[mesh->mNumVertices];
		mesh->mNumUVComponents[0] = 2;

		// copy texture coordinates
		aiVector3D* pv = mesh->mTextureCoords[0];
		for (MD5::VertexList::const_iterator iter =  meshSrc.mVertices.begin();iter != meshSrc.mVertices.end();++iter,++pv) {
			pv->x = (*iter).mUV.x;
			pv->y = 1.0f-(*iter).mUV.y; // D3D to OpenGL
			pv->z = 0.0f;
		}

		// sort all bone weights - per bone
		unsigned int* piCount = new unsigned int[meshParser.mJoints.size()];
		::memset(piCount,0,sizeof(unsigned int)*meshParser.mJoints.size());

		for (MD5::VertexList::const_iterator iter =  meshSrc.mVertices.begin();iter != meshSrc.mVertices.end();++iter,++pv) {
			for (unsigned int jub = (*iter).mFirstWeight, w = jub; w < jub + (*iter).mNumWeights;++w)
			{
				MD5::WeightDesc& desc = meshSrc.mWeights[w];
				/* FIX for some invalid exporters */
				if (!(desc.mWeight < AI_MD5_WEIGHT_EPSILON && desc.mWeight >= -AI_MD5_WEIGHT_EPSILON ))
					++piCount[desc.mBone];
			}
		}

		// check how many we will need
		for (unsigned int p = 0; p < meshParser.mJoints.size();++p)
			if (piCount[p])mesh->mNumBones++;

		if (mesh->mNumBones) // just for safety
		{
			mesh->mBones = new aiBone*[mesh->mNumBones];
			for (unsigned int q = 0,h = 0; q < meshParser.mJoints.size();++q)
			{
				if (!piCount[q])continue;
				aiBone* p = mesh->mBones[h] = new aiBone();
				p->mNumWeights = piCount[q];
				p->mWeights = new aiVertexWeight[p->mNumWeights];
				p->mName = aiString(meshParser.mJoints[q].mName);
				p->mOffsetMatrix = meshParser.mJoints[q].mInvTransform;

				// store the index for later use
				MD5::BoneDesc& boneSrc = meshParser.mJoints[q];
				boneSrc.mMap = h++;

				// compute w-component of quaternion
				MD5::ConvertQuaternion( boneSrc.mRotationQuat, boneSrc.mRotationQuatConverted );
			}

			//unsigned int g = 0;
			pv = mesh->mVertices;
			for (MD5::VertexList::const_iterator iter =  meshSrc.mVertices.begin();iter != meshSrc.mVertices.end();++iter,++pv) {
				// compute the final vertex position from all single weights
				*pv = aiVector3D();

				// there are models which have weights which don't sum to 1 ...
				float fSum = 0.0f;
				for (unsigned int jub = (*iter).mFirstWeight, w = jub; w < jub + (*iter).mNumWeights;++w)
					fSum += meshSrc.mWeights[w].mWeight;
				if (!fSum) {
					DefaultLogger::get()->error("MD5MESH: The sum of all vertex bone weights is 0");
					continue;
				}

				// process bone weights
				for (unsigned int jub = (*iter).mFirstWeight, w = jub; w < jub + (*iter).mNumWeights;++w)	{
					if (w >= meshSrc.mWeights.size())
						throw DeadlyImportError("MD5MESH: Invalid weight index");

					MD5::WeightDesc& desc = meshSrc.mWeights[w];
					if ( desc.mWeight < AI_MD5_WEIGHT_EPSILON && desc.mWeight >= -AI_MD5_WEIGHT_EPSILON) {
						continue;
					}

					const float fNewWeight = desc.mWeight / fSum;

					// transform the local position into worldspace
					MD5::BoneDesc& boneSrc = meshParser.mJoints[desc.mBone];
					const aiVector3D v = boneSrc.mRotationQuatConverted.Rotate (desc.vOffsetPosition);

					// use the original weight to compute the vertex position
					// (some MD5s seem to depend on the invalid weight values ...)
					*pv += ((boneSrc.mPositionXYZ+v)* desc.mWeight);

					aiBone* bone = mesh->mBones[boneSrc.mMap];
					*bone->mWeights++ = aiVertexWeight((unsigned int)(pv-mesh->mVertices),fNewWeight);
				}
			}

			// undo our nice offset tricks ...
			for (unsigned int p = 0; p < mesh->mNumBones;++p) {
				mesh->mBones[p]->mWeights -= mesh->mBones[p]->mNumWeights;
			}
		}

		delete[] piCount;

		// now setup all faces - we can directly copy the list
		// (however, take care that the aiFace destructor doesn't delete the mIndices array)
		mesh->mNumFaces = (unsigned int)meshSrc.mFaces.size();
		mesh->mFaces = new aiFace[mesh->mNumFaces];
		for (unsigned int c = 0; c < mesh->mNumFaces;++c)	{
			mesh->mFaces[c].mNumIndices = 3;
			mesh->mFaces[c].mIndices = meshSrc.mFaces[c].mIndices;
			meshSrc.mFaces[c].mIndices = NULL;
		}

		// generate a material for the mesh
		aiMaterial* mat = new aiMaterial();
		pScene->mMaterials[n] = mat;

		// insert the typical doom3 textures:
		// nnn_local.tga  - normal map
		// nnn_h.tga      - height map
		// nnn_s.tga      - specular map
		// nnn_d.tga      - diffuse map
		if (meshSrc.mShader.length && !strchr(meshSrc.mShader.data,'.')) {

			aiString temp(meshSrc.mShader);
			temp.Append("_local.tga");
			mat->AddProperty(&temp,AI_MATKEY_TEXTURE_NORMALS(0));

			temp =  aiString(meshSrc.mShader);
			temp.Append("_s.tga");
			mat->AddProperty(&temp,AI_MATKEY_TEXTURE_SPECULAR(0));

			temp =  aiString(meshSrc.mShader);
			temp.Append("_d.tga");
			mat->AddProperty(&temp,AI_MATKEY_TEXTURE_DIFFUSE(0));

			temp =  aiString(meshSrc.mShader);
			temp.Append("_h.tga");
			mat->AddProperty(&temp,AI_MATKEY_TEXTURE_HEIGHT(0));

			// set this also as material name
			mat->AddProperty(&meshSrc.mShader,AI_MATKEY_NAME);
		}
		else mat->AddProperty(&meshSrc.mShader,AI_MATKEY_TEXTURE_DIFFUSE(0));
		mesh->mMaterialIndex = n++;
	}
#endif
}

// ------------------------------------------------------------------------------------------------
// Load an MD5ANIM file
void MD5Importer::LoadMD5AnimFile ()
{
	std::string pFile = mFile + "md5anim";
	boost::scoped_ptr<IOStream> file( pIOHandler->Open( pFile, "rb"));

	// Check whether we can read from the file
	if( !file.get() || !file->FileSize())	{
		DefaultLogger::get()->warn("Failed to read MD5ANIM file: " + pFile);
		return;
	}
	LoadFileIntoMemory(file.get());

	// parse the basic file structure
	MD5::MD5Parser parser(mBuffer,fileSize);

	// load the animation information from the parse tree
	MD5::MD5AnimParser animParser(parser.mSections);

	// generate and fill the output animation
	if (animParser.mAnimatedBones.empty() || animParser.mFrames.empty() ||
		animParser.mBaseFrames.size() != animParser.mAnimatedBones.size())	{

		DefaultLogger::get()->error("MD5ANIM: No frames or animated bones loaded");
	}
	else {
		bHadMD5Anim = true;

		pScene->mAnimations = new aiAnimation*[pScene->mNumAnimations = 1];
		aiAnimation* anim = pScene->mAnimations[0] = new aiAnimation();
		anim->mNumChannels = (unsigned int)animParser.mAnimatedBones.size();
		anim->mChannels = new aiNodeAnim*[anim->mNumChannels];
		for (unsigned int i = 0; i < anim->mNumChannels;++i)	{
			aiNodeAnim* node = anim->mChannels[i] = new aiNodeAnim();
			node->mNodeName = aiString( animParser.mAnimatedBones[i].mName );

			// allocate storage for the keyframes
			node->mPositionKeys = new aiVectorKey[animParser.mFrames.size()];
			node->mRotationKeys = new aiQuatKey[animParser.mFrames.size()];
		}

		// 1 tick == 1 frame
		anim->mTicksPerSecond = animParser.fFrameRate;

		for (FrameList::const_iterator iter = animParser.mFrames.begin(), iterEnd = animParser.mFrames.end();iter != iterEnd;++iter){
			double dTime = (double)(*iter).iIndex;
			aiNodeAnim** pcAnimNode = anim->mChannels;
			if (!(*iter).mValues.empty() || iter == animParser.mFrames.begin()) /* be sure we have at least one frame */
			{
				// now process all values in there ... read all joints
				MD5::BaseFrameDesc* pcBaseFrame = &animParser.mBaseFrames[0];
				for (AnimBoneList::const_iterator iter2	= animParser.mAnimatedBones.begin(); iter2 != animParser.mAnimatedBones.end();++iter2,
					++pcAnimNode,++pcBaseFrame)
				{
					if((*iter2).iFirstKeyIndex >= (*iter).mValues.size()) {

						// Allow for empty frames
						if ((*iter2).iFlags != 0) {
							throw DeadlyImportError("MD5: Keyframe index is out of range");

						}
						continue;
					}
					const float* fpCur = &(*iter).mValues[(*iter2).iFirstKeyIndex];
					aiNodeAnim* pcCurAnimBone = *pcAnimNode;

					aiVectorKey* vKey = &pcCurAnimBone->mPositionKeys[pcCurAnimBone->mNumPositionKeys++];
					aiQuatKey* qKey = &pcCurAnimBone->mRotationKeys  [pcCurAnimBone->mNumRotationKeys++];
					aiVector3D vTemp;

					// translational component
					for (unsigned int i = 0; i < 3; ++i) {
						if ((*iter2).iFlags & (1u << i)) {
							vKey->mValue[i] =  *fpCur++;
						}
						else vKey->mValue[i] = pcBaseFrame->vPositionXYZ[i];
					}

					// orientation component
					for (unsigned int i = 0; i < 3; ++i) {
						if ((*iter2).iFlags & (8u << i)) {
							vTemp[i] =  *fpCur++;
						}
						else vTemp[i] = pcBaseFrame->vRotationQuat[i];
					}

					MD5::ConvertQuaternion(vTemp, qKey->mValue);
					qKey->mTime = vKey->mTime = dTime;

					// we need this to get to Assimp quaternion conventions
					qKey->mValue.w *= -1.f;
				}
			}

			// compute the duration of the animation
			anim->mDuration = std::max(dTime,anim->mDuration);
		}

		// If we didn't build the hierarchy yet (== we didn't load a MD5MESH),
		// construct it now from the data given in the MD5ANIM.
		if (!pScene->mRootNode) {
			pScene->mRootNode = new aiNode();
			pScene->mRootNode->mName.Set("<MD5_Hierarchy>");

			AttachChilds_Anim(-1,pScene->mRootNode,animParser.mAnimatedBones,(const aiNodeAnim**)anim->mChannels);

			// Call SkeletonMeshBuilder to construct a mesh to represent the shape
			if (pScene->mRootNode->mNumChildren) {
				SkeletonMeshBuilder skeleton_maker(pScene,pScene->mRootNode->mChildren[0]);
			}
		}
	}
}

// ------------------------------------------------------------------------------------------------
// Load an MD5CAMERA file
void MD5Importer::LoadMD5CameraFile ()
{
	std::string pFile = mFile + "md5camera";
	boost::scoped_ptr<IOStream> file( pIOHandler->Open( pFile, "rb"));

	// Check whether we can read from the file
	if( !file.get() || !file->FileSize())	{
		throw DeadlyImportError("Failed to read MD5CAMERA file: " + pFile);
	}
	bHadMD5Camera = true;
	LoadFileIntoMemory(file.get());

	// parse the basic file structure
	MD5::MD5Parser parser(mBuffer,fileSize);

	// load the camera animation data from the parse tree
	MD5::MD5CameraParser cameraParser(parser.mSections);

	if (cameraParser.frames.empty()) {
		throw DeadlyImportError("MD5CAMERA: No frames parsed");
	}

	std::vector<unsigned int>& cuts = cameraParser.cuts;
	std::vector<MD5::CameraAnimFrameDesc>& frames = cameraParser.frames;

	// Construct output graph - a simple root with a dummy child.
	// The root node performs the coordinate system conversion
	aiNode* root = pScene->mRootNode = new aiNode("<MD5CameraRoot>");
	root->mChildren = new aiNode*[root->mNumChildren = 1];
	root->mChildren[0] = new aiNode("<MD5Camera>");
	root->mChildren[0]->mParent = root;

	// ... but with one camera assigned to it
	pScene->mCameras = new aiCamera*[pScene->mNumCameras = 1];
	aiCamera* cam = pScene->mCameras[0] = new aiCamera();
	cam->mName = "<MD5Camera>";

	// FIXME: Fov is currently set to the first frame's value
	cam->mHorizontalFOV = AI_DEG_TO_RAD( frames.front().fFOV );

	// every cut is written to a separate aiAnimation
	if (!cuts.size()) {
		cuts.push_back(0);
		cuts.push_back(frames.size()-1);
	}
	else {
		cuts.insert(cuts.begin(),0);

		if (cuts.back() < frames.size()-1)
			cuts.push_back(frames.size()-1);
	}

	pScene->mNumAnimations = cuts.size()-1;
	aiAnimation** tmp = pScene->mAnimations = new aiAnimation*[pScene->mNumAnimations];
	for (std::vector<unsigned int>::const_iterator it = cuts.begin(); it != cuts.end()-1; ++it) {

		aiAnimation* anim = *tmp++ = new aiAnimation();
		anim->mName.length = ::sprintf(anim->mName.data,"anim%u_from_%u_to_%u",(unsigned int)(it-cuts.begin()),(*it),*(it+1));

		anim->mTicksPerSecond = cameraParser.fFrameRate;
		anim->mChannels = new aiNodeAnim*[anim->mNumChannels = 1];
		aiNodeAnim* nd  = anim->mChannels[0] = new aiNodeAnim();
		nd->mNodeName.Set("<MD5Camera>");

		nd->mNumPositionKeys = nd->mNumRotationKeys = *(it+1) - (*it);
		nd->mPositionKeys = new aiVectorKey[nd->mNumPositionKeys];
		nd->mRotationKeys = new aiQuatKey  [nd->mNumRotationKeys];
		for (unsigned int i = 0; i < nd->mNumPositionKeys; ++i) {

			nd->mPositionKeys[i].mValue = frames[*it+i].vPositionXYZ;
			MD5::ConvertQuaternion(frames[*it+i].vRotationQuat,nd->mRotationKeys[i].mValue);
			nd->mRotationKeys[i].mTime = nd->mPositionKeys[i].mTime = *it+i;
		}
	}
}

#endif // !! ASSIMP_BUILD_NO_MD5_IMPORTER


text:
/**
 * Copyright (c) 2020 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef pysupport_profiler_po_hpp_included_
#define pysupport_profiler_po_hpp_included_

#include <boost/program_options.hpp>

#include "utility/streams.hpp"

#include "profiler.hpp"

namespace pysupport {

inline void configuration(ProfilerOptions &options
                          , boost::program_options::options_description &od)
{
    namespace po = boost::program_options;

    od.add_options()
        ("profile"
         , po::value<ProfilerModule>()
         ->implicit_value(ProfilerModule::cProfile)
         , utility::concat
         ("Profiler to use. Profiler is inactive if unset (default); one of "
          , enumerationString(ProfilerModule())
          , ".").c_str())

        ("profile.builtins"
         , po::value(&options.builtins)->default_value(true)
         , "Profile built-in functions.")

        ("profile.timeunit"
         , po::value(&options.timeunit)->default_value(options.timeunit)
         , "Time multipler. Defaults to no multiplier.")

        ("profile.stats.print"
         , po::value(&options.printStats)->default_value(true)
         , "Print profiling statistics to output.")

        ("profile.stats.print.sort"
         , po::value<ProfilerSort>()
         , utility::concat
         ("Sorting order of printer profiling statistics; one of "
          , enumerationString(ProfilerSort())
          , ".").c_str())

        ("profile.stats.save"
         , po::value<boost::filesystem::path>()
         , "Write profiling statistics to given file.")

        ("profile.hook.multiprocessing"
         , po::value(&options.hookMultiprocessing)
         ->default_value(options.hookMultiprocessing)
         , "Add hook to multiprocessing.Process machinery.")
        ;
}

inline void configure(ProfilerOptions &options
                      , const boost::program_options::variables_map &vars)
{
    namespace po = boost::program_options;

    if (!vars.count("profile")) { return; }

    options.module = vars["profile"].as<ProfilerModule>();

    if (vars.count("profile.stats.print.sort")) {
        options.printStatsSort = vars["profile.stats.print.sort"]
            .as<ProfilerSort>();
    }

    if (vars.count("profile.stats.save")) {
        options.saveStats
            = vars["profile.stats.save"].as<boost::filesystem::path>();
    }
}

} // namespace pysupport

#endif // pysupport_profiler_po_hpp_included_

ext: hpp

max_stars_repo_name: Melown/libpysupport

max_issues_repo_name: Melown/libpysupport

max_forks_repo_name: Melown/libpysupport


text:
/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#define BOOST_TEST_MAIN

#include <string>

#include <memory>
#include <boost/test/unit_test.hpp>

#include "Tudat/InputOutput/fieldValue.h"
#include "Tudat/InputOutput/fieldTransform.h"

namespace tudat
{
namespace unit_tests
{

using namespace input_output;

//! Test transform struct used to test FieldTransform.
struct TestTransform : public input_output::FieldTransform
{
    static const std::shared_ptr< std::string > result;
    std::shared_ptr< std::string > transform( const std::string& input ) { return result; }
};

//! Set result of transform in TestTransform to a string.
const std::shared_ptr< std::string > TestTransform::result( new std::string( "Transformed!") );

// Define Boost test suite.
BOOST_AUTO_TEST_SUITE( test_field_value )

//! Test that the raw data is returned.
BOOST_AUTO_TEST_CASE( testFieldValueGetRawFunction )
{
    // Create raw string data.
    const std::string testString = "raw";

    // Create a test transformer.
    std::shared_ptr< TestTransform > transform( new TestTransform( ) );

    // Create a field value object.
    FieldValue testFieldValue( field_types::state::inclination, testString, transform );

    // Retrieve raw data.
    const std::string returnedData = testFieldValue.getRaw( );

    // Verify that returned data is correct.
    BOOST_CHECK_EQUAL( returnedData, testString );
}

//! Test that the raw transformed data is returned.
BOOST_AUTO_TEST_CASE( testFieldValueGetTransformedFunction )
{
    // Create raw string data.
    const std::string testString = "raw";

    // Create a test transformer.
    std::shared_ptr< TestTransform > transform( new TestTransform( ) );

    // Create a field value object.
    FieldValue testFieldValue( field_types::state::inclination, testString, transform );

    // Retrieve raw data.
    const std::string returnedData = testFieldValue.getTransformed( );

    // Verify that returned data is correct.
    BOOST_CHECK_EQUAL( returnedData, *TestTransform::result );
}

//! Test that the transformed data is returned from get.
BOOST_AUTO_TEST_CASE( testFieldValueGetFunction )
{
    // Create raw string data.
    const std::string testString = "raw";

    // Create a test transformer.
    std::shared_ptr< TestTransform > transform( new TestTransform( ) );

    // Test 1: Test the get function with a transform.
    // Create a field value object with transform.
    FieldValue testFieldValue( field_types::state::inclination, testString, transform );

    // Retrieve (transformed) data.
    const std::string returnedData = testFieldValue.get< std::string >( );

    // Expected data.
    const std::string expectedData = *TestTransform::result;

    // Verify that returned data is correct.
    BOOST_CHECK_EQUAL( returnedData, expectedData );

    // Test 2: Test the get function without transform.
    // Create a field value object.
    FieldValue testFieldValue2( field_types::state::inclination, testString );

    // Retrieve (raw) data.
    const std::string returnedRawData = testFieldValue2.get< std::string >( );

    // Verify that returned data is correct.
    BOOST_CHECK_EQUAL( returnedRawData, testString );
}

//! Test that the transformed data is returned from getPointer.
BOOST_AUTO_TEST_CASE( testFieldValueGetPointerFunction )
{
    // Create raw string data.
    const std::string testString = "raw";

    // Create a test transformer.
    std::shared_ptr< TestTransform > transform( new TestTransform( ) );

    // Test 1: Test the get function with a transform.
    // Create a field value object with transform.
    FieldValue testFieldValue( field_types::state::inclination, testString, transform );

    // Retrieve (transformed) data.
    std::shared_ptr< std::string > returnedData = testFieldValue.getPointer< std::string >( );

    // Expected data.
    const std::string expectedData = *TestTransform::result;

    // Verify that returned data is correct.
    BOOST_CHECK_EQUAL( *returnedData, expectedData );

    // Test 2: Test the get function without transform.
    // Create a field value object.
    FieldValue testFieldValue2( field_types::state::inclination, testString );

    // Retrieve (raw) data.
    std::shared_ptr< std::string > returnedRawData
            = testFieldValue2.getPointer< std::string >( );

    // Verify that returned data is correct.
    BOOST_CHECK_EQUAL( *returnedRawData, testString );
}

//! Test that the field type is stored correctly.
BOOST_AUTO_TEST_CASE( testFieldValueType )
{
    // Inserted and expected resulting field type.
    FieldType testType = field_types::general::name;

    // Test constructor without transform.
    FieldValue testFieldValue1( testType, "foo" );
    BOOST_CHECK_EQUAL( testType, testFieldValue1.type );

    // Test constructor with transform.
    FieldValue testFieldValue2( testType, "foo", std::make_shared< TestTransform >(  ) );
    BOOST_CHECK_EQUAL( testType, testFieldValue2.type );
}

// Close Boost test suite.
BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat

ext: cpp

max_stars_repo_name: J-Westin/tudat

max_issues_repo_name: J-Westin/tudat

max_forks_repo_name: J-Westin/tudat


text:
#include <boost/program_options.hpp>
#define DOCTEST_CONFIG_IMPLEMENT
#include <doctest/doctest.h>
#include "emulator.h"

namespace po = boost::program_options;

int main(int argc, const char** argv) {
  doctest::Context context;

  context.setOption("abort-after", 5);
  context.applyCommandLine(argc, argv);

  context.setOption("no-breaks", true);

  int res = context.run();

  if (context.shouldExit()) {
    return res;
  }

  po::options_description option_desc{"Options"};

  option_desc.add_options()(
      "trace", po::value<bool>()->default_value(false)->implicit_value(true),
      "enables CPU tracing");
  option_desc.add_options()(
      "no-save", po::value<bool>()->default_value(false)->implicit_value(true),
      "disables save RAM files");

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, option_desc), vm);

  po::notify(vm);
  const bool trace = vm["trace"].as<bool>();
  const bool save = vm["no-save"].as<bool>();
  std::string rom_name = argv[1];
  gb::run_with_options(rom_name, trace, !save);

  return 0;
}

max_stars_repo_name: jack-karamanian/gbemu

max_issues_repo_name: jack-karamanian/gbemu

max_forks_repo_name: jack-karamanian/gbemu


text:
#define _CRT_SECURE_NO_DEPRECATE
#define HEAD_INFO

#include "mylib.h"
#include <chrono>
#include <iostream>
#include <fstream>
#include <map>
#include <stdlib.h>
#include <set>
#include <string>
#include <time.h>
#include <stdlib.h>
#include <vector>
#include <algorithm>
#include "graph.h"
#include "config.h"
#include "algo.h"
#include "query.h"
#include "build_oracle.h"

#include <boost/progress.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

#include <chrono>

GlobalPR *gpr;

using namespace std::chrono;

using namespace boost;
using namespace boost::property_tree;

using namespace std;

string get_time_path() {
    using namespace boost::posix_time;
    auto tm = second_clock::local_time();
#ifdef WIN32
    return  "../../execution/" + to_iso_string(tm);
#else
    return parent_folder+FILESEP+"execution/" + to_iso_string(tm);
#endif
}

#include <boost/program_options.hpp>

namespace po = boost::program_options;

// vector<vector<int> > fwd_idx_uncompressed;
// vector<vector<unordered_map<int,int> > > fwd_idx;

//continuous memory,
//uncompressed version: e.g., dest-id,dest-id,dest-id...;
//compressed_version: e.g., hub-id,occur,hub-id,occur...
vector<int> fwd_idx;

//pointers to uncompressed fwd_idx, nodeid:start-pointer
map<int, pair<int64, int> > fwd_idx_ucp_pointers;
//pointers to compressed fwd_idx, nodeid:{ start-pointer, start-pointer, start-pointer,...,end-pointer }
map<int, vector<int64> > fwd_idx_cp_pointers;
vector<vector<int64>> fwd_idx_ptrs;

// vector<int> fwd_idx_size;
vector<int> fwd_idx_size;
iMap<int> fwd_idx_size_k;

// vector<int> forward_node_order;
iMap<int> statistic_hit_number;

iMap<int> hub_used_samples;
vector<iMap<int>> multi_hub_used_samples;
iMap<int> dest_nodes;
vector<iMap<int>> multi_dest_nodes;

vector<unsigned> global_seeds;
vector<int> sample_number;
iMap<double> bwd_residuals;
iMap<double> bwd_reserves;
iMap<int> component;

using namespace std;

int main(int argc, char *argv[]) {
    ios::sync_with_stdio(false);
    program_start(argc, argv);

    // this is main entry
    Saver::init();
    config.graph_alias = "nethept";
    for (int i = 0; i < argc; i++) {
        string help_str = ""
                "hubppr build-oracle --algo <algo> [options]\n"
                "hubppr query --algo <algo> [options]\n"
                "hubppr topk --algo <algo> [options]\n"
                "hubppr select-forward-hub --algo <algo> [options]\n"
                "hubppr select-backward-hub --algo <algo> [options]\n"
                "hubppr generate-topk-query --algo <algo> [options]\n"
                "hubppr generate-p2p-query --algo <algo> [options]\n\n"
                "hubppr\n"
                "\n"
                "algo: \n"
                "  bippr\n"
                "  fastppr\n"
                "  montecarlo\n"
                "  hubppr\n"
                "options: \n"
                "  --target_size <target-size>\n"
                "  --k_size <k-size>\n"
                "  --compress_fwd\n"
                "  --prefix <prefix>\n"
                "  --dataset <dataset>\n"
                "   --query_size\n"
                "  --space_consumption <space>                    The space it uses\n"
                "  --target_sample <uniform|global-pagerank>      target sample\n"
                "  --force_rebuild                                force rebuild the oracle\n";
        if (string(argv[i]) == "--help") {
            cout << help_str << endl;
            exit(0);
        }
    }

    config.action = argv[1];
    cout << "action: " << config.action << endl;

    // init graph first
    for (int i = 0; i < argc; i++) {
        if (string(argv[i]) == "--prefix") {
            config.prefix = argv[i + 1];
        }
        if (string(argv[i]) == "--dataset") {
            config.graph_alias = argv[i + 1];
        }
    }

    config.graph_location = config.get_graph_folder();

    bool load_order = true;

    if(config.action == "select-forward-hub" || config.action == "generate-p2p-query")
        load_order = false;

    Graph graph(config.graph_location, load_order);
    INFO("load graph finish");

    INFO(graph.n, graph.m);
    for (int i = 0; i < argc; i++) {
        string arg = argv[i];
        if (arg == "--algo") {
    		config.algo = string(argv[i + 1]);
        }
        else if (arg == "--target_sample") {
            config.target_sample = string(argv[i + 1]);
            INFO(config.target_sample);
            assert(config.target_sample == UNIFORM || config.target_sample == GLOBAL_PAGERANK || config.target_sample == RANDOM_WALK);
        }
        else if (arg == "--epsilon") {
            config.epsilon = atof(argv[i + 1]);
            INFO(config.epsilon);
        }
        else if (arg == "--space_consumption") {
            config.space_consumption = atof(argv[i + 1]);
            INFO(config.space_consumption);
        }
        else if (arg == "--target_size") {
            config.target_size = atoi(argv[i + 1]);
        }
        else if (arg == "--k_size"){
            config.k_size = atoi(argv[i+1]);
        }
        else if (arg == "--query_size"){
            config.query_size = atoi(argv[i+1]);
        }
        else if (arg == "--force_rebuild") {
            config.force_rebuild = true;
        }
        else if (arg == "--prefix" || arg == "--dataset") {
        }
        else if (arg.substr(0, 2) == "--") {
            cerr << "command not recognize " << arg << endl;
            exit(1);
        }
    }

    vector<string> possibleAlgo = {"bippr", "hubppr", "bipproracle",  "montecarlo", "fastppr"};
    auto f = find(possibleAlgo.begin(), possibleAlgo.end(), config.algo);
    assert (f != possibleAlgo.end());

    INFO(config.action);
    if( config.algo == "hubppr" && (config.action == "build-oracle" || config.action == "query")){
        gpr = new GlobalPR(config, graph);
    }

    init_parameter(config, graph);

    INFO("finished initing parameters");

    if (config.action == BUILD_ORACLE) {
        config.multithread = true;
        config.compress_fwd = true;
        hub_used_samples.initialize(graph.n);
        dest_nodes.initialize(graph.n);
        bwd_residuals.initialize(graph.n);
        bwd_reserves.initialize(graph.n);
        component.initialize(graph.n);
        tune_forward_backward_ratio(graph);
        build_oracle_entry(graph);
    }
    else if (config.action == QUERY) {
        config.compress_fwd = true;
        hub_used_samples.initialize(graph.n);
        dest_nodes.initialize(graph.n);
        bwd_residuals.initialize(graph.n);
        bwd_reserves.initialize(graph.n);
        component.initialize(graph.n);
        query(graph);
    }
    else if (config.action == TOPK) {
        config.compress_fwd = true;
        hub_used_samples.initialize(graph.n);
        dest_nodes.initialize(graph.n);
        bwd_residuals.initialize(graph.n);
        bwd_reserves.initialize(graph.n);
        component.initialize(graph.n);
        topk(graph);
    }
    else if (config.action == GEN_TOPK_QUERY){
        generate_topk_query(graph);
    }
    else if (config.action == GEN_P2P_QUERY){
        generate_p2p_query(graph);
    }
    else if (config.action == SELECT_FORWARD_HUB){
        select_forward_hub(graph);
    }
    else if (config.action == SELECT_BACKWARD_HUB){
        bippr_setting();
        bwd_residuals.initialize(graph.n);
        bwd_reserves.initialize(graph.n);
        select_backward_hub(graph);
    }
    else {
        cerr << "sub command not regoznized" << endl;
        exit(1);
    }

    auto args = combine_args(argc, argv);
    Saver::save_json(config, result, args);
    program_stop();
    return 0;
}

max_stars_repo_name: RapidsAtHKUST/SimRank

max_issues_repo_name: RapidsAtHKUST/SimRank

max_forks_repo_name: RapidsAtHKUST/SimRank


text:
// Copyright 2020 Peter Dimov.
// Distributed under the Boost Software License, Version 1.0.
// https://www.boost.org/LICENSE_1_0.txt

#if defined(_MSC_VER) && _MSC_VER < 1910
# pragma warning(disable: 4503) // decorated name length exceeded
#endif

#include <boost/variant2/variant.hpp>
#include <boost/core/lightweight_test_trait.hpp>
#include <boost/config.hpp>
#include <boost/config/workaround.hpp>

#include <boost/mp11.hpp>
using namespace boost::mp11;

//

using namespace boost::variant2;

struct D
{
    ~D() noexcept {}
};

struct CC1
{
    CC1( CC1 const& ) {}
};

struct CC2
{
    CC2( CC2 const& ) = delete;
};

struct MC1
{
    MC1( MC1 && ) {}
};

struct MC2
{
    MC2( MC2 && ) = delete;
};

struct CA1
{
    CA1& operator=( CA1 const& ) { return *this; }
};

struct CA2
{
    CA2& operator=( CA2 const& ) = delete;
};

struct MA1
{
    MA1& operator=( MA1 && ) { return *this; }
};

struct MA2
{
    MA2& operator=( MA2 && ) = delete;
};

struct test
{
    template<class... T> void operator()( mp_list<T...> ) const noexcept
    {
        using U = mp_inherit<T...>;

#if !BOOST_WORKAROUND( __GNUC__, < 5 )

        BOOST_TEST_EQ( std::is_copy_constructible<variant<U>>::value, std::is_copy_constructible<U>::value );
        BOOST_TEST_EQ( std::is_nothrow_copy_constructible<variant<U>>::value, std::is_nothrow_copy_constructible<U>::value );

#endif

#if !BOOST_WORKAROUND(BOOST_MSVC, < 1910)

        BOOST_TEST_EQ( std::is_move_constructible<variant<U>>::value, std::is_move_constructible<U>::value );

#else

        BOOST_TEST_GE( std::is_move_constructible<variant<U>>::value, std::is_move_constructible<U>::value );

#endif

        BOOST_TEST_EQ( std::is_nothrow_move_constructible<variant<U>>::value, std::is_nothrow_move_constructible<U>::value );

        BOOST_TEST_EQ( std::is_copy_assignable<variant<U>>::value, std::is_copy_constructible<U>::value && std::is_copy_assignable<U>::value );
        BOOST_TEST_EQ( std::is_nothrow_copy_assignable<variant<U>>::value, std::is_nothrow_copy_constructible<U>::value && std::is_copy_assignable<U>::value );

        BOOST_TEST_EQ( std::is_move_assignable<variant<U>>::value, std::is_move_constructible<U>::value && std::is_move_assignable<U>::value );
        BOOST_TEST_EQ( std::is_nothrow_move_assignable<variant<U>>::value, std::is_nothrow_move_constructible<U>::value && std::is_move_assignable<U>::value );
    }
};

int main()
{
    mp_for_each< mp_power_set< mp_list<D, CC1, CC2, MC1, MC2, CA1, CA2, MA1, MA2> > >( test() );
    return boost::report_errors();
}

max_issues_repo_name: qingkouwei/mediaones


text:
/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
 * All rights reserved.
 *
 * See LICENSE file in the root of the mrob library.
 *
 *
 * planeRegistration.cpp
 *
 *  Created on: Jan 28, 2019
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech
 */

#include "mrob/pc_registration.hpp"
#include "mrob/plane_registration.hpp"
#include <Eigen/LU> // for inverse and determinant
#include <Eigen/Eigenvalues>
#include <iostream>

#include <chrono>

using namespace mrob;

PlaneRegistration::PlaneRegistration():
        numberPlanes_(0), numberPoses_(0),isSolved_(0), trajectory_(new std::vector<SE3>(8,SE3())),
        solveMode_(SolveMode::GRADIENT),
        c1_(1e-4), c2_(0.9), alpha_(0.75), beta_(0.1)
{
    // Optmizer does not establish the size for this matrices and thus it is required
    gradient_.resize(6);
    hessian_.resize(6,6);
}

PlaneRegistration::~PlaneRegistration()
{

}

void PlaneRegistration::set_number_planes_and_poses(uint_t numberPlanes, uint_t numberPoses)
{
    planes_.clear();
    planes_.reserve(numberPlanes);
    trajectory_->clear();
    trajectory_->resize(numberPoses, SE3());
    numberPlanes_ = numberPlanes;
    numberPoses_ = numberPoses;

    previousState_.clear();
    previousState_.resize(numberPoses, Mat61::Zero());
}

void PlaneRegistration::reset_solution()
{
    // trajectory is reset
    trajectory_->clear();
    trajectory_->resize(numberPoses_, SE3());
    previousState_.clear();
    previousState_.resize(numberPoses_, Mat61::Zero());
}

uint_t PlaneRegistration::solve(SolveMode mode, bool singleIteration)
{
    // just in case some methods, such as gradient, has several modes
    solveMode_ = mode;
    switch(mode)
    {
        case SolveMode::INITIALIZE:
            return solve_initialize();
        case SolveMode::GRADIENT:
        case SolveMode::GRADIENT_BENGIOS_NAG:
            return solve_interpolate_gradient(singleIteration);
        case SolveMode::GN_HESSIAN:
            return optimize(NEWTON_RAPHSON);
        case SolveMode::GN_CLAMPED_HESSIAN:
            return solve_interpolate_hessian(singleIteration);
        case SolveMode::LM_SPHER:
            return optimize(LEVENBERG_MARQUARDT_SPHER);
        case SolveMode::LM_ELLIP:
            return optimize(LEVENBERG_MARQUARDT_ELLIP);
        default:
            return 0;
    }
}

uint_t PlaneRegistration::solve_interpolate_gradient(bool singleIteration)
{
    // This function is from the first implementation, is just kept for comparisons (but should not be used to solve the problem)
    // iterative process, on convergence basis | error_k - error_k-1| < tol
    solveIters_ = 0;
    double previousError = 1e20, diffError = 10;
    do
    {
        // 1) calculate plane estimation given the current trajectory
        double  initialError = 0.0;
        for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
        {
            initialError += it->second->estimate_plane();
        }
        diffError = previousError - initialError;
        previousError = initialError;

        std::cout << "current error iteration " << solveIters_ << " = "<< initialError << std::endl;

        // 2) calculate Gradient = Jacobian^T. We maintain the nomenclature Jacobian for coherence on the project,
        //    but actually this Jacobian should be transposed.
        Mat61 jacobian = Mat61::Zero(), accumulatedJacobian = Mat61::Zero();
        double  numberPoints, tau = 1.0 / (double)(numberPoses_-1);
        for (uint_t t = 1 ; t < numberPoses_; ++t)
        {
            numberPoints = 0.0;
            jacobian.setZero();
            for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
            {
                jacobian += it->second->calculate_gradient(t);
                numberPoints += it->second->get_number_points(t);
            }
            // XXX this could be changed to time stamps later
            accumulatedJacobian +=  (tau *  t  / numberPoints / numberPoses_) * jacobian;

        }
        // 3) update results Tf = exp(-dxi) * Tf (our convention, we expanded from the left)
        // 3-1)
        Mat61 dxi, xiFinal;
        if (solveMode_ == SolveMode::GRADIENT)
        {
            double alpha = alpha_;
            dxi = -alpha * accumulatedJacobian;
            //std::cout << "\nINterpolate jacobian : = " << accumulatedJacobian.transpose() << ", and increment update = " << dxi << std::endl;
        }
        // 3.4-B) Bengio's NAG: a modification to NAG as proposed in Bengio-2013. Fixed parameters
        //          1) momentum or velocity  v_k = beta_k-1 v _k-1 - alpha_k-1 Grad f (x_k-1)
        //          2) x_k+1 = x_k + beta_k+1 beta_k * v_k - (1 + beta_k+1)*alpha_k * Grad f(x_k)
        if (solveMode_ == SolveMode::GRADIENT_BENGIOS_NAG)
        {
            double alpha = alpha_;
            double beta = beta_;
            // x update
            dxi = beta * beta * previousState_.back() - (1 + beta) * alpha * accumulatedJacobian;

            // momentum
            previousState_.back() = beta * previousState_.back() - alpha * accumulatedJacobian;
        }
        trajectory_->back().update_lhs(dxi);
        xiFinal = trajectory_->back().ln_vee();
        for (uint_t t = 1 ; t < numberPoses_-1; ++t)
        {
            dxi = tau * t * xiFinal;// SE3 does not like all derived classes TODO
            trajectory_->at(t) = SE3(dxi);
        }
        ++solveIters_;
    }while(fabs(diffError) > 1e-4 && !singleIteration && solveIters_ < 1e4);
    return solveIters_;
}

// TO BE DEPRECATED. Only used for clamped Hessian, and that is shown NOT to work.
uint_t PlaneRegistration::solve_interpolate_hessian(bool singleIteration)
{
    // iterative process, on convergence basis | error_k - error_k-1| < tol
    // For now, only 1 iteration
    solveIters_ = 0;
    double previousError = 1e20, diffError = 10;

    do
    {

        // 1) calculate plane estimation given the current trajectory. Same as solve_interpolate
        double  initialError = get_current_error();

        diffError = previousError - initialError;
        previousError = initialError;

        solveIters_++;
        std::cout << "current error iteration " << solveIters_ << " = "<< initialError << std::endl;

        // 2) calculate Gradient and Hessian
        Mat61 gradient = Mat61::Zero();
        Mat6 hessian = Mat6::Zero();
        gradient__.setZero();
        hessian__.setZero();
        double  tau = 1.0 / (double)(numberPoses_-1);
        for (uint_t t = 1 ; t < numberPoses_; ++t)
        {
            gradient.setZero();
            hessian.setZero();
            for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
            {
                gradient += it->second->calculate_gradient(t);
                hessian += it->second->calculate_hessian(t);
            }
            // TODO this should be changed to time stamps later
            gradient__ +=  (tau *  t)  * gradient;
            hessian__ += (tau *  t) * hessian.selfadjointView<Eigen::Upper>();
        }
        // 3) calculate update Tf = exp(-dxi) * Tf (our convention, we expanded from the left)
        Mat61 dxi;
        if (solveMode_ == SolveMode::GN_CLAMPED_HESSIAN)
        {
            // we clamp the vector spaces corresponding to negative eigenvals
            Mat6 pseudoInv = Mat6::Zero();
            Eigen::SelfAdjointEigenSolver<Mat6> eigs(hessian__);
            for (uint_t i = 0; i < 6 ; ++i)
            {
                if(eigs.eigenvalues()[i] > 1e-4 || true ) //TODO set tolerance
                {
                    std::cout << "POSITIVE. cos distance to grad = " << eigs.eigenvectors().col(i).dot(gradient)/gradient.norm()
                              << ", eigs = " << eigs.eigenvalues()[i] << std::endl;
                    pseudoInv += (1.0/eigs.eigenvalues()(i)) * eigs.eigenvectors().col(i) * eigs.eigenvectors().col(i).transpose();
                    // XXX why is this function not monotonically decreasing? this is annoying, but makes clamping a bad idea: LM!
                }
                else
                {
                    std::cout << "NEGATIVE. cos distance to grad = " << eigs.eigenvectors().col(i).dot(gradient)/gradient.norm()
                              << ", eigs = " << eigs.eigenvalues()[i] << std::endl;

                }
            }
            dxi = - pseudoInv * gradient__;
        }
        else
            dxi = - hessian__.inverse() * gradient__;
        trajectory_->back().update_lhs(dxi);

        // 4) update full trajectory. Here we assume a full rank matrix TODO check for degenerate cases
        Mat61 xiFinal = trajectory_->back().ln_vee();
        for (uint_t t = 1 ; t < numberPoses_-1; ++t)
        {
            dxi = tau * t * xiFinal;
            trajectory_->at(t) = SE3(dxi);
        }
    }while(fabs(diffError) > 1e-4 && !singleIteration && solveIters_ < 1e4);
    return solveIters_;
}

uint_t PlaneRegistration::solve_quaternion_plane()
{
    solveIters_ = 0;
    double previousError = 1e20, diffError = 10;

    // TODO create factor graph or call dense solver?

    return solveIters_;
}

uint_t PlaneRegistration::solve_initialize()
{
    // TODO Maybe solve this as a plane-to-point alignment wrt T0
    // Initialize matrices of points
    MatX X(numberPlanes_,3), Y(numberPlanes_,3);

    // create points Y (from frame t =0), minimum 3 planes per pose
    uint_t t = 0;
    for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
    {
        it->second->calculate_all_matrices_S();
        Y.row(t) = it->second->get_mean_point(0);
        ++t;
    }

    // create points X, for t = 1, ... T, minimum 3 planes per pose
    for (t = 1; t < numberPoses_; ++t)
    {
        uint_t cont = 0;
        X.setZero();
        for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
        {
            X.row(cont) = it->second->get_mean_point(t);
            ++cont;
        }
        // Arun solver
        SE3 estimatedPose;
        if (!PCRegistration::arun(X,Y,estimatedPose))
            return 0;
        trajectory_->at(t) = estimatedPose;
    }

    // update current trajectory
    return 1;
}

double PlaneRegistration::get_current_error() const
{
    double  currentError = 0.0;
    for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
        currentError += it->second->estimate_plane();
    return currentError;
}

void PlaneRegistration::add_plane(uint_t id, std::shared_ptr<Plane> &plane)
{
    plane->set_trajectory(trajectory_);
    planes_.emplace(id, plane);
}

double PlaneRegistration::calculate_poses_rmse(std::vector<SE3> & groundTruth) const
{
    assert(groundTruth.size() >= numberPoses_ && "PlaneRegistration::calculate_poses_rmse: number of poses from GT is incorrect\n");
    double rmse= 0.0;
    uint_t t = 0;
    for (auto &pose: groundTruth)
    {
        Mat61 dxi = (trajectory_->at(t) * pose.inv()).ln_vee();
        //std::cout << pose.ln_vee().transpose() << " and solution inv first pose \n" << (invFirstPose * trajectory_->at(t) * pose.inv()).ln_vee().transpose() <<std::endl;
        rmse += dxi.dot(dxi)/(double)numberPoses_;
        ++t;
    }
    return std::sqrt(rmse);
}

//XXX this can be a reference, who does this interface with pybinds?
std::vector<Mat31> PlaneRegistration::get_point_cloud(uint_t time)
{
	std::vector<Mat31> aggregated_pc;
	for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
    {
        std::vector<Mat31>& plane_pc = it->second->get_points(time);
        // aggregating elements
        aggregated_pc.insert(aggregated_pc.end(), plane_pc.begin(), plane_pc.end());
	}
	return aggregated_pc;
}

Mat4 PlaneRegistration::get_trajectory(uint_t time)
{
    assert(time < numberPoses_ && "CreatePoints::getPointCloud: temporal index larger than number of calculated poses\n");
    if (time < numberPoses_ )
        return trajectory_->at(time).T();
    return Mat4::Identity();
}

void PlaneRegistration::print(bool plotPlanes) const
{
    std::cout << "Printing plane registration data :"<< std::endl;
    for (SE3 &transf : *trajectory_)
        transf.print();
    if (plotPlanes)
    {
        for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
            it->second->print();
    }
}

// resturns: [0]error, [1]iters, hessdet[2], conditioningNumber[3]
std::vector<double> PlaneRegistration::print_evaluate()
{
    std::vector<double> result(6,0.0);

    result[0] = get_current_error();
    result[1] = solveIters_;

    MatX allPlanes(numberPlanes_,4);
    MatX allNormals(numberPlanes_,3);
    uint_t i = 0;

    switch(solveMode_)
    {
        case SolveMode::GRADIENT:
        case SolveMode::GRADIENT_BENGIOS_NAG:
            hessian__.setZero();
            break;
        case SolveMode::GN_HESSIAN:
        case SolveMode::LM_SPHER:
        case SolveMode::LM_ELLIP:
            gradient__ = gradient_;
            hessian__ = hessian_;
    }

    // Normals on planes, check for rank
    for (auto plane : planes_)
    {
        Mat41 pi = plane.second->get_plane();
        //std::cout << "plane : \n" << pi << std::endl;
        allPlanes.row(i) = pi;
        allNormals.row(i) = pi.head(3)/(pi.head(3).norm());
        ++i;
    }
    // Orthogonality between planes (4 dim)
    std::cout << "current gradient \n" << gradient__ << std::endl;
    // XXX : NO, Orthogonolaity was not an issue
    //std::cout << "solution\n" << allPlanes << "\nOrthogonality between planes: \n" << allPlanes * allPlanes.transpose() <<
    //              "\n and det  = \n" << allPlanes.determinant() << std::endl;

    // XXX No, Orthogonality between normals
    //std::cout << "Orthogonality between normals: \n" << allNormals * allNormals.transpose() <<
    //             "\n and det = \n" << allNormals.determinant() << std::endl;

    // Hessian rank and eigen, look for negative vaps. Lasta hessina calculateds
    Eigen::EigenSolver<MatX> eigs(hessian__);
    std::cout << "eigen values are: \n" << eigs.eigenvalues() << std::endl;
    // Determinant of stacked normals
    std::cout << "det(Hessian) = \n" << hessian__ << std::endl;
    // Hessian conditioning number
    Eigen::JacobiSVD<Mat6> svd(hessian__, Eigen::ComputeThinU | Eigen::ComputeThinV);
    // TODO remove
    //std::cout << "SVD decomposition : \n" << svd.singularValues() <<
    //              "\n vectors :\n" << svd.matrixU() <<
    //             "\n and conditioning number = " << svd.singularValues()(0)/svd.singularValues()(5) << std::endl;

    result[2] = hessian__.determinant();
    //TODO count this and optimze
    result[3] = svd.singularValues()(0)/svd.singularValues()(5);

    return result;
}

// From parent class Optimizer:
// TOOD for now replicated, later we will substitute
matData_t PlaneRegistration::calculate_error()
{
    return get_current_error();
}

void PlaneRegistration::calculate_gradient_hessian()
{
    Mat61 gradient = Mat61::Zero();
    Mat6 hessian = Mat6::Zero();
    gradient_.setZero();
    hessian_.setZero();
    double  tau = 1.0 / (double)(numberPoses_-1);
    for (uint_t t = 1 ; t < numberPoses_; ++t)
    {
        gradient.setZero();
        hessian.setZero();
        for (auto it = planes_.cbegin();  it != planes_.cend(); ++it)
        {
            gradient += it->second->calculate_gradient(t);
            hessian += it->second->calculate_hessian(t);
        }
        // TODO this should be changed to time stamps later
        gradient_ +=  (tau *  t)  * gradient;
        hessian_ += (tau *  t) * hessian.selfadjointView<Eigen::Upper>();
    }
}

void PlaneRegistration::update_state(const MatX1 &dx)
{
    trajectory_->back().update_lhs(dx);
    Mat61 xiFinal = trajectory_->back().ln_vee();
    double  tau = 1.0 / (double)(numberPoses_-1);
    Mat61 dxi;
    for (uint_t t = 1 ; t < numberPoses_-1; ++t)
    {
        dxi = tau * t * xiFinal;
        trajectory_->at(t) = SE3(dxi);
    }
}

void PlaneRegistration::bookkeep_state()
{
    bookept_trajectory_ = trajectory_->back();
}

void PlaneRegistration::update_state_from_bookkeep()
{
    trajectory_->back() = bookept_trajectory_;
    Mat61 xiFinal = bookept_trajectory_.ln_vee();
    double  tau = 1.0 / (double)(numberPoses_-1);
    Mat61 dxi;
    for (uint_t t = 1 ; t < numberPoses_-1; ++t)
    {
        dxi = tau * t * xiFinal;
        trajectory_->at(t) = SE3(dxi);
    }
    calculate_error();// planes get recalculated, which is a requisite for later
    //(this class construction, in general grad should be self-contained...)
}

max_stars_repo_name: anastasiia-kornilova/mrob

max_issues_repo_name: anastasiia-kornilova/mrob

max_forks_repo_name: anastasiia-kornilova/mrob


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

// Primary Include
#include <Systems/NomadPlant.hpp>

// C System Includes

// C++ System Includes
#include <iostream>
#include <string>
#include <sstream>
#include <chrono>

// Third-Party Includes
#include <Eigen/Dense>

// Project Includes
#include <Realtime/RealTimeTask.hpp>

namespace Systems
{
namespace Nomad
{

NomadPlant::NomadPlant(const std::string &name, const double T_s) : Realtime::RealTimeTaskNode(name, 20000, Realtime::Priority::MEDIUM, -1, PTHREAD_STACK_MIN), num_states_(13), T_s_(T_s)
{

    // TODO: Should be SET from outside
    rigid_skel = dart::dynamics::Skeleton::create("body");
    rigid_body = rigid_skel->createJointAndBodyNodePair<dart::dynamics::FreeJoint>(nullptr).second;

    // Set the shape
    double boxWidth = 1.0;
    double boxDepth = 1.0;
    double boxHeight = 0.5;

    dart::dynamics::ShapePtr box_shape(new dart::dynamics::BoxShape(Eigen::Vector3d(boxWidth, boxDepth, boxHeight)));

    //auto box_shape = std::make_shared<dart::dynamics::BoxShape>(Eigen::Vector3d(boxWidth, boxDepth, boxHeight));

    rigid_body->createShapeNodeWith<dart::dynamics::CollisionAspect, dart::dynamics::DynamicsAspect>(box_shape);

    rigid_body->setMass(1.0);
    //
    // Create an empty Skeleton with the name "pendulum"
    //dart::dynamics::SkeletonPtr pendulum = dart::dynamics::Skeleton::create("pendulum");

    // Create a world and add the pendulum to the world
    world = dart::simulation::WorldPtr(new dart::simulation::World);
    world->addSkeleton(rigid_skel);
    world->setTimeStep(T_s_);
    world->setGravity(Eigen::Vector3d(0, 0, 0));

    current_force = 0;

    // Create Rigid Body
    block_ = RigidBlock1D(10.0, Eigen::Vector3d(1.0, 0.5, 0.25), T_s);

    // TODO: The below map is old.  Need to use Create Input/Output
    // TODO: Move to "CONNECT"
    // Create Ports
    // State Estimate Input Port
    input_port_map_[InputPort::FORCES] = std::make_shared<Communications::Port<double_vec_t>>("FORCES", Communications::PortInterface::Direction::INPUT, Communications::PortInterface::DataType::DOUBLE, 1, rt_period_);

    // Optimal Force Solution Output Port
    output_port_map_[OutputPort::STATE] = std::make_shared<Communications::Port<double_vec_t>>("STATE", Communications::PortInterface::Direction::OUTPUT, Communications::PortInterface::DataType::DOUBLE, num_states_, rt_period_);

    // Create Messages
    output_state_.length = num_states_;
    output_state_.data.resize(num_states_);

    // Initial State:
    output_state_.data[Idx::X] = 0.0;        // X Position
    output_state_.data[Idx::Y] = 0.0;        // Y Position
    output_state_.data[Idx::Z] = 0.0;        // Z Position
    output_state_.data[Idx::X_DOT] = 0.0;    // X Velocity
    output_state_.data[Idx::Y_DOT] = 0.0;    // Y Velocity
    output_state_.data[Idx::Z_DOT] = 0.0;    // Z Velocity
    output_state_.data[Idx::PHI] = 0.0;      // Roll Orientation
    output_state_.data[Idx::THETA] = 0.0;    // Pitch Orientation
    output_state_.data[Idx::PSI] = 0.0;      // Yaw Orientation
    output_state_.data[Idx::W_X] = 0.0;      // Roll Rate
    output_state_.data[Idx::W_Y] = 0.0;      // Pitch Rate
    output_state_.data[Idx::W_Z] = 0.0;      // Yaw Rate
    output_state_.data[Idx::GRAVITY] = 9.81; // Gravity

    Eigen::VectorXd initial_state(2);
    initial_state[0] = 0.0; // X
    initial_state[1] = 0.0; // X_DOT

    block_.SetState(initial_state);
}

void NomadPlant::Run()
{
    // Get Inputs
    // std::cout << "Time to RECEIVE in CONVEXMPC" << std::endl;
    // Receive State Estimate and Unpack
    bool force_recv = GetInputPort(InputPort::FORCES)->Receive(forces_in); // Receive State Estimate

    if (force_recv)
    {
        Eigen::VectorXd U = Eigen::Map<Eigen::VectorXd>(forces_in.data.data(), 1);
        current_force = U[0];
        // std::cout << "U: " << U << std::endl;
        // block_.Step(U);

       // std::cout << "[NomadPlant]: NOTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTReceive Buffer Empty!" << std::endl;
    }
    else
    {
        std::cout << "[NomadPlant]: Receive Buffer Empty!" << std::endl;
    }

    rigid_skel->getBodyNode(0)->addExtForce(Eigen::Vector3d::UnitX() * current_force, rigid_skel->getBodyNode(0)->getCOM(), false, true);
    //std::cout << "TIME: " << Systems::Time::GetTime() / 1e6<< std::endl;
    std::cout << world->getTime() << " : ";
    std::cout << "U: " << current_force << std::endl;
    std::cout << "X: " << rigid_skel->getPosition(3) << "\tY: " << rigid_skel->getPosition(4) << "\tZ: " << rigid_skel->getPosition(5) << std::endl;
    std::cout << "X_DOT: " << rigid_skel->getVelocity(3) << "\tY_DOT: " << rigid_skel->getVelocity(4) << "\tZ_DOT: " << rigid_skel->getVelocity(5) << std::endl;
    std::cout << std::endl;

    world->step(true);

    //std::cout << "NOMAD PLANT X: " << block_.GetState() << std::endl;

    output_state_.data[Idx::X] = rigid_skel->getPosition(3);
    output_state_.data[Idx::X_DOT] = rigid_skel->getVelocity(3);

    //output_state_.data[Idx::X] = block_.GetState()[0];
    //output_state_.data[Idx::X_DOT] = block_.GetState()[1];

    // std::cout << "NOMAD PLANT X: " << output_state_.data[Idx::X] << std::endl;
    // std::cout << "NOMAD PLANT X DOT: " << output_state_.data[Idx::X_DOT] << std::endl;

    // std::cout << "NOMAD PLANT X DOT: " << output_state_.data[Idx::X_DOT] << std::endl;

    // Output Optimal Forces
    //bool send_status = GetOutputPort(OutputPort::STATE)->Send(output_state_);
}

void NomadPlant::Setup()
{
    // Connect Input Ports
    //GetInputPort(InputPort::FORCES)->Connect(); // Forces

    // Bind Output Ports
    //GetOutputPort(OutputPort::STATE)->Bind(); // Optimal Force Output

    std::cout << "[NomadPlant]: "
              << "NomadPlant Task Node Running!" << std::endl;
}

} // namespace Nomad
} // namespace Systems

max_stars_repo_name: implementedrobotics/Nomad

max_issues_repo_name: implementedrobotics/Nomad

max_forks_repo_name: implementedrobotics/Nomad


text:
#include "Exceptions.hpp"
#include "mapcss/TextureAtlasParser.hpp"

#define BOOST_SPIRIT_USE_PHOENIX_V3
#include <boost/bind.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_fusion.hpp>
#include <boost/spirit/include/phoenix_object.hpp>

using namespace utymap::mapcss;

namespace qi = boost::spirit::qi;
namespace ascii = boost::spirit::ascii;
namespace phoenix = boost::phoenix;

namespace {

struct Region {
  std::string groupName;
  int x;
  int y;
  int width;
  int height;
};

struct Atlas {
  int id;
  int width;
  int height;
  std::vector<Region> regions;
};

}

BOOST_FUSION_ADAPT_STRUCT(
  Region,
  (std::string, groupName)
  (int, x)
  (int, y)
  (int, width)
  (int, height)
)

BOOST_FUSION_ADAPT_STRUCT(
  Atlas,
  (int, id)
  (int, width)
  (int, height)
  (std::vector<Region>, regions)
)

template<typename Iterator>
struct RegionGrammar : qi::grammar<Iterator, Region(), qi::space_type> {
  RegionGrammar() : RegionGrammar::base_type(start, "region") {
    start =
      qi::lexeme[+(ascii::char_ - ',')] >> ',' >>
      qi::int_ >> ',' >>
      qi::int_ >> ',' >>
      qi::int_ >> ',' >>
      qi::int_;

    start.name("region");
  }
  qi::rule<Iterator, Region(), qi::space_type> start;
};

template<typename Iterator>
struct AtlasGrammar : qi::grammar<Iterator, Atlas(), qi::space_type> {
  AtlasGrammar() : AtlasGrammar::base_type(start, "atlas") {
    start =
      qi::int_ >> ',' >>
      qi::int_ >> ',' >>
      qi::int_ >>
      +(region);
  }

  RegionGrammar<Iterator> region;
  qi::rule<Iterator, Atlas(), qi::space_type> start;
};

template<typename Iterator>
TextureAtlas parse(Iterator begin, Iterator end) {
  Atlas atlas;
  AtlasGrammar<Iterator> grammar;

  if (!phrase_parse(begin, end, grammar, qi::space, atlas))
    throw utymap::MapCssException(std::string("Cannot parse texture atlas."));

  std::unordered_map<std::string, TextureGroup> groups;
  for (const auto &region : atlas.regions) {
    groups[region.groupName].add(
        static_cast<std::uint16_t>(atlas.width),
        static_cast<std::uint16_t>(atlas.height),
        utymap::math::Rectangle(region.x,
                                region.y,
                                region.x + region.width,
                                region.y + region.height));
  }

  return TextureAtlas(static_cast<std::uint16_t>(atlas.id), groups);
}

TextureAtlas TextureAtlasParser::parse(const std::string &content) const {
  return ::parse(content.begin(), content.end());
}

TextureAtlas TextureAtlasParser::parse(std::istream &istream) const {
  std::string content((std::istreambuf_iterator<char>(istream)), std::istreambuf_iterator<char>());
  return parse(content);
}

max_stars_repo_name: roscopecoltran/SniperKit-Core

max_issues_repo_name: roscopecoltran/SniperKit-Core

max_forks_repo_name: roscopecoltran/SniperKit-Core


text:
//
// Copyright (c) 2016-2019 Vinnie Falco (vinnie dot falco at gmail dot com)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Official repository: https://github.com/boostorg/beast
//

// Test that header file is self-contained.
#include <boost/beast/core/buffers_adaptor.hpp>

#include "test_buffer.hpp"

#include <boost/beast/core/buffer_traits.hpp>
#include <boost/beast/core/multi_buffer.hpp>
#include <boost/beast/core/ostream.hpp>
#include <boost/beast/core/read_size.hpp>
#include <boost/beast/_experimental/unit_test/suite.hpp>
#include <boost/asio/buffer.hpp>
#include <boost/asio/streambuf.hpp>
#include <iterator>

namespace boost {
namespace beast {

struct buffers_adaptor_test_hook
{
    template<class MutableBufferSequence>
    static
    auto
    make_subrange(
        buffers_adaptor <MutableBufferSequence> &adaptor,
        std::size_t pos = 0,
        std::size_t n = (std::numeric_limits<std::size_t>::max)())
    -> typename buffers_adaptor<MutableBufferSequence>::mutable_buffers_type
    {
        return adaptor.make_subrange(pos, n);
    }

    template<class MutableBufferSequence>
    static
    auto
    make_subrange(
        buffers_adaptor<MutableBufferSequence> const& adaptor,
        std::size_t pos = 0,
        std::size_t n = (std::numeric_limits<std::size_t>::max)())
    -> typename buffers_adaptor<MutableBufferSequence>::const_buffers_type
    {
        return adaptor.make_subrange(pos, n);
    }
};

class buffers_adaptor_test : public unit_test::suite
{
public:
    BOOST_STATIC_ASSERT(
        is_mutable_dynamic_buffer<
            buffers_adaptor<buffers_triple>>::value);

    void
    testDynamicBuffer()
    {
        char s[13];
        buffers_triple tb(s, sizeof(s));
        buffers_adaptor<buffers_triple> b(tb);
        test_dynamic_buffer(b);
    }

    void
    testSpecial()
    {
        char s1[13];
        buffers_triple tb1(s1, sizeof(s1));
        BEAST_EXPECT(buffer_bytes(tb1) == sizeof(s1));

        char s2[15];
        buffers_triple tb2(s2, sizeof(s2));
        BEAST_EXPECT(buffer_bytes(tb2) == sizeof(s2));

        {
            // construction

            buffers_adaptor<buffers_triple> b1(tb1);
            BEAST_EXPECT(b1.value() == tb1);

            buffers_adaptor<buffers_triple> b2(tb2);
            BEAST_EXPECT(b2.value() == tb2);

            buffers_adaptor<buffers_triple> b3(b2);
            BEAST_EXPECT(b3.value() == tb2);

            char s3[15];
            buffers_adaptor<buffers_triple> b4(
                boost::in_place_init, s3, sizeof(s3));
            BEAST_EXPECT(b4.value() == buffers_triple(s3, sizeof(s3)));

            // assignment

            b3 = b1;
            BEAST_EXPECT(b3.value() == tb1);
        }
    }

    void
    testIssue386()
    {
        using type = net::streambuf;
        type buffer;
        buffers_adaptor<
            type::mutable_buffers_type> ba{buffer.prepare(512)};
        read_size(ba, 1024);
    }

    template<bool isMutable>
    void
    testSubrange()
    {
        auto exemplar = std::string("the quick brown fox jumps over the lazy dog");

        auto iterate_test = [&](
            std::size_t a,
            std::size_t b,
            std::size_t c)
        {
            static const auto func = "iterate_test";
            auto buffers = std::vector<net::mutable_buffer>();
            if (a)
                buffers.push_back(net::buffer(&exemplar[0], a));
            if (b - a)
                buffers.push_back(net::buffer(&exemplar[a], (b - a)));
            if (c - b)
                buffers.push_back(net::buffer(&exemplar[b], (c - b)));
            auto adapter = buffers_adaptor<std::vector<net::mutable_buffer>>(buffers);

            using value_type =
            typename std::conditional<
                isMutable,
                net::mutable_buffer,
                net::const_buffer>::type;

            using maybe_mutable =
            typename std::conditional<
                isMutable,
                buffers_adaptor<std::vector<net::mutable_buffer>>&,
                buffers_adaptor<std::vector<net::mutable_buffer>> const&>::type;

            auto sub = buffers_adaptor_test_hook::make_subrange(static_cast<maybe_mutable>(adapter));
            BEAST_EXPECTS(typeid(typename decltype(sub)::value_type) == typeid(value_type), func);
            BEAST_EXPECT(buffers_to_string(sub) == exemplar.substr(0, c));
        };

        iterate_test(0, 0, 1);

        for (std::size_t a = 0; a <= exemplar.size(); ++a)
            for (std::size_t b = a; b <= exemplar.size(); ++b)
                for (std::size_t c = b; c <= exemplar.size(); ++c)
                    iterate_test(a, b, c);
    }

    void
    run() override
    {
        testDynamicBuffer();
        testSpecial();
        testIssue386();
#if 0
        testBuffersAdapter();
        testCommit();
#endif
        testSubrange<true>();
        testSubrange<false>();
    }
};

BEAST_DEFINE_TESTSUITE(beast,core,buffers_adaptor);

} // beast
} // boost

max_stars_repo_name: jeasonzs/beast

max_issues_repo_name: jeasonzs/beast

max_forks_repo_name: jeasonzs/beast


text:
/******************************************************************
 *
 * Round for C
 *
 * Copyright (C) Satoshi Konno 2015
 *
 * This is licensed under BSD-style license, see file COPYING.
 *
 ******************************************************************/

#include <boost/test/unit_test.hpp>

#include <round/node_internal.h>
#include <round/method/system_method.h>

BOOST_AUTO_TEST_CASE(LocalNodeMessageNew)
{
  RoundMessage* msg = round_local_node_message_new();
  BOOST_CHECK(msg);

  BOOST_CHECK_EQUAL(round_local_node_message_geterror(msg), (RoundError*)NULL);
  BOOST_CHECK_EQUAL(round_local_node_message_getresponsejsonobject(msg), (RoundJSONObject**)NULL);

  BOOST_CHECK(round_local_node_message_delete(msg));
}

max_stars_repo_name: cybergarage/round-common

max_issues_repo_name: cybergarage/round-common

max_forks_repo_name: cybergarage/round-common


text:
/***************************************************************************
 * NASA Glenn Research Center, Cleveland, OH
 * Released under the NASA Open Source Agreement (NOSA)
 * May  2021
 ****************************************************************************
 */

#include "StorageConfig.h"
#include <boost/make_shared.hpp>
#include <boost/foreach.hpp>
#include <iostream>

static const std::vector<std::string> VALID_STORAGE_IMPLEMENTATION_NAMES = { "stdio_multi_threaded", "asio_single_threaded" };

storage_disk_config_t::storage_disk_config_t() : name(""), storeFilePath("") {}
storage_disk_config_t::~storage_disk_config_t() {}

storage_disk_config_t::storage_disk_config_t(const std::string & paramName, const std::string & paramStoreFilePath) :
    name(paramName), storeFilePath(paramStoreFilePath) {}

//a copy constructor: X(const X&)
storage_disk_config_t::storage_disk_config_t(const storage_disk_config_t& o) :
    name(o.name), storeFilePath(o.storeFilePath) { }

//a move constructor: X(X&&)
storage_disk_config_t::storage_disk_config_t(storage_disk_config_t&& o) :
    name(std::move(o.name)), storeFilePath(std::move(o.storeFilePath)) { }

//a copy assignment: operator=(const X&)
storage_disk_config_t& storage_disk_config_t::operator=(const storage_disk_config_t& o) {
    name = o.name;
    storeFilePath = o.storeFilePath;
    return *this;
}

//a move assignment: operator=(X&&)
storage_disk_config_t& storage_disk_config_t::operator=(storage_disk_config_t&& o) {
    name = std::move(o.name);
    storeFilePath = std::move(o.storeFilePath);
    return *this;
}

bool storage_disk_config_t::operator==(const storage_disk_config_t & other) const {
    return (name == other.name) && (storeFilePath == other.storeFilePath);
}

StorageConfig::StorageConfig() :
    m_storageImplementation("stdio_multi_threaded"),
    m_tryToRestoreFromDisk(false),
    m_autoDeleteFilesOnExit(true),
    m_totalStorageCapacityBytes(1),
    m_storageDiskConfigVector() { }

StorageConfig::~StorageConfig() {
}

//a copy constructor: X(const X&)
StorageConfig::StorageConfig(const StorageConfig& o) :
    m_storageImplementation(o.m_storageImplementation),
    m_tryToRestoreFromDisk(o.m_tryToRestoreFromDisk),
    m_autoDeleteFilesOnExit(o.m_autoDeleteFilesOnExit),
    m_totalStorageCapacityBytes(o.m_totalStorageCapacityBytes),
    m_storageDiskConfigVector(o.m_storageDiskConfigVector) { }

//a move constructor: X(X&&)
StorageConfig::StorageConfig(StorageConfig&& o) :
    m_storageImplementation(std::move(o.m_storageImplementation)),
    m_tryToRestoreFromDisk(o.m_tryToRestoreFromDisk),
    m_autoDeleteFilesOnExit(o.m_autoDeleteFilesOnExit),
    m_totalStorageCapacityBytes(o.m_totalStorageCapacityBytes),
    m_storageDiskConfigVector(std::move(o.m_storageDiskConfigVector)) { }

//a copy assignment: operator=(const X&)
StorageConfig& StorageConfig::operator=(const StorageConfig& o) {
    m_storageImplementation = o.m_storageImplementation;
    m_tryToRestoreFromDisk = o.m_tryToRestoreFromDisk;
    m_autoDeleteFilesOnExit = o.m_autoDeleteFilesOnExit;
    m_totalStorageCapacityBytes = o.m_totalStorageCapacityBytes;
    m_storageDiskConfigVector = o.m_storageDiskConfigVector;
    return *this;
}

//a move assignment: operator=(X&&)
StorageConfig& StorageConfig::operator=(StorageConfig&& o) {
    m_storageImplementation = std::move(o.m_storageImplementation);
    m_tryToRestoreFromDisk = o.m_tryToRestoreFromDisk;
    m_autoDeleteFilesOnExit = o.m_autoDeleteFilesOnExit;
    m_totalStorageCapacityBytes = o.m_totalStorageCapacityBytes;
    m_storageDiskConfigVector = std::move(o.m_storageDiskConfigVector);
    return *this;
}

bool StorageConfig::operator==(const StorageConfig & other) const {
    return
        (m_storageImplementation == other.m_storageImplementation) &&
        (m_tryToRestoreFromDisk == other.m_tryToRestoreFromDisk) &&
        (m_autoDeleteFilesOnExit == other.m_autoDeleteFilesOnExit) &&
        (m_totalStorageCapacityBytes == other.m_totalStorageCapacityBytes) &&
        (m_storageDiskConfigVector == other.m_storageDiskConfigVector);
}

bool StorageConfig::SetValuesFromPropertyTree(const boost::property_tree::ptree & pt) {
    try {
        m_storageImplementation = pt.get<std::string>("storageImplementation");
        {
            bool found = false;
            for (std::vector<std::string>::const_iterator it = VALID_STORAGE_IMPLEMENTATION_NAMES.cbegin(); it != VALID_STORAGE_IMPLEMENTATION_NAMES.cend(); ++it) {
                if (m_storageImplementation == *it) {
                    found = true;
                    break;
                }
            }
            if (!found) {
                std::cerr << "error parsing JSON Storage config:: invalid storage implementation " << m_storageImplementation << std::endl;
                return false;
            }
        }
        m_tryToRestoreFromDisk = pt.get<bool>("tryToRestoreFromDisk");
        m_autoDeleteFilesOnExit = pt.get<bool>("autoDeleteFilesOnExit");
        m_totalStorageCapacityBytes = pt.get<uint64_t>("totalStorageCapacityBytes");
    }
    catch (const boost::property_tree::ptree_error & e) {
        std::cerr << "error parsing JSON Storage config: " << e.what() << std::endl;
        return false;
    }

    if (m_totalStorageCapacityBytes == 0) {
        std::cerr << "error parsing JSON Storage config: totalStorageCapacityBytes must be defined and non-zero\n";
        return false;
    }
    const boost::property_tree::ptree & storageDiskConfigVectorPt = pt.get_child("storageDiskConfigVector", boost::property_tree::ptree()); //non-throw version
    m_storageDiskConfigVector.resize(storageDiskConfigVectorPt.size());
    unsigned int storageDiskConfigVectorIndex = 0;
    BOOST_FOREACH(const boost::property_tree::ptree::value_type & storageDiskConfigPt, storageDiskConfigVectorPt) {
        storage_disk_config_t & storageDiskConfig = m_storageDiskConfigVector[storageDiskConfigVectorIndex++];
        try {
            storageDiskConfig.name = storageDiskConfigPt.second.get<std::string>("name");
            storageDiskConfig.storeFilePath = storageDiskConfigPt.second.get<std::string>("storeFilePath");
        }
        catch (const boost::property_tree::ptree_error & e) {
            std::cerr << "error parsing JSON storageDiskConfigVector[" << (storageDiskConfigVectorIndex - 1) << "]: " << e.what() << std::endl;
            return false;
        }
        if (storageDiskConfig.storeFilePath == "") {
            std::cerr << "error parsing JSON storageDiskConfigVector[" << (storageDiskConfigVectorIndex - 1) << "]: storeFilePath must be defined\n";
            return false;
        }
    }

    return true;
}

StorageConfig_ptr StorageConfig::CreateFromJson(const std::string & jsonString) {
    try {
        return StorageConfig::CreateFromPtree(JsonSerializable::GetPropertyTreeFromJsonString(jsonString));
    }
    catch (boost::property_tree::json_parser::json_parser_error & e) {
        const std::string message = "In StorageConfig::CreateFromJson. Error: " + std::string(e.what());
        hdtn::Logger::getInstance()->logError("storage", message);
        std::cerr << message << std::endl;
    }

    return StorageConfig_ptr(); //NULL
}

StorageConfig_ptr StorageConfig::CreateFromJsonFile(const std::string & jsonFileName) {
    try {
        return StorageConfig::CreateFromPtree(JsonSerializable::GetPropertyTreeFromJsonFile(jsonFileName));
    }
    catch (boost::property_tree::json_parser::json_parser_error & e) {
        const std::string message = "In StorageConfig::CreateFromJsonFile. Error: " + std::string(e.what());
        hdtn::Logger::getInstance()->logError("storage", message);
        std::cerr << message << std::endl;
    }

    return StorageConfig_ptr(); //NULL
}

StorageConfig_ptr StorageConfig::CreateFromPtree(const boost::property_tree::ptree & pt) {

    StorageConfig_ptr ptrStorageConfig = boost::make_shared<StorageConfig>();
    if (!ptrStorageConfig->SetValuesFromPropertyTree(pt)) {
        ptrStorageConfig = StorageConfig_ptr(); //failed, so delete and set it NULL
    }
    return ptrStorageConfig;
}

boost::property_tree::ptree StorageConfig::GetNewPropertyTree() const {
    boost::property_tree::ptree pt;
    pt.put("storageImplementation", m_storageImplementation);
    pt.put("tryToRestoreFromDisk", m_tryToRestoreFromDisk);
    pt.put("autoDeleteFilesOnExit", m_autoDeleteFilesOnExit);
    pt.put("totalStorageCapacityBytes", m_totalStorageCapacityBytes);
    boost::property_tree::ptree & storageDiskConfigVectorPt = pt.put_child("storageDiskConfigVector", m_storageDiskConfigVector.empty() ? boost::property_tree::ptree("[]") : boost::property_tree::ptree());
    for (storage_disk_config_vector_t::const_iterator storageDiskConfigVectorIt = m_storageDiskConfigVector.cbegin(); storageDiskConfigVectorIt != m_storageDiskConfigVector.cend(); ++storageDiskConfigVectorIt) {
        const storage_disk_config_t & storageDiskConfig = *storageDiskConfigVectorIt;
        boost::property_tree::ptree & storageDiskConfigPt = (storageDiskConfigVectorPt.push_back(std::make_pair("", boost::property_tree::ptree())))->second; //using "" as key creates json array
        storageDiskConfigPt.put("name", storageDiskConfig.name);
        storageDiskConfigPt.put("storeFilePath", storageDiskConfig.storeFilePath);
    }

    return pt;
}

void StorageConfig::AddDisk(const std::string & name, const std::string & storeFilePath) {
    m_storageDiskConfigVector.push_back(storage_disk_config_t(name, storeFilePath));
}


text:
/*
 * InputPaths.cpp
 *
 *  Created on: 23 Oct 2015
 *      Author: hieu
 */
#include <boost/foreach.hpp>
#include <iostream>
#include "InputPaths.h"
#include "Sentence.h"
#include "../System.h"
#include "../legacy/Range.h"
#include "Manager.h"
#include "InputPath.h"
#include "Word.h"

using namespace std;

namespace Moses2
{
namespace SCFG
{
void InputPaths::Init(const InputType &input, const ManagerBase &mgr)
{
  const Sentence &sentence = static_cast<const Sentence&>(input);
  MemPool &pool = mgr.GetPool();
  size_t numPt = mgr.system.mappings.size();
  size_t size = sentence.GetSize();
  //cerr << "size=" << size << endl;

  m_matrix = new (pool.Allocate< Matrix<SCFG::InputPath*> >()) Matrix<SCFG::InputPath*>(pool,
      size, size + 1);
  m_matrix->Init(NULL);

  for (size_t startPos = 0; startPos < size; ++startPos) {
    // create path for 0 length string
    Range range(startPos, startPos - 1);
    SubPhrase<SCFG::Word> subPhrase = sentence.GetSubPhrase(startPos, 0);

    SCFG::InputPath *path = new (pool.Allocate<SCFG::InputPath>()) SCFG::InputPath(pool,
        subPhrase, range, numPt, NULL);
    //cerr << "path=" << *path << endl;
    m_inputPaths.push_back(path);
    m_matrix->SetValue(startPos, 0, path);

    // create normal paths of subphrases through the sentence
    const SCFG::InputPath *prefixPath = path;
    for (size_t phaseSize = 1; phaseSize <= size; ++phaseSize) {
      size_t endPos = startPos + phaseSize - 1; // pb-like indexing. eg. [1-1] covers 1 word, NOT 0

      if (endPos >= size) {
        break;
      }

      SubPhrase<SCFG::Word> subPhrase = sentence.GetSubPhrase(startPos, phaseSize);
      Range range(startPos, endPos);

      SCFG::InputPath *path = new (pool.Allocate<SCFG::InputPath>())
      SCFG::InputPath(pool, subPhrase, range, numPt, prefixPath);
      //cerr << "path=" << *path << endl;
      m_inputPaths.push_back(path);

      prefixPath = path;
      m_matrix->SetValue(startPos, phaseSize, path);
    }
  }

}

std::string InputPaths::Debug(const System &system) const
{
  stringstream out;
  const Matrix<InputPath*> &matrix = GetMatrix();
  for (size_t i = 0; i < matrix.GetRows(); ++i) {
    for (size_t j = 0; j < matrix.GetCols(); ++j) {
      SCFG::InputPath *path = matrix.GetValue(i, j);
      if (path) {
        out << path->Debug(system);
        out << endl;
      }
    }
  }
  return out.str();
}

}
}


text:
#include <boost/graph/adjacency_list.hpp>
#include <glog/logging.h>
#include <gtest/gtest.h>

#include "segmatch/recognizers/graph_utilities.hpp"
using namespace boost;
using namespace segmatch;

// Initialize common objects needed by multiple tests.
class GraphUtilitiesTest : public ::testing::Test {
 protected:
  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> Graph;
  typedef graph_traits<Graph>::vertex_descriptor Vertex;
  typedef std::vector<Vertex> Vertices;

  Graph graph_1;
  Graph graph_2;
  Graph graph_3;

  void SetUp() override {
    graph_1 = buildGraph(4, { { 0, 2 }, { 1, 2 }, { 1, 3 }, { 2, 3 } });
    graph_2 = buildGraph(8, {
        { 0, 5 }, { 0, 7 }, { 1, 3 }, { 1, 5 }, { 2, 3 }, { 2, 4 }, { 2, 6 }, { 2, 7 }, { 3, 4 },
        { 3, 6 }, { 4, 6 }
    });
    // Graph from page 2 of http://hbanaszak.mjr.uw.edu.pl/TempTxt/Batagelj/cores.pdf
    graph_3 = buildGraph(21, {
        { 0, 5 }, { 0, 7 }, { 0, 9 }, { 0, 12 }, { 1, 3 }, { 1, 6 }, { 2, 8 }, { 2, 10 },
        { 2, 16 }, { 3, 6 }, { 3, 7 }, { 3, 13 }, { 3, 15 }, { 3, 20 }, { 4, 17 }, { 5, 7 },
        { 5, 12 }, { 6, 7 }, { 6, 11 }, { 7, 12 }, { 8, 19 }, { 9, 15 }, { 9, 17 }, { 13, 15 },
        { 13, 20 }, { 14, 17 }, { 15, 17 }, { 15, 20 }, { 16, 19 }
    });
  }

  // Build a BGL graph from a vector of edges.
  Graph buildGraph(const size_t num_vertices,
                   const std::vector<std::pair<size_t, size_t>>& edges) {
    Graph graph(num_vertices);
    for (const auto& edge : edges) {
      boost::add_edge(edge.first, edge.second, graph);
    }
    return graph;
  }
};

TEST_F(GraphUtilitiesTest, test_find_max_clique_1) {
  // Arrange
  Vertices expected_max_clique = { 1, 2, 3 };

  // Act
  Vertices max_clique = GraphUtilities::findMaximumClique(graph_1, 3);

  // Assert
  std::sort(max_clique.begin(), max_clique.end());
  EXPECT_EQ(expected_max_clique, max_clique);
}

TEST_F(GraphUtilitiesTest, test_find_max_clique_2) {
  // Arrange
  Vertices expected_max_clique = { 2, 3, 4, 6 };

  // Act
  Vertices max_clique = GraphUtilities::findMaximumClique(graph_2, 3);

  // Assert
  std::sort(max_clique.begin(), max_clique.end());
  EXPECT_EQ(expected_max_clique, max_clique);
}

TEST_F(GraphUtilitiesTest, test_find_max_clique_3) {
  // Arrange
  Vertices expected_max_clique = { 3, 13, 15, 20 };
  // The graph contains two maximum cliques. Which one is found depends on the algorithm.
  // Vertices expected_max_clique = { 0, 5, 7, 12 };

  // Act
  Vertices max_clique = GraphUtilities::findMaximumClique(graph_3, 4);

  // Assert
  std::sort(max_clique.begin(), max_clique.end());
  EXPECT_EQ(expected_max_clique, max_clique);
}

TEST_F(GraphUtilitiesTest, test_find_max_clique_no_such_clique) {
  // Arrange
  Vertices expected_max_clique = { };

  // Act
  Vertices max_clique = GraphUtilities::findMaximumClique(graph_3, 5);

  // Assert
  std::sort(max_clique.begin(), max_clique.end());
  EXPECT_EQ(expected_max_clique, max_clique);
}

max_stars_repo_name: Oofs/segmap

max_issues_repo_name: Oofs/segmap

max_forks_repo_name: Oofs/segmap