<commit_before>/*
    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

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

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

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

#include "ch.h"
#include "hal.h"

#include "analog.h"
#include "encoder.h"
#include "encoderfoaw.h"
#include "simulation.pb.h"
#include "messageutil.h"
#include "filter/movingaverage.h"

#include "blink.h"
#include "saconfig.h"
#include "utility.h"
#include "sautility.h"

#include "parameters.h"

#include <array>
#include <type_traits>

#include "haptic.h"
#include "simbicycle.h"
#include "transmitter.h"

#if defined(USE_BICYCLE_KINEMATIC_MODEL)
#include "bicycle/kinematic.h" // simplified bicycle model
#else // No Whipple model simplifications
#include "bicycle/whipple.h"
#endif

namespace {
#if defined(USE_BICYCLE_KINEMATIC_MODEL)
    using model_t = model::BicycleKinematic;
    using haptic_drive_t = haptic::Handlebar0;
#else
    using model_t = model::BicycleWhipple;
#endif
    using bicycle_t = sim::Bicycle<model_t>;

    // sensors
    Analog<10> analog; // per channel buffer depth of 10

    Encoder encoder_steer(sa::RLS_ROLIN_ENC, sa::RLS_ROLIN_ENC_INDEX_CFG);
    EncoderFoaw<float, 32> encoder_rear_wheel(sa::RLS_GTS35_ENC,
                                              sa::RLS_GTS35_ENC_CFG,
                                              MS2ST(1),
                                              3.0f);
#if defined(USE_BICYCLE_KINEMATIC_MODEL)
    filter::MovingAverage<float, 5> velocity_filter;
#else
    constexpr float fixed_v = 5.0f; // fixed bicycle velocity
#endif

    // pose calculation loop
    constexpr systime_t pose_loop_period = US2ST(8333); // update pose at 120 Hz

    // dynamics loop
    constexpr systime_t dynamics_loop_period = MS2ST(1); // 1 ms -> 1 kHz
    constexpr float dt = static_cast<float>(dynamics_loop_period)/CH_CFG_ST_FREQUENCY;
    static_assert(dt > 0, "'dt' must be greater than zero. \
Verify 'dynamics_loop_period is greater than 1 ms.");

    // feedback and feedforward parameters
    constexpr float k_p = 20.0f;
    constexpr float k_d = 2.0f;
    constexpr float m = sa::ASSEMBLY_INERTIA;

    // Suspends the invoking thread until the system time arrives to the
    // specified value.
    // The system time is assumed to be between prev and time else the call is
    // assumed to have been called outside the allowed time interval, in this
    // case the thread yields the time slot and no sleep is performed.
    systime_t chThdSleepUntilWindowedOrYield(systime_t prev, systime_t next) {
        systime_t time;

        chSysLock();
        time = chVTGetSystemTimeX();
        if (chVTIsTimeWithinX(time, prev, next)) {
            chThdSleepS(next - time);
        } else {
            chSchDoYieldS();
        }
        chSysUnlock();

        return next;
    }

    struct pose_thread_arg {
        bicycle_t& bicycle;
        message::Transmitter& transmitter;
    };
    THD_WORKING_AREA(wa_pose_thread, 2048);
    THD_FUNCTION(pose_thread, arg) {
        pose_thread_arg* a = static_cast<pose_thread_arg*>(arg);

        chRegSetThreadName("pose");
        systime_t deadline = chVTGetSystemTime();
        while (true) {
            a->bicycle.update_kinematics();
            BicyclePoseMessage* msg = a->transmitter.alloc_pose_message();
            if (msg != nullptr) {
                *msg = a->bicycle.pose();
                if (a->transmitter.transmit_async(msg) != MSG_OK) {
                    // Discard pose message if transmission fails. As the
                    // receiver requires the most recent pose, we should not
                    // wait to queue a pose message.
                    a->transmitter.free_message(msg);
                }
            }

            deadline = chThdSleepUntilWindowedOrYield(deadline, deadline + pose_loop_period);
        }
    }

    void initialize_sensors_actuators() {
        // Start sensors.
        // Encoder:
        //   Initialize encoder driver 5 on pins PA0, PA1 (EXT2-4, EXT2-8).
        //   Pins for encoder driver 3 are already set in board.h.
        palSetLineMode(LINE_TIM5_CH1, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
        palSetLineMode(LINE_TIM5_CH2, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
        encoder_steer.start();
        encoder_rear_wheel.start();
        analog.start(10000); // trigger ADC conversion at 10 kHz

        //Set torque measurement enable line low.
        //The output of the Kistler torque sensor is not valid until after a falling edge
        //on the measurement line and it is held low. The 'LINE_TORQUE_MEAS_EN' line is
        //reversed due to NPN switch Q1.
        palClearLine(LINE_TORQUE_MEAS_EN);
        chThdSleepMilliseconds(1);
        palSetLine(LINE_TORQUE_MEAS_EN);

        // Start DAC1 driver and set output pin as analog as suggested in Reference Manual.
        // The default line configuration is OUTPUT_OPENDRAIN_PULLUP for SPI1_ENC1_NSS
        // and must be changed to use as analog output.
        palSetLineMode(LINE_KOLLM_ACTL_TORQUE, PAL_MODE_INPUT_ANALOG);
        dacStart(sa::KOLLM_DAC, sa::KOLLM_DAC_CFG);
        sa::set_kollmorgen_torque(0.0f);
    }
} // namespace

int main(void) {
    // System initializations.
    // - HAL initialization, this also initializes the configured device drivers
    //   and performs the board-specific initializations.
    // - Kernel initialization, the main() function becomes a thread and the
    //   RTOS is active.
    halInit();
    chSysInit();

    // Create LED blink thread
    chBlinkThreadCreateStatic();

    initialize_sensors_actuators();

    // Initialize bicycle.
    bicycle_t bicycle(
#if defined(USE_BICYCLE_KINEMATIC_MODEL)
            0.0f,
#else
            fixed_v,
#endif
            static_cast<model::real_t>(dt));

#if defined(USE_BICYCLE_KINEMATIC_MODEL)
    // Initialize handlebar object to calculate motor drive feedback torque
    haptic_drive_t haptic_drive(bicycle.model());
#endif

    // Initialize handlebar object to estimate torque due to handlebar inertia.
    haptic::Handlebar2 handlebar_inertia(bicycle.model(), sa::UPPER_ASSEMBLY_INERTIA);

    // Initialize time measurements
    time_measurement_t computation_time_measurement;
    time_measurement_t transmission_time_measurement;
    chTMObjectInit(&computation_time_measurement);
    chTMObjectInit(&transmission_time_measurement);

    // Initialize USB data transmission
    message::Transmitter transmitter;
    {   // Transmit initial message containing gitsha1 and model data.
        SimulationMessage* msg = transmitter.alloc_simulation_message();
        *msg = SimulationMessage_init_zero;
        msg->timestamp = chVTGetSystemTime();
        message::set_simulation_full_model(msg, bicycle);

        msg->controller.feedback.k_p = k_p;
        msg->controller.feedback.has_k_p = true;
        msg->controller.feedback.k_d = k_d;
        msg->controller.feedback.has_k_d = true;
        msg->controller.has_feedback = true;

        msg->controller.feedforward.inertia = m;
        msg->controller.feedforward.has_inertia = true;
        msg->controller.has_feedforward = true;

        msg->has_controller = true;

        transmitter.transmit(msg); // This blocks until USB data starts getting read
    }
    transmitter.start(NORMALPRIO + 1); // start transmission thread

    // Start running pose calculation thread
    pose_thread_arg a{bicycle, transmitter};
    chThdCreateStatic(wa_pose_thread, sizeof(wa_pose_thread),
            NORMALPRIO - 1, pose_thread, static_cast<void*>(&a));

    // Normal main() thread activity. This is the dynamics simulation loop.
    systime_t deadline = chVTGetSystemTime();
    float last_error = 0.0f;
    while (true) {
        systime_t starttime = chVTGetSystemTime();
        chTMStartMeasurementX(&computation_time_measurement);
        constexpr float roll_torque = 0.0f;

        // positive motor torque will rotate the steering shaft clockwise
        // positive kistler torque equal to positive motor torque will stop rotation
        // m1 * xdd = T_delta + T_s
        // m2 * xdd = -T_s + T_a
        // m1: upper mass
        // m2: lower mass
        // T_delta: rider applied steer torque
        // T_s: kistler torque (sensor measurement)
        // T_a: motor torque (actuator command)
        //
        // m1/m2*(-T_s + T_a) = T_delta + T_s
        // T_delta = -T_s + m1/m2*(-T_s + T_a)
        //
        // NOTE: we neglect inertia torque after finding it to be negligible
        // compared to the sensor torque
        const float steer_torque = -sa::get_kistler_sensor_torque(analog);

#if defined(USE_BICYCLE_KINEMATIC_MODEL)
        const float v = velocity_filter.output(
                -sa::REAR_WHEEL_RADIUS*(util::encoder_rate(encoder_rear_wheel)));
        bicycle.set_v(v);

        const float yaw_angle = util::wrap(bicycle.pose().yaw); // use previous state
        const float steer_angle = util::encoder_count<float>(encoder_steer);
        //const float rear_wheel_angle = std::fmod(-util::encoder_count<float>(encoder_rear_wheel),
        //                                         constants::two_pi);

        bicycle.update_dynamics(
                roll_torque,
                steer_torque,
                yaw_angle,
                steer_angle,
                0);

        const float desired_torque = haptic_drive.torque(
                model_t::get_state_part(bicycle.full_state()));
        const dacsample_t handlebar_reference_dac =
            sa::set_kollmorgen_torque(desired_torque);
#else // defined(USE_BICYCLE_KINEMATIC_MODEL)
        constexpr float v = fixed_v;
        bicycle.set_v(v);

        // Measurements are ignored in state update. Instead, state is stored
        // in sim::bicycle object.
        bicycle.update_dynamics(
                roll_torque,
                steer_torque,
                0,  // measurement ignored
                0,  // measurement ignored
                0); // measurement ignored

        const float desired_position = model_t::get_full_state_element(
                bicycle.full_state(),
                model_t::full_state_index_t::steer_angle);

        const float steer_angle = util::encoder_count<float>(encoder_steer);
        const float error = desired_position - steer_angle;

        const float error_derivative = (error - last_error)/dt;
        last_error = error;

        const float feedback_torque = k_p*error + k_d*error_derivative;

        const model_t& model = bicycle.model();
        const model_t::state_t state_deriv =
            model.A()*model_t::get_state_part(bicycle.full_state()) +
            model.B()*bicycle.input();
        const float steer_accel = model_t::get_state_element(
                state_deriv,
                model_t::state_index_t::steer_rate);
        const float feedforward_torque = m*steer_accel;

        const dacsample_t handlebar_reference_dac =
            sa::set_kollmorgen_torque(feedback_torque + feedforward_torque);
#endif // defined(USE_BICYCLE_KINEMATIC_MODEL)
        chTMStopMeasurementX(&computation_time_measurement);

        {   // prepare message for transmission
            SimulationMessage* msg = transmitter.alloc_simulation_message();
            if (msg != nullptr) {
                *msg = SimulationMessage_init_zero;
                msg->timestamp = starttime;
                msg->model.v = bicycle.v();
                msg->model.has_v = true;
                msg->has_model = true;
                message::set_bicycle_input(
                        &msg->input,
                        (model_t::input_t() << roll_torque, steer_torque).finished());
                msg->has_input = true;
                message::set_simulation_state(msg, bicycle);
                message::set_simulation_auxiliary_state(msg, bicycle);
                message::set_simulation_actuators(msg, handlebar_reference_dac);
                // The sensor values are sampled again at a slightly different time
                // but should be roughly the same is the ones used in computation
                message::set_simulation_sensors(msg,
                        analog.get_kistler_sensor(),
                        analog.get_kollmorgen_motor(),
                        encoder_steer.count(),
                        encoder_rear_wheel.count());
                message::set_simulation_timing(msg,
                        computation_time_measurement.last,
                        transmission_time_measurement.last);
#if not defined(USE_BICYCLE_KINEMATIC_MODEL)
                message::set_simulation_feedback(msg,
                        feedback_torque,
                        error,
                        error_derivative);
                message::set_simulation_feedforward(msg,
                        feedforward_torque,
                        steer_accel);
#endif
                if (transmitter.transmit_async(msg) != MSG_OK) {
                    // Discard simulation message if it cannot be processed quickly enough.
                    transmitter.free_message(msg);
                }
            }
        }
        deadline = chThdSleepUntilWindowedOrYield(
                deadline,
                deadline + dynamics_loop_period);
    }
}
<commit_msg>Use existing bicycle input when logging<commit_after>/*
    ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio

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

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

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

#include "ch.h"
#include "hal.h"

#include "analog.h"
#include "encoder.h"
#include "encoderfoaw.h"
#include "simulation.pb.h"
#include "messageutil.h"
#include "filter/movingaverage.h"

#include "blink.h"
#include "saconfig.h"
#include "utility.h"
#include "sautility.h"

#include "parameters.h"

#include <array>
#include <type_traits>

#include "haptic.h"
#include "simbicycle.h"
#include "transmitter.h"

#if defined(USE_BICYCLE_KINEMATIC_MODEL)
#include "bicycle/kinematic.h" // simplified bicycle model
#else // No Whipple model simplifications
#include "bicycle/whipple.h"
#endif

namespace {
#if defined(USE_BICYCLE_KINEMATIC_MODEL)
    using model_t = model::BicycleKinematic;
    using haptic_drive_t = haptic::Handlebar0;
#else
    using model_t = model::BicycleWhipple;
#endif
    using bicycle_t = sim::Bicycle<model_t>;

    // sensors
    Analog<10> analog; // per channel buffer depth of 10

    Encoder encoder_steer(sa::RLS_ROLIN_ENC, sa::RLS_ROLIN_ENC_INDEX_CFG);
    EncoderFoaw<float, 32> encoder_rear_wheel(sa::RLS_GTS35_ENC,
                                              sa::RLS_GTS35_ENC_CFG,
                                              MS2ST(1),
                                              3.0f);
#if defined(USE_BICYCLE_KINEMATIC_MODEL)
    filter::MovingAverage<float, 5> velocity_filter;
#else
    constexpr float fixed_v = 5.0f; // fixed bicycle velocity
#endif

    // pose calculation loop
    constexpr systime_t pose_loop_period = US2ST(8333); // update pose at 120 Hz

    // dynamics loop
    constexpr systime_t dynamics_loop_period = MS2ST(1); // 1 ms -> 1 kHz
    constexpr float dt = static_cast<float>(dynamics_loop_period)/CH_CFG_ST_FREQUENCY;
    static_assert(dt > 0, "'dt' must be greater than zero. \
Verify 'dynamics_loop_period is greater than 1 ms.");

    // feedback and feedforward parameters
    constexpr float k_p = 20.0f;
    constexpr float k_d = 2.0f;
    constexpr float m = sa::ASSEMBLY_INERTIA;

    // Suspends the invoking thread until the system time arrives to the
    // specified value.
    // The system time is assumed to be between prev and time else the call is
    // assumed to have been called outside the allowed time interval, in this
    // case the thread yields the time slot and no sleep is performed.
    systime_t chThdSleepUntilWindowedOrYield(systime_t prev, systime_t next) {
        systime_t time;

        chSysLock();
        time = chVTGetSystemTimeX();
        if (chVTIsTimeWithinX(time, prev, next)) {
            chThdSleepS(next - time);
        } else {
            chSchDoYieldS();
        }
        chSysUnlock();

        return next;
    }

    struct pose_thread_arg {
        bicycle_t& bicycle;
        message::Transmitter& transmitter;
    };
    THD_WORKING_AREA(wa_pose_thread, 2048);
    THD_FUNCTION(pose_thread, arg) {
        pose_thread_arg* a = static_cast<pose_thread_arg*>(arg);

        chRegSetThreadName("pose");
        systime_t deadline = chVTGetSystemTime();
        while (true) {
            a->bicycle.update_kinematics();
            BicyclePoseMessage* msg = a->transmitter.alloc_pose_message();
            if (msg != nullptr) {
                *msg = a->bicycle.pose();
                if (a->transmitter.transmit_async(msg) != MSG_OK) {
                    // Discard pose message if transmission fails. As the
                    // receiver requires the most recent pose, we should not
                    // wait to queue a pose message.
                    a->transmitter.free_message(msg);
                }
            }

            deadline = chThdSleepUntilWindowedOrYield(deadline, deadline + pose_loop_period);
        }
    }

    void initialize_sensors_actuators() {
        // Start sensors.
        // Encoder:
        //   Initialize encoder driver 5 on pins PA0, PA1 (EXT2-4, EXT2-8).
        //   Pins for encoder driver 3 are already set in board.h.
        palSetLineMode(LINE_TIM5_CH1, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
        palSetLineMode(LINE_TIM5_CH2, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUPDR_FLOATING);
        encoder_steer.start();
        encoder_rear_wheel.start();
        analog.start(10000); // trigger ADC conversion at 10 kHz

        //Set torque measurement enable line low.
        //The output of the Kistler torque sensor is not valid until after a falling edge
        //on the measurement line and it is held low. The 'LINE_TORQUE_MEAS_EN' line is
        //reversed due to NPN switch Q1.
        palClearLine(LINE_TORQUE_MEAS_EN);
        chThdSleepMilliseconds(1);
        palSetLine(LINE_TORQUE_MEAS_EN);

        // Start DAC1 driver and set output pin as analog as suggested in Reference Manual.
        // The default line configuration is OUTPUT_OPENDRAIN_PULLUP for SPI1_ENC1_NSS
        // and must be changed to use as analog output.
        palSetLineMode(LINE_KOLLM_ACTL_TORQUE, PAL_MODE_INPUT_ANALOG);
        dacStart(sa::KOLLM_DAC, sa::KOLLM_DAC_CFG);
        sa::set_kollmorgen_torque(0.0f);
    }
} // namespace

int main(void) {
    // System initializations.
    // - HAL initialization, this also initializes the configured device drivers
    //   and performs the board-specific initializations.
    // - Kernel initialization, the main() function becomes a thread and the
    //   RTOS is active.
    halInit();
    chSysInit();

    // Create LED blink thread
    chBlinkThreadCreateStatic();

    initialize_sensors_actuators();

    // Initialize bicycle.
    bicycle_t bicycle(
#if defined(USE_BICYCLE_KINEMATIC_MODEL)
            0.0f,
#else
            fixed_v,
#endif
            static_cast<model::real_t>(dt));

#if defined(USE_BICYCLE_KINEMATIC_MODEL)
    // Initialize handlebar object to calculate motor drive feedback torque
    haptic_drive_t haptic_drive(bicycle.model());
#endif

    // Initialize handlebar object to estimate torque due to handlebar inertia.
    haptic::Handlebar2 handlebar_inertia(bicycle.model(), sa::UPPER_ASSEMBLY_INERTIA);

    // Initialize time measurements
    time_measurement_t computation_time_measurement;
    time_measurement_t transmission_time_measurement;
    chTMObjectInit(&computation_time_measurement);
    chTMObjectInit(&transmission_time_measurement);

    // Initialize USB data transmission
    message::Transmitter transmitter;
    {   // Transmit initial message containing gitsha1 and model data.
        SimulationMessage* msg = transmitter.alloc_simulation_message();
        *msg = SimulationMessage_init_zero;
        msg->timestamp = chVTGetSystemTime();
        message::set_simulation_full_model(msg, bicycle);

        msg->controller.feedback.k_p = k_p;
        msg->controller.feedback.has_k_p = true;
        msg->controller.feedback.k_d = k_d;
        msg->controller.feedback.has_k_d = true;
        msg->controller.has_feedback = true;

        msg->controller.feedforward.inertia = m;
        msg->controller.feedforward.has_inertia = true;
        msg->controller.has_feedforward = true;

        msg->has_controller = true;

        transmitter.transmit(msg); // This blocks until USB data starts getting read
    }
    transmitter.start(NORMALPRIO + 1); // start transmission thread

    // Start running pose calculation thread
    pose_thread_arg a{bicycle, transmitter};
    chThdCreateStatic(wa_pose_thread, sizeof(wa_pose_thread),
            NORMALPRIO - 1, pose_thread, static_cast<void*>(&a));

    // Normal main() thread activity. This is the dynamics simulation loop.
    systime_t deadline = chVTGetSystemTime();
    float last_error = 0.0f;
    while (true) {
        systime_t starttime = chVTGetSystemTime();
        chTMStartMeasurementX(&computation_time_measurement);
        constexpr float roll_torque = 0.0f;

        // positive motor torque will rotate the steering shaft clockwise
        // positive kistler torque equal to positive motor torque will stop rotation
        // m1 * xdd = T_delta + T_s
        // m2 * xdd = -T_s + T_a
        // m1: upper mass
        // m2: lower mass
        // T_delta: rider applied steer torque
        // T_s: kistler torque (sensor measurement)
        // T_a: motor torque (actuator command)
        //
        // m1/m2*(-T_s + T_a) = T_delta + T_s
        // T_delta = -T_s + m1/m2*(-T_s + T_a)
        //
        // NOTE: we neglect inertia torque after finding it to be negligible
        // compared to the sensor torque
        const float steer_torque = -sa::get_kistler_sensor_torque(analog);

#if defined(USE_BICYCLE_KINEMATIC_MODEL)
        const float v = velocity_filter.output(
                -sa::REAR_WHEEL_RADIUS*(util::encoder_rate(encoder_rear_wheel)));
        bicycle.set_v(v);

        const float yaw_angle = util::wrap(bicycle.pose().yaw); // use previous state
        const float steer_angle = util::encoder_count<float>(encoder_steer);
        //const float rear_wheel_angle = std::fmod(-util::encoder_count<float>(encoder_rear_wheel),
        //                                         constants::two_pi);

        bicycle.update_dynamics(
                roll_torque,
                steer_torque,
                yaw_angle,
                steer_angle,
                0);

        const float desired_torque = haptic_drive.torque(
                model_t::get_state_part(bicycle.full_state()));
        const dacsample_t handlebar_reference_dac =
            sa::set_kollmorgen_torque(desired_torque);
#else // defined(USE_BICYCLE_KINEMATIC_MODEL)
        constexpr float v = fixed_v;
        bicycle.set_v(v);

        // Measurements are ignored in state update. Instead, state is stored
        // in sim::bicycle object.
        bicycle.update_dynamics(
                roll_torque,
                steer_torque,
                0,  // measurement ignored
                0,  // measurement ignored
                0); // measurement ignored

        const float desired_position = model_t::get_full_state_element(
                bicycle.full_state(),
                model_t::full_state_index_t::steer_angle);

        const float steer_angle = util::encoder_count<float>(encoder_steer);
        const float error = desired_position - steer_angle;

        const float error_derivative = (error - last_error)/dt;
        last_error = error;

        const float feedback_torque = k_p*error + k_d*error_derivative;

        const model_t& model = bicycle.model();
        const model_t::state_t state_deriv =
            model.A()*model_t::get_state_part(bicycle.full_state()) +
            model.B()*bicycle.input();
        const float steer_accel = model_t::get_state_element(
                state_deriv,
                model_t::state_index_t::steer_rate);
        const float feedforward_torque = m*steer_accel;

        const dacsample_t handlebar_reference_dac =
            sa::set_kollmorgen_torque(feedback_torque + feedforward_torque);
#endif // defined(USE_BICYCLE_KINEMATIC_MODEL)
        chTMStopMeasurementX(&computation_time_measurement);

        {   // prepare message for transmission
            SimulationMessage* msg = transmitter.alloc_simulation_message();
            if (msg != nullptr) {
                *msg = SimulationMessage_init_zero;
                msg->timestamp = starttime;
                msg->model.v = bicycle.v();
                msg->model.has_v = true;
                msg->has_model = true;
                message::set_bicycle_input(&msg->input, bicycle.input());
                msg->has_input = true;
                message::set_simulation_state(msg, bicycle);
                message::set_simulation_auxiliary_state(msg, bicycle);
                message::set_simulation_actuators(msg, handlebar_reference_dac);
                // The sensor values are sampled again at a slightly different time
                // but should be roughly the same is the ones used in computation
                message::set_simulation_sensors(msg,
                        analog.get_kistler_sensor(),
                        analog.get_kollmorgen_motor(),
                        encoder_steer.count(),
                        encoder_rear_wheel.count());
                message::set_simulation_timing(msg,
                        computation_time_measurement.last,
                        transmission_time_measurement.last);
#if not defined(USE_BICYCLE_KINEMATIC_MODEL)
                message::set_simulation_feedback(msg,
                        feedback_torque,
                        error,
                        error_derivative);
                message::set_simulation_feedforward(msg,
                        feedforward_torque,
                        steer_accel);
#endif
                if (transmitter.transmit_async(msg) != MSG_OK) {
                    // Discard simulation message if it cannot be processed quickly enough.
                    transmitter.free_message(msg);
                }
            }
        }
        deadline = chThdSleepUntilWindowedOrYield(
                deadline,
                deadline + dynamics_loop_period);
    }
}
<|endoftext|>


<commit_before>/* -*- mode: c++; c-basic-offset:4 -*-
    refreshkeyscommand.cpp

    This file is part of Kleopatra, the KDE keymanager
    Copyright (c) 2007 Klarälvdalens Datakonsult AB

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

    Kleopatra 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
    General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA

    In addition, as a special exception, the copyright holders give
    permission to link the code of this program with any edition of
    the Qt library by Trolltech AS, Norway (or with modified versions
    of Qt that use the same license as Qt), and distribute linked
    combinations including the two.  You must obey the GNU General
    Public License in all respects for all of the code used other than
    Qt.  If you modify this file, you may extend this exception to
    your version of the file, but you are not obligated to do so.  If
    you do not wish to do so, delete this exception statement from
    your version.
*/

