<commit_before>#include "ros/ros.h"
#include "std_msgs/String.h"

#include "EventTriggerUtility.h"
#include "elderly_care_simulation/EventTrigger.h"
#include <queue>
#include "EventNode.h"
#include <unistd.h> // sleep

// constants
const int MAX_CONCURRENT_WEIGHT = 2;

// globals
std::priority_queue<EventNode > eventQueue;
ros::Publisher eventTriggerPub;
ros::Subscriber eventTriggerSub;
ros::Subscriber randomEventSub;
int concurrentWeight = 0;

// ======================================
// =          SHOULD BE FALSE           =
// ======================================
bool allowNewEvents = true;
bool stopRosInfoSpam = false;

/**
 * Creates a Request EventTrigger message for requesting robot tasks.
 * @param eventType the event type for the message
 */
elderly_care_simulation::EventTrigger createEventRequestMsg(int eventType, int priority){
    elderly_care_simulation::EventTrigger msg;
    msg.msg_type = EVENT_TRIGGER_MSG_TYPE_REQUEST;
    msg.event_type = eventType;
    msg.event_priority = priority;
    msg.event_weight = getEventWeight(eventType);
    msg.result = EVENT_TRIGGER_RESULT_UNDEFINED;

    return msg;
}

/**
 * Clears the eventQueue.
 */
void clearEventQueue() {
    eventQueue = std::priority_queue<EventNode >();
}

/**
 * Callback function to deal with random events published by the resident
 */
void randomEventReceivedCallback(elderly_care_simulation::EventTrigger msg) {

    // Only allows random events to be added to event queue in the allowed
    // timeframe (between WAKE and SLEEP)
    if(!allowNewEvents) {
        ROS_INFO("Scheduler: Additional events are not allowed at this time.");
        return;
    }

    ROS_INFO("Scheduler: Enqueuing event: [%s] with priority [%s]",
             eventTypeToString(msg.event_type),
             priorityToString(msg.event_priority));

    eventQueue.push(EventNode(msg));
}

/**
 * Callback function to deal with events replied from service provider robots
 */
void eventTriggerCallback(elderly_care_simulation::EventTrigger msg) {

    if (msg.msg_type == EVENT_TRIGGER_MSG_TYPE_RESPONSE) {
        if(msg.result == EVENT_TRIGGER_RESULT_SUCCESS){

            if (msg.event_type == EVENT_TRIGGER_EVENT_TYPE_COOK) {
                elderly_care_simulation::EventTrigger msg;
                msg = createEventRequestMsg(EVENT_TRIGGER_EVENT_TYPE_EAT, EVENT_TRIGGER_PRIORITY_HIGH);

                ROS_INFO("Scheduler: Enqueuing event: [%s] with priority [%s]",
                          eventTypeToString(msg.event_type),
                          priorityToString(msg.event_priority));

                eventQueue.push(EventNode(msg));
            }else{
                concurrentWeight -= msg.event_weight;
                ROS_INFO("Scheduler: [%s] done.", eventTypeToString(msg.event_type));
            }
        }
    }
}

/**
 * Populates daily scheduled tasks into the event queue.
 *
 * Daily Schedule will be queued in the following sequence:
 * NOTE: The timestamp is just for referencing purpose.
 *
 *  07:00   WAKE
 *  07:00   COOK ---> EAT
 *  07:00   MEDICATION
 *  08:00   EXERCISE
 *  09:00   SHOWER
 *  10:00   ENTERTAINMENT
 *
 *  12:00   COOK ---> EAT
 *  12:00   MEDICATION
 *  13:00   CONVERSATION
 *  14:00   FRIEND & RELATIVE
 *  16:00   ENTERTAINMENT
 *
 *  18:00   COOK ---> EAT
 *  18:00   MEDICATION
 *  19:00   COMPANIONSHIP
 *  20:00   SLEEP
 *
 *  PAUSE FOR 20 SEC
 *  CLEAR LIST & REPOPULATE DAILY TASKS
 *
 */
void populateDailyTasks(void) {

    int eventSequence[][2] = {

        // ======================================
        // =        COMMENTED OUT STUFF         =
        // ======================================

        // Morning
        { EVENT_TRIGGER_EVENT_TYPE_WAKE,            EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_COOK,            EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_MEDICATION,      EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_EXERCISE,        EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_SHOWER,          EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_ENTERTAINMENT,   EVENT_TRIGGER_PRIORITY_LOW },

        // Noon
        { EVENT_TRIGGER_EVENT_TYPE_COOK,            EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_MEDICATION,      EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_CONVERSATION,    EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_FRIEND_RELATIVE, EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_ENTERTAINMENT,   EVENT_TRIGGER_PRIORITY_LOW },

        // Evening
        { EVENT_TRIGGER_EVENT_TYPE_COOK,            EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_MEDICATION,      EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_COMPANIONSHIP,   EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_SLEEP,           EVENT_TRIGGER_PRIORITY_VERY_LOW }
    };
    for(unsigned int i = 0; i < sizeof(eventSequence)/sizeof(*eventSequence); i++) {
        eventQueue.push(EventNode(createEventRequestMsg(eventSequence[i][0], eventSequence[i][1])));
    }
}

/**
 * Dequeues an event and publishes to the event_trigger topic.
 * Allows concurrent events to be triggered up to the limit
 * set in MAX_CONCURRENT_WEIGHT
 *
 * COOK event is not affected as it acts independently of the
 * Resident.
 */
void dequeueEvent(void) {

    if (eventQueue.size() < 1) {
        return;
    }

    elderly_care_simulation::EventTrigger msg;
    msg = eventQueue.top().getEventTriggerMessage();

    // Publish event if enough concurrent weight available
    if (concurrentWeight + msg.event_weight <= MAX_CONCURRENT_WEIGHT) {

        stopRosInfoSpam = false;
        switch(msg.event_type) {
            case EVENT_TRIGGER_EVENT_TYPE_SLEEP:
                allowNewEvents = false;

                eventQueue.pop();
                eventTriggerPub.publish(msg);
                ROS_INFO("Scheduler: Publishing event: [%s]", eventTypeToString(msg.event_type));
                concurrentWeight += msg.event_weight;

            case EVENT_TRIGGER_EVENT_TYPE_VERY_ILL:
                allowNewEvents = false;

                ROS_INFO("Scheduler: Publishing event: [%s]", eventTypeToString(msg.event_type));
                concurrentWeight += msg.event_weight;
                eventTriggerPub.publish(msg);

                clearEventQueue();

                // Enqueue a SLEEP event with max priority so when resident comes back from
                // hospital it goes to sleep right away.
                // Since the queue is now empty after the SLEEP event, a new batch of daily
                // schedules will be repopulated automatically (in the main method).
                eventQueue.push(EventNode(createEventRequestMsg(EVENT_TRIGGER_EVENT_TYPE_SLEEP,
                                                                EVENT_TRIGGER_PRIORITY_VERY_HIGH)));

            default:
                allowNewEvents = true;
                eventTriggerPub.publish(msg);
                ROS_INFO("Scheduler: Publishing event: [%s]", eventTypeToString(msg.event_type));
                concurrentWeight += msg.event_weight;
                eventQueue.pop();
                break;
        }

    } else {
        if(!stopRosInfoSpam){
            ROS_INFO("Scheduler: Event: [%s] waiting for additional concurrent weight.",
                eventTypeToString(msg.event_type));
            stopRosInfoSpam = true;
        }
    }
}

/**
 * Main
 */
int main(int argc, char **argv) {

    //You must call ros::init() first of all. ros::init() function needs to see argc and argv. The third argument is the name of the node
    ros::init(argc, argv, "Scheduler");

    //NodeHandle is the main access point to communicate with ros.
    ros::NodeHandle nodeHandle;

    // advertise to event_trigger topic
    eventTriggerPub = nodeHandle.advertise<elderly_care_simulation::EventTrigger>("event_trigger",1000, true);

    // subscribe to event_trigger topic
    eventTriggerSub = nodeHandle.subscribe<elderly_care_simulation::EventTrigger>("event_trigger",1000, eventTriggerCallback);
    randomEventSub = nodeHandle.subscribe<elderly_care_simulation::EventTrigger>("resident_event",1000, randomEventReceivedCallback);

    ros::Rate loop_rate(10);

    // populate queue with day's events
    populateDailyTasks();

    //a count of howmany messages we have sent
    int count = 0;

    while (ros::ok()) {

        // ======================================
        // =        COMMENTED OUT STUFF         =
        // ======================================
        // if(eventQueue.size() == 0 && concurrentWeight == 0) {
        //     sleep(5);
        //     clearEventQueue();
        //     populateDailyTasks();
        // }else {
        //     dequeueEvent();
        // }
        dequeueEvent();

        ros::spinOnce();
        loop_rate.sleep();
        ++count;
    }
    return 0;
}
<commit_msg>Tiny typo fix.<commit_after>#include "ros/ros.h"
#include "std_msgs/String.h"

#include "EventTriggerUtility.h"
#include "elderly_care_simulation/EventTrigger.h"
#include <queue>
#include "EventNode.h"
#include <unistd.h> // sleep

// constants
const int MAX_CONCURRENT_WEIGHT = 2;

// globals
std::priority_queue<EventNode > eventQueue;
ros::Publisher eventTriggerPub;
ros::Subscriber eventTriggerSub;
ros::Subscriber randomEventSub;
int concurrentWeight = 0;

// ======================================
// =          SHOULD BE FALSE           =
// ======================================
bool allowNewEvents = true;
bool stopRosInfoSpam = false;

/**
 * Creates a Request EventTrigger message for requesting robot tasks.
 * @param eventType the event type for the message
 */
elderly_care_simulation::EventTrigger createEventRequestMsg(int eventType, int priority){
    elderly_care_simulation::EventTrigger msg;
    msg.msg_type = EVENT_TRIGGER_MSG_TYPE_REQUEST;
    msg.event_type = eventType;
    msg.event_priority = priority;
    msg.event_weight = getEventWeight(eventType);
    msg.result = EVENT_TRIGGER_RESULT_UNDEFINED;

    return msg;
}

/**
 * Clears the eventQueue.
 */
void clearEventQueue() {
    eventQueue = std::priority_queue<EventNode >();
}

/**
 * Callback function to deal with random events published by the resident
 */
void randomEventReceivedCallback(elderly_care_simulation::EventTrigger msg) {

    // Only allows random events to be added to event queue in the allowed
    // timeframe (between WAKE and SLEEP)
    if(!allowNewEvents) {
        ROS_INFO("Scheduler: Additional events are not allowed at this time.");
        return;
    }

    ROS_INFO("Scheduler: Enqueuing event: [%s] with priority [%s]",
             eventTypeToString(msg.event_type),
             priorityToString(msg.event_priority));

    eventQueue.push(EventNode(msg));
}

/**
 * Callback function to deal with events replied from service provider robots
 */
void eventTriggerCallback(elderly_care_simulation::EventTrigger msg) {

    if (msg.msg_type == EVENT_TRIGGER_MSG_TYPE_RESPONSE) {
        if(msg.result == EVENT_TRIGGER_RESULT_SUCCESS){

            if (msg.event_type == EVENT_TRIGGER_EVENT_TYPE_COOK) {
                elderly_care_simulation::EventTrigger msg;
                msg = createEventRequestMsg(EVENT_TRIGGER_EVENT_TYPE_EAT, EVENT_TRIGGER_PRIORITY_HIGH);

                ROS_INFO("Scheduler: Enqueuing event: [%s] with priority [%s]",
                          eventTypeToString(msg.event_type),
                          priorityToString(msg.event_priority));

                eventQueue.push(EventNode(msg));
            }else{
                concurrentWeight -= msg.event_weight;
                ROS_INFO("Scheduler: [%s] done.", eventTypeToString(msg.event_type));
            }
        }
    }
}

/**
 * Populates daily scheduled tasks into the event queue.
 *
 * Daily Schedule will be queued in the following sequence:
 * NOTE: The timestamp is just for referencing purpose.
 *
 *  07:00   WAKE
 *  07:00   COOK ---> EAT
 *  07:00   MEDICATION
 *  08:00   EXERCISE
 *  09:00   SHOWER
 *  10:00   ENTERTAINMENT
 *
 *  12:00   COOK ---> EAT
 *  12:00   MEDICATION
 *  13:00   CONVERSATION
 *  14:00   FRIEND & RELATIVE
 *  16:00   ENTERTAINMENT
 *
 *  18:00   COOK ---> EAT
 *  18:00   MEDICATION
 *  19:00   COMPANIONSHIP
 *  20:00   SLEEP
 *
 *  PAUSE FOR 20 SEC
 *  CLEAR LIST & REPOPULATE DAILY TASKS
 *
 */
