<commit_before>#include <cmath>
#include <fstream>
#include <map>
#include <sstream>
#include <stdexcept>

#include "cvrp.h"
#include "tsplib_format.h"

namespace VrpSolver {

    unsigned int Cvrp::demand(unsigned int node_id) const {
        if ((1 > node_id) || (node_id > dimension_))
            throw std::out_of_range("error: in Cvrp::demand");
        return demands_[node_id];
    }

    int Cvrp::distance(unsigned int from, unsigned int to) const {
        if ((1 > from) || (from > dimension_) ||
            (1 > to) || (to > dimension_))
            throw std::out_of_range("error: in Cvrp::distance");

        const int index = (to > from) ? ((to-2)*(to-1)/2+(from-1)) :
                                        ((from-2)*(from-1)/2+(to-1));
        return distances_[index];
    }

    // 文字列strからtrim_char文字列に含まれている文字を削除
    void trim(std::string& str, const std::string& trim_char) {
        size_t pos;
        while ((pos = str.find_first_of(trim_char)) != std::string::npos)
            str.erase(pos, 1);
    }

    // セミコロン以後の文字列(空白の直前まで)を読み取る
    std::string get_parameter(std::ifstream& ifs) {
        std::string param;
        ifs >> param;
        while (param == ":") ifs >> param; // ":"は読み飛ばす
        return param;
    }

    enum TsplibKeyword {
        NAME, TYPE, COMMENT, DIMENSION, CAPACITY,
        EDGE_WEIGHT_TYPE, EDGE_WEIGHT_FORMAT, EDGE_DATA_FORMAT,
        NODE_COORD_TYPE, DISPLAY_DATA_TYPE,
        NODE_COORD_SECTION, DEPOT_SECTION, DEMAND_SECTION,
        EDGE_DATA_SECTION, EDGE_WEIGHT_SECTION,
        END_OF_FILE
    };

    std::map<std::string, TsplibKeyword> keyword_map = {
        { "NAME",                NAME },
        { "TYPE",                TYPE },
        { "COMMENT",             COMMENT },
        { "DIMENSION",           DIMENSION },
        { "CAPACITY",            CAPACITY },
        { "EDGE_WEIGHT_TYPE",      EDGE_WEIGHT_TYPE },
        { "EDGE_WEIGHT_FORMAT",  EDGE_WEIGHT_FORMAT },
        { "EDGE_DATA_FORMAT",    EDGE_DATA_FORMAT },
        { "NODE_COORD_TYPE",     NODE_COORD_TYPE },
        { "DISPLAY_DATA_TYPE",   DISPLAY_DATA_TYPE },
        { "NODE_COORD_SECTION",  NODE_COORD_SECTION },
        { "DEPOT_SECTION",       DEPOT_SECTION },
        { "DEMAND_SECTION",      DEMAND_SECTION },
        { "EDGE_DATA_SECTION",   EDGE_DATA_SECTION },
        { "EDGE_WEIGHT_SECTION", EDGE_WEIGHT_SECTION },
        { "EOF",                 END_OF_FILE }
    };

    // infileから情報を読み取りCvrpクラスをセットアップする
    void read_vrp(Cvrp& cvrp, const std::string &infile) {
        std::ifstream ifs(infile.c_str());
        if (!ifs)
            throw std::runtime_error("error: can't open file " + infile);

        std::string edge_weight_type,
                    edge_weight_format,
                    display_data_type;

        while (ifs) {
            std::string tsp_keyword;
            ifs >> tsp_keyword;
            if (ifs.eof()) break;
            trim(tsp_keyword, " :");
            switch (keyword_map[tsp_keyword]) {

                // The specification part
                case NAME :
                    cvrp.name_ = get_parameter(ifs);
                    break;
                case TYPE :
                    {
                        std::string not_use;
                        getline(ifs, not_use);
                    }
                    break;
                case COMMENT :
                    {
                        std::string not_use;
                        getline(ifs, not_use);
                    }
                    break;
                case DIMENSION :
                    cvrp.dimension_ = stoi(get_parameter(ifs));
                    break;
                case CAPACITY :
                    cvrp.capacity_ = stoi(get_parameter(ifs));
                    break;
                case EDGE_WEIGHT_TYPE :
                    edge_weight_type = get_parameter(ifs);
                    break;
                case EDGE_WEIGHT_FORMAT :
                    edge_weight_format = get_parameter(ifs);
                    break;
                case EDGE_DATA_FORMAT :
                    {
                        std::string not_use;
                        getline(ifs, not_use);
                        break;
                    }
                case NODE_COORD_TYPE :
                    {
                        std::string not_use;
                        getline(ifs, not_use);
                        break;
                    }
                case DISPLAY_DATA_TYPE :
                    display_data_type = get_parameter(ifs);
                    break;

                // The data part
                case NODE_COORD_SECTION :
                    {
                        int n=0, m, x, y; // m do not use
                        for (int i=0; i != cvrp.dimension_; i++) {
                            ifs >> m >> x >> y;
                            std::pair<int,int> c(x,y);
                            cvrp.coords_.push_back(c);
                        }
                        break;
                    }
                case DEPOT_SECTION :
                    {
                        cvrp.depot_ = stoi(get_parameter(ifs));
                        if (stoi(get_parameter(ifs)) != -1)
                            throw std::runtime_error("error:"
                                    "can't handle multiple depots");
                        break;
                    }
                case DEMAND_SECTION :
                    {
                        cvrp.demands_.push_back(0); // 0要素目は0にしておく
                        for (int i=1; i <= cvrp.dimension_; i++) {
                            unsigned int node_id, demand;
                            ifs >> node_id >> demand;
                            if (node_id != i)
                                throw std::runtime_error("error:"
                                        "DEMAND_SECTION format may be different");
                            cvrp.demands_.push_back(demand);
                        }
                        break;
                    }
                case EDGE_DATA_SECTION :
                    throw std::runtime_error("Sorry, can not handle 'EDGE_DATA_SECTION'");
                    break;
                case EDGE_WEIGHT_SECTION :
                    {
                        if (edge_weight_format != "LOWER_ROW")
                            throw std::runtime_error("Sorry, can not handle except EDGE_WEIGHT_FORMAT == LOWER_ROW");
                        for (int i=0; i < cvrp.dimension_; i++) {
                            for (int j=0; j < i; j++) {
                                int distance;
                                ifs >> distance;
                                cvrp.distances_.push_back(distance);
                            }
                        }
                        break;
                    }
                case END_OF_FILE :
                    // do nothing
                    break;
                default :
                    throw std::runtime_error("error: unknown keyword '" + tsp_keyword + "'");
                    break;
            }
        }

        // distancesの設定
        if (edge_weight_type != Tsplib::EdgeWeightType::EXPLICIT) {
            auto& distances = cvrp.distances_;
            auto& coords    = cvrp.coords_;
            for (int i=0; i < cvrp.dimension_; i++) {
                for (int j=0; j < i; j++) {
                    int dx = coords[j].first  - coords[i].first;
                    int dy = coords[j].second - coords[i].second;
                    distances.push_back(floor(sqrt(dx*dx + dy*dy)+0.5));
                }
            }
        }
    }

} // namespace VrpSolver
<commit_msg>インデントの調整<commit_after>#include <cmath>
#include <fstream>
#include <map>
#include <sstream>
#include <stdexcept>

#include "cvrp.h"
#include "tsplib_format.h"

namespace VrpSolver {

    unsigned int Cvrp::demand(unsigned int node_id) const {
        if ((1 > node_id) || (node_id > dimension_))
            throw std::out_of_range("error: in Cvrp::demand");
        return demands_[node_id];
    }

    int Cvrp::distance(unsigned int from, unsigned int to) const {
        if ((1 > from) || (from > dimension_) ||
            (1 > to) || (to > dimension_))
            throw std::out_of_range("error: in Cvrp::distance");

        const int index = (to > from) ? ((to-2)*(to-1)/2+(from-1)) :
                                        ((from-2)*(from-1)/2+(to-1));
        return distances_[index];
    }

    // 文字列strからtrim_char文字列に含まれている文字を削除
    void trim(std::string& str, const std::string& trim_char) {
        size_t pos;
        while ((pos = str.find_first_of(trim_char)) != std::string::npos)
            str.erase(pos, 1);
    }

    // セミコロン以後の文字列(空白の直前まで)を読み取る
    std::string get_parameter(std::ifstream& ifs) {
        std::string param;
        ifs >> param;
        while (param == ":") ifs >> param; // ":"は読み飛ばす
        return param;
    }

    enum TsplibKeyword {
        NAME, TYPE, COMMENT, DIMENSION, CAPACITY,
        EDGE_WEIGHT_TYPE, EDGE_WEIGHT_FORMAT, EDGE_DATA_FORMAT,
        NODE_COORD_TYPE, DISPLAY_DATA_TYPE,
        NODE_COORD_SECTION, DEPOT_SECTION, DEMAND_SECTION,
        EDGE_DATA_SECTION, EDGE_WEIGHT_SECTION,
        END_OF_FILE
    };

    std::map<std::string, TsplibKeyword> keyword_map = {
        { "NAME",                NAME },
        { "TYPE",                TYPE },
        { "COMMENT",             COMMENT },
        { "DIMENSION",           DIMENSION },
        { "CAPACITY",            CAPACITY },
        { "EDGE_WEIGHT_TYPE",    EDGE_WEIGHT_TYPE },
        { "EDGE_WEIGHT_FORMAT",  EDGE_WEIGHT_FORMAT },
        { "EDGE_DATA_FORMAT",    EDGE_DATA_FORMAT },
        { "NODE_COORD_TYPE",     NODE_COORD_TYPE },
        { "DISPLAY_DATA_TYPE",   DISPLAY_DATA_TYPE },
        { "NODE_COORD_SECTION",  NODE_COORD_SECTION },
        { "DEPOT_SECTION",       DEPOT_SECTION },
        { "DEMAND_SECTION",      DEMAND_SECTION },
        { "EDGE_DATA_SECTION",   EDGE_DATA_SECTION },
        { "EDGE_WEIGHT_SECTION", EDGE_WEIGHT_SECTION },
        { "EOF",                 END_OF_FILE }
    };

    // infileから情報を読み取りCvrpクラスをセットアップする
    void read_vrp(Cvrp& cvrp, const std::string &infile) {
        std::ifstream ifs(infile.c_str());
        if (!ifs)
            throw std::runtime_error("error: can't open file " + infile);

        std::string edge_weight_type,
                    edge_weight_format,
                    display_data_type;

        while (ifs) {
            std::string tsp_keyword;
            ifs >> tsp_keyword;
            if (ifs.eof()) break;
            trim(tsp_keyword, " :");
            switch (keyword_map[tsp_keyword]) {

                // The specification part
                case NAME :
                    cvrp.name_ = get_parameter(ifs);
                    break;
                case TYPE :
                    {
                        std::string not_use;
                        getline(ifs, not_use);
                    }
                    break;
                case COMMENT :
                    {
                        std::string not_use;
                        getline(ifs, not_use);
                    }
                    break;
                case DIMENSION :
                    cvrp.dimension_ = stoi(get_parameter(ifs));
                    break;
                case CAPACITY :
                    cvrp.capacity_ = stoi(get_parameter(ifs));
                    break;
                case EDGE_WEIGHT_TYPE :
                    edge_weight_type = get_parameter(ifs);
                    break;
                case EDGE_WEIGHT_FORMAT :
                    edge_weight_format = get_parameter(ifs);
                    break;
                case EDGE_DATA_FORMAT :
                    {
                        std::string not_use;
                        getline(ifs, not_use);
                        break;
                    }
                case NODE_COORD_TYPE :
                    {
                        std::string not_use;
                        getline(ifs, not_use);
                        break;
                    }
                case DISPLAY_DATA_TYPE :
                    display_data_type = get_parameter(ifs);
                    break;

                // The data part
                case NODE_COORD_SECTION :
                    {
                        int n=0, m, x, y; // m do not use
                        for (int i=0; i != cvrp.dimension_; i++) {
                            ifs >> m >> x >> y;
                            std::pair<int,int> c(x,y);
                            cvrp.coords_.push_back(c);
                        }
                        break;
                    }
                case DEPOT_SECTION :
                    {
                        cvrp.depot_ = stoi(get_parameter(ifs));
                        if (stoi(get_parameter(ifs)) != -1)
                            throw std::runtime_error("error:"
                                    "can't handle multiple depots");
                        break;
                    }
                case DEMAND_SECTION :
                    {
                        cvrp.demands_.push_back(0); // 0要素目は0にしておく
                        for (int i=1; i <= cvrp.dimension_; i++) {
                            unsigned int node_id, demand;
                            ifs >> node_id >> demand;
                            if (node_id != i)
                                throw std::runtime_error("error:"
                                        "DEMAND_SECTION format may be different");
                            cvrp.demands_.push_back(demand);
                        }
                        break;
                    }
                case EDGE_DATA_SECTION :
                    throw std::runtime_error("Sorry, can not handle 'EDGE_DATA_SECTION'");
                    break;
                case EDGE_WEIGHT_SECTION :
                    {
                        if (edge_weight_format != "LOWER_ROW")
                            throw std::runtime_error("Sorry, can not handle except EDGE_WEIGHT_FORMAT == LOWER_ROW");
                        for (int i=0; i < cvrp.dimension_; i++) {
                            for (int j=0; j < i; j++) {
                                int distance;
                                ifs >> distance;
                                cvrp.distances_.push_back(distance);
                            }
                        }
                        break;
                    }
                case END_OF_FILE :
                    // do nothing
                    break;
                default :
                    throw std::runtime_error("error: unknown keyword '" + tsp_keyword + "'");
                    break;
            }
        }

        // distancesの設定
        if (edge_weight_type != Tsplib::EdgeWeightType::EXPLICIT) {
            auto& distances = cvrp.distances_;
            auto& coords    = cvrp.coords_;
            for (int i=0; i < cvrp.dimension_; i++) {
                for (int j=0; j < i; j++) {
                    int dx = coords[j].first  - coords[i].first;
                    int dy = coords[j].second - coords[i].second;
                    distances.push_back(floor(sqrt(dx*dx + dy*dy)+0.5));
                }
            }
        }
    }

} // namespace VrpSolver
<|endoftext|>