#include "refreshkeyscommand.h"
#include "command_p.h"

#include <models/keycache.h>
#include <models/predicates.h>

#include <utils/stl_util.h>

#include <gpgme++/key.h>
#include <gpgme++/keylistresult.h>

#include <kleo/cryptobackendfactory.h>
#include <kleo/keylistjob.h>

#include <QStringList>

#include <boost/bind.hpp>

#include <deque>
#include <vector>
#include <algorithm>
#include <cassert>

using namespace Kleo;
using namespace boost;
using namespace GpgME;

class RefreshKeysCommand::Private : public Command::Private {
    friend class ::Kleo::RefreshKeysCommand;
public:
    Private( RefreshKeysCommand * qq, Mode mode, KeyListController* controller );
    ~Private();

    enum KeyType {
        PublicKeys,
        SecretKeys
    };
    void startKeyListing( const char* backend, KeyType type );

    void publicKeyListingDone( const KeyListResult & result );
    void secretKeyListingDone( const KeyListResult & result );

    void addKey( const Key & key );

    void mergeKeysAndUpdateKeyCache();

private:
    const Mode mode;
    uint m_pubKeysJobs;
    uint m_secKeysJobs;

    std::deque<Key> m_pubkeys, m_seckeys;
};

RefreshKeysCommand::Private * RefreshKeysCommand::d_func() { return static_cast<Private*>( d.get() ); }
const RefreshKeysCommand::Private * RefreshKeysCommand::d_func() const { return static_cast<const Private*>( d.get() ); }

