<commit_before>/*******************************************************
 * Copyright (c) 2014, ArrayFire
 * All rights reserved.
 *
 * This file is distributed under 3-clause BSD license.
 * The complete license agreement can be obtained at:
 * http://arrayfire.com/licenses/BSD-3-Clause
 ********************************************************/

#include <af/defines.h>
#include <ops.hpp>
#include <backend.hpp>
#include <Param.hpp>
#include <dispatch.hpp>
#include <math.hpp>
#include <err_cuda.hpp>
#include <debug_cuda.hpp>
#include <memory.hpp>
#include "config.hpp"

namespace cuda
{
namespace kernel
{

    template<typename Ti, typename To, af_op_t op, int dim, bool isFinalPass, uint DIMY>
    __global__
    static void scan_dim_kernel(Param<To> out,
                                Param<To> tmp,
                                CParam<Ti>  in,
                                uint blocks_x,
                                uint blocks_y,
                                uint blocks_dim,
                                uint lim)
    {
        const uint tidx = threadIdx.x;
        const uint tidy = threadIdx.y;
        const uint tid  = tidy * THREADS_X + tidx;

        const uint zid = blockIdx.x / blocks_x;
        const uint wid = blockIdx.y / blocks_y;
        const uint blockIdx_x = blockIdx.x - (blocks_x) * zid;
        const uint blockIdx_y = blockIdx.y - (blocks_y) * wid;
        const uint xid = blockIdx_x * blockDim.x + tidx;
        const uint yid = blockIdx_y; // yid  of output. updated for input later.

        uint ids[4] = {xid, yid, zid, wid};

        const Ti *iptr = in.ptr;
        To *optr = out.ptr;
        To *tptr = tmp.ptr;

        // There is only one element per block for out
        // There are blockDim.y elements per block for in
        // Hence increment ids[dim] just after offseting out and before offsetting in
        tptr += ids[3] * tmp.strides[3] + ids[2] * tmp.strides[2] + ids[1] * tmp.strides[1] + ids[0];
        const uint blockIdx_dim = ids[dim];

        ids[dim] = ids[dim] * blockDim.y * lim + tidy;
        optr  += ids[3] * out.strides[3] + ids[2] * out.strides[2] + ids[1] * out.strides[1] + ids[0];
        iptr  += ids[3] *  in.strides[3] + ids[2] *  in.strides[2] + ids[1] *  in.strides[1] + ids[0];
        uint id_dim = ids[dim];
        const uint out_dim = out.dims[dim];

        bool is_valid =
            (ids[0] < out.dims[0]) &&
            (ids[1] < out.dims[1]) &&
            (ids[2] < out.dims[2]) &&
            (ids[3] < out.dims[3]);

        const uint ostride_dim = out.strides[dim];
        const uint istride_dim =  in.strides[dim];

        __shared__ To s_val[THREADS_X * DIMY * 2];
        __shared__ To s_tmp[THREADS_X];
        To *sptr =  s_val + tid;

        Transform<Ti, To, op> transform;
        Binary<To, op> binop;

        const To init = binop.init();
        To val = init;

        const bool isLast = (tidy == (DIMY - 1));

        for (int k = 0; k < lim; k++) {

            if (isLast) s_tmp[tidx] = val;

            bool cond = (is_valid) && (id_dim < out_dim);
            val = cond ? transform(*iptr) : init;
            *sptr = val;
            __syncthreads();

            uint start = 0;
            for (int off = 1; off < DIMY; off *= 2) {

                if (tidy >= off) val = binop(val, sptr[(start - off) * THREADS_X]);
                start = DIMY - start;
                sptr[start * THREADS_X] = val;

                __syncthreads();
            }

            val = binop(val, s_tmp[tidx]);
            if (cond) *optr = val;

            id_dim += blockDim.y;
            iptr += blockDim.y * istride_dim;
            optr += blockDim.y * ostride_dim;
        }

        if (!isFinalPass &&
            is_valid &&
            (blockIdx_dim < tmp.dims[dim]) &&
            isLast) {
            *tptr = val;
            }
    }

    template<typename To, af_op_t op, int dim>
    __global__
    static void bcast_dim_kernel(Param<To> out,
                                 CParam<To> tmp,
                                 uint blocks_x,
                                 uint blocks_y,
                                 uint blocks_dim,
                                 uint lim)
    {
        const uint tidx = threadIdx.x;
        const uint tidy = threadIdx.y;

        const uint zid = blockIdx.x / blocks_x;
        const uint wid = blockIdx.y / blocks_y;
        const uint blockIdx_x = blockIdx.x - (blocks_x) * zid;
        const uint blockIdx_y = blockIdx.y - (blocks_y) * wid;
        const uint xid = blockIdx_x * blockDim.x + tidx;
        const uint yid = blockIdx_y; // yid  of output. updated for input later.

        uint ids[4] = {xid, yid, zid, wid};

        const To *tptr = tmp.ptr;
        To *optr = out.ptr;

        // There is only one element per block for out
        // There are blockDim.y elements per block for in
        // Hence increment ids[dim] just after offseting out and before offsetting in
        tptr += ids[3] * tmp.strides[3] + ids[2] * tmp.strides[2] + ids[1] * tmp.strides[1] + ids[0];
        const uint blockIdx_dim = ids[dim];

        ids[dim] = ids[dim] * blockDim.y * lim + tidy;
        optr  += ids[3] * out.strides[3] + ids[2] * out.strides[2] + ids[1] * out.strides[1] + ids[0];
        const uint id_dim = ids[dim];
        const uint out_dim = out.dims[dim];

        bool is_valid =
            (ids[0] < out.dims[0]) &&
            (ids[1] < out.dims[1]) &&
            (ids[2] < out.dims[2]) &&
            (ids[3] < out.dims[3]);

        if (!is_valid) return;
        if (blockIdx_dim == 0) return;

        To accum = *(tptr - tmp.strides[dim]);

        Binary<To, op> binop;
        const uint ostride_dim = out.strides[dim];

        for (int k = 0, id = id_dim;
             is_valid && k < lim && (id < out_dim);
             k++, id += blockDim.y) {

            *optr = binop(*optr,accum);
            optr += blockDim.y * ostride_dim;
        }
    }

    template<typename Ti, typename To, af_op_t op, int dim, bool isFinalPass>
    static void scan_dim_launcher(Param<To> out,
                           Param<To> tmp,
                           CParam<Ti> in,
                           const uint threads_y,
                           const uint blocks_all[4])
    {
        dim3 threads(THREADS_X, threads_y);

        dim3 blocks(blocks_all[0] * blocks_all[2],
                    blocks_all[1] * blocks_all[3]);

        uint lim = divup(out.dims[dim], (threads_y * blocks_all[dim]));

        switch (threads_y) {
        case 8:
            (scan_dim_kernel<Ti, To, op, dim, isFinalPass, 8>)<<<blocks, threads>>>(
                out, tmp, in, blocks_all[0], blocks_all[1], blocks_all[dim], lim); break;
        case 4:
            (scan_dim_kernel<Ti, To, op, dim, isFinalPass, 4>)<<<blocks, threads>>>(
                out, tmp, in, blocks_all[0], blocks_all[1], blocks_all[dim], lim); break;
        case 2:
            (scan_dim_kernel<Ti, To, op, dim, isFinalPass, 2>)<<<blocks, threads>>>(
                out, tmp, in, blocks_all[0], blocks_all[1], blocks_all[dim], lim); break;
        case 1:
            (scan_dim_kernel<Ti, To, op, dim, isFinalPass, 1>)<<<blocks, threads>>>(
                out, tmp, in, blocks_all[0], blocks_all[1], blocks_all[dim], lim); break;
        }

        POST_LAUNCH_CHECK();
    }

    template<typename To, af_op_t op, int dim>
    static void bcast_dim_launcher(Param<To> out,
                                   CParam<To> tmp,
                                   const uint threads_y,
                                   const uint blocks_all[4])
    {

        dim3 threads(THREADS_X, threads_y);

        dim3 blocks(blocks_all[0] * blocks_all[2],
                    blocks_all[1] * blocks_all[3]);

        uint lim = divup(out.dims[dim], (threads_y * blocks_all[dim]));

        (bcast_dim_kernel<To, op, dim>)<<<blocks, threads>>>(
            out, tmp, blocks_all[0], blocks_all[1], blocks_all[dim], lim);

        POST_LAUNCH_CHECK();
    }

    template<typename Ti, typename To, af_op_t op, int dim>
    static void scan_dim(Param<To> out, CParam<Ti> in)
    {
        uint threads_y = std::min(THREADS_Y, nextpow2(out.dims[dim]));
        uint threads_x = THREADS_X;

        uint blocks_all[] = {divup(out.dims[0], threads_x),
                             out.dims[1], out.dims[2], out.dims[3]};

        blocks_all[dim] = divup(out.dims[dim], threads_y * REPEAT);

        if (blocks_all[dim] == 1) {

            scan_dim_launcher<Ti, To, op, dim, true>(out, out, in,
                                                     threads_y,
                                                     blocks_all);

        } else {

            Param<To> tmp = out;

            tmp.dims[dim] = blocks_all[dim];
            tmp.strides[0] = 1;
            for (int k = 1; k < 4; k++) tmp.strides[k] = tmp.strides[k - 1] * tmp.dims[k - 1];

            dim_type tmp_elements = tmp.strides[3] * tmp.dims[3];
            tmp.ptr = memAlloc<To>(tmp_elements);

            scan_dim_launcher<Ti, To, op, dim, false>(out, tmp, in,
                                                      threads_y,
                                                      blocks_all);

            int bdim = blocks_all[dim];
            blocks_all[dim] = 1;

            //FIXME: Is there an alternative to the if condition ?
            if (op == af_notzero_t) {
                scan_dim_launcher<To, To, af_add_t, dim, true>(tmp, tmp, tmp,
                                                               threads_y,
                                                               blocks_all);
            } else {
                scan_dim_launcher<To, To,       op, dim, true>(tmp, tmp, tmp,
                                                               threads_y,
                                                               blocks_all);
            }

            blocks_all[dim] = bdim;
            bcast_dim_launcher<To, op, dim>(out, tmp, threads_y, blocks_all);

            memFree(tmp.ptr);
        }
    }

}
}
<commit_msg>BUGFIX: for accum along non-first dimension<commit_after>/*******************************************************
 * Copyright (c) 2014, ArrayFire
 * All rights reserved.
 *
 * This file is distributed under 3-clause BSD license.
 * The complete license agreement can be obtained at:
 * http://arrayfire.com/licenses/BSD-3-Clause
 ********************************************************/

#include <af/defines.h>
#include <ops.hpp>
#include <backend.hpp>
#include <Param.hpp>
#include <dispatch.hpp>
#include <math.hpp>
#include <err_cuda.hpp>
#include <debug_cuda.hpp>
#include <memory.hpp>
#include "config.hpp"

namespace cuda
{
namespace kernel
{

