<commit_before>/** Copyright (C) 2013 David Braam - Released under terms of the AGPLv3 License */
#include "raft.h"
#include "support.h"

namespace cura {

void generateRaft(SliceDataStorage& storage, int distance)
{
    if (storage.draft_protection_shield.size() > 0)
    {
        storage.raftOutline = storage.raftOutline.unionPolygons(storage.draft_protection_shield.offset(distance));
    }
    else if (storage.oozeShield.size() > 0 && storage.oozeShield[0].size() > 0)
    {
        storage.raftOutline = storage.raftOutline.unionPolygons(storage.oozeShield[0].offset(distance));
    }
    else
    {
        storage.raftOutline = storage.getLayerOutlines(0, true).offset(distance);
    }
}

}//namespace cura
<commit_msg>fix: raft outline now uses rounded offset (CURA-1835)<commit_after>/** Copyright (C) 2013 David Braam - Released under terms of the AGPLv3 License */
#include <clipper/clipper.hpp>

#include "raft.h"
#include "support.h"

namespace cura {

void generateRaft(SliceDataStorage& storage, int distance)
{
    if (storage.draft_protection_shield.size() > 0)
    {
        storage.raftOutline = storage.raftOutline.unionPolygons(storage.draft_protection_shield.offset(distance, ClipperLib::jtRound));
    }
    else if (storage.oozeShield.size() > 0 && storage.oozeShield[0].size() > 0)
    {
        storage.raftOutline = storage.raftOutline.unionPolygons(storage.oozeShield[0].offset(distance, ClipperLib::jtRound));
    }
    else
    {
        storage.raftOutline = storage.getLayerOutlines(0, true).offset(distance, ClipperLib::jtRound);
    }
}

}//namespace cura
<|endoftext|>


<commit_before>#include "message.h"
#include <yuni/core/system/console/console.h>

using namespace Yuni;

namespace Nany
{
namespace Logs
{

	Message& Message::createEntry(Level level)
	{
		Message* entry = new Message{level};

		ThreadingPolicy::MutexLocker locker{*this};
		entry->origins = origins;
		entries.push_back(entry);
		return *entry;
	}

	void Message::appendEntry(const Message::Ptr& entry)
	{
		if (!!entry)
		{
			if (not (entry->entries.empty() and entry->level == Level::none))
			{
				ThreadingPolicy::MutexLocker locker{*this};
				entries.push_back(entry);
				hasErrors &= entry->hasErrors;
			}
		}
	}

	namespace // anonymous
	{

		template<bool unify>
		static void printMessage(nyconsole_t& out, const Message& message, uint32_t indent, String& xx)
		{
			#ifndef YUNI_OS_WINDOWS
			#define SEP " \u205E "
			#else
			#define SEP " : "
			#endif

			Message::ThreadingPolicy::MutexLocker locker{message};

			// which output ?
			nyconsole_output_t omode = (unify or (message.level != Level::error and message.level != Level::warning))
				? nycout : nycerr;
			// function writer
			auto wrfn = (omode == nycout) ? out.write_stdout : out.write_stderr;
			// print method
			auto print = [&](const AnyString& s) { wrfn(out.internal, s.c_str(), s.size()); };

			if (message.level != Level::none)
			{
				switch (message.level)
				{
					case Level::error:
					{
						out.set_color(out.internal, omode, nyc_red);
						print("   error" SEP);
						out.set_color(out.internal, omode, nyc_none);
						break;
					}
					case Level::warning:
					{
						out.set_color(out.internal, omode, nyc_yellow);
						print(" warning" SEP);
						out.set_color(out.internal, omode, nyc_none);
						break;
					}
					case Level::info:
					{
						if (message.section.empty())
						{
							out.set_color(out.internal, omode, nyc_none);
							print("        " SEP);
						}
						else
						{
							out.set_color(out.internal, omode, nyc_lightblue);
							xx = "        ";
							xx.overwriteRight(message.section);
							xx << SEP;
							print(xx);
							out.set_color(out.internal, omode, nyc_none);
						}
						break;
					}
					case Level::hint:
					case Level::suggest:
					{
						out.set_color(out.internal, omode, nyc_none);
						print("        " SEP);
						out.set_color(out.internal, omode, nyc_none);
						break;
					}
					case Level::success:
					{
						out.set_color(out.internal, omode, nyc_green);
						#ifndef YUNI_OS_WINDOWS
						print("      \u2713 " SEP);
						#else
						print("      ok" SEP);
						#endif
						out.set_color(out.internal, omode, nyc_none);
						break;
					}
					case Level::trace:
					{
						out.set_color(out.internal, omode, nyc_purple);
						print("      ::");
						out.set_color(out.internal, omode, nyc_none);
						print(SEP);
						break;
					}
					case Level::verbose:
					{
						out.set_color(out.internal, omode, nyc_green);
						print("      ::");
						out.set_color(out.internal, omode, nyc_none);
						print(SEP);
						break;
					}
					case Level::ICE:
					{
						out.set_color(out.internal, omode, nyc_red);
						print("     ICE" SEP);
						out.set_color(out.internal, omode, nyc_none);
						break;
					}
					case Level::none:
					{
						// unreachable - cf condition above
						break;
					}
				}

				if (indent)
				{
					xx.clear();
					for (uint32_t i = indent; i--; )
						xx << "    ";
					print(xx);
				}

				if (not message.prefix.empty())
				{
					out.set_color(out.internal, omode, nyc_white);
					print(message.prefix);
					out.set_color(out.internal, omode, nyc_none);
				}

				if (message.level == Level::suggest)
				{
					out.set_color(out.internal, omode, nyc_lightblue);
					print("suggest: ");
					out.set_color(out.internal, omode, nyc_none);
				}
				else if (message.level == Level::hint)
				{
					out.set_color(out.internal, omode, nyc_lightblue);
					print("hint: ");
					out.set_color(out.internal, omode, nyc_none);
				}

				if (not message.origins.location.target.empty())
				{
					print("{");
					print(message.origins.location.target);
					print("} ");
				}

				if (not message.origins.location.filename.empty())
				{
					out.set_color(out.internal, omode, nyc_white);
					xx = message.origins.location.filename;

					if (message.origins.location.pos.line > 0)
					{
						xx << ':';
						xx << message.origins.location.pos.line;
						if (message.origins.location.pos.offset != 0)
						{
							xx << ':';
							xx << message.origins.location.pos.offset;
							if (message.origins.location.pos.offsetEnd != 0)
							{
								xx << '-';
								xx << message.origins.location.pos.offsetEnd;
							}
						}
						print(xx);
					}
					xx << ": ";
					print(xx);
					out.set_color(out.internal, omode, nyc_none);
				}

				if (not message.message.empty())
				{
					String msg{message.message};
					msg.trimRight(" \t\r\n");
					msg.replace("\t", "    "); // tabs

					auto firstLF = msg.find('\n');
					if (not (firstLF < message.message.size()))
					{
						print(msg);
					}
					else
					{
						xx.clear() << "\n        " SEP;
						for (uint i = indent; i--; )
							xx.write("    ", 4);

						bool addLF = false;
						msg.words("\n", [&](const AnyString& word) -> bool
						{
							if (addLF)
								print(xx);
							print(word);
							addLF = true;
							return true;
						});
					}
				}

				print("\n");
				++indent;
			}

			for (auto& ptr: message.entries)
				printMessage<unify>(out, *ptr, indent, xx);
		}

	} // anonymous namespace

	void Message::print(nyconsole_t& out, bool unify)
	{
		assert(out.set_color);
		if (out.set_color and out.write_stderr and out.write_stdout)
		{
			String tmp;
			if (unify)
				printMessage<true>(out, *this, 0, tmp);
			else
				printMessage<false>(out, *this, 0, tmp);
		}
	}

} // namespace Logs
} // namespace Nany
<commit_msg>err reporting: moved macro outside function implementation<commit_after>#include "message.h"
#include <yuni/core/system/console/console.h>

using namespace Yuni;

#ifndef YUNI_OS_WINDOWS
#define SEP " \u205E "
#else
#define SEP " : "
#endif

namespace Nany
{
namespace Logs
{

	Message& Message::createEntry(Level level)
	{
		Message* entry = new Message{level};

		ThreadingPolicy::MutexLocker locker{*this};
		entry->origins = origins;
		entries.push_back(entry);
		return *entry;
	}

	void Message::appendEntry(const Message::Ptr& entry)
	{
		if (!!entry)
		{
			if (not (entry->entries.empty() and entry->level == Level::none))
			{
				ThreadingPolicy::MutexLocker locker{*this};
				entries.push_back(entry);
				hasErrors &= entry->hasErrors;
			}
		}
	}

	namespace // anonymous
	{