void populateDailyTasks(void) {

    int eventSequence[][2] = {

        // ======================================
        // =        COMMENTED OUT STUFF         =
        // ======================================

        // Morning
        { EVENT_TRIGGER_EVENT_TYPE_WAKE,            EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_COOK,            EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_MEDICATION,      EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_EXERCISE,        EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_SHOWER,          EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_ENTERTAINMENT,   EVENT_TRIGGER_PRIORITY_LOW },

        // Noon
        { EVENT_TRIGGER_EVENT_TYPE_COOK,            EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_MEDICATION,      EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_CONVERSATION,    EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_FRIEND_RELATIVE, EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_ENTERTAINMENT,   EVENT_TRIGGER_PRIORITY_LOW },

        // Evening
        { EVENT_TRIGGER_EVENT_TYPE_COOK,            EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_MEDICATION,      EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_COMPANIONSHIP,   EVENT_TRIGGER_PRIORITY_LOW },
        { EVENT_TRIGGER_EVENT_TYPE_SLEEP,           EVENT_TRIGGER_PRIORITY_VERY_LOW }
    };
    for(unsigned int i = 0; i < sizeof(eventSequence)/sizeof(*eventSequence); i++) {
        eventQueue.push(EventNode(createEventRequestMsg(eventSequence[i][0], eventSequence[i][1])));
    }
}

/**
 * Dequeues an event and publishes to the event_trigger topic.
 * Allows concurrent events to be triggered up to the limit
 * set in MAX_CONCURRENT_WEIGHT
 *
 * COOK event is not affected as it acts independently of the
 * Resident.
 */
void dequeueEvent(void) {

    if (eventQueue.size() < 1) {
        return;
    }

    elderly_care_simulation::EventTrigger msg;
    msg = eventQueue.top().getEventTriggerMessage();

    // Publish event if enough concurrent weight available
    if (concurrentWeight + msg.event_weight <= MAX_CONCURRENT_WEIGHT) {

        stopRosInfoSpam = false;
        switch(msg.event_type) {
            case EVENT_TRIGGER_EVENT_TYPE_SLEEP:
                allowNewEvents = false;

                eventQueue.pop();
                eventTriggerPub.publish(msg);
                ROS_INFO("Scheduler: Publishing event: [%s]", eventTypeToString(msg.event_type));
                concurrentWeight += msg.event_weight;

            case EVENT_TRIGGER_EVENT_TYPE_VERY_ILL:
                allowNewEvents = false;

                ROS_INFO("Scheduler: Publishing event: [%s]", eventTypeToString(msg.event_type));
                concurrentWeight += msg.event_weight;
                eventTriggerPub.publish(msg);

                clearEventQueue();

                // Enqueue a SLEEP event with max priority so when resident comes back from
                // hospital it goes to sleep right away.
                // Since the queue is now empty after the SLEEP event, a new batch of daily
                // schedules will be repopulated automatically (in the main method).
                eventQueue.push(EventNode(createEventRequestMsg(EVENT_TRIGGER_EVENT_TYPE_SLEEP,
                                                                EVENT_TRIGGER_PRIORITY_VERY_HIGH)));

            default:
                allowNewEvents = true;
                eventTriggerPub.publish(msg);
                ROS_INFO("Scheduler: Publishing event: [%s]", eventTypeToString(msg.event_type));
                concurrentWeight += msg.event_weight;
                eventQueue.pop();
                break;
        }

    } else {
        if(!stopRosInfoSpam){
            ROS_INFO("Scheduler: Event: [%s] is waiting for additional concurrent weight.",
                eventTypeToString(msg.event_type));
            stopRosInfoSpam = true;
        }
    }
}

/**
 * Main
 */
int main(int argc, char **argv) {

    //You must call ros::init() first of all. ros::init() function needs to see argc and argv. The third argument is the name of the node
    ros::init(argc, argv, "Scheduler");

    //NodeHandle is the main access point to communicate with ros.
    ros::NodeHandle nodeHandle;

    // advertise to event_trigger topic
    eventTriggerPub = nodeHandle.advertise<elderly_care_simulation::EventTrigger>("event_trigger",1000, true);

    // subscribe to event_trigger topic
    eventTriggerSub = nodeHandle.subscribe<elderly_care_simulation::EventTrigger>("event_trigger",1000, eventTriggerCallback);
    randomEventSub = nodeHandle.subscribe<elderly_care_simulation::EventTrigger>("resident_event",1000, randomEventReceivedCallback);

    ros::Rate loop_rate(10);

    // populate queue with day's events
    populateDailyTasks();

    //a count of howmany messages we have sent
    int count = 0;

    while (ros::ok()) {

        // ======================================
        // =        COMMENTED OUT STUFF         =
        // ======================================
        // if(eventQueue.size() == 0 && concurrentWeight == 0) {
        //     sleep(5);
        //     clearEventQueue();
        //     populateDailyTasks();
        // }else {
        //     dequeueEvent();
        // }
        dequeueEvent();

        ros::spinOnce();
        loop_rate.sleep();
        ++count;
    }
    return 0;
}
<|endoftext|>


<commit_before>#pragma once

#if defined(_WIN32) || defined(WIN32) || defined(_MSC_VER)
#define ARBITER_WINDOWS
#endif

#ifndef ARBITER_DLL
#if defined(ARBITER_WINDOWS)
#if defined(ARBITER_DLL_EXPORT)
#   define ARBITER_DLL   __declspec(dllexport)
#elif defined(PDAL_DLL_IMPORT)
#   define ARBITER_DLL   __declspec(dllimport)
#else
#   define ARBITER_DLL
#endif
#else
#  if defined(USE_GCC_VISIBILITY_FLAG)
#    define ARBITER_DLL     __attribute__ ((visibility("default")))
#  else
#    define ARBITER_DLL
#  endif
#endif
#endif<commit_msg>Warnings.<commit_after>#pragma once

#if defined(_WIN32) || defined(WIN32) || defined(_MSC_VER)
#define ARBITER_WINDOWS
#endif

#ifndef ARBITER_DLL
#if defined(ARBITER_WINDOWS)
#if defined(ARBITER_DLL_EXPORT)
#   define ARBITER_DLL   __declspec(dllexport)
#elif defined(PDAL_DLL_IMPORT)
#   define ARBITER_DLL   __declspec(dllimport)
#else
#   define ARBITER_DLL
#endif
#else
#  if defined(USE_GCC_VISIBILITY_FLAG)
#    define ARBITER_DLL     __attribute__ ((visibility("default")))
#  else
#    define ARBITER_DLL
#  endif
#endif
#endif

#ifdef _WIN32
#pragma warning(disable:4251)// [templated class] needs to have dll-interface...
#endif

<|endoftext|>


<commit_before>#include <iostream>
#include <queue>
#include <utility>
#include <stdio.h>
#include <vector>
using namespace std;
typedef  pair<long long ,long long> node;

long long re[1000000];
int main(int argc, char const *argv[])
{
	priority_queue<node ,vector<node> ,greater<node> > pq;

	pq.push(node(1,2));
	node n;
	int i=0;
	while(i<1000){

		node n=pq.top();
		//printf("%d",i);
		pq.pop();
		switch(n.second){
			case 2:
				pq.push(node(n.first*2,2));

			case 3:
				pq.push(node(n.first*3,3));

			case 5:
				pq.push(node(n.first*5,5));
		}
		re[i++]=n.first;
	}
	for (int j = 0; j < i; ++j)
	{
		printf("%d\n",re[j]);
	}

	return 0;
}
<commit_msg>update 346588<commit_after>#include <iostream>
#include <queue>
#include <utility>
#include <stdio.h>
#include <vector>
using namespace std;
typedef  pair<long long ,long long> node;

long long re[1000000];
int main(int argc, char const *argv[])
{
	priority_queue<node ,vector<node> ,greater<node> > pq;

	pq.push(node(1,2));
	node n;
	int i=0;
	while(i<1200){

		node n=pq.top();
		//printf("%d",i);
		pq.pop();
		switch(n.second){
			case 2:
				pq.push(node(n.first*2,2));

			case 3:
				pq.push(node(n.first*3,3));

			case 5:
				pq.push(node(n.first*5,5));
		}
		re[i++]=n.first;
	}
	for (int j = 0; j < i; ++j)
	{
		printf("%d\n",re[j]);
	}

	return 0;
}
<|endoftext|>


<commit_before>
#include "Beetle.h"

#include <boost/thread/lock_types.hpp>
#include <boost/thread/pthread/shared_mutex.hpp>
#include <cassert>
#include <thread>
#include <utility>

#include "CLI.h"
#include "device/BeetleDevice.h"
#include "Debug.h"
#include "Device.h"
#include "hat/BlockAllocator.h"
#include "hat/HAT.h"
#include "Router.h"
#include "tcp/TCPDeviceServer.h"

/* Global debug variable */
bool debug = true;

int main(int argc, char *argv[]) {
	Beetle btl;
	TCPDeviceServer(btl, 5000);

	CLI cli(btl);
	cli.join();
}

Beetle::Beetle() {
	hat = new BlockAllocator(256);
	router = new Router(*this);
	beetleDevice = new BeetleDevice(*this, "Beetle");
	devices[BEETLE_RESERVED_DEVICE] = beetleDevice;
}

Beetle::~Beetle() {

}

void Beetle::addDevice(Device *d, bool allocateHandles) {
	boost::unique_lock<boost::shared_mutex> deviceslk(devicesMutex);
	boost::unique_lock<boost::shared_mutex> hatLk(hatMutex);
	devices[d->getId()] = d;
	if (allocateHandles) {
		handle_range_t range = hat->reserve(d->getId());
		assert(!HAT::isNullRange(range));
	}
}

void Beetle::removeDevice(device_t id) {
	if (id == BEETLE_RESERVED_DEVICE) {
		pdebug("not allowed to remove Beetle!");
		return;
	}
	boost::unique_lock<boost::shared_mutex> lk(devicesMutex);
	Device *d = devices[id];
	devices.erase(id);
	for (auto &kv : devices) {
		kv.second->unsubscribeAll(id);
	}
	std::thread t([d]() { delete d; });
	t.detach();
}

<commit_msg>fixed bug with server exiting<commit_after>
#include "Beetle.h"

#include <boost/thread/lock_types.hpp>
#include <boost/thread/pthread/shared_mutex.hpp>
#include <cassert>
#include <thread>
#include <utility>

#include "CLI.h"
#include "device/BeetleDevice.h"
#include "Debug.h"
#include "Device.h"
#include "hat/BlockAllocator.h"
#include "hat/HAT.h"
#include "Router.h"
#include "tcp/TCPDeviceServer.h"

/* Global debug variable */
bool debug = true;

int main(int argc, char *argv[]) {
	Beetle btl;
	TCPDeviceServer tcpServer(btl, 5000);

	CLI cli(btl);
	cli.join();
}

Beetle::Beetle() {
	hat = new BlockAllocator(256);
	router = new Router(*this);
	beetleDevice = new BeetleDevice(*this, "Beetle");
	devices[BEETLE_RESERVED_DEVICE] = beetleDevice;
}

Beetle::~Beetle() {

}

void Beetle::addDevice(Device *d, bool allocateHandles) {
	boost::unique_lock<boost::shared_mutex> deviceslk(devicesMutex);
	boost::unique_lock<boost::shared_mutex> hatLk(hatMutex);
	devices[d->getId()] = d;
	if (allocateHandles) {
		handle_range_t range = hat->reserve(d->getId());
		assert(!HAT::isNullRange(range));
	}
}

void Beetle::removeDevice(device_t id) {
	if (id == BEETLE_RESERVED_DEVICE) {
		pdebug("not allowed to remove Beetle!");
		return;
	}
	boost::unique_lock<boost::shared_mutex> lk(devicesMutex);
	Device *d = devices[id];
	devices.erase(id);
	for (auto &kv : devices) {
		kv.second->unsubscribeAll(id);
	}
	std::thread t([d]() { delete d; });
	t.detach();
}

<|endoftext|>


<commit_before>#ifndef SORTED__HPP__
#define SORTED__HPP__

#include "iterbase.hpp"

#include <iterator>
#include <algorithm>
#include <vector>

namespace iter {
    template <typename Container, typename CompareFunc>
    class Sorted;

    template <typename Container, typename CompareFunc>
    Sorted<Container, CompareFunc> sorted(Container&, CompareFunc);

    template <typename Container, typename CompareFunc>
    class Sorted  {
        private:
            friend Sorted
                sorted<Container, CompareFunc>(Container&, CompareFunc);

            std::vector<iterator_type<Container>> sorted_iters;

            using sorted_iter_type = decltype(std::begin(sorted_iters));

