<commit_before>/*
 * bacteria-core, core for cellular automaton
 * Copyright (C) 2016 Pavel Dolgov
 *
 * See the LICENSE file for terms of use.
 */

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

#include "Model.hpp"

typedef int (Implementation::Model::*IntOneArgMethod) (
    const Abstract::Point& coordinates
) const;

typedef void (Implementation::Model::*OneArgMethod) (
    const Abstract::Point& coordinates
);

typedef void (Implementation::Model::*TwoArgsMethod) (
    const Abstract::Point& coordinates,
    int change
);

typedef void (Implementation::Model::*MultiArgsMethod) (
    const Abstract::Point& coordinates,
    int mass,
    int direction,
    int team,
    int instruction
);

template<typename Func>
static void checkOneArgMethod(
    Implementation::Model* model,
    Func model_method,
    int arg1,
    int arg2
) {
    Abstract::Point coordinates(arg1, arg2);
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(coordinates), Exception
    );
}

template<typename Func>
void checkModelMethodForThrow(
    Implementation::Model* model,
    Func model_method,
    int arg1,
    int arg2
) {
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(arg1, arg2), Exception
    );
}

template<>
void checkModelMethodForThrow<IntOneArgMethod>(
    Implementation::Model* model,
    IntOneArgMethod model_method,
    int arg1,
    int arg2
) {
    checkOneArgMethod(model, model_method, arg1, arg2);
}

template<>
void checkModelMethodForThrow<OneArgMethod>(
    Implementation::Model* model,
    OneArgMethod model_method,
    int arg1,
    int arg2
) {
    checkOneArgMethod(model, model_method, arg1, arg2);
}

template<>
void checkModelMethodForThrow<TwoArgsMethod>(
    Implementation::Model* model,
    TwoArgsMethod model_method,
    int arg1,
    int arg2
) {
    Abstract::Point coordinates(arg1, arg2);
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(coordinates, 0), Exception
    );
}

template<>
void checkModelMethodForThrow<MultiArgsMethod>(
    Implementation::Model* model,
    MultiArgsMethod model_method,
    int arg1,
    int arg2
) {
    Abstract::Point coordinates(arg1, arg2);
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(
            coordinates,
            DEFAULT_MASS,
            0,
            0,
            0
        ),
        Exception
    );
}

// "dead" test: attempt to do something with dead bacterium
template<typename Func>
static void deadTest(
    Implementation::Model* model,
    Func model_method
) {
    model->kill(0, 0);
    checkModelMethodForThrow(
        model,
        model_method,
        0,
        0
    );
}

template<typename Func>
static void checkErrorHandling(
    Implementation::Model* model,
    Func model_method,
    bool dead_test
) {
    // Range errors: test all combinations of
    // "wrong" (outside of correct range) arguments.
    // This solution works for coordinates and non-coordinates
    // methods of Model. (< 0, = 0, > MAX)
    int wrong_args[] = {-1, 0, MIN_WIDTH};
    BOOST_FOREACH (int arg1, wrong_args) {
        BOOST_FOREACH (int arg2, wrong_args) {
            if ((arg1 != 0) || (arg2 != 0)) {
                // (0, 0) is correct
                checkModelMethodForThrow(
                    model,
                    model_method,
                    arg1,
                    arg2
                );
            }
        }
    }
    if (dead_test) {
        // "dead" error
        // (attempt to do something with dead bacterium)
        deadTest(model, model_method);
    }
}

static Abstract::Point createInBaseCoordinates(
    Implementation::Model* model
) {
    Abstract::Point coordinates(0, 0);
    model->createNewByCoordinates(
        coordinates,
        DEFAULT_MASS,
        0,
        0,
        0
    );
    return coordinates;
}

static Implementation::Model* createBaseModel(
    int bacteria = 0,
    int teams = 1
) {
    Implementation::Model* model =
        Abstract::makeModel<Implementation::Model>(
            MIN_WIDTH,
            MIN_HEIGHT,
            bacteria,
            teams
        );
    return model;
}

BOOST_AUTO_TEST_CASE (get_team_coordinates_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    int team = model->getTeamByCoordinates(coordinates);
    BOOST_REQUIRE(team == 0);
    checkErrorHandling<IntOneArgMethod>(
        model,
        &Implementation::Model::getTeamByCoordinates,
        true
    );
    delete model;
}

BOOST_AUTO_TEST_CASE (width_test) {
    Implementation::Model* model = createBaseModel();
    BOOST_REQUIRE(model->getWidth() == MIN_WIDTH);
    delete model;
}

BOOST_AUTO_TEST_CASE (height_test) {
    Implementation::Model* model = createBaseModel();
    BOOST_REQUIRE(model->getHeight() == MIN_HEIGHT);
    delete model;
}

BOOST_AUTO_TEST_CASE (bacteria_number_test) {
    Implementation::Model* model = createBaseModel();
    int bacteria_number = model->getBacteriaNumber(0);
    BOOST_REQUIRE(bacteria_number == 0);
    createInBaseCoordinates(model);
    bacteria_number = model->getBacteriaNumber(0);
    BOOST_REQUIRE(bacteria_number == 1);
    // range errors
    BOOST_REQUIRE_THROW(model->getBacteriaNumber(-1), Exception);
    BOOST_REQUIRE_THROW(model->getBacteriaNumber(1), Exception);
    delete model;
}

BOOST_AUTO_TEST_CASE (is_alive_test) {
    Implementation::Model* model = createBaseModel(1, 1);
    checkErrorHandling(
        model,
        &Implementation::Model::isAlive,
        false
    );
    BOOST_REQUIRE(model->isAlive(0, 0) == true);
    model->kill(0, 0);
    BOOST_REQUIRE(model->isAlive(0, 0) == false);
    delete model;
}

BOOST_AUTO_TEST_CASE (get_instruction_test) {
    Implementation::Model* model = createBaseModel(1, 1);
    int instruction = model->getInstruction(0, 0);
    BOOST_REQUIRE(instruction == 0);
    checkErrorHandling(
        model,
        &Implementation::Model::getInstruction,
        true
    );
    delete model;
}

BOOST_AUTO_TEST_CASE (get_coordinates_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    Abstract::Point derived_coordinates = model->getCoordinates(0, 0);
    BOOST_REQUIRE(derived_coordinates == coordinates);
    checkErrorHandling(
        model,
        &Implementation::Model::getCoordinates,
        true
    );
    delete model;
}

BOOST_AUTO_TEST_CASE (get_direction_test) {
    Implementation::Model* model = createBaseModel();
    createInBaseCoordinates(model);
    int direction = model->getDirection(0, 0);
    BOOST_REQUIRE(direction == Abstract::LEFT);
    checkErrorHandling(
        model,
        &Implementation::Model::getDirection,
        true
    );
    delete model;
}

BOOST_AUTO_TEST_CASE (get_mass_test) {
    Implementation::Model* model = createBaseModel(1, 1);
    int mass = model->getMass(0, 0);
    BOOST_REQUIRE(mass == DEFAULT_MASS);
    checkErrorHandling(
        model,
        &Implementation::Model::getMass,
        true
    );
    delete model;
}

BOOST_AUTO_TEST_CASE (kill_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    model->kill(0, 0);
    Abstract::CellState state = model->cellState(coordinates);
    BOOST_REQUIRE(state == Abstract::EMPTY);
    BOOST_REQUIRE(model->isAlive(0, 0) == false);
    // error handling checks
    createInBaseCoordinates(model);
    // FIXME test doesn't work correctly without this function call.
    // The solution is to use set instead of vector in model.
    model->clearBeforeMove(0);
    checkErrorHandling(
        model,
        &Implementation::Model::kill,
        true
    );
    delete model;
}

BOOST_AUTO_TEST_CASE (kill_coordinates_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    model->killByCoordinates(coordinates);
    Abstract::CellState state = model->cellState(coordinates);
    BOOST_REQUIRE(state == Abstract::EMPTY);
    BOOST_REQUIRE(model->isAlive(0, 0) == false);
    // error handling checks
    createInBaseCoordinates(model);
    model->clearBeforeMove(0);
    checkErrorHandling<OneArgMethod>(
        model,
        &Implementation::Model::killByCoordinates,
        true
    );
    delete model;
}

BOOST_AUTO_TEST_CASE (change_mass_coordinates_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    int test_val = 1;
    model->changeMassByCoordinates(coordinates, test_val);
    int new_mass = model->getMassByCoordinates(coordinates);
    BOOST_REQUIRE(new_mass == (DEFAULT_MASS + test_val));
    model->changeMassByCoordinates(coordinates, -test_val);
    new_mass = model->getMassByCoordinates(coordinates);
    BOOST_REQUIRE(new_mass == DEFAULT_MASS);
    checkErrorHandling<TwoArgsMethod>(
        model,
        &Implementation::Model::changeMassByCoordinates,
        true
    );
    delete model;
}

BOOST_AUTO_TEST_CASE (create_coordinates_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    Abstract::CellState state = model->cellState(coordinates);
    BOOST_REQUIRE(state == Abstract::BACTERIUM);
    checkErrorHandling<MultiArgsMethod>(
        model,
        &Implementation::Model::createNewByCoordinates,
        false
    );
    delete model;
}
<commit_msg>Model tests: remove FIXME<commit_after>/*
 * bacteria-core, core for cellular automaton
 * Copyright (C) 2016 Pavel Dolgov
 *
 * See the LICENSE file for terms of use.
 */

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

#include "Model.hpp"

typedef int (Implementation::Model::*IntOneArgMethod) (
    const Abstract::Point& coordinates
) const;

typedef void (Implementation::Model::*OneArgMethod) (
    const Abstract::Point& coordinates
);

typedef void (Implementation::Model::*TwoArgsMethod) (
    const Abstract::Point& coordinates,
    int change
);

typedef void (Implementation::Model::*MultiArgsMethod) (
    const Abstract::Point& coordinates,
    int mass,
    int direction,
    int team,
    int instruction
);

template<typename Func>
static void checkOneArgMethod(
    Implementation::Model* model,
    Func model_method,
    int arg1,
    int arg2
) {
    Abstract::Point coordinates(arg1, arg2);
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(coordinates), Exception
    );
}

template<typename Func>
void checkModelMethodForThrow(
    Implementation::Model* model,
    Func model_method,
    int arg1,
    int arg2
) {
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(arg1, arg2), Exception
    );
}

template<>
void checkModelMethodForThrow<IntOneArgMethod>(
    Implementation::Model* model,
    IntOneArgMethod model_method,
    int arg1,
    int arg2
) {
    checkOneArgMethod(model, model_method, arg1, arg2);
}

template<>
void checkModelMethodForThrow<OneArgMethod>(
    Implementation::Model* model,
    OneArgMethod model_method,
    int arg1,
    int arg2
) {
    checkOneArgMethod(model, model_method, arg1, arg2);
}

template<>
void checkModelMethodForThrow<TwoArgsMethod>(
    Implementation::Model* model,
    TwoArgsMethod model_method,
    int arg1,
    int arg2
) {
    Abstract::Point coordinates(arg1, arg2);
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(coordinates, 0), Exception
    );
}

template<>
void checkModelMethodForThrow<MultiArgsMethod>(
    Implementation::Model* model,
    MultiArgsMethod model_method,
    int arg1,
    int arg2
) {
    Abstract::Point coordinates(arg1, arg2);
    BOOST_REQUIRE_THROW(
        ((*model).*model_method)(
            coordinates,
            DEFAULT_MASS,
            0,
            0,
            0
        ),
        Exception
    );
}

// "dead" test: attempt to do something with dead bacterium
template<typename Func>
static void deadTest(
    Implementation::Model* model,
    Func model_method
) {
    model->kill(0, 0);
    checkModelMethodForThrow(
        model,
        model_method,
        0,
        0
    );
}