		template<bool unify>
		static void printMessage(nyconsole_t& out, const Message& message, uint32_t indent, String& xx)
		{
			Message::ThreadingPolicy::MutexLocker locker{message};

			// which output ?
			nyconsole_output_t omode = (unify or (message.level != Level::error and message.level != Level::warning))
				? nycout : nycerr;
			// function writer
			auto wrfn = (omode == nycout) ? out.write_stdout : out.write_stderr;
			// print method
			auto print = [&](const AnyString& s) { wrfn(out.internal, s.c_str(), s.size()); };

			if (message.level != Level::none)
			{
				switch (message.level)
				{
					case Level::error:
					{
						out.set_color(out.internal, omode, nyc_red);
						print("   error" SEP);
						out.set_color(out.internal, omode, nyc_none);
						break;
					}
					case Level::warning:
					{
						out.set_color(out.internal, omode, nyc_yellow);
						print(" warning" SEP);
						out.set_color(out.internal, omode, nyc_none);
						break;
					}
					case Level::info:
					{
						if (message.section.empty())
						{
							out.set_color(out.internal, omode, nyc_none);
							print("        " SEP);
						}
						else
						{
							out.set_color(out.internal, omode, nyc_lightblue);
							xx = "        ";
							xx.overwriteRight(message.section);
							xx << SEP;
							print(xx);
							out.set_color(out.internal, omode, nyc_none);
						}
						break;
					}
					case Level::hint:
					case Level::suggest:
					{
						out.set_color(out.internal, omode, nyc_none);
						print("        " SEP);
						out.set_color(out.internal, omode, nyc_none);
						break;
					}
					case Level::success:
					{
						out.set_color(out.internal, omode, nyc_green);
						#ifndef YUNI_OS_WINDOWS
						print("      \u2713 " SEP);
						#else
						print("      ok" SEP);
						#endif
						out.set_color(out.internal, omode, nyc_none);
						break;
					}
					case Level::trace:
					{
						out.set_color(out.internal, omode, nyc_purple);
						print("      ::");
						out.set_color(out.internal, omode, nyc_none);
						print(SEP);
						break;
					}
					case Level::verbose:
					{
						out.set_color(out.internal, omode, nyc_green);
						print("      ::");
						out.set_color(out.internal, omode, nyc_none);
						print(SEP);
						break;
					}
					case Level::ICE:
					{
						out.set_color(out.internal, omode, nyc_red);
						print("     ICE" SEP);
						out.set_color(out.internal, omode, nyc_none);
						break;
					}
					case Level::none:
					{
						// unreachable - cf condition above
						break;
					}
				}

				if (indent)
				{
					xx.clear();
					for (uint32_t i = indent; i--; )
						xx << "    ";
					print(xx);
				}

				if (not message.prefix.empty())
				{
					out.set_color(out.internal, omode, nyc_white);
					print(message.prefix);
					out.set_color(out.internal, omode, nyc_none);
				}

				if (message.level == Level::suggest)
				{
					out.set_color(out.internal, omode, nyc_lightblue);
					print("suggest: ");
					out.set_color(out.internal, omode, nyc_none);
				}
				else if (message.level == Level::hint)
				{
					out.set_color(out.internal, omode, nyc_lightblue);
					print("hint: ");
					out.set_color(out.internal, omode, nyc_none);
				}

				if (not message.origins.location.target.empty())
				{
					print("{");
					print(message.origins.location.target);
					print("} ");
				}

				if (not message.origins.location.filename.empty())
				{
					out.set_color(out.internal, omode, nyc_white);
					xx = message.origins.location.filename;

					if (message.origins.location.pos.line > 0)
					{
						xx << ':';
						xx << message.origins.location.pos.line;
						if (message.origins.location.pos.offset != 0)
						{
							xx << ':';
							xx << message.origins.location.pos.offset;
							if (message.origins.location.pos.offsetEnd != 0)
							{
								xx << '-';
								xx << message.origins.location.pos.offsetEnd;
							}
						}
						print(xx);
					}
					xx << ": ";
					print(xx);
					out.set_color(out.internal, omode, nyc_none);
				}

				if (not message.message.empty())
				{
					String msg{message.message};
					msg.trimRight(" \t\r\n");
					msg.replace("\t", "    "); // tabs

					auto firstLF = msg.find('\n');
					if (not (firstLF < message.message.size()))
					{
						print(msg);
					}
					else
					{
						xx.clear() << "\n        " SEP;
						for (uint i = indent; i--; )
							xx.write("    ", 4);

						bool addLF = false;
						msg.words("\n", [&](const AnyString& word) -> bool
						{
							if (addLF)
								print(xx);
							print(word);
							addLF = true;
							return true;
						});
					}
				}

				print("\n");
				++indent;
			}

			for (auto& ptr: message.entries)
				printMessage<unify>(out, *ptr, indent, xx);
		}

	} // anonymous namespace

	void Message::print(nyconsole_t& out, bool unify)
	{
		assert(out.set_color);
		if (out.set_color and out.write_stderr and out.write_stdout)
		{
			String tmp;
			if (unify)
				printMessage<true>(out, *this, 0, tmp);
			else
				printMessage<false>(out, *this, 0, tmp);
		}
	}

} // namespace Logs
} // namespace Nany
<|endoftext|>


<commit_before>//
// Copyright 2012 Francisco Jerez
//
// 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.
//

#include "core/platform.hpp"

using namespace clover;

platform::platform() : adaptor_range(derefs(), devs) {
   int n = pipe_loader_probe(NULL, 0);
   std::vector<pipe_loader_device *> ldevs(n);

   pipe_loader_probe(&ldevs.front(), n);

   for (pipe_loader_device *ldev : ldevs) {
      try {
         devs.push_back(transfer(new device(*this, ldev)));
      } catch (error &) {}
   }
}
<commit_msg>clover: Fix memory leak when initializing a device object fails.<commit_after>//
// Copyright 2012 Francisco Jerez
//
// 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.
//

#include "core/platform.hpp"

using namespace clover;

platform::platform() : adaptor_range(derefs(), devs) {
   int n = pipe_loader_probe(NULL, 0);
   std::vector<pipe_loader_device *> ldevs(n);

   pipe_loader_probe(&ldevs.front(), n);

   for (pipe_loader_device *ldev : ldevs) {
      try {
         devs.push_back(transfer(new device(*this, ldev)));
      } catch (error &) {
         pipe_loader_release(&ldev, 1);
      }
   }
}
<|endoftext|>


<commit_before>#include "HashTable.hpp"
#include <math.h>

void array_swap(int &a, int &b);
void print_array(int *array, int n);
void unrank(int n, int r, int *array);

int main(int argc, char const *argv[]) {
    int n = atoi(argv[1]);
    int rank = atoi(argv[2]);
    int *array = new int[n];
    int *array2 = new int[n];

    //  Arreglo identidad
    for (unsigned i = 0; i < n; ++i) {
        array[i] = i;
        array2[i] = i;
    }

    std::cout << rank << ": ";
    unrank(n , rank, array2);
    print_array(array2, n);

    return 0;
}

void array_swap(int &a, int &b) {
    int temp;
    temp = b;
    b = a;
    a = temp;
}

void print_array(int *array, int n) {
    for (unsigned i = 0; i < n; ++i) {
        std::cout << array[i] << " ";
    }
    std::cout << std::endl;
}

void unrank(int n, int r, int *array) {
    if (n > 0) {
        array_swap(array[n-1], array[r % n]);
        unrank(n-1, floor(r/n), array);
    }
}
<commit_msg>Argumentos para test<commit_after>#include "HashTable.hpp"
#include <math.h>

void array_swap(int &a, int &b);
void print_array(int *array, int n);
void unrank(int n, int r, int *array);

int main(int argc, char const *argv[]) {
    if (argc != 3) {
        std::cout << "Wrong number of arguments: 2 expected." << std::endl;
        return -1;
    }
    int n = atoi(argv[1]);
    int rank = atoi(argv[2]);
    int *array = new int[n];
    int *array2 = new int[n];

    //  Arreglo identidad
    for (unsigned i = 0; i < n; ++i) {
        array[i] = i;
        array2[i] = i;
    }

    std::cout << rank << ": ";
    unrank(n , rank, array2);
    print_array(array2, n);

    return 0;
}

void array_swap(int &a, int &b) {
    int temp;
    temp = b;
    b = a;
    a = temp;
}

void print_array(int *array, int n) {
    for (unsigned i = 0; i < n; ++i) {
        std::cout << array[i] << " ";
    }
    std::cout << std::endl;
}

void unrank(int n, int r, int *array) {
    if (n > 0) {
        array_swap(array[n-1], array[r % n]);
        unrank(n-1, floor(r/n), array);
    }
}
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2015 Fondazione Istituto Italiano di Tecnologia
 * Authors: Silvio Traversaro
 * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
 *
 */

#include "testModels.h"
#include <iDynTree/Core/TestUtils.h>

#include <iDynTree/Core/Transform.h>
#include <iDynTree/Core/Position.h>
#include <iDynTree/Core/Twist.h>
#include <iDynTree/Core/SpatialAcc.h>
#include <iDynTree/Core/SpatialMomentum.h>
#include <iDynTree/Core/VectorDynSize.h>

#include <iDynTree/Core/EigenHelpers.h>

#include <iDynTree/KinDynComputations.h>
#include <iDynTree/Model/JointState.h>
#include <iDynTree/Model/FreeFloatingState.h>

using namespace iDynTree;

double random_double()
{
    return 0.0*((double)rand()-RAND_MAX/2)/((double)RAND_MAX);
}

double real_random_double()
{
    return 1.0*((double)rand()-RAND_MAX/2)/((double)RAND_MAX);
}

void setRandomState(iDynTree::KinDynComputations & dynComp)
{
    size_t dofs = dynComp.getNrOfDegreesOfFreedom();
    Transform    worldTbase;
    Twist        baseVel;
    Vector3 gravity;

    iDynTree::VectorDynSize qj(dofs), dqj(dofs), ddqj(dofs);

    worldTbase = //iDynTree::Transform::Identity();
    iDynTree::Transform(Rotation::RPY(random_double(),random_double(),random_double()),
            Position(random_double(),random_double(),random_double()));

    for(int i=0; i < 3; i++)
    {
        gravity(i) = random_double();
    }

    gravity(2) = 0.0;

    for(int i=0; i < 6; i++)
    {
        baseVel(i) = i; //real_random_double();
    }

    for(size_t dof=0; dof < dofs; dof++)

    {
        qj(dof) = random_double();
        dqj(dof) = random_double();
        ddqj(dof) = random_double();
    }

    bool ok = dynComp.setRobotState(worldTbase,qj,baseVel,dqj,gravity);

    iDynTree::VectorDynSize qj_read(dofs), dqj_read(dofs);
    Vector3 gravity_read;
    Transform    worldTbase_read;
    Twist        baseVel_read;
    dynComp.getRobotState(worldTbase_read, qj_read, baseVel_read, dqj_read, gravity_read);

    ASSERT_EQUAL_VECTOR(qj_read, qj);
    ASSERT_EQUAL_VECTOR(dqj_read, dqj);
    ASSERT_EQUAL_VECTOR(gravity_read, gravity);
    ASSERT_EQUAL_TRANSFORM(worldTbase_read, worldTbase);
    ASSERT_EQUAL_VECTOR(baseVel_read.asVector(), baseVel.asVector());

    ASSERT_EQUAL_DOUBLE(ok,true);
}