            Sorted() = delete;
            Sorted& operator=(const Sorted&) = delete;

            Sorted(Container& container, CompareFunc compare_func) {
                // Fill the sorted_iters vector with an iterator to each
                // element in the container
                for (auto iter = std::begin(container);
                        iter != std::end(container);
                        ++iter) {
                    sorted_iters.push_back(iter);
                }

                // sort by comparing the elements that the iterators point to
                std::sort(std::begin(sorted_iters), std::end(sorted_iters),
                        [&] (const iterator_type<Container>& it1,
                            const iterator_type<Container>& it2)
                        { return compare_func(*it1, *it2); });
            }

        public:

            Sorted(const Sorted&) = default;

            // Iterates over a series of Iterators, automatically dereferencing
            // them when accessed with operator *
            class IteratorIterator : public sorted_iter_type {
                public:
                    IteratorIterator(sorted_iter_type iter)
                        : sorted_iter_type{iter}
                    { }
                    IteratorIterator(const IteratorIterator&) = default;

                    // Dereference the current iterator before returning
                    iterator_deref<Container> operator*() {
                        return *sorted_iter_type::operator*();
                    }
            };

            IteratorIterator begin() {
                return {std::begin(sorted_iters)};

            }

            IteratorIterator end() {
                return {std::end(sorted_iters)};
            }
    };

    template <typename Container, typename CompareFunc>
    Sorted<Container, CompareFunc> sorted(
            Container& container, CompareFunc compare_func) {
        return {container, compare_func};
    }

    template <typename Container>
    auto sorted(Container& container) ->
             decltype(sorted(
                         container,
                         std::less<decltype(
                             *std::begin(std::declval<Container>()))>()
                         ))
     {
         return sorted(
                 container,
                 std::less<decltype(
                     *std::begin(std::declval<Container>()))>()
                 );
    }

}

#endif //#ifndef SORTED__HPP__
<commit_msg>Adds support for temporaries to sorted +more<commit_after>#ifndef SORTED__HPP__
#define SORTED__HPP__

#include "iterbase.hpp"

#include <iterator>
#include <algorithm>
#include <vector>

namespace iter {
    template <typename Container, typename CompareFunc>
    class Sorted;

    template <typename Container, typename CompareFunc>
    Sorted<Container, CompareFunc> sorted(Container&&, CompareFunc);

    template <typename Container, typename CompareFunc>
    class Sorted  {
        private:
            Container container;
            std::vector<iterator_type<Container>> sorted_iters;

            friend Sorted
                sorted<Container, CompareFunc>(Container&&, CompareFunc);

            using sorted_iter_type = iterator_type<decltype(sorted_iters)>;

            Sorted() = delete;
            Sorted& operator=(const Sorted&) = delete;

            Sorted(Container in_container, CompareFunc compare_func)
                : container(std::forward<Container>(in_container))
            {
                // Fill the sorted_iters vector with an iterator to each
                // element in the container
                for (auto iter = std::begin(this->container);
                        iter != std::end(this->container);
                        ++iter) {
                    this->sorted_iters.push_back(iter);
                }

                // sort by comparing the elements that the iterators point to
                std::sort(std::begin(sorted_iters), std::end(sorted_iters),
                        [&] (const iterator_type<Container>& it1,
                            const iterator_type<Container>& it2)
                        { return compare_func(*it1, *it2); });
            }

        public:

            Sorted(const Sorted&) = default;

            // Iterates over a series of Iterators, automatically dereferencing
            // them when accessed with operator *
            class IteratorIterator : public sorted_iter_type {
                public:
                    IteratorIterator(sorted_iter_type iter)
                        : sorted_iter_type{iter}
                    { }
                    IteratorIterator(const IteratorIterator&) = default;

                    // Dereference the current iterator before returning
                    iterator_deref<Container> operator*() {
                        return *sorted_iter_type::operator*();
                    }
            };

            IteratorIterator begin() {
                return {std::begin(sorted_iters)};

            }

            IteratorIterator end() {
                return {std::end(sorted_iters)};
            }
    };

    template <typename Container, typename CompareFunc>
    Sorted<Container, CompareFunc> sorted(
            Container&& container, CompareFunc compare_func) {
        return {std::forward<Container>(container), compare_func};
    }

    template <typename Container>
    auto sorted(Container&& container) ->
             decltype(sorted(std::forward<Container>(container),
                         std::less<iterator_deref<Container>>()))
     {
         return sorted(std::forward<Container>(container),
                 std::less<iterator_deref<Container>>());
     }

}

#endif //#ifndef SORTED__HPP__
<|endoftext|>


<commit_before>#ifndef PACMAN_HH
#define PACMAN_HH

#include <memory>
#include <string>
#include <variant>
#include <vector>

#include <alpm.h>

namespace dlr {

class Pacman {
 public:
  struct Package {
    Package(std::string pkgname, std::string pkgver)
        : pkgname(std::move(pkgname)), pkgver(std::move(pkgver)) {}
    std::string pkgname;
    std::string pkgver;
  };

  // Factory constructor.
  static std::unique_ptr<Pacman> NewFromConfig(const std::string& config_file);

  ~Pacman();

  static int Vercmp(const std::string& a, const std::string& b);
  struct VersionLess {
    bool operator()(const std::string& a, const std::string& b) {
      return Pacman::Vercmp(a, b) < 0;
    }
  };

  // Returns true if the package is ignored.
  bool ShouldIgnorePackage(const std::string& package) const;

  // Returns the name of the repo that the package belongs to, or empty string
  // if the package was not found in any repo.
  std::string RepoForPackage(const std::string& package) const;

  bool DependencyIsSatisfied(const std::string& package) const;

  // A list of installed packages which are not found in any currently enabled
  // Sync DB.
  std::vector<Package> ForeignPackages() const;

  std::variant<bool, Package> GetLocalPackage(const std::string& name) const;

 private:
  Pacman(alpm_handle_t* alpm, std::vector<std::string> ignored_packages);

  alpm_handle_t* alpm_;
  alpm_db_t* local_db_;

  std::vector<std::string> ignored_packages_;
};

}  // namespace dlr

#endif  // PACMAN_HH
<commit_msg>Drop unused VersionLess struct<commit_after>#ifndef PACMAN_HH
#define PACMAN_HH

#include <memory>
#include <string>
#include <variant>
#include <vector>

#include <alpm.h>

namespace dlr {

class Pacman {
 public:
  struct Package {
    Package(std::string pkgname, std::string pkgver)
        : pkgname(std::move(pkgname)), pkgver(std::move(pkgver)) {}
    std::string pkgname;
    std::string pkgver;
  };

  // Factory constructor.
  static std::unique_ptr<Pacman> NewFromConfig(const std::string& config_file);

  ~Pacman();

  static int Vercmp(const std::string& a, const std::string& b);

  // Returns true if the package is ignored.
  bool ShouldIgnorePackage(const std::string& package) const;

  // Returns the name of the repo that the package belongs to, or empty string
  // if the package was not found in any repo.
  std::string RepoForPackage(const std::string& package) const;

  bool DependencyIsSatisfied(const std::string& package) const;

  // A list of installed packages which are not found in any currently enabled
  // Sync DB.
  std::vector<Package> ForeignPackages() const;

  std::variant<bool, Package> GetLocalPackage(const std::string& name) const;

 private:
  Pacman(alpm_handle_t* alpm, std::vector<std::string> ignored_packages);

  alpm_handle_t* alpm_;
  alpm_db_t* local_db_;

  std::vector<std::string> ignored_packages_;
};

}  // namespace dlr

#endif  // PACMAN_HH
<|endoftext|>


<commit_before>/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, development version     *
*                (c) 2006-2017 INRIA, USTL, UJF, CNRS, MGH                    *
*                                                                             *
* This program 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 program 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 program. If not, see <http://www.gnu.org/licenses/>.        *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#ifndef SOFA_COMPONENT_MAPPING_SUBSETMULTIMAPPING_INL
#define SOFA_COMPONENT_MAPPING_SUBSETMULTIMAPPING_INL

#include <sofa/core/visual/VisualParams.h>
#include <SofaMiscMapping/SubsetMultiMapping.h>
#include <SofaEigen2Solver/EigenSparseMatrix.h>
#include <iostream>
#include <SofaBaseMechanics/IdentityMapping.h>