    template<typename Ti, typename To, af_op_t op, int dim, bool isFinalPass, uint DIMY>
    __global__
    static void scan_dim_kernel(Param<To> out,
                                Param<To> tmp,
                                CParam<Ti>  in,
                                uint blocks_x,
                                uint blocks_y,
                                uint blocks_dim,
                                uint lim)
    {
        const uint tidx = threadIdx.x;
        const uint tidy = threadIdx.y;
        const uint tid  = tidy * THREADS_X + tidx;

        const uint zid = blockIdx.x / blocks_x;
        const uint wid = blockIdx.y / blocks_y;
        const uint blockIdx_x = blockIdx.x - (blocks_x) * zid;
        const uint blockIdx_y = blockIdx.y - (blocks_y) * wid;
        const uint xid = blockIdx_x * blockDim.x + tidx;
        const uint yid = blockIdx_y; // yid  of output. updated for input later.

        uint ids[4] = {xid, yid, zid, wid};

        const Ti *iptr = in.ptr;
        To *optr = out.ptr;
        To *tptr = tmp.ptr;

        // There is only one element per block for out
        // There are blockDim.y elements per block for in
        // Hence increment ids[dim] just after offseting out and before offsetting in
        tptr += ids[3] * tmp.strides[3] + ids[2] * tmp.strides[2] + ids[1] * tmp.strides[1] + ids[0];
        const uint blockIdx_dim = ids[dim];

        ids[dim] = ids[dim] * blockDim.y * lim + tidy;
        optr  += ids[3] * out.strides[3] + ids[2] * out.strides[2] + ids[1] * out.strides[1] + ids[0];
        iptr  += ids[3] *  in.strides[3] + ids[2] *  in.strides[2] + ids[1] *  in.strides[1] + ids[0];
        uint id_dim = ids[dim];
        const uint out_dim = out.dims[dim];

        bool is_valid =
            (ids[0] < out.dims[0]) &&
            (ids[1] < out.dims[1]) &&
            (ids[2] < out.dims[2]) &&
            (ids[3] < out.dims[3]);

        const uint ostride_dim = out.strides[dim];
        const uint istride_dim =  in.strides[dim];

        __shared__ To s_val[THREADS_X * DIMY * 2];
        __shared__ To s_tmp[THREADS_X];
        To *sptr =  s_val + tid;

        Transform<Ti, To, op> transform;
        Binary<To, op> binop;

        const To init = binop.init();
        To val = init;

        const bool isLast = (tidy == (DIMY - 1));

        for (int k = 0; k < lim; k++) {

            if (isLast) s_tmp[tidx] = val;

            bool cond = (is_valid) && (id_dim < out_dim);
            val = cond ? transform(*iptr) : init;
            *sptr = val;
            __syncthreads();

            uint start = 0;
            for (int off = 1; off < DIMY; off *= 2) {

                if (tidy >= off) val = binop(val, sptr[(start - off) * THREADS_X]);
                start = DIMY - start;
                sptr[start * THREADS_X] = val;

                __syncthreads();
            }

            val = binop(val, s_tmp[tidx]);
            __syncthreads();
            if (cond) *optr = val;

            id_dim += blockDim.y;
            iptr += blockDim.y * istride_dim;
            optr += blockDim.y * ostride_dim;
        }

        if (!isFinalPass &&
            is_valid &&
            (blockIdx_dim < tmp.dims[dim]) &&
            isLast) {
            *tptr = val;
            }
    }

    template<typename To, af_op_t op, int dim>
    __global__
    static void bcast_dim_kernel(Param<To> out,
                                 CParam<To> tmp,
                                 uint blocks_x,
                                 uint blocks_y,
                                 uint blocks_dim,
                                 uint lim)
    {
        const uint tidx = threadIdx.x;
        const uint tidy = threadIdx.y;

        const uint zid = blockIdx.x / blocks_x;
        const uint wid = blockIdx.y / blocks_y;
        const uint blockIdx_x = blockIdx.x - (blocks_x) * zid;
        const uint blockIdx_y = blockIdx.y - (blocks_y) * wid;
        const uint xid = blockIdx_x * blockDim.x + tidx;
        const uint yid = blockIdx_y; // yid  of output. updated for input later.

        uint ids[4] = {xid, yid, zid, wid};

        const To *tptr = tmp.ptr;
        To *optr = out.ptr;

        // There is only one element per block for out
        // There are blockDim.y elements per block for in
        // Hence increment ids[dim] just after offseting out and before offsetting in
        tptr += ids[3] * tmp.strides[3] + ids[2] * tmp.strides[2] + ids[1] * tmp.strides[1] + ids[0];
        const uint blockIdx_dim = ids[dim];

        ids[dim] = ids[dim] * blockDim.y * lim + tidy;
        optr  += ids[3] * out.strides[3] + ids[2] * out.strides[2] + ids[1] * out.strides[1] + ids[0];
        const uint id_dim = ids[dim];
        const uint out_dim = out.dims[dim];

        bool is_valid =
            (ids[0] < out.dims[0]) &&
            (ids[1] < out.dims[1]) &&
            (ids[2] < out.dims[2]) &&
            (ids[3] < out.dims[3]);

        if (!is_valid) return;
        if (blockIdx_dim == 0) return;

        To accum = *(tptr - tmp.strides[dim]);

        Binary<To, op> binop;
        const uint ostride_dim = out.strides[dim];

        for (int k = 0, id = id_dim;
             is_valid && k < lim && (id < out_dim);
             k++, id += blockDim.y) {

            *optr = binop(*optr,accum);
            optr += blockDim.y * ostride_dim;
        }
    }

    template<typename Ti, typename To, af_op_t op, int dim, bool isFinalPass>
    static void scan_dim_launcher(Param<To> out,
                           Param<To> tmp,
                           CParam<Ti> in,
                           const uint threads_y,
                           const uint blocks_all[4])
    {
        dim3 threads(THREADS_X, threads_y);

        dim3 blocks(blocks_all[0] * blocks_all[2],
                    blocks_all[1] * blocks_all[3]);

        uint lim = divup(out.dims[dim], (threads_y * blocks_all[dim]));

        switch (threads_y) {
        case 8:
            (scan_dim_kernel<Ti, To, op, dim, isFinalPass, 8>)<<<blocks, threads>>>(
                out, tmp, in, blocks_all[0], blocks_all[1], blocks_all[dim], lim); break;
        case 4:
            (scan_dim_kernel<Ti, To, op, dim, isFinalPass, 4>)<<<blocks, threads>>>(
                out, tmp, in, blocks_all[0], blocks_all[1], blocks_all[dim], lim); break;
        case 2:
            (scan_dim_kernel<Ti, To, op, dim, isFinalPass, 2>)<<<blocks, threads>>>(
                out, tmp, in, blocks_all[0], blocks_all[1], blocks_all[dim], lim); break;
        case 1:
            (scan_dim_kernel<Ti, To, op, dim, isFinalPass, 1>)<<<blocks, threads>>>(
                out, tmp, in, blocks_all[0], blocks_all[1], blocks_all[dim], lim); break;
        }

        POST_LAUNCH_CHECK();
    }

    template<typename To, af_op_t op, int dim>
    static void bcast_dim_launcher(Param<To> out,
                                   CParam<To> tmp,
                                   const uint threads_y,
                                   const uint blocks_all[4])
    {

        dim3 threads(THREADS_X, threads_y);

        dim3 blocks(blocks_all[0] * blocks_all[2],
                    blocks_all[1] * blocks_all[3]);

        uint lim = divup(out.dims[dim], (threads_y * blocks_all[dim]));

        (bcast_dim_kernel<To, op, dim>)<<<blocks, threads>>>(
            out, tmp, blocks_all[0], blocks_all[1], blocks_all[dim], lim);

        POST_LAUNCH_CHECK();
    }

    template<typename Ti, typename To, af_op_t op, int dim>
    static void scan_dim(Param<To> out, CParam<Ti> in)
    {
        uint threads_y = std::min(THREADS_Y, nextpow2(out.dims[dim]));
        uint threads_x = THREADS_X;

        uint blocks_all[] = {divup(out.dims[0], threads_x),
                             out.dims[1], out.dims[2], out.dims[3]};

        blocks_all[dim] = divup(out.dims[dim], threads_y * REPEAT);

        if (blocks_all[dim] == 1) {

            scan_dim_launcher<Ti, To, op, dim, true>(out, out, in,
                                                     threads_y,
                                                     blocks_all);

        } else {

            Param<To> tmp = out;

            tmp.dims[dim] = blocks_all[dim];
            tmp.strides[0] = 1;
            for (int k = 1; k < 4; k++) tmp.strides[k] = tmp.strides[k - 1] * tmp.dims[k - 1];

            dim_type tmp_elements = tmp.strides[3] * tmp.dims[3];
            tmp.ptr = memAlloc<To>(tmp_elements);

            scan_dim_launcher<Ti, To, op, dim, false>(out, tmp, in,
                                                      threads_y,
                                                      blocks_all);

            int bdim = blocks_all[dim];
            blocks_all[dim] = 1;

            //FIXME: Is there an alternative to the if condition ?
            if (op == af_notzero_t) {
                scan_dim_launcher<To, To, af_add_t, dim, true>(tmp, tmp, tmp,
                                                               threads_y,
                                                               blocks_all);
            } else {
                scan_dim_launcher<To, To,       op, dim, true>(tmp, tmp, tmp,
                                                               threads_y,
                                                               blocks_all);
            }

            blocks_all[dim] = bdim;
            bcast_dim_launcher<To, op, dim>(out, tmp, threads_y, blocks_all);

            memFree(tmp.ptr);
        }
    }

}
}
<|endoftext|>


<commit_before>#include <string>
#include <iostream>
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <pcl_ros/transforms.h>
#include <nav_msgs/OccupancyGrid.h>
#include <visualization_msgs/MarkerArray.h>
#include <unordered_set>
#include <gpregressor.h>
#include "bgkoctomap.h"
#include "markerarray_pub.h"

tf::TransformListener *listener;
std::string frame_id("/map");
double resolution = 0.1;
double free_resolution = 0.1;
double max_range = 8.0;
bool original_size = false;
bgkoctomap::BGKOctoMap *map;

bgkoctomap::MarkerArrayPub *m_pub;;
int count = 0;

tf::Vector3 last_position;
tf::Quaternion last_orientation;
bool first = true;
double position_change_thresh = 0.1;
double orientation_change_thresh = 0.2;
bool updated = false;

bgkoctomap::MarkerArrayPub *f_pub;
bgkoctomap::MarkerArrayPub *ray_pub;

ros::Publisher grid_pub;
nav_msgs::OccupancyGrid grid;

// bgkoctomap::MarkerArrayPub *ig_pub;
bgkoctomap::MarkerArrayPub *colorbar_pub;

ros::Publisher arrow_pub;
visualization_msgs::MarkerArray arrow_msg;

struct pair_hash {
    inline std::size_t operator()(const std::pair<bgkoctomap::BlockHashKey, bgkoctomap::OcTreeHashKey> &p) const {
        std::hash<bgkoctomap::BlockHashKey> b_hash;
        std::hash<bgkoctomap::OcTreeHashKey > n_hash;
        return b_hash(p.first) ^ n_hash(p.second);
    }
};