void testRelativeTransform(iDynTree::KinDynComputations & dynComp)
{
    using namespace iDynTree;

    size_t frames = dynComp.getNrOfFrames();

    // Create some frames for random consistency checks
    FrameIndex A = getRandomInteger(0,frames-1);
    FrameIndex B = getRandomInteger(0,frames-1);
    FrameIndex C = getRandomInteger(0,frames-1);
    FrameIndex D = getRandomInteger(0,frames-1);
    FrameIndex E = getRandomInteger(0,frames-1);
    FrameIndex F = getRandomInteger(0,frames-1);

    Transform A_B_X_E_F = dynComp.getRelativeTransformExplicit(A,B,C,D)*dynComp.getRelativeTransformExplicit(C,D,E,F);
    Transform A_B_X_E_F_check = dynComp.getRelativeTransformExplicit(A,B,E,F);
    Transform E_F_X_A_B = dynComp.getRelativeTransformExplicit(E,F,A,B);

    ASSERT_EQUAL_TRANSFORM(A_B_X_E_F,A_B_X_E_F_check);
    ASSERT_EQUAL_TRANSFORM(A_B_X_E_F,E_F_X_A_B.inverse());
}

void testAverageVelocityAndTotalMomentumJacobian(iDynTree::KinDynComputations & dynComp)
{
    iDynTree::Twist avgVel;
    iDynTree::SpatialMomentum mom;
    iDynTree::Vector6 avgVelCheck, momCheck;
    iDynTree::VectorDynSize nu(dynComp.getNrOfDegreesOfFreedom()+6);
    dynComp.getModelVel(nu);

    MomentumFreeFloatingJacobian momJac(dynComp.getRobotModel());
    FrameFreeFloatingJacobian    avgVelJac(dynComp.getRobotModel());

    avgVel = dynComp.getAverageVelocity();
    bool ok = dynComp.getAverageVelocityJacobian(avgVelJac);

    ASSERT_IS_TRUE(ok);

    mom = dynComp.getLinearAngularMomentum();
    ok = dynComp.getLinearAngularMomentumJacobian(momJac);

    ASSERT_IS_TRUE(ok);

    toEigen(momCheck) = toEigen(momJac)*toEigen(nu);
    toEigen(avgVelCheck) = toEigen(avgVelJac)*toEigen(nu);

    ASSERT_EQUAL_VECTOR(momCheck,mom.asVector());
    ASSERT_EQUAL_VECTOR(avgVelCheck,avgVel.asVector());
}

inline Eigen::VectorXd toEigen(const Vector6 & baseAcc, const VectorDynSize & jntAccs)
{
    Eigen::VectorXd concat(6+jntAccs.size());
    if( jntAccs.size() > 0 )
    {
        concat << toEigen(baseAcc), toEigen(jntAccs);
    }
    else
    {
        concat = toEigen(baseAcc);
    }
    return concat;
}

inline Eigen::VectorXd toEigen(const FreeFloatingGeneralizedTorques & genForces)
{
    Eigen::VectorXd concat(6+genForces.jointTorques().size());
    // TODO(traversaro) : We should teach toEigen to handle empty matrices correctly?
    // relevant: https://forum.kde.org/viewtopic.php?f=74&t=107974
    if( genForces.jointTorques().size() > 0 )
    {
        concat << toEigen(genForces.baseWrench()), toEigen(genForces.jointTorques());
    }
    else
    {
        concat = toEigen(genForces.baseWrench());
    }
    return concat;
}

// Test different ways of computing inverse dynamics
void testInverseDynamics(KinDynComputations & dynComp)
{
    int dofs = dynComp.getNrOfDegreesOfFreedom();
    iDynTree::Vector6 baseAcc;
    iDynTree::JointDOFsDoubleArray shapeAccs(dynComp.model());

    iDynTree::LinkNetExternalWrenches netExternalWrenches(dynComp.model());
    netExternalWrenches.zero();

    // Go component for component, for simplifyng debugging
    for(int i=0; i < 6+dofs; i++)
    {
        baseAcc.zero();
        shapeAccs.zero();
        if( i < 6 )
        {
            baseAcc(i) = 0.0;
        }
        else
        {
            shapeAccs(i-6) = 0.0;
        }

        FreeFloatingGeneralizedTorques invDynForces(dynComp.model());
        FreeFloatingGeneralizedTorques massMatrixInvDynForces(dynComp.model());

        // Run classical inverse dynamics
        bool ok = dynComp.inverseDynamics(baseAcc,shapeAccs,netExternalWrenches,invDynForces);
        ASSERT_IS_TRUE(ok);

        // Run inverse dynamics with mass matrix
        FreeFloatingMassMatrix massMatrix(dynComp.model());
        ok = dynComp.getFreeFloatingMassMatrix(massMatrix);
        ASSERT_IS_TRUE(ok);

        FreeFloatingGeneralizedTorques invDynBiasForces(dynComp.model());
        ok = dynComp.generalizedBiasForces(invDynBiasForces);
        ASSERT_IS_TRUE(ok);

        VectorDynSize massMatrixInvDynForcesContinuous(6+dofs);
        toEigen(massMatrixInvDynForcesContinuous) = toEigen(massMatrix)*toEigen(baseAcc,shapeAccs) + toEigen(invDynBiasForces);
        toEigen(massMatrixInvDynForces.baseWrench().getLinearVec3()) = toEigen(massMatrixInvDynForcesContinuous).segment<3>(0);
        toEigen(massMatrixInvDynForces.baseWrench().getAngularVec3()) = toEigen(massMatrixInvDynForcesContinuous).segment<3>(3);
        toEigen(massMatrixInvDynForces.jointTorques()) = toEigen(massMatrixInvDynForcesContinuous).segment(6,dofs);

        ASSERT_EQUAL_SPATIAL_FORCE(massMatrixInvDynForces.baseWrench(),invDynForces.baseWrench());
        ASSERT_EQUAL_VECTOR(massMatrixInvDynForces.jointTorques(),invDynForces.jointTorques());

    }
}

void testModelConsistency(std::string modelFilePath, const FrameVelocityRepresentation frameVelRepr)
{
    iDynTree::KinDynComputations dynComp;

    bool ok = dynComp.loadRobotModelFromFile(modelFilePath);
    ASSERT_IS_TRUE(ok);

    ok = dynComp.setFrameVelocityRepresentation(frameVelRepr);
    ASSERT_IS_TRUE(ok);

    for(int i=0; i < 5; i++)
    {
        setRandomState(dynComp);
        testRelativeTransform(dynComp);
        testAverageVelocityAndTotalMomentumJacobian(dynComp);
        testInverseDynamics(dynComp);
    }

}

int main()
{
    for(unsigned int mdl = 0; mdl < IDYNTREE_TESTS_URDFS_NR; mdl++ )
    {
        std::string urdfFileName = getAbsModelPath(std::string(IDYNTREE_TESTS_URDFS[mdl]));
        std::cout << "Testing file " << std::string(IDYNTREE_TESTS_URDFS[mdl]) <<  std::endl;
        testModelConsistency(urdfFileName,iDynTree::MIXED_REPRESENTATION);
        testModelConsistency(urdfFileName,iDynTree::BODY_FIXED_REPRESENTATION);
        testModelConsistency(urdfFileName,iDynTree::INERTIAL_FIXED_REPRESENTATION);
    }

    return EXIT_SUCCESS;
}
<commit_msg>Added tests for Relative Jacobians<commit_after>/*
 * Copyright (C) 2015 Fondazione Istituto Italiano di Tecnologia
 * Authors: Silvio Traversaro
 * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
 *
 */

#include "testModels.h"
#include <iDynTree/Core/TestUtils.h>

#include <iDynTree/Core/Transform.h>
#include <iDynTree/Core/Position.h>
#include <iDynTree/Core/Twist.h>
#include <iDynTree/Core/SpatialAcc.h>
#include <iDynTree/Core/SpatialMomentum.h>
#include <iDynTree/Core/VectorDynSize.h>

#include <iDynTree/Core/EigenHelpers.h>

#include <iDynTree/KinDynComputations.h>
#include <iDynTree/Model/JointState.h>
#include <iDynTree/Model/FreeFloatingState.h>

using namespace iDynTree;

double random_double()
{
    return 1.0*((double)rand()-RAND_MAX/2)/((double)RAND_MAX);
}

double real_random_double()
{
    return 1.0*((double)rand()-RAND_MAX/2)/((double)RAND_MAX);
}

int real_random_int(int initialValue, int finalValue)
{
    int length = finalValue - initialValue;
    return initialValue + rand() % length;
}

void setRandomState(iDynTree::KinDynComputations & dynComp)
{
    size_t dofs = dynComp.getNrOfDegreesOfFreedom();
    Transform    worldTbase;
    Twist        baseVel;
    Vector3 gravity;

    iDynTree::VectorDynSize qj(dofs), dqj(dofs), ddqj(dofs);

    worldTbase = //iDynTree::Transform::Identity();
    iDynTree::Transform(Rotation::RPY(random_double(),random_double(),random_double()),
            Position(random_double(),random_double(),random_double()));

    for(int i=0; i < 3; i++)
    {
        gravity(i) = random_double();
    }

    gravity(2) = 0.0;

    for(int i=0; i < 6; i++)
    {
        baseVel(i) = i; //real_random_double();
    }

    for(size_t dof=0; dof < dofs; dof++)

    {
        qj(dof) = random_double();
        dqj(dof) = random_double();
        ddqj(dof) = random_double();
    }

    bool ok = dynComp.setRobotState(worldTbase,qj,baseVel,dqj,gravity);

    iDynTree::VectorDynSize qj_read(dofs), dqj_read(dofs);
    Vector3 gravity_read;
    Transform    worldTbase_read;
    Twist        baseVel_read;
    dynComp.getRobotState(worldTbase_read, qj_read, baseVel_read, dqj_read, gravity_read);

    ASSERT_EQUAL_VECTOR(qj_read, qj);
    ASSERT_EQUAL_VECTOR(dqj_read, dqj);
    ASSERT_EQUAL_VECTOR(gravity_read, gravity);
    ASSERT_EQUAL_TRANSFORM(worldTbase_read, worldTbase);
    ASSERT_EQUAL_VECTOR(baseVel_read.asVector(), baseVel.asVector());

    ASSERT_EQUAL_DOUBLE(ok,true);
}