namespace sofa
{

namespace component
{

namespace mapping
{

template <class TIn, class TOut>
void SubsetMultiMapping<TIn, TOut>::init()
{
    assert( indexPairs.getValue().size()%2==0 );
    const unsigned indexPairSize = indexPairs.getValue().size()/2;

    this->toModels[0]->resize( indexPairSize );

    Inherit::init();

    unsigned Nin = TIn::deriv_total_size, Nout = TOut::deriv_total_size;

    for( unsigned i=0; i<baseMatrices.size(); i++ )
        delete baseMatrices[i];

    typedef linearsolver::EigenSparseMatrix<TIn,TOut> Jacobian;
    baseMatrices.resize( this->getFrom().size() );
    helper::vector<Jacobian*> jacobians( this->getFrom().size() );
    for(unsigned i=0; i<baseMatrices.size(); i++ )
    {
        baseMatrices[i] = jacobians[i] = new linearsolver::EigenSparseMatrix<TIn,TOut>;
        jacobians[i]->resize(Nout*indexPairSize,Nin*this->fromModels[i]->getSize() ); // each jacobian has the same number of rows
    }

    // fill the Jacobians
    for(unsigned i=0; i<indexPairSize; i++)
    {
        unsigned parent = indexPairs.getValue()[i*2];
        Jacobian* jacobian = jacobians[parent];
        unsigned bcol = indexPairs.getValue()[i*2+1];  // parent particle
        for(unsigned k=0; k<Nout; k++ )
        {
            unsigned row = i*Nout + k;

            // the Jacobian could be filled in order, but empty rows should have to be managed
            jacobian->add( row, Nin*bcol +k, (SReal)1. );
        }
    }

    // finalize the Jacobians
    for(unsigned i=0; i<baseMatrices.size(); i++ )
        baseMatrices[i]->compress();
}

template <class TIn, class TOut>
SubsetMultiMapping<TIn, TOut>::SubsetMultiMapping()
    : Inherit()
    , indexPairs( initData( &indexPairs, helper::vector<unsigned>(), "indexPairs", "list of couples (parent index + index in the parent)"))
{}

template <class TIn, class TOut>
SubsetMultiMapping<TIn, TOut>::~SubsetMultiMapping()
{
    for(unsigned i=0; i<baseMatrices.size(); i++ )
    {
        delete baseMatrices[i];
    }
}

template <class TIn, class TOut>
const helper::vector<sofa::defaulttype::BaseMatrix*>* SubsetMultiMapping<TIn, TOut>::getJs()
{
    return &baseMatrices;
}

template <class TIn, class TOut>
void SubsetMultiMapping<TIn, TOut>::addPoint( const core::BaseState* from, int index)
{

    // find the index of the parent state
    unsigned i;
    for(i=0; i<this->fromModels.size(); i++)
        if(this->fromModels.get(i)==from )
            break;
    if(i==this->fromModels.size())
    {
        serr<<"SubsetMultiMapping<TIn, TOut>::addPoint, parent "<<from->getName()<<" not found !"<< sendl;
        assert(0);
    }

    addPoint(i, index);
}

template <class TIn, class TOut>
void SubsetMultiMapping<TIn, TOut>::addPoint( int from, int index)
{
    assert((size_t)from < this->fromModels.size());
    helper::vector<unsigned>& indexPairsVector = *indexPairs.beginEdit();
    indexPairsVector.push_back(from);
    indexPairsVector.push_back(index);
    indexPairs.endEdit();
}

template <class TIn, class TOut>
void SubsetMultiMapping<TIn, TOut>::apply(const core::MechanicalParams* mparams, const helper::vector<OutDataVecCoord*>& dataVecOutPos, const helper::vector<const InDataVecCoord*>& dataVecInPos)
{
    //apply(const vecOutVecCoord& outPos, const vecConstInVecCoord& inPos)
    //OutVecCoord& out = *outPos[0];

    OutVecCoord& out = *(dataVecOutPos[0]->beginEdit(mparams));

    for(unsigned i=0; i<out.size(); i++)
    {
//        cerr<<"SubsetMultiMapping<TIn, TOut>::apply, i = "<< i <<", indexPair = " << indexPairs[i*2] << ", " << indexPairs[i*2+1] <<", inPos size = "<< inPos.size() <<", inPos[i] = " << (*inPos[indexPairs[i*2]]) << endl;
//        cerr<<"SubsetMultiMapping<TIn, TOut>::apply, out = "<< out << endl;
        const InDataVecCoord* inPosPtr = dataVecInPos[indexPairs.getValue()[i*2]];
        const InVecCoord& inPos = (*inPosPtr).getValue();

        //out[i] =  inPos[indexPairs.getValue()[i*2+1]];
        helper::eq( out[i], inPos[indexPairs.getValue()[i*2+1]] );
    }

    dataVecOutPos[0]->endEdit(mparams);

}

template <class TIn, class TOut>
void SubsetMultiMapping<TIn, TOut>::applyJ(const core::MechanicalParams* mparams, const helper::vector<OutDataVecDeriv*>& dataVecOutVel, const helper::vector<const InDataVecDeriv*>& dataVecInVel)
{
    OutVecDeriv& out = *(dataVecOutVel[0]->beginEdit(mparams));

    for(unsigned i=0; i<out.size(); i++)
    {
        const InDataVecDeriv* inDerivPtr = dataVecInVel[indexPairs.getValue()[i*2]];
        const InVecDeriv& inDeriv = (*inDerivPtr).getValue();

//        out[i] = inDeriv[indexPairs.getValue()[i*2+1]];
        helper::eq( out[i], inDeriv[indexPairs.getValue()[i*2+1]] );
    }

    dataVecOutVel[0]->endEdit(mparams);
}

template <class TIn, class TOut>
void SubsetMultiMapping<TIn, TOut>::applyJT( const core::ConstraintParams* /*cparams*/, const helper::vector< InDataMatrixDeriv* >& dOut, const helper::vector< const OutDataMatrixDeriv* >& dIn)
{
    helper::vector<unsigned>  indexP = indexPairs.getValue();

    // hypothesis: one child only:
    const OutMatrixDeriv& in = dIn[0]->getValue();

    if (dOut.size() != this->fromModels.size())
    {
        serr<<"problem with number of output constraint matrices"<<sendl;
        return;
    }

    typename OutMatrixDeriv::RowConstIterator rowItEnd = in.end();
    // loop on the constraints defined on the child of the mapping
    for (typename OutMatrixDeriv::RowConstIterator rowIt = in.begin(); rowIt != rowItEnd; ++rowIt)
    {

        typename OutMatrixDeriv::ColConstIterator colIt = rowIt.begin();
        typename OutMatrixDeriv::ColConstIterator colItEnd = rowIt.end();

        // A constraint can be shared by several nodes,
        // these nodes can be linked to 2 different parents. // TODO handle more parents
        // we need to add a line to each parent that is concerned by the constraint

        while (colIt != colItEnd)
        {
            unsigned int index_parent = indexP[colIt.index()*2]; // 0 or 1 (for now...)
            // writeLine provide an iterator on the line... if this line does not exist, the line is created:
            typename InMatrixDeriv::RowIterator o = dOut[index_parent]->beginEdit()->writeLine(rowIt.index());
            dOut[index_parent]->endEdit();

            // for each col of the constraint direction, it adds a col in the corresponding parent's constraint direction
            if(indexPairs.getValue()[colIt.index()*2+1] < (unsigned int)this->fromModels[index_parent]->getSize())
            {
                InDeriv tmp; helper::eq( tmp, colIt.val() );
                o.addCol(indexP[colIt.index()*2+1], tmp);
            }
            ++colIt;
        }

    }

    //    std::cout<<" dIn ="<<(*dIn[0])<<std::endl;
    //    std::cout<<" dOut ="<<(*dOut[0])<<"  "<<(*dOut[1])<<std::endl;
}

template <class TIn, class TOut>
void SubsetMultiMapping<TIn, TOut>::applyJT(const core::MechanicalParams* mparams, const helper::vector<InDataVecDeriv*>& dataVecOutForce, const helper::vector<const OutDataVecDeriv*>& dataVecInForce)
{
    const OutDataVecDeriv* cderData = dataVecInForce[0];
    const OutVecDeriv& cder = cderData->getValue();
    //const InVecDeriv& cder = *childDeriv[0];

    for(unsigned i=0; i<cder.size(); i++)
    {
        //(*parentDeriv[indexPairs.getValue()[i*2]])[indexPairs.getValue()[i*2+1]] += cder[i];
        InDataVecDeriv* inDerivPtr = dataVecOutForce[indexPairs.getValue()[i*2]];
        InVecDeriv& inDeriv = *(*inDerivPtr).beginEdit(mparams);
//        inDeriv[indexPairs.getValue()[i*2+1]] += cder[i];
        helper::peq( inDeriv[indexPairs.getValue()[i*2+1]], cder[i] );
        (*inDerivPtr).endEdit(mparams);
    }
}

} // namespace mapping

} // namespace component

} // namespace sofa

#endif //SOFA_COMPONENT_MAPPING_SUBSETMULTIMAPPING_INL
<commit_msg>SubsetMultiMapping: adding alias 'pairs' for 'indexPairs'<commit_after>/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, development version     *
*                (c) 2006-2017 INRIA, USTL, UJF, CNRS, MGH                    *
*                                                                             *
* This program 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 program 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 program. If not, see <http://www.gnu.org/licenses/>.        *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#ifndef SOFA_COMPONENT_MAPPING_SUBSETMULTIMAPPING_INL
#define SOFA_COMPONENT_MAPPING_SUBSETMULTIMAPPING_INL

#include <sofa/core/visual/VisualParams.h>
#include <SofaMiscMapping/SubsetMultiMapping.h>
#include <SofaEigen2Solver/EigenSparseMatrix.h>
#include <iostream>
#include <SofaBaseMechanics/IdentityMapping.h>

namespace sofa
{

namespace component
{

namespace mapping
{

template <class TIn, class TOut>
void SubsetMultiMapping<TIn, TOut>::init()
{
    assert( indexPairs.getValue().size()%2==0 );
    const unsigned indexPairSize = indexPairs.getValue().size()/2;

    this->toModels[0]->resize( indexPairSize );

    Inherit::init();

    unsigned Nin = TIn::deriv_total_size, Nout = TOut::deriv_total_size;

    for( unsigned i=0; i<baseMatrices.size(); i++ )
        delete baseMatrices[i];

    typedef linearsolver::EigenSparseMatrix<TIn,TOut> Jacobian;
    baseMatrices.resize( this->getFrom().size() );
    helper::vector<Jacobian*> jacobians( this->getFrom().size() );
    for(unsigned i=0; i<baseMatrices.size(); i++ )
    {
        baseMatrices[i] = jacobians[i] = new linearsolver::EigenSparseMatrix<TIn,TOut>;
        jacobians[i]->resize(Nout*indexPairSize,Nin*this->fromModels[i]->getSize() ); // each jacobian has the same number of rows
    }

    // fill the Jacobians
    for(unsigned i=0; i<indexPairSize; i++)
    {
        unsigned parent = indexPairs.getValue()[i*2];
        Jacobian* jacobian = jacobians[parent];
        unsigned bcol = indexPairs.getValue()[i*2+1];  // parent particle
        for(unsigned k=0; k<Nout; k++ )
        {
            unsigned row = i*Nout + k;

            // the Jacobian could be filled in order, but empty rows should have to be managed
            jacobian->add( row, Nin*bcol +k, (SReal)1. );
        }
    }

    // finalize the Jacobians
    for(unsigned i=0; i<baseMatrices.size(); i++ )
        baseMatrices[i]->compress();
}

template <class TIn, class TOut>
SubsetMultiMapping<TIn, TOut>::SubsetMultiMapping()
    : Inherit()
    , indexPairs( initData( &indexPairs, helper::vector<unsigned>(), "indexPairs", "list of couples (parent index + index in the parent)"))
{
    this->addAlias( &indexPairs, "pairs" );
}

template <class TIn, class TOut>
SubsetMultiMapping<TIn, TOut>::~SubsetMultiMapping()
{
    for(unsigned i=0; i<baseMatrices.size(); i++ )
    {
        delete baseMatrices[i];
    }
}

template <class TIn, class TOut>
const helper::vector<sofa::defaulttype::BaseMatrix*>* SubsetMultiMapping<TIn, TOut>::getJs()
{
    return &baseMatrices;
}

template <class TIn, class TOut>
void SubsetMultiMapping<TIn, TOut>::addPoint( const core::BaseState* from, int index)
{

    // find the index of the parent state
    unsigned i;
    for(i=0; i<this->fromModels.size(); i++)
        if(this->fromModels.get(i)==from )
            break;
    if(i==this->fromModels.size())
    {
        serr<<"SubsetMultiMapping<TIn, TOut>::addPoint, parent "<<from->getName()<<" not found !"<< sendl;
        assert(0);
    }

    addPoint(i, index);
}

template <class TIn, class TOut>
void SubsetMultiMapping<TIn, TOut>::addPoint( int from, int index)
{
    assert((size_t)from < this->fromModels.size());
    helper::vector<unsigned>& indexPairsVector = *indexPairs.beginEdit();
    indexPairsVector.push_back(from);
    indexPairsVector.push_back(index);
    indexPairs.endEdit();
}

template <class TIn, class TOut>
void SubsetMultiMapping<TIn, TOut>::apply(const core::MechanicalParams* mparams, const helper::vector<OutDataVecCoord*>& dataVecOutPos, const helper::vector<const InDataVecCoord*>& dataVecInPos)
{
    //apply(const vecOutVecCoord& outPos, const vecConstInVecCoord& inPos)
    //OutVecCoord& out = *outPos[0];

    OutVecCoord& out = *(dataVecOutPos[0]->beginEdit(mparams));

    for(unsigned i=0; i<out.size(); i++)
    {
//        cerr<<"SubsetMultiMapping<TIn, TOut>::apply, i = "<< i <<", indexPair = " << indexPairs[i*2] << ", " << indexPairs[i*2+1] <<", inPos size = "<< inPos.size() <<", inPos[i] = " << (*inPos[indexPairs[i*2]]) << endl;
//        cerr<<"SubsetMultiMapping<TIn, TOut>::apply, out = "<< out << endl;
        const InDataVecCoord* inPosPtr = dataVecInPos[indexPairs.getValue()[i*2]];
        const InVecCoord& inPos = (*inPosPtr).getValue();

        //out[i] =  inPos[indexPairs.getValue()[i*2+1]];
        helper::eq( out[i], inPos[indexPairs.getValue()[i*2+1]] );
    }

    dataVecOutPos[0]->endEdit(mparams);

}

template <class TIn, class TOut>
void SubsetMultiMapping<TIn, TOut>::applyJ(const core::MechanicalParams* mparams, const helper::vector<OutDataVecDeriv*>& dataVecOutVel, const helper::vector<const InDataVecDeriv*>& dataVecInVel)
{
    OutVecDeriv& out = *(dataVecOutVel[0]->beginEdit(mparams));

    for(unsigned i=0; i<out.size(); i++)
    {
        const InDataVecDeriv* inDerivPtr = dataVecInVel[indexPairs.getValue()[i*2]];
        const InVecDeriv& inDeriv = (*inDerivPtr).getValue();

//        out[i] = inDeriv[indexPairs.getValue()[i*2+1]];
        helper::eq( out[i], inDeriv[indexPairs.getValue()[i*2+1]] );
    }

    dataVecOutVel[0]->endEdit(mparams);
}

template <class TIn, class TOut>
void SubsetMultiMapping<TIn, TOut>::applyJT( const core::ConstraintParams* /*cparams*/, const helper::vector< InDataMatrixDeriv* >& dOut, const helper::vector< const OutDataMatrixDeriv* >& dIn)
{
    helper::vector<unsigned>  indexP = indexPairs.getValue();

    // hypothesis: one child only:
    const OutMatrixDeriv& in = dIn[0]->getValue();

    if (dOut.size() != this->fromModels.size())
    {
        serr<<"problem with number of output constraint matrices"<<sendl;
        return;
    }

    typename OutMatrixDeriv::RowConstIterator rowItEnd = in.end();
    // loop on the constraints defined on the child of the mapping
    for (typename OutMatrixDeriv::RowConstIterator rowIt = in.begin(); rowIt != rowItEnd; ++rowIt)
    {

        typename OutMatrixDeriv::ColConstIterator colIt = rowIt.begin();
        typename OutMatrixDeriv::ColConstIterator colItEnd = rowIt.end();

        // A constraint can be shared by several nodes,
        // these nodes can be linked to 2 different parents. // TODO handle more parents
        // we need to add a line to each parent that is concerned by the constraint

        while (colIt != colItEnd)
        {
            unsigned int index_parent = indexP[colIt.index()*2]; // 0 or 1 (for now...)
            // writeLine provide an iterator on the line... if this line does not exist, the line is created:
            typename InMatrixDeriv::RowIterator o = dOut[index_parent]->beginEdit()->writeLine(rowIt.index());
            dOut[index_parent]->endEdit();

            // for each col of the constraint direction, it adds a col in the corresponding parent's constraint direction
            if(indexPairs.getValue()[colIt.index()*2+1] < (unsigned int)this->fromModels[index_parent]->getSize())
            {
                InDeriv tmp; helper::eq( tmp, colIt.val() );
                o.addCol(indexP[colIt.index()*2+1], tmp);
            }
            ++colIt;
        }

    }

    //    std::cout<<" dIn ="<<(*dIn[0])<<std::endl;
    //    std::cout<<" dOut ="<<(*dOut[0])<<"  "<<(*dOut[1])<<std::endl;
}

template <class TIn, class TOut>
void SubsetMultiMapping<TIn, TOut>::applyJT(const core::MechanicalParams* mparams, const helper::vector<InDataVecDeriv*>& dataVecOutForce, const helper::vector<const OutDataVecDeriv*>& dataVecInForce)
{
    const OutDataVecDeriv* cderData = dataVecInForce[0];
    const OutVecDeriv& cder = cderData->getValue();
    //const InVecDeriv& cder = *childDeriv[0];

    for(unsigned i=0; i<cder.size(); i++)
    {
        //(*parentDeriv[indexPairs.getValue()[i*2]])[indexPairs.getValue()[i*2+1]] += cder[i];
        InDataVecDeriv* inDerivPtr = dataVecOutForce[indexPairs.getValue()[i*2]];
        InVecDeriv& inDeriv = *(*inDerivPtr).beginEdit(mparams);
//        inDeriv[indexPairs.getValue()[i*2+1]] += cder[i];
        helper::peq( inDeriv[indexPairs.getValue()[i*2+1]], cder[i] );
        (*inDerivPtr).endEdit(mparams);
    }
}

} // namespace mapping

} // namespace component

} // namespace sofa