template<typename Func>
static void checkErrorHandling(
    Implementation::Model* model,
    Func model_method,
    bool dead_test
) {
    // Range errors: test all combinations of
    // "wrong" (outside of correct range) arguments.
    // This solution works for coordinates and non-coordinates
    // methods of Model. (< 0, = 0, > MAX)
    int wrong_args[] = {-1, 0, MIN_WIDTH};
    BOOST_FOREACH (int arg1, wrong_args) {
        BOOST_FOREACH (int arg2, wrong_args) {
            if ((arg1 != 0) || (arg2 != 0)) {
                // (0, 0) is correct
                checkModelMethodForThrow(
                    model,
                    model_method,
                    arg1,
                    arg2
                );
            }
        }
    }
    if (dead_test) {
        // "dead" error
        // (attempt to do something with dead bacterium)
        deadTest(model, model_method);
    }
}

static Abstract::Point createInBaseCoordinates(
    Implementation::Model* model
) {
    Abstract::Point coordinates(0, 0);
    model->createNewByCoordinates(
        coordinates,
        DEFAULT_MASS,
        0,
        0,
        0
    );
    return coordinates;
}

static Implementation::Model* createBaseModel(
    int bacteria = 0,
    int teams = 1
) {
    Implementation::Model* model =
        Abstract::makeModel<Implementation::Model>(
            MIN_WIDTH,
            MIN_HEIGHT,
            bacteria,
            teams
        );
    return model;
}

BOOST_AUTO_TEST_CASE (get_team_coordinates_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    int team = model->getTeamByCoordinates(coordinates);
    BOOST_REQUIRE(team == 0);
    checkErrorHandling<IntOneArgMethod>(
        model,
        &Implementation::Model::getTeamByCoordinates,
        true
    );
    delete model;
}

BOOST_AUTO_TEST_CASE (width_test) {
    Implementation::Model* model = createBaseModel();
    BOOST_REQUIRE(model->getWidth() == MIN_WIDTH);
    delete model;
}

BOOST_AUTO_TEST_CASE (height_test) {
    Implementation::Model* model = createBaseModel();
    BOOST_REQUIRE(model->getHeight() == MIN_HEIGHT);
    delete model;
}

BOOST_AUTO_TEST_CASE (bacteria_number_test) {
    Implementation::Model* model = createBaseModel();
    int bacteria_number = model->getBacteriaNumber(0);
    BOOST_REQUIRE(bacteria_number == 0);
    createInBaseCoordinates(model);
    bacteria_number = model->getBacteriaNumber(0);
    BOOST_REQUIRE(bacteria_number == 1);
    // range errors
    BOOST_REQUIRE_THROW(model->getBacteriaNumber(-1), Exception);
    BOOST_REQUIRE_THROW(model->getBacteriaNumber(1), Exception);
    delete model;
}

BOOST_AUTO_TEST_CASE (is_alive_test) {
    Implementation::Model* model = createBaseModel(1, 1);
    checkErrorHandling(
        model,
        &Implementation::Model::isAlive,
        false
    );
    BOOST_REQUIRE(model->isAlive(0, 0) == true);
    model->kill(0, 0);
    BOOST_REQUIRE(model->isAlive(0, 0) == false);
    delete model;
}

BOOST_AUTO_TEST_CASE (get_instruction_test) {
    Implementation::Model* model = createBaseModel(1, 1);
    int instruction = model->getInstruction(0, 0);
    BOOST_REQUIRE(instruction == 0);
    checkErrorHandling(
        model,
        &Implementation::Model::getInstruction,
        true
    );
    delete model;
}

BOOST_AUTO_TEST_CASE (get_coordinates_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    Abstract::Point derived_coordinates = model->getCoordinates(0, 0);
    BOOST_REQUIRE(derived_coordinates == coordinates);
    checkErrorHandling(
        model,
        &Implementation::Model::getCoordinates,
        true
    );
    delete model;
}

BOOST_AUTO_TEST_CASE (get_direction_test) {
    Implementation::Model* model = createBaseModel();
    createInBaseCoordinates(model);
    int direction = model->getDirection(0, 0);
    BOOST_REQUIRE(direction == Abstract::LEFT);
    checkErrorHandling(
        model,
        &Implementation::Model::getDirection,
        true
    );
    delete model;
}

BOOST_AUTO_TEST_CASE (get_mass_test) {
    Implementation::Model* model = createBaseModel(1, 1);
    int mass = model->getMass(0, 0);
    BOOST_REQUIRE(mass == DEFAULT_MASS);
    checkErrorHandling(
        model,
        &Implementation::Model::getMass,
        true
    );
    delete model;
}

BOOST_AUTO_TEST_CASE (kill_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    model->kill(0, 0);
    Abstract::CellState state = model->cellState(coordinates);
    BOOST_REQUIRE(state == Abstract::EMPTY);
    BOOST_REQUIRE(model->isAlive(0, 0) == false);
    // error handling checks
    createInBaseCoordinates(model);
    model->clearBeforeMove(0);
    checkErrorHandling(
        model,
        &Implementation::Model::kill,
        true
    );
    delete model;
}

BOOST_AUTO_TEST_CASE (kill_coordinates_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    model->killByCoordinates(coordinates);
    Abstract::CellState state = model->cellState(coordinates);
    BOOST_REQUIRE(state == Abstract::EMPTY);
    BOOST_REQUIRE(model->isAlive(0, 0) == false);
    // error handling checks
    createInBaseCoordinates(model);
    model->clearBeforeMove(0);
    checkErrorHandling<OneArgMethod>(
        model,
        &Implementation::Model::killByCoordinates,
        true
    );
    delete model;
}

BOOST_AUTO_TEST_CASE (change_mass_coordinates_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    int test_val = 1;
    model->changeMassByCoordinates(coordinates, test_val);
    int new_mass = model->getMassByCoordinates(coordinates);
    BOOST_REQUIRE(new_mass == (DEFAULT_MASS + test_val));
    model->changeMassByCoordinates(coordinates, -test_val);
    new_mass = model->getMassByCoordinates(coordinates);
    BOOST_REQUIRE(new_mass == DEFAULT_MASS);
    checkErrorHandling<TwoArgsMethod>(
        model,
        &Implementation::Model::changeMassByCoordinates,
        true
    );
    delete model;
}

BOOST_AUTO_TEST_CASE (create_coordinates_test) {
    Implementation::Model* model = createBaseModel();
    Abstract::Point coordinates = createInBaseCoordinates(model);
    Abstract::CellState state = model->cellState(coordinates);
    BOOST_REQUIRE(state == Abstract::BACTERIUM);
    checkErrorHandling<MultiArgsMethod>(
        model,
        &Implementation::Model::createNewByCoordinates,
        false
    );
    delete model;
}
<|endoftext|>


<commit_before>/*******************************************************************************
*    ALMA - Atacama Large Millimiter Array
*    (c) Associated Universities Inc., 2002 *
*    (c) European Southern Observatory, 2002
*    Copyright by ESO (in the framework of the ALMA collaboration)
*    and Cosylab 2002, All rights reserved
*
*    This library is free software; you can redistribute it and/or
*    modify it under the terms of the GNU Lesser General Public
*    License as published by the Free Software Foundation; either
*    version 2.1 of the License, or (at your option) any later version.
*
*    This library is distributed in the hope that it will be useful,
*    but WITHOUT ANY WARRANTY; without even the implied warranty of
*    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
*    Lesser General Public License for more details.
*
*    You should have received a copy of the GNU Lesser General Public
*    License along with this library; if not, write to the Free Software
*    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307  USA
*
*
*
* "@(#) $Id: contLogTestImpl.cpp,v 1.8 2008/01/09 10:11:38 eallaert Exp $"
*
* who       when        what
* --------  ----------  ----------------------------------------------
* eallaert  2007-11-05  initial version
*
*/

#include <contLogTestImpl.h>
#include <ACSErrTypeCommon.h>
#include <loggingLogLevelDefinition.h>
#include <loggingLogger.h>
#include "loggingGetLogger.h"
#include <iostream>

ACE_RCSID(contLogTest, contLogTestImpl, "$Id: contLogTestImpl.cpp,v 1.8 2008/01/09 10:11:38 eallaert Exp $")

/* ----------------------------------------------------------------*/
TestLogLevelsComp::TestLogLevelsComp(
		       const ACE_CString &name,
		       maci::ContainerServices * containerServices) :
    ACSComponentImpl(name, containerServices)
{
    // ACS_TRACE is used for debugging purposes
    ACS_TRACE("::TestLogLevelsComp::TestLogLevelsComp");
}
/* ----------------------------------------------------------------*/
TestLogLevelsComp::~TestLogLevelsComp()
{
    // ACS_TRACE is used for debugging purposes
    ACS_TRACE("::TestLogLevelsComp::~TestLogLevelsComp");
    ACS_DEBUG_PARAM("::TestLogLevelsComp::~TestLogLevelsComp", "Destroying %s...", name());
}
/* --------------------- [ CORBA interface ] ----------------------*/
::contLogTest::LongSeq*
TestLogLevelsComp::getLevels ()
    throw (CORBA::SystemException, ACSErrTypeCommon::CouldntPerformActionEx)
{
    Logging::Logger *l = getLogger();
    ::contLogTest::LongSeq_var level = new ::contLogTest::LongSeq(5);
	level->length(5);

    // need the equivalent of Java's logConfig.getMinLogLevel() etc. in C++
    for (int i = 0; i <5 ; i++)
    	level[i] = static_cast< CORBA::Long >(i);

    level[3] = static_cast< CORBA::Long >(l->getRemoteLevel());
    level[4] = static_cast< CORBA::Long >(l->getLocalLevel());
    return level._retn();
}

void
TestLogLevelsComp::logDummyMessages (const ::contLogTest::LongSeq & levels)
{
	ACE_Log_Priority p;
	CORBA::ULong t=0;
	for (t=0; t<levels.length(); t++){
		p = LogLevelDefinition::getACELogPriority(levels[t]);
		LogLevelDefinition lld = LogLevelDefinition::fromInteger(levels[t]);
		ACS_SHORT_LOG((p, "dummy log message for core level %d/%s", lld.getValue(), lld.getName().c_str()));
	}
	// log last message always at highest, non-OFF level (so it should get always through,
	// unless the central level is put to OFF).
	p = LogLevelDefinition::getACELogPriority(levels[levels.length()-2]);
	ACS_SHORT_LOG((p, "===last log messsage==="));

}

/* --------------- [ MACI DLL support functions ] -----------------*/
#include <maciACSComponentDefines.h>
MACI_DLL_SUPPORT_FUNCTIONS(TestLogLevelsComp)
/* ----------------------------------------------------------------*/

/*___oOo___*/

<commit_msg>Corrected type in "last log message" within logDummyMessages() method. This lead to this last message not being recognised as such, and a timeout in LogSeriesExpectant.java<commit_after>/*******************************************************************************
*    ALMA - Atacama Large Millimiter Array
*    (c) Associated Universities Inc., 2002 *
*    (c) European Southern Observatory, 2002
*    Copyright by ESO (in the framework of the ALMA collaboration)
*    and Cosylab 2002, All rights reserved
*
*    This library is free software; you can redistribute it and/or
*    modify it under the terms of the GNU Lesser General Public
*    License as published by the Free Software Foundation; either
*    version 2.1 of the License, or (at your option) any later version.
*
*    This library is distributed in the hope that it will be useful,
*    but WITHOUT ANY WARRANTY; without even the implied warranty of
*    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
*    Lesser General Public License for more details.
*
*    You should have received a copy of the GNU Lesser General Public
*    License along with this library; if not, write to the Free Software
*    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307  USA
*
*
*
* "@(#) $Id: contLogTestImpl.cpp,v 1.9 2008/01/10 09:32:20 eallaert Exp $"
*
* who       when        what
* --------  ----------  ----------------------------------------------
* eallaert  2007-11-05  initial version
*
*/

#include <contLogTestImpl.h>
#include <ACSErrTypeCommon.h>
#include <loggingLogLevelDefinition.h>
#include <loggingLogger.h>
#include "loggingGetLogger.h"
#include <iostream>

ACE_RCSID(contLogTest, contLogTestImpl, "$Id: contLogTestImpl.cpp,v 1.9 2008/01/10 09:32:20 eallaert Exp $")