<commit_before>/*
 Initial version copied from Initial version copied from https://github.com/JetBrains/intellij-community/blob/master/native/fsNotifier/mac/fsnotifier.c
 */
// Copyright 2000-2018 JetBrains s.r.o. Use of this source code is governed by the Apache 2.0 license that can be found in the LICENSE file.

/*
 * Apple specific functions.
 */
#if defined(__APPLE__)

#include "native.h"
#include "generic.h"
#include <CoreServices/CoreServices.h>
#include <pthread.h>
#include <strings.h>

JavaVM* jvm = NULL;
bool invalidStateDetected = false;

typedef struct watch_details {
    CFMutableArrayRef rootsToWatch;
    FSEventStreamRef watcherStream;
    pthread_t watcherThread;
    jobject watcherCallback;
    CFRunLoopRef threadLoop;
} watch_details_t;

static void reportEvent(const char *event, char *path, jobject watcherCallback) {
    size_t len = 0;
    if (path != NULL) {
        len = strlen(path);
        for (char *p = path; *p != '\0'; p++) {
            if (*p == '\n') {
                *p = '\0';
            }
        }
    }
    JNIEnv* env;
    int getEnvStat = jvm->GetEnv((void **)&env, JNI_VERSION_1_6);
    if (getEnvStat == JNI_EDETACHED) {
        if (jvm->AttachCurrentThread((void **) &env, NULL) != JNI_OK) {
            invalidStateDetected = true;
            return;
        }
    } else if (getEnvStat == JNI_EVERSION) {
        invalidStateDetected = true;
        return;
    }

    jclass callback_class = env->GetObjectClass(watcherCallback);
    jmethodID methodCallback = env->GetMethodID(callback_class, "pathChanged", "(Ljava/lang/String;)V");
    env->CallVoidMethod(watcherCallback, methodCallback, env->NewStringUTF(path));
}

static void callback(ConstFSEventStreamRef streamRef,
                     void *clientCallBackInfo,
                     size_t numEvents,
                     void *eventPaths,
                     const FSEventStreamEventFlags eventFlags[],
                     const FSEventStreamEventId eventIds[]) {
    if (invalidStateDetected) return;
    char **paths = (char**) eventPaths;

    jobject watcherCallback = (jobject) clientCallBackInfo;

    for (int i = 0; i < numEvents; i++) {
        // TODO[max] Lion has much more detailed flags we need accurately process. For now just reduce to SL events range.
        FSEventStreamEventFlags flags = eventFlags[i] & 0xFF;
        if ((flags & kFSEventStreamEventFlagMustScanSubDirs) != 0) {
            reportEvent("RECDIRTY", paths[i], watcherCallback);
        } else if (flags != kFSEventStreamEventFlagNone) {
            reportEvent("RESET", NULL, watcherCallback);
        } else {
            reportEvent("DIRTY", paths[i], watcherCallback);
        }
    }
}

static void *EventProcessingThread(void *data) {
    watch_details_t *details = (watch_details_t*) data;
    CFRunLoopRef threadLoop = CFRunLoopGetCurrent();
    FSEventStreamScheduleWithRunLoop(details->watcherStream, threadLoop, kCFRunLoopDefaultMode);
    FSEventStreamStart(details->watcherStream);
    details->threadLoop = threadLoop;
    // This triggers run loop for this thread, causing it to run until we explicitly stop it.
    CFRunLoopRun();
    return NULL;
}

JNIEXPORT jobject JNICALL
Java_net_rubygrapefruit_platform_internal_jni_OsxFileEventFunctions_startWatch(JNIEnv *env, jclass target, jobjectArray paths, CFAbsoluteTime latency, jobject javaCallback, jobject result) {
    invalidStateDetected = false;
    CFMutableArrayRef rootsToWatch = CFArrayCreateMutable(NULL, 0, NULL);
    if (rootsToWatch == NULL) {
        mark_failed_with_errno(env, "Could not allocate array to store roots to watch.", result);
        return NULL;
    }
    int count = env->GetArrayLength(paths);
    if (count == 0) {
        mark_failed_with_errno(env, "No paths given to watch.", result);
        return NULL;
    }
    for (int i = 0; i < count; i++) {
        jstring path = (jstring) env->GetObjectArrayElement(paths, i);
        char* pathString = java_to_char(env, path, result);
        if (pathString == NULL) {
            mark_failed_with_errno(env, "Could not allocate string to store root to watch.", result);
            return NULL;
        }
        CFStringRef stringPath = CFStringCreateWithCString(NULL, pathString, kCFStringEncodingUTF8);
        free(pathString);
        if (stringPath == NULL) {
            mark_failed_with_errno(env, "Could not create CFStringRef.", result);
            return NULL;
        }
        CFArrayAppendValue(rootsToWatch, stringPath);
    }

    jobject watcherCallback = env->NewGlobalRef(javaCallback);
    if (watcherCallback == NULL) {
        mark_failed_with_errno(env, "Could not create global reference for callback.", result);
        return NULL;
    }

    FSEventStreamContext context = {0, (void*) watcherCallback, NULL, NULL, NULL};
    FSEventStreamRef watcherStream = FSEventStreamCreate (
                NULL,
                &callback,
                &context,
                rootsToWatch,
                kFSEventStreamEventIdSinceNow,
                latency,
                kFSEventStreamCreateFlagNoDefer);
    if (watcherStream == NULL) {
         mark_failed_with_errno(env, "Could not create FSEventStreamCreate to track changes.", result);
         return NULL;
    }

    jclass clsWatch = env->FindClass("net/rubygrapefruit/platform/internal/jni/OsxFileEventFunctions$WatchImpl");
    jmethodID constructor = env->GetMethodID(clsWatch, "<init>", "(Ljava/lang/Object;)V");
    watch_details_t* details = (watch_details_t*)malloc(sizeof(watch_details_t));
    details->rootsToWatch = rootsToWatch;
    details->watcherStream = watcherStream;
    details->watcherCallback = watcherCallback;

    if (pthread_create(&(details->watcherThread), NULL, EventProcessingThread, details) != 0) {
        mark_failed_with_errno(env, "Could not create file watcher thread.", result);
        return NULL;
    }

    // TODO Should this be somewhere global?
    int jvmStatus = env->GetJavaVM(&jvm);
    if (jvmStatus < 0) {
        mark_failed_with_errno(env, "Could not store jvm instance.", result);
        return NULL;
    }

    return env->NewObject(clsWatch, constructor, env->NewDirectByteBuffer(details, sizeof(details)));
}

JNIEXPORT void JNICALL
Java_net_rubygrapefruit_platform_internal_jni_OsxFileEventFunctions_stopWatch(JNIEnv *env, jclass target, jobject detailsObj, jobject result) {
    watch_details_t *details = (watch_details_t*) env->GetDirectBufferAddress(detailsObj);
    CFMutableArrayRef rootsToWatch = details->rootsToWatch;
    FSEventStreamRef watcherStream = details->watcherStream;
    pthread_t watcherThread = details->watcherThread;
    jobject watcherCallback = details->watcherCallback;
    CFRunLoopRef threadLoop = details->threadLoop;
    free(details);

    // if there were no roots to watch, there are no resources to release
    if (rootsToWatch == NULL) return;
    if (invalidStateDetected) {
        // report and reset flag, but try to clean up state as much as possible
        mark_failed_with_errno(env, "Watcher is in invalid state, reported changes may be incorrect.", result);
    }

    for (int i = 0; i < CFArrayGetCount(rootsToWatch); i++) {
        const void *value = CFArrayGetValueAtIndex(rootsToWatch, i);
        CFRelease(value);
    }
    // TODO Can we release these earlier?
    CFRelease(rootsToWatch);
    rootsToWatch = NULL;

    FSEventStreamStop(watcherStream);
    FSEventStreamInvalidate(watcherStream);
    FSEventStreamRelease(watcherStream);
    // TODO: consider using FSEventStreamFlushSync to flush all pending events.
    watcherStream = NULL;

    // TODO Shouldn't we stop this first and then release the rest?
    CFRunLoopStop(threadLoop);
    threadLoop = NULL;

    env->DeleteGlobalRef(watcherCallback);
    watcherCallback = NULL;

    pthread_join(watcherThread, NULL);
    watcherThread = NULL;
}

#endif<commit_msg>Code polish<commit_after>/*
 Initial version copied from Initial version copied from https://github.com/JetBrains/intellij-community/blob/master/native/fsNotifier/mac/fsnotifier.c
 */
// Copyright 2000-2018 JetBrains s.r.o. Use of this source code is governed by the Apache 2.0 license that can be found in the LICENSE file.

/*
 * Apple specific functions.
 */
#if defined(__APPLE__)

#include "native.h"
#include "generic.h"
#include <CoreServices/CoreServices.h>
#include <pthread.h>
#include <strings.h>

JavaVM* jvm = NULL;
bool invalidStateDetected = false;

typedef struct watch_details {
    CFMutableArrayRef rootsToWatch;
    FSEventStreamRef watcherStream;
    pthread_t watcherThread;
    jobject watcherCallback;
    CFRunLoopRef threadLoop;
} watch_details_t;

static void reportEvent(const char *event, char *path, jobject watcherCallback) {
    size_t len = 0;
    if (path != NULL) {
        len = strlen(path);
        for (char *p = path; *p != '\0'; p++) {
            if (*p == '\n') {
                *p = '\0';
            }
        }
    }
    JNIEnv* env;
    int getEnvStat = jvm->GetEnv((void **)&env, JNI_VERSION_1_6);
    if (getEnvStat == JNI_EDETACHED) {
        if (jvm->AttachCurrentThread((void **) &env, NULL) != JNI_OK) {
            invalidStateDetected = true;
            return;
        }
    } else if (getEnvStat == JNI_EVERSION) {
        invalidStateDetected = true;
        return;
    }

    jclass callback_class = env->GetObjectClass(watcherCallback);
    jmethodID methodCallback = env->GetMethodID(callback_class, "pathChanged", "(Ljava/lang/String;)V");
    env->CallVoidMethod(watcherCallback, methodCallback, env->NewStringUTF(path));
}

static void callback(ConstFSEventStreamRef streamRef,
                     void *clientCallBackInfo,
                     size_t numEvents,
                     void *eventPaths,
                     const FSEventStreamEventFlags eventFlags[],
                     const FSEventStreamEventId eventIds[]) {
    if (invalidStateDetected) return;
    char **paths = (char**) eventPaths;

    jobject watcherCallback = (jobject) clientCallBackInfo;

    for (int i = 0; i < numEvents; i++) {
        // TODO[max] Lion has much more detailed flags we need accurately process. For now just reduce to SL events range.
        FSEventStreamEventFlags flags = eventFlags[i] & 0xFF;
        if ((flags & kFSEventStreamEventFlagMustScanSubDirs) != 0) {
            reportEvent("RECDIRTY", paths[i], watcherCallback);
        } else if (flags != kFSEventStreamEventFlagNone) {
            reportEvent("RESET", NULL, watcherCallback);
        } else {
            reportEvent("DIRTY", paths[i], watcherCallback);
        }
    }
}