#endif //SOFA_COMPONENT_MAPPING_SUBSETMULTIMAPPING_INL
<|endoftext|>


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

#include <modules/autonavigation/avoidcollisioncurve.h>

#include <modules/autonavigation/helperfunctions.h>
#include <openspace/engine/globals.h>
#include <openspace/rendering/renderengine.h>
#include <openspace/scene/scene.h>
#include <openspace/query/query.h>
#include <ghoul/logging/logmanager.h>
#include <glm/gtx/intersect.hpp>
#include <glm/gtx/perpendicular.hpp>
#include <glm/gtx/projection.hpp>
#include <algorithm>
#include <vector>

namespace {
    constexpr const char* _loggerCat = "AvoidCollisionCurve";
    const double Epsilon = 1E-7;
} // namespace

namespace openspace::autonavigation {

AvoidCollisionCurve::AvoidCollisionCurve(const CameraState& start, const CameraState& end) {
    // default rotation interpolation
    _rotationInterpolator = RotationInterpolator{ start, end, this, Slerp };

    _points.push_back(start.position);
    _points.push_back(end.position);

    std::vector<SceneGraphNode*> relevantNodes = findRelevantNodes();

    // Create extra points to avoid collision
    removeCollisions(relevantNodes);

    int nrSegments = _points.size() - 1;

    // default values for the curve parameter - equally spaced
    for (double t = 0.0; t <= 1.0; t += 1.0 / (double)nrSegments) {
        _parameterIntervals.push_back(t);
    }

    _length = arcLength(1.0);

    initParameterIntervals();
}

// Interpolate a list of control points and knot times
glm::dvec3 AvoidCollisionCurve::positionAt(double u) {
    return interpolatePoints(u);
}

// TODO: refactor
bool isRelevant(const SceneGraphNode* node) {
    // TODO: move this list somewhere else
    const std::vector<std::string> relevantTags{
        "planet_solarSystem",
        "moon_solarSystem"
        // TODO
    };

    // does the node match any of the relevant tags?
    const std::vector<std::string> tags = node->tags();
    auto result = std::find_first_of(relevantTags.begin(), relevantTags.end(), tags.begin(), tags.end());

    // does not match any tags => not interesting
    if (result == relevantTags.end()) {
        return false;
    }

    return node->renderable() && (node->boundingSphere() > 0.0);
}

std::vector<SceneGraphNode*> AvoidCollisionCurve::findRelevantNodes() {
    // TODO: make more efficient
    const std::vector<SceneGraphNode*>& allNodes =
        global::renderEngine.scene()->allSceneGraphNodes();

    std::vector<SceneGraphNode*> result{};
    std::copy_if(allNodes.begin(), allNodes.end(), std::back_inserter(result), isRelevant);

    return result;
}

void AvoidCollisionCurve::removeCollisions(std::vector<SceneGraphNode*>& relevantNodes, int step) {
    int nrSegments = _points.size() - 1;
    int maxSteps = 10;

    // TODO: handle better / present warning if early out
    if (step > maxSteps) return;

    // go through all segments and check for collisions
    for (int i = 0; i < nrSegments; ++i) {
        const glm::dvec3 lineStart = _points[i];
        const glm::dvec3 lineEnd = _points[i + 1];

        for (SceneGraphNode* node : relevantNodes) {
            // do collision check in relative coordinates, to avoid huge numbers
            const glm::dmat4 modelTransform = node->modelTransform();
            glm::dvec3 p1 = glm::inverse(modelTransform) * glm::dvec4(lineStart, 1.0);
            glm::dvec3 p2 = glm::inverse(modelTransform) * glm::dvec4(lineEnd, 1.0);

            // sphere to check for collision
            double bs = node->boundingSphere();
            double radius = bs;
            // TODO: add a buffer (i.e. sue a little larger sphere), when we have
            // behaviour for leaving a target. Don't want to pass too close to objects
            glm::dvec3 center = glm::dvec3(0.0, 0.0, 0.0);
            glm::dvec3 point;

            bool collision = helpers::lineSphereIntersection(p1, p2, center, radius, point);

            // convert back to world coordinates
            glm::dvec3 pointWorld = modelTransform * glm::dvec4(point, 1.0);

            if (collision) {
                LINFO(fmt::format("Collision with node: {}!", node->identifier()));

                // to avoid collision, take a step in an orhtogonal direction of the
                // collision point and add a new point
                glm::dvec3 lineDir = glm::normalize(lineEnd - lineStart);
                glm::dvec3 nodePos = node->worldPosition();
                glm::dvec3 collisionPointToCenter = nodePos - pointWorld;
                glm::dvec3 parallell = glm::proj(collisionPointToCenter, lineDir);
                glm::dvec3 orthogonal = collisionPointToCenter - parallell;

                double distance = 2.0 * radius;
                glm::dvec3 extraKnot = pointWorld - distance * glm::normalize(orthogonal);
                _points.insert(_points.begin() + i + 1, extraKnot);

                // TODO: maybe make this more efficient, and make sure that we have a base case.
                removeCollisions(relevantNodes, ++step);
                break;
            }
        }
    }
}

// compute curve parameter intervals based on relative arc length
void AvoidCollisionCurve::initParameterIntervals() {
    std::vector<double> newIntervals;
    int nrSegments = _points.size() - 1;
    for (double t = 0.0; t <= 1.0; t += 1.0 / (double)nrSegments) {
        newIntervals.push_back(arcLength(t) / _length);
    }
    _parameterIntervals.swap(newIntervals);
}

// Catmull-Rom inspired interpolation of the curve points
glm::dvec3 AvoidCollisionCurve::interpolatePoints(double u)
{
    ghoul_assert(_points.size() == _parameterIntervals.size(),
        "Must have equal number of points and times!");
    ghoul_assert(_points.size() > 2, "Minimum of two control points needed for interpolation!");

    size_t nrSegments = _points.size() - 1;
    const glm::dvec3 start = _points.front();
    const glm::dvec3 end = _points.back();

    if (u <= 0.0) return _points.front();
    if (u >= 1.0) return _points.back();

    if (nrSegments == 1) {
        return roundedSpline(u, start, start, end, end);
    }

    // compute current segment index
    std::vector<double>::const_iterator segmentEndIt =
        std::lower_bound(_parameterIntervals.begin(), _parameterIntervals.end(), u);
    unsigned int idx = (segmentEndIt - 1) - _parameterIntervals.begin();

    double segmentStart = _parameterIntervals[idx];
    double segmentDuration = (_parameterIntervals[idx + 1] - _parameterIntervals[idx]);
    double tScaled = (u - segmentStart) / segmentDuration;

    glm::dvec3 first = (idx == 0) ? start : _points[idx - 1];
    glm::dvec3 last = (idx == (nrSegments - 1)) ? end : _points[idx + 2];

    return roundedSpline(tScaled, first, _points[idx], _points[idx + 1], last);
}

glm::dvec3 AvoidCollisionCurve::roundedSpline(double t, const glm::dvec3 &a,
              const glm::dvec3 &b, const glm::dvec3 &c, const glm::dvec3 &d)
{
    ghoul_assert(t >= 0 && t <= 1.0, "Interpolation variable out of range [0, 1]");

    if (t <= 0.0) return b;
    if (t >= 1.0) return c;

    auto isNormalizable = [](const glm::dvec3 v) {
        return !(abs(glm::length(v)) < Epsilon);
    };

    // find velocities at b and c
    glm::dvec3 cb = c - b;
    if (!isNormalizable(cb)) {
        return b;
    }

    glm::dvec3 ab = a - b;

    // a and b are the same point. try finding a substitue
    if (!isNormalizable(ab)) {
        ab = b - c;
        if (!isNormalizable(ab)) {
            ab = glm::dvec3(0.0, 1.0, 0.0);
        }
    }

    glm::dvec3 bVelocity = glm::normalize(cb) - glm::normalize(ab);
    bVelocity = isNormalizable(bVelocity) ?
        glm::normalize(bVelocity) : glm::dvec3(0.0, 1.0, 0.0);

    glm::dvec3 dc = d - c;
    // d and c are the same point. try finding a substitue
    if (!isNormalizable(dc)) {
        dc = c - b;
        if (!isNormalizable(dc)) {
            dc = glm::dvec3(0.0, 1.0, 0.0);
        }
    }

    glm::dvec3 bc = (-1.0)*cb;
    glm::dvec3 cVelocity = glm::normalize(dc) - glm::normalize(bc);
    cVelocity = isNormalizable(cVelocity) ?
        glm::normalize(cVelocity) : glm::dvec3(0.0, 1.0, 0.0);

    double cbDistance = glm::length(cb);
    double tangetLength = cbDistance;

    // Distances in space can be extremely large, so we dont want to let the tangents have the complete length.
    const double tangentlengthThreshold = 1E10; // TODO: What's a good threshold?? ALSO make global
    if (tangetLength > tangentlengthThreshold)
        tangetLength = tangentlengthThreshold;

    // the resulting curve gets much better and more predictable when using a genric
    // hermite curve compared to the Catmull Rom implementation
    return interpolation::hermite(t, b, c, bVelocity * tangetLength, cVelocity * tangetLength);

    //return interpolation::catmullRom(t, b - bVelocity * cbDistance, b, c, c + cVelocity * cbDistance);
}
} // namespace openspace::autonavigation
<commit_msg>refactor<commit_after>/*****************************************************************************************
*                                                                                *
* OpenSpace                                                                             *
*                                                                                *
* Copyright (c) 2014-2019                                                               *
*                                                                                *
* Permission is hereby granted, free of charge, to any person obtaining a copy of this  *
* software and associated documentation files (the "Software"), to deal in the Software *
* without restriction, including without limitation the rights to use, copy, modify,    *
* merge, publish, distribute, sublicense, and/or sell copies of the Software, and to    *
* permit persons to whom the Software is furnished to do so, subject to the following   *
* conditions:                                                                           *
*                                                                                *
* The above copyright notice and this permission notice shall be included in all copies *
* or substantial portions of the Software.                                              *
*                                                                                *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,   *
* INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A         *
* PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT    *
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF  *
* CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE  *
* OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                                         *
****************************************************************************************/

#include <modules/autonavigation/avoidcollisioncurve.h>

#include <modules/autonavigation/helperfunctions.h>
#include <openspace/engine/globals.h>
#include <openspace/rendering/renderengine.h>
#include <openspace/scene/scene.h>
#include <openspace/query/query.h>
#include <ghoul/logging/logmanager.h>
#include <glm/gtx/intersect.hpp>
#include <glm/gtx/perpendicular.hpp>
#include <glm/gtx/projection.hpp>
#include <algorithm>
#include <vector>

namespace {
    constexpr const char* _loggerCat = "AvoidCollisionCurve";
    const double Epsilon = 1E-7;

    // TODO: move this list somewhere else
    const std::vector<std::string> RelevantTags{
        "planet_solarSystem",
        "moon_solarSystem"
        // TODO
    };

} // namespace