/* ----------------------------------------------------------------*/
TestLogLevelsComp::TestLogLevelsComp(
		       const ACE_CString &name,
		       maci::ContainerServices * containerServices) :
    ACSComponentImpl(name, containerServices)
{
    // ACS_TRACE is used for debugging purposes
    ACS_TRACE("::TestLogLevelsComp::TestLogLevelsComp");
}
/* ----------------------------------------------------------------*/
TestLogLevelsComp::~TestLogLevelsComp()
{
    // ACS_TRACE is used for debugging purposes
    ACS_TRACE("::TestLogLevelsComp::~TestLogLevelsComp");
    ACS_DEBUG_PARAM("::TestLogLevelsComp::~TestLogLevelsComp", "Destroying %s...", name());
}
/* --------------------- [ CORBA interface ] ----------------------*/
::contLogTest::LongSeq*
TestLogLevelsComp::getLevels ()
    throw (CORBA::SystemException, ACSErrTypeCommon::CouldntPerformActionEx)
{
    Logging::Logger *l = getLogger();
    ::contLogTest::LongSeq_var level = new ::contLogTest::LongSeq(5);
	level->length(5);

    // need the equivalent of Java's logConfig.getMinLogLevel() etc. in C++
    for (int i = 0; i <5 ; i++)
    	level[i] = static_cast< CORBA::Long >(i);

    level[3] = static_cast< CORBA::Long >(l->getRemoteLevel());
    level[4] = static_cast< CORBA::Long >(l->getLocalLevel());
    return level._retn();
}

void
TestLogLevelsComp::logDummyMessages (const ::contLogTest::LongSeq & levels)
{
	ACE_Log_Priority p;
	CORBA::ULong t=0;
	for (t=0; t<levels.length(); t++){
		p = LogLevelDefinition::getACELogPriority(levels[t]);
		LogLevelDefinition lld = LogLevelDefinition::fromInteger(levels[t]);
		ACS_SHORT_LOG((p, "dummy log message for core level %d/%s", lld.getValue(), lld.getName().c_str()));
	}
	// log last message always at highest, non-OFF level (so it should get always through,
	// unless the central level is put to OFF).
	p = LogLevelDefinition::getACELogPriority(AcsLogLevels::EMERGENCY_VAL);
	ACS_SHORT_LOG((p, "===last log message==="));

}

/* --------------- [ MACI DLL support functions ] -----------------*/
#include <maciACSComponentDefines.h>
MACI_DLL_SUPPORT_FUNCTIONS(TestLogLevelsComp)
/* ----------------------------------------------------------------*/

/*___oOo___*/

<|endoftext|>


<commit_before>/*
    Copyright 2011 Google Inc.

    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

         http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.
 */

#include "GrTesselatedPathRenderer.h"

#include "GrMemory.h"
#include "GrPathUtils.h"

#include <internal_glu.h>

struct PolygonData {
    PolygonData(GrTDArray<GrPoint>* vertices, GrTDArray<short>* indices)
      : fVertices(vertices)
      , fIndices(indices)
    {
    }
    GrTDArray<GrPoint>* fVertices;
    GrTDArray<short>* fIndices;
};

static void beginData(GLenum type, void* data)
{
    GR_DEBUGASSERT(type == GL_TRIANGLES);
}

static void edgeFlagData(GLboolean flag, void* data)
{
}

static void vertexData(void* vertexData, void* data)
{
    short* end = static_cast<PolygonData*>(data)->fIndices->append();
    *end = reinterpret_cast<long>(vertexData);
}

static void endData(void* data)
{
}

static void combineData(GLdouble coords[3], void* vertexData[4],
                                 GLfloat weight[4], void **outData, void* data)
{
    PolygonData* polygonData = static_cast<PolygonData*>(data);
    int index = polygonData->fVertices->count();
    *polygonData->fVertices->append() = GrPoint::Make(static_cast<float>(coords[0]),
                                                      static_cast<float>(coords[1]));
    *outData = reinterpret_cast<void*>(index);
}

typedef void (*TESSCB)();

static unsigned fill_type_to_glu_winding_rule(GrPathFill fill) {
    switch (fill) {
        case kWinding_PathFill:
            return GLU_TESS_WINDING_NONZERO;
        case kEvenOdd_PathFill:
            return GLU_TESS_WINDING_ODD;
        case kInverseWinding_PathFill:
            return GLU_TESS_WINDING_POSITIVE;
        case kInverseEvenOdd_PathFill:
            return GLU_TESS_WINDING_ODD;
        case kHairLine_PathFill:
            return GLU_TESS_WINDING_NONZERO;  // FIXME:  handle this
        default:
            GrAssert(!"Unknown path fill!");
            return 0;
    }
}

GrTesselatedPathRenderer::GrTesselatedPathRenderer() {
}

class Edge {
  public:
    Edge() {}
    Edge(float x, float y, float z) : fX(x), fY(y), fZ(z) {}
    GrPoint intersect(const Edge& other) {
        return GrPoint::Make(
            (fY * other.fZ - other.fY * fZ) / (fX * other.fY - other.fX * fY),
            (fX * other.fZ - other.fX * fZ) / (other.fX * fY - fX * other.fY));
    }
    float fX, fY, fZ;
};

typedef GrTDArray<Edge> EdgeArray;

bool isCCW(const GrPoint* v)
{
    GrVec v1 = v[1] - v[0];
    GrVec v2 = v[2] - v[1];
    return v1.cross(v2) < 0;
}

static size_t computeEdgesAndOffsetVertices(const GrMatrix& matrix,
                                            const GrMatrix& inverse,
                                            GrPoint* vertices,
                                            size_t numVertices,
                                            EdgeArray* edges)
{
    GrPoint p = vertices[numVertices - 1];
    matrix.mapPoints(&p, 1);
    float sign = isCCW(vertices) ? -1.0f : 1.0f;
    for (size_t i = 0; i < numVertices; ++i) {
        GrPoint q = vertices[i];
        matrix.mapPoints(&q, 1);
        if (p == q) continue;
        GrVec tangent = GrVec::Make(p.fY - q.fY, q.fX - p.fX);
        float scale = sign / tangent.length();
        float cross2 = p.fX * q.fY - q.fX * p.fY;
        Edge edge(tangent.fX * scale,
                  tangent.fY * scale,
                  cross2 * scale + 0.5f);
        *edges->append() = edge;
        p = q;
    }
    Edge prev_edge = *edges->back();
    for (size_t i = 0; i < edges->count(); ++i) {
        Edge edge = edges->at(i);
        vertices[i] = prev_edge.intersect(edge);
        inverse.mapPoints(&vertices[i], 1);
        prev_edge = edge;
    }
    return edges->count();
}

void GrTesselatedPathRenderer::drawPath(GrDrawTarget* target,
                                        GrDrawTarget::StageBitfield stages,
                                        GrPathIter* path,
                                        GrPathFill fill,
                                        const GrPoint* translate) {
    GrDrawTarget::AutoStateRestore asr(target);
    bool colorWritesWereDisabled = target->isColorWriteDisabled();
    // face culling doesn't make sense here
    GrAssert(GrDrawTarget::kBoth_DrawFace == target->getDrawFace());

    GrMatrix viewM = target->getViewMatrix();
    // In order to tesselate the path we get a bound on how much the matrix can
    // stretch when mapping to screen coordinates.
    GrScalar stretch = viewM.getMaxStretch();
    bool useStretch = stretch > 0;
    GrScalar tol = GrPathUtils::gTolerance;

    if (!useStretch) {
        // TODO: deal with perspective in some better way.
        tol /= 10;
    } else {
        GrScalar sinv = GR_Scalar1 / stretch;
        tol = GrMul(tol, sinv);
    }
    GrScalar tolSqd = GrMul(tol, tol);

    path->rewind();

    int subpathCnt;
    int maxPts = GrPathUtils::worstCasePointCount(path, &subpathCnt, tol);

    GrVertexLayout layout = 0;
    for (int s = 0; s < GrDrawTarget::kNumStages; ++s) {
        if ((1 << s) & stages) {
            layout |= GrDrawTarget::StagePosAsTexCoordVertexLayoutBit(s);
        }
    }

    bool inverted = IsFillInverted(fill);
    if (inverted) {
        maxPts += 4;
        subpathCnt++;
    }
    GrPoint* base = new GrPoint[maxPts];
    GrPoint* vert = base;
    GrPoint* subpathBase = base;

    GrAutoSTMalloc<8, uint16_t> subpathVertCount(subpathCnt);

    path->rewind();

    GrPoint pts[4];

    bool first = true;
    int subpath = 0;

    for (;;) {
        GrPathCmd cmd = path->next(pts);
        switch (cmd) {
            case kMove_PathCmd:
                if (!first) {
                    subpathVertCount[subpath] = vert-subpathBase;
                    subpathBase = vert;
                    ++subpath;
                }
                *vert = pts[0];
                vert++;
                break;
            case kLine_PathCmd:
                *vert = pts[1];
                vert++;
                break;
            case kQuadratic_PathCmd: {
                GrPathUtils::generateQuadraticPoints(pts[0], pts[1], pts[2],
                                                     tolSqd, &vert,
                                                     GrPathUtils::quadraticPointCount(pts, tol));
                break;
            }
            case kCubic_PathCmd: {
                GrPathUtils::generateCubicPoints(pts[0], pts[1], pts[2], pts[3],
                                                 tolSqd, &vert,
                                                 GrPathUtils::cubicPointCount(pts, tol));
                break;
            }
            case kClose_PathCmd:
                break;
            case kEnd_PathCmd:
                subpathVertCount[subpath] = vert-subpathBase;
                ++subpath; // this could be only in debug
                goto FINISHED;
        }
        first = false;
    }
FINISHED:
    if (translate) {
        for (int i = 0; i < vert - base; i++) {
            base[i].offset(translate->fX, translate->fY);
        }
    }

    if (inverted) {
        GrRect bounds;
        GrAssert(NULL != target->getRenderTarget());
        bounds.setLTRB(0, 0,
                       GrIntToScalar(target->getRenderTarget()->width()),
                       GrIntToScalar(target->getRenderTarget()->height()));
        GrMatrix vmi;
        if (target->getViewInverse(&vmi)) {
            vmi.mapRect(&bounds);
        }
        *vert++ = GrPoint::Make(bounds.fLeft, bounds.fTop);
        *vert++ = GrPoint::Make(bounds.fLeft, bounds.fBottom);
        *vert++ = GrPoint::Make(bounds.fRight, bounds.fBottom);
        *vert++ = GrPoint::Make(bounds.fRight, bounds.fTop);
        subpathVertCount[subpath++] = 4;
    }

    GrAssert(subpath == subpathCnt);
    GrAssert((vert - base) <= maxPts);

    size_t count = vert - base;

    if (count < 3) {
      delete[] base;
      return;
    }

    if (subpathCnt == 1 && !inverted && path->convexHint() == kConvex_ConvexHint) {
        if (target->isAntialiasState()) {
            target->enableState(GrDrawTarget::kEdgeAA_StateBit);
            EdgeArray edges;
            GrMatrix inverse, matrix = target->getViewMatrix();
            target->getViewInverse(&inverse);

            count = computeEdgesAndOffsetVertices(matrix, inverse, base, count, &edges);
            GrPoint triangle[3];
            triangle[0] = base[0];
            Edge triangleEdges[6];
            triangleEdges[0] = *edges.back();
            triangleEdges[1] = edges[0];
            for (size_t i = 1; i < count - 1; i++) {
                triangle[1] = base[i];
                triangle[2] = base[i + 1];
                triangleEdges[2] = edges[i - 1];
                triangleEdges[3] = edges[i];
                triangleEdges[4] = edges[i];
                triangleEdges[5] = edges[i + 1];
                target->setVertexSourceToArray(layout, triangle, 3);
                target->setEdgeAAData(&triangleEdges[0].fX);
                target->drawNonIndexed(kTriangles_PrimitiveType, 0, 3);
            }
            target->disableState(GrDrawTarget::kEdgeAA_StateBit);
        } else {
            target->setVertexSourceToArray(layout, base, count);
            target->drawNonIndexed(kTriangleFan_PrimitiveType, 0, count);
        }
        delete[] base;
        return;
    }

    // FIXME:  This copy could be removed if we had (templated?) versions of
    // generate_*_point above that wrote directly into doubles.
    double* inVertices = new double[count * 3];
    for (size_t i = 0; i < count; ++i) {
        inVertices[i * 3]     = base[i].fX;
        inVertices[i * 3 + 1] = base[i].fY;
        inVertices[i * 3 + 2] = 1.0;
    }

    GLUtesselator* tess = internal_gluNewTess();
    unsigned windingRule = fill_type_to_glu_winding_rule(fill);
    internal_gluTessProperty(tess, GLU_TESS_WINDING_RULE, windingRule);
    internal_gluTessCallback(tess, GLU_TESS_BEGIN_DATA, (TESSCB) &beginData);
    internal_gluTessCallback(tess, GLU_TESS_VERTEX_DATA, (TESSCB) &vertexData);
    internal_gluTessCallback(tess, GLU_TESS_END_DATA, (TESSCB) &endData);
    internal_gluTessCallback(tess, GLU_TESS_EDGE_FLAG_DATA, (TESSCB) &edgeFlagData);
    internal_gluTessCallback(tess, GLU_TESS_COMBINE_DATA, (TESSCB) &combineData);
    GrTDArray<short> indices;
    GrTDArray<GrPoint> vertices;
    PolygonData data(&vertices, &indices);

    internal_gluTessBeginPolygon(tess, &data);
    size_t i = 0;
    for (int sp = 0; sp < subpathCnt; ++sp) {
        internal_gluTessBeginContour(tess);
        int start = i;
        int end = start + subpathVertCount[sp];
        for (; i < end; ++i) {
            double* inVertex = &inVertices[i * 3];
            *vertices.append() = GrPoint::Make(inVertex[0], inVertex[1]);
            internal_gluTessVertex(tess, inVertex, reinterpret_cast<void*>(i));
        }
        internal_gluTessEndContour(tess);
    }

    internal_gluTessEndPolygon(tess);
    internal_gluDeleteTess(tess);

    if (indices.count() > 0) {
        target->setVertexSourceToArray(layout, vertices.begin(), vertices.count());
        target->setIndexSourceToArray(indices.begin(), indices.count());
        target->drawIndexed(kTriangles_PrimitiveType,
                            0,
                            0,
                            vertices.count(),
                            indices.count());
    }
    delete[] inVertices;
    delete[] base;
}