RefreshKeysCommand::RefreshKeysCommand( Mode mode, KeyListController * p )
    : Command( new Private( this, mode, p ) )
{

}

RefreshKeysCommand::RefreshKeysCommand( Mode mode, QAbstractItemView * v, KeyListController * p )
    : Command( v, new Private( this, mode, p ) )
{

}

RefreshKeysCommand::~RefreshKeysCommand() {}

RefreshKeysCommand::Private::Private( RefreshKeysCommand * qq, Mode m, KeyListController * controller )
    : Command::Private( qq, controller ),
      mode( m ),
      m_pubKeysJobs( 0 ),
      m_secKeysJobs( 0 ),
      m_pubkeys(),
      m_seckeys()
{

}

RefreshKeysCommand::Private::~Private() {}

void RefreshKeysCommand::Private::startKeyListing( const char* backend, KeyType type )
{
    const Kleo::CryptoBackend::Protocol * const protocol = Kleo::CryptoBackendFactory::instance()->protocol( backend );
    if ( !protocol )
        return;
    Kleo::KeyListJob * const job = protocol->keyListJob( /*remote*/false, /*includeSigs*/false, mode == Validate );
    if ( !job )
        return;
    if ( type == PublicKeys ) {
        connect( job, SIGNAL(result(GpgME::KeyListResult)),
                 q, SLOT(publicKeyListingDone(GpgME::KeyListResult)) );
    } else {
        connect( job, SIGNAL(result(GpgME::KeyListResult)),
                 q, SLOT(secretKeyListingDone(GpgME::KeyListResult)) );
    }
    connect( job, SIGNAL(progress(QString,int,int)),
             q, SIGNAL(progress(QString,int,int)) );
    connect( job, SIGNAL(nextKey(GpgME::Key)),
             q, SLOT(addKey(GpgME::Key)) );
    connect( q, SIGNAL(canceled()),
             job, SLOT(slotCancel()) );
    job->start( QStringList(), type == SecretKeys );
    ++( type == PublicKeys ? m_pubKeysJobs : m_secKeysJobs );
}

void RefreshKeysCommand::Private::publicKeyListingDone( const KeyListResult & result )
{
    assert( m_pubKeysJobs > 0 );
    --m_pubKeysJobs;
    if ( result.error().isCanceled() )
        finished();
    else if ( m_pubKeysJobs == 0 ) {
        startKeyListing( "openpgp", Private::SecretKeys );
        startKeyListing( "smime", Private::SecretKeys );
    }
}

void RefreshKeysCommand::Private::secretKeyListingDone( const KeyListResult & result )
{
    assert( m_secKeysJobs > 0 );
    --m_secKeysJobs;
    if ( result.error().isCanceled() || m_secKeysJobs == 0 )
        finished();
    if ( m_secKeysJobs == 0 )
        mergeKeysAndUpdateKeyCache();
}

namespace {
    template <typename ForwardIterator, typename BinaryPredicate>
    ForwardIterator unique_by_merge( ForwardIterator first, ForwardIterator last, BinaryPredicate pred ) {
        first = std::adjacent_find( first, last, pred );
        if ( first == last )
            return last;

        ForwardIterator dest = first;
        dest->merge( *++first );
        while ( ++first != last )
            if ( pred( *dest, *first ) )
                dest->merge( *first );
            else
                *++dest = *first;
        return ++dest;
    }
}

void RefreshKeysCommand::Private::mergeKeysAndUpdateKeyCache()
{
    std::sort( m_pubkeys.begin(), m_pubkeys.end(), _detail::ByFingerprint<std::less>() );
    std::sort( m_seckeys.begin(), m_seckeys.end(), _detail::ByFingerprint<std::less>() );

    std::vector<Key> keys;
    keys.reserve( m_pubkeys.size() + m_seckeys.size() );

    std::merge( m_pubkeys.begin(), m_pubkeys.end(),
                m_seckeys.begin(), m_seckeys.end(),
                std::back_inserter( keys ),
                _detail::ByFingerprint<std::less>() );

    keys.erase( unique_by_merge( keys.begin(), keys.end(), _detail::ByFingerprint<std::equal_to>() ),
                keys.end() );

    std::vector<Key> sec;
    sec.reserve( m_seckeys.size() );
    kdtools::copy_if( keys.begin(), keys.end(), std::back_inserter( sec ),
                      bind( &Key::hasSecret, _1 ) );

    PublicKeyCache::mutableInstance()->insert( keys );
    SecretKeyCache::mutableInstance()->insert( sec );
}

void RefreshKeysCommand::Private::addKey( const Key & key )
{
    if ( key.hasSecret() )
        m_seckeys.push_back( key );
    else
        m_seckeys.push_back( key );
}

#define d d_func()

void RefreshKeysCommand::doStart() {
    /* NOTE: first fetch public keys. when done, fetch secret keys. hasSecret() works only
       correctly when the key was retrieved with --list-secret-keys (secretOnly flag
       in gpgme keylist operations) so we overwrite the key from --list-keys (secret
       not set) with the one from --list-secret-keys (with secret set).
    */
    d->startKeyListing( "openpgp", Private::PublicKeys );
    d->startKeyListing( "smime", Private::PublicKeys );
}

void RefreshKeysCommand::doCancel() {
    // empty implementation, as canceled(), emitted from
    // Command::cancel(), is connected to Kleo::Job::slotCanceled()
    // for all our jobs.
}

#undef d

#include "moc_refreshkeyscommand.cpp"
<commit_msg>GpgME::Key::merge -> mergeWith, for consistency with KeyListResult::mergeWith<commit_after>/* -*- mode: c++; c-basic-offset:4 -*-
    refreshkeyscommand.cpp

    This file is part of Kleopatra, the KDE keymanager
    Copyright (c) 2007 Klarälvdalens Datakonsult AB

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

    Kleopatra 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
    General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA

    In addition, as a special exception, the copyright holders give
    permission to link the code of this program with any edition of
    the Qt library by Trolltech AS, Norway (or with modified versions
    of Qt that use the same license as Qt), and distribute linked
    combinations including the two.  You must obey the GNU General
    Public License in all respects for all of the code used other than
    Qt.  If you modify this file, you may extend this exception to
    your version of the file, but you are not obligated to do so.  If
    you do not wish to do so, delete this exception statement from
    your version.
*/

#include "refreshkeyscommand.h"
#include "command_p.h"

#include <models/keycache.h>
#include <models/predicates.h>

#include <utils/stl_util.h>

#include <gpgme++/key.h>
#include <gpgme++/keylistresult.h>

#include <kleo/cryptobackendfactory.h>
#include <kleo/keylistjob.h>

#include <QStringList>

#include <boost/bind.hpp>

#include <deque>
#include <vector>
#include <algorithm>
#include <cassert>

using namespace Kleo;
using namespace boost;
using namespace GpgME;

class RefreshKeysCommand::Private : public Command::Private {
    friend class ::Kleo::RefreshKeysCommand;
public:
    Private( RefreshKeysCommand * qq, Mode mode, KeyListController* controller );
    ~Private();

    enum KeyType {
        PublicKeys,
        SecretKeys
    };
    void startKeyListing( const char* backend, KeyType type );

    void publicKeyListingDone( const KeyListResult & result );
    void secretKeyListingDone( const KeyListResult & result );

    void addKey( const Key & key );

    void mergeKeysAndUpdateKeyCache();

private:
    const Mode mode;
    uint m_pubKeysJobs;
    uint m_secKeysJobs;

    std::deque<Key> m_pubkeys, m_seckeys;
};

RefreshKeysCommand::Private * RefreshKeysCommand::d_func() { return static_cast<Private*>( d.get() ); }
const RefreshKeysCommand::Private * RefreshKeysCommand::d_func() const { return static_cast<const Private*>( d.get() ); }

RefreshKeysCommand::RefreshKeysCommand( Mode mode, KeyListController * p )
    : Command( new Private( this, mode, p ) )
{

}

RefreshKeysCommand::RefreshKeysCommand( Mode mode, QAbstractItemView * v, KeyListController * p )
    : Command( v, new Private( this, mode, p ) )
{

}

RefreshKeysCommand::~RefreshKeysCommand() {}

RefreshKeysCommand::Private::Private( RefreshKeysCommand * qq, Mode m, KeyListController * controller )
    : Command::Private( qq, controller ),
      mode( m ),
      m_pubKeysJobs( 0 ),
      m_secKeysJobs( 0 ),
      m_pubkeys(),
      m_seckeys()
{

}

RefreshKeysCommand::Private::~Private() {}

void RefreshKeysCommand::Private::startKeyListing( const char* backend, KeyType type )
{
    const Kleo::CryptoBackend::Protocol * const protocol = Kleo::CryptoBackendFactory::instance()->protocol( backend );
    if ( !protocol )
        return;
    Kleo::KeyListJob * const job = protocol->keyListJob( /*remote*/false, /*includeSigs*/false, mode == Validate );
    if ( !job )
        return;
    if ( type == PublicKeys ) {
        connect( job, SIGNAL(result(GpgME::KeyListResult)),
                 q, SLOT(publicKeyListingDone(GpgME::KeyListResult)) );
    } else {
        connect( job, SIGNAL(result(GpgME::KeyListResult)),
                 q, SLOT(secretKeyListingDone(GpgME::KeyListResult)) );
    }
    connect( job, SIGNAL(progress(QString,int,int)),
             q, SIGNAL(progress(QString,int,int)) );
    connect( job, SIGNAL(nextKey(GpgME::Key)),
             q, SLOT(addKey(GpgME::Key)) );
    connect( q, SIGNAL(canceled()),
             job, SLOT(slotCancel()) );
    job->start( QStringList(), type == SecretKeys );
    ++( type == PublicKeys ? m_pubKeysJobs : m_secKeysJobs );
}

void RefreshKeysCommand::Private::publicKeyListingDone( const KeyListResult & result )
{
    assert( m_pubKeysJobs > 0 );
    --m_pubKeysJobs;
    if ( result.error().isCanceled() )
        finished();
    else if ( m_pubKeysJobs == 0 ) {
        startKeyListing( "openpgp", Private::SecretKeys );
        startKeyListing( "smime", Private::SecretKeys );
    }
}