void publish_project_2d_map(const bgkoctomap::BGKOctoMap &map) {
    ROS_INFO_STREAM("Projecting 2D map...");
    bgkoctomap::point3f lim_min, lim_max;
    map.get_bbox(lim_min, lim_max);
    float resolution = map.get_resolution();
    unsigned short lim = static_cast<unsigned short>(pow(2, map.get_block_depth() - 1));
    unsigned short block_width = static_cast<unsigned short>((lim_max.x() - lim_min.x() + 0.01) / map.get_block_size());
    unsigned short block_height = static_cast<unsigned short>((lim_max.y() - lim_min.y() + 0.01) /
                                                              map.get_block_size());
    unsigned int width = lim * block_width;
    unsigned int height = lim * block_height;
    grid.info.width = width;
    grid.info.height = height;
    grid.info.origin.position.x = lim_min.x();
    grid.info.origin.position.y = lim_min.y();
    grid.info.resolution = resolution;
    grid.header.frame_id = frame_id;
    grid.header.stamp = ros::Time::now();
    float z0 = 0.05f, z_sensor = 0.35f;

    std::vector<bgkoctomap::point3f> candidates;
    grid.data = std::vector<int8_t>(width * height, -1);
    for (unsigned short bi = 0; bi < block_height; ++bi) {
        float y = lim_min.y() + (bi + 0.5f) * map.get_block_size();
        for (unsigned short bj = 0; bj < block_width; ++bj) {
            float x = lim_min.x() + (bj + 0.5f) * map.get_block_size();
            bgkoctomap::Block *block = map.search(bgkoctomap::block_to_hash_key(x, y, z0));
            if (block == nullptr)
                continue;

            unsigned short ix, iy, iz;
            block->get_index(bgkoctomap::point3f(x, y, z0), ix, iy, iz);
            for (unsigned short i = 0; i < lim; ++i) {
                for (unsigned short j = 0; j < lim; ++j) {
                    bgkoctomap::OcTreeNode &node = (*block)[block->get_node(j, i, iz)];
                    int index = (bi * lim + i) * width + bj * lim + j;
                    if (node.get_state() == bgkoctomap::State::FREE) {
                        grid.data[index] = 0;
                        candidates.emplace_back(lim_min.x() + resolution * (bj * lim + j + 0.5),
                                                lim_min.y() + resolution * (bi * lim + i + 0.5),
                                                z_sensor + 0.5 * resolution);
                    } else if (node.get_state() == bgkoctomap::State::OCCUPIED)
                        grid.data[index] = 100;
                    else
                        grid.data[index] = -1;
                }
            }
        }
    }
}

void cloud_callback(const sensor_msgs::PointCloud2ConstPtr &cloud) {
    tf::StampedTransform transform;
    try {
        listener->lookupTransform(frame_id, cloud->header.frame_id, cloud->header.stamp, transform);
    } catch (tf::TransformException ex) {
        ROS_ERROR("%s", ex.what());
        return;
    }

    ros::Time start = ros::Time::now();
    bgkoctomap::point3f origin;
    tf::Vector3 translation = transform.getOrigin();
    tf::Quaternion orientation = transform.getRotation();
    if (first || orientation.angleShortestPath(last_orientation) > orientation_change_thresh ||
        translation.distance(last_position) > position_change_thresh) {
        first = false;
        last_position = translation;
        last_orientation = orientation;
        origin.x() = (float) translation.x();
        origin.y() = (float) translation.y();
        origin.z() = (float) translation.z();
        ROS_INFO_STREAM(origin);

        sensor_msgs::PointCloud2 cloud_map;
        pcl_ros::transformPointCloud(frame_id, *cloud, cloud_map, *listener);

        bgkoctomap::PCLPointCloud pcl_cloud;
        pcl::fromROSMsg(cloud_map, pcl_cloud);
        map->insert_pointcloud(pcl_cloud, origin, (float) resolution, (float) free_resolution, (float) max_range);

        ros::Time end = ros::Time::now();
        ROS_INFO_STREAM("One cloud finished in " << (end - start).toSec() << "s");
        updated = true;
    }

    if (count == 0 && updated) {
        bgkoctomap::point3f lim_min, lim_max;
        float min_z, max_z;
        map->get_bbox(lim_min, lim_max);
        min_z = lim_min.z();
        max_z = lim_max.z();
        m_pub->clear();
        for (auto it = map->begin_leaf(); it != map->end_leaf(); ++it) {
            if (it.get_node().get_state() == bgkoctomap::State::OCCUPIED) {
                if (original_size) {
                    bgkoctomap::point3f p = it.get_loc();
                    m_pub->insert_point3d(p.x(), p.y(), p.z(), min_z, max_z, it.get_size());
                } else {
                    auto pruned = it.get_pruned_locs();
                    for (auto n = pruned.cbegin(); n < pruned.cend(); ++n) {
                        m_pub->insert_point3d(n->x(), n->y(), n->z(), min_z, max_z, map->get_resolution());
                    }
                }
            }
        }
        updated = false;

        ///////// Compute Frontiers /////////////////////
        ROS_INFO_STREAM("Computing frontiers");
        f_pub->clear();
        for (auto it = map->begin_leaf(); it != map->end_leaf(); ++it) {
            bgkoctomap::point3f p = it.get_loc();
            if (p.z() > 2.0 || p.z() < 0.3)
                continue;

            if (it.get_node().get_var() > 0.02 &&
                it.get_node().get_prob() < 0.1) {
                f_pub->insert_point3d(p.x(), p.y(), p.z());
            }
        }

        publish_project_2d_map(*map);

        m_pub->publish();
        f_pub->publish();
        grid_pub.publish(grid);
        // ig_pub->publish();
        colorbar_pub->publish();
        arrow_pub.publish(arrow_msg);
    }
    count = (++count) % 10;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "bgkoctomap_server");
    ros::NodeHandle nh;

    std::string cloud_topic("/scan_pc2");
    std::string map_topic("/occupied_cells_vis_array");
    int block_depth = 4;
    double sf2 = 1.0;
    double ell = 1.0;
    double noise = 0.01;
    double l = 100;
    double min_var = 0.001;
    double max_var = 1000;
    double max_known_var = 0.02;
    double ds_resolution = 0.1;
    double free_thresh = 0.3;
    double occupied_thresh = 0.7;
    float var_thresh = 1.0f;
    float prior_A = 1.0f;
    float prior_B = 1.0f;

    nh.param<std::string>("map_topic", map_topic, map_topic);
    nh.param<std::string>("cloud_topic", cloud_topic, cloud_topic);
    nh.param<std::string>("frame_id", frame_id, frame_id);
    nh.param<double>("max_range", max_range, max_range);
    nh.param<double>("resolution", resolution, resolution);
    nh.param<int>("block_depth", block_depth, block_depth);
    nh.param<double>("sf2", sf2, sf2);
    nh.param<double>("ell", ell, ell);
    nh.param<double>("noise", noise, noise);
    nh.param<double>("l", l, l);
    nh.param<double>("min_var", min_var, min_var);
    nh.param<double>("max_var", max_var, max_var);
    nh.param<double>("max_known_var", max_known_var, max_known_var);
    nh.param<double>("free_resolution", free_resolution, free_resolution);
    nh.param<double>("ds_resolution", ds_resolution, ds_resolution);
    nh.param<double>("free_thresh", free_thresh, free_thresh);
    nh.param<double>("occupied_thresh", occupied_thresh, occupied_thresh);
    nh.param<bool>("original_size", original_size, original_size);
    nh.param<float>("var_thresh", var_thresh, var_thresh);
    nh.param<float>("prior_A", prior_A, prior_A);
    nh.param<float>("prior_B", prior_B, prior_B);

    ROS_INFO_STREAM("Parameters:" << std::endl <<
                    "map_topic: " << map_topic << std::endl <<
                    "cloud_topic: " << cloud_topic << std::endl <<
                    "frame_id: " << frame_id << std::endl <<
                    "max_range: " << max_range << std::endl <<
                    "resolution: " << resolution << std::endl <<
                    "block_depth: " << block_depth << std::endl <<
                    "sf2: " << sf2 << std::endl <<
                    "ell: " << ell << std::endl <<
                    "l: " << l << std::endl <<
                    "min_var: " << min_var << std::endl <<
                    "max_var: " << max_var << std::endl <<
                    "max_known_var: " << max_known_var << std::endl <<
                    "free_resolution: " << free_resolution << std::endl <<
                    "ds_resolution: " << ds_resolution << std::endl <<
                    "free_thresh: " << free_thresh << std::endl <<
                    "occupied_thresh: " << occupied_thresh << std::endl <<
                    "original_size: " << original_size << std::endl <<
                    "var_thresh" << var_thresh << std::endl <<
                    "prior_A" << prior_A << std::endl <<
                    "prior_B" << prior_B
    );

    map = new bgkoctomap::BGKOctoMap(resolution, block_depth, sf2, ell, noise, l, min_var, max_var, max_known_var,
                                   free_thresh, occupied_thresh, var_thresh, prior_A, prior_B);

    ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>(cloud_topic, 0, cloud_callback);
    m_pub = new bgkoctomap::MarkerArrayPub(nh, map_topic, 0.1f);
    listener = new tf::TransformListener();

    f_pub = new bgkoctomap::MarkerArrayPub(nh, "frontier_map", resolution);
    ray_pub = new bgkoctomap::MarkerArrayPub(nh, "ray", resolution);
    // ig_pub = new bgkoctomap::MarkerArrayPub(nh, "ig", resolution);
    colorbar_pub = new bgkoctomap::MarkerArrayPub(nh, "colorbar", resolution);

    grid_pub = nh.advertise<nav_msgs::OccupancyGrid>("/map", 0, false);

    arrow_pub = nh.advertise<visualization_msgs::MarkerArray>("/op_orient", 0, true);

    ROS_INFO_STREAM("Start mapping...");
    ros::spin();
    delete map;
    delete listener;
    delete m_pub;
    delete f_pub;
    delete ray_pub;

    return 0;
}
<commit_msg>Fixed spacing in parameter printout<commit_after>#include <string>
#include <iostream>
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <pcl_ros/transforms.h>
#include <nav_msgs/OccupancyGrid.h>
#include <visualization_msgs/MarkerArray.h>
#include <unordered_set>
#include <gpregressor.h>
#include "bgkoctomap.h"
#include "markerarray_pub.h"

tf::TransformListener *listener;
std::string frame_id("/map");
double resolution = 0.1;
double free_resolution = 0.1;
double max_range = 8.0;
bool original_size = false;
bgkoctomap::BGKOctoMap *map;

bgkoctomap::MarkerArrayPub *m_pub;;
int count = 0;

tf::Vector3 last_position;
tf::Quaternion last_orientation;
bool first = true;
double position_change_thresh = 0.1;
double orientation_change_thresh = 0.2;
bool updated = false;

bgkoctomap::MarkerArrayPub *f_pub;
bgkoctomap::MarkerArrayPub *ray_pub;

ros::Publisher grid_pub;
nav_msgs::OccupancyGrid grid;

// bgkoctomap::MarkerArrayPub *ig_pub;
bgkoctomap::MarkerArrayPub *colorbar_pub;

ros::Publisher arrow_pub;
visualization_msgs::MarkerArray arrow_msg;

struct pair_hash {
    inline std::size_t operator()(const std::pair<bgkoctomap::BlockHashKey, bgkoctomap::OcTreeHashKey> &p) const {
        std::hash<bgkoctomap::BlockHashKey> b_hash;
        std::hash<bgkoctomap::OcTreeHashKey > n_hash;
        return b_hash(p.first) ^ n_hash(p.second);
    }
};