bool GrTesselatedPathRenderer::canDrawPath(const GrDrawTarget* target,
                                           GrPathIter* path,
                                           GrPathFill fill) const {
    return kHairLine_PathFill != fill;
}

void GrTesselatedPathRenderer::drawPathToStencil(GrDrawTarget* target,
                                                 GrPathIter* path,
                                                 GrPathFill fill,
                                                 const GrPoint* translate) {
    GrAlwaysAssert(!"multipass stencil should not be needed");
}

bool GrTesselatedPathRenderer::supportsAA(GrDrawTarget* target,
                                                  GrPathIter* path,
                                                  GrPathFill fill) {
    int subpathCnt = 0;
    int tol = GrPathUtils::gTolerance;
    GrPathUtils::worstCasePointCount(path, &subpathCnt, tol);
    return (subpathCnt == 1 &&
            !IsFillInverted(fill) &&
            path->convexHint() == kConvex_ConvexHint);
}
<commit_msg>Fix winding order check for negative scale in tesselated path rendering.  The isCCW() code in GrTesselatedPathRenderer was using untransformed vertices, which fails for transforms with negative scale.  Doing the check after transformation fixes it.  This was causing some missing geometry in the PolyToPoly and Shapes tests in SampleApp.<commit_after>/*
    Copyright 2011 Google Inc.

    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

         http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.
 */

#include "GrTesselatedPathRenderer.h"

#include "GrMemory.h"
#include "GrPathUtils.h"

#include <internal_glu.h>

struct PolygonData {
    PolygonData(GrTDArray<GrPoint>* vertices, GrTDArray<short>* indices)
      : fVertices(vertices)
      , fIndices(indices)
    {
    }
    GrTDArray<GrPoint>* fVertices;
    GrTDArray<short>* fIndices;
};

static void beginData(GLenum type, void* data)
{
    GR_DEBUGASSERT(type == GL_TRIANGLES);
}

static void edgeFlagData(GLboolean flag, void* data)
{
}

static void vertexData(void* vertexData, void* data)
{
    short* end = static_cast<PolygonData*>(data)->fIndices->append();
    *end = reinterpret_cast<long>(vertexData);
}

static void endData(void* data)
{
}

static void combineData(GLdouble coords[3], void* vertexData[4],
                                 GLfloat weight[4], void **outData, void* data)
{
    PolygonData* polygonData = static_cast<PolygonData*>(data);
    int index = polygonData->fVertices->count();
    *polygonData->fVertices->append() = GrPoint::Make(static_cast<float>(coords[0]),
                                                      static_cast<float>(coords[1]));
    *outData = reinterpret_cast<void*>(index);
}

typedef void (*TESSCB)();

static unsigned fill_type_to_glu_winding_rule(GrPathFill fill) {
    switch (fill) {
        case kWinding_PathFill:
            return GLU_TESS_WINDING_NONZERO;
        case kEvenOdd_PathFill:
            return GLU_TESS_WINDING_ODD;
        case kInverseWinding_PathFill:
            return GLU_TESS_WINDING_POSITIVE;
        case kInverseEvenOdd_PathFill:
            return GLU_TESS_WINDING_ODD;
        case kHairLine_PathFill:
            return GLU_TESS_WINDING_NONZERO;  // FIXME:  handle this
        default:
            GrAssert(!"Unknown path fill!");
            return 0;
    }
}

GrTesselatedPathRenderer::GrTesselatedPathRenderer() {
}

class Edge {
  public:
    Edge() {}
    Edge(float x, float y, float z) : fX(x), fY(y), fZ(z) {}
    GrPoint intersect(const Edge& other) {
        return GrPoint::Make(
            (fY * other.fZ - other.fY * fZ) / (fX * other.fY - other.fX * fY),
            (fX * other.fZ - other.fX * fZ) / (other.fX * fY - fX * other.fY));
    }
    float fX, fY, fZ;
};

typedef GrTDArray<Edge> EdgeArray;

bool isCCW(const GrPoint* pts)
{
    GrVec v1 = pts[1] - pts[0];
    GrVec v2 = pts[2] - pts[1];
    return v1.cross(v2) < 0;
}

static size_t computeEdgesAndOffsetVertices(const GrMatrix& matrix,
                                            const GrMatrix& inverse,
                                            GrPoint* vertices,
                                            size_t numVertices,
                                            EdgeArray* edges)
{
    matrix.mapPoints(vertices, numVertices);
    GrPoint p = vertices[numVertices - 1];
    float sign = isCCW(vertices) ? -1.0f : 1.0f;
    for (size_t i = 0; i < numVertices; ++i) {
        GrPoint q = vertices[i];
        if (p == q) continue;
        GrVec tangent = GrVec::Make(p.fY - q.fY, q.fX - p.fX);
        float scale = sign / tangent.length();
        float cross2 = p.fX * q.fY - q.fX * p.fY;
        Edge edge(tangent.fX * scale,
                  tangent.fY * scale,
                  cross2 * scale + 0.5f);
        *edges->append() = edge;
        p = q;
    }
    Edge prev_edge = *edges->back();
    for (size_t i = 0; i < edges->count(); ++i) {
        Edge edge = edges->at(i);
        vertices[i] = prev_edge.intersect(edge);
        inverse.mapPoints(&vertices[i], 1);
        prev_edge = edge;
    }
    return edges->count();
}

void GrTesselatedPathRenderer::drawPath(GrDrawTarget* target,
                                        GrDrawTarget::StageBitfield stages,
                                        GrPathIter* path,
                                        GrPathFill fill,
                                        const GrPoint* translate) {
    GrDrawTarget::AutoStateRestore asr(target);
    bool colorWritesWereDisabled = target->isColorWriteDisabled();
    // face culling doesn't make sense here
    GrAssert(GrDrawTarget::kBoth_DrawFace == target->getDrawFace());

    GrMatrix viewM = target->getViewMatrix();
    // In order to tesselate the path we get a bound on how much the matrix can
    // stretch when mapping to screen coordinates.
    GrScalar stretch = viewM.getMaxStretch();
    bool useStretch = stretch > 0;
    GrScalar tol = GrPathUtils::gTolerance;

    if (!useStretch) {
        // TODO: deal with perspective in some better way.
        tol /= 10;
    } else {
        GrScalar sinv = GR_Scalar1 / stretch;
        tol = GrMul(tol, sinv);
    }
    GrScalar tolSqd = GrMul(tol, tol);

    path->rewind();

    int subpathCnt;
    int maxPts = GrPathUtils::worstCasePointCount(path, &subpathCnt, tol);

    GrVertexLayout layout = 0;
    for (int s = 0; s < GrDrawTarget::kNumStages; ++s) {
        if ((1 << s) & stages) {
            layout |= GrDrawTarget::StagePosAsTexCoordVertexLayoutBit(s);
        }
    }

    bool inverted = IsFillInverted(fill);
    if (inverted) {
        maxPts += 4;
        subpathCnt++;
    }
    GrPoint* base = new GrPoint[maxPts];
    GrPoint* vert = base;
    GrPoint* subpathBase = base;

    GrAutoSTMalloc<8, uint16_t> subpathVertCount(subpathCnt);

    path->rewind();

    GrPoint pts[4];

    bool first = true;
    int subpath = 0;

    for (;;) {
        GrPathCmd cmd = path->next(pts);
        switch (cmd) {
            case kMove_PathCmd:
                if (!first) {
                    subpathVertCount[subpath] = vert-subpathBase;
                    subpathBase = vert;
                    ++subpath;
                }
                *vert = pts[0];
                vert++;
                break;
            case kLine_PathCmd:
                *vert = pts[1];
                vert++;
                break;
            case kQuadratic_PathCmd: {
                GrPathUtils::generateQuadraticPoints(pts[0], pts[1], pts[2],
                                                     tolSqd, &vert,
                                                     GrPathUtils::quadraticPointCount(pts, tol));
                break;
            }
            case kCubic_PathCmd: {
                GrPathUtils::generateCubicPoints(pts[0], pts[1], pts[2], pts[3],
                                                 tolSqd, &vert,
                                                 GrPathUtils::cubicPointCount(pts, tol));
                break;
            }
            case kClose_PathCmd:
                break;
            case kEnd_PathCmd:
                subpathVertCount[subpath] = vert-subpathBase;
                ++subpath; // this could be only in debug
                goto FINISHED;
        }
        first = false;
    }
FINISHED:
    if (translate) {
        for (int i = 0; i < vert - base; i++) {
            base[i].offset(translate->fX, translate->fY);
        }
    }

    if (inverted) {
        GrRect bounds;
        GrAssert(NULL != target->getRenderTarget());
        bounds.setLTRB(0, 0,
                       GrIntToScalar(target->getRenderTarget()->width()),
                       GrIntToScalar(target->getRenderTarget()->height()));
        GrMatrix vmi;
        if (target->getViewInverse(&vmi)) {
            vmi.mapRect(&bounds);
        }
        *vert++ = GrPoint::Make(bounds.fLeft, bounds.fTop);
        *vert++ = GrPoint::Make(bounds.fLeft, bounds.fBottom);
        *vert++ = GrPoint::Make(bounds.fRight, bounds.fBottom);
        *vert++ = GrPoint::Make(bounds.fRight, bounds.fTop);
        subpathVertCount[subpath++] = 4;
    }

    GrAssert(subpath == subpathCnt);
    GrAssert((vert - base) <= maxPts);

    size_t count = vert - base;

    if (count < 3) {
      delete[] base;
      return;
    }

    if (subpathCnt == 1 && !inverted && path->convexHint() == kConvex_ConvexHint) {
        if (target->isAntialiasState()) {
            target->enableState(GrDrawTarget::kEdgeAA_StateBit);
            EdgeArray edges;
            GrMatrix inverse, matrix = target->getViewMatrix();
            target->getViewInverse(&inverse);

            count = computeEdgesAndOffsetVertices(matrix, inverse, base, count, &edges);
            GrPoint triangle[3];
            triangle[0] = base[0];
            Edge triangleEdges[6];
            triangleEdges[0] = *edges.back();
            triangleEdges[1] = edges[0];
            for (size_t i = 1; i < count - 1; i++) {
                triangle[1] = base[i];
                triangle[2] = base[i + 1];
                triangleEdges[2] = edges[i - 1];
                triangleEdges[3] = edges[i];
                triangleEdges[4] = edges[i];
                triangleEdges[5] = edges[i + 1];
                target->setVertexSourceToArray(layout, triangle, 3);
                target->setEdgeAAData(&triangleEdges[0].fX);
                target->drawNonIndexed(kTriangles_PrimitiveType, 0, 3);
            }
            target->disableState(GrDrawTarget::kEdgeAA_StateBit);
        } else {
            target->setVertexSourceToArray(layout, base, count);
            target->drawNonIndexed(kTriangleFan_PrimitiveType, 0, count);
        }
        delete[] base;
        return;
    }

    // FIXME:  This copy could be removed if we had (templated?) versions of
    // generate_*_point above that wrote directly into doubles.
    double* inVertices = new double[count * 3];
    for (size_t i = 0; i < count; ++i) {
        inVertices[i * 3]     = base[i].fX;
        inVertices[i * 3 + 1] = base[i].fY;
        inVertices[i * 3 + 2] = 1.0;
    }

    GLUtesselator* tess = internal_gluNewTess();
    unsigned windingRule = fill_type_to_glu_winding_rule(fill);
    internal_gluTessProperty(tess, GLU_TESS_WINDING_RULE, windingRule);
    internal_gluTessCallback(tess, GLU_TESS_BEGIN_DATA, (TESSCB) &beginData);
    internal_gluTessCallback(tess, GLU_TESS_VERTEX_DATA, (TESSCB) &vertexData);
    internal_gluTessCallback(tess, GLU_TESS_END_DATA, (TESSCB) &endData);
    internal_gluTessCallback(tess, GLU_TESS_EDGE_FLAG_DATA, (TESSCB) &edgeFlagData);
    internal_gluTessCallback(tess, GLU_TESS_COMBINE_DATA, (TESSCB) &combineData);
    GrTDArray<short> indices;
    GrTDArray<GrPoint> vertices;
    PolygonData data(&vertices, &indices);

    internal_gluTessBeginPolygon(tess, &data);
    size_t i = 0;
    for (int sp = 0; sp < subpathCnt; ++sp) {
        internal_gluTessBeginContour(tess);
        int start = i;
        int end = start + subpathVertCount[sp];
        for (; i < end; ++i) {
            double* inVertex = &inVertices[i * 3];
            *vertices.append() = GrPoint::Make(inVertex[0], inVertex[1]);
            internal_gluTessVertex(tess, inVertex, reinterpret_cast<void*>(i));
        }
        internal_gluTessEndContour(tess);
    }

    internal_gluTessEndPolygon(tess);
    internal_gluDeleteTess(tess);

    if (indices.count() > 0) {
        target->setVertexSourceToArray(layout, vertices.begin(), vertices.count());
        target->setIndexSourceToArray(indices.begin(), indices.count());
        target->drawIndexed(kTriangles_PrimitiveType,
                            0,
                            0,
                            vertices.count(),
                            indices.count());
    }
    delete[] inVertices;
    delete[] base;
}