namespace openspace::autonavigation {

AvoidCollisionCurve::AvoidCollisionCurve(const CameraState& start, const CameraState& end) {
    // default rotation interpolation
    _rotationInterpolator = RotationInterpolator{ start, end, this, Slerp };

    _points.push_back(start.position);
    _points.push_back(end.position);

    std::vector<SceneGraphNode*> relevantNodes = findRelevantNodes();

    // Create extra points to avoid collision
    removeCollisions(relevantNodes);

    int nrSegments = _points.size() - 1;

    // default values for the curve parameter - equally spaced
    for (double t = 0.0; t <= 1.0; t += 1.0 / (double)nrSegments) {
        _parameterIntervals.push_back(t);
    }

    _length = arcLength(1.0);

    initParameterIntervals();
}

// Interpolate a list of control points and knot times
glm::dvec3 AvoidCollisionCurve::positionAt(double u) {
    return interpolatePoints(u);
}

std::vector<SceneGraphNode*> AvoidCollisionCurve::findRelevantNodes() {
    // TODO: make more efficient
    const std::vector<SceneGraphNode*>& allNodes =
        global::renderEngine.scene()->allSceneGraphNodes();

    auto isRelevant = [](const SceneGraphNode* node) {
        // does the node match any of the relevant tags?
        const std::vector<std::string> tags = node->tags();
        auto result = std::find_first_of(RelevantTags.begin(), RelevantTags.end(), tags.begin(), tags.end());

        // does not match any tags => not interesting
        if (result == RelevantTags.end()) {
            return false;
        }

        return node->renderable() && (node->boundingSphere() > 0.0);
    };

    std::vector<SceneGraphNode*> result{};
    std::copy_if(allNodes.begin(), allNodes.end(), std::back_inserter(result), isRelevant);

    return result;
}

void AvoidCollisionCurve::removeCollisions(std::vector<SceneGraphNode*>& relevantNodes, int step) {
    int nrSegments = _points.size() - 1;
    int maxSteps = 10;

    // TODO: handle better / present warning if early out
    if (step > maxSteps) return;

    // go through all segments and check for collisions
    for (int i = 0; i < nrSegments; ++i) {
        const glm::dvec3 lineStart = _points[i];
        const glm::dvec3 lineEnd = _points[i + 1];

        for (SceneGraphNode* node : relevantNodes) {
            // do collision check in relative coordinates, to avoid huge numbers
            const glm::dmat4 modelTransform = node->modelTransform();
            glm::dvec3 p1 = glm::inverse(modelTransform) * glm::dvec4(lineStart, 1.0);
            glm::dvec3 p2 = glm::inverse(modelTransform) * glm::dvec4(lineEnd, 1.0);

            // sphere to check for collision
            double bs = node->boundingSphere();
            double radius = bs;
            // TODO: add a buffer (i.e. sue a little larger sphere), when we have
            // behaviour for leaving a target. Don't want to pass too close to objects
            glm::dvec3 center = glm::dvec3(0.0, 0.0, 0.0);
            glm::dvec3 point;

            bool collision = helpers::lineSphereIntersection(p1, p2, center, radius, point);

            // convert back to world coordinates
            glm::dvec3 pointWorld = modelTransform * glm::dvec4(point, 1.0);

            if (collision) {
                LINFO(fmt::format("Collision with node: {}!", node->identifier()));

                // to avoid collision, take a step in an orhtogonal direction of the
                // collision point and add a new point
                glm::dvec3 lineDir = glm::normalize(lineEnd - lineStart);
                glm::dvec3 nodePos = node->worldPosition();
                glm::dvec3 collisionPointToCenter = nodePos - pointWorld;
                glm::dvec3 parallell = glm::proj(collisionPointToCenter, lineDir);
                glm::dvec3 orthogonal = collisionPointToCenter - parallell;

                double distance = 2.0 * radius;
                glm::dvec3 extraKnot = pointWorld - distance * glm::normalize(orthogonal);
                _points.insert(_points.begin() + i + 1, extraKnot);

                // TODO: maybe make this more efficient, and make sure that we have a base case.
                removeCollisions(relevantNodes, ++step);
                break;
            }
        }
    }
}

// compute curve parameter intervals based on relative arc length
void AvoidCollisionCurve::initParameterIntervals() {
    std::vector<double> newIntervals;
    int nrSegments = _points.size() - 1;
    for (double t = 0.0; t <= 1.0; t += 1.0 / (double)nrSegments) {
        newIntervals.push_back(arcLength(t) / _length);
    }
    _parameterIntervals.swap(newIntervals);
}

// Catmull-Rom inspired interpolation of the curve points
glm::dvec3 AvoidCollisionCurve::interpolatePoints(double u)
{
    ghoul_assert(_points.size() == _parameterIntervals.size(),
        "Must have equal number of points and times!");
    ghoul_assert(_points.size() > 2, "Minimum of two control points needed for interpolation!");

    size_t nrSegments = _points.size() - 1;
    const glm::dvec3 start = _points.front();
    const glm::dvec3 end = _points.back();

    if (u <= 0.0) return _points.front();
    if (u >= 1.0) return _points.back();

    if (nrSegments == 1) {
        return roundedSpline(u, start, start, end, end);
    }

    // compute current segment index
    std::vector<double>::const_iterator segmentEndIt =
        std::lower_bound(_parameterIntervals.begin(), _parameterIntervals.end(), u);
    unsigned int idx = (segmentEndIt - 1) - _parameterIntervals.begin();

    double segmentStart = _parameterIntervals[idx];
    double segmentDuration = (_parameterIntervals[idx + 1] - _parameterIntervals[idx]);
    double tScaled = (u - segmentStart) / segmentDuration;

    glm::dvec3 first = (idx == 0) ? start : _points[idx - 1];
    glm::dvec3 last = (idx == (nrSegments - 1)) ? end : _points[idx + 2];

    return roundedSpline(tScaled, first, _points[idx], _points[idx + 1], last);
}

glm::dvec3 AvoidCollisionCurve::roundedSpline(double t, const glm::dvec3 &a,
              const glm::dvec3 &b, const glm::dvec3 &c, const glm::dvec3 &d)
{
    ghoul_assert(t >= 0 && t <= 1.0, "Interpolation variable out of range [0, 1]");

    if (t <= 0.0) return b;
    if (t >= 1.0) return c;

    auto isNormalizable = [](const glm::dvec3 v) {
        return !(abs(glm::length(v)) < Epsilon);
    };

    // find velocities at b and c
    glm::dvec3 cb = c - b;
    if (!isNormalizable(cb)) {
        return b;
    }

    glm::dvec3 ab = a - b;

    // a and b are the same point. try finding a substitue
    if (!isNormalizable(ab)) {
        ab = b - c;
        if (!isNormalizable(ab)) {
            ab = glm::dvec3(0.0, 1.0, 0.0);
        }
    }

    glm::dvec3 bVelocity = glm::normalize(cb) - glm::normalize(ab);
    bVelocity = isNormalizable(bVelocity) ?
        glm::normalize(bVelocity) : glm::dvec3(0.0, 1.0, 0.0);

    glm::dvec3 dc = d - c;
    // d and c are the same point. try finding a substitue
    if (!isNormalizable(dc)) {
        dc = c - b;
        if (!isNormalizable(dc)) {
            dc = glm::dvec3(0.0, 1.0, 0.0);
        }
    }

    glm::dvec3 bc = (-1.0)*cb;
    glm::dvec3 cVelocity = glm::normalize(dc) - glm::normalize(bc);
    cVelocity = isNormalizable(cVelocity) ?
        glm::normalize(cVelocity) : glm::dvec3(0.0, 1.0, 0.0);

    double cbDistance = glm::length(cb);
    double tangetLength = cbDistance;

    // Distances in space can be extremely large, so we dont want to let the tangents have the complete length.
    const double tangentlengthThreshold = 1E10; // TODO: What's a good threshold?? ALSO make global
    if (tangetLength > tangentlengthThreshold)
        tangetLength = tangentlengthThreshold;

    // the resulting curve gets much better and more predictable when using a genric
    // hermite curve compared to the Catmull Rom implementation
    return interpolation::hermite(t, b, c, bVelocity * tangetLength, cVelocity * tangetLength);

    //return interpolation::catmullRom(t, b - bVelocity * cbDistance, b, c, c + cVelocity * cbDistance);
}
} // namespace openspace::autonavigation
<|endoftext|>


<commit_before>/*
 *  Copyright (c) 2013 The WebRTC project authors. All Rights Reserved.
 *
 *  Use of this source code is governed by a BSD-style license
 *  that can be found in the LICENSE file in the root of the source
 *  tree. An additional intellectual property rights grant can be found
 *  in the file PATENTS.  All contributing project authors may
 *  be found in the AUTHORS file in the root of the source tree.
 */

#include "webrtc/modules/desktop_capture/window_capturer.h"

#include <assert.h>

#include "webrtc/base/scoped_ptr.h"
#include "webrtc/base/win32.h"
#include "webrtc/modules/desktop_capture/desktop_frame_win.h"
#include "webrtc/modules/desktop_capture/win/window_capture_utils.h"
#include "webrtc/system_wrappers/interface/logging.h"

namespace webrtc {

namespace {

BOOL CALLBACK WindowsEnumerationHandler(HWND hwnd, LPARAM param) {
  WindowCapturer::WindowList* list =
      reinterpret_cast<WindowCapturer::WindowList*>(param);

  // Skip windows that are invisible, minimized, have no title, or are owned,
  // unless they have the app window style set.
  int len = GetWindowTextLength(hwnd);
  HWND owner = GetWindow(hwnd, GW_OWNER);
  LONG exstyle = GetWindowLong(hwnd, GWL_EXSTYLE);
  if (len == 0 || IsIconic(hwnd) || !IsWindowVisible(hwnd) ||
      (owner && !(exstyle & WS_EX_APPWINDOW))) {
    return TRUE;
  }

  // Skip the Program Manager window and the Start button.
  const size_t kClassLength = 256;
  WCHAR class_name[kClassLength];
  GetClassName(hwnd, class_name, kClassLength);
  // Skip Program Manager window and the Start button. This is the same logic
  // that's used in Win32WindowPicker in libjingle. Consider filtering other
  // windows as well (e.g. toolbars).
  if (wcscmp(class_name, L"Progman") == 0 || wcscmp(class_name, L"Button") == 0)
    return TRUE;

  WindowCapturer::Window window;
  window.id = reinterpret_cast<WindowCapturer::WindowId>(hwnd);

  const size_t kTitleLength = 500;
  WCHAR window_title[kTitleLength];
  // Truncate the title if it's longer than kTitleLength.
  GetWindowText(hwnd, window_title, kTitleLength);
  window.title = rtc::ToUtf8(window_title);

  // Skip windows when we failed to convert the title or it is empty.
  if (window.title.empty())
    return TRUE;

  list->push_back(window);

  return TRUE;
}

class WindowCapturerWin : public WindowCapturer {
 public:
  WindowCapturerWin();
  virtual ~WindowCapturerWin();

  // WindowCapturer interface.
  bool GetWindowList(WindowList* windows) override;
  bool SelectWindow(WindowId id) override;
  bool BringSelectedWindowToFront() override;

  // DesktopCapturer interface.
  void Start(Callback* callback) override;
  void Capture(const DesktopRegion& region) override;

 private:
  Callback* callback_;

  // HWND and HDC for the currently selected window or NULL if window is not
  // selected.
  HWND window_;

  DesktopSize previous_size_;

  AeroChecker aero_checker_;