void publish_project_2d_map(const bgkoctomap::BGKOctoMap &map) {
    ROS_INFO_STREAM("Projecting 2D map...");
    bgkoctomap::point3f lim_min, lim_max;
    map.get_bbox(lim_min, lim_max);
    float resolution = map.get_resolution();
    unsigned short lim = static_cast<unsigned short>(pow(2, map.get_block_depth() - 1));
    unsigned short block_width = static_cast<unsigned short>((lim_max.x() - lim_min.x() + 0.01) / map.get_block_size());
    unsigned short block_height = static_cast<unsigned short>((lim_max.y() - lim_min.y() + 0.01) /
                                                              map.get_block_size());
    unsigned int width = lim * block_width;
    unsigned int height = lim * block_height;
    grid.info.width = width;
    grid.info.height = height;
    grid.info.origin.position.x = lim_min.x();
    grid.info.origin.position.y = lim_min.y();
    grid.info.resolution = resolution;
    grid.header.frame_id = frame_id;
    grid.header.stamp = ros::Time::now();
    float z0 = 0.05f, z_sensor = 0.35f;

    std::vector<bgkoctomap::point3f> candidates;
    grid.data = std::vector<int8_t>(width * height, -1);
    for (unsigned short bi = 0; bi < block_height; ++bi) {
        float y = lim_min.y() + (bi + 0.5f) * map.get_block_size();
        for (unsigned short bj = 0; bj < block_width; ++bj) {
            float x = lim_min.x() + (bj + 0.5f) * map.get_block_size();
            bgkoctomap::Block *block = map.search(bgkoctomap::block_to_hash_key(x, y, z0));
            if (block == nullptr)
                continue;

            unsigned short ix, iy, iz;
            block->get_index(bgkoctomap::point3f(x, y, z0), ix, iy, iz);
            for (unsigned short i = 0; i < lim; ++i) {
                for (unsigned short j = 0; j < lim; ++j) {
                    bgkoctomap::OcTreeNode &node = (*block)[block->get_node(j, i, iz)];
                    int index = (bi * lim + i) * width + bj * lim + j;
                    if (node.get_state() == bgkoctomap::State::FREE) {
                        grid.data[index] = 0;
                        candidates.emplace_back(lim_min.x() + resolution * (bj * lim + j + 0.5),
                                                lim_min.y() + resolution * (bi * lim + i + 0.5),
                                                z_sensor + 0.5 * resolution);
                    } else if (node.get_state() == bgkoctomap::State::OCCUPIED)
                        grid.data[index] = 100;
                    else
                        grid.data[index] = -1;
                }
            }
        }
    }
}

void cloud_callback(const sensor_msgs::PointCloud2ConstPtr &cloud) {
    tf::StampedTransform transform;
    try {
        listener->lookupTransform(frame_id, cloud->header.frame_id, cloud->header.stamp, transform);
    } catch (tf::TransformException ex) {
        ROS_ERROR("%s", ex.what());
        return;
    }

    ros::Time start = ros::Time::now();
    bgkoctomap::point3f origin;
    tf::Vector3 translation = transform.getOrigin();
    tf::Quaternion orientation = transform.getRotation();
    if (first || orientation.angleShortestPath(last_orientation) > orientation_change_thresh ||
        translation.distance(last_position) > position_change_thresh) {
        first = false;
        last_position = translation;
        last_orientation = orientation;
        origin.x() = (float) translation.x();
        origin.y() = (float) translation.y();
        origin.z() = (float) translation.z();
        ROS_INFO_STREAM(origin);

        sensor_msgs::PointCloud2 cloud_map;
        pcl_ros::transformPointCloud(frame_id, *cloud, cloud_map, *listener);

        bgkoctomap::PCLPointCloud pcl_cloud;
        pcl::fromROSMsg(cloud_map, pcl_cloud);
        map->insert_pointcloud(pcl_cloud, origin, (float) resolution, (float) free_resolution, (float) max_range);

        ros::Time end = ros::Time::now();
        ROS_INFO_STREAM("One cloud finished in " << (end - start).toSec() << "s");
        updated = true;
    }

    if (count == 0 && updated) {
        bgkoctomap::point3f lim_min, lim_max;
        float min_z, max_z;
        map->get_bbox(lim_min, lim_max);
        min_z = lim_min.z();
        max_z = lim_max.z();
        m_pub->clear();
        for (auto it = map->begin_leaf(); it != map->end_leaf(); ++it) {
            if (it.get_node().get_state() == bgkoctomap::State::OCCUPIED) {
                if (original_size) {
                    bgkoctomap::point3f p = it.get_loc();
                    m_pub->insert_point3d(p.x(), p.y(), p.z(), min_z, max_z, it.get_size());
                } else {
                    auto pruned = it.get_pruned_locs();
                    for (auto n = pruned.cbegin(); n < pruned.cend(); ++n) {
                        m_pub->insert_point3d(n->x(), n->y(), n->z(), min_z, max_z, map->get_resolution());
                    }
                }
            }
        }
        updated = false;

        ///////// Compute Frontiers /////////////////////
        ROS_INFO_STREAM("Computing frontiers");
        f_pub->clear();
        for (auto it = map->begin_leaf(); it != map->end_leaf(); ++it) {
            bgkoctomap::point3f p = it.get_loc();
            if (p.z() > 2.0 || p.z() < 0.3)
                continue;

            if (it.get_node().get_var() > 0.02 &&
                it.get_node().get_prob() < 0.1) {
                f_pub->insert_point3d(p.x(), p.y(), p.z());
            }
        }

        publish_project_2d_map(*map);

        m_pub->publish();
        f_pub->publish();
        grid_pub.publish(grid);
        // ig_pub->publish();
        colorbar_pub->publish();
        arrow_pub.publish(arrow_msg);
    }
    count = (++count) % 10;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "bgkoctomap_server");
    ros::NodeHandle nh;

    std::string cloud_topic("/scan_pc2");
    std::string map_topic("/occupied_cells_vis_array");
    int block_depth = 4;
    double sf2 = 1.0;
    double ell = 1.0;
    double noise = 0.01;
    double l = 100;
    double min_var = 0.001;
    double max_var = 1000;
    double max_known_var = 0.02;
    double ds_resolution = 0.1;
    double free_thresh = 0.3;
    double occupied_thresh = 0.7;
    float var_thresh = 1.0f;
    float prior_A = 1.0f;
    float prior_B = 1.0f;

    nh.param<std::string>("map_topic", map_topic, map_topic);
    nh.param<std::string>("cloud_topic", cloud_topic, cloud_topic);
    nh.param<std::string>("frame_id", frame_id, frame_id);
    nh.param<double>("max_range", max_range, max_range);
    nh.param<double>("resolution", resolution, resolution);
    nh.param<int>("block_depth", block_depth, block_depth);
    nh.param<double>("sf2", sf2, sf2);
    nh.param<double>("ell", ell, ell);
    nh.param<double>("noise", noise, noise);
    nh.param<double>("l", l, l);
    nh.param<double>("min_var", min_var, min_var);
    nh.param<double>("max_var", max_var, max_var);
    nh.param<double>("max_known_var", max_known_var, max_known_var);
    nh.param<double>("free_resolution", free_resolution, free_resolution);
    nh.param<double>("ds_resolution", ds_resolution, ds_resolution);
    nh.param<double>("free_thresh", free_thresh, free_thresh);
    nh.param<double>("occupied_thresh", occupied_thresh, occupied_thresh);
    nh.param<bool>("original_size", original_size, original_size);
    nh.param<float>("var_thresh", var_thresh, var_thresh);
    nh.param<float>("prior_A", prior_A, prior_A);
    nh.param<float>("prior_B", prior_B, prior_B);

    ROS_INFO_STREAM("Parameters:" << std::endl <<
                    "map_topic: " << map_topic << std::endl <<
                    "cloud_topic: " << cloud_topic << std::endl <<
                    "frame_id: " << frame_id << std::endl <<
                    "max_range: " << max_range << std::endl <<
                    "resolution: " << resolution << std::endl <<
                    "block_depth: " << block_depth << std::endl <<
                    "sf2: " << sf2 << std::endl <<
                    "ell: " << ell << std::endl <<
                    "l: " << l << std::endl <<
                    "min_var: " << min_var << std::endl <<
                    "max_var: " << max_var << std::endl <<
                    "max_known_var: " << max_known_var << std::endl <<
                    "free_resolution: " << free_resolution << std::endl <<
                    "ds_resolution: " << ds_resolution << std::endl <<
                    "free_thresh: " << free_thresh << std::endl <<
                    "occupied_thresh: " << occupied_thresh << std::endl <<
                    "original_size: " << original_size << std::endl <<
                    "var_thresh: " << var_thresh << std::endl <<
                    "prior_A: " << prior_A << std::endl <<
                    "prior_B: " << prior_B
    );

    map = new bgkoctomap::BGKOctoMap(resolution, block_depth, sf2, ell, noise, l, min_var, max_var, max_known_var,
                                   free_thresh, occupied_thresh, var_thresh, prior_A, prior_B);

    ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>(cloud_topic, 0, cloud_callback);
    m_pub = new bgkoctomap::MarkerArrayPub(nh, map_topic, 0.1f);
    listener = new tf::TransformListener();

    f_pub = new bgkoctomap::MarkerArrayPub(nh, "frontier_map", resolution);
    ray_pub = new bgkoctomap::MarkerArrayPub(nh, "ray", resolution);
    // ig_pub = new bgkoctomap::MarkerArrayPub(nh, "ig", resolution);
    colorbar_pub = new bgkoctomap::MarkerArrayPub(nh, "colorbar", resolution);

    grid_pub = nh.advertise<nav_msgs::OccupancyGrid>("/map", 0, false);

    arrow_pub = nh.advertise<visualization_msgs::MarkerArray>("/op_orient", 0, true);

    ROS_INFO_STREAM("Start mapping...");
    ros::spin();
    delete map;
    delete listener;
    delete m_pub;
    delete f_pub;
    delete ray_pub;

    return 0;
}
<|endoftext|>


<commit_before>// Copyright (c) Facebook, Inc. and its affiliates.

// This source code is licensed under the MIT license found in the
// LICENSE file in the root directory of this source tree.

#include "ShadowTree.h"

#include <react/core/LayoutContext.h>
#include <react/core/LayoutPrimitives.h>
#include <react/debug/SystraceSection.h>
#include <react/mounting/Differentiator.h>
#include <react/mounting/ShadowViewMutation.h>
#include <react/uimanager/TimeUtils.h>

#include "ShadowTreeDelegate.h"