void RefreshKeysCommand::Private::secretKeyListingDone( const KeyListResult & result )
{
    assert( m_secKeysJobs > 0 );
    --m_secKeysJobs;
    if ( result.error().isCanceled() || m_secKeysJobs == 0 )
        finished();
    if ( m_secKeysJobs == 0 )
        mergeKeysAndUpdateKeyCache();
}

namespace {
    template <typename ForwardIterator, typename BinaryPredicate>
    ForwardIterator unique_by_merge( ForwardIterator first, ForwardIterator last, BinaryPredicate pred ) {
        first = std::adjacent_find( first, last, pred );
        if ( first == last )
            return last;

        ForwardIterator dest = first;
        dest->mergeWith( *++first );
        while ( ++first != last )
            if ( pred( *dest, *first ) )
                dest->mergeWith( *first );
            else
                *++dest = *first;
        return ++dest;
    }
}

void RefreshKeysCommand::Private::mergeKeysAndUpdateKeyCache()
{
    std::sort( m_pubkeys.begin(), m_pubkeys.end(), _detail::ByFingerprint<std::less>() );
    std::sort( m_seckeys.begin(), m_seckeys.end(), _detail::ByFingerprint<std::less>() );

    std::vector<Key> keys;
    keys.reserve( m_pubkeys.size() + m_seckeys.size() );

    std::merge( m_pubkeys.begin(), m_pubkeys.end(),
                m_seckeys.begin(), m_seckeys.end(),
                std::back_inserter( keys ),
                _detail::ByFingerprint<std::less>() );

    keys.erase( unique_by_merge( keys.begin(), keys.end(), _detail::ByFingerprint<std::equal_to>() ),
                keys.end() );

    std::vector<Key> sec;
    sec.reserve( m_seckeys.size() );
    kdtools::copy_if( keys.begin(), keys.end(), std::back_inserter( sec ),
                      bind( &Key::hasSecret, _1 ) );

    PublicKeyCache::mutableInstance()->insert( keys );
    SecretKeyCache::mutableInstance()->insert( sec );
}

void RefreshKeysCommand::Private::addKey( const Key & key )
{
    if ( key.hasSecret() )
        m_seckeys.push_back( key );
    else
        m_seckeys.push_back( key );
}

#define d d_func()

void RefreshKeysCommand::doStart() {
    /* NOTE: first fetch public keys. when done, fetch secret keys. hasSecret() works only
       correctly when the key was retrieved with --list-secret-keys (secretOnly flag
       in gpgme keylist operations) so we overwrite the key from --list-keys (secret
       not set) with the one from --list-secret-keys (with secret set).
    */
    d->startKeyListing( "openpgp", Private::PublicKeys );
    d->startKeyListing( "smime", Private::PublicKeys );
}

void RefreshKeysCommand::doCancel() {
    // empty implementation, as canceled(), emitted from
    // Command::cancel(), is connected to Kleo::Job::slotCanceled()
    // for all our jobs.
}

#undef d

#include "moc_refreshkeyscommand.cpp"
<|endoftext|>


<commit_before>// Copyright 2016-2020 TriAxis Games L.L.C. All Rights Reserved.

#include "RuntimeMeshComponent.h"
#include "RuntimeMeshComponentPlugin.h"
#include "PhysicsEngine/BodySetup.h"
#include "PhysicsEngine/PhysicsSettings.h"
#include "IPhysXCookingModule.h"
#include "RuntimeMeshCore.h"
#include "RuntimeMeshRenderable.h"
#include "RuntimeMeshSectionProxy.h"
#include "RuntimeMesh.h"
#include "RuntimeMeshComponentProxy.h"
#include "NavigationSystem.h"

DECLARE_CYCLE_STAT(TEXT("RuntimeMeshComponent - Collision Data Received"), STAT_RuntimeMeshComponent_NewCollisionMeshReceived, STATGROUP_RuntimeMesh);
DECLARE_CYCLE_STAT(TEXT("RuntimeMeshComponent - Create Scene Proxy"), STAT_RuntimeMeshComponent_CreateSceneProxy, STATGROUP_RuntimeMesh);

URuntimeMeshComponent::URuntimeMeshComponent()
{
	SetNetAddressable();

	Super::SetTickGroup(TG_DuringPhysics);
	Super::SetTickableWhenPaused(true);
}

void URuntimeMeshComponent::EnsureHasRuntimeMesh()
{
	if (RuntimeMeshReference == nullptr)
	{
		SetRuntimeMesh(NewObject<URuntimeMesh>(this));
	}
}

void URuntimeMeshComponent::SetRuntimeMesh(URuntimeMesh* NewMesh)
{
	// Unlink from any existing runtime mesh
	if (RuntimeMeshReference)
	{
		RuntimeMeshReference->UnRegisterLinkedComponent(this);
		RuntimeMeshReference = nullptr;
	}

	if (NewMesh)
	{
		RuntimeMeshReference = NewMesh;
		RuntimeMeshReference->RegisterLinkedComponent(this);
	}

	MarkRenderStateDirty();
}

FRuntimeMeshCollisionHitInfo URuntimeMeshComponent::GetHitSource(int32 FaceIndex) const
{
	URuntimeMesh* Mesh = GetRuntimeMesh();
	if (Mesh)
	{
		FRuntimeMeshCollisionHitInfo HitInfo = Mesh->GetHitSource(FaceIndex);
		if (HitInfo.SectionId >= 0)
		{
			HitInfo.Material = GetMaterial(HitInfo.SectionId);
		}
		return HitInfo;
	}
	return FRuntimeMeshCollisionHitInfo();
}

void URuntimeMeshComponent::NewCollisionMeshReceived()
{
	SCOPE_CYCLE_COUNTER(STAT_RuntimeMeshComponent_NewCollisionMeshReceived);

	// First recreate the physics state
	RecreatePhysicsState();

 	// Now update the navigation.
	FNavigationSystem::UpdateComponentData(*this);
}

void URuntimeMeshComponent::NewBoundsReceived()
{
	UpdateBounds();
	if (bRenderStateCreated)
	{
		MarkRenderTransformDirty();
	}
}

void URuntimeMeshComponent::ForceProxyRecreate()
{
	check(IsInGameThread());
	if (bRenderStateCreated)
	{
		MarkRenderStateDirty();
	}
}

FBoxSphereBounds URuntimeMeshComponent::CalcBounds(const FTransform& LocalToWorld) const
{
	if (GetRuntimeMesh())
	{
		FBoxSphereBounds TempBounds = GetRuntimeMesh()->GetLocalBounds();
		return TempBounds.TransformBy(LocalToWorld);
	}

	return FBoxSphereBounds(FSphere(FVector::ZeroVector, 1));
}

FPrimitiveSceneProxy* URuntimeMeshComponent::CreateSceneProxy()
{
	SCOPE_CYCLE_COUNTER(STAT_RuntimeMeshComponent_CreateSceneProxy);

	if (RuntimeMeshReference == nullptr || !RuntimeMeshReference->GetRenderProxy(GetScene()->GetFeatureLevel()).IsValid())
	{
		return nullptr;
	}

	return new FRuntimeMeshComponentSceneProxy(this);
}

UBodySetup* URuntimeMeshComponent::GetBodySetup()
{
	if (GetRuntimeMesh())
	{
		return GetRuntimeMesh()->BodySetup;
	}

	return nullptr;
}

int32 URuntimeMeshComponent::GetMaterialIndex(FName MaterialSlotName) const
{
	if (URuntimeMesh* Mesh = GetRuntimeMesh())
	{
		return Mesh->GetMaterialIndex(MaterialSlotName);
	}
	return INDEX_NONE;
}

TArray<FName> URuntimeMeshComponent::GetMaterialSlotNames() const
{
	if (URuntimeMesh* Mesh = GetRuntimeMesh())
	{
		return Mesh->GetMaterialSlotNames();
	}
	return TArray<FName>();
}

bool URuntimeMeshComponent::IsMaterialSlotNameValid(FName MaterialSlotName) const
{
	if (URuntimeMesh* Mesh = GetRuntimeMesh())
	{
		return Mesh->IsMaterialSlotNameValid(MaterialSlotName);
	}
	return false;
}

void URuntimeMeshComponent::GetUsedMaterials(TArray<UMaterialInterface*>& OutMaterials, bool bGetDebugMaterials /*= false*/) const
{
	TArray<FRuntimeMeshMaterialSlot> Slots = GetMaterialSlots();

	for (int32 Index = 0; Index < Slots.Num(); Index++)
	{
		UMaterialInterface* Mat = Super::GetMaterial(Index);

		if (Mat == nullptr)
		{
			Mat = Slots[Index].Material;
		}

		if (Mat)
		{
			OutMaterials.Add(Mat);
		}
	}
}

int32 URuntimeMeshComponent::GetNumMaterials() const
{
	int32 RuntimeMeshSections = GetRuntimeMesh() != nullptr ? GetRuntimeMesh()->GetNumMaterials() : 0;

	return FMath::Max(Super::GetNumMaterials(), RuntimeMeshSections);
}

UMaterialInterface* URuntimeMeshComponent::GetMaterial(int32 ElementIndex) const
{
	UMaterialInterface* Mat = Super::GetMaterial(ElementIndex);

	// Use default override material system
	if (Mat != nullptr)
		return Mat;

	// fallback to RM sections material
	if (URuntimeMesh* Mesh = GetRuntimeMesh())
	{
		return Mesh->GetMaterial(ElementIndex);
	}

	// Had no RM/Section return null
	return nullptr;
}

void URuntimeMeshComponent::PostLoad()
{
	Super::PostLoad();
}

void URuntimeMeshComponent::BeginDestroy()
{
	Super::BeginDestroy();
}

void URuntimeMeshComponent::OnRegister()
{
	Super::OnRegister();

	if (RuntimeMeshReference)
	{
		RuntimeMeshReference->RegisterLinkedComponent(this);
	}
}

void URuntimeMeshComponent::OnUnregister()
{
	if (RuntimeMeshReference)
	{
		RuntimeMeshReference->UnRegisterLinkedComponent(this);
		RuntimeMeshReference = nullptr;
	}

	Super::OnUnregister();
}<commit_msg>Restores velocities after recreating physics state<commit_after>// Copyright 2016-2020 TriAxis Games L.L.C. All Rights Reserved.

#include "RuntimeMeshComponent.h"
#include "RuntimeMeshComponentPlugin.h"
#include "PhysicsEngine/BodySetup.h"
#include "PhysicsEngine/PhysicsSettings.h"
#include "IPhysXCookingModule.h"
#include "RuntimeMeshCore.h"
#include "RuntimeMeshRenderable.h"
#include "RuntimeMeshSectionProxy.h"
#include "RuntimeMesh.h"
#include "RuntimeMeshComponentProxy.h"
#include "NavigationSystem.h"

DECLARE_CYCLE_STAT(TEXT("RuntimeMeshComponent - Collision Data Received"), STAT_RuntimeMeshComponent_NewCollisionMeshReceived, STATGROUP_RuntimeMesh);
DECLARE_CYCLE_STAT(TEXT("RuntimeMeshComponent - Create Scene Proxy"), STAT_RuntimeMeshComponent_CreateSceneProxy, STATGROUP_RuntimeMesh);

URuntimeMeshComponent::URuntimeMeshComponent()
{
	SetNetAddressable();

	Super::SetTickGroup(TG_DuringPhysics);
	Super::SetTickableWhenPaused(true);
}

void URuntimeMeshComponent::EnsureHasRuntimeMesh()
{
	if (RuntimeMeshReference == nullptr)
	{
		SetRuntimeMesh(NewObject<URuntimeMesh>(this));
	}
}

void URuntimeMeshComponent::SetRuntimeMesh(URuntimeMesh* NewMesh)
{
	// Unlink from any existing runtime mesh
	if (RuntimeMeshReference)
	{
		RuntimeMeshReference->UnRegisterLinkedComponent(this);
		RuntimeMeshReference = nullptr;
	}

	if (NewMesh)
	{
		RuntimeMeshReference = NewMesh;
		RuntimeMeshReference->RegisterLinkedComponent(this);
	}

	MarkRenderStateDirty();
}

FRuntimeMeshCollisionHitInfo URuntimeMeshComponent::GetHitSource(int32 FaceIndex) const
{
	URuntimeMesh* Mesh = GetRuntimeMesh();
	if (Mesh)
	{
		FRuntimeMeshCollisionHitInfo HitInfo = Mesh->GetHitSource(FaceIndex);
		if (HitInfo.SectionId >= 0)
		{
			HitInfo.Material = GetMaterial(HitInfo.SectionId);
		}
		return HitInfo;
	}
	return FRuntimeMeshCollisionHitInfo();
}