  RTC_DISALLOW_COPY_AND_ASSIGN(WindowCapturerWin);
};

WindowCapturerWin::WindowCapturerWin()
    : callback_(NULL),
      window_(NULL) {
}

WindowCapturerWin::~WindowCapturerWin() {
}

bool WindowCapturerWin::GetWindowList(WindowList* windows) {
  WindowList result;
  LPARAM param = reinterpret_cast<LPARAM>(&result);
  if (!EnumWindows(&WindowsEnumerationHandler, param))
    return false;
  windows->swap(result);
  return true;
}

bool WindowCapturerWin::SelectWindow(WindowId id) {
  HWND window = reinterpret_cast<HWND>(id);
  if (!IsWindow(window) || !IsWindowVisible(window) || IsIconic(window))
    return false;
  window_ = window;
  previous_size_.set(0, 0);
  return true;
}

bool WindowCapturerWin::BringSelectedWindowToFront() {
  if (!window_)
    return false;

  if (!IsWindow(window_) || !IsWindowVisible(window_) || IsIconic(window_))
    return false;

  return SetForegroundWindow(window_) != 0;
}

void WindowCapturerWin::Start(Callback* callback) {
  assert(!callback_);
  assert(callback);

  callback_ = callback;
}

void WindowCapturerWin::Capture(const DesktopRegion& region) {
  if (!window_) {
    LOG(LS_ERROR) << "Window hasn't been selected: " << GetLastError();
    callback_->OnCaptureCompleted(NULL);
    return;
  }

  // Stop capturing if the window has been closed or hidden.
  if (!IsWindow(window_) || !IsWindowVisible(window_)) {
    callback_->OnCaptureCompleted(NULL);
    return;
  }

  // Return a 1x1 black frame if the window is minimized, to match the behavior
  // on Mac.
  if (IsIconic(window_)) {
    BasicDesktopFrame* frame = new BasicDesktopFrame(DesktopSize(1, 1));
    memset(frame->data(), 0, frame->stride() * frame->size().height());

    previous_size_ = frame->size();
    callback_->OnCaptureCompleted(frame);
    return;
  }

  DesktopRect original_rect;
  DesktopRect cropped_rect;
  if (!GetCroppedWindowRect(window_, &cropped_rect, &original_rect)) {
    LOG(LS_WARNING) << "Failed to get window info: " << GetLastError();
    callback_->OnCaptureCompleted(NULL);
    return;
  }

  HDC window_dc = GetWindowDC(window_);
  if (!window_dc) {
    LOG(LS_WARNING) << "Failed to get window DC: " << GetLastError();
    callback_->OnCaptureCompleted(NULL);
    return;
  }

  rtc::scoped_ptr<DesktopFrameWin> frame(
      DesktopFrameWin::Create(cropped_rect.size(), NULL, window_dc));
  if (!frame.get()) {
    ReleaseDC(window_, window_dc);
    callback_->OnCaptureCompleted(NULL);
    return;
  }

  HDC mem_dc = CreateCompatibleDC(window_dc);
  HGDIOBJ previous_object = SelectObject(mem_dc, frame->bitmap());
  BOOL result = FALSE;

  // When desktop composition (Aero) is enabled each window is rendered to a
  // private buffer allowing BitBlt() to get the window content even if the
  // window is occluded. PrintWindow() is slower but lets rendering the window
  // contents to an off-screen device context when Aero is not available.
  // PrintWindow() is not supported by some applications.
  //
  // If Aero is enabled, we prefer BitBlt() because it's faster and avoids
  // window flickering. Otherwise, we prefer PrintWindow() because BitBlt() may
  // render occluding windows on top of the desired window.
  //
  // When composition is enabled the DC returned by GetWindowDC() doesn't always
  // have window frame rendered correctly. Windows renders it only once and then
  // caches the result between captures. We hack it around by calling
  // PrintWindow() whenever window size changes, including the first time of
  // capturing - it somehow affects what we get from BitBlt() on the subsequent
  // captures.

  if (!aero_checker_.IsAeroEnabled() || !previous_size_.equals(frame->size())) {
    result = PrintWindow(window_, mem_dc, 0);
  }

  // Aero is enabled or PrintWindow() failed, use BitBlt.
  if (!result) {
    result = BitBlt(mem_dc, 0, 0, frame->size().width(), frame->size().height(),
                    window_dc,
                    cropped_rect.left() - original_rect.left(),
                    cropped_rect.top() - original_rect.top(),
                    SRCCOPY);
  }

  SelectObject(mem_dc, previous_object);
  DeleteDC(mem_dc);
  ReleaseDC(window_, window_dc);

  previous_size_ = frame->size();

  frame->mutable_updated_region()->SetRect(
      DesktopRect::MakeSize(frame->size()));

  if (!result) {
    LOG(LS_ERROR) << "Both PrintWindow() and BitBlt() failed.";
    frame.reset();
  }

  callback_->OnCaptureCompleted(frame.release());
}

}  // namespace

// static
WindowCapturer* WindowCapturer::Create(const DesktopCaptureOptions& options) {
  return new WindowCapturerWin();
}

}  // namespace webrtc
<commit_msg>WebRtc Win Desktop capture: ignore Win8+ Modern Apps' windows.<commit_after>/*
 *  Copyright (c) 2013 The WebRTC project authors. All Rights Reserved.
 *
 *  Use of this source code is governed by a BSD-style license
 *  that can be found in the LICENSE file in the root of the source
 *  tree. An additional intellectual property rights grant can be found
 *  in the file PATENTS.  All contributing project authors may
 *  be found in the AUTHORS file in the root of the source tree.
 */

#include "webrtc/modules/desktop_capture/window_capturer.h"

#include <assert.h>

#include "webrtc/base/scoped_ptr.h"
#include "webrtc/base/checks.h"
#include "webrtc/base/win32.h"
#include "webrtc/modules/desktop_capture/desktop_frame_win.h"
#include "webrtc/modules/desktop_capture/win/window_capture_utils.h"
#include "webrtc/system_wrappers/interface/logging.h"

namespace webrtc {

namespace {

BOOL CALLBACK WindowsEnumerationHandler(HWND hwnd, LPARAM param) {
  WindowCapturer::WindowList* list =
      reinterpret_cast<WindowCapturer::WindowList*>(param);

  // Skip windows that are invisible, minimized, have no title, or are owned,
  // unless they have the app window style set.
  int len = GetWindowTextLength(hwnd);
  HWND owner = GetWindow(hwnd, GW_OWNER);
  LONG exstyle = GetWindowLong(hwnd, GWL_EXSTYLE);
  if (len == 0 || IsIconic(hwnd) || !IsWindowVisible(hwnd) ||
      (owner && !(exstyle & WS_EX_APPWINDOW))) {
    return TRUE;
  }

  // Skip the Program Manager window and the Start button.
  const size_t kClassLength = 256;
  WCHAR class_name[kClassLength];
  const int class_name_length = GetClassName(hwnd, class_name, kClassLength);
  RTC_DCHECK(class_name_length)
      << "Error retrieving the application's class name";

  // Skip Program Manager window and the Start button. This is the same logic
  // that's used in Win32WindowPicker in libjingle. Consider filtering other
  // windows as well (e.g. toolbars).
  if (wcscmp(class_name, L"Progman") == 0 || wcscmp(class_name, L"Button") == 0)
    return TRUE;

  // Windows 8 introduced a "Modern App" identified by their class name being
  // either ApplicationFrameWindow or windows.UI.Core.coreWindow. The
  // associated windows cannot be captured, so we skip them.
  // http://crbug.com/526883.
  if (rtc::IsWindows8OrLater() &&
      (wcscmp(class_name, L"ApplicationFrameWindow") == 0 ||
       wcscmp(class_name, L"Windows.UI.Core.CoreWindow") == 0)) {
    return TRUE;
  }

  WindowCapturer::Window window;
  window.id = reinterpret_cast<WindowCapturer::WindowId>(hwnd);

  const size_t kTitleLength = 500;
  WCHAR window_title[kTitleLength];
  // Truncate the title if it's longer than kTitleLength.
  GetWindowText(hwnd, window_title, kTitleLength);
  window.title = rtc::ToUtf8(window_title);

  // Skip windows when we failed to convert the title or it is empty.
  if (window.title.empty())
    return TRUE;

  list->push_back(window);

  return TRUE;
}

class WindowCapturerWin : public WindowCapturer {
 public:
  WindowCapturerWin();
  virtual ~WindowCapturerWin();

  // WindowCapturer interface.
  bool GetWindowList(WindowList* windows) override;
  bool SelectWindow(WindowId id) override;
  bool BringSelectedWindowToFront() override;

  // DesktopCapturer interface.
  void Start(Callback* callback) override;
  void Capture(const DesktopRegion& region) override;

 private:
  Callback* callback_;

  // HWND and HDC for the currently selected window or NULL if window is not
  // selected.
  HWND window_;

  DesktopSize previous_size_;

  AeroChecker aero_checker_;

  RTC_DISALLOW_COPY_AND_ASSIGN(WindowCapturerWin);
};

WindowCapturerWin::WindowCapturerWin()
    : callback_(NULL),
      window_(NULL) {
}

WindowCapturerWin::~WindowCapturerWin() {
}

bool WindowCapturerWin::GetWindowList(WindowList* windows) {
  WindowList result;
  LPARAM param = reinterpret_cast<LPARAM>(&result);
  if (!EnumWindows(&WindowsEnumerationHandler, param))
    return false;
  windows->swap(result);
  return true;
}

bool WindowCapturerWin::SelectWindow(WindowId id) {
  HWND window = reinterpret_cast<HWND>(id);
  if (!IsWindow(window) || !IsWindowVisible(window) || IsIconic(window))
    return false;
  window_ = window;
  previous_size_.set(0, 0);
  return true;
}

bool WindowCapturerWin::BringSelectedWindowToFront() {
  if (!window_)
    return false;

  if (!IsWindow(window_) || !IsWindowVisible(window_) || IsIconic(window_))
    return false;

  return SetForegroundWindow(window_) != 0;
}

void WindowCapturerWin::Start(Callback* callback) {
  assert(!callback_);
  assert(callback);

  callback_ = callback;
}

void WindowCapturerWin::Capture(const DesktopRegion& region) {
  if (!window_) {
    LOG(LS_ERROR) << "Window hasn't been selected: " << GetLastError();
    callback_->OnCaptureCompleted(NULL);
    return;
  }

  // Stop capturing if the window has been closed or hidden.
  if (!IsWindow(window_) || !IsWindowVisible(window_)) {
    callback_->OnCaptureCompleted(NULL);
    return;
  }

  // Return a 1x1 black frame if the window is minimized, to match the behavior
  // on Mac.
  if (IsIconic(window_)) {
    BasicDesktopFrame* frame = new BasicDesktopFrame(DesktopSize(1, 1));
    memset(frame->data(), 0, frame->stride() * frame->size().height());

    previous_size_ = frame->size();
    callback_->OnCaptureCompleted(frame);
    return;
  }

  DesktopRect original_rect;
  DesktopRect cropped_rect;
  if (!GetCroppedWindowRect(window_, &cropped_rect, &original_rect)) {
    LOG(LS_WARNING) << "Failed to get window info: " << GetLastError();
    callback_->OnCaptureCompleted(NULL);
    return;
  }

  HDC window_dc = GetWindowDC(window_);
  if (!window_dc) {
    LOG(LS_WARNING) << "Failed to get window DC: " << GetLastError();
    callback_->OnCaptureCompleted(NULL);
    return;
  }

  rtc::scoped_ptr<DesktopFrameWin> frame(
      DesktopFrameWin::Create(cropped_rect.size(), NULL, window_dc));
  if (!frame.get()) {
    ReleaseDC(window_, window_dc);
    callback_->OnCaptureCompleted(NULL);
    return;
  }

  HDC mem_dc = CreateCompatibleDC(window_dc);
  HGDIOBJ previous_object = SelectObject(mem_dc, frame->bitmap());
  BOOL result = FALSE;

  // When desktop composition (Aero) is enabled each window is rendered to a
  // private buffer allowing BitBlt() to get the window content even if the
  // window is occluded. PrintWindow() is slower but lets rendering the window
  // contents to an off-screen device context when Aero is not available.
  // PrintWindow() is not supported by some applications.
  //
  // If Aero is enabled, we prefer BitBlt() because it's faster and avoids
  // window flickering. Otherwise, we prefer PrintWindow() because BitBlt() may
  // render occluding windows on top of the desired window.
  //
  // When composition is enabled the DC returned by GetWindowDC() doesn't always
  // have window frame rendered correctly. Windows renders it only once and then
  // caches the result between captures. We hack it around by calling
  // PrintWindow() whenever window size changes, including the first time of
  // capturing - it somehow affects what we get from BitBlt() on the subsequent
  // captures.

  if (!aero_checker_.IsAeroEnabled() || !previous_size_.equals(frame->size())) {
    result = PrintWindow(window_, mem_dc, 0);
  }

  // Aero is enabled or PrintWindow() failed, use BitBlt.
  if (!result) {
    result = BitBlt(mem_dc, 0, 0, frame->size().width(), frame->size().height(),
                    window_dc,
                    cropped_rect.left() - original_rect.left(),
                    cropped_rect.top() - original_rect.top(),
                    SRCCOPY);
  }

  SelectObject(mem_dc, previous_object);
  DeleteDC(mem_dc);
  ReleaseDC(window_, window_dc);

  previous_size_ = frame->size();

  frame->mutable_updated_region()->SetRect(
      DesktopRect::MakeSize(frame->size()));

  if (!result) {
    LOG(LS_ERROR) << "Both PrintWindow() and BitBlt() failed.";
    frame.reset();
  }

  callback_->OnCaptureCompleted(frame.release());
}

}  // namespace

// static
WindowCapturer* WindowCapturer::Create(const DesktopCaptureOptions& options) {
  return new WindowCapturerWin();
}

}  // namespace webrtc
<|endoftext|>