void testRelativeTransform(iDynTree::KinDynComputations & dynComp)
{
    using namespace iDynTree;

    size_t frames = dynComp.getNrOfFrames();

    // Create some frames for random consistency checks
    FrameIndex A = getRandomInteger(0,frames-1);
    FrameIndex B = getRandomInteger(0,frames-1);
    FrameIndex C = getRandomInteger(0,frames-1);
    FrameIndex D = getRandomInteger(0,frames-1);
    FrameIndex E = getRandomInteger(0,frames-1);
    FrameIndex F = getRandomInteger(0,frames-1);

    Transform A_B_X_E_F = dynComp.getRelativeTransformExplicit(A,B,C,D)*dynComp.getRelativeTransformExplicit(C,D,E,F);
    Transform A_B_X_E_F_check = dynComp.getRelativeTransformExplicit(A,B,E,F);
    Transform E_F_X_A_B = dynComp.getRelativeTransformExplicit(E,F,A,B);

    ASSERT_EQUAL_TRANSFORM(A_B_X_E_F,A_B_X_E_F_check);
    ASSERT_EQUAL_TRANSFORM(A_B_X_E_F,E_F_X_A_B.inverse());
}

void testAverageVelocityAndTotalMomentumJacobian(iDynTree::KinDynComputations & dynComp)
{
    iDynTree::Twist avgVel;
    iDynTree::SpatialMomentum mom;
    iDynTree::Vector6 avgVelCheck, momCheck;
    iDynTree::VectorDynSize nu(dynComp.getNrOfDegreesOfFreedom()+6);
    dynComp.getModelVel(nu);

    MomentumFreeFloatingJacobian momJac(dynComp.getRobotModel());
    FrameFreeFloatingJacobian    avgVelJac(dynComp.getRobotModel());

    avgVel = dynComp.getAverageVelocity();
    bool ok = dynComp.getAverageVelocityJacobian(avgVelJac);

    ASSERT_IS_TRUE(ok);

    mom = dynComp.getLinearAngularMomentum();
    ok = dynComp.getLinearAngularMomentumJacobian(momJac);

    ASSERT_IS_TRUE(ok);

    toEigen(momCheck) = toEigen(momJac)*toEigen(nu);
    toEigen(avgVelCheck) = toEigen(avgVelJac)*toEigen(nu);

    ASSERT_EQUAL_VECTOR(momCheck,mom.asVector());
    ASSERT_EQUAL_VECTOR(avgVelCheck,avgVel.asVector());
}

inline Eigen::VectorXd toEigen(const Vector6 & baseAcc, const VectorDynSize & jntAccs)
{
    Eigen::VectorXd concat(6+jntAccs.size());
    if( jntAccs.size() > 0 )
    {
        concat << toEigen(baseAcc), toEigen(jntAccs);
    }
    else
    {
        concat = toEigen(baseAcc);
    }
    return concat;
}

inline Eigen::VectorXd toEigen(const FreeFloatingGeneralizedTorques & genForces)
{
    Eigen::VectorXd concat(6+genForces.jointTorques().size());
    // TODO(traversaro) : We should teach toEigen to handle empty matrices correctly?
    // relevant: https://forum.kde.org/viewtopic.php?f=74&t=107974
    if( genForces.jointTorques().size() > 0 )
    {
        concat << toEigen(genForces.baseWrench()), toEigen(genForces.jointTorques());
    }
    else
    {
        concat = toEigen(genForces.baseWrench());
    }
    return concat;
}

// Test different ways of computing inverse dynamics
void testInverseDynamics(KinDynComputations & dynComp)
{
    int dofs = dynComp.getNrOfDegreesOfFreedom();
    iDynTree::Vector6 baseAcc;
    iDynTree::JointDOFsDoubleArray shapeAccs(dynComp.model());

    iDynTree::LinkNetExternalWrenches netExternalWrenches(dynComp.model());
    netExternalWrenches.zero();

    // Go component for component, for simplifyng debugging
    for(int i=0; i < 6+dofs; i++)
    {
        baseAcc.zero();
        shapeAccs.zero();
        if( i < 6 )
        {
            baseAcc(i) = 0.0;
        }
        else
        {
            shapeAccs(i-6) = 0.0;
        }

        FreeFloatingGeneralizedTorques invDynForces(dynComp.model());
        FreeFloatingGeneralizedTorques massMatrixInvDynForces(dynComp.model());

        // Run classical inverse dynamics
        bool ok = dynComp.inverseDynamics(baseAcc,shapeAccs,netExternalWrenches,invDynForces);
        ASSERT_IS_TRUE(ok);

        // Run inverse dynamics with mass matrix
        FreeFloatingMassMatrix massMatrix(dynComp.model());
        ok = dynComp.getFreeFloatingMassMatrix(massMatrix);
        ASSERT_IS_TRUE(ok);

        FreeFloatingGeneralizedTorques invDynBiasForces(dynComp.model());
        ok = dynComp.generalizedBiasForces(invDynBiasForces);
        ASSERT_IS_TRUE(ok);

        VectorDynSize massMatrixInvDynForcesContinuous(6+dofs);
        toEigen(massMatrixInvDynForcesContinuous) = toEigen(massMatrix)*toEigen(baseAcc,shapeAccs) + toEigen(invDynBiasForces);
        toEigen(massMatrixInvDynForces.baseWrench().getLinearVec3()) = toEigen(massMatrixInvDynForcesContinuous).segment<3>(0);
        toEigen(massMatrixInvDynForces.baseWrench().getAngularVec3()) = toEigen(massMatrixInvDynForcesContinuous).segment<3>(3);
        toEigen(massMatrixInvDynForces.jointTorques()) = toEigen(massMatrixInvDynForcesContinuous).segment(6,dofs);

        ASSERT_EQUAL_SPATIAL_FORCE(massMatrixInvDynForces.baseWrench(),invDynForces.baseWrench());
        ASSERT_EQUAL_VECTOR(massMatrixInvDynForces.jointTorques(),invDynForces.jointTorques());

    }
}

void testRelativeJacobians(KinDynComputations & dynComp)
{
    if (dynComp.getNrOfLinks() < 2) return;
    FrameIndex frame = -1;
    FrameIndex refFrame = -1;

    if (dynComp.getNrOfLinks() == 2) {
        frame = 0;
        refFrame = 1;
    } else {
        //Pick two frames at random
        frame = real_random_int(0, dynComp.getNrOfFrames());
        refFrame = -1;
        //be sure to pick two different frames
        do {
            refFrame = real_random_int(0, dynComp.getNrOfFrames());
        } while (refFrame == frame && frame >= 0);
    }

    FrameVelocityRepresentation representation = dynComp.getFrameVelocityRepresentation();
    dynComp.setFrameVelocityRepresentation(MIXED_REPRESENTATION);

    //Compute the relative Jacobian
    iDynTree::MatrixDynSize relativeJacobian(6, dynComp.getNrOfDegreesOfFreedom());
    dynComp.getRelativeJacobian(refFrame, frame, relativeJacobian);

    iDynTree::VectorDynSize qj(dynComp.getNrOfDegreesOfFreedom()), dqj(dynComp.getNrOfDegreesOfFreedom());
    Vector3 gravity;
    dynComp.getRobotState(qj, dqj, gravity);

    Twist relativeVel;
    Eigen::Matrix<double, 6, 1> relativeVelTemp;
    relativeVelTemp = toEigen(relativeJacobian) * toEigen(dqj);
    fromEigen(relativeVel, relativeVelTemp);

    //this velocity depends on where the Jacobian is expressed

    Twist frameVel = dynComp.getFrameVel(frame);
    Twist refFrameVel = dynComp.getFrameVel(refFrame);

    if (dynComp.getFrameVelocityRepresentation() == INERTIAL_FIXED_REPRESENTATION) {
        //Inertial = right trivialized.
        //frameVel is written wrt A
        //refFrameVel is written wrt A
        //relativeJacobian is written wrt refFrame
        Eigen::Matrix<double, 6, 1> temp = toEigen(dynComp.getWorldTransform(refFrame).asAdjointTransform()) * toEigen(relativeVel);
        fromEigen(relativeVel, temp);
    } else if (dynComp.getFrameVelocityRepresentation() == BODY_FIXED_REPRESENTATION) {
        //BODY = left trivialized.
        //frameVel is written wrt frame
        //refFrameVel is written wrt refFrame
        //relativeJacobian is written wrt frame
        //convert refFrameVel to frame
        Eigen::Matrix<double, 6, 1> temp = toEigen(dynComp.getRelativeTransform(frame, refFrame).asAdjointTransform()) * toEigen(refFrameVel);
        fromEigen(refFrameVel, temp);
    } else if (dynComp.getFrameVelocityRepresentation() == MIXED_REPRESENTATION) {
        //MIXED
        //frameVel is written wrt frame, [A]
        //refFrameVel is written wrt refFrame, [A]
        //relativeJacobian is written wrt frame, [refFrame]
        //convert refFrameVel to frame, [A]
        Transform frame_A_H_frame(dynComp.getWorldTransform(frame).getRotation(), Position::Zero());

        //refFrameVel = ref_[A]_v_ref, I want frame_[A]_v_ref. As I do not have an explicit A frame I do the following:
        // frame_[A]_H_frame_[frame] * frame_[frame]_H_ref_[frame] * ref_[frame]_H_ref_[A] * ref_[A]_v_ref
        Eigen::Matrix<double, 6, 1> temp = toEigen((frame_A_H_frame * dynComp.getRelativeTransformExplicit(frame, frame, refFrame, frame) * frame_A_H_frame.inverse()).asAdjointTransform()) * toEigen(refFrameVel);
        fromEigen(refFrameVel, temp);
        //and relativeVel to frame [A]
        temp = toEigen((frame_A_H_frame * dynComp.getRelativeTransformExplicit(frame, frame, frame, refFrame)).asAdjointTransform()) * toEigen(relativeVel);
        fromEigen(relativeVel, temp);
    }

    //now compute the error velocity
    //now they should be expressed in the same frame
    Twist velDifference = frameVel - refFrameVel;
    ASSERT_EQUAL_VECTOR(velDifference, relativeVel);

    dynComp.setFrameVelocityRepresentation(representation);
}