namespace facebook {
namespace react {

static void updateMountedFlag(
    const SharedShadowNodeList &oldChildren,
    const SharedShadowNodeList &newChildren) {
  // This is a simplified version of Diffing algorithm that only updates
  // `mounted` flag on `ShadowNode`s. The algorithm sets "mounted" flag before
  // "unmounted" to allow `ShadowNode` detect a situation where the node was
  // remounted.

  if (&oldChildren == &newChildren) {
    // Lists are identical, nothing to do.
    return;
  }

  if (oldChildren.size() == 0 && newChildren.size() == 0) {
    // Both lists are empty, nothing to do.
    return;
  }

  int index;

  // Stage 1: Mount and unmount "updated" children.
  for (index = 0; index < oldChildren.size() && index < newChildren.size();
       index++) {
    const auto &oldChild = oldChildren[index];
    const auto &newChild = newChildren[index];

    if (oldChild == newChild) {
      // Nodes are identical, skipping the subtree.
      continue;
    }

    if (oldChild->getTag() != newChild->getTag()) {
      // Totally different nodes, updating is impossible.
      break;
    }

    newChild->setMounted(true);
    oldChild->setMounted(false);

    updateMountedFlag(oldChild->getChildren(), newChild->getChildren());
  }

  int lastIndexAfterFirstStage = index;

  // State 2: Mount new children.
  for (index = lastIndexAfterFirstStage; index < newChildren.size(); index++) {
    const auto &newChild = newChildren[index];
    newChild->setMounted(true);
    updateMountedFlag({}, newChild->getChildren());
  }

  // State 3: Unmount old children.
  for (index = lastIndexAfterFirstStage; index < oldChildren.size(); index++) {
    const auto &oldChild = oldChildren[index];
    oldChild->setMounted(false);
    updateMountedFlag(oldChild->getChildren(), {});
  }
}

ShadowTree::ShadowTree(
    SurfaceId surfaceId,
    const LayoutConstraints &layoutConstraints,
    const LayoutContext &layoutContext)
    : surfaceId_(surfaceId) {
  const auto noopEventEmitter = std::make_shared<const ViewEventEmitter>(
      nullptr, -1, std::shared_ptr<const EventDispatcher>());

  const auto props = std::make_shared<const RootProps>(
      *RootShadowNode::defaultSharedProps(), layoutConstraints, layoutContext);

  rootShadowNode_ = std::make_shared<RootShadowNode>(
      ShadowNodeFragment{
          .tag = surfaceId,
          .rootTag = surfaceId,
          .props = props,
          .eventEmitter = noopEventEmitter,
      },
      nullptr);
}

ShadowTree::~ShadowTree() {
  commit(
      [](const SharedRootShadowNode &oldRootShadowNode) {
        return std::make_shared<RootShadowNode>(
            *oldRootShadowNode,
            ShadowNodeFragment{
                .children = ShadowNode::emptySharedShadowNodeSharedList()});
      },
      getTime());
}

Tag ShadowTree::getSurfaceId() const {
  return surfaceId_;
}

void ShadowTree::commit(
    ShadowTreeCommitTransaction transaction,
    long commitStartTime,
    int *revision) const {
  SystraceSection s("ShadowTree::commit");

  int attempts = 0;

  while (true) {
    attempts++;
    if (tryCommit(transaction, commitStartTime, revision)) {
      return;
    }

    // After multiple attempts, we failed to commit the transaction.
    // Something internally went terribly wrong.
    assert(attempts < 1024);
  }
}

bool ShadowTree::tryCommit(
    ShadowTreeCommitTransaction transaction,
    long commitStartTime,
    int *revision) const {
  SystraceSection s("ShadowTree::tryCommit");

  SharedRootShadowNode oldRootShadowNode;

  {
    // Reading `rootShadowNode_` in shared manner.
    std::shared_lock<folly::SharedMutex> lock(commitMutex_);
    oldRootShadowNode = rootShadowNode_;
  }

  UnsharedRootShadowNode newRootShadowNode = transaction(oldRootShadowNode);

  if (!newRootShadowNode) {
    return false;
  }

  long layoutTime = getTime();
  newRootShadowNode->layout();
  layoutTime = getTime() - layoutTime;
  newRootShadowNode->sealRecursive();

  auto mutations =
      calculateShadowViewMutations(*oldRootShadowNode, *newRootShadowNode);

  {
    // Updating `rootShadowNode_` in unique manner if it hasn't changed.
    std::unique_lock<folly::SharedMutex> lock(commitMutex_);

    if (rootShadowNode_ != oldRootShadowNode) {
      return false;
    }

    rootShadowNode_ = newRootShadowNode;

    {
      std::lock_guard<std::mutex> dispatchLock(EventEmitter::DispatchMutex());

      updateMountedFlag(
          oldRootShadowNode->getChildren(), newRootShadowNode->getChildren());
    }

    revision_++;

    // Returning last revision if requested.
    if (revision) {
      *revision = revision_;
    }
  }

  emitLayoutEvents(mutations);

  if (delegate_) {
    delegate_->shadowTreeDidCommit(
        *this, mutations, commitStartTime, layoutTime);
  }

  return true;
}

void ShadowTree::emitLayoutEvents(
    const ShadowViewMutationList &mutations) const {
  SystraceSection s("ShadowTree::emitLayoutEvents");

  for (const auto &mutation : mutations) {
    // Only `Insert` and `Update` mutations can affect layout metrics.
    if (mutation.type != ShadowViewMutation::Insert &&
        mutation.type != ShadowViewMutation::Update) {
      continue;
    }

    const auto viewEventEmitter =
        std::dynamic_pointer_cast<const ViewEventEmitter>(
            mutation.newChildShadowView.eventEmitter);

    // Checking if particular shadow node supports `onLayout` event (part of
    // `ViewEventEmitter`).
    if (!viewEventEmitter) {
      continue;
    }

    // Checking if the `onLayout` event was requested for the particular Shadow
    // Node.
    const auto viewProps = std::dynamic_pointer_cast<const ViewProps>(
        mutation.newChildShadowView.props);
    if (viewProps && !viewProps->onLayout) {
      continue;
    }

    // In case if we have `oldChildShadowView`, checking that layout metrics
    // have changed.
    if (mutation.type != ShadowViewMutation::Update &&
        mutation.oldChildShadowView.layoutMetrics ==
            mutation.newChildShadowView.layoutMetrics) {
      continue;
    }

    viewEventEmitter->onLayout(mutation.newChildShadowView.layoutMetrics);
  }
}

#pragma mark - Delegate

void ShadowTree::setDelegate(ShadowTreeDelegate const *delegate) {
  delegate_ = delegate;
}

ShadowTreeDelegate const *ShadowTree::getDelegate() const {
  return delegate_;
}

} // namespace react
} // namespace facebook
<commit_msg>Fabric: Clone function for RootShadowNode<commit_after>// Copyright (c) Facebook, Inc. and its affiliates.

// This source code is licensed under the MIT license found in the
// LICENSE file in the root directory of this source tree.

#include "ShadowTree.h"

#include <react/core/LayoutContext.h>
#include <react/core/LayoutPrimitives.h>
#include <react/debug/SystraceSection.h>
#include <react/mounting/Differentiator.h>
#include <react/mounting/ShadowViewMutation.h>
#include <react/uimanager/TimeUtils.h>

#include "ShadowTreeDelegate.h"

namespace facebook {
namespace react {

static void updateMountedFlag(
    const SharedShadowNodeList &oldChildren,
    const SharedShadowNodeList &newChildren) {
  // This is a simplified version of Diffing algorithm that only updates
  // `mounted` flag on `ShadowNode`s. The algorithm sets "mounted" flag before
  // "unmounted" to allow `ShadowNode` detect a situation where the node was
  // remounted.

  if (&oldChildren == &newChildren) {
    // Lists are identical, nothing to do.
    return;
  }

  if (oldChildren.size() == 0 && newChildren.size() == 0) {
    // Both lists are empty, nothing to do.
    return;
  }

  int index;

  // Stage 1: Mount and unmount "updated" children.
  for (index = 0; index < oldChildren.size() && index < newChildren.size();
       index++) {
    const auto &oldChild = oldChildren[index];
    const auto &newChild = newChildren[index];

    if (oldChild == newChild) {
      // Nodes are identical, skipping the subtree.
      continue;
    }

    if (oldChild->getTag() != newChild->getTag()) {
      // Totally different nodes, updating is impossible.
      break;
    }

    newChild->setMounted(true);
    oldChild->setMounted(false);

    updateMountedFlag(oldChild->getChildren(), newChild->getChildren());
  }

  int lastIndexAfterFirstStage = index;

  // State 2: Mount new children.
  for (index = lastIndexAfterFirstStage; index < newChildren.size(); index++) {
    const auto &newChild = newChildren[index];
    newChild->setMounted(true);
    updateMountedFlag({}, newChild->getChildren());
  }

  // State 3: Unmount old children.
  for (index = lastIndexAfterFirstStage; index < oldChildren.size(); index++) {
    const auto &oldChild = oldChildren[index];
    oldChild->setMounted(false);
    updateMountedFlag(oldChild->getChildren(), {});
  }
}

ShadowTree::ShadowTree(
    SurfaceId surfaceId,
    const LayoutConstraints &layoutConstraints,
    const LayoutContext &layoutContext)
    : surfaceId_(surfaceId) {
  const auto noopEventEmitter = std::make_shared<const ViewEventEmitter>(
      nullptr, -1, std::shared_ptr<const EventDispatcher>());

  const auto props = std::make_shared<const RootProps>(
      *RootShadowNode::defaultSharedProps(), layoutConstraints, layoutContext);

  rootShadowNode_ = std::make_shared<RootShadowNode>(
      ShadowNodeFragment{
          .tag = surfaceId,
          .rootTag = surfaceId,
          .props = props,
          .eventEmitter = noopEventEmitter,
      },
      [](const ShadowNode &shadowNode, const ShadowNodeFragment &fragment) {
        return std::make_shared<RootShadowNode>(shadowNode, fragment);
      });
}

ShadowTree::~ShadowTree() {
  commit(
      [](const SharedRootShadowNode &oldRootShadowNode) {
        return std::make_shared<RootShadowNode>(
            *oldRootShadowNode,
            ShadowNodeFragment{
                .children = ShadowNode::emptySharedShadowNodeSharedList()});
      },
      getTime());
}

Tag ShadowTree::getSurfaceId() const {
  return surfaceId_;
}

void ShadowTree::commit(
    ShadowTreeCommitTransaction transaction,
    long commitStartTime,
    int *revision) const {
  SystraceSection s("ShadowTree::commit");

  int attempts = 0;

  while (true) {
    attempts++;
    if (tryCommit(transaction, commitStartTime, revision)) {
      return;
    }

    // After multiple attempts, we failed to commit the transaction.
    // Something internally went terribly wrong.
    assert(attempts < 1024);
  }
}

bool ShadowTree::tryCommit(
    ShadowTreeCommitTransaction transaction,
    long commitStartTime,
    int *revision) const {
  SystraceSection s("ShadowTree::tryCommit");

  SharedRootShadowNode oldRootShadowNode;

  {
    // Reading `rootShadowNode_` in shared manner.
    std::shared_lock<folly::SharedMutex> lock(commitMutex_);
    oldRootShadowNode = rootShadowNode_;
  }

  UnsharedRootShadowNode newRootShadowNode = transaction(oldRootShadowNode);

  if (!newRootShadowNode) {
    return false;
  }

  long layoutTime = getTime();
  newRootShadowNode->layout();
  layoutTime = getTime() - layoutTime;
  newRootShadowNode->sealRecursive();

  auto mutations =
      calculateShadowViewMutations(*oldRootShadowNode, *newRootShadowNode);

  {
    // Updating `rootShadowNode_` in unique manner if it hasn't changed.
    std::unique_lock<folly::SharedMutex> lock(commitMutex_);

    if (rootShadowNode_ != oldRootShadowNode) {
      return false;
    }

    rootShadowNode_ = newRootShadowNode;

    {
      std::lock_guard<std::mutex> dispatchLock(EventEmitter::DispatchMutex());

      updateMountedFlag(
          oldRootShadowNode->getChildren(), newRootShadowNode->getChildren());
    }

    revision_++;

    // Returning last revision if requested.
    if (revision) {
      *revision = revision_;
    }
  }

  emitLayoutEvents(mutations);

  if (delegate_) {
    delegate_->shadowTreeDidCommit(
        *this, mutations, commitStartTime, layoutTime);
  }

  return true;
}

void ShadowTree::emitLayoutEvents(
    const ShadowViewMutationList &mutations) const {
  SystraceSection s("ShadowTree::emitLayoutEvents");

  for (const auto &mutation : mutations) {
    // Only `Insert` and `Update` mutations can affect layout metrics.
    if (mutation.type != ShadowViewMutation::Insert &&
        mutation.type != ShadowViewMutation::Update) {
      continue;
    }

    const auto viewEventEmitter =
        std::dynamic_pointer_cast<const ViewEventEmitter>(
            mutation.newChildShadowView.eventEmitter);

    // Checking if particular shadow node supports `onLayout` event (part of
    // `ViewEventEmitter`).
    if (!viewEventEmitter) {
      continue;
    }

    // Checking if the `onLayout` event was requested for the particular Shadow
    // Node.
    const auto viewProps = std::dynamic_pointer_cast<const ViewProps>(
        mutation.newChildShadowView.props);
    if (viewProps && !viewProps->onLayout) {
      continue;
    }

    // In case if we have `oldChildShadowView`, checking that layout metrics
    // have changed.
    if (mutation.type != ShadowViewMutation::Update &&
        mutation.oldChildShadowView.layoutMetrics ==
            mutation.newChildShadowView.layoutMetrics) {
      continue;
    }

    viewEventEmitter->onLayout(mutation.newChildShadowView.layoutMetrics);
  }
}

#pragma mark - Delegate

void ShadowTree::setDelegate(ShadowTreeDelegate const *delegate) {
  delegate_ = delegate;
}

ShadowTreeDelegate const *ShadowTree::getDelegate() const {
  return delegate_;
}

} // namespace react
} // namespace facebook
<|endoftext|>