<commit_before>#include "Software/NeuralNet.h"
#include "Software/Backpropagation.h"
#include <iostream>

/*
	This is example code which runs a Backpropagating neural network.
	It is supposed to double any input number.
	This is the example code from the readme of https://github.com/FidoProject/Fido

	To compile, run in terminal:
	fcc linear.cpp
	This will make an executable file a.out which will be this program.

	PS.
	Try tweaking the paramaters of the backprop initialization to make the nerual network more correct
*/

int main() {
	// Creates a neural network with
	// 1 input, 1 output, 2 hidden layers, 4 neurons per hidden layer, and a sigmoid activation function.
	net::NeuralNet neuralNetwork = net::NeuralNet(1, 1, 2, 4, "sigmoid");
	std::vector< std::vector<double> > input = { {1}, {2}, {5}, {6} };
	std::vector< std::vector<double> > correctOutput = { {2}, {4}, {10}, {12} };
	neuralNetwork.setOutputActivationFunction("simpleLinear");

	// Create backpropagation object with
	// a learning rate of 10%, a momentum term of 0.001, an acceptable error level of 5%,
	// and a maximum number of training iterations of 10000
	net::Backpropagation backprop = net::Backpropagation(0.1, 0.001, 0.05, 10000);
	backprop.trainOnData(&neuralNetwork, input, correctOutput);

	// Cycle through inputs and print the outputs
	for (std::vector<double> current: input) {
		std::vector<double> final = neuralNetwork.getOutput(current);
		std::cout << "Correct answer: " << current[0] << "\tActual answer:" << final[0] << std::endl;
	}
}
<commit_msg>Delete linear.cpp<commit_after><|endoftext|>


<commit_before>/*
 * Copyright 2010-2017 Branimir Karadzic. All rights reserved.
 * License: https://github.com/bkaradzic/bx#license-bsd-2-clause
 */

#include <bx/debug.h>
#include <bx/string.h> // isPrint
#include <inttypes.h>  // PRIx*

#if BX_PLATFORM_ANDROID
#	include <android/log.h>
#elif BX_PLATFORM_WINDOWS || BX_PLATFORM_WINRT || BX_PLATFORM_XBOX360 || BX_PLATFORM_XBOXONE
extern "C" __declspec(dllimport) void __stdcall OutputDebugStringA(const char* _str);
#elif BX_PLATFORM_IOS || BX_PLATFORM_OSX
#	if defined(__OBJC__)
#		import <Foundation/NSObjCRuntime.h>
#	else
#		include <CoreFoundation/CFString.h>
extern "C" void NSLog(CFStringRef _format, ...);
#	endif // defined(__OBJC__)
#elif 0 // BX_PLATFORM_EMSCRIPTEN
#	include <emscripten.h>
#else
#	include <stdio.h>
#endif // BX_PLATFORM_WINDOWS

namespace bx
{
	void debugBreak()
	{
#if BX_COMPILER_MSVC
		__debugbreak();
#elif BX_CPU_ARM
		__builtin_trap();
//		asm("bkpt 0");
#elif !BX_PLATFORM_NACL && BX_CPU_X86 && (BX_COMPILER_GCC || BX_COMPILER_CLANG)
		// NaCl doesn't like int 3:
		// NativeClient: NaCl module load failed: Validation failure. File violates Native Client safety rules.
		__asm__ ("int $3");
#else // cross platform implementation
		int* int3 = (int*)3L;
		*int3 = 3;
#endif // BX
	}

	void debugOutput(const char* _out)
	{
#if BX_PLATFORM_ANDROID
#	ifndef BX_ANDROID_LOG_TAG
#		define BX_ANDROID_LOG_TAG ""
#	endif // BX_ANDROID_LOG_TAG
		__android_log_write(ANDROID_LOG_DEBUG, BX_ANDROID_LOG_TAG, _out);
#elif BX_PLATFORM_WINDOWS || BX_PLATFORM_WINRT || BX_PLATFORM_XBOX360 || BX_PLATFORM_XBOXONE
		OutputDebugStringA(_out);
#elif BX_PLATFORM_IOS || BX_PLATFORM_OSX
#	if defined(__OBJC__)
		NSLog(@"%s", _out);
#	else
		NSLog(__CFStringMakeConstantString("%s"), _out);
#	endif // defined(__OBJC__)
#elif 0 // BX_PLATFORM_EMSCRIPTEN
		emscripten_log(EM_LOG_CONSOLE, "%s", _out);
#else
		fputs(_out, stdout);
		fflush(stdout);
#endif // BX_PLATFORM_
	}

	void debugPrintfVargs(const char* _format, va_list _argList)
	{
		char temp[8192];
		char* out = temp;
		int32_t len = vsnprintf(out, sizeof(temp), _format, _argList);
		if ( (int32_t)sizeof(temp) < len)
		{
			out = (char*)alloca(len+1);
			len = vsnprintf(out, len, _format, _argList);
		}
		out[len] = '\0';
		debugOutput(out);
	}

	void debugPrintf(const char* _format, ...)
	{
		va_list argList;
		va_start(argList, _format);
		debugPrintfVargs(_format, argList);
		va_end(argList);
	}

#define DBG_ADDRESS "%" PRIxPTR

	void debugPrintfData(const void* _data, uint32_t _size, const char* _format, ...)
	{
#define HEX_DUMP_WIDTH 16
#define HEX_DUMP_SPACE_WIDTH 48
#define HEX_DUMP_FORMAT "%-" BX_STRINGIZE(HEX_DUMP_SPACE_WIDTH) "." BX_STRINGIZE(HEX_DUMP_SPACE_WIDTH) "s"

		va_list argList;
		va_start(argList, _format);
		debugPrintfVargs(_format, argList);
		va_end(argList);

		debugPrintf("\ndata: " DBG_ADDRESS ", size: %d\n", _data, _size);

		if (NULL != _data)
		{
			const uint8_t* data = reinterpret_cast<const uint8_t*>(_data);
			char hex[HEX_DUMP_WIDTH*3+1];
			char ascii[HEX_DUMP_WIDTH+1];
			uint32_t hexPos = 0;
			uint32_t asciiPos = 0;
			for (uint32_t ii = 0; ii < _size; ++ii)
			{
				snprintf(&hex[hexPos], sizeof(hex)-hexPos, "%02x ", data[asciiPos]);
				hexPos += 3;

				ascii[asciiPos] = isPrint(data[asciiPos]) ? data[asciiPos] : '.';
				asciiPos++;

				if (HEX_DUMP_WIDTH == asciiPos)
				{
					ascii[asciiPos] = '\0';
					debugPrintf("\t" DBG_ADDRESS "\t" HEX_DUMP_FORMAT "\t%s\n", data, hex, ascii);
					data += asciiPos;
					hexPos = 0;
					asciiPos = 0;
				}
			}

			if (0 != asciiPos)
			{
				ascii[asciiPos] = '\0';
				debugPrintf("\t" DBG_ADDRESS "\t" HEX_DUMP_FORMAT "\t%s\n", data, hex, ascii);
			}
		}

#undef HEX_DUMP_WIDTH
#undef HEX_DUMP_SPACE_WIDTH
#undef HEX_DUMP_FORMAT
	}

} // namespace bx
<commit_msg>Cleanup.<commit_after>/*
 * Copyright 2010-2017 Branimir Karadzic. All rights reserved.
 * License: https://github.com/bkaradzic/bx#license-bsd-2-clause
 */

#include <bx/debug.h>
#include <bx/string.h> // isPrint
#include <inttypes.h>  // PRIx*

#if BX_PLATFORM_ANDROID
#	include <android/log.h>
#elif BX_PLATFORM_WINDOWS || BX_PLATFORM_WINRT || BX_PLATFORM_XBOX360 || BX_PLATFORM_XBOXONE
extern "C" __declspec(dllimport) void __stdcall OutputDebugStringA(const char* _str);
#elif BX_PLATFORM_IOS || BX_PLATFORM_OSX
#	if defined(__OBJC__)
#		import <Foundation/NSObjCRuntime.h>
#	else
#		include <CoreFoundation/CFString.h>
extern "C" void NSLog(CFStringRef _format, ...);
#	endif // defined(__OBJC__)
#elif 0 // BX_PLATFORM_EMSCRIPTEN
#	include <emscripten.h>
#endif // BX_PLATFORM_WINDOWS

namespace bx
{
	void debugBreak()
	{
#if BX_COMPILER_MSVC
		__debugbreak();
#elif BX_CPU_ARM
		__builtin_trap();
//		asm("bkpt 0");
#elif !BX_PLATFORM_NACL && BX_CPU_X86 && (BX_COMPILER_GCC || BX_COMPILER_CLANG)
		// NaCl doesn't like int 3:
		// NativeClient: NaCl module load failed: Validation failure. File violates Native Client safety rules.
		__asm__ ("int $3");
#else // cross platform implementation
		int* int3 = (int*)3L;
		*int3 = 3;
#endif // BX
	}

	void debugOutput(const char* _out)
	{
#if BX_PLATFORM_ANDROID
#	ifndef BX_ANDROID_LOG_TAG
#		define BX_ANDROID_LOG_TAG ""
#	endif // BX_ANDROID_LOG_TAG
		__android_log_write(ANDROID_LOG_DEBUG, BX_ANDROID_LOG_TAG, _out);
#elif BX_PLATFORM_WINDOWS || BX_PLATFORM_WINRT || BX_PLATFORM_XBOX360 || BX_PLATFORM_XBOXONE
		OutputDebugStringA(_out);
#elif BX_PLATFORM_IOS || BX_PLATFORM_OSX
#	if defined(__OBJC__)
		NSLog(@"%s", _out);
#	else
		NSLog(__CFStringMakeConstantString("%s"), _out);
#	endif // defined(__OBJC__)
#elif 0 // BX_PLATFORM_EMSCRIPTEN
		emscripten_log(EM_LOG_CONSOLE, "%s", _out);
#else
		fputs(_out, stdout);
		fflush(stdout);
#endif // BX_PLATFORM_
	}

	void debugPrintfVargs(const char* _format, va_list _argList)
	{
		char temp[8192];
		char* out = temp;
		int32_t len = vsnprintf(out, sizeof(temp), _format, _argList);
		if ( (int32_t)sizeof(temp) < len)
		{
			out = (char*)alloca(len+1);
			len = vsnprintf(out, len, _format, _argList);
		}
		out[len] = '\0';
		debugOutput(out);
	}

	void debugPrintf(const char* _format, ...)
	{
		va_list argList;
		va_start(argList, _format);
		debugPrintfVargs(_format, argList);
		va_end(argList);
	}

#define DBG_ADDRESS "%" PRIxPTR

	void debugPrintfData(const void* _data, uint32_t _size, const char* _format, ...)
	{
#define HEX_DUMP_WIDTH 16
#define HEX_DUMP_SPACE_WIDTH 48
#define HEX_DUMP_FORMAT "%-" BX_STRINGIZE(HEX_DUMP_SPACE_WIDTH) "." BX_STRINGIZE(HEX_DUMP_SPACE_WIDTH) "s"

		va_list argList;
		va_start(argList, _format);
		debugPrintfVargs(_format, argList);
		va_end(argList);

		debugPrintf("\ndata: " DBG_ADDRESS ", size: %d\n", _data, _size);

		if (NULL != _data)
		{
			const uint8_t* data = reinterpret_cast<const uint8_t*>(_data);
			char hex[HEX_DUMP_WIDTH*3+1];
			char ascii[HEX_DUMP_WIDTH+1];
			uint32_t hexPos = 0;
			uint32_t asciiPos = 0;
			for (uint32_t ii = 0; ii < _size; ++ii)
			{
				snprintf(&hex[hexPos], sizeof(hex)-hexPos, "%02x ", data[asciiPos]);
				hexPos += 3;

				ascii[asciiPos] = isPrint(data[asciiPos]) ? data[asciiPos] : '.';
				asciiPos++;

				if (HEX_DUMP_WIDTH == asciiPos)
				{
					ascii[asciiPos] = '\0';
					debugPrintf("\t" DBG_ADDRESS "\t" HEX_DUMP_FORMAT "\t%s\n", data, hex, ascii);
					data += asciiPos;
					hexPos = 0;
					asciiPos = 0;
				}
			}

			if (0 != asciiPos)
			{
				ascii[asciiPos] = '\0';
				debugPrintf("\t" DBG_ADDRESS "\t" HEX_DUMP_FORMAT "\t%s\n", data, hex, ascii);
			}
		}

#undef HEX_DUMP_WIDTH
#undef HEX_DUMP_SPACE_WIDTH
#undef HEX_DUMP_FORMAT
	}

} // namespace bx
<|endoftext|>