void testModelConsistency(std::string modelFilePath, const FrameVelocityRepresentation frameVelRepr)
{
    iDynTree::KinDynComputations dynComp;

    bool ok = dynComp.loadRobotModelFromFile(modelFilePath);
    ASSERT_IS_TRUE(ok);

    ok = dynComp.setFrameVelocityRepresentation(frameVelRepr);
    ASSERT_IS_TRUE(ok);

    for(int i=0; i < 5; i++)
    {
        setRandomState(dynComp);
        testRelativeTransform(dynComp);
        testAverageVelocityAndTotalMomentumJacobian(dynComp);
        testInverseDynamics(dynComp);
        testRelativeJacobians(dynComp);
    }

}

int main()
{
    for(unsigned int mdl = 0; mdl < IDYNTREE_TESTS_URDFS_NR; mdl++ )
    {
        std::string urdfFileName = getAbsModelPath(std::string(IDYNTREE_TESTS_URDFS[mdl]));
        std::cout << "Testing file " << std::string(IDYNTREE_TESTS_URDFS[mdl]) <<  std::endl;
        testModelConsistency(urdfFileName,iDynTree::MIXED_REPRESENTATION);
        testModelConsistency(urdfFileName,iDynTree::BODY_FIXED_REPRESENTATION);
        testModelConsistency(urdfFileName,iDynTree::INERTIAL_FIXED_REPRESENTATION);
    }

    return EXIT_SUCCESS;
}
<|endoftext|>


<commit_before>#include "tile.hxx"

#include <cstring>
#include <iostream>

namespace game_of_life
{

CoordRect::CoordRect() {}

CoordRect::CoordRect(coord_t r1, coord_t c1, coord_t r2, coord_t c2):
    r1(r1), c1(c1), r2(r2), c2(c2)
{}

size_t CoordRect::getHeight() const { return r2 - r1; }
size_t CoordRect::getWidth() const { return c2 - c1; }

bool CoordRect::contains(coord_t r, coord_t c) const
{ return (r >= r1 && r < r2) && (c >= c1 && c < c2); }

CoordRect CoordRect::shifted(coord_t dr, coord_t dc) const
{ return CoordRect(r1 + dr, c1 + dc, r2 + dr, c2 + dc); }

AbstractTile::AbstractTile()
{
    for (size_t i = 0; i < SIDE_COUNT; ++i)
        borders[i] = NULL;
    inner = NULL;
}

AbstractTile::~AbstractTile()
{
    for (size_t i = 0; i < SIDE_COUNT; ++i)
        delete borders[i];
    delete inner;
}

void AbstractTile::copyValues(const AbstractTile& t)
{
    size_t h = getHeight(), w = getWidth();
    for (size_t r = 0; r < h; ++r)
        for (size_t c = 0; c < w; ++c)
            set(r, c, t.at(r, c));
}

CoordRect AbstractTile::getBorderRect(Side s) const
{
    const coord_t h = getHeight();
    const coord_t w = getWidth();
    // borders are distributed like so:
    // NNNN
    // W  E
    // SSSS
    switch (s)
    {
    case SIDE_N: return CoordRect(0,   0,   1,   w);
    case SIDE_W: return CoordRect(1,   0,   h-1, 1);
    case SIDE_E: return CoordRect(1,   w-1, h-1, w);
    case SIDE_S: return CoordRect(h-1, 0,   h,   w);
    }
}

CoordRect AbstractTile::getInnerRect() const
{
    return CoordRect(1, 1, (coord_t)getHeight()-1, (coord_t)getWidth()-1);
}

Borders AbstractTile::getBorders()
{
    for (size_t i = 0; i < SIDE_COUNT; ++i)
        getBorder(static_cast<Side>(i));
    return borders;
}

Border AbstractTile::getBorder(Side s)
{
    return borders[s] ? borders[s] : borders[s] = makeSlice(getBorderRect(s));
}

TileView* AbstractTile::getInner()
{
    return inner ? inner : inner = makeSlice(getInnerRect());
}

TileView* AbstractTile::makeSlice(const CoordRect& r)
{
    return new TileView(this, r);
}

void AbstractTile::output(std::ostream& out) const
{
    out << getHeight() << " x " << getWidth() << std::endl;
    for (size_t i = 0; i < getHeight(); ++i)
        for (size_t j = 0; j < getWidth(); ++j)
            out << (at(i, j) ? 1 : 0) << " \n"[j+1 == getWidth()];
}

TileView::TileView()
{}

TileView::TileView(AbstractTile* viewed):
    viewed(viewed),
    window(0, 0, -10, -10)
{}

TileView::TileView(TileView* that):
    viewed(that->viewed),
    window(that->window)
{}

TileView::TileView(AbstractTile* viewed, const CoordRect& r):
    viewed(viewed),
    window(r)
{}

TileView& TileView::operator=(const TileView& that)
{
    viewed = that.viewed;
    window = that.window;
    return *this;
}

size_t TileView::getWidth() const
{
    return window.r2 != -10 ? window.getWidth() : viewed->getWidth();
}

size_t TileView::getHeight() const
{
    return window.r2 != -10 ? window.getHeight() : viewed->getHeight();
}

bool TileView::at(coord_t r, coord_t c) const
{ return viewed->at(window.r1 + r, window.c1 + c); }

void TileView::set(coord_t r, coord_t c, bool v)
{ return viewed->set(window.r1 + r, window.c1 + c, v); }

TileView* TileView::makeSlice(const CoordRect& reg)
{
    return new TileView(viewed, reg.shifted(window.r1, window.c1));
}

TorusView::TorusView(AbstractTile* viewed):
    TileView(viewed)
{}

bool TorusView::at(coord_t r, coord_t c) const
{
    return TileView::at(
            normalizeCoord(r, getHeight()),
            normalizeCoord(c, getWidth()));
}

void TorusView::set(coord_t r, coord_t c, bool v)
{ return TileView::set(normalizeRow(r), normalizeColumn(c), v); }

coord_t TorusView::normalizeRow(coord_t r) const
{ return normalizeCoord(r, getHeight()); }

coord_t TorusView::normalizeColumn(coord_t c) const
{ return normalizeCoord(c, getWidth()); }

coord_t TorusView::normalizeCoord(coord_t c, coord_t dimen)
{ return ((c % dimen) + dimen) % dimen; }

Matrix::Matrix()
{}

Matrix::Matrix(size_t height, size_t width):
    height(height),
    width(width),
    data(new bool[height * width])
{}

Matrix::Matrix(const AbstractTile& t):
    height(t.getHeight()),
    width(t.getWidth()),
    data(new bool[height * width])
{
    AbstractTile::copyValues(t);
}

Matrix::~Matrix() { delete[] data; }

size_t Matrix::getHeight() const { return height; }
size_t Matrix::getWidth() const { return width; }

bool Matrix::at(coord_t r, coord_t c) const { return data[r * getWidth() + c]; }

void Matrix::set(coord_t r, coord_t c, bool v) { data[r * getWidth() + c] = v; }

void Matrix::operator=(const Matrix& m)
{
    if (width != m.width || height != m.height)
    {
        width = m.width;
        height = m.height;
        delete[] data;
        data = new bool[width * height];
    }
    memcpy(data, m.data, height * width * sizeof(data[0]));
}

Matrix m(2, 2);
TileView t(&m);

} // namespace game_of_life

<commit_msg>Fix memsetting of new matrices<commit_after>#include "tile.hxx"

#include <cstring>
#include <iostream>