static void *EventProcessingThread(void *data) {
    watch_details_t *details = (watch_details_t*) data;
    CFRunLoopRef threadLoop = CFRunLoopGetCurrent();
    FSEventStreamScheduleWithRunLoop(details->watcherStream, threadLoop, kCFRunLoopDefaultMode);
    FSEventStreamStart(details->watcherStream);
    details->threadLoop = threadLoop;
    // This triggers run loop for this thread, causing it to run until we explicitly stop it.
    CFRunLoopRun();
    return NULL;
}

JNIEXPORT jobject JNICALL
Java_net_rubygrapefruit_platform_internal_jni_OsxFileEventFunctions_startWatch(JNIEnv *env, jclass target, jobjectArray paths, CFAbsoluteTime latency, jobject javaCallback, jobject result) {
    invalidStateDetected = false;
    CFMutableArrayRef rootsToWatch = CFArrayCreateMutable(NULL, 0, NULL);
    if (rootsToWatch == NULL) {
        mark_failed_with_errno(env, "Could not allocate array to store roots to watch.", result);
        return NULL;
    }
    int count = env->GetArrayLength(paths);
    if (count == 0) {
        mark_failed_with_errno(env, "No paths given to watch.", result);
        return NULL;
    }
    for (int i = 0; i < count; i++) {
        jstring path = (jstring) env->GetObjectArrayElement(paths, i);
        char* pathString = java_to_char(env, path, result);
        if (pathString == NULL) {
            mark_failed_with_errno(env, "Could not allocate string to store root to watch.", result);
            return NULL;
        }
        CFStringRef stringPath = CFStringCreateWithCString(NULL, pathString, kCFStringEncodingUTF8);
        free(pathString);
        if (stringPath == NULL) {
            mark_failed_with_errno(env, "Could not create CFStringRef.", result);
            return NULL;
        }
        CFArrayAppendValue(rootsToWatch, stringPath);
    }

    jobject watcherCallback = env->NewGlobalRef(javaCallback);
    if (watcherCallback == NULL) {
        mark_failed_with_errno(env, "Could not create global reference for callback.", result);
        return NULL;
    }

    FSEventStreamContext context = {0, (void*) watcherCallback, NULL, NULL, NULL};
    FSEventStreamRef watcherStream = FSEventStreamCreate (
                NULL,
                &callback,
                &context,
                rootsToWatch,
                kFSEventStreamEventIdSinceNow,
                latency,
                kFSEventStreamCreateFlagNoDefer);
    if (watcherStream == NULL) {
         mark_failed_with_errno(env, "Could not create FSEventStreamCreate to track changes.", result);
         return NULL;
    }

    watch_details_t* details = (watch_details_t*)malloc(sizeof(watch_details_t));
    details->rootsToWatch = rootsToWatch;
    details->watcherStream = watcherStream;
    details->watcherCallback = watcherCallback;

    if (pthread_create(&(details->watcherThread), NULL, EventProcessingThread, details) != 0) {
        mark_failed_with_errno(env, "Could not create file watcher thread.", result);
        return NULL;
    }

    // TODO Should this be somewhere global?
    int jvmStatus = env->GetJavaVM(&jvm);
    if (jvmStatus < 0) {
        mark_failed_with_errno(env, "Could not store jvm instance.", result);
        return NULL;
    }

    jclass clsWatch = env->FindClass("net/rubygrapefruit/platform/internal/jni/OsxFileEventFunctions$WatchImpl");
    jmethodID constructor = env->GetMethodID(clsWatch, "<init>", "(Ljava/lang/Object;)V");
    return env->NewObject(clsWatch, constructor, env->NewDirectByteBuffer(details, sizeof(details)));
}

JNIEXPORT void JNICALL
Java_net_rubygrapefruit_platform_internal_jni_OsxFileEventFunctions_stopWatch(JNIEnv *env, jclass target, jobject detailsObj, jobject result) {
    watch_details_t *details = (watch_details_t*) env->GetDirectBufferAddress(detailsObj);
    CFMutableArrayRef rootsToWatch = details->rootsToWatch;
    FSEventStreamRef watcherStream = details->watcherStream;
    pthread_t watcherThread = details->watcherThread;
    jobject watcherCallback = details->watcherCallback;
    CFRunLoopRef threadLoop = details->threadLoop;
    free(details);

    // if there were no roots to watch, there are no resources to release
    if (rootsToWatch == NULL) return;
    if (invalidStateDetected) {
        // report and reset flag, but try to clean up state as much as possible
        mark_failed_with_errno(env, "Watcher is in invalid state, reported changes may be incorrect.", result);
    }

    for (int i = 0; i < CFArrayGetCount(rootsToWatch); i++) {
        const void *value = CFArrayGetValueAtIndex(rootsToWatch, i);
        CFRelease(value);
    }
    // TODO Can we release these earlier?
    CFRelease(rootsToWatch);
    rootsToWatch = NULL;

    FSEventStreamStop(watcherStream);
    FSEventStreamInvalidate(watcherStream);
    FSEventStreamRelease(watcherStream);
    // TODO: consider using FSEventStreamFlushSync to flush all pending events.
    watcherStream = NULL;

    // TODO Shouldn't we stop this first and then release the rest?
    CFRunLoopStop(threadLoop);
    threadLoop = NULL;

    env->DeleteGlobalRef(watcherCallback);
    watcherCallback = NULL;

    pthread_join(watcherThread, NULL);
    watcherThread = NULL;
}

#endif<|endoftext|>


<commit_before><commit_msg>revert sample runs section of comment in sundials modification of ex9<commit_after><|endoftext|>


<commit_before>/*
 *  despot1_main.cpp
 *
 *  Created by Tobias Wood on 17/10/2011.
 *  Copyright (c) 2011-2013 Tobias Wood.
 *
 *  This Source Code Form is subject to the terms of the Mozilla Public
 *  License, v. 2.0. If a copy of the MPL was not distributed with this
 *  file, You can obtain one at http://mozilla.org/MPL/2.0/.
 *
 */

#include <time.h>
#include <getopt.h>
#include <iostream>
#include <atomic>
#include <Eigen/Dense>

#include "Nifti/Nifti.h"
#include "Nifti/Volume.h"
#include "ThreadPool.h"
#include "Model.h"
#include "DESPOT_Functors.h"
#include "unsupported/Eigen/NonLinearOptimization"
#include "unsupported/Eigen/NumericalDiff"

#ifdef AGILENT
#include "procpar.h"
using namespace Agilent;
#endif

using namespace std;
using namespace Eigen;

//******************************************************************************
// Arguments / Usage
//******************************************************************************
const string usage {
"Usage is: despot1 [options] spgr_input \n\
\
Options:\n\
	--help, -h        : Print this message\n\
	--verbose, -v     : Print writeResiduals information\n\
	--mask, -m file   : Mask input with specified file\n\
	--out, -o path    : Add a prefix to the output filenames\n\
	--B1, -b file     : B1 Map file (ratio)\n\
	--algo, -a l      : LLS algorithm (default)\n\
	           w      : WLLS algorithm\n\
			   n      : NLLS (Levenberg-Marquardt)\n\
	--nits, -n N      : Max iterations for WLLS (default 4)\n"
};

enum class Algos { LLS, WLLS, NLLS };
static bool verbose = false;
static size_t nIterations = 4;
static string outPrefix;
static Algos algo;
static struct option long_options[] =
{
	{"B1", required_argument, 0, 'b'},
	{"mask", required_argument, 0, 'm'},
	{"out", required_argument, 0, 'o'},
	{"verbose", no_argument, 0, 'v'},
	{0, 0, 0, 0}
};
//******************************************************************************
// Main
//******************************************************************************
int main(int argc, char **argv)
{
	//**************************************************************************
	// Argument Processing
	//**************************************************************************
	cout << version << endl << credit_shared << endl;
	Nifti spgrFile, B1File, maskFile;
	Volume<float> spgrVol, B1Vol;
	Volume<char> maskVol;

	int indexptr = 0, c;
	while ((c = getopt_long(argc, argv, "hvm:o:b:a:n", long_options, &indexptr)) != -1) {
		switch (c) {
			case 'v': verbose = true; break;
			case 'm':
				cout << "Reading mask file " << optarg << endl;
				maskFile.open(optarg, Nifti::Mode::Read);
				maskVol.readFrom(maskFile);
				break;
			case 'o':
				outPrefix = optarg;
				cout << "Output prefix will be: " << outPrefix << endl;
				break;
			case 'b':
				cout << "Reading B1 file: " << optarg << endl;
				B1File.open(optarg, Nifti::Mode::Read);
				B1Vol.readFrom(B1File);
				break;
			case 'a':
				switch (*optarg) {
					case 'l': algo = Algos::LLS; break;
					case 'w': algo = Algos::WLLS; break;
					case 'n': algo = Algos::NLLS; break;
					default:
						cout << "Unknown algorithm type " << optarg << endl;
						exit(EXIT_FAILURE);
						break;
				} break;
			case 'n':
				nIterations = atoi(optarg);
				break;
			case 'h':
			case '?': // getopt will print an error message
				exit(EXIT_FAILURE);
		}
	}
	if ((argc - optind) != 1) {
		cout << "Incorrect number of arguments." << endl << usage << endl;
		exit(EXIT_FAILURE);
	}

	//**************************************************************************
	#pragma mark Gather SPGR data
	//**************************************************************************
	SimpleModel spgrMdl(Signal::Components::One, Model::Scaling::None);
	cout << "Opening SPGR file: " << argv[optind] << endl;
	spgrFile.open(argv[optind], Nifti::Mode::Read);
	if ((maskFile.isOpen() && !maskFile.matchesSpace(spgrFile)) ||
	    (B1File.isOpen() && !B1File.matchesSpace(spgrFile))) {
		cerr << "Mask or B1 dimensions/transform do not match SPGR file." << endl;
		exit(EXIT_FAILURE);
	}
	#ifdef AGILENT
	ProcPar pp;
	if (ReadPP(spgrFile, pp)) {
		spgrMdl.procparseSPGR(pp);
	} else
	#endif
	{
		spgrMdl.parseSPGR(spgrFile.dim(4), true);
	}
	if (verbose) {
		cout << spgrMdl;
		cout << "Ouput prefix will be: " << outPrefix << endl;
	}
	double TR = spgrMdl.m_signals.at(0)->m_TR;
	//**************************************************************************
	// Allocate memory for slices
	//**************************************************************************
	cout << "Reading SPGR data..." << flush;
	spgrVol.readFrom(spgrFile);
	cout << "done." << endl;
	//**************************************************************************
	// Create results data storage
	//**************************************************************************
	Volume<float> T1Vol(spgrVol.dims().head(3)), PDVol(spgrVol.dims().head(3)),
	              SoSVol(spgrVol.dims().head(3));
	//**************************************************************************
	// Do the fitting
	//**************************************************************************
	ThreadPool pool;
	for (size_t k = 0; k < spgrFile.dim(3); k++) {
		clock_t loopStart;
		// Read in data
		if (verbose)
			cout << "Starting slice " << k << "..." << flush;
		loopStart = clock();
		atomic<int> voxCount{0};

		for (size_t j = 0; j < spgrFile.dim(2); j++) {
			function<void (const size_t)> processVox = [&] (const size_t i) {
				if (!maskFile.isOpen() || (maskVol[{i, j, k}])) {
					voxCount++;
					//cout << spgrMdl.m_signals[0]->m_flip.transpose() << endl;
					double B1 = B1File.isOpen() ? B1Vol[{i, j, k}] : 1.;
					ArrayXd localAngles(spgrMdl.m_signals.at(0)->B1flip(B1));
					//cout << localAngles.transpose() << endl;
					double T1, PD, SoS;
					ArrayXd signal = spgrVol.series({i, j, k}).cast<double>();
					VectorXd Y = signal / localAngles.sin();
					MatrixXd X(Y.rows(), 2);
					X.col(0) = signal / localAngles.tan();
					X.col(1).setOnes();
					VectorXd b = (X.transpose() * X).partialPivLu().solve(X.transpose() * Y);
					T1 = -TR / log(b[0]);
					PD = b[1] / (1. - b[0]);
					if (algo == Algos::WLLS) {
						VectorXd W(spgrMdl.size());
						for (size_t n = 0; n < nIterations; n++) {
							W = (localAngles.sin() / (1. - (exp(-TR/T1)*localAngles.cos()))).square();
							b = (X.transpose() * W.asDiagonal() * X).partialPivLu().solve(X.transpose() * W.asDiagonal() * Y);
							T1 = -TR / log(b[0]);
							PD = b[1] / (1. - b[0]);
						}
					} else if (algo == Algos::NLLS) {
						DESPOTFunctor f(make_shared<SimpleModel>(spgrMdl), signal, B1, false);
						NumericalDiff<DESPOTFunctor> nDiff(f);
						LevenbergMarquardt<NumericalDiff<DESPOTFunctor>> lm(nDiff);
						lm.parameters.maxfev = nIterations;
						VectorXd p(4);
						p << PD, T1, 0., 0.; // Don't need T2 of f0 for this (yet)
						lm.lmder1(p);
						PD = p(0); T1 = p(1);
					}
					ArrayXd theory = spgrMdl.signal(Vector4d(PD, T1, 0., 0.), B1);
					SoS = (signal - theory).square().sum();
					T1Vol[{i, j, k}]  = static_cast<float>(T1);
					PDVol[{i, j, k}]  = static_cast<float>(PD);
					SoSVol[{i, j, k}] = static_cast<float>(SoS);
				}
			};

			pool.for_loop(processVox, spgrFile.dim(1));
		}

		if (verbose) {
			clock_t loopEnd = clock();
			if (voxCount > 0)
				cout << voxCount << " unmasked voxels, CPU time per voxel was "
				          << ((loopEnd - loopStart) / ((float)voxCount * CLOCKS_PER_SEC)) << " s, ";
			cout << "finished." << endl;
		}
	}

	if (verbose)
		cout << "Writing results." << endl;
	Nifti outFile(spgrFile, 1);
	outFile.description = version;
	outFile.open(outPrefix + "D1_T1.nii.gz", Nifti::Mode::Write);
	T1Vol.writeTo(outFile);
	outFile.close();
	outFile.open(outPrefix + "D1_PD.nii.gz", Nifti::Mode::Write);
	PDVol.writeTo(outFile);
	outFile.close();
	outFile.open(outPrefix + "D1_SoS.nii.gz", Nifti::Mode::Write);
	SoSVol.writeTo(outFile);
	outFile.close();

	cout << "All done." << endl;
	exit(EXIT_SUCCESS);
}
<commit_msg>Changed back to Volume<bool> for mask after fixing Nifti/Volume.<commit_after>/*
 *  despot1_main.cpp
 *
 *  Created by Tobias Wood on 17/10/2011.
 *  Copyright (c) 2011-2013 Tobias Wood.
 *
 *  This Source Code Form is subject to the terms of the Mozilla Public
 *  License, v. 2.0. If a copy of the MPL was not distributed with this
 *  file, You can obtain one at http://mozilla.org/MPL/2.0/.
 *
 */