<commit_before>// -*- mode: c++; coding: utf-8 -*-
/*! @file cell.cpp
    @brief Implementation of Cell class
*/
#include "cell.h"

#include <iostream>
#include <sstream>

#include <boost/program_options.hpp>

#include <cxxwtils/prandom.hpp>
#include <cxxwtils/iostr.hpp>

double Cell::MUTATION_RATE_ = 1e-1;
double Cell::MUTATION_SIGMA_ = 0.0;
int Cell::DRIVER_EFFECTS_ = 1;
double Cell::DRIVER_FRACTION_ = 0.0;
double Cell::BIRTH_RATE_ = 1.0;
double Cell::DEATH_RATE_ = 0.0;
double Cell::MIGRATION_RATE_ = 0.0;
double Cell::GAMMA_SHAPE_ = 1.0;
double Cell::PROB_SYMMETRIC_DIVISION_ = 1.0;
size_t Cell::MAX_PROLIFERATION_CAPACITY_ = 10;
size_t Cell::ID_TAIL_ = 0;

std::vector<double> Cell::MUTATION_EFFECTS_;
std::vector<size_t> Cell::MUTANT_IDS_;

//! Program options
/*! @return Program options description

    Command line option | Symbol                  | Variable
    --------------------| ----------------------- | -------------------------
    `-u,--mutation`     | \f$\mu\f$               | Cell::MUTATION_RATE_
    `-s,--sigma`        | \f$\sigma\f$            | Cell::MUTATION_SIGMA_
    `-e,--effect`       |                         | Cell::DRIVER_EFFECTS_
    `-f,--fraction`     |                         | Cell::DRIVER_FRACTION_
    `-b,--birth`        | \f$\beta_0\f$           | Cell::BIRTH_RATE_
    `-d,--death`        | \f$\delta_0\f$          | Cell::DEATH_RATE_
    `-m,--migration`    | \f$m\f$                 | Cell::MIGRATION_RATE_
    `-p,--symmetric`    | \f$p_s\f$               | Cell::PROB_SYMMETRIC_DIVISION_
    `-r,--prolif`       | \f$\rho_\mathrm{max}\f$ | Cell::PROB_SYMMETRIC_DIVISION_
    `-k,--shape`        | \f$k\f$                 | Cell::GAMMA_SHAPE_
*/
boost::program_options::options_description& Cell::opt_description() {
    namespace po = boost::program_options;
    static po::options_description desc{"Cell"};
    desc.add_options()
        ("mutation,u", po::value<double>(&MUTATION_RATE_)->default_value(MUTATION_RATE_))
        ("sigma,s", po::value<double>(&MUTATION_SIGMA_)->default_value(MUTATION_SIGMA_))
        ("effect,e", po::value<int>(&DRIVER_EFFECTS_)->default_value(DRIVER_EFFECTS_))
        ("fraction,f", po::value<double>(&DRIVER_FRACTION_)->default_value(DRIVER_FRACTION_))
        ("birth,b", po::value<double>(&BIRTH_RATE_)->default_value(BIRTH_RATE_))
        ("death,d", po::value<double>(&DEATH_RATE_)->default_value(DEATH_RATE_))
        ("migration,m", po::value<double>(&MIGRATION_RATE_)->default_value(MIGRATION_RATE_))
        ("shape,k", po::value<double>(&GAMMA_SHAPE_)->default_value(GAMMA_SHAPE_))
        ("symmetric,p", po::value<double>(&PROB_SYMMETRIC_DIVISION_)->default_value(PROB_SYMMETRIC_DIVISION_))
        ("prolif,r", po::value<size_t>(&MAX_PROLIFERATION_CAPACITY_)->default_value(MAX_PROLIFERATION_CAPACITY_))
    ;
    return desc;
}

Cell::Cell(const Cell& other):
    coord_(other.coord_), sites_(other.sites_),
    birth_rate_(other.birth_rate_), death_rate_(other.death_rate_),
    migra_rate_(other.migra_rate_),
    type_(other.type_), proliferation_capacity_(other.proliferation_capacity_),
    id_(++ID_TAIL_), ancestors_(other.ancestors_) {
    ancestors_.push_back(other.id_);
    if (type_ == CellType::stem) {
        if (!std::bernoulli_distribution(PROB_SYMMETRIC_DIVISION_)(wtl::sfmt())) {
            type_ = CellType::nonstem;
        }
    }
}

void Cell::mutate() {
    static std::bernoulli_distribution bernoulli_driver(DRIVER_FRACTION_);
    static std::normal_distribution<double> normal_sigma(0.0, MUTATION_SIGMA_);
    double effect = 0.0;
    if (DRIVER_FRACTION_ > 0.0) {
        if (bernoulli_driver(wtl::sfmt())) {
            effect = MUTATION_SIGMA_;
        }
    } else {
        effect = normal_sigma(wtl::sfmt());
    }
    sites_.push_back(MUTATION_EFFECTS_.size());
    MUTATION_EFFECTS_.push_back(effect);
    MUTANT_IDS_.push_back(id());
    if (DRIVER_EFFECTS_ & 0b001) {
        birth_rate_ *= (1.0 + effect);
    }
    if (DRIVER_EFFECTS_ & 0b010) {
        death_rate_ *= (1.0 - effect);
    }
    if (DRIVER_EFFECTS_ & 0b100) {
        migra_rate_ *= (1.0 + effect);
    }
}

double Cell::delta_time(const double positional_value) {
    double t_birth = std::numeric_limits<double>::infinity();
    if (proliferation_capacity_ > 0) {
        double theta = 1.0;
        theta /= birth_rate_;
        theta /= positional_value;
        theta /= GAMMA_SHAPE_;
        std::gamma_distribution<double> gamma(GAMMA_SHAPE_, theta);
        t_birth = gamma(wtl::sfmt());
    }
    std::exponential_distribution<double> exponential_death(death_rate_);
    std::exponential_distribution<double> exponential_migra(migra_rate_);
    const double t_death = exponential_death(wtl::sfmt());
    const double t_migra = exponential_migra(wtl::sfmt());

    if (t_birth < t_death && t_birth < t_migra) {
        next_event_ = Event::birth;
        return t_birth;
    } else if (t_death < t_migra) {
        next_event_ = Event::death;
        return t_death;
    } else {
        next_event_ = Event::migration;
        return t_migra;
    }
}

std::vector<size_t> Cell::haplotype(std::vector<size_t> segsites) const {
    size_t i = 0;
    for (auto& x: segsites) {
        if (i >= sites_.size() || x < sites_[i]) {
            x = 0;
        } else {
            x = 1;
            ++i;
        }
    }
    return segsites;
}

std::string Cell::header(const size_t dimensions, const char* sep) {
    std::vector<std::string> axes{"x", "y", "z"};
    axes.resize(dimensions);
    std::ostringstream oss;
    oss << "id" << sep << "ancestors" << sep
        << "birth" << sep << "death" << sep
        << wtl::join(axes, sep) << sep << "sites" << sep
        << "beta" << sep << "delta" << sep << "rho\n";
    return oss.str();
}

std::ostream& Cell::write(std::ostream& ost, const char* sep) const {
    return ost << id_ << sep << wtl::join(ancestors_, ":") << sep
        << time_of_birth_ << sep << time_of_death_ << sep
        << wtl::join(coord(), sep) << sep
        << wtl::join(sites(), ":") << sep
        << birth_rate_ << sep
        << death_rate_ << sep
        << migra_rate_ << "\n";
}

std::string Cell::str(const char* sep) const {
    std::ostringstream oss;
    write(oss, sep);
    return oss.str();
}

//! Stream operator for debug print
std::ostream& operator<< (std::ostream& ost, const Cell& x) {
    return x.write(ost, "\t");
}

void Cell::unit_test() {
    std::cerr << __PRETTY_FUNCTION__ << std::endl;
    std::cerr.precision(15);
    Cell cell({1, 2, 3});
    std::cerr << Cell::header(3, "\t") << "\n" << cell << std::endl;
}
<commit_msg>doxygen comment<commit_after>// -*- mode: c++; coding: utf-8 -*-
/*! @file cell.cpp
    @brief Implementation of Cell class
*/
#include "cell.h"

#include <iostream>
#include <sstream>

#include <boost/program_options.hpp>

#include <cxxwtils/prandom.hpp>
#include <cxxwtils/iostr.hpp>

double Cell::MUTATION_RATE_ = 1e-1;
double Cell::MUTATION_SIGMA_ = 0.0;
int Cell::DRIVER_EFFECTS_ = 1;
double Cell::DRIVER_FRACTION_ = 0.0;
double Cell::BIRTH_RATE_ = 1.0;
double Cell::DEATH_RATE_ = 0.0;
double Cell::MIGRATION_RATE_ = 0.0;
double Cell::GAMMA_SHAPE_ = 1.0;
double Cell::PROB_SYMMETRIC_DIVISION_ = 1.0;
size_t Cell::MAX_PROLIFERATION_CAPACITY_ = 10;
size_t Cell::ID_TAIL_ = 0;

std::vector<double> Cell::MUTATION_EFFECTS_;
std::vector<size_t> Cell::MUTANT_IDS_;

//! Program options
/*! @return Program options description

    Command line option | Symbol                  | Variable
    --------------------| ----------------------- | -------------------------
    `-u,--mutation`     | \f$\mu\f$               | Cell::MUTATION_RATE_
    `-s,--sigma`        | \f$\sigma_s\f$          | Cell::MUTATION_SIGMA_
    `-e,--effect`       |                         | Cell::DRIVER_EFFECTS_
    `-f,--fraction`     |                         | Cell::DRIVER_FRACTION_
    `-b,--birth`        | \f$\beta_0\f$           | Cell::BIRTH_RATE_
    `-d,--death`        | \f$\delta_0\f$          | Cell::DEATH_RATE_
    `-m,--migration`    | \f$\rho_0\f$            | Cell::MIGRATION_RATE_
    `-p,--symmetric`    | \f$p_s\f$               | Cell::PROB_SYMMETRIC_DIVISION_
    `-r,--prolif`       | \f$\omega_\text{max}\f$ | Cell::PROB_SYMMETRIC_DIVISION_
    `-k,--shape`        | \f$k\f$                 | Cell::GAMMA_SHAPE_
*/
boost::program_options::options_description& Cell::opt_description() {
    namespace po = boost::program_options;
    static po::options_description desc{"Cell"};
    desc.add_options()
        ("mutation,u", po::value<double>(&MUTATION_RATE_)->default_value(MUTATION_RATE_))
        ("sigma,s", po::value<double>(&MUTATION_SIGMA_)->default_value(MUTATION_SIGMA_))
        ("effect,e", po::value<int>(&DRIVER_EFFECTS_)->default_value(DRIVER_EFFECTS_))
        ("fraction,f", po::value<double>(&DRIVER_FRACTION_)->default_value(DRIVER_FRACTION_))
        ("birth,b", po::value<double>(&BIRTH_RATE_)->default_value(BIRTH_RATE_))
        ("death,d", po::value<double>(&DEATH_RATE_)->default_value(DEATH_RATE_))
        ("migration,m", po::value<double>(&MIGRATION_RATE_)->default_value(MIGRATION_RATE_))
        ("shape,k", po::value<double>(&GAMMA_SHAPE_)->default_value(GAMMA_SHAPE_))
        ("symmetric,p", po::value<double>(&PROB_SYMMETRIC_DIVISION_)->default_value(PROB_SYMMETRIC_DIVISION_))
        ("prolif,r", po::value<size_t>(&MAX_PROLIFERATION_CAPACITY_)->default_value(MAX_PROLIFERATION_CAPACITY_))
    ;
    return desc;
}