bool GrTesselatedPathRenderer::canDrawPath(const GrDrawTarget* target,
                                           GrPathIter* path,
                                           GrPathFill fill) const {
    return kHairLine_PathFill != fill;
}

void GrTesselatedPathRenderer::drawPathToStencil(GrDrawTarget* target,
                                                 GrPathIter* path,
                                                 GrPathFill fill,
                                                 const GrPoint* translate) {
    GrAlwaysAssert(!"multipass stencil should not be needed");
}

bool GrTesselatedPathRenderer::supportsAA(GrDrawTarget* target,
                                                  GrPathIter* path,
                                                  GrPathFill fill) {
    int subpathCnt = 0;
    int tol = GrPathUtils::gTolerance;
    GrPathUtils::worstCasePointCount(path, &subpathCnt, tol);
    return (subpathCnt == 1 &&
            !IsFillInverted(fill) &&
            path->convexHint() == kConvex_ConvexHint);
}
<|endoftext|>


<commit_before>// The libMesh Finite Element Library.
// Copyright (C) 2002-2020 Benjamin S. Kirk, John W. Peterson, Roy H. Stogner

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

// Local Includes
#include "libmesh/libmesh_config.h"

#ifdef LIBMESH_ENABLE_PERIODIC

#include "libmesh/periodic_boundaries.h"
#include "libmesh/point_locator_base.h"
#include "libmesh/elem.h"
#include "libmesh/periodic_boundary.h"
#include "libmesh/mesh_base.h"
#include "libmesh/boundary_info.h"

namespace libMesh
{

PeriodicBoundaries::~PeriodicBoundaries()
{
  for (auto & pr : *this)
    delete pr.second;
}

PeriodicBoundaryBase * PeriodicBoundaries::boundary(boundary_id_type id)
{
  iterator i = this->find(id);
  if (i == this->end())
    return nullptr;
  return i->second;
}

const PeriodicBoundaryBase * PeriodicBoundaries::boundary(boundary_id_type id) const
{
  const_iterator i = this->find(id);
  if (i == this->end())
    return nullptr;
  return i->second;
}

const Elem * PeriodicBoundaries::neighbor(boundary_id_type boundary_id,
                                          const PointLocatorBase & point_locator,
                                          const Elem * e,
                                          unsigned int side) const
{
  // Find a point on that side (and only that side)

  Point p = e->build_side_ptr(side)->centroid();

  const PeriodicBoundaryBase * b = this->boundary(boundary_id);
  libmesh_assert (b);
  p = b->get_corresponding_pos(p);

  std::set<const Elem *> candidate_elements;
  point_locator.operator()(p, candidate_elements);

  // We might have found multiple elements, e.g. if two distinct periodic
  // boundaries are overlapping (see systems_of_equations_ex9, for example).
  // As a result, we need to search for the element that has boundary_id.
  const MeshBase & mesh = point_locator.get_mesh();
  for(const Elem * elem_it : candidate_elements)
    {
      unsigned int s_neigh =
        mesh.get_boundary_info().side_with_boundary_id(elem_it, b->pairedboundary);

      // If s_neigh is not invalid then we have found an element that contains
      // boundary_id, so return this element
      if(s_neigh != libMesh::invalid_uint)
        {
          return elem_it;
        }
    }

  libmesh_error_msg("Periodic boundary neighbor not found");
  return nullptr;
}

} // namespace libMesh

#endif // LIBMESH_ENABLE_PERIODIC
<commit_msg>Make overzealous PBC error into careful assertion<commit_after>// The libMesh Finite Element Library.
// Copyright (C) 2002-2020 Benjamin S. Kirk, John W. Peterson, Roy H. Stogner

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

// Local Includes
#include "libmesh/libmesh_config.h"

#ifdef LIBMESH_ENABLE_PERIODIC

#include "libmesh/periodic_boundaries.h"

#include "libmesh/boundary_info.h"
#include "libmesh/elem.h"
#include "libmesh/mesh_base.h"
#include "libmesh/periodic_boundary.h"
#include "libmesh/point_locator_base.h"
#include "libmesh/remote_elem.h"

namespace libMesh
{

PeriodicBoundaries::~PeriodicBoundaries()
{
  for (auto & pr : *this)
    delete pr.second;
}

PeriodicBoundaryBase * PeriodicBoundaries::boundary(boundary_id_type id)
{
  iterator i = this->find(id);
  if (i == this->end())
    return nullptr;
  return i->second;
}

const PeriodicBoundaryBase * PeriodicBoundaries::boundary(boundary_id_type id) const
{
  const_iterator i = this->find(id);
  if (i == this->end())
    return nullptr;
  return i->second;
}

const Elem * PeriodicBoundaries::neighbor(boundary_id_type boundary_id,
                                          const PointLocatorBase & point_locator,
                                          const Elem * e,
                                          unsigned int side) const
{
  // Find a point on that side (and only that side)

  Point p = e->build_side_ptr(side)->centroid();

  const PeriodicBoundaryBase * b = this->boundary(boundary_id);
  libmesh_assert (b);
  p = b->get_corresponding_pos(p);

  std::set<const Elem *> candidate_elements;
  point_locator.operator()(p, candidate_elements);

  // We might have found multiple elements, e.g. if two distinct periodic
  // boundaries are overlapping (see systems_of_equations_ex9, for example).
  // As a result, we need to search for the element that has boundary_id.
  const MeshBase & mesh = point_locator.get_mesh();
  for(const Elem * elem_it : candidate_elements)
    {
      unsigned int s_neigh =
        mesh.get_boundary_info().side_with_boundary_id(elem_it, b->pairedboundary);

      // If s_neigh is not invalid then we have found an element that contains
      // boundary_id, so return this element
      if(s_neigh != libMesh::invalid_uint)
        {
          return elem_it;
        }
    }

  // If we should have found a periodic neighbor but didn't then
  // either we're on a ghosted element with a remote periodic neighbor
  // or we're on a mesh with an inconsistent periodic boundary.
  libmesh_assert_msg(!mesh.is_serial() &&
                     (e->processor_id() != mesh.processor_id()),
                     "Periodic boundary neighbor not found");
  return remote_elem;
}

} // namespace libMesh

#endif // LIBMESH_ENABLE_PERIODIC
<|endoftext|>


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

Program:   Medical Imaging & Interaction Toolkit
Language:  C++
Date:      $Date: 2009-05-12 19:56:03 +0200 (Di, 12 Mai 2009) $
Version:   $Revision: 17179 $

Copyright (c) German Cancer Research Center, Division of Medical and
Biological Informatics. All rights reserved.
See MITKCopyright.txt or http://www.mitk.org/copyright.html for details.

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

=========================================================================*/

#include "mitkPointSetDifferenceStatisticsCalculator.h"

mitk::PointSetDifferenceStatisticsCalculator::PointSetDifferenceStatisticsCalculator() :
  m_StatisticsCalculated(false)
{
  m_PointSet1 = mitk::PointSet::New();
  m_PointSet2 = mitk::PointSet::New();
  m_Statistics.Reset();
}

mitk::PointSetDifferenceStatisticsCalculator::PointSetDifferenceStatisticsCalculator(mitk::PointSet::Pointer pSet1, mitk::PointSet::Pointer pSet2)
{
  m_PointSet1 = pSet1;
  m_PointSet2 = pSet2;
  m_StatisticsCalculated = false;
  m_Statistics.Reset();
}

mitk::PointSetDifferenceStatisticsCalculator::~PointSetDifferenceStatisticsCalculator()
{
}

void mitk::PointSetDifferenceStatisticsCalculator::SetPointSets(mitk::PointSet::Pointer pSet1, mitk::PointSet::Pointer pSet2)
{
  if (pSet1.IsNotNull())
  {
    m_PointSet1 = pSet1;
  }
  if (pSet2.IsNotNull())
  {
    m_PointSet2 = pSet2;
  }
  m_StatisticsCalculated = false;
  m_Statistics.Reset();
}

std::vector<double> mitk::PointSetDifferenceStatisticsCalculator::GetDifferences()
{
  if (!m_StatisticsCalculated)
  {
    this->ComputeStatistics();
  }
  return m_DifferencesVector;
}

std::vector<double> mitk::PointSetDifferenceStatisticsCalculator::GetSquaredDifferences()
{
  if (!m_StatisticsCalculated)
  {
    this->ComputeStatistics();
  }
  return m_SquaredDifferencesVector;
}