#include <time.h>
#include <getopt.h>
#include <iostream>
#include <atomic>
#include <Eigen/Dense>

#include "Nifti/Nifti.h"
#include "Nifti/Volume.h"
#include "ThreadPool.h"
#include "Model.h"
#include "DESPOT_Functors.h"
#include "unsupported/Eigen/NonLinearOptimization"
#include "unsupported/Eigen/NumericalDiff"

#ifdef AGILENT
#include "procpar.h"
using namespace Agilent;
#endif

using namespace std;
using namespace Eigen;

//******************************************************************************
// Arguments / Usage
//******************************************************************************
const string usage {
"Usage is: despot1 [options] spgr_input \n\
\
Options:\n\
	--help, -h        : Print this message\n\
	--verbose, -v     : Print writeResiduals information\n\
	--mask, -m file   : Mask input with specified file\n\
	--out, -o path    : Add a prefix to the output filenames\n\
	--B1, -b file     : B1 Map file (ratio)\n\
	--algo, -a l      : LLS algorithm (default)\n\
	           w      : WLLS algorithm\n\
			   n      : NLLS (Levenberg-Marquardt)\n\
	--nits, -n N      : Max iterations for WLLS (default 4)\n"
};

enum class Algos { LLS, WLLS, NLLS };
static bool verbose = false;
static size_t nIterations = 4;
static string outPrefix;
static Algos algo;
static struct option long_options[] =
{
	{"B1", required_argument, 0, 'b'},
	{"mask", required_argument, 0, 'm'},
	{"out", required_argument, 0, 'o'},
	{"verbose", no_argument, 0, 'v'},
	{0, 0, 0, 0}
};
//******************************************************************************
// Main
//******************************************************************************
int main(int argc, char **argv)
{
	//**************************************************************************
	// Argument Processing
	//**************************************************************************
	cout << version << endl << credit_shared << endl;
	Nifti spgrFile, B1File, maskFile;
	Volume<float> spgrVol, B1Vol;
	Volume<bool> maskVol;

	int indexptr = 0, c;
	while ((c = getopt_long(argc, argv, "hvm:o:b:a:n", long_options, &indexptr)) != -1) {
		switch (c) {
			case 'v': verbose = true; break;
			case 'm':
				cout << "Reading mask file " << optarg << endl;
				maskFile.open(optarg, Nifti::Mode::Read);
				maskVol.readFrom(maskFile);
				break;
			case 'o':
				outPrefix = optarg;
				cout << "Output prefix will be: " << outPrefix << endl;
				break;
			case 'b':
				cout << "Reading B1 file: " << optarg << endl;
				B1File.open(optarg, Nifti::Mode::Read);
				B1Vol.readFrom(B1File);
				break;
			case 'a':
				switch (*optarg) {
					case 'l': algo = Algos::LLS; break;
					case 'w': algo = Algos::WLLS; break;
					case 'n': algo = Algos::NLLS; break;
					default:
						cout << "Unknown algorithm type " << optarg << endl;
						exit(EXIT_FAILURE);
						break;
				} break;
			case 'n':
				nIterations = atoi(optarg);
				break;
			case 'h':
			case '?': // getopt will print an error message
				exit(EXIT_FAILURE);
		}
	}
	if ((argc - optind) != 1) {
		cout << "Incorrect number of arguments." << endl << usage << endl;
		exit(EXIT_FAILURE);
	}

	//**************************************************************************
	#pragma mark Gather SPGR data
	//**************************************************************************
	SimpleModel spgrMdl(Signal::Components::One, Model::Scaling::None);
	cout << "Opening SPGR file: " << argv[optind] << endl;
	spgrFile.open(argv[optind], Nifti::Mode::Read);
	if ((maskFile.isOpen() && !maskFile.matchesSpace(spgrFile)) ||
	    (B1File.isOpen() && !B1File.matchesSpace(spgrFile))) {
		cerr << "Mask or B1 dimensions/transform do not match SPGR file." << endl;
		exit(EXIT_FAILURE);
	}
	#ifdef AGILENT
	ProcPar pp;
	if (ReadPP(spgrFile, pp)) {
		spgrMdl.procparseSPGR(pp);
	} else
	#endif
	{
		spgrMdl.parseSPGR(spgrFile.dim(4), true);
	}
	if (verbose) {
		cout << spgrMdl;
		cout << "Ouput prefix will be: " << outPrefix << endl;
	}
	double TR = spgrMdl.m_signals.at(0)->m_TR;
	//**************************************************************************
	// Allocate memory for slices
	//**************************************************************************
	cout << "Reading SPGR data..." << flush;
	spgrVol.readFrom(spgrFile);
	cout << "done." << endl;
	//**************************************************************************
	// Create results data storage
	//**************************************************************************
	Volume<float> T1Vol(spgrVol.dims().head(3)), PDVol(spgrVol.dims().head(3)),
	              SoSVol(spgrVol.dims().head(3));
	//**************************************************************************
	// Do the fitting
	//**************************************************************************
	ThreadPool pool;
	for (size_t k = 0; k < spgrFile.dim(3); k++) {
		clock_t loopStart;
		// Read in data
		if (verbose)
			cout << "Starting slice " << k << "..." << flush;
		loopStart = clock();
		atomic<int> voxCount{0};

		for (size_t j = 0; j < spgrFile.dim(2); j++) {
			function<void (const size_t)> processVox = [&] (const size_t i) {
				if (!maskFile.isOpen() || (maskVol[{i, j, k}])) {
					voxCount++;
					//cout << spgrMdl.m_signals[0]->m_flip.transpose() << endl;
					double B1 = B1File.isOpen() ? B1Vol[{i, j, k}] : 1.;
					ArrayXd localAngles(spgrMdl.m_signals.at(0)->B1flip(B1));
					//cout << localAngles.transpose() << endl;
					double T1, PD, SoS;
					ArrayXd signal = spgrVol.series({i, j, k}).cast<double>();
					VectorXd Y = signal / localAngles.sin();
					MatrixXd X(Y.rows(), 2);
					X.col(0) = signal / localAngles.tan();
					X.col(1).setOnes();
					VectorXd b = (X.transpose() * X).partialPivLu().solve(X.transpose() * Y);
					T1 = -TR / log(b[0]);
					PD = b[1] / (1. - b[0]);
					if (algo == Algos::WLLS) {
						VectorXd W(spgrMdl.size());
						for (size_t n = 0; n < nIterations; n++) {
							W = (localAngles.sin() / (1. - (exp(-TR/T1)*localAngles.cos()))).square();
							b = (X.transpose() * W.asDiagonal() * X).partialPivLu().solve(X.transpose() * W.asDiagonal() * Y);
							T1 = -TR / log(b[0]);
							PD = b[1] / (1. - b[0]);
						}
					} else if (algo == Algos::NLLS) {
						DESPOTFunctor f(make_shared<SimpleModel>(spgrMdl), signal, B1, false);
						NumericalDiff<DESPOTFunctor> nDiff(f);
						LevenbergMarquardt<NumericalDiff<DESPOTFunctor>> lm(nDiff);
						lm.parameters.maxfev = nIterations;
						VectorXd p(4);
						p << PD, T1, 0., 0.; // Don't need T2 of f0 for this (yet)
						lm.lmder1(p);
						PD = p(0); T1 = p(1);
					}
					ArrayXd theory = spgrMdl.signal(Vector4d(PD, T1, 0., 0.), B1);
					SoS = (signal - theory).square().sum();
					T1Vol[{i, j, k}]  = static_cast<float>(T1);
					PDVol[{i, j, k}]  = static_cast<float>(PD);
					SoSVol[{i, j, k}] = static_cast<float>(SoS);
				}
			};

			pool.for_loop(processVox, spgrFile.dim(1));
		}

		if (verbose) {
			clock_t loopEnd = clock();
			if (voxCount > 0)
				cout << voxCount << " unmasked voxels, CPU time per voxel was "
				          << ((loopEnd - loopStart) / ((float)voxCount * CLOCKS_PER_SEC)) << " s, ";
			cout << "finished." << endl;
		}
	}

	if (verbose)
		cout << "Writing results." << endl;
	Nifti outFile(spgrFile, 1);
	outFile.description = version;
	outFile.open(outPrefix + "D1_T1.nii.gz", Nifti::Mode::Write);
	T1Vol.writeTo(outFile);
	outFile.close();
	outFile.open(outPrefix + "D1_PD.nii.gz", Nifti::Mode::Write);
	PDVol.writeTo(outFile);
	outFile.close();
	outFile.open(outPrefix + "D1_SoS.nii.gz", Nifti::Mode::Write);
	SoSVol.writeTo(outFile);
	outFile.close();

	cout << "All done." << endl;
	exit(EXIT_SUCCESS);
}
<|endoftext|>


<commit_before>//cell.cpp
//===================
// Forward Declarations

//===================
// Include Dependencies
#include <iostream>
#include <sstream>
#include <fstream>
#include <vector>

#include "phys.h"
#include "coord.h"
#include "node.h"
#include "cell.h"
#include "tissue.h"
//===================

// Cell Class Member functions

// Constructors