void URuntimeMeshComponent::NewCollisionMeshReceived()
{
	SCOPE_CYCLE_COUNTER(STAT_RuntimeMeshComponent_NewCollisionMeshReceived);

	// First Store Velocities
	FVector PrevLinearVelocity = GetPhysicsLinearVelocity();
	FVector PrevAngularVelocity = GetPhysicsAngularVelocityInDegrees();

	// Recreate the physics state
	RecreatePhysicsState();

	// Apply Velocities
	SetPhysicsLinearVelocity(PrevLinearVelocity, false);
	SetPhysicsAngularVelocityInDegrees(PrevAngularVelocity, false);

 	// Now update the navigation.
	FNavigationSystem::UpdateComponentData(*this);
}

void URuntimeMeshComponent::NewBoundsReceived()
{
	UpdateBounds();
	if (bRenderStateCreated)
	{
		MarkRenderTransformDirty();
	}
}

void URuntimeMeshComponent::ForceProxyRecreate()
{
	check(IsInGameThread());
	if (bRenderStateCreated)
	{
		MarkRenderStateDirty();
	}
}

FBoxSphereBounds URuntimeMeshComponent::CalcBounds(const FTransform& LocalToWorld) const
{
	if (GetRuntimeMesh())
	{
		FBoxSphereBounds TempBounds = GetRuntimeMesh()->GetLocalBounds();
		return TempBounds.TransformBy(LocalToWorld);
	}

	return FBoxSphereBounds(FSphere(FVector::ZeroVector, 1));
}

FPrimitiveSceneProxy* URuntimeMeshComponent::CreateSceneProxy()
{
	SCOPE_CYCLE_COUNTER(STAT_RuntimeMeshComponent_CreateSceneProxy);

	if (RuntimeMeshReference == nullptr || !RuntimeMeshReference->GetRenderProxy(GetScene()->GetFeatureLevel()).IsValid())
	{
		return nullptr;
	}

	return new FRuntimeMeshComponentSceneProxy(this);
}

UBodySetup* URuntimeMeshComponent::GetBodySetup()
{
	if (GetRuntimeMesh())
	{
		return GetRuntimeMesh()->BodySetup;
	}

	return nullptr;
}

int32 URuntimeMeshComponent::GetMaterialIndex(FName MaterialSlotName) const
{
	if (URuntimeMesh* Mesh = GetRuntimeMesh())
	{
		return Mesh->GetMaterialIndex(MaterialSlotName);
	}
	return INDEX_NONE;
}

TArray<FName> URuntimeMeshComponent::GetMaterialSlotNames() const
{
	if (URuntimeMesh* Mesh = GetRuntimeMesh())
	{
		return Mesh->GetMaterialSlotNames();
	}
	return TArray<FName>();
}

bool URuntimeMeshComponent::IsMaterialSlotNameValid(FName MaterialSlotName) const
{
	if (URuntimeMesh* Mesh = GetRuntimeMesh())
	{
		return Mesh->IsMaterialSlotNameValid(MaterialSlotName);
	}
	return false;
}

void URuntimeMeshComponent::GetUsedMaterials(TArray<UMaterialInterface*>& OutMaterials, bool bGetDebugMaterials /*= false*/) const
{
	TArray<FRuntimeMeshMaterialSlot> Slots = GetMaterialSlots();

	for (int32 Index = 0; Index < Slots.Num(); Index++)
	{
		UMaterialInterface* Mat = Super::GetMaterial(Index);

		if (Mat == nullptr)
		{
			Mat = Slots[Index].Material;
		}

		if (Mat)
		{
			OutMaterials.Add(Mat);
		}
	}
}

int32 URuntimeMeshComponent::GetNumMaterials() const
{
	int32 RuntimeMeshSections = GetRuntimeMesh() != nullptr ? GetRuntimeMesh()->GetNumMaterials() : 0;

	return FMath::Max(Super::GetNumMaterials(), RuntimeMeshSections);
}

UMaterialInterface* URuntimeMeshComponent::GetMaterial(int32 ElementIndex) const
{
	UMaterialInterface* Mat = Super::GetMaterial(ElementIndex);

	// Use default override material system
	if (Mat != nullptr)
		return Mat;

	// fallback to RM sections material
	if (URuntimeMesh* Mesh = GetRuntimeMesh())
	{
		return Mesh->GetMaterial(ElementIndex);
	}

	// Had no RM/Section return null
	return nullptr;
}

void URuntimeMeshComponent::PostLoad()
{
	Super::PostLoad();
}

void URuntimeMeshComponent::BeginDestroy()
{
	Super::BeginDestroy();
}

void URuntimeMeshComponent::OnRegister()
{
	Super::OnRegister();

	if (RuntimeMeshReference)
	{
		RuntimeMeshReference->RegisterLinkedComponent(this);
	}
}

void URuntimeMeshComponent::OnUnregister()
{
	if (RuntimeMeshReference)
	{
		RuntimeMeshReference->UnRegisterLinkedComponent(this);
		RuntimeMeshReference = nullptr;
	}

	Super::OnUnregister();
}
<|endoftext|>


<commit_before>//=======================================================================
// Copyright (c) 2015-2016 Baptiste Wicht
// Distributed under the terms of the MIT License.
// (See accompanying file LICENSE or copy at
//  http://opensource.org/licenses/MIT)
//=======================================================================

namespace asgard {

const std::size_t UNIX_PATH_MAX = 108;
const std::size_t buffer_size = 4096;

struct KeyValue {
    std::string key;
    std::string value;
};

inline void load_config(std::vector<KeyValue>& config) {
    std::ifstream file("/etc/asgard/conf");
    std::string str;
    std::size_t i = 0;

    while (std::getline(file, str)) {
        std::istringstream line(str);
        config.push_back(KeyValue());
        if (std::getline(line, config[i].key, '=')) {
            if (std::getline(line, config[i].value)) {
                i++;
            }
        }
    }
}

inline std::string get_string_value(std::vector<KeyValue>& config, const std::string& key) {
    for (std::size_t i = 0; i < config.size(); ++i) {
        if (config[i].key == key) {
            return config[i].value;
        }
    }
    std::cout << "Key not found !" << std::endl;
    return "";
}

inline int get_int_value(std::vector<KeyValue>& config, const std::string& key) {
    for (std::size_t i = 0; i < config.size(); ++i) {
        if (config[i].key == key) {
            return std::stoi(config[i].value);
        }
    }
    std::cout << "Key not found !" << std::endl;
    return -1;
}

} //end of namespace asgard
<commit_msg>Change get_string_value() format<commit_after>//=======================================================================
// Copyright (c) 2015-2016 Baptiste Wicht
// Distributed under the terms of the MIT License.
// (See accompanying file LICENSE or copy at
//  http://opensource.org/licenses/MIT)
//=======================================================================

namespace asgard {

const std::size_t UNIX_PATH_MAX = 108;
const std::size_t buffer_size = 4096;

struct KeyValue {
    std::string key;
    std::string value;
};

inline void load_config(std::vector<KeyValue>& config) {
    std::ifstream file("/etc/asgard/conf");
    std::string str;
    std::size_t i = 0;

    while (std::getline(file, str)) {
        std::istringstream line(str);
        config.push_back(KeyValue());
        if (std::getline(line, config[i].key, '=')) {
            if (std::getline(line, config[i].value)) {
                i++;
            }
        }
    }
}

inline const char* get_string_value(std::vector<KeyValue>& config, const std::string& key) {
    for (std::size_t i = 0; i < config.size(); ++i) {
        if (config[i].key == key) {
            return config[i].value.c_str();
        }
    }
    std::cout << "Key not found !" << std::endl;
    return "";
}

inline int get_int_value(std::vector<KeyValue>& config, const std::string& key) {
    for (std::size_t i = 0; i < config.size(); ++i) {
        if (config[i].key == key) {
            return std::stoi(config[i].value);
        }
    }
    std::cout << "Key not found !" << std::endl;
    return -1;
}

} //end of namespace asgard
<|endoftext|>


<commit_before>// Copyright blackw 2015-2020

#include "UnrealSandboxTerrainPrivatePCH.h"
#include "TerrainZoneComponent.h"
#include "SandboxTerrainController.h"

#include "DrawDebugHelpers.h"

UTerrainZoneComponent::UTerrainZoneComponent(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer) {

}

void UTerrainZoneComponent::makeTerrain() {
	if (voxel_data == NULL) {
		voxel_data = GetTerrainController()->GetTerrainVoxelDataByPos(GetComponentLocation());
	}

	if (voxel_data == NULL) {
		return;
	}

	std::shared_ptr<MeshData> md_ptr = generateMesh();

	if (IsInGameThread()) {
		applyTerrainMesh(md_ptr);
		voxel_data->resetLastMeshRegenerationTime();
	} else {
		UE_LOG(LogTemp, Warning, TEXT("non-game thread -> invoke async task"));
		if (GetTerrainController() != NULL) {
			GetTerrainController()->invokeZoneMeshAsync(this, md_ptr);
		}
	}
}

std::shared_ptr<MeshData> UTerrainZoneComponent::generateMesh() {
	double start = FPlatformTime::Seconds();

	if (voxel_data->getDensityFillState() == VoxelDataFillState::ZERO || voxel_data->getDensityFillState() == VoxelDataFillState::ALL) {
		return NULL;
	}

	bool enableLOD = GetTerrainController()->bEnableLOD;

	VoxelDataParam vdp;

	if (enableLOD) {
		vdp.bGenerateLOD = true;
		vdp.collisionLOD = 1;
	} else {
		vdp.bGenerateLOD = false;
		vdp.collisionLOD = 0;
	}

	MeshDataPtr md_ptr = sandboxVoxelGenerateMesh(*voxel_data, vdp);

	double end = FPlatformTime::Seconds();
	double time = (end - start) * 1000;

	//UE_LOG(LogTemp, Warning, TEXT("ASandboxTerrainZone::generateMesh -------------> %f %f %f --> %f ms"), GetComponentLocation().X, GetComponentLocation().Y, GetComponentLocation().Z, time);

	return md_ptr;
}

void UTerrainZoneComponent::applyTerrainMesh(std::shared_ptr<MeshData> mesh_data_ptr) {
	double start = FPlatformTime::Seconds();

	MeshData* mesh_data = mesh_data_ptr.get();

	if (mesh_data == NULL) {
		return;
	}

	//##########################################
	// draw debug points
	//##########################################
	/*
	MeshLodSection& section6 = mesh_data->MeshSectionLodArray[4];
	for (auto p : section6.DebugPointList) {
		DrawDebugPoint(GetWorld(), p, 5, FColor(0, 0, 255, 100), false, 1000000);
		UE_LOG(LogTemp, Warning, TEXT("DebugPointList ---> %f %f %f "), p.X, p.Y, p.Z);
	}
	*/
	//##########################################

	MainTerrainMesh->SetMobility(EComponentMobility::Movable);

	MainTerrainMesh->AddLocalRotation(FRotator(0.0f, 0.01, 0.0f));  // workaround
	MainTerrainMesh->AddLocalRotation(FRotator(0.0f, -0.01, 0.0f)); // workaround

	MainTerrainMesh->bLodFlag = GetTerrainController()->bEnableLOD;

	MainTerrainMesh->SetMeshData(mesh_data_ptr);

	MainTerrainMesh->SetMobility(EComponentMobility::Stationary);

	MainTerrainMesh->SetCastShadow(true);
	MainTerrainMesh->bCastHiddenShadow = true;
	MainTerrainMesh->SetMaterial(0, GetTerrainController()->TerrainMaterial);
	MainTerrainMesh->SetVisibility(true);

	CollisionMesh->SetMeshData(mesh_data_ptr);
	CollisionMesh->SetCollisionProfileName(TEXT("BlockAll"));

	double end = FPlatformTime::Seconds();
	double time = (end - start) * 1000;
	//UE_LOG(LogTemp, Warning, TEXT("ASandboxTerrainZone::applyTerrainMesh ---------> %f %f %f --> %f ms"), GetComponentLocation().X, GetComponentLocation().Y, GetComponentLocation().Z, time);

	if (voxel_data->isNewGenerated()) {
		voxel_data->DataState = VoxelDataState::NORMAL;
		GetTerrainController()->OnGenerateNewZone(this);
	}

	if (voxel_data->isNewLoaded()) {
		voxel_data->DataState = VoxelDataState::NORMAL;
		GetTerrainController()->OnLoadZone(this);
	}
}