namespace game_of_life
{

CoordRect::CoordRect() {}

CoordRect::CoordRect(coord_t r1, coord_t c1, coord_t r2, coord_t c2):
    r1(r1), c1(c1), r2(r2), c2(c2)
{}

size_t CoordRect::getHeight() const { return r2 - r1; }
size_t CoordRect::getWidth() const { return c2 - c1; }

bool CoordRect::contains(coord_t r, coord_t c) const
{ return (r >= r1 && r < r2) && (c >= c1 && c < c2); }

CoordRect CoordRect::shifted(coord_t dr, coord_t dc) const
{ return CoordRect(r1 + dr, c1 + dc, r2 + dr, c2 + dc); }

AbstractTile::AbstractTile()
{
    for (size_t i = 0; i < SIDE_COUNT; ++i)
        borders[i] = NULL;
    inner = NULL;
}

AbstractTile::~AbstractTile()
{
    for (size_t i = 0; i < SIDE_COUNT; ++i)
        delete borders[i];
    delete inner;
}

void AbstractTile::copyValues(const AbstractTile& t)
{
    size_t h = getHeight(), w = getWidth();
    for (size_t r = 0; r < h; ++r)
        for (size_t c = 0; c < w; ++c)
            set(r, c, t.at(r, c));
}

CoordRect AbstractTile::getBorderRect(Side s) const
{
    const coord_t h = getHeight();
    const coord_t w = getWidth();
    // borders are distributed like so:
    // NNNN
    // W  E
    // SSSS
    switch (s)
    {
    case SIDE_N: return CoordRect(0,   0,   1,   w);
    case SIDE_W: return CoordRect(1,   0,   h-1, 1);
    case SIDE_E: return CoordRect(1,   w-1, h-1, w);
    case SIDE_S: return CoordRect(h-1, 0,   h,   w);
    }
}

CoordRect AbstractTile::getInnerRect() const
{
    return CoordRect(1, 1, (coord_t)getHeight()-1, (coord_t)getWidth()-1);
}

Borders AbstractTile::getBorders()
{
    for (size_t i = 0; i < SIDE_COUNT; ++i)
        getBorder(static_cast<Side>(i));
    return borders;
}

Border AbstractTile::getBorder(Side s)
{
    return borders[s] ? borders[s] : borders[s] = makeSlice(getBorderRect(s));
}

TileView* AbstractTile::getInner()
{
    return inner ? inner : inner = makeSlice(getInnerRect());
}

TileView* AbstractTile::makeSlice(const CoordRect& r)
{
    return new TileView(this, r);
}

void AbstractTile::output(std::ostream& out) const
{
    out << getHeight() << " x " << getWidth() << std::endl;
    for (size_t i = 0; i < getHeight(); ++i)
        for (size_t j = 0; j < getWidth(); ++j)
            out << (at(i, j) ? 1 : 0) << " \n"[j+1 == getWidth()];
}

TileView::TileView()
{}

TileView::TileView(AbstractTile* viewed):
    viewed(viewed),
    window(0, 0, -10, -10)
{}

TileView::TileView(TileView* that):
    viewed(that->viewed),
    window(that->window)
{}

TileView::TileView(AbstractTile* viewed, const CoordRect& r):
    viewed(viewed),
    window(r)
{}

TileView& TileView::operator=(const TileView& that)
{
    viewed = that.viewed;
    window = that.window;
    return *this;
}

size_t TileView::getWidth() const
{
    return window.r2 != -10 ? window.getWidth() : viewed->getWidth();
}

size_t TileView::getHeight() const
{
    return window.r2 != -10 ? window.getHeight() : viewed->getHeight();
}

bool TileView::at(coord_t r, coord_t c) const
{ return viewed->at(window.r1 + r, window.c1 + c); }

void TileView::set(coord_t r, coord_t c, bool v)
{ return viewed->set(window.r1 + r, window.c1 + c, v); }

TileView* TileView::makeSlice(const CoordRect& reg)
{
    return new TileView(viewed, reg.shifted(window.r1, window.c1));
}

TorusView::TorusView(AbstractTile* viewed):
    TileView(viewed)
{}

bool TorusView::at(coord_t r, coord_t c) const
{
    return TileView::at(
            normalizeCoord(r, getHeight()),
            normalizeCoord(c, getWidth()));
}

void TorusView::set(coord_t r, coord_t c, bool v)
{ return TileView::set(normalizeRow(r), normalizeColumn(c), v); }

coord_t TorusView::normalizeRow(coord_t r) const
{ return normalizeCoord(r, getHeight()); }

coord_t TorusView::normalizeColumn(coord_t c) const
{ return normalizeCoord(c, getWidth()); }

coord_t TorusView::normalizeCoord(coord_t c, coord_t dimen)
{ return ((c % dimen) + dimen) % dimen; }

Matrix::Matrix()
{}

Matrix::Matrix(size_t height, size_t width):
    height(height),
    width(width),
    data(new bool[height * width])
{
    memset(data, 0, height * width * sizeof(bool));
}

Matrix::Matrix(const AbstractTile& t):
    height(t.getHeight()),
    width(t.getWidth()),
    data(new bool[height * width])
{
    AbstractTile::copyValues(t);
}

Matrix::~Matrix() { delete[] data; }

size_t Matrix::getHeight() const { return height; }
size_t Matrix::getWidth() const { return width; }

bool Matrix::at(coord_t r, coord_t c) const { return data[r * getWidth() + c]; }

void Matrix::set(coord_t r, coord_t c, bool v) { data[r * getWidth() + c] = v; }

void Matrix::operator=(const Matrix& m)
{
    if (width != m.width || height != m.height)
    {
        width = m.width;
        height = m.height;
        delete[] data;
        data = new bool[width * height];
    }
    memcpy(data, m.data, height * width * sizeof(data[0]));
}

Matrix m(2, 2);
TileView t(&m);

} // namespace game_of_life

<|endoftext|>


<commit_before>#include "tool.hpp"

#include <regex>
#include <stdexcept>

#include "paths.hpp"

namespace caliber {

namespace {

  struct tool_def {
    std::regex match;
    std::vector<std::string> parents;
  };

  std::vector<std::string> get_identity(const std::string &name)  {
    static std::vector<tool_def> known = {
      { std::regex(R"/(clang\+\+-\d\.\d)/"), {"clang++", "c++"} },
      { std::regex(R"/(clang-\d\.\d)/"),     {"clang",   "cc" } },
      { std::regex(R"/(g\+\+-\d\.\d)/"),     {"g++",     "c++"} },
      { std::regex(R"/(gcc-\d\.\d)/"),       {"gcc",     "cc" } }
    };

    for(const auto &i : known) {
      if(std::regex_match(name, i.match)) {
        std::vector<std::string> result = {name};
        result.insert(result.end(), i.parents.begin(), i.parents.end());
        return result;
      }
    }
    return {name};
  }

  std::string tool_name(const std::string &filename) {
    std::unique_ptr<char, void (*)(void*)> linkname(
      realpath(which(filename).c_str(), nullptr),
      std::free
    );
    if(!linkname)
      throw std::system_error(errno, std::system_category());

    return leafname(linkname.get());
  }
}

tool::tool(std::string filename)
  : path(std::move(filename)), identity(get_identity(tool_name(path))) {}

std::vector<std::string>
translate_args(const std::string &file, const compiler_options &args,
               const std::string &base_path) {
  // XXX: This will eventually need to support different compiler front-ends.
  std::vector<std::string> result;
  for(const auto &arg : args) {
    if(arg.string_key == "std") {
      result.push_back("-std=" + arg.value.front());
    }
    else if(arg.string_key == "-I") {
      result.push_back("-I");
      result.push_back(base_path + arg.value.front());
    }
    else {
      result.push_back(arg.string_key);
      result.insert(result.end(), arg.value.begin(), arg.value.end());
    }
  }

  result.insert(result.end(), {"-fsyntax-only", file});
  return result;
}

} // namespace caliber
<commit_msg>Be more-lenient with inferring compiler types<commit_after>#include "tool.hpp"

#include <regex>
#include <stdexcept>

#include "paths.hpp"

namespace caliber {

namespace {

  struct tool_def {
    std::regex match;
    std::vector<std::string> parents;
  };

  std::vector<std::string> get_identity(const std::string &name)  {
    static std::vector<tool_def> known = {
      { std::regex(R"/(.*clang\+\+(-\d(\.\d)?)?)/"), {"clang++", "c++"} },
      { std::regex(R"/(.*clang(-\d(\.\d)?)?)/"),     {"clang",   "cc" } },
      { std::regex(R"/(.*g\+\+(-\d(\.\d)?)?)/"),     {"g++",     "c++"} },
      { std::regex(R"/(.*gcc(-\d(\.\d)?)?)/"),       {"gcc",     "cc" } }
    };

    for(const auto &i : known) {
      if(std::regex_match(name, i.match)) {
        std::vector<std::string> result = {name};
        result.insert(result.end(), i.parents.begin(), i.parents.end());
        return result;
      }
    }
    return {name};
  }

  std::string tool_name(const std::string &filename) {
    std::unique_ptr<char, void (*)(void*)> linkname(
      realpath(which(filename).c_str(), nullptr),
      std::free
    );
    if(!linkname)
      throw std::system_error(errno, std::system_category());

    return leafname(linkname.get());
  }
}

tool::tool(std::string filename)
  : path(std::move(filename)), identity(get_identity(tool_name(path))) {}

std::vector<std::string>
translate_args(const std::string &file, const compiler_options &args,
               const std::string &base_path) {
  // XXX: This will eventually need to support different compiler front-ends.
  std::vector<std::string> result;
  for(const auto &arg : args) {
    if(arg.string_key == "std") {
      result.push_back("-std=" + arg.value.front());
    }
    else if(arg.string_key == "-I") {
      result.push_back("-I");
      result.push_back(base_path + arg.value.front());
    }
    else {
      result.push_back(arg.string_key);
      result.insert(result.end(), arg.value.begin(), arg.value.end());
    }
  }

  result.insert(result.end(), {"-fsyntax-only", file});
  return result;
}

} // namespace caliber
<|endoftext|>


<commit_before>#include "unit.h"

#include "segment.h"

TUnit::TUnit(const TSegment& pivot, TSegments&& segments)
    : Pivot(pivot)
    , Segments(std::move(segments)) {}

TUnit TUnit::Move(EMoveOperations operation) const {
    if (IsSlideOperation(operation)) {
        return Slide(operation);
    }
    else if (IsRotateOperation(operation)) {
        return Rotate(operation);
    }
    else {
        // throw
    }

    return Clone();
}

TUnit TUnit::Slide(EMoveOperations direction) const {
    TSegment newPivot = Pivot.Slide(direction);
    return TUnit(newPivot, SlideSegments(direction));
}

TUnit::TSegments TUnit::SlideSegments(EMoveOperations direction) const {
    TSegments ret;
    ret.reserve(Segments.size());
    for (const TSegment& segment : Segments) {
        TSegment slidedSegment = segment.Slide(direction);
        ret.push_back(slidedSegment);
    }

    return ret;
}

TUnit TUnit::Rotate(EMoveOperations direction) const {
    return TUnit(Pivot, RotateSegments(direction));
}

TUnit::TSegments TUnit::RotateSegments(EMoveOperations direction) const {
    TSegments ret;
    ret.reserve(Segments.size());
    for (const TSegment& segment : Segments) {
        TSegment rotatedSegment = segment.RotateAround(Pivot, direction);
        ret.push_back(rotatedSegment);
    }

    return ret;
}

TUnit TUnit::Clone() const {
    return TUnit(Pivot, TSegments(Segments));
}
<commit_msg>added exception throw for invalid movement operation<commit_after>#include "unit.h"

#include "exception.h"
#include "segment.h"