Cell::Cell(int rank, Coord corner, double height,
			double width, int Ti, Tissue* tiss)    {

	this->my_tissue = tiss;
	this->rank = rank;

	init_cell_time = Ti;

	int num_Init_Wall_Nodes = 100;
	double perim = (height * 2) + (width * 2);
	//space between wall nodes
	double space = perim / num_Init_Wall_Nodes;

	int num_end_nodes = (width / space);
	int num_flank_nodes = (height / space);

	double curr_X;
	double curr_Y;
	Coord location;

	Wall_Node* currW;
	Wall_Node* prevW;

	// Create first corner
	prevW = new Corner_Node(corner, this);
	corners.push_back(prevW);
	num_wall_nodes++;

	//create lower end
	curr_X = corner.get_X() + space;
	curr_Y = corner.get_Y();

	for (int i = 0; i < num_end_nodes; i++) {
		location = Coord(curr_X, curr_Y);
		currW = new End_Node(location, this);

		// Set neighbor relationships
		currW->set_Right_Neighbor(prevW);
		prevW->set_Left_Neighbor(currW);

		//update for next iteration
		prevW = currW;
		curr_X += space;
		num_wall_nodes++;
	}

	//create second corner
	location = Coord(curr_X, curr_Y);
	currW = new Corner_Node(location, this);
	currW->set_Right_Neighbor(prevW);
	prevW->set_Left_Neighbor(currW);
	corners.push_back(currW);
	prevW = currW;
	num_wall_nodes++;

	//create right flank
	//   curr_X should be good
	curr_Y += space;

	for (int i = 0; i < num_flank_nodes; i++) {
		location = Coord(curr_X, curr_Y);
		currW = new Flank_Node(location, this);

		// Set neighbor relationships
		currW->set_Right_Neighbor(prevW);
		prevW->set_Left_Neighbor(currW);

		//update for next iteration
		prevW = currW;
		curr_Y += space;
		num_wall_nodes++;
	}

	//create third corner
	location = Coord(curr_X, curr_Y);
	currW = new Corner_Node(location, this);
	currW->set_Right_Neighbor(prevW);
	prevW->set_Left_Neighbor(currW);
	corners.push_back(currW);
	prevW = currW;
	num_wall_nodes++;

	//create upper end
	curr_X -= space;
		//curr_Y should be good

	for (int i = 0; i < num_end_nodes; i++) {
		location = Coord(curr_X, curr_Y);
		currW = new End_Node(location, this);

		// Set neighbor relationships
		currW->set_Right_Neighbor(prevW);
		prevW->set_Left_Neighbor(currW);

		//update for next iteration
		prevW = currW;
		curr_X -= space;
		num_wall_nodes++;
	}

	//create fourth corner
	location = Coord(curr_X, curr_Y);
	currW = new Corner_Node(location, this);
	currW->set_Right_Neighbor(prevW);
	prevW->set_Left_Neighbor(currW);
	corners.push_back(currW);
	prevW = currW;
	num_wall_nodes++;

	//create left flank
	//	remember that we already have first corner.
	//	don't make it again.

	//   curr_X should be good
	curr_Y -= space;

	for (int i = 0; i < num_flank_nodes; i++) {
		location = Coord(curr_X, curr_Y);
		currW = new Flank_Node(location, this);

		// Set neighbor relationships
		currW->set_Right_Neighbor(prevW);
		prevW->set_Left_Neighbor(currW);

		//update for next iteration
		prevW = currW;
		curr_Y -= space;
		num_wall_nodes++;
	}

	//connect first and last nodes
	prevW->set_Left_Neighbor(corners.at(0));
	corners.at(0)->set_Right_Neighbor(prevW);

	//initialize angles
	update_Wall_Angles();

	//make cytoplasm node
	double init_Cell_Radius = .9375;
	int init_Cyt_Count = 20;
	double random_angle;
	double random_radius;
	double x;
	double y;
	for(int i = 0; i < init_Cyt_Count; i++) {
		random_angle = 2*pi*(rand() / RAND_MAX);
		random_radius = init_Cell_Radius*(rand() / RAND_MAX);
		x = random_radius*cos(random_angle);
		//also need to add the x and y value of corner node
		//of this cell to put these values in the right place
		y = random_radius*sin(random_angle);
		location = Coord(x,y);
		Cyt_Node* cyt = new Cyt_Node(location,this);
		cyt_nodes.push_back(cyt);
		num_cyt_nodes++;
	}
}

// Getters and Setters

void Cell::get_CytNodes(vector<Cyt_Node*>& cyts) {
	cyts = cyt_nodes;
	return;
}

Wall_Node* Cell::get_WallNodes() {
	return corners.at(0);
}

void Cell::get_Neighbor_Cells(vector<Cell*>& cells) {
	my_tissue->get_Cells(cells);
	return;
}

int Cell::get_Num_Nodes() {
	return num_wall_nodes + num_cyt_nodes;
}

// Calc Force

void Cell::calc_New_Forces() {
	//calc forces on cyt nodes
	for (unsigned int i = 0; i < cyt_nodes.size(); i++) {
		cyt_nodes.at(i)->calc_Forces();
	}

	//calc forces on wall nodes
	Wall_Node* curr = corners.at(0);

	do {
		curr->calc_Forces();
		curr = curr->get_Left_Neighbor();

	} while(curr != corners.at(0));

	return;
}

// Update Node Locations

void Cell::update_Node_Locations() {

	//update cyt nodes
	for (unsigned int i = 0; i < cyt_nodes.size(); i++) {
		cyt_nodes.at(i)->update_Location();
	}

	//update wall nodes
	Wall_Node* curr = corners.at(0);

	do {
		curr->update_Location();
		curr = curr->get_Left_Neighbor();

	} while(curr != corners.at(0));

	return;
}

void Cell::update_Wall_Angles() {

	Wall_Node* curr = corners.at(0);

	do {
		curr->update_Angle();
		curr = curr->get_Left_Neighbor();
	} while (curr != corners.at(0));

	return;
}

void Cell::print_Data_Output(ofstream& ofs) {

	ofs << "This is where data output goes" << endl;

	return;
}

void Cell::print_VTK_Points(ofstream& ofs, int& count) {

	Wall_Node* curr_wall = corners.at(0);
	do {
		Coord loc = curr_wall->get_Location();
		ofs << loc.get_X() << ' ' << loc.get_Y() << ' ' << 0 << endl;

		curr_wall = curr_wall->get_Left_Neighbor();
		count++;
	} while(curr_wall != corners.at(0));

	for (unsigned int i = 0; i < cyt_nodes.size(); i++) {
		Coord loc = cyt_nodes.at(i)->get_Location();
		ofs << loc.get_X() << ' ' << loc.get_Y() << ' ' << 0 << endl;
		count++;
	}

	return;
}

Wall_Node* Cell::find_Largest_Length(int& side) {
	side = 0; //know that we start on bottom flank
	// We know we start at first entry of corner vector.
	//Each time we pass another corner, increment side by 1
	// side = 1 or 3 => end
	// side = 2 or 4 => flank
	Wall_Node* curr = corners.at(0);
	Wall_Node* biggest = NULL;
	Coord left_Neighb_loc;
	Coord curr_Loc;
	Coord diff_vect;
	double max_len = 0;
	double len;
	//loop through all possible Cell Wall 'links' to find biggest
	do {
		//if encounter a corner, increment side to know if end or flank
		if (curr->is_Corner()) {
			side++;
		}

		//finding current lengths and comparing
		left_Neighb_loc = curr->get_Left_Neighbor()->get_Location();
		curr_Loc = curr->get_Location();
		diff_vect = left_Neighb_loc - curr_Loc;
		len = diff_vect.length();
		if(len > max_len) {
			max_len = len;
			biggest = curr;
		}
		curr = curr->get_Left_Neighbor();

	} while (curr != corners.at(0));

	return biggest;
}

void Cell::add_Wall_Node(const int Ti) {

	if ((Ti - init_cell_time) % ADD_WALL_TIMER != 0) {
		return;
	}

	//we will add the node based on what the function find_Largest_Length() returns
	//find_Largest_Length() returns a pointer to the node where the largest
	//Cell Wall link is to the left of that node
	//first we apply find_Largest_Length() to the cell to get a pointer to the right
	//of where the new node is added
	int side = 0;
	Wall_Node* right_Node = find_Largest_Length(side);
	//to the left of this node will be the node to the left of the new node
	Wall_Node* left_Node = right_Node->get_Left_Neighbor();
	//now we find the coords of each of these nodes to use to find the new coords
	Coord right_Coords = right_Node->get_Location();
//	cout << "Right side coordinates" << endl;
//	cout << right_Coords << endl;
	Coord left_Coords = left_Node->get_Location();
//	cout << "Left side coordinates" << endl;
//	cout << left_Coords << endl;
	//add it halfway between these two coords

	Coord new_Coords = (right_Coords + left_Coords)*(.5);
//	cout << "New Coords: " << new_Coords << endl;
	Wall_Node* new_Node;

	if (side % 2 == 1 ) { //end
		new_Node = new End_Node(new_Coords, this, left_Node, right_Node);
	}
	else { //flank
		new_Node = new Flank_Node(new_Coords, this, left_Node, right_Node);
	}

	right_Node->set_Left_Neighbor(new_Node);
	left_Node->set_Right_Neighbor(new_Node);

	num_wall_nodes++;
	return;
}

void Cell::add_Cyt_Node(const int Ti) {

	if ((Ti - init_cell_time) % ADD_CYT_TIMER != 0) {
		return;
	}

	Coord len_vect = corners.at(0)->get_Location() - corners.at(3)->get_Location();
	Coord width_vect = corners.at(0)->get_Location() - corners.at(1)->get_Location();
	double new_x = (rand() / RAND_MAX) * width_vect.length();
	double new_y = (rand() / RAND_MAX) * len_vect.length();

	Coord new_Coord(new_x, new_y);
	new_Coord += corners.at(0)->get_Location();
	double away_from_edge_x = .5;
	double away_from_edge_y = .5;
	Coord away_from_Edge(away_from_edge_x,away_from_edge_y);
	new_Coord += away_from_Edge;

	Cyt_Node* cyt = new Cyt_Node(new_Coord, this);
	cyt_nodes.push_back(cyt);

	num_cyt_nodes++;
	return;
}

<commit_msg>fixed init cyt nodes loc's<commit_after>//cell.cpp
//===================
// Forward Declarations

//===================
// Include Dependencies
#include <iostream>
#include <sstream>
#include <fstream>
#include <vector>

#include "phys.h"
#include "coord.h"
#include "node.h"
#include "cell.h"
#include "tissue.h"
//===================

// Cell Class Member functions

// Constructors

Cell::Cell(int rank, Coord corner, double height,
			double width, int Ti, Tissue* tiss)    {

	this->my_tissue = tiss;
	this->rank = rank;

	init_cell_time = Ti;

	int num_Init_Wall_Nodes = 100;
	double perim = (height * 2) + (width * 2);
	//space between wall nodes
	double space = perim / num_Init_Wall_Nodes;

	int num_end_nodes = (width / space);
	int num_flank_nodes = (height / space);

	double curr_X;
	double curr_Y;
	Coord location;

	Wall_Node* currW;
	Wall_Node* prevW;

	// Create first corner
	prevW = new Corner_Node(corner, this);
	corners.push_back(prevW);
	num_wall_nodes++;

	//create lower end
	curr_X = corner.get_X() + space;
	curr_Y = corner.get_Y();

	for (int i = 0; i < num_end_nodes; i++) {
		location = Coord(curr_X, curr_Y);
		currW = new End_Node(location, this);

		// Set neighbor relationships
		currW->set_Right_Neighbor(prevW);
		prevW->set_Left_Neighbor(currW);

		//update for next iteration
		prevW = currW;
		curr_X += space;
		num_wall_nodes++;
	}

	//create second corner
	location = Coord(curr_X, curr_Y);
	currW = new Corner_Node(location, this);
	currW->set_Right_Neighbor(prevW);
	prevW->set_Left_Neighbor(currW);
	corners.push_back(currW);
	prevW = currW;
	num_wall_nodes++;

	//create right flank
	//   curr_X should be good
	curr_Y += space;

	for (int i = 0; i < num_flank_nodes; i++) {
		location = Coord(curr_X, curr_Y);
		currW = new Flank_Node(location, this);

		// Set neighbor relationships
		currW->set_Right_Neighbor(prevW);
		prevW->set_Left_Neighbor(currW);

		//update for next iteration
		prevW = currW;
		curr_Y += space;
		num_wall_nodes++;
	}

	//create third corner
	location = Coord(curr_X, curr_Y);
	currW = new Corner_Node(location, this);
	currW->set_Right_Neighbor(prevW);
	prevW->set_Left_Neighbor(currW);
	corners.push_back(currW);
	prevW = currW;
	num_wall_nodes++;

	//create upper end
	curr_X -= space;
		//curr_Y should be good

	for (int i = 0; i < num_end_nodes; i++) {
		location = Coord(curr_X, curr_Y);
		currW = new End_Node(location, this);

		// Set neighbor relationships
		currW->set_Right_Neighbor(prevW);
		prevW->set_Left_Neighbor(currW);

		//update for next iteration
		prevW = currW;
		curr_X -= space;
		num_wall_nodes++;
	}

	//create fourth corner
	location = Coord(curr_X, curr_Y);
	currW = new Corner_Node(location, this);
	currW->set_Right_Neighbor(prevW);
	prevW->set_Left_Neighbor(currW);
	corners.push_back(currW);
	prevW = currW;
	num_wall_nodes++;

	//create left flank
	//	remember that we already have first corner.
	//	don't make it again.

	//   curr_X should be good
	curr_Y -= space;

	for (int i = 0; i < num_flank_nodes; i++) {
		location = Coord(curr_X, curr_Y);
		currW = new Flank_Node(location, this);

		// Set neighbor relationships
		currW->set_Right_Neighbor(prevW);
		prevW->set_Left_Neighbor(currW);

		//update for next iteration
		prevW = currW;
		curr_Y -= space;
		num_wall_nodes++;
	}

	//connect first and last nodes
	prevW->set_Left_Neighbor(corners.at(0));
	corners.at(0)->set_Right_Neighbor(prevW);

	//initialize angles
	update_Wall_Angles();

	//make cytoplasm node
	double init_Cell_Radius = .9375;
	int init_Cyt_Count = 20;
	double random_angle;
	double random_radius;
	double x;
	double y;
	for(int i = 0; i < init_Cyt_Count; i++) {
		random_angle = 2*pi*(static_cast<double>(rand()) / RAND_MAX);
		random_radius = init_Cell_Radius*(static_cast<double>(rand()) / RAND_MAX);
		x = (random_radius * cos(random_angle)) + corner.get_X() + (width / 2);
		//also need to add the x and y value of corner node
		//of this cell to put these values in the right place
		y = (random_radius * sin(random_angle)) + corner.get_Y() + (height / 2);
		location = Coord(x,y);
		Cyt_Node* cyt = new Cyt_Node(location,this);
		cyt_nodes.push_back(cyt);
		num_cyt_nodes++;
	}
}