void UTerrainZoneComponent::SaveInstancedMeshesToFile() {
	FString SavePath = FPaths::GameSavedDir();
	FVector Index = GetTerrainController()->getZoneIndex(GetComponentLocation());

	int tx = Index.X;
	int ty = Index.Y;
	int tz = Index.Z;

	FString FileName = SavePath + TEXT("/Map/") + GetTerrainController()->MapName + TEXT("/zone_inst_mesh.") + FString::FromInt(tx) + TEXT(".") + FString::FromInt(ty) + TEXT(".") + FString::FromInt(tz) + TEXT(".dat");

	FBufferArchive BinaryData;

	SerializeInstancedMeshes(BinaryData);

	if (FFileHelper::SaveArrayToFile(BinaryData, *FileName)) {
		BinaryData.FlushCache();
		BinaryData.Empty();
	}
}

void UTerrainZoneComponent::LoadInstancedMeshesFromFile() {
	FString SavePath = FPaths::GameSavedDir();
	FVector Index = GetTerrainController()->getZoneIndex(GetComponentLocation());

	int tx = Index.X;
	int ty = Index.Y;
	int tz = Index.Z;

	FString FileName = SavePath + TEXT("/Map/") + GetTerrainController()->MapName + TEXT("/zone_inst_mesh.") + FString::FromInt(tx) + TEXT(".") + FString::FromInt(ty) + TEXT(".") + FString::FromInt(tz) + TEXT(".dat");

	TArray<uint8> BinaryArray;
	if (!FFileHelper::LoadFileToArray(BinaryArray, *FileName)) {
		UE_LOG(LogTemp, Warning, TEXT("file not found -> %s"), *FileName);
		return;
	}

	if (BinaryArray.Num() <= 0) return;

	FMemoryReader BinaryData = FMemoryReader(BinaryArray, true); //true, free data after done
	BinaryData.Seek(0);

	// ==============================
	int32 MeshCount;
	int32 MeshTypeId;
	int32 MeshInstanceCount;

	BinaryData << MeshCount;
	BinaryData << MeshTypeId;
	BinaryData << MeshInstanceCount;

	UE_LOG(LogTemp, Warning, TEXT("MeshCount -> %d"), MeshCount);
	UE_LOG(LogTemp, Warning, TEXT("MeshTypeId -> %d"), MeshTypeId);
	UE_LOG(LogTemp, Warning, TEXT("MeshInstanceCount -> %d"), MeshInstanceCount);

	FTerrainInstancedMeshType MeshType;
	if (GetTerrainController()->FoliageMap.Contains(MeshTypeId)) {
		MeshType.Mesh = GetTerrainController()->FoliageMap[MeshTypeId].Mesh;
	}

	for (int32 InstanceIdx = 0; InstanceIdx < MeshInstanceCount; InstanceIdx++) {
		float X;
		float Y;
		float Z;

		float Roll;
		float Pitch;
		float Yaw;

		float ScaleX;
		float ScaleY;
		float ScaleZ;

		BinaryData << X;
		BinaryData << Y;
		BinaryData << Z;

		BinaryData << Roll;
		BinaryData << Pitch;
		BinaryData << Yaw;

		BinaryData << ScaleX;
		BinaryData << ScaleY;
		BinaryData << ScaleZ;

		FRotator Rotator(Pitch, Yaw, Roll);
		FTransform Transform(Rotator, FVector(X, Y, Z), FVector(ScaleX, ScaleY, ScaleZ));

		if (MeshType.Mesh != nullptr) {
			SpawnInstancedMesh(MeshType, Transform);
		}
	}

	// ==============================

	BinaryData.FlushCache();
	BinaryArray.Empty();
	BinaryData.Close();
}

void UTerrainZoneComponent::SerializeInstancedMeshes(FBufferArchive& BinaryData) {
	if (InstancedMeshMap.Num() == 0) {
		return;
	}

	int32 MeshCount = InstancedMeshMap.Num();
	BinaryData << MeshCount;

	for (auto& Elem : InstancedMeshMap) {
		UHierarchicalInstancedStaticMeshComponent* InstancedStaticMeshComponent = Elem.Value;
		int32 MeshTypeId = Elem.Key;

		int32 MeshInstanceCount = InstancedStaticMeshComponent->GetInstanceCount();

		BinaryData << MeshTypeId;
		BinaryData << MeshInstanceCount;

		for (int32 InstanceIdx = 0; InstanceIdx < MeshInstanceCount; InstanceIdx++) {
			FTransform InstanceTransform;
			InstancedStaticMeshComponent->GetInstanceTransform(InstanceIdx, InstanceTransform, true);

			float X = InstanceTransform.GetLocation().X;
			float Y = InstanceTransform.GetLocation().Y;
			float Z = InstanceTransform.GetLocation().Z;

			float Roll = InstanceTransform.Rotator().Roll;
			float Pitch = InstanceTransform.Rotator().Pitch;
			float Yaw = InstanceTransform.Rotator().Yaw;

			float ScaleX = InstanceTransform.GetScale3D().X;
			float ScaleY = InstanceTransform.GetScale3D().Y;
			float ScaleZ = InstanceTransform.GetScale3D().Z;

			BinaryData << X;
			BinaryData << Y;
			BinaryData << Z;

			BinaryData << Roll;
			BinaryData << Pitch;
			BinaryData << Yaw;

			BinaryData << ScaleX;
			BinaryData << ScaleY;
			BinaryData << ScaleZ;
		}
	}
}

void UTerrainZoneComponent::SpawnInstancedMesh(FTerrainInstancedMeshType& MeshType, FTransform& Transform) {
	UHierarchicalInstancedStaticMeshComponent* InstancedStaticMeshComponent = nullptr;

	if (InstancedMeshMap.Contains(MeshType.MeshTypeId)) {
		InstancedStaticMeshComponent = InstancedMeshMap[MeshType.MeshTypeId];
	}

	if (InstancedStaticMeshComponent == nullptr) {
		FString InstancedStaticMeshCompName = FString::Printf(TEXT("InstancedStaticMesh -%d -> [%.0f, %.0f, %.0f]"), MeshType.MeshTypeId, GetComponentLocation().X, GetComponentLocation().Y, GetComponentLocation().Z);

		InstancedStaticMeshComponent = NewObject<UHierarchicalInstancedStaticMeshComponent>(this, FName(*InstancedStaticMeshCompName));

		InstancedStaticMeshComponent->RegisterComponent();
		InstancedStaticMeshComponent->AttachTo(this);
		InstancedStaticMeshComponent->SetStaticMesh(MeshType.Mesh);
		InstancedStaticMeshComponent->SetCullDistances(100, 500);
		InstancedStaticMeshComponent->SetMobility(EComponentMobility::Static);
		InstancedStaticMeshComponent->SetSimulatePhysics(false);

		//InstancedStaticMeshComponent->SetCollisionEnabled(ECollisionEnabled::NoCollision);

		InstancedStaticMeshComponent->SetCollisionEnabled(ECollisionEnabled::QueryOnly);
		InstancedStaticMeshComponent->SetCollisionProfileName(TEXT("OverlapAll"));

		InstancedMeshMap.Add(MeshType.MeshTypeId, InstancedStaticMeshComponent);
	}

	InstancedStaticMeshComponent->AddInstanceWorldSpace(Transform);
}<commit_msg>load instance mesh file with multiple mesh types<commit_after>// Copyright blackw 2015-2020

#include "UnrealSandboxTerrainPrivatePCH.h"
#include "TerrainZoneComponent.h"
#include "SandboxTerrainController.h"

#include "DrawDebugHelpers.h"

UTerrainZoneComponent::UTerrainZoneComponent(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer) {

}

void UTerrainZoneComponent::makeTerrain() {
	if (voxel_data == NULL) {
		voxel_data = GetTerrainController()->GetTerrainVoxelDataByPos(GetComponentLocation());
	}

	if (voxel_data == NULL) {
		return;
	}

	std::shared_ptr<MeshData> md_ptr = generateMesh();

	if (IsInGameThread()) {
		applyTerrainMesh(md_ptr);
		voxel_data->resetLastMeshRegenerationTime();
	} else {
		UE_LOG(LogTemp, Warning, TEXT("non-game thread -> invoke async task"));
		if (GetTerrainController() != NULL) {
			GetTerrainController()->invokeZoneMeshAsync(this, md_ptr);
		}
	}
}

std::shared_ptr<MeshData> UTerrainZoneComponent::generateMesh() {
	double start = FPlatformTime::Seconds();

	if (voxel_data->getDensityFillState() == VoxelDataFillState::ZERO || voxel_data->getDensityFillState() == VoxelDataFillState::ALL) {
		return NULL;
	}

	bool enableLOD = GetTerrainController()->bEnableLOD;

	VoxelDataParam vdp;

	if (enableLOD) {
		vdp.bGenerateLOD = true;
		vdp.collisionLOD = 1;
	} else {
		vdp.bGenerateLOD = false;
		vdp.collisionLOD = 0;
	}

	MeshDataPtr md_ptr = sandboxVoxelGenerateMesh(*voxel_data, vdp);

	double end = FPlatformTime::Seconds();
	double time = (end - start) * 1000;

	//UE_LOG(LogTemp, Warning, TEXT("ASandboxTerrainZone::generateMesh -------------> %f %f %f --> %f ms"), GetComponentLocation().X, GetComponentLocation().Y, GetComponentLocation().Z, time);

	return md_ptr;
}

void UTerrainZoneComponent::applyTerrainMesh(std::shared_ptr<MeshData> mesh_data_ptr) {
	double start = FPlatformTime::Seconds();

	MeshData* mesh_data = mesh_data_ptr.get();

	if (mesh_data == NULL) {
		return;
	}

	//##########################################
	// draw debug points
	//##########################################
	/*
	MeshLodSection& section6 = mesh_data->MeshSectionLodArray[4];
	for (auto p : section6.DebugPointList) {
		DrawDebugPoint(GetWorld(), p, 5, FColor(0, 0, 255, 100), false, 1000000);
		UE_LOG(LogTemp, Warning, TEXT("DebugPointList ---> %f %f %f "), p.X, p.Y, p.Z);
	}
	*/
	//##########################################

	MainTerrainMesh->SetMobility(EComponentMobility::Movable);

	MainTerrainMesh->AddLocalRotation(FRotator(0.0f, 0.01, 0.0f));  // workaround
	MainTerrainMesh->AddLocalRotation(FRotator(0.0f, -0.01, 0.0f)); // workaround

	MainTerrainMesh->bLodFlag = GetTerrainController()->bEnableLOD;

	MainTerrainMesh->SetMeshData(mesh_data_ptr);

	MainTerrainMesh->SetMobility(EComponentMobility::Stationary);

	MainTerrainMesh->SetCastShadow(true);
	MainTerrainMesh->bCastHiddenShadow = true;
	MainTerrainMesh->SetMaterial(0, GetTerrainController()->TerrainMaterial);
	MainTerrainMesh->SetVisibility(true);

	CollisionMesh->SetMeshData(mesh_data_ptr);
	CollisionMesh->SetCollisionProfileName(TEXT("BlockAll"));

	double end = FPlatformTime::Seconds();
	double time = (end - start) * 1000;
	//UE_LOG(LogTemp, Warning, TEXT("ASandboxTerrainZone::applyTerrainMesh ---------> %f %f %f --> %f ms"), GetComponentLocation().X, GetComponentLocation().Y, GetComponentLocation().Z, time);

	if (voxel_data->isNewGenerated()) {
		voxel_data->DataState = VoxelDataState::NORMAL;
		GetTerrainController()->OnGenerateNewZone(this);
	}

	if (voxel_data->isNewLoaded()) {
		voxel_data->DataState = VoxelDataState::NORMAL;
		GetTerrainController()->OnLoadZone(this);
	}
}

void UTerrainZoneComponent::SaveInstancedMeshesToFile() {
	FString SavePath = FPaths::GameSavedDir();
	FVector Index = GetTerrainController()->getZoneIndex(GetComponentLocation());

	int tx = Index.X;
	int ty = Index.Y;
	int tz = Index.Z;

	FString FileName = SavePath + TEXT("/Map/") + GetTerrainController()->MapName + TEXT("/zone_inst_mesh.") + FString::FromInt(tx) + TEXT(".") + FString::FromInt(ty) + TEXT(".") + FString::FromInt(tz) + TEXT(".dat");

	FBufferArchive BinaryData;

	SerializeInstancedMeshes(BinaryData);

	if (FFileHelper::SaveArrayToFile(BinaryData, *FileName)) {
		BinaryData.FlushCache();
		BinaryData.Empty();
	}
}