Cell::Cell(const Cell& other):
    coord_(other.coord_), sites_(other.sites_),
    birth_rate_(other.birth_rate_), death_rate_(other.death_rate_),
    migra_rate_(other.migra_rate_),
    type_(other.type_), proliferation_capacity_(other.proliferation_capacity_),
    id_(++ID_TAIL_), ancestors_(other.ancestors_) {
    ancestors_.push_back(other.id_);
    if (type_ == CellType::stem) {
        if (!std::bernoulli_distribution(PROB_SYMMETRIC_DIVISION_)(wtl::sfmt())) {
            type_ = CellType::nonstem;
        }
    }
}

void Cell::mutate() {
    static std::bernoulli_distribution bernoulli_driver(DRIVER_FRACTION_);
    static std::normal_distribution<double> normal_sigma(0.0, MUTATION_SIGMA_);
    double effect = 0.0;
    if (DRIVER_FRACTION_ > 0.0) {
        if (bernoulli_driver(wtl::sfmt())) {
            effect = MUTATION_SIGMA_;
        }
    } else {
        effect = normal_sigma(wtl::sfmt());
    }
    sites_.push_back(MUTATION_EFFECTS_.size());
    MUTATION_EFFECTS_.push_back(effect);
    MUTANT_IDS_.push_back(id());
    if (DRIVER_EFFECTS_ & 0b001) {
        birth_rate_ *= (1.0 + effect);
    }
    if (DRIVER_EFFECTS_ & 0b010) {
        death_rate_ *= (1.0 - effect);
    }
    if (DRIVER_EFFECTS_ & 0b100) {
        migra_rate_ *= (1.0 + effect);
    }
}

double Cell::delta_time(const double positional_value) {
    double t_birth = std::numeric_limits<double>::infinity();
    if (proliferation_capacity_ > 0) {
        double theta = 1.0;
        theta /= birth_rate_;
        theta /= positional_value;
        theta /= GAMMA_SHAPE_;
        std::gamma_distribution<double> gamma(GAMMA_SHAPE_, theta);
        t_birth = gamma(wtl::sfmt());
    }
    std::exponential_distribution<double> exponential_death(death_rate_);
    std::exponential_distribution<double> exponential_migra(migra_rate_);
    const double t_death = exponential_death(wtl::sfmt());
    const double t_migra = exponential_migra(wtl::sfmt());

    if (t_birth < t_death && t_birth < t_migra) {
        next_event_ = Event::birth;
        return t_birth;
    } else if (t_death < t_migra) {
        next_event_ = Event::death;
        return t_death;
    } else {
        next_event_ = Event::migration;
        return t_migra;
    }
}

std::vector<size_t> Cell::haplotype(std::vector<size_t> segsites) const {
    size_t i = 0;
    for (auto& x: segsites) {
        if (i >= sites_.size() || x < sites_[i]) {
            x = 0;
        } else {
            x = 1;
            ++i;
        }
    }
    return segsites;
}

std::string Cell::header(const size_t dimensions, const char* sep) {
    std::vector<std::string> axes{"x", "y", "z"};
    axes.resize(dimensions);
    std::ostringstream oss;
    oss << "id" << sep << "ancestors" << sep
        << "birth" << sep << "death" << sep
        << wtl::join(axes, sep) << sep << "sites" << sep
        << "beta" << sep << "delta" << sep << "rho\n";
    return oss.str();
}

std::ostream& Cell::write(std::ostream& ost, const char* sep) const {
    return ost << id_ << sep << wtl::join(ancestors_, ":") << sep
        << time_of_birth_ << sep << time_of_death_ << sep
        << wtl::join(coord(), sep) << sep
        << wtl::join(sites(), ":") << sep
        << birth_rate_ << sep
        << death_rate_ << sep
        << migra_rate_ << "\n";
}

std::string Cell::str(const char* sep) const {
    std::ostringstream oss;
    write(oss, sep);
    return oss.str();
}

//! Stream operator for debug print
std::ostream& operator<< (std::ostream& ost, const Cell& x) {
    return x.write(ost, "\t");
}

void Cell::unit_test() {
    std::cerr << __PRETTY_FUNCTION__ << std::endl;
    std::cerr.precision(15);
    Cell cell({1, 2, 3});
    std::cerr << Cell::header(3, "\t") << "\n" << cell << std::endl;
}
<|endoftext|>


<commit_before>/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2008 Scientific Computing and Imaging Institute,
   University of Utah.

   Permission is hereby granted, free of charge, to any person obtaining a
   copy of this software and associated documentation files (the "Software"),
   to deal in the Software without restriction, including without limitation
   the rights to use, copy, modify, merge, publish, distribute, sublicense,
   and/or sell copies of the Software, and to permit persons to whom the
   Software is furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included
   in all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
   THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
   FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
   DEALINGS IN THE SOFTWARE.
*/

/**
  \file    GPUMemManDataStructs.cpp
  \author  Jens Krueger
           SCI Institute
           University of Utah
  \date    August 2008
*/

#include "GPUMemManDataStructs.h"

Texture3DListElem::Texture3DListElem(VolumeDataset* _pDataset, const std::vector<UINT64>& _vLOD, const std::vector<UINT64>& _vBrick, bool bIsPaddedToPowerOfTwo, UINT64 iIntraFrameCounter, UINT64 iFrameCounter) :
  pData(NULL),
  pTexture(NULL),
  pDataset(_pDataset),
  iUserCount(1),
  m_iIntraFrameCounter(iIntraFrameCounter),
  m_iFrameCounter(iFrameCounter),
  vLOD(_vLOD),
  vBrick(_vBrick),
  m_bIsPaddedToPowerOfTwo(bIsPaddedToPowerOfTwo)
{
  if (!CreateTexture()) {
    pTexture->Delete();
    delete pTexture;
    pTexture = NULL;
  }
}

Texture3DListElem::~Texture3DListElem() {
  FreeData();
  FreeTexture();
}

bool Texture3DListElem::Equals(VolumeDataset* _pDataset, const std::vector<UINT64>& _vLOD, const std::vector<UINT64>& _vBrick, bool bIsPaddedToPowerOfTwo) {
  if (_pDataset != pDataset || _vLOD.size() != vLOD.size() || _vBrick.size() != vBrick.size() || m_bIsPaddedToPowerOfTwo != bIsPaddedToPowerOfTwo) return false;

  for (size_t i = 0;i<vLOD.size();i++)   if (vLOD[i] != _vLOD[i]) return false;
  for (size_t i = 0;i<vBrick.size();i++) if (vBrick[i] != _vBrick[i]) return false;

  return true;
}

GLTexture3D* Texture3DListElem::Access(UINT64& iIntraFrameCounter, UINT64& iFrameCounter) {
  m_iIntraFrameCounter = iIntraFrameCounter;
  m_iFrameCounter = iFrameCounter;
  iUserCount++;

  return pTexture;
}

bool Texture3DListElem::BestMatch(const std::vector<UINT64>& vDimension, bool bIsPaddedToPowerOfTwo, UINT64& iIntraFrameCounter, UINT64& iFrameCounter) {
  if (!Match(vDimension) || iUserCount > 0 || m_bIsPaddedToPowerOfTwo != bIsPaddedToPowerOfTwo) return false;

  // framewise older data as before found -> use this object
  if (iFrameCounter > m_iFrameCounter) {
      iFrameCounter = m_iFrameCounter;
      iIntraFrameCounter = m_iIntraFrameCounter;

      return true;
  }

  // framewise older data as before found -> use this object
  if (iFrameCounter == m_iFrameCounter &&
      iIntraFrameCounter < m_iIntraFrameCounter) {

      iFrameCounter = m_iFrameCounter;
      iIntraFrameCounter = m_iIntraFrameCounter;

      return true;
  }

  return false;
}

bool Texture3DListElem::Match(const std::vector<UINT64>& vDimension) {
  if (pTexture == NULL) return false;

  const std::vector<UINT64> vSize = pDataset->GetInfo()->GetBrickSizeND(vLOD, vBrick);

  if (vDimension.size() != vSize.size()) return false;
  for (size_t i = 0;i<vSize.size();i++)   if (vSize[i] != vDimension[i]) return false;

  return true;
}

bool Texture3DListElem::Replace(VolumeDataset* _pDataset, const std::vector<UINT64>& _vLOD, const std::vector<UINT64>& _vBrick, bool bIsPaddedToPowerOfTwo, UINT64 iIntraFrameCounter, UINT64 iFrameCounter) {
  if (pTexture == NULL) return false;

  pDataset = _pDataset;
  vLOD     = _vLOD;
  vBrick   = _vBrick;
  m_bIsPaddedToPowerOfTwo = bIsPaddedToPowerOfTwo;

  m_iIntraFrameCounter = iIntraFrameCounter;
  m_iFrameCounter = iFrameCounter;

  LoadData();
  glGetError();
  pTexture->SetData(pData);
	return GL_NO_ERROR==glGetError();
}

bool Texture3DListElem::LoadData() {
  FreeData();
  return pDataset->GetBrick(&pData, vLOD, vBrick);
}

void  Texture3DListElem::FreeData() {
  delete [] pData;
  pData = NULL;
}

bool Texture3DListElem::CreateTexture(bool bDeleteOldTexture) {
  if (bDeleteOldTexture) FreeTexture();

  if (pData == NULL)
    if (!LoadData()) return false;

  const std::vector<UINT64> vSize = pDataset->GetInfo()->GetBrickSizeND(vLOD, vBrick);

  UINT64 iBitWidth  = pDataset->GetInfo()->GetBitwith();
  UINT64 iCompCount = pDataset->GetInfo()->GetComponentCount();

  GLint glInternalformat;
  GLenum glFormat;
  GLenum glType;

  switch (iCompCount) {
    case 1 : glFormat = GL_LUMINANCE; break;
    case 3 : glFormat = GL_RGB; break;
    case 4 : glFormat = GL_RGBA; break;
    default : return false;
  }

  if (iBitWidth == 8) {
      glType = GL_UNSIGNED_BYTE;

      switch (iCompCount) {
        case 1 : glInternalformat = GL_LUMINANCE8; break;
        case 3 : glInternalformat = GL_RGB8; break;
        case 4 : glInternalformat = GL_RGBA8; break;
        default : return false;
      }
  } else {
    if (iBitWidth == 16) {
      glType = GL_UNSIGNED_SHORT;
      switch (iCompCount) {
        case 1 : glInternalformat = GL_LUMINANCE16; break;
        case 3 : glInternalformat = GL_RGB16; break;
        case 4 : glInternalformat = GL_RGBA16; break;
        default : return false;
      }

    } else {
        return false;
    }
  }

  glGetError();
  if (!m_bIsPaddedToPowerOfTwo || (MathTools::IsPow2(vSize[0]) && MathTools::IsPow2(vSize[1]) && MathTools::IsPow2(vSize[2]))) {
    pTexture = new GLTexture3D(GLuint(vSize[0]), GLuint(vSize[1]), GLuint(vSize[2]), glInternalformat, glFormat, glType, (unsigned int)(iBitWidth*iCompCount), pData, GL_LINEAR, GL_LINEAR);
  } else {
    // pad the data to a power of two
    UINTVECTOR3 vPaddedSize(MathTools::NextPow2(vSize[0]), MathTools::NextPow2(vSize[1]), MathTools::NextPow2(vSize[2]));

    size_t iTarget = 0;
    size_t iSource = 0;
    size_t iElementSize = iBitWidth/8*iCompCount;
    size_t iRowSizeSource = vSize[0]*iElementSize;
    size_t iRowSizeTarget = vPaddedSize[0]*iElementSize;

    unsigned char* pPaddedData = new unsigned char[iRowSizeTarget*vPaddedSize[1]*vPaddedSize[2]];
    for (size_t z = 0;z<vSize[2];z++) {
      for (size_t y = 0;y<vSize[1];y++) {
        memcpy(pPaddedData+iTarget, pData+iSource, iRowSizeSource);

        // if the x sizes differ copy one more element to make the texture behave like clamp
        if (iRowSizeTarget > iRowSizeSource) memcpy(pPaddedData+iTarget+iRowSizeSource, pPaddedData+iTarget+iRowSizeSource-iElementSize, iElementSize);
        iTarget += iRowSizeTarget;
        iSource += iRowSizeSource;
      }
      if (vPaddedSize[1] > vSize[1]) {
        memcpy(pPaddedData+iTarget, pPaddedData+iTarget-iRowSizeTarget, iRowSizeTarget);
        iTarget += (vPaddedSize[1]-vSize[1])*iRowSizeTarget;
      }
    }
    // if the z sizes differ copy one more slice to make the texture behave like clamp
    if (vPaddedSize[2] > vSize[2])
      memcpy(pPaddedData+iTarget, pPaddedData+iTarget-vPaddedSize[1]*iRowSizeTarget, vPaddedSize[1]*iRowSizeTarget);

    pTexture = new GLTexture3D(GLuint(vPaddedSize[0]), GLuint(vPaddedSize[1]), GLuint(vPaddedSize[2]), glInternalformat, glFormat, glType, (unsigned int)(iBitWidth*iCompCount), pPaddedData, GL_LINEAR, GL_LINEAR);

    delete [] pPaddedData;
  }
  FreeData();
	return GL_NO_ERROR==glGetError();
}