double mitk::PointSetDifferenceStatisticsCalculator::GetMean()
{
  if (!m_StatisticsCalculated)
  {
    this->ComputeStatistics();
  }
  return m_Statistics.Mean;
}

double mitk::PointSetDifferenceStatisticsCalculator::GetSD()
{
  if (!m_StatisticsCalculated)
  {
    this->ComputeStatistics();
  }
  return m_Statistics.Sigma;
}

double mitk::PointSetDifferenceStatisticsCalculator::GetVariance()
{
  if (!m_StatisticsCalculated)
  {
    this->ComputeStatistics();
  }
  return m_Statistics.Variance;
}

double mitk::PointSetDifferenceStatisticsCalculator::GetRMS()
{
  if (!m_StatisticsCalculated)
  {
    this->ComputeStatistics();
  }
  return m_Statistics.RMS;
}

double mitk::PointSetDifferenceStatisticsCalculator::GetMedian()
{
  if (!m_StatisticsCalculated)
  {
    this->ComputeStatistics();
  }
  return m_Statistics.Median;
}

double mitk::PointSetDifferenceStatisticsCalculator::GetMax()
{
  if (!m_StatisticsCalculated)
  {
    this->ComputeStatistics();
  }
  return m_Statistics.Max;
}

double mitk::PointSetDifferenceStatisticsCalculator::GetMin()
{
  if (!m_StatisticsCalculated)
  {
    this->ComputeStatistics();
  }
  return m_Statistics.Min;
}

double mitk::PointSetDifferenceStatisticsCalculator::GetNumberOfPoints()
{
  if (!m_StatisticsCalculated)
  {
    this->ComputeStatistics();
  }
  return m_Statistics.N;
}

void mitk::PointSetDifferenceStatisticsCalculator::ComputeStatistics()
{
  if ((m_PointSet1==NULL)||(m_PointSet2==NULL))
  {
    itkExceptionMacro("Point sets specified are not valid. Please specify correct Point sets");
  }
  else if (m_PointSet1->GetSize()!=m_PointSet2->GetSize())
  {
    itkExceptionMacro("PointSets are not equal. Please make sure that your PointSets have the same size and hold corresponding points.");
  }
  else if (m_PointSet1->GetSize()==0)
  {
    itkExceptionMacro("There are no points in the PointSets. Please make sure that the PointSets contain points");
  }
  else
  {
    double mean, sd, rms= 0.0;
    std::vector<double> differencesVector;
    mitk::Point3D point1;
    mitk::Point3D point2;
    int numberOfPoints = m_PointSet1->GetSize();
    for (int i=0; i<numberOfPoints; i++)
    {
      point1 = m_PointSet1->GetPoint(i);
      point2 = m_PointSet2->GetPoint(i);
      double squaredDistance = point1.SquaredEuclideanDistanceTo(point2);
      mean+=sqrt(squaredDistance);
      rms+=squaredDistance;
      this->m_SquaredDifferencesVector.push_back(squaredDistance);
      differencesVector.push_back(sqrt(squaredDistance));
    }
    m_DifferencesVector = differencesVector;
    mean = mean/numberOfPoints;
    rms = sqrt(rms/numberOfPoints);
    for (int i=0; i<differencesVector.size(); i++)
    {
      sd+=(differencesVector.at(i)-mean)*(differencesVector.at(i)-mean);
    }
    double variance = sd/numberOfPoints;
    sd = sqrt(variance);
    std::sort(differencesVector.begin(),differencesVector.end());
    double min = differencesVector.at(0);
    double max = differencesVector.at(numberOfPoints-1);
    double median = 0.0;
    if (numberOfPoints%2 == 0)
    {
      median = (differencesVector.at(numberOfPoints/2)+differencesVector.at(numberOfPoints/2-1))/2;
    }
    else
    {
      median = differencesVector.at((numberOfPoints-1)/2+1);
    }
    m_Statistics.Mean = mean;
    m_Statistics.Sigma = sd;
    m_Statistics.Variance = variance;
    m_Statistics.RMS = rms;
    m_Statistics.Min = differencesVector.at(0);
    m_Statistics.Max = differencesVector.at(numberOfPoints-1);
    m_Statistics.Median = median;
    m_Statistics.N = numberOfPoints;

    m_StatisticsCalculated = true;
  }
}
<commit_msg>corrected zero check<commit_after>/*=========================================================================

Program:   Medical Imaging & Interaction Toolkit
Language:  C++
Date:      $Date: 2009-05-12 19:56:03 +0200 (Di, 12 Mai 2009) $
Version:   $Revision: 17179 $

Copyright (c) German Cancer Research Center, Division of Medical and
Biological Informatics. All rights reserved.
See MITKCopyright.txt or http://www.mitk.org/copyright.html for details.

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

=========================================================================*/

#include "mitkPointSetDifferenceStatisticsCalculator.h"

mitk::PointSetDifferenceStatisticsCalculator::PointSetDifferenceStatisticsCalculator() :
  m_StatisticsCalculated(false)
{
  m_PointSet1 = mitk::PointSet::New();
  m_PointSet2 = mitk::PointSet::New();
  m_Statistics.Reset();
}

mitk::PointSetDifferenceStatisticsCalculator::PointSetDifferenceStatisticsCalculator(mitk::PointSet::Pointer pSet1, mitk::PointSet::Pointer pSet2)
{
  m_PointSet1 = pSet1;
  m_PointSet2 = pSet2;
  m_StatisticsCalculated = false;
  m_Statistics.Reset();
}

mitk::PointSetDifferenceStatisticsCalculator::~PointSetDifferenceStatisticsCalculator()
{
}

void mitk::PointSetDifferenceStatisticsCalculator::SetPointSets(mitk::PointSet::Pointer pSet1, mitk::PointSet::Pointer pSet2)
{
  if (pSet1.IsNotNull())
  {
    m_PointSet1 = pSet1;
  }
  if (pSet2.IsNotNull())
  {
    m_PointSet2 = pSet2;
  }
  m_StatisticsCalculated = false;
  m_Statistics.Reset();
}

std::vector<double> mitk::PointSetDifferenceStatisticsCalculator::GetDifferences()
{
  if (!m_StatisticsCalculated)
  {
    this->ComputeStatistics();
  }
  return m_DifferencesVector;
}

std::vector<double> mitk::PointSetDifferenceStatisticsCalculator::GetSquaredDifferences()
{
  if (!m_StatisticsCalculated)
  {
    this->ComputeStatistics();
  }
  return m_SquaredDifferencesVector;
}

double mitk::PointSetDifferenceStatisticsCalculator::GetMean()
{
  if (!m_StatisticsCalculated)
  {
    this->ComputeStatistics();
  }
  return m_Statistics.Mean;
}

double mitk::PointSetDifferenceStatisticsCalculator::GetSD()
{
  if (!m_StatisticsCalculated)
  {
    this->ComputeStatistics();
  }
  return m_Statistics.Sigma;
}

double mitk::PointSetDifferenceStatisticsCalculator::GetVariance()
{
  if (!m_StatisticsCalculated)
  {
    this->ComputeStatistics();
  }
  return m_Statistics.Variance;
}

double mitk::PointSetDifferenceStatisticsCalculator::GetRMS()
{
  if (!m_StatisticsCalculated)
  {
    this->ComputeStatistics();
  }
  return m_Statistics.RMS;
}

double mitk::PointSetDifferenceStatisticsCalculator::GetMedian()
{
  if (!m_StatisticsCalculated)
  {
    this->ComputeStatistics();
  }
  return m_Statistics.Median;
}

double mitk::PointSetDifferenceStatisticsCalculator::GetMax()
{
  if (!m_StatisticsCalculated)
  {
    this->ComputeStatistics();
  }
  return m_Statistics.Max;
}

double mitk::PointSetDifferenceStatisticsCalculator::GetMin()
{
  if (!m_StatisticsCalculated)
  {
    this->ComputeStatistics();
  }
  return m_Statistics.Min;
}

double mitk::PointSetDifferenceStatisticsCalculator::GetNumberOfPoints()
{
  if (!m_StatisticsCalculated)
  {
    this->ComputeStatistics();
  }
  return m_Statistics.N;
}

void mitk::PointSetDifferenceStatisticsCalculator::ComputeStatistics()
{
  if ((m_PointSet1.IsNull())||(m_PointSet2.IsNull()))
  {
    itkExceptionMacro("Point sets specified are not valid. Please specify correct Point sets");
  }
  else if (m_PointSet1->GetSize()!=m_PointSet2->GetSize())
  {
    itkExceptionMacro("PointSets are not equal. Please make sure that your PointSets have the same size and hold corresponding points.");
  }
  else if (m_PointSet1->GetSize()==0)
  {
    itkExceptionMacro("There are no points in the PointSets. Please make sure that the PointSets contain points");
  }
  else
  {
    double mean, sd, rms= 0.0;
    std::vector<double> differencesVector;
    mitk::Point3D point1;
    mitk::Point3D point2;
    int numberOfPoints = m_PointSet1->GetSize();
    for (int i=0; i<numberOfPoints; i++)
    {
      point1 = m_PointSet1->GetPoint(i);
      point2 = m_PointSet2->GetPoint(i);
      double squaredDistance = point1.SquaredEuclideanDistanceTo(point2);
      mean+=sqrt(squaredDistance);
      rms+=squaredDistance;
      this->m_SquaredDifferencesVector.push_back(squaredDistance);
      differencesVector.push_back(sqrt(squaredDistance));
    }
    m_DifferencesVector = differencesVector;
    mean = mean/numberOfPoints;
    rms = sqrt(rms/numberOfPoints);
    for (int i=0; i<differencesVector.size(); i++)
    {
      sd+=(differencesVector.at(i)-mean)*(differencesVector.at(i)-mean);
    }
    double variance = sd/numberOfPoints;
    sd = sqrt(variance);
    std::sort(differencesVector.begin(),differencesVector.end());
    double min = differencesVector.at(0);
    double max = differencesVector.at(numberOfPoints-1);
    double median = 0.0;
    if (numberOfPoints%2 == 0)
    {
      median = (differencesVector.at(numberOfPoints/2)+differencesVector.at(numberOfPoints/2-1))/2;
    }
    else
    {
      median = differencesVector.at((numberOfPoints-1)/2+1);
    }
    m_Statistics.Mean = mean;
    m_Statistics.Sigma = sd;
    m_Statistics.Variance = variance;
    m_Statistics.RMS = rms;
    m_Statistics.Min = differencesVector.at(0);
    m_Statistics.Max = differencesVector.at(numberOfPoints-1);
    m_Statistics.Median = median;
    m_Statistics.N = numberOfPoints;

    m_StatisticsCalculated = true;
  }
}
<|endoftext|>


<commit_before>#include <cstdlib>

#include <Eigen/Geometry>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit/robot_state/conversions.h>
#include "cros.hpp"
#include "c_container.h"
#include "moveit_container.hpp"

static void
robot_state_zero( robot_state::RobotState *state )
{
    double *x = state->getVariablePositions();
    size_t n = state->getVariableNames().size();
    memset(x, 0, n * sizeof(x[0]) );
}

struct container *
tms_container_create( cros_node_handle *nh, const char *robot_description )
{
    return new container(nh->node_handle, robot_description);
}

void
tms_container_destroy( struct container * c)
{
    delete c;
}

size_t
tms_container_variable_count( struct container *c )
{
    return c->robot_model->getVariableCount();
}

int
tms_container_merge_group( struct container *c, const char *group,
                           size_t n_group, const double *q_group,
                           size_t n_all, double *q_all )
{
    if( n_all != tms_container_variable_count(c) ) {
        fprintf(stderr, "error: Size mismatch between robot model (%lu) and provided array (%lu)\n",
                tms_container_variable_count(c), n_all);
        return -1;
    }

    robot_state::RobotState state(c->robot_model);
    state.setVariablePositions(q_all);
    size_t n_all_model = state.getVariableCount();
    state.setJointGroupPositions(group, q_group);
    memcpy(q_all, state.getVariablePositions(), n_all_model*sizeof(q_all[0]));
    return 0;
}