void UTerrainZoneComponent::LoadInstancedMeshesFromFile() {
	FString SavePath = FPaths::GameSavedDir();
	FVector Index = GetTerrainController()->getZoneIndex(GetComponentLocation());

	int tx = Index.X;
	int ty = Index.Y;
	int tz = Index.Z;

	FString FileName = SavePath + TEXT("/Map/") + GetTerrainController()->MapName + TEXT("/zone_inst_mesh.") + FString::FromInt(tx) + TEXT(".") + FString::FromInt(ty) + TEXT(".") + FString::FromInt(tz) + TEXT(".dat");

	TArray<uint8> BinaryArray;
	if (!FFileHelper::LoadFileToArray(BinaryArray, *FileName)) {
		UE_LOG(LogTemp, Warning, TEXT("file not found -> %s"), *FileName);
		return;
	}

	if (BinaryArray.Num() <= 0) return;

	FMemoryReader BinaryData = FMemoryReader(BinaryArray, true); //true, free data after done
	BinaryData.Seek(0);

	// ==============================
	int32 MeshCount;
	BinaryData << MeshCount;
	UE_LOG(LogTemp, Warning, TEXT("MeshCount -> %d"), MeshCount);

	for (int Idx = 0; Idx < MeshCount; Idx++) {
		int32 MeshTypeId;
		int32 MeshInstanceCount;

		BinaryData << MeshTypeId;
		BinaryData << MeshInstanceCount;

		UE_LOG(LogTemp, Warning, TEXT("MeshTypeId -> %d"), MeshTypeId);
		UE_LOG(LogTemp, Warning, TEXT("MeshInstanceCount -> %d"), MeshInstanceCount);

		FTerrainInstancedMeshType MeshType;
		if (GetTerrainController()->FoliageMap.Contains(MeshTypeId)) {
			MeshType.Mesh = GetTerrainController()->FoliageMap[MeshTypeId].Mesh;
			MeshType.MeshTypeId = MeshTypeId;
		}

		for (int32 InstanceIdx = 0; InstanceIdx < MeshInstanceCount; InstanceIdx++) {
			float X;
			float Y;
			float Z;

			float Roll;
			float Pitch;
			float Yaw;

			float ScaleX;
			float ScaleY;
			float ScaleZ;

			BinaryData << X;
			BinaryData << Y;
			BinaryData << Z;

			BinaryData << Roll;
			BinaryData << Pitch;
			BinaryData << Yaw;

			BinaryData << ScaleX;
			BinaryData << ScaleY;
			BinaryData << ScaleZ;

			FRotator Rotator(Pitch, Yaw, Roll);
			FTransform Transform(Rotator, FVector(X, Y, Z), FVector(ScaleX, ScaleY, ScaleZ));

			if (MeshType.Mesh != nullptr) {
				SpawnInstancedMesh(MeshType, Transform);
			}
		}
	}

	// ==============================

	BinaryData.FlushCache();
	BinaryArray.Empty();
	BinaryData.Close();
}

void UTerrainZoneComponent::SerializeInstancedMeshes(FBufferArchive& BinaryData) {
	if (InstancedMeshMap.Num() == 0) {
		return;
	}

	int32 MeshCount = InstancedMeshMap.Num();
	BinaryData << MeshCount;

	for (auto& Elem : InstancedMeshMap) {
		UHierarchicalInstancedStaticMeshComponent* InstancedStaticMeshComponent = Elem.Value;
		int32 MeshTypeId = Elem.Key;

		int32 MeshInstanceCount = InstancedStaticMeshComponent->GetInstanceCount();

		BinaryData << MeshTypeId;
		BinaryData << MeshInstanceCount;

		for (int32 InstanceIdx = 0; InstanceIdx < MeshInstanceCount; InstanceIdx++) {
			FTransform InstanceTransform;
			InstancedStaticMeshComponent->GetInstanceTransform(InstanceIdx, InstanceTransform, true);

			float X = InstanceTransform.GetLocation().X;
			float Y = InstanceTransform.GetLocation().Y;
			float Z = InstanceTransform.GetLocation().Z;

			float Roll = InstanceTransform.Rotator().Roll;
			float Pitch = InstanceTransform.Rotator().Pitch;
			float Yaw = InstanceTransform.Rotator().Yaw;

			float ScaleX = InstanceTransform.GetScale3D().X;
			float ScaleY = InstanceTransform.GetScale3D().Y;
			float ScaleZ = InstanceTransform.GetScale3D().Z;

			BinaryData << X;
			BinaryData << Y;
			BinaryData << Z;

			BinaryData << Roll;
			BinaryData << Pitch;
			BinaryData << Yaw;

			BinaryData << ScaleX;
			BinaryData << ScaleY;
			BinaryData << ScaleZ;
		}
	}
}

void UTerrainZoneComponent::SpawnInstancedMesh(FTerrainInstancedMeshType& MeshType, FTransform& Transform) {
	UHierarchicalInstancedStaticMeshComponent* InstancedStaticMeshComponent = nullptr;

	if (InstancedMeshMap.Contains(MeshType.MeshTypeId)) {
		InstancedStaticMeshComponent = InstancedMeshMap[MeshType.MeshTypeId];
	}

	if (InstancedStaticMeshComponent == nullptr) {
		FString InstancedStaticMeshCompName = FString::Printf(TEXT("InstancedStaticMesh -%d -> [%.0f, %.0f, %.0f]"), MeshType.MeshTypeId, GetComponentLocation().X, GetComponentLocation().Y, GetComponentLocation().Z);

		InstancedStaticMeshComponent = NewObject<UHierarchicalInstancedStaticMeshComponent>(this, FName(*InstancedStaticMeshCompName));

		InstancedStaticMeshComponent->RegisterComponent();
		InstancedStaticMeshComponent->AttachTo(this);
		InstancedStaticMeshComponent->SetStaticMesh(MeshType.Mesh);
		InstancedStaticMeshComponent->SetCullDistances(100, 500);
		InstancedStaticMeshComponent->SetMobility(EComponentMobility::Static);
		InstancedStaticMeshComponent->SetSimulatePhysics(false);

		//InstancedStaticMeshComponent->SetCollisionEnabled(ECollisionEnabled::NoCollision);

		InstancedStaticMeshComponent->SetCollisionEnabled(ECollisionEnabled::QueryOnly);
		InstancedStaticMeshComponent->SetCollisionProfileName(TEXT("OverlapAll"));

		InstancedMeshMap.Add(MeshType.MeshTypeId, InstancedStaticMeshComponent);
	}

	InstancedStaticMeshComponent->AddInstanceWorldSpace(Transform);
}<|endoftext|>


<commit_before>/*
 * Copyright (C) 2008-2011, Gostai S.A.S.
 *
 * This software is provided "as is" without warranty of any kind,
 * either expressed or implied, including but not limited to the
 * implied warranties of fitness for a particular purpose.
 *
 * See the LICENSE file for more information.
 */

#ifndef LIBPORT_TRAITS_HH
# define LIBPORT_TRAITS_HH

# include <boost/tr1/type_traits.hpp>

# include <libport/meta.hh>

namespace libport
{
  namespace traits
  {
    /*------.
    | IsPOD |
    `------*/

    /// Whether 'T' is a POD.
    template <typename T>
    struct IsPOD
    {
      static const bool res = std::tr1::is_pod<T>::value;
    };

    /*---.
    | Id |
    `---*/

    /// Identity
    template <typename T>
    struct Id
    {
      typedef T res;
    };

    /*------.
    | Const |
    `------*/

    /// Constify T
    template <typename T>
    struct Const
    {
      typedef const T res;
    };

    /*----.
    | Ptr |
    `----*/

    /// Make T a pointer
    template <typename T>
    struct Ptr
    {
      typedef T* res;
    };

    /*----.
    | Ref |
    `----*/

    /// Make T a reference
    template <typename T>
    struct Ref
    {
      typedef T& res;
    };

    /*---------.
    | ConstPtr |
    `---------*/

    /// Make T a const pointer
    template <typename T>
    struct ConstPtr
    {
      typedef typename meta::Compose<Ptr, Const>::Res<T>::res res;
    };

    /*---------.
    | ConstRef |
    `---------*/

    /// Make T a const reference
    template <typename T>
    struct ConstRef
    {
      typedef typename meta::Compose<Ref, Const>::Res<T>::res res;
    };

    /*-------------------.
    | Remove reference.  |
    `-------------------*/

    /// Remove reference mark from T.
    template <typename T>
    struct RemoveReference
    {
      typedef T res;
    };

    template <typename T>
    struct RemoveReference<T&>
    {
      typedef T res;
    };

    /// Remove reference mark from T, tr1 style.
    template <typename T>
    struct remove_reference
    {
      typedef T type;
    };

    template <typename T>
    struct remove_reference<T&>
    {
      typedef T type;
    };

    /*----.
    | Arg |
    `----*/

    /// Give the best type for passing a T as argument
    /* That is, T if T is a POD, const T& otherwise.
     *
     */
    template <typename T>
    struct Arg
    {
      typedef typename meta::If<IsPOD<T>::res, T, const T&>::res res;
    };
  }
}

#endif
<commit_msg>Traits: add RemoveConst.<commit_after>/*
 * Copyright (C) 2008-2011, Gostai S.A.S.
 *
 * This software is provided "as is" without warranty of any kind,
 * either expressed or implied, including but not limited to the
 * implied warranties of fitness for a particular purpose.
 *
 * See the LICENSE file for more information.
 */

#ifndef LIBPORT_TRAITS_HH
# define LIBPORT_TRAITS_HH

# include <boost/tr1/type_traits.hpp>

# include <libport/meta.hh>

namespace libport
{
  namespace traits
  {
    /*------.
    | IsPOD |
    `------*/

    /// Whether 'T' is a POD.
    template <typename T>
    struct IsPOD
    {
      static const bool res = std::tr1::is_pod<T>::value;
    };

    /*---.
    | Id |
    `---*/

    /// Identity
    template <typename T>
    struct Id
    {
      typedef T res;
    };

    /*------.
    | Const |
    `------*/

    /// Constify T
    template <typename T>
    struct Const
    {
      typedef const T res;
    };

    /*----.
    | Ptr |
    `----*/

    /// Make T a pointer
    template <typename T>
    struct Ptr
    {
      typedef T* res;
    };

    /*----.
    | Ref |
    `----*/

    /// Make T a reference
    template <typename T>
    struct Ref
    {
      typedef T& res;
    };

    /*---------.
    | ConstPtr |
    `---------*/

    /// Make T a const pointer
    template <typename T>
    struct ConstPtr
    {
      typedef typename meta::Compose<Ptr, Const>::Res<T>::res res;
    };

    /*---------.
    | ConstRef |
    `---------*/

    /// Make T a const reference
    template <typename T>
    struct ConstRef
    {
      typedef typename meta::Compose<Ref, Const>::Res<T>::res res;
    };

    /*-------------------.
    | Remove reference.  |
    `-------------------*/

    /// Remove reference mark from T.
    template <typename T>
    struct RemoveReference
    {
      typedef T res;
    };

    template <typename T>
    struct RemoveReference<T&>
    {
      typedef T res;
    };

    /// Remove reference mark from T, tr1 style.
    template <typename T>
    struct remove_reference
    {
      typedef T type;
    };

    template <typename T>
    struct remove_reference<T&>
    {
      typedef T type;
    };

    /*---------------.
    | Remove const.  |
    `---------------*/

    /// Remove const mark from T.
    template <typename T>
    struct RemoveConst
    {
      typedef T res;
    };

    template <typename T>
    struct RemoveConst<const T>
    {
      typedef T res;
    };

    /*----.
    | Arg |
    `----*/

    /// Give the best type for passing a T as argument
    /* That is, T if T is a POD, const T& otherwise.
     *
     */
    template <typename T>
    struct Arg
    {
      typedef typename meta::If<IsPOD<T>::res, T, const T&>::res res;
    };
  }
}

#endif
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2009, Gostai S.A.S.
 *
 * This software is provided "as is" without warranty of any kind,
 * either expressed or implied, including but not limited to the
 * implied warranties of fitness for a particular purpose.
 *
 * See the LICENSE file for more information.
 */
#ifndef LIBPORT_UTIME_HXX
# define LIBPORT_UTIME_HXX

namespace libport
{