// Getters and Setters

void Cell::get_CytNodes(vector<Cyt_Node*>& cyts) {
	cyts = cyt_nodes;
	return;
}

Wall_Node* Cell::get_WallNodes() {
	return corners.at(0);
}

void Cell::get_Neighbor_Cells(vector<Cell*>& cells) {
	my_tissue->get_Cells(cells);
	return;
}

int Cell::get_Num_Nodes() {
	return num_wall_nodes + num_cyt_nodes;
}

// Calc Force

void Cell::calc_New_Forces() {
	//calc forces on cyt nodes
	for (unsigned int i = 0; i < cyt_nodes.size(); i++) {
		cyt_nodes.at(i)->calc_Forces();
	}

	//calc forces on wall nodes
	Wall_Node* curr = corners.at(0);

	do {
		curr->calc_Forces();
		curr = curr->get_Left_Neighbor();

	} while(curr != corners.at(0));

	return;
}

// Update Node Locations

void Cell::update_Node_Locations() {

	//update cyt nodes
	for (unsigned int i = 0; i < cyt_nodes.size(); i++) {
		cyt_nodes.at(i)->update_Location();
	}

	//update wall nodes
	Wall_Node* curr = corners.at(0);

	do {
		curr->update_Location();
		curr = curr->get_Left_Neighbor();

	} while(curr != corners.at(0));

	return;
}

void Cell::update_Wall_Angles() {

	Wall_Node* curr = corners.at(0);

	do {
		curr->update_Angle();
		curr = curr->get_Left_Neighbor();
	} while (curr != corners.at(0));

	return;
}

void Cell::print_Data_Output(ofstream& ofs) {

	ofs << "This is where data output goes" << endl;

	return;
}

void Cell::print_VTK_Points(ofstream& ofs, int& count) {

	Wall_Node* curr_wall = corners.at(0);
	do {
		Coord loc = curr_wall->get_Location();
		ofs << loc.get_X() << ' ' << loc.get_Y() << ' ' << 0 << endl;

		curr_wall = curr_wall->get_Left_Neighbor();
		count++;
	} while(curr_wall != corners.at(0));

	for (unsigned int i = 0; i < cyt_nodes.size(); i++) {
		Coord loc = cyt_nodes.at(i)->get_Location();
		ofs << loc.get_X() << ' ' << loc.get_Y() << ' ' << 0 << endl;
		count++;
	}

	return;
}

Wall_Node* Cell::find_Largest_Length(int& side) {
	side = 0; //know that we start on bottom flank
	// We know we start at first entry of corner vector.
	//Each time we pass another corner, increment side by 1
	// side = 1 or 3 => end
	// side = 2 or 4 => flank
	Wall_Node* curr = corners.at(0);
	Wall_Node* biggest = NULL;
	Coord left_Neighb_loc;
	Coord curr_Loc;
	Coord diff_vect;
	double max_len = 0;
	double len;
	//loop through all possible Cell Wall 'links' to find biggest
	do {
		//if encounter a corner, increment side to know if end or flank
		if (curr->is_Corner()) {
			side++;
		}

		//finding current lengths and comparing
		left_Neighb_loc = curr->get_Left_Neighbor()->get_Location();
		curr_Loc = curr->get_Location();
		diff_vect = left_Neighb_loc - curr_Loc;
		len = diff_vect.length();
		if(len > max_len) {
			max_len = len;
			biggest = curr;
		}
		curr = curr->get_Left_Neighbor();

	} while (curr != corners.at(0));

	return biggest;
}

void Cell::add_Wall_Node(const int Ti) {

	if ((Ti - init_cell_time) % ADD_WALL_TIMER != 0) {
		return;
	}

	//we will add the node based on what the function find_Largest_Length() returns
	//find_Largest_Length() returns a pointer to the node where the largest
	//Cell Wall link is to the left of that node
	//first we apply find_Largest_Length() to the cell to get a pointer to the right
	//of where the new node is added
	int side = 0;
	Wall_Node* right_Node = find_Largest_Length(side);
	//to the left of this node will be the node to the left of the new node
	Wall_Node* left_Node = right_Node->get_Left_Neighbor();
	//now we find the coords of each of these nodes to use to find the new coords
	Coord right_Coords = right_Node->get_Location();
//	cout << "Right side coordinates" << endl;
//	cout << right_Coords << endl;
	Coord left_Coords = left_Node->get_Location();
//	cout << "Left side coordinates" << endl;
//	cout << left_Coords << endl;
	//add it halfway between these two coords

	Coord new_Coords = (right_Coords + left_Coords)*(.5);
//	cout << "New Coords: " << new_Coords << endl;
	Wall_Node* new_Node;

	if (side % 2 == 1 ) { //end
		new_Node = new End_Node(new_Coords, this, left_Node, right_Node);
	}
	else { //flank
		new_Node = new Flank_Node(new_Coords, this, left_Node, right_Node);
	}

	right_Node->set_Left_Neighbor(new_Node);
	left_Node->set_Right_Neighbor(new_Node);

	num_wall_nodes++;
	return;
}

void Cell::add_Cyt_Node(const int Ti) {

	if ((Ti - init_cell_time) % ADD_CYT_TIMER != 0) {
		return;
	}

	Coord len_vect = corners.at(0)->get_Location() - corners.at(3)->get_Location();
	Coord width_vect = corners.at(0)->get_Location() - corners.at(1)->get_Location();
	double new_x = (rand() / RAND_MAX) * width_vect.length();
	double new_y = (rand() / RAND_MAX) * len_vect.length();

	Coord new_Coord(new_x, new_y);
	new_Coord += corners.at(0)->get_Location();
	double away_from_edge_x = .5;
	double away_from_edge_y = .5;
	Coord away_from_Edge(away_from_edge_x,away_from_edge_y);
	new_Coord += away_from_Edge;

	Cyt_Node* cyt = new Cyt_Node(new_Coord, this);
	cyt_nodes.push_back(cyt);

	num_cyt_nodes++;
	return;
}

<|endoftext|>


<commit_before>#ifndef GENERIC_CIFY_HPP
# define GENERIC_CIFY_HPP
# pragma once

#include <utility>

namespace generic
{

namespace
{

template <typename R, typename ...A> using return_type = R (*)(A...);

}

template <typename F, typename R, typename ...A>
return_type<R, A...> cify(R (*)(A...), F&& f)
{
  static F f_(::std::forward<F>(f));

  new (static_cast<void*>(&f_)) F(::std::forward<F>(f));

  struct S
  {
    static R f(A... args)
    {
      f_(::std::forward<A>(args)...);
    }
  };

  return &S::f;
}

}

#endif // GENERIC_CIFY_HPP
<commit_msg>some fixes<commit_after>#ifndef GENERIC_CIFY_HPP
# define GENERIC_CIFY_HPP
# pragma once

#include <utility>

namespace generic
{

namespace
{

template <typename R, typename ...A> using return_type = R (*)(A...);

}

template <typename F, typename R, typename ...A>
return_type<R, A...> cify(R (*)(A...), F&& f)
{
  static F const f_(::std::forward<F>(f));

  //new (static_cast<void*>(&f_)) F(::std::forward<F>(f));

  struct S
  {
    static R f(A... args)
    {
      f_(::std::forward<A>(args)...);
    }
  };

  return &S::f;
}

}

#endif // GENERIC_CIFY_HPP
<|endoftext|>


<commit_before>//#define _USE_MATH_DEFINES
//#include <cmath>
#include "core/crc32.h"
#include "core/event_manager.h"
#include "core/math_utils.h"
#include "core/matrix.h"
#include "core/quat.h"
#include "editor/gizmo.h"
#include "graphics/irender_device.h"
#include "graphics/model.h"
#include "graphics/renderer.h"
#include "universe/entity_moved_event.h"
#include "universe/universe.h"

namespace Lux
{

Gizmo::Gizmo()
{
//	m_handle = 0;
	m_selected_entity.index = -1;
}

Gizmo::~Gizmo()
{
}

void Gizmo::destroy()
{
	//h3dRemoveNode(m_handle);
}

void Gizmo::create(Renderer& renderer)
{
	m_scale = 1;
	m_renderer = &renderer;
	m_model = renderer.getModel("models/gizmo.msh");
}

void Gizmo::hide()
{
	//h3dSetNodeFlags(m_handle, h3dGetNodeFlags(m_handle) | H3DNodeFlags::Inactive, true);
}

void Gizmo::show()
{
	/*h3dSetNodeFlags(m_handle, h3dGetNodeFlags(m_handle) & ~H3DNodeFlags::Inactive, true);
	h3dSetNodeFlags(m_handle, h3dGetNodeFlags(m_handle) | H3DNodeFlags::NoCastShadow, true);	*/
}

void Gizmo::getMatrix(Matrix& mtx)
{
	m_selected_entity.getMatrix(mtx);
}

void Gizmo::updateScale(Component camera)
{
	if (m_selected_entity.isValid())
	{
		Matrix camera_mtx;
		camera.entity.getMatrix(camera_mtx);
		Matrix mtx;
		getMatrix(mtx);
		Vec3 pos = mtx.getTranslation();
		float fov;
		m_renderer->getCameraFov(camera, fov);
		float scale = tanf(fov * Math::PI / 180 * 0.5f) * (mtx.getTranslation() - camera_mtx.getTranslation()).length() * 2;
		scale /= 20 * mtx.getXVector().length();
		m_scale = scale;
	}
}

void Gizmo::setEntity(Entity entity)
{
	//h3dSetNodeFlags(m_handle, h3dGetNodeFlags(m_handle) | H3DNodeFlags::NoCastShadow, true);
	m_selected_entity = entity;
	if(m_selected_entity.index != -1)
	{
		show();
	}
	else
	{
		hide();
	}
}

void Gizmo::setUniverse(Universe* universe)
{
	m_universe = universe;
}

RayCastModelHit Gizmo::castRay(const Vec3& origin, const Vec3& dir)
{
	if (m_selected_entity.isValid())
	{
		return m_model->castRay(origin, dir, m_selected_entity.getMatrix(), m_scale);
	}
	RayCastModelHit hit;
	hit.m_is_hit = false;
	return hit;
}

void Gizmo::render(Renderer& renderer, IRenderDevice& render_device)
{
	if(m_selected_entity.isValid())
	{
		Matrix scale_mtx = Matrix::IDENTITY;
		scale_mtx.m11 = scale_mtx.m22 = scale_mtx.m33 = m_scale;
		Matrix mtx = m_selected_entity.getMatrix() * scale_mtx;
		renderer.renderModel(*m_model, mtx, render_device.getPipeline());
	}
}

void Gizmo::startTransform(Component camera, int x, int y, TransformMode mode)
{
	m_transform_mode = mode;
	m_transform_point = getMousePlaneIntersection(camera, x, y);
	m_relx_accum = m_rely_accum = 0;
}

void Gizmo::transform(Component camera, TransformOperation operation, int x, int y, int relx, int rely, int flags)
{
	if(m_selected_entity.index != -1)
	{
		if(operation == TransformOperation::ROTATE && m_transform_mode != TransformMode::CAMERA_XZ)
		{
			Vec3 pos = m_selected_entity.getPosition();
			Matrix emtx;
			m_selected_entity.getMatrix(emtx);
			Vec3 axis;
			switch(m_transform_mode)
			{
				case TransformMode::X:
					axis = emtx.getXVector();
					break;
				case TransformMode::Y:
					axis = emtx.getYVector();
					break;
				case TransformMode::Z:
					axis = emtx.getZVector();
					break;
			}
			float angle = 0;
			if(flags & Flags::FIXED_STEP)
			{
				m_relx_accum += relx;
				m_rely_accum += rely;
				if(m_relx_accum + m_rely_accum > 50)
				{
					angle = (float)Math::PI / 4;
					m_relx_accum = m_rely_accum = 0;
				}
				else if(m_relx_accum + m_rely_accum < -50)
				{
					angle = -(float)Math::PI / 4;
					m_relx_accum = m_rely_accum = 0;
				}
				else
				{
					angle = 0;
				}
			}
			else
			{
				angle = (relx + rely) / 100.0f;
			}
			m_selected_entity.setRotation(m_selected_entity.getRotation() * Quat(axis, angle));
			m_selected_entity.setPosition(pos);
		}
		else
		{
			Vec3 intersection = getMousePlaneIntersection(camera, x, y);
			Vec3 delta = intersection - m_transform_point;
			m_transform_point = intersection;
			Matrix mtx;
			m_selected_entity.getMatrix(mtx);
			mtx.translate(delta);
			m_selected_entity.setMatrix(mtx);
		}
	}
}

Vec3 Gizmo::getMousePlaneIntersection(Component camera, int x, int y)
{
	Vec3 origin, dir;
	m_renderer->getRay(camera, (float)x, (float)y, origin, dir);
	dir.normalize();
	Matrix camera_mtx;
	camera.entity.getMatrix(camera_mtx);
	if(m_transform_mode == TransformMode::CAMERA_XZ)
	{
		Vec3 a = crossProduct(Vec3(0, 1, 0), camera_mtx.getXVector());
		Vec3 b = crossProduct(Vec3(0, 1, 0), camera_mtx.getZVector());
		Vec3 plane_normal = crossProduct(a, b);
		return Math::getRayPlaneIntersecion(origin, dir, m_selected_entity.getPosition(), plane_normal);
	}
	Vec3 axis;
	switch(m_transform_mode)
	{
		case TransformMode::X:
			axis = m_selected_entity.getMatrix().getXVector();
			break;
		case TransformMode::Y:
			axis = m_selected_entity.getMatrix().getYVector();
			break;
		case TransformMode::Z:
			axis = m_selected_entity.getMatrix().getZVector();
			break;
	}
	Vec3 pos = m_selected_entity.getPosition();
	Vec3 normal = crossProduct(crossProduct(dir, axis), dir);
	float d = dotProduct(origin - pos, normal) / dotProduct(axis, normal);
	return axis * d + pos;
}

} // !namespace Lux<commit_msg>fixed float imprecisions when moving objects with a gizmo<commit_after>//#define _USE_MATH_DEFINES
//#include <cmath>
#include "core/crc32.h"
#include "core/event_manager.h"
#include "core/math_utils.h"
#include "core/matrix.h"
#include "core/quat.h"
#include "editor/gizmo.h"
#include "graphics/irender_device.h"
#include "graphics/model.h"
#include "graphics/renderer.h"
#include "universe/entity_moved_event.h"
#include "universe/universe.h"