int
tms_container_set_start( struct container * c, size_t n_all, const double *q_all )
{

    if( n_all != tms_container_variable_count(c) ) {
        fprintf(stderr, "error: Size mismatch between robot model (%lu) and provided array (%lu)\n",
                tms_container_variable_count(c), n_all);
        return -1;
    }
    robot_state::RobotState start_state = c->planning_scene->getCurrentState();
    start_state.setVariablePositions(q_all);
    start_state.update(true);

    // c->req.start_state.joint_state.name =  start_state.getVariableNames();
    // {
    //     size_t n = c->req.start_state.joint_state.name.size();
    //     double *p = start_state.getVariablePositions();
    //     c->req.start_state.joint_state.position.resize( n );
    //     std::copy ( p, p+n,
    //                 c->req.start_state.joint_state.position.begin() );
    // }
    // c->req.start_state.is_diff = true;
    c->planning_scene->setCurrentState(start_state);

    // // Print stuff
    // {
    //     double r[4],v[3];
    //     tms_container_group_fk( c, group, n, q, r, v );

    //     fprintf(stderr,
    //             "r_start[4] = {%f, %f, %f, %f}\n"
    //             "v_start[3] = {%f, %f, %f}\n",
    //             r[0], r[1], r[2], r[3],
    //             v[0], v[1], v[2] );
    // }
    return 0;
}

int
tms_container_set_group( struct container * c, const char *group )
{
    c->req.group_name = group;
    return 0;
}

int
tms_container_goal_clear( struct container *c )
{
    c->req.goal_constraints.clear();
    return 0;
}

int
tms_container_set_js_goal( struct container * c, const char *group, size_t n_all, double *q_all )
{

    if( n_all != tms_container_variable_count(c) ) {
        fprintf(stderr, "error: Size mismatch between robot model (%lu) and provided array (%lu)\n",
                tms_container_variable_count(c), n_all);
        return -1;
    }

    robot_state::RobotState goal_state(c->robot_model);
    goal_state.setVariablePositions(q_all);

    const robot_state::JointModelGroup* joint_model_group = goal_state.getJointModelGroup(group);

    moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state,
                                                                                joint_model_group);
    c->req.goal_constraints.push_back(joint_goal);

    {
        double *x = goal_state.getVariablePositions();
        for( size_t i = 0; i < goal_state.getVariableNames().size(); i++ ) {
            fprintf(stderr, "goal %lu: %f\n", i, x[i] );
        }
    }

    return 0;
}

int
tms_container_set_ws_goal( struct container * c, const char *link, const double quat[4], const double vec[3],
                           double tol_x, double tol_angle )
{
    geometry_msgs::PoseStamped stamped_pose;
    stamped_pose.header.frame_id = "base";
    geometry_msgs::Pose &pose = stamped_pose.pose;

    pose.orientation.x = quat[0];
    pose.orientation.y = quat[1];
    pose.orientation.z = quat[2];
    pose.orientation.w = quat[3];
    pose.position.x = vec[0];
    pose.position.y = vec[1];
    pose.position.z = vec[2];

    fprintf(stderr,"constructing pose goal\n");
    moveit_msgs::Constraints pose_goal =
        kinematic_constraints::constructGoalConstraints(link, stamped_pose, tol_x, tol_angle );
    fprintf(stderr,"adding pose goal...\n");
    c->req.goal_constraints.push_back(pose_goal);
    fprintf(stderr,"...added\n");
    return 0;

    // robot_state::RobotState goal_state(c->robot_model);
    // /* Zero positions because somebody's not inititializing their shit */
    // robot_state_zero( &goal_state);

    // const robot_state::JointModelGroup* joint_model_group = goal_state.getJointModelGroup(group);
    // bool got_ik = goal_state.setFromIK(joint_model_group,pose);
    // fprintf(stderr, "IK: %s\n", got_ik ? "yes" : "no" );

    // if( ! got_ik ) return -1;

    // moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state,
    //                                                                                joint_model_group);
    // c->req.goal_constraints.push_back(joint_goal);

    // {
    //     double *x = goal_state.getVariablePositions();
    //     for( size_t i = 0; i < goal_state.getVariableNames().size(); i++ ) {
    //         fprintf(stderr, "goal %lu: %f\n", i, x[i] );
    //     }
    // }

    // {
    //     double r[4],v[3];

    //     tms_container_link_fk( c, tms_container_group_endlink( c, group ),
    //                        goal_state.getVariableNames().size(),
    //                        goal_state.getVariablePositions(),
    //                        r, v );

    //     fprintf(stderr,
    //             "r_goal[4] = {%f, %f, %f, %f}\n"
    //             "v_goal[3] = {%f, %f, %f}\n",
    //             r[0], r[1], r[2], r[3],
    //             v[0], v[1], v[2] );
    // }

}

int
tms_container_plan( struct container * c,
                    double timeout,
                    size_t *n_vars, size_t *n_points, double **points)
{
    /**********/
    /*  PLAN  */
    /**********/
    moveit_msgs::MoveItErrorCodes err;
    planning_interface::MotionPlanResponse res;
    c->req.allowed_planning_time = timeout;

    moveit::core::robotStateToRobotStateMsg(c->planning_scene->getCurrentState(), c->req.start_state);

    std::cout << c->req << std::endl;

    planning_interface::PlanningContextPtr context =
        c->planner_instance->getPlanningContext(c->planning_scene, c->req, err);
    context->solve(res);
    if(res.error_code_.val != res.error_code_.SUCCESS)
    {
        //TODO: Why is this 0 instead of SUCCESS?
        fprintf(stderr, "Planning failed: %d\n", res.error_code_.val );
        //return 0;
    }

    if(res.error_code_.val < 0 ) {
        fprintf(stderr, "returning failure\n");
        return -1;
    }

    /************/
    /*  Result  */
    /************/
    moveit_msgs::MotionPlanResponse res_msg;
    res.getMessage(res_msg);
    int i = 0;
    for( auto itr = res_msg.trajectory.joint_trajectory.points.begin();
         itr != res_msg.trajectory.joint_trajectory.points.end();
         itr++ )
    {
        printf("waypoint %02d: ", i++);
        for( auto j = itr->positions.begin();
             j != itr->positions.end();
             j++ )
        {
            printf("%f\t", *j );
        }
        printf("\n");
    }

    /* Copy Result */
    *n_points = res_msg.trajectory.joint_trajectory.points.size();
    if( 0 == n_points ) return 0;
    *n_vars = res_msg.trajectory.joint_trajectory.points[0].positions.size();
    i = 0;
    double *start = (double*)malloc( sizeof(*start) * (*n_points) * (*n_vars) );
    *points = start;
    for( auto itr = res_msg.trajectory.joint_trajectory.points.begin();
         itr != res_msg.trajectory.joint_trajectory.points.end();
         itr++ )
    {
        size_t s = itr->positions.size();
        if( s != *n_vars ) {
            fprintf(stderr, "Bogus variable count\n");
            *n_points = 0;
            free(points);
            return -1;
        }
        std::copy( itr->positions.begin(), itr->positions.end(), start );

        // for( size_t j = 0; j < *n_vars; j ++ ) {
        //     printf("%f\t", start[j] );
        // }
        // printf("\n");

        start = start +  (*n_vars);
    }

    // printf("%lx\n",*points);

    // for(i = 0; i < (*n_vars)*(*n_points); i++ ){
    //     printf("%f%c", (*points)[i], ( (i+1) % (*n_vars) ) ? '\t' : '\n'   );
    // }

    /***************/
    /*  Visualize  */
    /***************/
    moveit_msgs::DisplayTrajectory display_trajectory;

    ROS_INFO("Visualizing plan 1 (again)");
    display_trajectory.trajectory_start = res_msg.trajectory_start;
    display_trajectory.trajectory.push_back(res_msg.trajectory);
    c->display_publisher.publish(display_trajectory);

    return 0;
}

const char *
tms_container_group_endlink( struct container * c, const char *group )
{
    robot_state::RobotState state(c->robot_model);
    const robot_state::JointModelGroup* joint_model_group
        = state.getJointModelGroup(group);
    const std::vector<std::string> &link_names = joint_model_group->getLinkModelNames();
    const std::string & end_link =  link_names[ link_names.size()-1];
    return end_link.c_str();
}

size_t
tms_container_group_joint_count( struct container * c, const char *group )
{
    robot_state::RobotState state(c->robot_model);
    const robot_state::JointModelGroup* joint_model_group
        = state.getJointModelGroup(group);
    return joint_model_group->getVariableCount();
}

const char *
tms_container_group_joint_name( struct container * c, const char *group, size_t i )
{
    robot_state::RobotState state(c->robot_model);
    const robot_state::JointModelGroup* joint_model_group
        = state.getJointModelGroup(group);
    const std::vector< std::string > & names = joint_model_group->getActiveJointModelNames();
    if( i < names.size() ) {
        return names[i].c_str();
    } else {
        return NULL;
    }
}

int
tms_container_group_fk( struct container * c, const char *group, size_t n, const double *q,
                    double r[4], double v[3]  )
{
    /* Find link for end of group */
    const char *end_link = tms_container_group_endlink(c, group);

    robot_state::RobotState state(c->robot_model);
    /* Zero positions because somebody's not inititializing their shit */
    robot_state_zero(&state);
    const robot_state::JointModelGroup* joint_model_group
        = state.getJointModelGroup(group);

    /* Set state */
    {
        std::vector<double> joint_values(n, 0.0);
        std::copy ( q, q+n, joint_values.begin() );
        state.setJointGroupPositions(joint_model_group, joint_values);
    }

    return tms_container_link_fk( c, end_link,
                              state.getVariableNames().size(),
                              state.getVariablePositions(),
                              r, v );

}

int
tms_container_link_fk( struct container * c, const char *link, size_t n, const double *q,
                       double r[4], double v[3] )
{

    robot_state::RobotState state(c->robot_model);
    // Make robot state
    {
        double *x = state.getVariablePositions();
        size_t n2 = tms_container_variable_count(c);
        if( n2 != n ) {
            fprintf(stderr, "error: Size mismatch between robot model (%lu) and provided array (%lu)\n",
                    n2, n);
            return -1;
        }
        memcpy( x, q, n*sizeof(x[0]) );
    }

    // get TF
    const Eigen::Affine3d estart_tf = state.getFrameTransform(link);
    {
        const Eigen::Quaternion<double> equat( estart_tf.rotation() );
        r[0] = equat.x();
        r[1] = equat.y();
        r[2] = equat.z();
        r[3] = equat.w();
    }

    {
        const auto e_start = estart_tf.translation();
        v[0] = e_start[0];
        v[1] = e_start[1];
        v[2] = e_start[2];
    }

    return 0;
}

// int
// tms_container_set_planner( struct container * c, const char *planner )
// {
//     c->req.planner_id = planner;
//     return 0;
// }

int
tms_container_set_volume( struct container * c,
                          const double min[3],
                          const double max[3] )
{
    c->req.workspace_parameters.min_corner.x = min[0];
    c->req.workspace_parameters.min_corner.y = min[1];
    c->req.workspace_parameters.min_corner.z = min[2];

    c->req.workspace_parameters.max_corner.x = max[0];
    c->req.workspace_parameters.max_corner.y = max[1];
    c->req.workspace_parameters.max_corner.z = max[2];

    return 0;

}
<commit_msg>Omit moveit printing<commit_after>#include <cstdlib>

#include <Eigen/Geometry>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit/robot_state/conversions.h>
#include "cros.hpp"
#include "c_container.h"
#include "moveit_container.hpp"

static void
robot_state_zero( robot_state::RobotState *state )
{
    double *x = state->getVariablePositions();
    size_t n = state->getVariableNames().size();
    memset(x, 0, n * sizeof(x[0]) );
}

struct container *
tms_container_create( cros_node_handle *nh, const char *robot_description )
{
    return new container(nh->node_handle, robot_description);
}

void
tms_container_destroy( struct container * c)
{
    delete c;
}

size_t
tms_container_variable_count( struct container *c )
{
    return c->robot_model->getVariableCount();
}