void Texture3DListElem::FreeTexture() {
  if (pTexture != NULL) {
    pTexture->Delete();
    delete pTexture;
  }
  pTexture = NULL;
}
<commit_msg>better border handling for padded data<commit_after>/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2008 Scientific Computing and Imaging Institute,
   University of Utah.

   Permission is hereby granted, free of charge, to any person obtaining a
   copy of this software and associated documentation files (the "Software"),
   to deal in the Software without restriction, including without limitation
   the rights to use, copy, modify, merge, publish, distribute, sublicense,
   and/or sell copies of the Software, and to permit persons to whom the
   Software is furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included
   in all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
   THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
   FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
   DEALINGS IN THE SOFTWARE.
*/

/**
  \file    GPUMemManDataStructs.cpp
  \author  Jens Krueger
           SCI Institute
           University of Utah
  \date    August 2008
*/

#include "GPUMemManDataStructs.h"

Texture3DListElem::Texture3DListElem(VolumeDataset* _pDataset, const std::vector<UINT64>& _vLOD, const std::vector<UINT64>& _vBrick, bool bIsPaddedToPowerOfTwo, UINT64 iIntraFrameCounter, UINT64 iFrameCounter) :
  pData(NULL),
  pTexture(NULL),
  pDataset(_pDataset),
  iUserCount(1),
  m_iIntraFrameCounter(iIntraFrameCounter),
  m_iFrameCounter(iFrameCounter),
  vLOD(_vLOD),
  vBrick(_vBrick),
  m_bIsPaddedToPowerOfTwo(bIsPaddedToPowerOfTwo)
{
  if (!CreateTexture()) {
    pTexture->Delete();
    delete pTexture;
    pTexture = NULL;
  }
}

Texture3DListElem::~Texture3DListElem() {
  FreeData();
  FreeTexture();
}

bool Texture3DListElem::Equals(VolumeDataset* _pDataset, const std::vector<UINT64>& _vLOD, const std::vector<UINT64>& _vBrick, bool bIsPaddedToPowerOfTwo) {
  if (_pDataset != pDataset || _vLOD.size() != vLOD.size() || _vBrick.size() != vBrick.size() || m_bIsPaddedToPowerOfTwo != bIsPaddedToPowerOfTwo) return false;

  for (size_t i = 0;i<vLOD.size();i++)   if (vLOD[i] != _vLOD[i]) return false;
  for (size_t i = 0;i<vBrick.size();i++) if (vBrick[i] != _vBrick[i]) return false;

  return true;
}

GLTexture3D* Texture3DListElem::Access(UINT64& iIntraFrameCounter, UINT64& iFrameCounter) {
  m_iIntraFrameCounter = iIntraFrameCounter;
  m_iFrameCounter = iFrameCounter;
  iUserCount++;

  return pTexture;
}

bool Texture3DListElem::BestMatch(const std::vector<UINT64>& vDimension, bool bIsPaddedToPowerOfTwo, UINT64& iIntraFrameCounter, UINT64& iFrameCounter) {
  if (!Match(vDimension) || iUserCount > 0 || m_bIsPaddedToPowerOfTwo != bIsPaddedToPowerOfTwo) return false;

  // framewise older data as before found -> use this object
  if (iFrameCounter > m_iFrameCounter) {
      iFrameCounter = m_iFrameCounter;
      iIntraFrameCounter = m_iIntraFrameCounter;

      return true;
  }

  // framewise older data as before found -> use this object
  if (iFrameCounter == m_iFrameCounter &&
      iIntraFrameCounter < m_iIntraFrameCounter) {

      iFrameCounter = m_iFrameCounter;
      iIntraFrameCounter = m_iIntraFrameCounter;

      return true;
  }

  return false;
}

bool Texture3DListElem::Match(const std::vector<UINT64>& vDimension) {
  if (pTexture == NULL) return false;

  const std::vector<UINT64> vSize = pDataset->GetInfo()->GetBrickSizeND(vLOD, vBrick);

  if (vDimension.size() != vSize.size()) return false;
  for (size_t i = 0;i<vSize.size();i++)   if (vSize[i] != vDimension[i]) return false;

  return true;
}

bool Texture3DListElem::Replace(VolumeDataset* _pDataset, const std::vector<UINT64>& _vLOD, const std::vector<UINT64>& _vBrick, bool bIsPaddedToPowerOfTwo, UINT64 iIntraFrameCounter, UINT64 iFrameCounter) {
  if (pTexture == NULL) return false;

  pDataset = _pDataset;
  vLOD     = _vLOD;
  vBrick   = _vBrick;
  m_bIsPaddedToPowerOfTwo = bIsPaddedToPowerOfTwo;

  m_iIntraFrameCounter = iIntraFrameCounter;
  m_iFrameCounter = iFrameCounter;

  LoadData();
  glGetError();
  pTexture->SetData(pData);
	return GL_NO_ERROR==glGetError();
}

bool Texture3DListElem::LoadData() {
  FreeData();
  return pDataset->GetBrick(&pData, vLOD, vBrick);
}

void  Texture3DListElem::FreeData() {
  delete [] pData;
  pData = NULL;
}

bool Texture3DListElem::CreateTexture(bool bDeleteOldTexture) {
  if (bDeleteOldTexture) FreeTexture();

  if (pData == NULL)
    if (!LoadData()) return false;

  const std::vector<UINT64> vSize = pDataset->GetInfo()->GetBrickSizeND(vLOD, vBrick);

  UINT64 iBitWidth  = pDataset->GetInfo()->GetBitwith();
  UINT64 iCompCount = pDataset->GetInfo()->GetComponentCount();

  GLint glInternalformat;
  GLenum glFormat;
  GLenum glType;

  switch (iCompCount) {
    case 1 : glFormat = GL_LUMINANCE; break;
    case 3 : glFormat = GL_RGB; break;
    case 4 : glFormat = GL_RGBA; break;
    default : return false;
  }

  if (iBitWidth == 8) {
      glType = GL_UNSIGNED_BYTE;

      switch (iCompCount) {
        case 1 : glInternalformat = GL_LUMINANCE8; break;
        case 3 : glInternalformat = GL_RGB8; break;
        case 4 : glInternalformat = GL_RGBA8; break;
        default : return false;
      }
  } else {
    if (iBitWidth == 16) {
      glType = GL_UNSIGNED_SHORT;
      switch (iCompCount) {
        case 1 : glInternalformat = GL_LUMINANCE16; break;
        case 3 : glInternalformat = GL_RGB16; break;
        case 4 : glInternalformat = GL_RGBA16; break;
        default : return false;
      }

    } else {
        return false;
    }
  }

  glGetError();
  if (!m_bIsPaddedToPowerOfTwo || (MathTools::IsPow2(vSize[0]) && MathTools::IsPow2(vSize[1]) && MathTools::IsPow2(vSize[2]))) {
    pTexture = new GLTexture3D(GLuint(vSize[0]), GLuint(vSize[1]), GLuint(vSize[2]), glInternalformat, glFormat, glType, (unsigned int)(iBitWidth*iCompCount), pData, GL_LINEAR, GL_LINEAR);
  } else {
    // pad the data to a power of two
    UINTVECTOR3 vPaddedSize(MathTools::NextPow2(vSize[0]), MathTools::NextPow2(vSize[1]), MathTools::NextPow2(vSize[2]));

    size_t iTarget = 0;
    size_t iSource = 0;
    size_t iElementSize = iBitWidth/8*iCompCount;
    size_t iRowSizeSource = vSize[0]*iElementSize;
    size_t iRowSizeTarget = vPaddedSize[0]*iElementSize;

    unsigned char* pPaddedData = new unsigned char[iRowSizeTarget*vPaddedSize[1]*vPaddedSize[2]];
    memset(pPaddedData, 0, iRowSizeTarget*vPaddedSize[1]*vPaddedSize[2]);

    for (size_t z = 0;z<vSize[2];z++) {
      for (size_t y = 0;y<vSize[1];y++) {
        memcpy(pPaddedData+iTarget, pData+iSource, iRowSizeSource);

        // if the x sizes differ copy one more element to make the texture behave like clamp
        if (iRowSizeTarget > iRowSizeSource) memcpy(pPaddedData+iTarget+iRowSizeSource, pPaddedData+iTarget+iRowSizeSource-iElementSize, iElementSize);
        iTarget += iRowSizeTarget;
        iSource += iRowSizeSource;
      }
      if (vPaddedSize[1] > vSize[1]) {
        memcpy(pPaddedData+iTarget, pPaddedData+iTarget-iRowSizeTarget, iRowSizeTarget);
        iTarget += (vPaddedSize[1]-vSize[1])*iRowSizeTarget;
      }
    }
    // if the z sizes differ copy one more slice to make the texture behave like clamp
    if (vPaddedSize[2] > vSize[2])
      memcpy(pPaddedData+iTarget, pPaddedData+iTarget-vPaddedSize[1]*iRowSizeTarget, vPaddedSize[1]*iRowSizeTarget);

    pTexture = new GLTexture3D(GLuint(vPaddedSize[0]), GLuint(vPaddedSize[1]), GLuint(vPaddedSize[2]), glInternalformat, glFormat, glType, (unsigned int)(iBitWidth*iCompCount), pPaddedData, GL_LINEAR, GL_LINEAR);

    delete [] pPaddedData;
  }
  FreeData();
	return GL_NO_ERROR==glGetError();
}

void Texture3DListElem::FreeTexture() {
  if (pTexture != NULL) {
    pTexture->Delete();
    delete pTexture;
  }
  pTexture = NULL;
}
<|endoftext|>