TUnit::TUnit(const TSegment& pivot, TSegments&& segments)
    : Pivot(pivot)
    , Segments(std::move(segments)) {}

TUnit TUnit::Move(EMoveOperations operation) const {
    if (IsSlideOperation(operation)) {
        return Slide(operation);
    }
    else if (IsRotateOperation(operation)) {
        return Rotate(operation);
    }
    else {
        throw TException("Invalid move operation recieved")
            << __FILE__ << ":" << __LINE__;
    }

    return Clone();
}

TUnit TUnit::Slide(EMoveOperations direction) const {
    TSegment newPivot = Pivot.Slide(direction);
    return TUnit(newPivot, SlideSegments(direction));
}

TUnit::TSegments TUnit::SlideSegments(EMoveOperations direction) const {
    TSegments ret;
    ret.reserve(Segments.size());
    for (const TSegment& segment : Segments) {
        TSegment slidedSegment = segment.Slide(direction);
        ret.push_back(slidedSegment);
    }

    return ret;
}

TUnit TUnit::Rotate(EMoveOperations direction) const {
    return TUnit(Pivot, RotateSegments(direction));
}

TUnit::TSegments TUnit::RotateSegments(EMoveOperations direction) const {
    TSegments ret;
    ret.reserve(Segments.size());
    for (const TSegment& segment : Segments) {
        TSegment rotatedSegment = segment.RotateAround(Pivot, direction);
        ret.push_back(rotatedSegment);
    }

    return ret;
}

TUnit TUnit::Clone() const {
    return TUnit(Pivot, TSegments(Segments));
}
<|endoftext|>


<commit_before>#include "util.h"
#include <cmath>

namespace nt
{

void setGLFrustum(GLdouble fovy, GLdouble aspect, GLdouble zNear, GLdouble zFar)
{
	GLdouble xmin, xmax, ymin, ymax;

	ymax = zNear * tan(fovy * M_PI / 360.0);
	ymin = -ymax;
	xmin = ymin * aspect;
	xmax = ymax * aspect;

	glFrustum(xmin, xmax, ymin, ymax, zNear, zFar);
}

void glVertex(const QVector3D& vec)
{
	glVertex3d(vec.x(), vec.y(), vec.z());
}

void glNormal(const QVector3D& vec)
{
	glNormal3d(vec.x(), vec.y(), vec.z());
}

}
<commit_msg>FOV affects the horizontal view angle<commit_after>#include "util.h"
#include <cmath>

namespace nt
{

void setGLFrustum(GLdouble fovy, GLdouble aspect, GLdouble zNear, GLdouble zFar)
{
	GLdouble xmin, xmax, ymin, ymax;

	xmax = zNear * tan(fovy * M_PI / 360.0);
	xmin = -xmax;
	ymin = xmin / aspect;
	ymax = xmax / aspect;

	glFrustum(xmin, xmax, ymin, ymax, zNear, zFar);
}

void glVertex(const QVector3D& vec)
{
	glVertex3d(vec.x(), vec.y(), vec.z());
}

void glNormal(const QVector3D& vec)
{
	glNormal3d(vec.x(), vec.y(), vec.z());
}

}
<|endoftext|>


<commit_before>
#include <stdlib.h>
#include <time.h>

#include <cstdio>
#include <iostream>
#include <thread>
#include <mutex>
#include <vector>
#include <string>
#include <condition_variable>

using std::vector;
using std::string;
using std::condition_variable;

constexpr int kMaxMS = 20;

int randomN() {
    int rn = std::rand() % kMaxMS;
    return rn+1;
}

template<typename T>
class Queue {
    vector<T> buffer;
    int head;  // points to the next place to dump
    int tail;  // points to the oldest item
    int size, capacity;

    // protects access to common state above.
    mutable std::mutex mut;

    // signaled when queue went from being full to non-full, to unblock waiting producers.
    condition_variable cv_on_consumer;

    // signaled when queue went from empty to non-empty, to unblock waiting consumers.
    condition_variable cv_on_producer;

  public:
    Queue(int cap) : capacity(cap) {
        buffer = vector<T>(capacity, T());
        head = 0;
        tail = 0;
        size = 0;
    }

    bool isFull() const {
        std::lock_guard<std::mutex> lg(mut);
        return size == capacity;
    }

    bool isEmpty() const {
        std::lock_guard<std::mutex> lg(mut);
        return size == 0;
    }

    void addElem(const T& elem) {
        bool should_notify = false;
        {
            std::unique_lock<std::mutex> ul(mut);
            while (size == capacity) {
                std::cout << "*** Thread: " << std::this_thread::get_id() << " waiting on consumer." << std::endl;
                cv_on_consumer.wait(ul);
            }

            buffer[head] = elem;
            head = (head+1) % capacity;
            if (size == 0) should_notify = true;
            size++;
        }
        if (should_notify) cv_on_producer.notify_one();
    }

    void getElem(T& elem) {
        bool should_notify = false;
        {
            std::unique_lock<std::mutex> ul(mut);
            while (size == 0) {
                std::cout << "*** Thread: " << std::this_thread::get_id() << " waiting on producer." << std::endl;
                cv_on_producer.wait(ul);
            }

            elem = buffer[tail];
            tail = (tail+1) % capacity;
            if (size == capacity) should_notify = true;
            size--;
        }
        if (should_notify) cv_on_consumer.notify_one();
    }
};

template <typename T>
class Producer {
    int id=0;
    Queue<T>& queue;
    int limit=0;
  public:

    Producer(Queue<T>& q, int i, int lim) : queue(q), id(i), limit(lim) {
    }

    bool operator()() {
        for (int i=1; i<=limit; i++) {
            // sleep simulates time to produce the next item
            std::this_thread::sleep_for(std::chrono::milliseconds(randomN()));

            string item = "[P " + std::to_string(id) + "; Item " + std::to_string(i) + "]";
            queue.addElem(item);
            std::cout << "Enqueing: " << item << std::endl;
        }
        return true;
    }
};

template <typename T>
class Consumer {
    int id=0;
    Queue<T>& queue;
    int limit=0;
  public:

    Consumer(Queue<T>& q, int i, int lim) : queue(q), id(i), limit(lim) {
    }

    bool operator()() {
        for (int i=1; i<=limit; i++) {
            string item;
            queue.getElem(item);
            std::cout << "        C: " << id <<  "; Dequeing: " << item << std::endl;

            // sleep simulates doing something with the item
            std::this_thread::sleep_for(std::chrono::milliseconds(randomN()));
        }
        return true;
    }
};

int main() {
    Queue<string> buf(10);
    int kNumProducers = 5;
    int kNumConsumers = 5;
    int kLimit=10;

    std::vector<std::thread> producers, consumers;
    for (int i=0; i<kNumProducers; i++) {
        producers.push_back(std::thread(Producer<string>(std::ref(buf), i+1, kLimit)));
    }
    for (int i=0; i<kNumConsumers; i++) {
        consumers.push_back(std::thread(Consumer<string>(std::ref(buf), i+1, kLimit)));
    }
    for (auto& t : producers) {
       t.join();
    }
    for (auto& t : consumers) {
       t.join();
    }
}

<commit_msg>[MT] Using cond vars. Fixed problem.<commit_after>
#include <stdlib.h>
#include <time.h>

#include <cstdio>
#include <iostream>
#include <thread>
#include <mutex>
#include <vector>
#include <string>
#include <condition_variable>

using std::vector;
using std::string;
using std::condition_variable;

constexpr int kMaxMS = 20;

int randomN() {
    int rn = std::rand() % kMaxMS;
    return rn+1;
}

template<typename T>
class Queue {
    vector<T> buffer;
    int head;  // points to the next place to dump
    int tail;  // points to the oldest item
    int size, capacity;

    // protects access to common state above.
    mutable std::mutex mut;

    // signaled when queue went from being full to non-full, to unblock waiting producers.
    condition_variable cv_on_consumer;

    // signaled when queue went from empty to non-empty, to unblock waiting consumers.
    condition_variable cv_on_producer;

  public:
    Queue(int cap) : capacity(cap) {
        buffer = vector<T>(capacity, T());
        head = 0;
        tail = 0;
        size = 0;
    }

    bool isFull() const {
        std::lock_guard<std::mutex> lg(mut);
        return size == capacity;
    }

    bool isEmpty() const {
        std::lock_guard<std::mutex> lg(mut);
        return size == 0;
    }

    void addElem(const T& elem) {
        bool should_notify = false;
        {
            std::unique_lock<std::mutex> ul(mut);
            while (size == capacity) {
                std::cout << "*** Thread: " << std::this_thread::get_id() << " waiting on consumer." << std::endl;
                cv_on_consumer.wait(ul);
            }

            buffer[head] = elem;
            head = (head+1) % capacity;
            if (size == 0) should_notify = true;
            size++;
        }
        if (should_notify) cv_on_producer.notify_all();
    }

    void getElem(T& elem) {
        bool should_notify = false;
        {
            std::unique_lock<std::mutex> ul(mut);
            while (size == 0) {
                std::cout << "*** Thread: " << std::this_thread::get_id() << " waiting on producer." << std::endl;
                cv_on_producer.wait(ul);
            }

            elem = buffer[tail];
            tail = (tail+1) % capacity;
            if (size == capacity) should_notify = true;
            size--;
        }
        if (should_notify) cv_on_consumer.notify_all();
    }
};

template <typename T>
class Producer {
    int id=0;
    Queue<T>& queue;
    int limit=0;
  public:

    Producer(Queue<T>& q, int i, int lim) : queue(q), id(i), limit(lim) {
    }

    bool operator()() {
        for (int i=1; i<=limit; i++) {
            // sleep simulates time to produce the next item
            std::this_thread::sleep_for(std::chrono::milliseconds(randomN()));

            string item = "[P " + std::to_string(id) + "; Item " + std::to_string(i) + "]";
            queue.addElem(item);
            std::cout << "Enqueing: " << item << std::endl;
        }
        return true;
    }
};