namespace Lux
{

Gizmo::Gizmo()
{
//	m_handle = 0;
	m_selected_entity.index = -1;
}

Gizmo::~Gizmo()
{
}

void Gizmo::destroy()
{
	//h3dRemoveNode(m_handle);
}

void Gizmo::create(Renderer& renderer)
{
	m_scale = 1;
	m_renderer = &renderer;
	m_model = renderer.getModel("models/gizmo.msh");
}

void Gizmo::hide()
{
	//h3dSetNodeFlags(m_handle, h3dGetNodeFlags(m_handle) | H3DNodeFlags::Inactive, true);
}

void Gizmo::show()
{
	/*h3dSetNodeFlags(m_handle, h3dGetNodeFlags(m_handle) & ~H3DNodeFlags::Inactive, true);
	h3dSetNodeFlags(m_handle, h3dGetNodeFlags(m_handle) | H3DNodeFlags::NoCastShadow, true);	*/
}

void Gizmo::getMatrix(Matrix& mtx)
{
	m_selected_entity.getMatrix(mtx);
}

void Gizmo::updateScale(Component camera)
{
	if (m_selected_entity.isValid())
	{
		Matrix camera_mtx;
		camera.entity.getMatrix(camera_mtx);
		Matrix mtx;
		getMatrix(mtx);
		Vec3 pos = mtx.getTranslation();
		float fov;
		m_renderer->getCameraFov(camera, fov);
		float scale = tanf(fov * Math::PI / 180 * 0.5f) * (mtx.getTranslation() - camera_mtx.getTranslation()).length() * 2;
		scale /= 20 * mtx.getXVector().length();
		m_scale = scale;
	}
}

void Gizmo::setEntity(Entity entity)
{
	//h3dSetNodeFlags(m_handle, h3dGetNodeFlags(m_handle) | H3DNodeFlags::NoCastShadow, true);
	m_selected_entity = entity;
	if(m_selected_entity.index != -1)
	{
		show();
	}
	else
	{
		hide();
	}
}

void Gizmo::setUniverse(Universe* universe)
{
	m_universe = universe;
}

RayCastModelHit Gizmo::castRay(const Vec3& origin, const Vec3& dir)
{
	if (m_selected_entity.isValid())
	{
		return m_model->castRay(origin, dir, m_selected_entity.getMatrix(), m_scale);
	}
	RayCastModelHit hit;
	hit.m_is_hit = false;
	return hit;
}

void Gizmo::render(Renderer& renderer, IRenderDevice& render_device)
{
	if(m_selected_entity.isValid())
	{
		Matrix scale_mtx = Matrix::IDENTITY;
		scale_mtx.m11 = scale_mtx.m22 = scale_mtx.m33 = m_scale;
		Matrix mtx = m_selected_entity.getMatrix() * scale_mtx;
		renderer.renderModel(*m_model, mtx, render_device.getPipeline());
	}
}

void Gizmo::startTransform(Component camera, int x, int y, TransformMode mode)
{
	m_transform_mode = mode;
	m_transform_point = getMousePlaneIntersection(camera, x, y);
	m_relx_accum = m_rely_accum = 0;
}

void Gizmo::transform(Component camera, TransformOperation operation, int x, int y, int relx, int rely, int flags)
{
	if(m_selected_entity.index != -1)
	{
		if(operation == TransformOperation::ROTATE && m_transform_mode != TransformMode::CAMERA_XZ)
		{
			Vec3 pos = m_selected_entity.getPosition();
			Matrix emtx;
			m_selected_entity.getMatrix(emtx);
			Vec3 axis;
			switch(m_transform_mode)
			{
				case TransformMode::X:
					axis = emtx.getXVector();
					break;
				case TransformMode::Y:
					axis = emtx.getYVector();
					break;
				case TransformMode::Z:
					axis = emtx.getZVector();
					break;
			}
			float angle = 0;
			if(flags & Flags::FIXED_STEP)
			{
				m_relx_accum += relx;
				m_rely_accum += rely;
				if(m_relx_accum + m_rely_accum > 50)
				{
					angle = (float)Math::PI / 4;
					m_relx_accum = m_rely_accum = 0;
				}
				else if(m_relx_accum + m_rely_accum < -50)
				{
					angle = -(float)Math::PI / 4;
					m_relx_accum = m_rely_accum = 0;
				}
				else
				{
					angle = 0;
				}
			}
			else
			{
				angle = (relx + rely) / 100.0f;
			}
			m_selected_entity.setRotation(m_selected_entity.getRotation() * Quat(axis, angle));
			m_selected_entity.setPosition(pos);
		}
		else
		{
			Vec3 intersection = getMousePlaneIntersection(camera, x, y);
			Vec3 delta = intersection - m_transform_point;
			m_transform_point = intersection;
			Vec3 pos = m_selected_entity.getPosition();
			pos += delta;
			m_selected_entity.setPosition(pos);
		}
	}
}

Vec3 Gizmo::getMousePlaneIntersection(Component camera, int x, int y)
{
	Vec3 origin, dir;
	m_renderer->getRay(camera, (float)x, (float)y, origin, dir);
	dir.normalize();
	Matrix camera_mtx;
	camera.entity.getMatrix(camera_mtx);
	if(m_transform_mode == TransformMode::CAMERA_XZ)
	{
		Vec3 a = crossProduct(Vec3(0, 1, 0), camera_mtx.getXVector());
		Vec3 b = crossProduct(Vec3(0, 1, 0), camera_mtx.getZVector());
		Vec3 plane_normal = crossProduct(a, b);
		return Math::getRayPlaneIntersecion(origin, dir, m_selected_entity.getPosition(), plane_normal);
	}
	Vec3 axis;
	switch(m_transform_mode)
	{
		case TransformMode::X:
			axis = m_selected_entity.getMatrix().getXVector();
			break;
		case TransformMode::Y:
			axis = m_selected_entity.getMatrix().getYVector();
			break;
		case TransformMode::Z:
			axis = m_selected_entity.getMatrix().getZVector();
			break;
	}
	Vec3 pos = m_selected_entity.getPosition();
	Vec3 normal = crossProduct(crossProduct(dir, axis), dir);
	float d = dotProduct(origin - pos, normal) / dotProduct(axis, normal);
	return axis * d + pos;
}

} // !namespace Lux<|endoftext|>


<commit_before>// #include "ros/ros.h"
#include "std_msgs/String.h"
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sstream>
#include "rpc/RpcLibClient.hpp"
#include <iostream>
#include <chrono>
#include <math.h>
#include <iterator>
#include "common/Common.hpp"
#include <fstream>
#include "input_sampler.h"
#include "Callbacks/callbacks.h"

using namespace std;

sensor_msgs::CameraInfo getCameraParams(){
    double Tx, Fx, Fy, cx, cy, width, height;
    sensor_msgs::CameraInfo CameraParam;
    ros::param::get("/airsim_imgPublisher/Tx",Tx);
    ros::param::get("/airsim_imgPublisher/Fx",Fx);
    ros::param::get("/airsim_imgPublisher/Fy",Fy);
    ros::param::get("/airsim_imgPublisher/cx",cx);
    ros::param::get("/airsim_imgPublisher/cy",cy);
    ros::param::get("/airsim_imgPublisher/scale_x",width);
    ros::param::get("/airsim_imgPublisher/scale_y",height);

    CameraParam.header.frame_id = "camera";

    CameraParam.height = height;
    CameraParam.width = width;

    CameraParam.distortion_model = "plumb_bob";
    CameraParam.D = {0.0, 0.0, 0.0, 0.0, 0.0};

    CameraParam.K = {Fx,  0.0, cx,
                     0.0, Fy,  cy,
                     0.0, 0.0, 1};
    CameraParam.R = {1.0, 0.0, 0.0,
                     0.0, 1.0, 0.0,
                     0.0, 0.0, 1.0};
    CameraParam.P = {Fx,  0.0, cx,  Tx,
                     0.0, Fy,  cy,  0.0,
                     0.0, 0.0, 1.0, 0.0};

    CameraParam.binning_x = 0;
    CameraParam.binning_y = 0;

    return CameraParam;

}

void CameraPosePublisher(geometry_msgs::Pose CamPose){
  static tf::TransformBroadcaster br;
  tf::Transform transformQuad, transformCamera;
  double sqrt_2 = 1.41421356237;

  transformCamera.setOrigin(tf::Vector3(CamPose.position.y,
                                        CamPose.position.x,
                                        -CamPose.position.z));
   Eigen::Matrix3d R;
   // R <<  0.0,  0.0, 1.0,
   //      -1.0,  0.0, 0.0,
   //       0.0, -1.0, 0.0;
   // R <<  1.0,  0.0, 0.0,
   //       0.0,  1.0, 0.0,
   //       0.0,  0.0, 1.0;
   // R = rotx(-M_PI/2.0);

         //Function to get yaw from a quaternion
  double YawCam = getHeadingFromQuat(CamPose.orientation);

  // geometry_msgs::Quaternion q = rot2quat(R);
  geometry_msgs::Quaternion q_Yaw = setQuat(0.0,0.0,sin(-YawCam),cos(-YawCam));
  geometry_msgs::Quaternion q_body2cam = setQuat(-sqrt_2/2.0, 0, 0, sqrt_2/2.0);
  geometry_msgs::Quaternion q_cam = quatProd(q_Yaw,CamPose.orientation);
  q_cam = quatProd(q_body2cam,q_cam);
  transformCamera.setRotation(tf::Quaternion(q_cam.x,
                                             q_cam.y,
                                             q_cam.z,
                                             q_cam.w));

  br.sendTransform(tf::StampedTransform(transformCamera, ros::Time::now(), "fcu", "camera"));
}

/**
 * This tutorial demonstrates simple sending of messages over the ROS system.
 */