int
tms_container_merge_group( struct container *c, const char *group,
                           size_t n_group, const double *q_group,
                           size_t n_all, double *q_all )
{
    if( n_all != tms_container_variable_count(c) ) {
        fprintf(stderr, "error: Size mismatch between robot model (%lu) and provided array (%lu)\n",
                tms_container_variable_count(c), n_all);
        return -1;
    }

    robot_state::RobotState state(c->robot_model);
    state.setVariablePositions(q_all);
    size_t n_all_model = state.getVariableCount();
    state.setJointGroupPositions(group, q_group);
    memcpy(q_all, state.getVariablePositions(), n_all_model*sizeof(q_all[0]));
    return 0;
}

int
tms_container_set_start( struct container * c, size_t n_all, const double *q_all )
{

    if( n_all != tms_container_variable_count(c) ) {
        fprintf(stderr, "error: Size mismatch between robot model (%lu) and provided array (%lu)\n",
                tms_container_variable_count(c), n_all);
        return -1;
    }
    robot_state::RobotState start_state = c->planning_scene->getCurrentState();
    start_state.setVariablePositions(q_all);
    start_state.update(true);

    // c->req.start_state.joint_state.name =  start_state.getVariableNames();
    // {
    //     size_t n = c->req.start_state.joint_state.name.size();
    //     double *p = start_state.getVariablePositions();
    //     c->req.start_state.joint_state.position.resize( n );
    //     std::copy ( p, p+n,
    //                 c->req.start_state.joint_state.position.begin() );
    // }
    // c->req.start_state.is_diff = true;
    c->planning_scene->setCurrentState(start_state);

    // // Print stuff
    // {
    //     double r[4],v[3];
    //     tms_container_group_fk( c, group, n, q, r, v );

    //     fprintf(stderr,
    //             "r_start[4] = {%f, %f, %f, %f}\n"
    //             "v_start[3] = {%f, %f, %f}\n",
    //             r[0], r[1], r[2], r[3],
    //             v[0], v[1], v[2] );
    // }
    return 0;
}

int
tms_container_set_group( struct container * c, const char *group )
{
    c->req.group_name = group;
    return 0;
}

int
tms_container_goal_clear( struct container *c )
{
    c->req.goal_constraints.clear();
    return 0;
}

int
tms_container_set_js_goal( struct container * c, const char *group, size_t n_all, double *q_all )
{

    if( n_all != tms_container_variable_count(c) ) {
        fprintf(stderr, "error: Size mismatch between robot model (%lu) and provided array (%lu)\n",
                tms_container_variable_count(c), n_all);
        return -1;
    }

    robot_state::RobotState goal_state(c->robot_model);
    goal_state.setVariablePositions(q_all);

    const robot_state::JointModelGroup* joint_model_group = goal_state.getJointModelGroup(group);

    moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state,
                                                                                joint_model_group);
    c->req.goal_constraints.push_back(joint_goal);

    {
        double *x = goal_state.getVariablePositions();
        for( size_t i = 0; i < goal_state.getVariableNames().size(); i++ ) {
            fprintf(stderr, "goal %lu: %f\n", i, x[i] );
        }
    }

    return 0;
}

int
tms_container_set_ws_goal( struct container * c, const char *link, const double quat[4], const double vec[3],
                           double tol_x, double tol_angle )
{
    geometry_msgs::PoseStamped stamped_pose;
    stamped_pose.header.frame_id = "base";
    geometry_msgs::Pose &pose = stamped_pose.pose;

    pose.orientation.x = quat[0];
    pose.orientation.y = quat[1];
    pose.orientation.z = quat[2];
    pose.orientation.w = quat[3];
    pose.position.x = vec[0];
    pose.position.y = vec[1];
    pose.position.z = vec[2];

    fprintf(stderr,"constructing pose goal\n");
    moveit_msgs::Constraints pose_goal =
        kinematic_constraints::constructGoalConstraints(link, stamped_pose, tol_x, tol_angle );
    fprintf(stderr,"adding pose goal...\n");
    c->req.goal_constraints.push_back(pose_goal);
    fprintf(stderr,"...added\n");
    return 0;

    // robot_state::RobotState goal_state(c->robot_model);
    // /* Zero positions because somebody's not inititializing their shit */
    // robot_state_zero( &goal_state);

    // const robot_state::JointModelGroup* joint_model_group = goal_state.getJointModelGroup(group);
    // bool got_ik = goal_state.setFromIK(joint_model_group,pose);
    // fprintf(stderr, "IK: %s\n", got_ik ? "yes" : "no" );

    // if( ! got_ik ) return -1;

    // moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state,
    //                                                                                joint_model_group);
    // c->req.goal_constraints.push_back(joint_goal);

    // {
    //     double *x = goal_state.getVariablePositions();
    //     for( size_t i = 0; i < goal_state.getVariableNames().size(); i++ ) {
    //         fprintf(stderr, "goal %lu: %f\n", i, x[i] );
    //     }
    // }

    // {
    //     double r[4],v[3];

    //     tms_container_link_fk( c, tms_container_group_endlink( c, group ),
    //                        goal_state.getVariableNames().size(),
    //                        goal_state.getVariablePositions(),
    //                        r, v );

    //     fprintf(stderr,
    //             "r_goal[4] = {%f, %f, %f, %f}\n"
    //             "v_goal[3] = {%f, %f, %f}\n",
    //             r[0], r[1], r[2], r[3],
    //             v[0], v[1], v[2] );
    // }

}

int
tms_container_plan( struct container * c,
                    double timeout,
                    size_t *n_vars, size_t *n_points, double **points)
{
    /**********/
    /*  PLAN  */
    /**********/
    moveit_msgs::MoveItErrorCodes err;
    planning_interface::MotionPlanResponse res;
    c->req.allowed_planning_time = timeout;

    moveit::core::robotStateToRobotStateMsg(c->planning_scene->getCurrentState(), c->req.start_state);

    //std::cout << c->req << std::endl;

    planning_interface::PlanningContextPtr context =
        c->planner_instance->getPlanningContext(c->planning_scene, c->req, err);
    context->solve(res);
    if(res.error_code_.val != res.error_code_.SUCCESS)
    {
        //TODO: Why is this 0 instead of SUCCESS?
        fprintf(stderr, "Planning failed: %d\n", res.error_code_.val );
        //return 0;
    }

    if(res.error_code_.val < 0 ) {
        fprintf(stderr, "returning failure\n");
        return -1;
    }

    /************/
    /*  Result  */
    /************/
    moveit_msgs::MotionPlanResponse res_msg;
    res.getMessage(res_msg);
    int i = 0;
    // for( auto itr = res_msg.trajectory.joint_trajectory.points.begin();
    //      itr != res_msg.trajectory.joint_trajectory.points.end();
    //      itr++ )
    // {
    //     printf("waypoint %02d: ", i++);
    //     for( auto j = itr->positions.begin();
    //          j != itr->positions.end();
    //          j++ )
    //     {
    //         printf("%f\t", *j );
    //     }
    //     printf("\n");
    // }

    /* Copy Result */
    *n_points = res_msg.trajectory.joint_trajectory.points.size();
    if( 0 == n_points ) return 0;
    *n_vars = res_msg.trajectory.joint_trajectory.points[0].positions.size();
    i = 0;
    double *start = (double*)malloc( sizeof(*start) * (*n_points) * (*n_vars) );
    *points = start;
    for( auto itr = res_msg.trajectory.joint_trajectory.points.begin();
         itr != res_msg.trajectory.joint_trajectory.points.end();
         itr++ )
    {
        size_t s = itr->positions.size();
        if( s != *n_vars ) {
            fprintf(stderr, "Bogus variable count\n");
            *n_points = 0;
            free(points);
            return -1;
        }
        std::copy( itr->positions.begin(), itr->positions.end(), start );

        // for( size_t j = 0; j < *n_vars; j ++ ) {
        //     printf("%f\t", start[j] );
        // }
        // printf("\n");

        start = start +  (*n_vars);
    }

    // printf("%lx\n",*points);

    // for(i = 0; i < (*n_vars)*(*n_points); i++ ){
    //     printf("%f%c", (*points)[i], ( (i+1) % (*n_vars) ) ? '\t' : '\n'   );
    // }

    /***************/
    /*  Visualize  */
    /***************/
    moveit_msgs::DisplayTrajectory display_trajectory;

    ROS_INFO("Visualizing plan 1 (again)");
    display_trajectory.trajectory_start = res_msg.trajectory_start;
    display_trajectory.trajectory.push_back(res_msg.trajectory);
    c->display_publisher.publish(display_trajectory);

    return 0;
}

const char *
tms_container_group_endlink( struct container * c, const char *group )
{
    robot_state::RobotState state(c->robot_model);
    const robot_state::JointModelGroup* joint_model_group
        = state.getJointModelGroup(group);
    const std::vector<std::string> &link_names = joint_model_group->getLinkModelNames();
    const std::string & end_link =  link_names[ link_names.size()-1];
    return end_link.c_str();
}

size_t
tms_container_group_joint_count( struct container * c, const char *group )
{
    robot_state::RobotState state(c->robot_model);
    const robot_state::JointModelGroup* joint_model_group
        = state.getJointModelGroup(group);
    return joint_model_group->getVariableCount();
}

const char *
tms_container_group_joint_name( struct container * c, const char *group, size_t i )
{
    robot_state::RobotState state(c->robot_model);
    const robot_state::JointModelGroup* joint_model_group
        = state.getJointModelGroup(group);
    const std::vector< std::string > & names = joint_model_group->getActiveJointModelNames();
    if( i < names.size() ) {
        return names[i].c_str();
    } else {
        return NULL;
    }
}

int
tms_container_group_fk( struct container * c, const char *group, size_t n, const double *q,
                    double r[4], double v[3]  )
{
    /* Find link for end of group */
    const char *end_link = tms_container_group_endlink(c, group);

    robot_state::RobotState state(c->robot_model);
    /* Zero positions because somebody's not inititializing their shit */
    robot_state_zero(&state);
    const robot_state::JointModelGroup* joint_model_group
        = state.getJointModelGroup(group);

    /* Set state */
    {
        std::vector<double> joint_values(n, 0.0);
        std::copy ( q, q+n, joint_values.begin() );
        state.setJointGroupPositions(joint_model_group, joint_values);
    }

    return tms_container_link_fk( c, end_link,
                              state.getVariableNames().size(),
                              state.getVariablePositions(),
                              r, v );

}

int
tms_container_link_fk( struct container * c, const char *link, size_t n, const double *q,
                       double r[4], double v[3] )
{

    robot_state::RobotState state(c->robot_model);
    // Make robot state
    {
        double *x = state.getVariablePositions();
        size_t n2 = tms_container_variable_count(c);
        if( n2 != n ) {
            fprintf(stderr, "error: Size mismatch between robot model (%lu) and provided array (%lu)\n",
                    n2, n);
            return -1;
        }
        memcpy( x, q, n*sizeof(x[0]) );
    }

    // get TF
    const Eigen::Affine3d estart_tf = state.getFrameTransform(link);
    {
        const Eigen::Quaternion<double> equat( estart_tf.rotation() );
        r[0] = equat.x();
        r[1] = equat.y();
        r[2] = equat.z();
        r[3] = equat.w();
    }

    {
        const auto e_start = estart_tf.translation();
        v[0] = e_start[0];
        v[1] = e_start[1];
        v[2] = e_start[2];
    }

    return 0;
}

// int
// tms_container_set_planner( struct container * c, const char *planner )
// {
//     c->req.planner_id = planner;
//     return 0;
// }

int
tms_container_set_volume( struct container * c,
                          const double min[3],
                          const double max[3] )
{
    c->req.workspace_parameters.min_corner.x = min[0];
    c->req.workspace_parameters.min_corner.y = min[1];
    c->req.workspace_parameters.min_corner.z = min[2];

    c->req.workspace_parameters.max_corner.x = max[0];
    c->req.workspace_parameters.max_corner.y = max[1];
    c->req.workspace_parameters.max_corner.z = max[2];

    return 0;

}
<|endoftext|>