template <typename T>
class Consumer {
    int id=0;
    Queue<T>& queue;
    int limit=0;
  public:

    Consumer(Queue<T>& q, int i, int lim) : queue(q), id(i), limit(lim) {
    }

    bool operator()() {
        for (int i=1; i<=limit; i++) {
            string item;
            queue.getElem(item);
            std::cout << "        C: " << id <<  "; Dequeing: " << item << std::endl;

            // sleep simulates doing something with the item
            std::this_thread::sleep_for(std::chrono::milliseconds(randomN()));
        }
        return true;
    }
};

int main() {
    Queue<string> buf(10);
    int kNumProducers = 1000;
    int kNumConsumers = 1000;
    int kLimit=500;  // number of items produced by each

    std::vector<std::thread> producers, consumers;
    for (int i=0; i<kNumProducers; i++) {
        producers.push_back(std::thread(Producer<string>(std::ref(buf), i+1, kLimit)));
    }
    for (int i=0; i<kNumConsumers; i++) {
        consumers.push_back(std::thread(Consumer<string>(std::ref(buf), i+1, kLimit)));
    }
    for (auto& t : producers) {
       t.join();
    }
    for (auto& t : consumers) {
       t.join();
    }
}

<|endoftext|>


<commit_before>/*! @file util.hpp
    @brief Common functions
*/
#pragma once
#ifndef LIKELIGRID_UTIL_HPP_
#define LIKELIGRID_UTIL_HPP_

#include <boost/math/distributions/chi_squared.hpp>

#include <wtl/iostr.hpp>
#include <wtl/numeric.hpp>

#include <string>
#include <vector>
#include <valarray>
#include <array>
#include <map>
#include <iterator>

namespace likeligrid {

constexpr std::array<double, 6> STEPS = {{0.32, 0.16, 0.08, 0.04, 0.02, 0.01}};
constexpr std::array<size_t, 6> BREAKS = {{5, 5, 5, 5, 5, 5}};

inline std::vector<std::valarray<double>>
make_vicinity(const std::valarray<double>& center, const size_t breaks, const double radius, const double max=2.001) {
    std::vector<std::valarray<double>> axes;
    axes.reserve(center.size());
    for (const double x: center) {
        auto axis = wtl::lin_spaced(breaks, x + radius, x - radius);
        // grid precision = 0.01
        axis = (axis * 100.0).apply(std::round) / 100.0;
        const std::valarray<bool> positive = axis > 0.0;
        axes.emplace_back(axis[positive & (axis < max)]);
    }
    return axes;
}

inline size_t guess_stage(const double step) {
    const auto it = std::find_if(STEPS.begin(), STEPS.end(), wtl::approx(step));
    if (it == STEPS.end()) throw std::runtime_error("invalid step size");
    return static_cast<size_t>(it - STEPS.begin());
}

inline double radius(const size_t stage) {
    return (BREAKS.at(stage) - 1u) * STEPS.at(stage) * 0.5;
}

inline std::tuple<std::string, size_t, size_t, double>
read_metadata(std::istream& ist) {
    std::string buffer;
    ist.ignore(std::numeric_limits<std::streamsize>::max(), '=');
    std::getline(ist, buffer);
    const std::string genotype_file = buffer;
    ist.ignore(std::numeric_limits<std::streamsize>::max(), '=');
    std::getline(ist, buffer);
    const size_t max_sites = std::stoul(buffer);
    ist.ignore(std::numeric_limits<std::streamsize>::max(), '=');
    std::getline(ist, buffer);
    const size_t max_count = std::stoul(buffer);
    ist.ignore(std::numeric_limits<std::streamsize>::max(), '=');
    std::getline(ist, buffer);
    const double step = std::stod(buffer);
    return std::make_tuple(genotype_file, max_sites, max_count, step);
}

inline double d2_from_neutral(std::valarray<double> v) {
    v -= 1.0;
    v *= v;
    return v.sum();
}

inline double d2_from_neutral(const std::vector<double>& v) {
    double d = 0.0;
    for (auto x: v) {
        x -= 1.0;
        x *= x;
        d += x;
    }
    return d;
}

inline std::tuple<size_t, std::vector<std::string>, std::valarray<double>>
read_body(std::istream& ist) {
    std::string buffer;
    ist >> buffer; // loglik
    std::getline(ist, buffer); // header
    buffer.erase(0u, 1u); // \t
    const std::vector<std::string> colnames = wtl::split(buffer, "\t");
    size_t nrow = 0u;
    double max_ll = std::numeric_limits<double>::lowest();
    std::vector<double> mle;
    while (std::getline(ist, buffer)) {
        ++nrow;
        std::istringstream iss(buffer);
        std::istream_iterator<double> it(iss);
        if (wtl::approx(*it, max_ll)) {
            std::vector<double> chalenger(++it, std::istream_iterator<double>());
            if (d2_from_neutral(chalenger) < d2_from_neutral(mle)) {
                max_ll = *it;
                mle.swap(chalenger);
            }
        } else if (*it > max_ll) {
            max_ll = *it;
            mle.assign(++it, std::istream_iterator<double>());
        }
    }
    std::valarray<double> mle_params(mle.data(), mle.size());
    return std::make_tuple(nrow, colnames, mle_params);
}

inline std::valarray<double>
read_loglik(std::istream& ist, const size_t nrow) {
    std::valarray<double> values(nrow);
    for (size_t i=0u; i<5u; ++i) {
        ist.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
    }
    for (size_t i=0u; i<nrow; ++i) {
        ist >> values[i];
        ist.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
    }
    return values;
}

} // namespace likeligrid

#endif // LIKELIGRID_UTIL_HPP_
<commit_msg>:arrow_up: wtl::approx()<commit_after>/*! @file util.hpp
    @brief Common functions
*/
#pragma once
#ifndef LIKELIGRID_UTIL_HPP_
#define LIKELIGRID_UTIL_HPP_

#include <boost/math/distributions/chi_squared.hpp>

#include <wtl/iostr.hpp>
#include <wtl/numeric.hpp>

#include <string>
#include <vector>
#include <valarray>
#include <array>
#include <map>
#include <iterator>

namespace likeligrid {

constexpr std::array<double, 6> STEPS = {{0.32, 0.16, 0.08, 0.04, 0.02, 0.01}};
constexpr std::array<size_t, 6> BREAKS = {{5, 5, 5, 5, 5, 5}};

inline std::vector<std::valarray<double>>
make_vicinity(const std::valarray<double>& center, const size_t breaks, const double radius, const double max=2.001) {
    std::vector<std::valarray<double>> axes;
    axes.reserve(center.size());
    for (const double x: center) {
        auto axis = wtl::lin_spaced(breaks, x + radius, x - radius);
        // grid precision = 0.01
        axis = (axis * 100.0).apply(std::round) / 100.0;
        const std::valarray<bool> positive = axis > 0.0;
        axes.emplace_back(axis[positive & (axis < max)]);
    }
    return axes;
}

inline size_t guess_stage(const double step) {
    const auto it = std::find_if(STEPS.begin(), STEPS.end(), wtl::Approx(step));
    if (it == STEPS.end()) throw std::runtime_error("invalid step size");
    return static_cast<size_t>(it - STEPS.begin());
}

inline double radius(const size_t stage) {
    return (BREAKS.at(stage) - 1u) * STEPS.at(stage) * 0.5;
}

inline std::tuple<std::string, size_t, size_t, double>
read_metadata(std::istream& ist) {
    std::string buffer;
    ist.ignore(std::numeric_limits<std::streamsize>::max(), '=');
    std::getline(ist, buffer);
    const std::string genotype_file = buffer;
    ist.ignore(std::numeric_limits<std::streamsize>::max(), '=');
    std::getline(ist, buffer);
    const size_t max_sites = std::stoul(buffer);
    ist.ignore(std::numeric_limits<std::streamsize>::max(), '=');
    std::getline(ist, buffer);
    const size_t max_count = std::stoul(buffer);
    ist.ignore(std::numeric_limits<std::streamsize>::max(), '=');
    std::getline(ist, buffer);
    const double step = std::stod(buffer);
    return std::make_tuple(genotype_file, max_sites, max_count, step);
}

inline double d2_from_neutral(std::valarray<double> v) {
    v -= 1.0;
    v *= v;
    return v.sum();
}

inline double d2_from_neutral(const std::vector<double>& v) {
    double d = 0.0;
    for (auto x: v) {
        x -= 1.0;
        x *= x;
        d += x;
    }
    return d;
}

inline std::tuple<size_t, std::vector<std::string>, std::valarray<double>>
read_body(std::istream& ist) {
    std::string buffer;
    ist >> buffer; // loglik
    std::getline(ist, buffer); // header
    buffer.erase(0u, 1u); // \t
    const std::vector<std::string> colnames = wtl::split(buffer, "\t");
    size_t nrow = 0u;
    double max_ll = std::numeric_limits<double>::lowest();
    std::vector<double> mle;
    while (std::getline(ist, buffer)) {
        ++nrow;
        std::istringstream iss(buffer);
        std::istream_iterator<double> it(iss);
        if (wtl::approx(*it, max_ll)) {
            std::vector<double> chalenger(++it, std::istream_iterator<double>());
            if (d2_from_neutral(chalenger) < d2_from_neutral(mle)) {
                max_ll = *it;
                mle.swap(chalenger);
            }
        } else if (*it > max_ll) {
            max_ll = *it;
            mle.assign(++it, std::istream_iterator<double>());
        }
    }
    std::valarray<double> mle_params(mle.data(), mle.size());
    return std::make_tuple(nrow, colnames, mle_params);
}

inline std::valarray<double>
read_loglik(std::istream& ist, const size_t nrow) {
    std::valarray<double> values(nrow);
    for (size_t i=0u; i<5u; ++i) {
        ist.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
    }
    for (size_t i=0u; i<nrow; ++i) {
        ist >> values[i];
        ist.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
    }
    return values;
}

} // namespace likeligrid

#endif // LIKELIGRID_UTIL_HPP_
<|endoftext|>