int main(int argc, char **argv)
{

  //Start ROS ----------------------------------------------------------------
  ros::init(argc, argv, "airsim_imgPublisher");
  ros::NodeHandle n;
  ros::Rate loop_rate(5);

  //Subscribers --------------------------------------------------------------
  std::string odomTopic;
  ros::param::get("/airsim_imgPublisher/odomTopic", odomTopic);
  ros::Subscriber tfSub = n.subscribe(odomTopic, 10, tfCallback); //Create tf tree

  //Publishers ---------------------------------------------------------------
  image_transport::ImageTransport it(n);
  image_transport::Publisher imgL_pub = it.advertise("/stereo/left/image_raw", 1);
  image_transport::Publisher imgR_pub = it.advertise("/stereo/right/image_raw", 1);
  image_transport::Publisher depth_pub = it.advertise("/Airsim/depth", 1);
  ros::Publisher imgParamL_pub = n.advertise<sensor_msgs::CameraInfo> ("/stereo/left/camera_info", 1);
  ros::Publisher imgParamR_pub = n.advertise<sensor_msgs::CameraInfo> ("/Airsim/right/camera_info", 1);
  ros::Publisher imgParamDepth_pub = n.advertise<sensor_msgs::CameraInfo> ("/Airsim/camera_info", 1);

  //ROS Messages
  sensor_msgs::ImagePtr msgImgL, msgImgR, msgDepth;
  sensor_msgs::CameraInfo msgCameraInfo;

	//Parameters for communicating with Airsim
  string ip_addr;
  int portParam;
  ros::param::get("/airsim_imgPublisher/Airsim_ip",ip_addr);
  ros::param::get("/airsim_imgPublisher/Airsim_port", portParam);
  uint16_t port = portParam;

  //Verbose
  ROS_INFO("Image publisher started! Connecting to:");
  ROS_INFO("IP: %s", ip_addr.c_str());
  ROS_INFO("Port: %d", port);

  //Local variables
  input_sampler input_sampler__obj(ip_addr.c_str(), port);
  cv::Mat img, imgDepth, imgDepthThr; //images to store the polled images
  const string display_name = "Drone View";
  msgCameraInfo = getCameraParams();
  float scale, maxDist;

  // *** F:DN end of communication with simulator (Airsim)

  while (ros::ok())
  {
    ros::param::get("/airsim_imgPublisher/scale",scale);
    auto imgs = input_sampler__obj.poll_frame(scale);

    //cv::imshow("left", imgs.left);
    //cv::imshow("right", imgs.right);
    // cv::imshow("depth", imgs.depth);

    // imgs.planar_depth.convertTo(imgs.planar_depth, CV_8UC1);
    // cv::imshow("planar_depth", imgs.planar_depth);

    // imgs.disparity.convertTo(imgs.disparity, CV_8UC1);
    // cv::imshow("disparity", imgs.disparity);

    // imgs.planar_depth.convertTo(imgDepth16,CV_16U,255);
    // cv::waitKey(10);

    //Saturate depth to maximum threshold
    ros::param::get("/airsim_imgPublisher/maxDist",maxDist);
    printf("Max dist: %f", maxDist);
    threshold(imgs.depth, imgDepthThr, maxDist,100, 4);

    // *** F:DN conversion of opencv images to ros images
    msgImgL = cv_bridge::CvImage(std_msgs::Header(), "bgr8", imgs.left).toImageMsg();
    msgImgR = cv_bridge::CvImage(std_msgs::Header(), "bgr8", imgs.right).toImageMsg();
    msgDepth = cv_bridge::CvImage(std_msgs::Header(), "32FC1", imgDepthThr).toImageMsg();

    //Stamp messages
    msgCameraInfo.header.stamp = ros::Time::now();
    msgImgL->header.stamp = msgCameraInfo.header.stamp;
    msgImgR->header.stamp = msgCameraInfo.header.stamp;
    msgDepth->header.stamp =  msgCameraInfo.header.stamp;

    //Publish transforms into tf tree
    CameraPosePublisher(imgs.pose);

    //Publish images
    ROS_INFO("New images arrived! Publishing...");
    imgL_pub.publish(msgImgL);
    imgR_pub.publish(msgImgR);
    depth_pub.publish(msgDepth);
    imgParamL_pub.publish(msgCameraInfo);
    imgParamR_pub.publish(msgCameraInfo);
    imgParamDepth_pub.publish(msgCameraInfo);

    ros::spinOnce();

    loop_rate.sleep();
  }

  cv::destroyAllWindows();

  return 0;
}
<commit_msg>Fixed issue where point-cloud would be filled in on the drone's own location.<commit_after>// #include "ros/ros.h"
#include "std_msgs/String.h"
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sstream>
#include "rpc/RpcLibClient.hpp"
#include <iostream>
#include <chrono>
#include <math.h>
#include <iterator>
#include "common/Common.hpp"
#include <fstream>
#include "input_sampler.h"
#include "Callbacks/callbacks.h"

using namespace std;

sensor_msgs::CameraInfo getCameraParams(){
    double Tx, Fx, Fy, cx, cy, width, height;
    sensor_msgs::CameraInfo CameraParam;
    ros::param::get("/airsim_imgPublisher/Tx",Tx);
    ros::param::get("/airsim_imgPublisher/Fx",Fx);
    ros::param::get("/airsim_imgPublisher/Fy",Fy);
    ros::param::get("/airsim_imgPublisher/cx",cx);
    ros::param::get("/airsim_imgPublisher/cy",cy);
    ros::param::get("/airsim_imgPublisher/scale_x",width);
    ros::param::get("/airsim_imgPublisher/scale_y",height);

    CameraParam.header.frame_id = "camera";

    CameraParam.height = height;
    CameraParam.width = width;

    CameraParam.distortion_model = "plumb_bob";
    CameraParam.D = {0.0, 0.0, 0.0, 0.0, 0.0};

    CameraParam.K = {Fx,  0.0, cx,
                     0.0, Fy,  cy,
                     0.0, 0.0, 1};
    CameraParam.R = {1.0, 0.0, 0.0,
                     0.0, 1.0, 0.0,
                     0.0, 0.0, 1.0};
    CameraParam.P = {Fx,  0.0, cx,  Tx,
                     0.0, Fy,  cy,  0.0,
                     0.0, 0.0, 1.0, 0.0};

    CameraParam.binning_x = 0;
    CameraParam.binning_y = 0;

    return CameraParam;

}

void CameraPosePublisher(geometry_msgs::Pose CamPose){
  static tf::TransformBroadcaster br;
  tf::Transform transformQuad, transformCamera;
  double sqrt_2 = 1.41421356237;

  transformCamera.setOrigin(tf::Vector3(CamPose.position.y,
                                        CamPose.position.x,
                                        -CamPose.position.z));
   Eigen::Matrix3d R;
   // R <<  0.0,  0.0, 1.0,
   //      -1.0,  0.0, 0.0,
   //       0.0, -1.0, 0.0;
   // R <<  1.0,  0.0, 0.0,
   //       0.0,  1.0, 0.0,
   //       0.0,  0.0, 1.0;
   // R = rotx(-M_PI/2.0);

         //Function to get yaw from a quaternion
  double YawCam = getHeadingFromQuat(CamPose.orientation);

  // geometry_msgs::Quaternion q = rot2quat(R);
  geometry_msgs::Quaternion q_Yaw = setQuat(0.0,0.0,sin(-YawCam),cos(-YawCam));
  geometry_msgs::Quaternion q_body2cam = setQuat(-sqrt_2/2.0, 0, 0, sqrt_2/2.0);
  geometry_msgs::Quaternion q_cam = quatProd(q_Yaw,CamPose.orientation);
  q_cam = quatProd(q_body2cam,q_cam);
  transformCamera.setRotation(tf::Quaternion(q_cam.x,
                                             q_cam.y,
                                             q_cam.z,
                                             q_cam.w));

  br.sendTransform(tf::StampedTransform(transformCamera, ros::Time::now(), "fcu", "camera"));
}

/**
 * This tutorial demonstrates simple sending of messages over the ROS system.
 */
int main(int argc, char **argv)
{

  //Start ROS ----------------------------------------------------------------
  ros::init(argc, argv, "airsim_imgPublisher");
  ros::NodeHandle n;
  ros::Rate loop_rate(5);

  //Subscribers --------------------------------------------------------------
  std::string odomTopic;
  ros::param::get("/airsim_imgPublisher/odomTopic", odomTopic);
  ros::Subscriber tfSub = n.subscribe(odomTopic, 10, tfCallback); //Create tf tree

  //Publishers ---------------------------------------------------------------
  image_transport::ImageTransport it(n);
  image_transport::Publisher imgL_pub = it.advertise("/stereo/left/image_raw", 1);
  image_transport::Publisher imgR_pub = it.advertise("/stereo/right/image_raw", 1);
  image_transport::Publisher depth_pub = it.advertise("/Airsim/depth", 1);
  ros::Publisher imgParamL_pub = n.advertise<sensor_msgs::CameraInfo> ("/stereo/left/camera_info", 1);
  ros::Publisher imgParamR_pub = n.advertise<sensor_msgs::CameraInfo> ("/Airsim/right/camera_info", 1);
  ros::Publisher imgParamDepth_pub = n.advertise<sensor_msgs::CameraInfo> ("/Airsim/camera_info", 1);

  //ROS Messages
  sensor_msgs::ImagePtr msgImgL, msgImgR, msgDepth;
  sensor_msgs::CameraInfo msgCameraInfo;

	//Parameters for communicating with Airsim
  string ip_addr;
  int portParam;
  ros::param::get("/airsim_imgPublisher/Airsim_ip",ip_addr);
  ros::param::get("/airsim_imgPublisher/Airsim_port", portParam);
  uint16_t port = portParam;

  //Verbose
  ROS_INFO("Image publisher started! Connecting to:");
  ROS_INFO("IP: %s", ip_addr.c_str());
  ROS_INFO("Port: %d", port);

  //Local variables
  input_sampler input_sampler__obj(ip_addr.c_str(), port);
  cv::Mat img, imgDepth, imgDepthThr, imgDepthBinaryThr; //images to store the polled images
  const string display_name = "Drone View";
  msgCameraInfo = getCameraParams();
  float scale, maxDist;

  // *** F:DN end of communication with simulator (Airsim)

  while (ros::ok())
  {
    ros::param::get("/airsim_imgPublisher/scale",scale);
    auto imgs = input_sampler__obj.poll_frame(scale);

    //cv::imshow("left", imgs.left);
    //cv::imshow("right", imgs.right);
    // cv::imshow("depth", imgs.depth);

    // imgs.planar_depth.convertTo(imgs.planar_depth, CV_8UC1);
    // cv::imshow("planar_depth", imgs.planar_depth);

    // imgs.disparity.convertTo(imgs.disparity, CV_8UC1);
    // cv::imshow("disparity", imgs.disparity);

    // imgs.planar_depth.convertTo(imgDepth16,CV_16U,255);
    // cv::waitKey(10);

    //Saturate depth to maximum threshold
    ros::param::get("/airsim_imgPublisher/maxDist",maxDist);
    printf("Max dist: %f", maxDist);
    cv::threshold(imgs.depth, imgDepthThr, maxDist, 0, cv::THRESH_TOZERO_INV);
    cv::threshold(imgs.depth, imgDepthBinaryThr, maxDist, numeric_limits<float>::infinity(), cv::THRESH_BINARY);
    cv::add(imgDepthThr, imgDepthBinaryThr, imgDepthThr);

    // *** F:DN conversion of opencv images to ros images
    msgImgL = cv_bridge::CvImage(std_msgs::Header(), "bgr8", imgs.left).toImageMsg();
    msgImgR = cv_bridge::CvImage(std_msgs::Header(), "bgr8", imgs.right).toImageMsg();
    msgDepth = cv_bridge::CvImage(std_msgs::Header(), "32FC1", imgDepthThr).toImageMsg();

    //Stamp messages
    msgCameraInfo.header.stamp = ros::Time::now();
    msgImgL->header.stamp = msgCameraInfo.header.stamp;
    msgImgR->header.stamp = msgCameraInfo.header.stamp;
    msgDepth->header.stamp =  msgCameraInfo.header.stamp;

    //Publish transforms into tf tree
    CameraPosePublisher(imgs.pose);

    //Publish images
    ROS_INFO("New images arrived! Publishing...");
    imgL_pub.publish(msgImgL);
    imgR_pub.publish(msgImgR);
    depth_pub.publish(msgDepth);
    imgParamL_pub.publish(msgCameraInfo);
    imgParamR_pub.publish(msgCameraInfo);
    imgParamDepth_pub.publish(msgCameraInfo);

    ros::spinOnce();

    loop_rate.sleep();
  }

  cv::destroyAllWindows();

  return 0;
}
<|endoftext|>