  template <typename Unit>
  inline
  utime_t
  seconds_to_utime(Unit seconds)
  {
    return seconds * 1000LL * 1000LL;
  }

  inline
  utime_t
  timeval_to_utime(const timeval& t)
  {
    return seconds_to_utime(t.tv_sec) + t.tv_usec;
  }

  inline
  timeval
  utime_to_timeval(utime_t t)
  {
    timeval res;
    res.tv_sec =  t / (1000 * 1000);
    res.tv_usec =  t % (1000 * 1000);
    return res;
  }

}

#endif
<commit_msg>Please G++ 4.4.<commit_after>/*
 * Copyright (C) 2009, Gostai S.A.S.
 *
 * This software is provided "as is" without warranty of any kind,
 * either expressed or implied, including but not limited to the
 * implied warranties of fitness for a particular purpose.
 *
 * See the LICENSE file for more information.
 */
#ifndef LIBPORT_UTIME_HXX
# define LIBPORT_UTIME_HXX

namespace libport
{

  template <typename Unit>
  inline
  utime_t
  seconds_to_utime(Unit seconds)
  {
    return utime_t(seconds * 1000LL * 1000LL);
  }

  inline
  utime_t
  timeval_to_utime(const timeval& t)
  {
    return seconds_to_utime(t.tv_sec) + t.tv_usec;
  }

  inline
  timeval
  utime_to_timeval(utime_t t)
  {
    timeval res;
    res.tv_sec =  t / (1000 * 1000);
    res.tv_usec =  t % (1000 * 1000);
    return res;
  }

}

#endif
<|endoftext|>


<commit_before>#pragma once

#ifndef SIPL_MATRIX_VECTOR_H
#define SIPL_MATRIX_VECTOR_H

#include <algorithm>
#include <functional>
#include <array>
#include "matrix/Wrappers.hpp"
#include "matrix/VectorBase.hpp"
#include "Constants.hpp"

namespace sipl
{

// Derived class for statically allocated Vector.
template <typename Dtype, int32_t Length>
class Vector
    : public VectorBase<Dtype, Length, StaticArrayWrapper<Dtype, Length>>
{
public:
    using ContainerType = StaticArrayWrapper<Dtype, Length>;
    using BaseClass = VectorBase<Dtype, Length, ContainerType>;
    using BaseClass::BaseClass;

    Vector()
    {
        this->nelements_ = Length;
        this->nbytes_ = this->nelements_ * int32_t(sizeof(Dtype));
        this->data_ = ContainerType();
    }

    Vector(Dtype fill_value)
    {
        this->nelements_ = Length;
        this->nbytes_ = this->nelements_ * int32_t(sizeof(Dtype));
        this->data_ = ContainerType();
        std::fill(std::begin(this->data_), std::end(this->data_), fill_value);
    }

    template <typename OtherType>
    Vector(const Vector<OtherType, Length>& v)
    {
        this->nelements_ = v.size();
        this->nbytes_ = v.size_in_bytes();
        this->data_ = ContainerType();
        std::transform(std::begin(v), std::end(v), std::begin(this->data_),
                       [](auto e) { return Dtype(e); });
    }

    template <typename OtherType>
    Vector<OtherType, Length> as_type() const
    {
        const auto tmp = (*this) / double(this->max());
        const auto max = std::numeric_limits<OtherType>::max();
        return tmp.apply(
            [max](auto e) { return OtherType(std::round(e * max)); });
    }

    void transform(std::function<Dtype(Dtype)> f)
    {
        std::transform(this->begin(), this->end(), this->begin(), f);
    }

    // Template magic from: http://stackoverflow.com/a/26383814
    template <
        typename Functor,
        typename OutputType = typename std::result_of<Functor&(Dtype)>::type>
    decltype(auto) apply(Functor f) const
    {
        Vector<OutputType, Length> new_m;
        std::transform(this->begin(), this->end(), std::begin(new_m), f);
        return new_m;
    }
};

// Specialization of the above for dynamically-allocated Vector.
template <typename Dtype>
class Vector<Dtype, Dynamic>
    : public VectorBase<Dtype, Dynamic, DynamicArrayWrapper<Dtype, Dynamic>>
{
public:
    using ContainerType = DynamicArrayWrapper<Dtype, Dynamic>;
    using BaseClass = VectorBase<Dtype, Dynamic, ContainerType>;
    using BaseClass::BaseClass;
    // Need to use 'this' pointer below because templated base class members are
    // not visible in a certain phase of compilation. See here:
    // http://stackoverflow.com/a/6592617
    Vector(int32_t size)
    {
        this->nelements_ = size;
        this->nbytes_ = this->nelements_ * int32_t(sizeof(Dtype));
        this->data_ = ContainerType(this->nelements_);
    }

    Vector(int32_t size, Dtype fill_value)
    {
        this->nelements_ = size;
        this->data_ = ContainerType(this->nelements_);
        this->nbytes_ = this->nelements_ * int32_t(sizeof(Dtype));
        std::fill(std::begin(this->data_), std::end(this->data_), fill_value);
    }

    template <typename OtherType>
    Vector(const Vector<OtherType, Dynamic>& v)
    {
        this->nelements_ = v.size();
        this->nbytes_ = v.size_in_bytes();
        this->data_ = ContainerType(this->nelements_);
        std::transform(std::begin(v), std::end(v), std::begin(this->data_),
                       [](auto e) { return Dtype(e); });
    }

    template <typename OtherType>
    Vector<OtherType, Dynamic> as_type() const
    {
        const auto tmp = (*this) / double(this->max());
        const auto max = std::numeric_limits<OtherType>::max();
        return tmp.apply(
            [max](auto e) { return OtherType(std::round(e * max)); });
    }

    void transform(std::function<Dtype(Dtype)> f)
    {
        std::transform(this->begin(), this->end(), this->begin(), f);
    }

    // Template magic from: http://stackoverflow.com/a/26383814
    template <
        typename Functor,
        typename OutputType = typename std::result_of<Functor&(Dtype)>::type>
    decltype(auto) apply(Functor f) const
    {
        Vector<OutputType, Dynamic> new_m(this->nelements_);
        std::transform(this->begin(), this->end(), std::begin(new_m), f);
        return new_m;
    }
};

// Static vector aliases
template <typename Dtype>
using Vector3 = Vector<Dtype, 3>;
using Vector3b = Vector3<uint8_t>;
using Vector3d = Vector3<double>;
using Vector3f = Vector3<float>;
using Vector3i = Vector3<int32_t>;
using RgbPixel = Vector3b;

template <typename Dtype>
using Vector4 = Vector<Dtype, 4>;
using Vector4d = Vector4<double>;
using Vector4i = Vector4<int32_t>;
using Vector4b = Vector4<uint8_t>;

// Dynamic vector aliases
template <typename Dtype>
using VectorX = Vector<Dtype, Dynamic>;
using VectorXf = VectorX<float>;
using VectorXd = VectorX<double>;
using VectorXi = VectorX<int32_t>;
}

#endif
<commit_msg>Aliases for Vector2<commit_after>#pragma once

#ifndef SIPL_MATRIX_VECTOR_H
#define SIPL_MATRIX_VECTOR_H

#include <algorithm>
#include <functional>
#include <array>
#include "matrix/Wrappers.hpp"
#include "matrix/VectorBase.hpp"
#include "Constants.hpp"

namespace sipl
{

// Derived class for statically allocated Vector.
template <typename Dtype, int32_t Length>
class Vector
    : public VectorBase<Dtype, Length, StaticArrayWrapper<Dtype, Length>>
{
public:
    using ContainerType = StaticArrayWrapper<Dtype, Length>;
    using BaseClass = VectorBase<Dtype, Length, ContainerType>;
    using BaseClass::BaseClass;

    Vector()
    {
        this->nelements_ = Length;
        this->nbytes_ = this->nelements_ * int32_t(sizeof(Dtype));
        this->data_ = ContainerType();
    }

    Vector(Dtype fill_value)
    {
        this->nelements_ = Length;
        this->nbytes_ = this->nelements_ * int32_t(sizeof(Dtype));
        this->data_ = ContainerType();
        std::fill(std::begin(this->data_), std::end(this->data_), fill_value);
    }

    template <typename OtherType>
    Vector(const Vector<OtherType, Length>& v)
    {
        this->nelements_ = v.size();
        this->nbytes_ = v.size_in_bytes();
        this->data_ = ContainerType();
        std::transform(std::begin(v), std::end(v), std::begin(this->data_),
                       [](auto e) { return Dtype(e); });
    }

    template <typename OtherType>
    Vector<OtherType, Length> as_type() const
    {
        const auto tmp = (*this) / double(this->max());
        const auto max = std::numeric_limits<OtherType>::max();
        return tmp.apply(
            [max](auto e) { return OtherType(std::round(e * max)); });
    }

    void transform(std::function<Dtype(Dtype)> f)
    {
        std::transform(this->begin(), this->end(), this->begin(), f);
    }

    // Template magic from: http://stackoverflow.com/a/26383814
    template <
        typename Functor,
        typename OutputType = typename std::result_of<Functor&(Dtype)>::type>
    decltype(auto) apply(Functor f) const
    {
        Vector<OutputType, Length> new_m;
        std::transform(this->begin(), this->end(), std::begin(new_m), f);
        return new_m;
    }
};

// Specialization of the above for dynamically-allocated Vector.
template <typename Dtype>
class Vector<Dtype, Dynamic>
    : public VectorBase<Dtype, Dynamic, DynamicArrayWrapper<Dtype, Dynamic>>
{
public:
    using ContainerType = DynamicArrayWrapper<Dtype, Dynamic>;
    using BaseClass = VectorBase<Dtype, Dynamic, ContainerType>;
    using BaseClass::BaseClass;
    // Need to use 'this' pointer below because templated base class members are
    // not visible in a certain phase of compilation. See here:
    // http://stackoverflow.com/a/6592617
    Vector(int32_t size)
    {
        this->nelements_ = size;
        this->nbytes_ = this->nelements_ * int32_t(sizeof(Dtype));
        this->data_ = ContainerType(this->nelements_);
    }

    Vector(int32_t size, Dtype fill_value)
    {
        this->nelements_ = size;
        this->data_ = ContainerType(this->nelements_);
        this->nbytes_ = this->nelements_ * int32_t(sizeof(Dtype));
        std::fill(std::begin(this->data_), std::end(this->data_), fill_value);
    }

    template <typename OtherType>
    Vector(const Vector<OtherType, Dynamic>& v)
    {
        this->nelements_ = v.size();
        this->nbytes_ = v.size_in_bytes();
        this->data_ = ContainerType(this->nelements_);
        std::transform(std::begin(v), std::end(v), std::begin(this->data_),
                       [](auto e) { return Dtype(e); });
    }

    template <typename OtherType>
    Vector<OtherType, Dynamic> as_type() const
    {
        const auto tmp = (*this) / double(this->max());
        const auto max = std::numeric_limits<OtherType>::max();
        return tmp.apply(
            [max](auto e) { return OtherType(std::round(e * max)); });
    }

    void transform(std::function<Dtype(Dtype)> f)
    {
        std::transform(this->begin(), this->end(), this->begin(), f);
    }

    // Template magic from: http://stackoverflow.com/a/26383814
    template <
        typename Functor,
        typename OutputType = typename std::result_of<Functor&(Dtype)>::type>
    decltype(auto) apply(Functor f) const
    {
        Vector<OutputType, Dynamic> new_m(this->nelements_);
        std::transform(this->begin(), this->end(), std::begin(new_m), f);
        return new_m;
    }
};

// Static vector aliases
template <typename Dtype>
using Vector2 = Vector<Dtype, 2>;
using Vector2b = Vector2<uint8_t>;
using Vector2d = Vector2<double>;
using Vector2f = Vector2<float>;
using Vector2i = Vector2<int32_t>;

template <typename Dtype>
using Vector3 = Vector<Dtype, 3>;
using Vector3b = Vector3<uint8_t>;
using Vector3d = Vector3<double>;
using Vector3f = Vector3<float>;
using Vector3i = Vector3<int32_t>;
using RgbPixel = Vector3b;

template <typename Dtype>
using Vector4 = Vector<Dtype, 4>;
using Vector4d = Vector4<double>;
using Vector4i = Vector4<int32_t>;
using Vector4b = Vector4<uint8_t>;

// Dynamic vector aliases
template <typename Dtype>
using VectorX = Vector<Dtype, Dynamic>;
using VectorXf = VectorX<float>;
using VectorXd = VectorX<double>;
using VectorXi = VectorX<int32_t>;
}

#endif
<|endoftext|>