<commit_before>/* -*- coding: utf-8; mode: c++; tab-width: 3; indent-tabs-mode: nil -*-

Copyright 2015
Raffaello D. Di Napoli

This file is part of Abaclade.

Abaclade 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 3 of the
License, or (at your option) any later version.

Abaclade 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 Abaclade. If not, see
<http://www.gnu.org/licenses/>.
--------------------------------------------------------------------------------------------------*/

#ifndef _ABACLADE_COROUTINE_HXX
#define _ABACLADE_COROUTINE_HXX

#ifndef _ABACLADE_HXX
   #error "Please #include <abaclade.hxx> before this file"
#endif
#ifdef ABC_CXX_PRAGMA_ONCE
   #pragma once
#endif

////////////////////////////////////////////////////////////////////////////////////////////////////
// abc::coroutine

namespace abc {

/*! Subroutine for use in non-preemptive multitasking, enabling asynchronous I/O in most abc::io
classes. */
class ABACLADE_SYM coroutine : public noncopyable {
private:
   friend class coroutine_scheduler;

public:
   //! OS-dependent execution context for the coroutine.
   class context;

public:
   /*! Constructor.

   @param fnMain
      Function to invoke once the coroutine is first scheduled.
   @param coro
      Source object.
   */
   coroutine();
   explicit coroutine(std::function<void ()> fnMain);
   coroutine(coroutine && coro) :
      m_pctx(std::move(coro.m_pctx)) {
   }

   //! Destructor.
   ~coroutine();

private:
   //! Pointer to the coroutine’s execution context.
   std::shared_ptr<context> m_pctx;
};

} //namespace abc

////////////////////////////////////////////////////////////////////////////////////////////////////
// abc::coroutine_scheduler

namespace abc {

//! Schedules coroutine execution.
class ABACLADE_SYM coroutine_scheduler : public noncopyable {
public:
   //! Constructor.
   coroutine_scheduler();

   //! Destructor.
   virtual ~coroutine_scheduler();

   void add(coroutine const & coro);

   static std::shared_ptr<coroutine_scheduler> const & attach_to_this_thread(
      std::shared_ptr<coroutine_scheduler> pcorosched = nullptr
   );

   static std::shared_ptr<coroutine_scheduler> const & get_for_current_thread() {
      return sm_pcorosched;
   }

   /*! Begins scheduling and running coroutines on the current thread. Only returns after every
   coroutine added with add_coroutine() returns. */
   virtual void run() = 0;

   /*! Allows other coroutines to run while the asynchronous I/O operation completes, as an
   alternative to blocking while waiting for its completion.

   @param fd
      File descriptor that the calling coroutine is waiting for I/O on.
   @param bWrite
      true if the coroutine is waiting to write to fd, or false if it’s waiting to read from it.
   */
   virtual void yield_while_async_pending(io::filedesc const & fd, bool bWrite) = 0;

protected:
   //! Pointer to the active (current) coroutine, or nullptr if none is active.
   std::shared_ptr<coroutine::context> m_pcoroctxActive;
   //! List of coroutines that have been scheduled, but have not been started yet.
   collections::list<std::shared_ptr<coroutine::context>> m_listStartingCoros;
   //! Pointer to the coroutine_scheduler for the current thread.
   static thread_local_value<std::shared_ptr<coroutine_scheduler>> sm_pcorosched;
};

} //namespace abc

////////////////////////////////////////////////////////////////////////////////////////////////////

#endif //ifndef _ABACLADE_COROUTINE_HXX
<commit_msg>Make abc::coroutine_scheduler::coroutine_scheduler() protected<commit_after>/* -*- coding: utf-8; mode: c++; tab-width: 3; indent-tabs-mode: nil -*-

Copyright 2015
Raffaello D. Di Napoli

This file is part of Abaclade.

Abaclade 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 3 of the
License, or (at your option) any later version.

Abaclade 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 Abaclade. If not, see
<http://www.gnu.org/licenses/>.
--------------------------------------------------------------------------------------------------*/

#ifndef _ABACLADE_COROUTINE_HXX
#define _ABACLADE_COROUTINE_HXX

#ifndef _ABACLADE_HXX
   #error "Please #include <abaclade.hxx> before this file"
#endif
#ifdef ABC_CXX_PRAGMA_ONCE
   #pragma once
#endif

////////////////////////////////////////////////////////////////////////////////////////////////////
// abc::coroutine

namespace abc {

/*! Subroutine for use in non-preemptive multitasking, enabling asynchronous I/O in most abc::io
classes. */
class ABACLADE_SYM coroutine : public noncopyable {
private:
   friend class coroutine_scheduler;

public:
   //! OS-dependent execution context for the coroutine.
   class context;

public:
   /*! Constructor.

   @param fnMain
      Function to invoke once the coroutine is first scheduled.
   @param coro
      Source object.
   */
   coroutine();
   explicit coroutine(std::function<void ()> fnMain);
   coroutine(coroutine && coro) :
      m_pctx(std::move(coro.m_pctx)) {
   }

   //! Destructor.
   ~coroutine();

private:
   //! Pointer to the coroutine’s execution context.
   std::shared_ptr<context> m_pctx;
};

} //namespace abc

////////////////////////////////////////////////////////////////////////////////////////////////////
// abc::coroutine_scheduler

namespace abc {

//! Schedules coroutine execution.
class ABACLADE_SYM coroutine_scheduler : public noncopyable {
public:
   //! Destructor.
   virtual ~coroutine_scheduler();

   void add(coroutine const & coro);

   static std::shared_ptr<coroutine_scheduler> const & attach_to_this_thread(
      std::shared_ptr<coroutine_scheduler> pcorosched = nullptr
   );

   static std::shared_ptr<coroutine_scheduler> const & get_for_current_thread() {
      return sm_pcorosched;
   }

   /*! Begins scheduling and running coroutines on the current thread. Only returns after every
   coroutine added with add_coroutine() returns. */
   virtual void run() = 0;

   /*! Allows other coroutines to run while the asynchronous I/O operation completes, as an
   alternative to blocking while waiting for its completion.

   @param fd
      File descriptor that the calling coroutine is waiting for I/O on.
   @param bWrite
      true if the coroutine is waiting to write to fd, or false if it’s waiting to read from it.
   */
   virtual void yield_while_async_pending(io::filedesc const & fd, bool bWrite) = 0;

protected:
   //! Constructor.
   coroutine_scheduler();

protected:
   //! Pointer to the active (current) coroutine, or nullptr if none is active.
   std::shared_ptr<coroutine::context> m_pcoroctxActive;
   //! List of coroutines that have been scheduled, but have not been started yet.
   collections::list<std::shared_ptr<coroutine::context>> m_listStartingCoros;
   //! Pointer to the coroutine_scheduler for the current thread.
   static thread_local_value<std::shared_ptr<coroutine_scheduler>> sm_pcorosched;
};

} //namespace abc

////////////////////////////////////////////////////////////////////////////////////////////////////

#endif //ifndef _ABACLADE_COROUTINE_HXX
<|endoftext|>


<commit_before>/*
 * eos - A 3D Morphable Model fitting library written in modern C++11/14.
 *
 * File: include/eos/render/normals.hpp
 *
 * Copyright 2014-2019 Patrik Huber
 *
 * 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.
 */
#pragma once

#ifndef EOS_RENDER_NORMALS_HPP
#define EOS_RENDER_NORMALS_HPP

#include "glm/vec3.hpp"
#include "glm/geometric.hpp"

#include "Eigen/Core"

namespace eos {
namespace render {

/**
 * Computes the normal of a face (triangle), i.e. the per-face normal. Returned normal will be unit length.
 *
 * Assumes the triangle is given in CCW order, i.e. vertices in counterclockwise order on the screen are
 * front-facing.
 *
 * @param[in] v0 First vertex.
 * @param[in] v1 Second vertex.
 * @param[in] v2 Third vertex.
 * @return The unit-length normal of the given triangle.
 */
inline Eigen::Vector3f compute_face_normal(const Eigen::Vector3f& v0, const Eigen::Vector3f& v1,
                                           const Eigen::Vector3f& v2)
{
    Eigen::Vector3f n = (v1 - v0).cross(v2 - v0); // v0-to-v1 x v0-to-v2
    return n.normalized();
};

/**
 * Computes the normal of a face (triangle), i.e. the per-face normal. Returned normal will be unit length.
 *
 * Assumes the triangle is given in CCW order, i.e. vertices in counterclockwise order on the screen are
 * front-facing.
 *
 * @param[in] v0 First vertex.
 * @param[in] v1 Second vertex.
 * @param[in] v2 Third vertex.
 * @return The unit-length normal of the given triangle.
 */
inline Eigen::Vector3f compute_face_normal(const Eigen::Vector4f& v0, const Eigen::Vector4f& v1,
                                           const Eigen::Vector4f& v2)
{
    Eigen::Vector4f n = (v1 - v0).cross3(v2 - v0); // v0-to-v1 x v0-to-v2
    return n.head<3>().normalized();
};

/**
 * Computes the normal of a face (triangle), i.e. the per-face normal. Returned normal will be unit length.
 *
 * Assumes the triangle is given in CCW order, i.e. vertices in counterclockwise order on the screen are
 * front-facing.
 *
 * @param[in] v0 First vertex.
 * @param[in] v1 Second vertex.
 * @param[in] v2 Third vertex.
 * @return The unit-length normal of the given triangle.
 */
inline glm::vec3 compute_face_normal(const glm::vec3& v0, const glm::vec3& v1, const glm::vec3& v2)
{
    glm::vec3 n = glm::cross(v1 - v0, v2 - v0); // v0-to-v1 x v0-to-v2
    n = glm::normalize(n);
    return n;
};

} /* namespace render */
} /* namespace eos */

#endif /* EOS_RENDER_NORMALS_HPP */
<commit_msg>Add functions to compute face and vertex normals of a mesh<commit_after>/*
 * eos - A 3D Morphable Model fitting library written in modern C++11/14.
 *
 * File: include/eos/render/normals.hpp
 *
 * Copyright 2014-2019 Patrik Huber
 *
 * 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.
 */
#pragma once

#ifndef EOS_RENDER_NORMALS_HPP
#define EOS_RENDER_NORMALS_HPP

#include "glm/vec3.hpp"
#include "glm/geometric.hpp"

#include "Eigen/Core"

#include <vector>

namespace eos {
namespace render {

/**
 * Computes the normal of a face (triangle), i.e. the per-face normal. Returned normal will be unit length.
 *
 * Assumes the triangle is given in CCW order, i.e. vertices in counterclockwise order on the screen are
 * front-facing.
 *
 * @param[in] v0 First vertex.
 * @param[in] v1 Second vertex.
 * @param[in] v2 Third vertex.
 * @return The unit-length normal of the given triangle.
 */
inline Eigen::Vector3f compute_face_normal(const Eigen::Vector3f& v0, const Eigen::Vector3f& v1,
                                           const Eigen::Vector3f& v2)
{
    Eigen::Vector3f n = (v1 - v0).cross(v2 - v0); // v0-to-v1 x v0-to-v2
    return n.normalized();
};

/**
 * Computes the normal of a face (triangle), i.e. the per-face normal. Returned normal will be unit length.
 *
 * Assumes the triangle is given in CCW order, i.e. vertices in counterclockwise order on the screen are
 * front-facing.
 *
 * @param[in] v0 First vertex.
 * @param[in] v1 Second vertex.
 * @param[in] v2 Third vertex.
 * @return The unit-length normal of the given triangle.
 */
inline Eigen::Vector3f compute_face_normal(const Eigen::Vector4f& v0, const Eigen::Vector4f& v1,
                                           const Eigen::Vector4f& v2)
{
    Eigen::Vector4f n = (v1 - v0).cross3(v2 - v0); // v0-to-v1 x v0-to-v2
    return n.head<3>().normalized();
};

/**
 * Computes the normal of a face (triangle), i.e. the per-face normal. Returned normal will be unit length.
 *
 * Assumes the triangle is given in CCW order, i.e. vertices in counterclockwise order on the screen are
 * front-facing.
 *
 * @param[in] v0 First vertex.
 * @param[in] v1 Second vertex.
 * @param[in] v2 Third vertex.
 * @return The unit-length normal of the given triangle.
 */
inline glm::vec3 compute_face_normal(const glm::vec3& v0, const glm::vec3& v1, const glm::vec3& v2)
{
    glm::vec3 n = glm::cross(v1 - v0, v2 - v0); // v0-to-v1 x v0-to-v2
    n = glm::normalize(n);
    return n;
};

/**
* Computes the per-face (per-triangle) normals of all triangles of the given mesh. Returned normals will be unit length.
*
* Assumes triangles are given in CCW order, i.e. vertices in counterclockwise order on the screen are front-facing.
*
* @param[in] vertices A list of vertices.
* @param[in] triangle_vertex_indices Triangle list for the given vertices.
* @return The unit-length per-face normals.
*/
inline std::vector<Eigen::Vector3f>
compute_face_normals(const std::vector<Eigen::Vector3f>& vertices,
                     const std::vector<std::array<int, 3>>& triangle_vertex_indices)
{
    std::vector<Eigen::Vector3f> face_normals;
    for (const auto& tvi : triangle_vertex_indices)
    {
        const auto face_normal = compute_face_normal(vertices[tvi[0]], vertices[tvi[1]], vertices[tvi[2]]);
        face_normals.push_back(face_normal);
    }
    return face_normals;
};

/**
 * Computes the per-vertex normals of all vertices of the given mesh. Returned normals will be unit length.
 *
 * Assumes triangles are given in CCW order, i.e. vertices in counterclockwise order on the screen are
 * front-facing.
 *
 * @param[in] vertices A list of vertices.
 * @param[in] triangle_vertex_indices Triangle list for the given vertices.
 * @param[in] face_normals Per-face normals for all triangles.
 * @return The unit-length per-vertex normals.
 */
inline std::vector<Eigen::Vector3f>
compute_vertex_normals(const std::vector<Eigen::Vector3f>& vertices,
                       const std::vector<std::array<int, 3>>& triangle_vertex_indices,
                       const std::vector<Eigen::Vector3f>& face_normals)
{
    std::vector<Eigen::Vector3f> per_vertex_normals;
    // Initialise with zeros:
    for (int i = 0; i < vertices.size(); ++i)
    {
        per_vertex_normals.emplace_back(Eigen::Vector3f(0.0f, 0.0f, 0.0f));
    }

    // Loop over the faces again:
    for (int i = 0; i < triangle_vertex_indices.size(); ++i)
    {
        const auto& tvi = triangle_vertex_indices[i];
        // Throw normal at each corner:
        per_vertex_normals[tvi[0]] += face_normals[i];
        per_vertex_normals[tvi[1]] += face_normals[i];
        per_vertex_normals[tvi[2]] += face_normals[i];
    }

    // Take average via normalization:
    for (auto& n : per_vertex_normals)
    {
        n.normalize();
    }
    return per_vertex_normals;
};

} /* namespace render */
} /* namespace eos */

#endif /* EOS_RENDER_NORMALS_HPP */
<|endoftext|>


<commit_before>#pragma once
#include <string>

#include <engine/gfx/shapes.hpp>
#include <stlw/result.hpp>
#include <stlw/type_ctors.hpp>

namespace game
{
namespace boomhs
{

template <typename L>
class boomhs_game
{
  L &logger_;

  boomhs_game(L &l)
      : logger_(l)
  {
  }

  friend struct boomhs_library;

  inline static bool is_quit_event(SDL_Event &event)
  {
    bool is_quit = false;

    switch (event.type) {
    case SDL_QUIT: {
      is_quit = true;
      break;
    }
    case SDL_KEYDOWN: {
      switch (event.key.keysym.sym) {
      case SDLK_ESCAPE: {
        is_quit = true;
        break;
      }
      }
    }
    }
    return is_quit;
  }

public:
  NO_COPY(boomhs_game);
  MOVE_DEFAULT(boomhs_game);

  template <typename R>
  stlw::result<stlw::empty_type, std::string> init(R &renderer)
  {
    // nothing to do for now
    return stlw::make_empty();
  }

  template <typename R>
  void game_loop(R &renderer)
  {
    // Set up vertex data (and buffer(s)) and attribute pointers
    constexpr float Wcoord = 1.0f;
    // clang-format off
    constexpr std::array<float, 12> v0 =
    {
      -0.5f , -0.5f, 0.0f, Wcoord, // bottom left
      0.0f  , -0.5f, 0.0f, Wcoord, // bottom right
      -0.25f, 0.5f , 0.0f, Wcoord  // top middle
    };
    constexpr std::array<float, 12> v1 =
    {
      0.5f  , 0.0f, 0.0f, Wcoord, // bottom left
      1.00f , 0.0f, 0.0f, Wcoord, // bottom right
      0.75f , 1.0f, 0.0f, Wcoord  // top middle
    };
    constexpr std::array<float, 12> c0 =
    {
      1.0f, 0.0f, 0.0f, 1.0f,
      0.0f, 1.0f, 0.0f, 1.0f,
      0.0f, 0.0f, 1.0f, 1.0f,
    };
    // clang-format on
    using namespace engine::gfx;
    constexpr triangle t0 = make_triangle(v0, c0);
    constexpr triangle t1 = make_triangle(v1, c0);

    renderer.draw(this->logger_, t0, t1);
  }

  bool process_event(SDL_Event &event) { return is_quit_event(event); }
};

struct boomhs_library {
  boomhs_library() = delete;

  DEFINE_STATIC_WRAPPER_FUNCTION(make_game, boomhs_game<P...>);
};

} // ns boomhs
} // ns game
<commit_msg>Inject triangle's initial position<commit_after>#pragma once
#include <string>

#include <engine/gfx/shapes.hpp>
#include <stlw/result.hpp>
#include <stlw/type_ctors.hpp>

namespace game
{
namespace boomhs
{

template <typename L>
class boomhs_game
{
  L &logger_;
  engine::gfx::triangle t0_;
  engine::gfx::triangle t1_;

  boomhs_game(L &l, engine::gfx::triangle const& t0, engine::gfx::triangle const& t1)
      : logger_(l)
      , t0_(std::move(t0))
      , t1_(std::move(t1))
  {
  }

  friend struct boomhs_library;

  inline static bool is_quit_event(SDL_Event &event)
  {
    bool is_quit = false;

    switch (event.type) {
    case SDL_QUIT: {
      is_quit = true;
      break;
    }
    case SDL_KEYDOWN: {
      switch (event.key.keysym.sym) {
      case SDLK_ESCAPE: {
        is_quit = true;
        break;
      }
      }
    }
    }
    return is_quit;
  }

public:
  NO_COPY(boomhs_game);
  MOVE_DEFAULT(boomhs_game);

  template <typename R>
  stlw::result<stlw::empty_type, std::string> init(R &renderer)
  {
    // nothing to do for now
    return stlw::make_empty();
  }

  template <typename R>
  void game_loop(R &renderer)
  {
    renderer.draw(this->logger_, this->t0_, this->t1_);
  }

  bool process_event(SDL_Event &event) { return is_quit_event(event); }
};

struct boomhs_library {
  boomhs_library() = delete;

  template<typename L>
  static inline auto
  make_game(L &logger)
  {
    // Set up vertex data (and buffer(s)) and attribute pointers
    constexpr float Wcoord = 1.0f;
    // clang-format off
    constexpr std::array<float, 12> v0 =
    {
      -0.5f , -0.5f, 0.0f, Wcoord, // bottom left
      0.0f  , -0.5f, 0.0f, Wcoord, // bottom right
      -0.25f, 0.5f , 0.0f, Wcoord  // top middle
    };
    constexpr std::array<float, 12> v1 =
    {
      0.5f  , 0.0f, 0.0f, Wcoord, // bottom left
      1.00f , 0.0f, 0.0f, Wcoord, // bottom right
      0.75f , 1.0f, 0.0f, Wcoord  // top middle
    };
    constexpr std::array<float, 12> c0 =
    {
      1.0f, 0.0f, 0.0f, 1.0f,
      0.0f, 1.0f, 0.0f, 1.0f,
      0.0f, 0.0f, 1.0f, 1.0f,
    };
    // clang-format on
    using namespace engine::gfx;
    triangle t0 = make_triangle(v0, c0);
    triangle t1 = make_triangle(v1, c0);

    return boomhs_game<L>{logger, t0, t1};
  }
};

} // ns boomhs
} // ns game
<|endoftext|>


<commit_before>//
//  DemoFog.cpp
//  IcEngDemo
//
//  Created by Sherman Chen on 3/22/17.
//  Copyright (c) 2017 Simviu Technology Inc.
//  All rights reserved.
//  http://www.simviu.com/dev
//

#include "DemoScene.h"

using namespace std;
using namespace ctl;
using namespace Ic3d;
static const string K_sModel = "IcDemo/MixShapes/MixShapes.obj";
static const int K_objAry_N = 10;   // NxNxN of objs
static const int K_objAry_W = 10.0;  // Width of objs
static const int K_objAry_H = 20.0;  // Height of objs

//----------------------------------------------
//  onInit
//----------------------------------------------
void DemoFog::onInit()
{
    //--- Always call parent class onInit()
    IcScene::onInit();

    //--- Init Fog

    //--- Load Scene
    loadScene();

}
//----------------------------------------------
//  loadScene
//----------------------------------------------
void DemoFog::loadScene()
{
    //--- Load Object
    const auto& cfg = IcApp::getInstance()->m_cfg;
    string sPathRes = cfg.m_sPathRes;
    string sFile = sPathRes + K_sModel;
    auto pModel = ctl::makeSp<IcModel>(sFile);
    // Add array of objs to Scene
    int N = K_objAry_N;
    float w = K_objAry_W;
    float h = K_objAry_H;
    for(int y=0;y<N;y++)
        for(int x=0;x<N;x++)
            for(int z=0;z<N;z++)
            {
                auto pObj   = ctl::makeSp<IcObject>(pModel);
                TVec3 pos((x-N/2)*w, (y-N/2)*h, (z-N/2)*w);
                pObj->setPos(pos);
                addObj(pObj);
            }

}
<commit_msg>FogDemo added.<commit_after>//
//  DemoFog.cpp
//  IcEngDemo
//
//  Created by Sherman Chen on 3/22/17.
//  Copyright (c) 2017 Simviu Technology Inc.
//  All rights reserved.
//  http://www.simviu.com/dev
//

#include "DemoScene.h"

using namespace std;
using namespace ctl;
using namespace Ic3d;
static const string K_sModel = "IcDemo/MixShapes/MixShapes.obj";
static const int K_objAry_N = 10;   // NxNxN of objs
static const int K_objAry_W = 10.0;  // Width of objs
static const int K_objAry_H = 20.0;  // Height of objs

//----------------------------------------------
//  onInit
//----------------------------------------------
void DemoFog::onInit()
{
    //--- Always call parent class onInit()
    IcScene::onInit();

    //--- Init Fog
    m_cfg.m_fogPara.m_color = TColor(1,1,1,1);
    m_cfg.m_fogPara.m_K_linear = 0.1;

    //--- Load Scene
    loadScene();

}
//----------------------------------------------
//  loadScene
//----------------------------------------------
void DemoFog::loadScene()
{
    //--- Load Object
    const auto& cfg = IcApp::getInstance()->m_cfg;
    string sPathRes = cfg.m_sPathRes;
    string sFile = sPathRes + K_sModel;
    auto pModel = ctl::makeSp<IcModel>(sFile);
    // Add array of objs to Scene
    int N = K_objAry_N;
    float w = K_objAry_W;
    float h = K_objAry_H;
    for(int y=0;y<N;y++)
        for(int x=0;x<N;x++)
            for(int z=0;z<N;z++)
            {
                auto pObj   = ctl::makeSp<IcObject>(pModel);
                TVec3 pos((x-N/2)*w, (y-N/2)*h, (z-N/2)*w);
                pObj->setPos(pos);
                addObj(pObj);
            }

}
<|endoftext|>


<commit_before>/**
 * Copyright (c) 2016-present, Facebook, Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <condition_variable>
#include <mutex>
#include <stack>

#if !defined(_MSC_VER) && !defined(__APPLE__)
#include <sched.h>
#endif

#include "caffe2/core/context_gpu.h"
#include "caffe2/core/net_simple.h"
#include "caffe2/core/operator.h"
#include "caffe2/proto/caffe2.pb.h"

namespace caffe2 {

namespace gpu_single_thread {

struct Task {
  std::vector<std::unique_ptr<OperatorBase>>* ops_;
  std::condition_variable* cv_;
  std::mutex* mtx_;
  int stream_id_;
  bool done_ = false;
};

class GPUExecutor {
 public:
  explicit GPUExecutor(int gpu_id) : gpu_id_(gpu_id) {}

  ~GPUExecutor() {
    queue_.NoMoreJobs();
    thread_.join();
  }

  void RunJob(Task* task) {
    queue_.Push(task);
  }

  void start() {
    thread_ = std::thread(&GPUExecutor::WorkerFunction, this);
  }

  static std::shared_ptr<GPUExecutor> Get(int gpu);
  static void Release(int gpu);

 private:
  void set_affinity();
  void WorkerFunction();

  std::thread thread_;
  int gpu_id_;
  SimpleQueue<Task*> queue_;
  static std::shared_ptr<GPUExecutor> executors_[CAFFE2_COMPILE_TIME_MAX_GPUS];
  static std::mutex gpu_mtx_[CAFFE2_COMPILE_TIME_MAX_GPUS];
};

std::shared_ptr<GPUExecutor>
    GPUExecutor::executors_[CAFFE2_COMPILE_TIME_MAX_GPUS];
std::mutex GPUExecutor::gpu_mtx_[CAFFE2_COMPILE_TIME_MAX_GPUS];

std::shared_ptr<GPUExecutor> GPUExecutor::Get(int gpu) {
  std::lock_guard<std::mutex> grd(gpu_mtx_[gpu]);
  if (!executors_[gpu].get()) {
    executors_[gpu].reset(new GPUExecutor(gpu));
    executors_[gpu].get()->start();
  }
  return executors_[gpu];
}

void GPUExecutor::Release(int gpu) {
  std::lock_guard<std::mutex> grd(gpu_mtx_[gpu]);
  if (executors_[gpu].use_count() == 1) {
    executors_[gpu].reset();
  }
}

void GPUExecutor::set_affinity() {
// TODO: find a Windows-compatible affinity setting approach.
// Currently, set_affinity has no effect in Windows. The code is still
// correct with possible slowdowns.
#if !defined(_MSC_VER) && !defined(__APPLE__)
  /* Set CPU affinity */
  int num_cores = std::thread::hardware_concurrency();
  if (num_cores > 0) {
    cpu_set_t mask;
    CPU_ZERO(&mask);

    CPU_SET(gpu_id_ % num_cores, &mask);
    if (sched_setaffinity(0, sizeof(cpu_set_t), &mask)) {
      LOG(WARNING) << "Could not set CPU affinity";
    }
  }
#endif
}

// Worker that takes list of operators from the queue
// and executes them.
void GPUExecutor::WorkerFunction() {
  int stream_id_seq = 0;
  std::stack<int> streams;
  set_affinity();

  while (true) {
    Task* task = nullptr;
    vector<Task*> task_batch;

    if (!queue_.Pop(&task)) {
      return;
    }
    int num_tasks = 1 + queue_.size();

    // Grab all tasks currently in queue so we can run them in parallel
    // Since we have only one producer, we know this does not block

    // TODO: launch ops in "zig-zag" manner so that we can start multiple
    // streams as simultaneously as possible
    for (int i = num_tasks - 1; i >= 0; i--) {
      assert(task != nullptr);
      if (streams.empty()) {
        task->stream_id_ = stream_id_seq++;
      } else {
        task->stream_id_ = streams.top();
        streams.pop();
      }

      for (auto& op : *task->ops_) {
        op->RunAsync(task->stream_id_);
      }
      task_batch.push_back(task);

      // Get the next one
      if (i > 0) {
        if (!queue_.Pop(&task)) {
          return;
        }
      }
    }

    // Wait for the currently executing streams
    for (auto& pendtask : task_batch) {
      cudaStream_t stream =
          CUDAContext::cuda_stream(gpu_id_, pendtask->stream_id_);
      CUDA_ENFORCE(cudaStreamSynchronize(stream));
      streams.push(pendtask->stream_id_);
      std::unique_lock<std::mutex> lk(*pendtask->mtx_);
      pendtask->done_ = true;
      pendtask->cv_->notify_one();
    }
  }
}

class SingleThreadAsyncNet : public SimpleNet {
 public:
  using SimpleNet::SimpleNet;

  ~SingleThreadAsyncNet() {
    if (executor_.get()) {
      // Explicitly reset my holding of the exeuctor so it can be
      // killed.
      executor_.reset();
      GPUExecutor::Release(gpu_id_);
    }
  }

  bool Run() {
    if (!executor_.get()) {
      initialize();
    }

    // Dispatch jobs to the gpu-specific executor thread
    std::unique_lock<std::mutex> lk(mutex_);
    Task t;
    t.ops_ = &operators_;
    t.cv_ = &cv_;
    t.mtx_ = &mutex_;
    t.done_ = false;
    executor_.get()->RunJob(&t);

    while (!t.done_) {
      cv_.wait(lk);
    }

    return true;
  }

 private:
  std::condition_variable cv_;
  std::mutex mutex_;

  void initialize() {
    std::lock_guard<std::mutex> grd(mutex_);

    /* Check the gpu id of this net and check that only one
       GPU has operators on this net */
    gpu_id_ = (-1);
    for (auto& op : operators_) {
      if (op->device_option().device_type() == CUDA) {
        if (gpu_id_ < 0) {
          gpu_id_ = op->device_option().cuda_gpu_id();
        } else {
          CAFFE_ENFORCE_EQ(
              gpu_id_,
              op->device_option().cuda_gpu_id(),
              "One net can only have operators for one GPU");
        }
      }
    }
    executor_ = GPUExecutor::Get(gpu_id_);
  }

  int gpu_id_;
  std::shared_ptr<GPUExecutor> executor_;
};

REGISTER_NET(singlethread_async, SingleThreadAsyncNet)

} // namespace gpu_single_thread
} // namespace caffe2
<commit_msg>Update SingleThreadAsyncNet<commit_after>/**
 * Copyright (c) 2016-present, Facebook, Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <condition_variable>
#include <mutex>
#include <stack>

#if !defined(_MSC_VER) && !defined(__APPLE__)
#include <sched.h>
#endif

#include "caffe2/core/context_gpu.h"
#include "caffe2/core/net_simple.h"
#include "caffe2/core/operator.h"
#include "caffe2/proto/caffe2.pb.h"

namespace caffe2 {

namespace gpu_single_thread {

struct Task {
  std::vector<std::unique_ptr<OperatorBase>>* ops_;
  std::condition_variable* cv_;
  std::mutex* mtx_;
  int stream_id_;
  bool done_ = false;
};

class GPUExecutor {
 public:
  explicit GPUExecutor(int gpu_id) : gpu_id_(gpu_id) {}

  ~GPUExecutor() {
    queue_.NoMoreJobs();
    thread_.join();
  }

  void RunJob(Task* task) {
    queue_.Push(task);
  }

  void start() {
    thread_ = std::thread(&GPUExecutor::WorkerFunction, this);
  }

  static std::shared_ptr<GPUExecutor> Get(int gpu);
  static void Release(int gpu);

 private:
  void set_affinity();
  void WorkerFunction();

  std::thread thread_;
  int gpu_id_;
  SimpleQueue<Task*> queue_;
  static std::shared_ptr<GPUExecutor> executors_[CAFFE2_COMPILE_TIME_MAX_GPUS];
  static std::mutex gpu_mtx_[CAFFE2_COMPILE_TIME_MAX_GPUS];
};

std::shared_ptr<GPUExecutor>
    GPUExecutor::executors_[CAFFE2_COMPILE_TIME_MAX_GPUS];
std::mutex GPUExecutor::gpu_mtx_[CAFFE2_COMPILE_TIME_MAX_GPUS];

std::shared_ptr<GPUExecutor> GPUExecutor::Get(int gpu) {
  std::lock_guard<std::mutex> grd(gpu_mtx_[gpu]);
  if (!executors_[gpu].get()) {
    executors_[gpu].reset(new GPUExecutor(gpu));
    executors_[gpu].get()->start();
  }
  return executors_[gpu];
}

void GPUExecutor::Release(int gpu) {
  std::lock_guard<std::mutex> grd(gpu_mtx_[gpu]);
  if (executors_[gpu].use_count() == 1) {
    executors_[gpu].reset();
  }
}

void GPUExecutor::set_affinity() {
// TODO: find a Windows-compatible affinity setting approach.
// Currently, set_affinity has no effect in Windows. The code is still
// correct with possible slowdowns.
#if !defined(_MSC_VER) && !defined(__APPLE__)
  /* Set CPU affinity */
  int num_cores = std::thread::hardware_concurrency();
  if (num_cores > 0) {
    cpu_set_t mask;
    CPU_ZERO(&mask);

    CPU_SET(gpu_id_ % num_cores, &mask);
    if (sched_setaffinity(0, sizeof(cpu_set_t), &mask)) {
      LOG(WARNING) << "Could not set CPU affinity";
    }
  }
#endif
}

// Worker that takes list of operators from the queue
// and executes them.
void GPUExecutor::WorkerFunction() {
  int stream_id_seq = 0;
  std::stack<int> streams;
  set_affinity();

  while (true) {
    Task* task = nullptr;
    vector<Task*> task_batch;

    if (!queue_.Pop(&task)) {
      return;
    }
    int num_tasks = 1 + queue_.size();

    // Grab all tasks currently in queue so we can run them in parallel
    // Since we have only one producer, we know this does not block

    // TODO: launch ops in "zig-zag" manner so that we can start multiple
    // streams as simultaneously as possible
    for (int i = num_tasks - 1; i >= 0; i--) {
      assert(task != nullptr);
      if (streams.empty()) {
        task->stream_id_ = stream_id_seq++;
      } else {
        task->stream_id_ = streams.top();
        streams.pop();
      }

      for (auto& op : *task->ops_) {
        op->RunAsync(task->stream_id_);
      }
      task_batch.push_back(task);

      // Get the next one
      if (i > 0) {
        if (!queue_.Pop(&task)) {
          return;
        }
      }
    }

    // Wait for the currently executing streams
    for (auto& pendtask : task_batch) {
      cudaStream_t stream =
          CUDAContext::cuda_stream(gpu_id_, pendtask->stream_id_);
      CUDA_ENFORCE(cudaStreamSynchronize(stream));
      streams.push(pendtask->stream_id_);
      std::unique_lock<std::mutex> lk(*pendtask->mtx_);
      pendtask->done_ = true;
      pendtask->cv_->notify_one();
    }
  }
}

class SingleThreadAsyncNet : public SimpleNet {
 public:
  using SimpleNet::SimpleNet;

  ~SingleThreadAsyncNet() {
    if (executor_.get()) {
      // Explicitly reset my holding of the exeuctor so it can be
      // killed.
      executor_.reset();
      GPUExecutor::Release(gpu_id_);
    }
  }

  bool DoRunAsync() override {
    if (!executor_.get()) {
      initialize();
    }

    // Dispatch jobs to the gpu-specific executor thread
    std::unique_lock<std::mutex> lk(mutex_);
    Task t;
    t.ops_ = &operators_;
    t.cv_ = &cv_;
    t.mtx_ = &mutex_;
    t.done_ = false;
    executor_.get()->RunJob(&t);

    while (!t.done_) {
      cv_.wait(lk);
    }

    return true;
  }

 private:
  std::condition_variable cv_;
  std::mutex mutex_;

  void initialize() {
    std::lock_guard<std::mutex> grd(mutex_);

    /* Check the gpu id of this net and check that only one
       GPU has operators on this net */
    gpu_id_ = (-1);
    for (auto& op : operators_) {
      if (op->device_option().device_type() == CUDA) {
        if (gpu_id_ < 0) {
          gpu_id_ = op->device_option().cuda_gpu_id();
        } else {
          CAFFE_ENFORCE_EQ(
              gpu_id_,
              op->device_option().cuda_gpu_id(),
              "One net can only have operators for one GPU");
        }
      }
    }
    executor_ = GPUExecutor::Get(gpu_id_);
  }

  int gpu_id_;
  std::shared_ptr<GPUExecutor> executor_;
};

REGISTER_NET(singlethread_async, SingleThreadAsyncNet)

} // namespace gpu_single_thread
} // namespace caffe2
<|endoftext|>


<commit_before>/*******************************************************************************
 * BAREFOOT NETWORKS CONFIDENTIAL & PROPRIETARY
 *
 * Copyright (c) 2017-2021 Barefoot Networks, Inc.

 * All Rights Reserved.
 *
 * NOTICE: All information contained herein is, and remains the property of
 * Barefoot Networks, Inc. and its suppliers, if any. The intellectual and
 * technical concepts contained herein are proprietary to Barefoot Networks,
 * Inc.
 * and its suppliers and may be covered by U.S. and Foreign Patents, patents in
 * process, and are protected by trade secret or copyright law.
 * Dissemination of this information or reproduction of this material is
 * strictly forbidden unless prior written permission is obtained from
 * Barefoot Networks, Inc.
 *
 * No warranty, explicit or implicit is provided, unless granted under a
 * written agreement with Barefoot Networks, Inc.
 *
 * $Id: $
 *
 ******************************************************************************/
/** @file tdi_rt_init.hpp
 *
 *  @brief Contains TDI Dev Manager APIs. These APIs help manage TdiInfo
 *  \n objects with respect to the devices and programs in the ecosystem.
 *
 *  \n Contains DevMgr, Device, Target, Flags
 */
#ifndef _TDI_RT_INIT_HPP_
#define _TDI_RT_INIT_HPP_

#include <functional>
#include <memory>
#include <mutex>

// tdi includes
//#include <tdi/common/tdi_target.hpp>
//#include <tdi/common/tdi_info.hpp>
//#include <tdi/common/tdi_session.hpp>

// pna includes
#include <tdi/arch/pna/pna_init.hpp>

// local includes

#ifdef __cplusplus
extern "C" {
#endif

#include <dvm/bf_drv_intf.h>

#ifdef __cplusplus
}
#endif

namespace tdi {
namespace pna {
namespace rt {

/**
 * @brief Class which encapsulates static info of a device eg.
 * Arch type,
 * Mgrs it was started with, State information, TdiInfo of all
 * programs, Pipeline profile information.
 *
 * Static info means that none of this can be changed after
 * device add happens.
 */
class Device : public tdi::pna::Device {
 public:
  Device(const tdi_dev_id_t &device_id,
         const tdi_arch_type_e &arch_type,
         const std::vector<tdi::ProgramConfig> &device_config,
         const std::vector<tdi_mgr_type_e> mgr_type_list,
         void *cookie);

  virtual tdi_status_t createSession(
      std::shared_ptr<tdi::Session> * /*session*/) const override final {
    return TDI_SUCCESS;
  }
  virtual tdi_status_t createTarget(
      std::unique_ptr<tdi::Target> * target) const override final {
    *target=std::unique_ptr<tdi::Target>(new tdi::pna::Target(this->device_id_, 0, PNA_DIRECTION_ALL));
    return TDI_SUCCESS;
  }
  virtual tdi_status_t createFlags(
      const uint64_t & /*flags_val*/,
      std::unique_ptr<tdi::Flags> * /*flags*/) const override final {
    return TDI_SUCCESS;
  }

 private:
  // This is where the real state map wil go
  // std::map<std::string, std::shared_ptr<DeviceState>> tdi_dev_state_map_;
};

/**
 * @brief Class to manage initialization of TDI

 * Creation:  Cannot be created
 */
class Init : public tdi::Init {
 public:
  /**
   * @brief Bf Rt Module Init API. This function needs to be called to
   * initialize TDI. Some specific managers can be specified to be skipped
   * TDI initialization. This allows TDI session layer to not know about these
   * managers. By default, no mgr initialization is skipped if empty vector is
   * passed
   *
   * @param[in] mgr_type_list vector of mgrs to skip initializing. If
   * empty, don't skip anything
   * @return Status of the API call
   */
  static tdi_status_t tdiModuleInit(
      const std::vector<tdi_mgr_type_e> mgr_type_list);
  static std::string tdi_module_name;
  static bf_drv_client_handle_t tdi_drv_hdl;
};  // Init

}  // namespace rt
}  // namespace pna
}  // namespace tdi

#endif
<commit_msg>need to include the proper path for tdi_defs.h.<commit_after>/*******************************************************************************
 * BAREFOOT NETWORKS CONFIDENTIAL & PROPRIETARY
 *
 * Copyright (c) 2017-2021 Barefoot Networks, Inc.

 * All Rights Reserved.
 *
 * NOTICE: All information contained herein is, and remains the property of
 * Barefoot Networks, Inc. and its suppliers, if any. The intellectual and
 * technical concepts contained herein are proprietary to Barefoot Networks,
 * Inc.
 * and its suppliers and may be covered by U.S. and Foreign Patents, patents in
 * process, and are protected by trade secret or copyright law.
 * Dissemination of this information or reproduction of this material is
 * strictly forbidden unless prior written permission is obtained from
 * Barefoot Networks, Inc.
 *
 * No warranty, explicit or implicit is provided, unless granted under a
 * written agreement with Barefoot Networks, Inc.
 *
 * $Id: $
 *
 ******************************************************************************/
/** @file tdi_rt_init.hpp
 *
 *  @brief Contains TDI Dev Manager APIs. These APIs help manage TdiInfo
 *  \n objects with respect to the devices and programs in the ecosystem.
 *
 *  \n Contains DevMgr, Device, Target, Flags
 */
#ifndef _TDI_RT_INIT_HPP_
#define _TDI_RT_INIT_HPP_

#include <functional>
#include <memory>
#include <mutex>

// tdi includes
//#include <tdi/common/tdi_target.hpp>
//#include <tdi/common/tdi_info.hpp>
//#include <tdi/common/tdi_session.hpp>

// pna includes
#include <tdi/arch/pna/pna_init.hpp>

// local includes

#ifdef __cplusplus
extern "C" {
#endif
#include <tdi/common/tdi_defs.h>
#include <dvm/bf_drv_intf.h>

#ifdef __cplusplus
}
#endif

namespace tdi {
namespace pna {
namespace rt {

/**
 * @brief Class which encapsulates static info of a device eg.
 * Arch type,
 * Mgrs it was started with, State information, TdiInfo of all
 * programs, Pipeline profile information.
 *
 * Static info means that none of this can be changed after
 * device add happens.
 */
class Device : public tdi::pna::Device {
 public:
  Device(const tdi_dev_id_t &device_id,
         const tdi_arch_type_e &arch_type,
         const std::vector<tdi::ProgramConfig> &device_config,
         const std::vector<tdi_mgr_type_e> mgr_type_list,
         void *cookie);

  virtual tdi_status_t createSession(
      std::shared_ptr<tdi::Session> * /*session*/) const override final {
    return TDI_SUCCESS;
  }
  virtual tdi_status_t createTarget(
      std::unique_ptr<tdi::Target> * target) const override final {
    *target=std::unique_ptr<tdi::Target>(new tdi::pna::Target(this->device_id_, 0, PNA_DIRECTION_ALL));
    return TDI_SUCCESS;
  }
  virtual tdi_status_t createFlags(
      const uint64_t & /*flags_val*/,
      std::unique_ptr<tdi::Flags> * /*flags*/) const override final {
    return TDI_SUCCESS;
  }

 private:
  // This is where the real state map wil go
  // std::map<std::string, std::shared_ptr<DeviceState>> tdi_dev_state_map_;
};

/**
 * @brief Class to manage initialization of TDI

 * Creation:  Cannot be created
 */
class Init : public tdi::Init {
 public:
  /**
   * @brief Bf Rt Module Init API. This function needs to be called to
   * initialize TDI. Some specific managers can be specified to be skipped
   * TDI initialization. This allows TDI session layer to not know about these
   * managers. By default, no mgr initialization is skipped if empty vector is
   * passed
   *
   * @param[in] mgr_type_list vector of mgrs to skip initializing. If
   * empty, don't skip anything
   * @return Status of the API call
   */
  static tdi_status_t tdiModuleInit(
      const std::vector<tdi_mgr_type_e> mgr_type_list);
  static std::string tdi_module_name;
  static bf_drv_client_handle_t tdi_drv_hdl;
};  // Init

}  // namespace rt
}  // namespace pna
}  // namespace tdi

#endif
<|endoftext|>


<commit_before>#ifndef V_SMC_CORE_PARTICLE_HPP
#define V_SMC_CORE_PARTICLE_HPP

#include <vector>
#include <cmath>
#include <cstddef>
#include <boost/function.hpp>
#include <boost/random/binomial_distribution.hpp>
#include <boost/random/uniform_01.hpp>
#include <Eigen/Dense>
#include <vSMC/internal/rng.hpp>
#include <vSMC/internal/version.hpp>

namespace vSMC {

/// Resample scheme
enum ResampleScheme {
    MULTINOMIAL, RESIDUAL, STRATIFIED, SYSTEMATIC, RESIDUAL_STRATIFIED};

/// \brief Particle class
///
/// Particle class store the particle set and arrays of weights and log
/// weights. It provides access to particle values as well as weights. It
/// computes and manages resources for ESS, resampling, etc, tasks unique to
/// each iteration.
template <typename T>
class Particle
{
    public :

    typedef V_SMC_RNG_TYPE rng_type;

    /// \brief Construct a Particle object with given number of particles
    ///
    /// \param N The number of particles
    /// \param seed The seed to the parallel RNG system
    Particle (std::size_t N, rng_type::seed_type seed = V_SMC_RNG_SEED) :
        size_(N), value_(N),
        weight_(N), log_weight_(N), inc_weight_(N), replication_(N),
        ess_(0), resampled_(false), zconst_(0), prng_(N)
    {
        rng_type rng(seed);
        rng.step_size(size_);
        for (std::size_t i = 0; i != size_; ++i) {
            rng.advance_ctr(i);
            prng_[i] = rng;
        }

        weight_.setConstant(1.0 / size_);
        log_weight_.setConstant(0);
    }

    /// \brief Size of the particle set
    ///
    /// \return The number of particles
    std::size_t size () const
    {
        return size_;
    }

    /// \brief Read and write access to particle values
    ///
    /// \return A reference to the particle values, type (T &)
    ///
    /// \note The Particle class guarantee that during the life type of the
    /// object, the reference returned by this member will no be a dangle
    /// handler.
    T &value ()
    {
        return value_;
    }

    /// \brief Read only access to particle values
    ///
    /// \return A const reference to the particle values
    const T &value () const
    {
        return value_;
    }

    /// \brief Read only access to the weights
    ///
    /// \return A const pointer to the weights
    ///
    /// \note The Particle class guarantee that during the life type of the
    /// object, the pointer returned by this always valid and point to the
    /// same address
    const double *weight_ptr () const
    {
        return weight_.data();
    }

    /// \brief Read only access to the log weights
    ///
    /// \return A const pointer to the log weights
    const double *log_weight_ptr () const
    {
        return log_weight_.data();
    }

    const Eigen::VectorXd &weight () const
    {
        return weight_;
    }

    const Eigen::VectorXd &log_weight () const
    {
        return log_weight_;
    }

    /// \brief Set the log weights
    ///
    /// \param new_weight New log weights
    /// \param delta A multiplier appiled to new_weight
    void set_log_weight (const double *new_weight, double delta = 1)
    {
        Eigen::Map<const Eigen::VectorXd> w(new_weight, size_);
        log_weight_ = delta * w;
        set_weight();
    }

    /// \brief Add to the log weights
    ///
    /// \param inc_weight Incremental log weights
    /// \param delta A multiplier applied to inc_weight
    /// \param add_zconst Whether this incremental weights should contribute
    /// the esitmates of normalizing constants
    void add_log_weight (const double *inc_weight, double delta = 1,
            bool add_zconst = true)
    {
        Eigen::Map<const Eigen::VectorXd> w(inc_weight, size_);
        if (add_zconst) {
            inc_weight_ = (delta * w).array().exp();
            zconst_ += std::log(weight_.dot(inc_weight_));
        }
        log_weight_ += delta * w;
        set_weight();
    }

    /// \brief The ESS (Effective Sample Size)
    ///
    /// \return The value of ESS for current particle set
    double ess () const
    {
        return ess_;
    }

    /// \brief Get indicator of resampling
    ///
    /// \return A bool value, \b true if the current iteration was resampled
    bool resampled () const
    {
        return resampled_;
    }

    /// \brief Set indicator of resampling
    ///
    /// \param resampled \b true if the current iteration was resampled
    void resampled (bool resampled)
    {
        resampled_ = resampled;
    }

    /// \brief Get the value of SMC normalizing constant
    ///
    /// \return SMC normalizng constant estimate
    double zconst () const
    {
        return zconst_;
    }

    /// \brief Reset the value of SMC normalizing constant
    void reset_zconst ()
    {
        zconst_ = 0;
    }

    /// \brief Perform resampling
    ///
    /// \param scheme The resampling scheme, see ResamplingScheme
    void resample (ResampleScheme scheme)
    {
        switch (scheme) {
            case MULTINOMIAL :
                resample_multinomial();
                break;
            case RESIDUAL :
                resample_residual();
                break;
            case STRATIFIED :
                resample_stratified();
                break;
            case SYSTEMATIC :
                resample_systematic();
                break;
            case RESIDUAL_STRATIFIED :
                resample_residual_stratified ();
                break;
            default :
                throw std::runtime_error(
                        "ERROR: vSMC::Particle::resample: "
                        "Unknown Resample Scheme");
        }
    }

    rng_type &prng (std::size_t id)
    {
        return prng_[id];
    }

    private :

    std::size_t size_;
    T value_;

    Eigen::VectorXd weight_;
    Eigen::VectorXd log_weight_;
    Eigen::VectorXd inc_weight_;
    Eigen::VectorXi replication_;

    double ess_;
    bool resampled_;
    double zconst_;

    std::vector<rng_type> prng_;
    typedef boost::random::binomial_distribution<> binom_type;
    binom_type binom_;

    void set_weight ()
    {
        double max_weight = log_weight_.maxCoeff();
        log_weight_ = log_weight_.array() - max_weight;
        weight_ = log_weight_.array().exp();
        double sum = weight_.sum();
        weight_ *= 1 / sum;
        ess_ = 1 / weight_.squaredNorm();
    }

    void resample_multinomial ()
    {
        weight2replication(size_);
        resample_do();
    }

    void resample_residual ()
    {
        // Reuse weight and log_weight. weight: act as the fractional part of
        // N * weight. log_weight: act as the integral part of N * weight.
        // They all will be reset to equal weights after resampling.  So it is
        // safe to modify them here.
        for (std::size_t i = 0; i != size_; ++i)
            weight_[i] = std::modf(size_ * weight_[i], log_weight_.data() + i);
        std::size_t size = size_ - log_weight_.sum();
        weight2replication(size);
        for (std::size_t i = 0; i != size_; ++i)
            replication_[i] += log_weight_[i];
        resample_do();
    }

    void resample_stratified ()
    {
        replication_.setConstant(0);
        std::size_t j = 0;
        std::size_t k = 0;
        boost::random::uniform_01<> unif;
        double u = unif(prng_[0]);
        double cw = weight_[0];
        while (j != size_) {
            while (j < cw * size_ - u && j != size_) {
                ++replication_[k];
                u = unif(prng_[j++]);
            }
            cw += weight_[++k];
        }
        resample_do();
    }

    void resample_systematic ()
    {
        replication_.setConstant(0);
        std::size_t j = 0;
        std::size_t k = 0;
        boost::random::uniform_01<> unif;
        double u = unif(prng_[0]);
        double cw = weight_[0];
        while (j != size_) {
            while (j < cw * size_ - u && j != size_) {
                ++replication_[k];
		++j;
            }
            cw += weight_[++k];
        }
        resample_do();
    }

    void resample_residual_stratified ()
    {
        for (std::size_t i = 0; i != size_; ++i)
            weight_[i] = std::modf(size_ * weight_[i], log_weight_.data() + i);
        resample_stratified();
        for (std::size_t i = 0; i != size_; ++i)
            replication_[i] += log_weight_[i];
        resample_do();
    }

    void weight2replication (std::size_t size)
    {
        double tp = weight_.sum();
        double sum_p = 0;
        std::size_t sum_n = 0;
        replication_.setConstant(0);
        for (std::size_t i = 0; i != size_; ++i) {
            if (sum_n < size && weight_[i] > 0) {
                binom_type::param_type
                    param(size - sum_n, weight_[i] / (tp - sum_p));
                replication_[i] = binom_(prng_[i], param);
            }
            sum_p += weight_[i];
            sum_n += replication_[i];
        }
    }

    void resample_do ()
    {
        std::size_t from = 0;
        std::size_t time = 0;
        for (std::size_t to = 0; to != size_; ++to) {
            if (!replication_[to]) {
                // replication_[to] has zero child, copy from elsewhere
                if (replication_[from] - time <= 1) {
                    // only 1 child left on replication_[from]
                    time = 0;
                    do // move from to some position with at least 2 children
                        ++from;
                    while (replication_[from] < 2);
                }
                value_.copy(from, to);
                ++time;
            }
        }

        ess_ = size_;
        weight_.setConstant(1.0 / size_);
        log_weight_.setConstant(0);
    }
}; // class Particle

} // namespace vSMC

#endif // V_SMC_CORE_PARTICLE_HPP
<commit_msg>restore the check of case where replication does not sum up to number of particles<commit_after>#ifndef V_SMC_CORE_PARTICLE_HPP
#define V_SMC_CORE_PARTICLE_HPP

#include <vector>
#include <cmath>
#include <cstddef>
#include <boost/function.hpp>
#include <boost/random/binomial_distribution.hpp>
#include <boost/random/uniform_01.hpp>
#include <Eigen/Dense>
#include <vSMC/internal/rng.hpp>
#include <vSMC/internal/version.hpp>

namespace vSMC {

/// Resample scheme
enum ResampleScheme {
    MULTINOMIAL, RESIDUAL, STRATIFIED, SYSTEMATIC, RESIDUAL_STRATIFIED};

/// \brief Particle class
///
/// Particle class store the particle set and arrays of weights and log
/// weights. It provides access to particle values as well as weights. It
/// computes and manages resources for ESS, resampling, etc, tasks unique to
/// each iteration.
template <typename T>
class Particle
{
    public :

    typedef V_SMC_RNG_TYPE rng_type;

    /// \brief Construct a Particle object with given number of particles
    ///
    /// \param N The number of particles
    /// \param seed The seed to the parallel RNG system
    Particle (std::size_t N, rng_type::seed_type seed = V_SMC_RNG_SEED) :
        size_(N), value_(N),
        weight_(N), log_weight_(N), inc_weight_(N), replication_(N),
        ess_(0), resampled_(false), zconst_(0), prng_(N)
    {
        rng_type rng(seed);
        rng.step_size(size_);
        for (std::size_t i = 0; i != size_; ++i) {
            rng.advance_ctr(i);
            prng_[i] = rng;
        }

        weight_.setConstant(1.0 / size_);
        log_weight_.setConstant(0);
    }

    /// \brief Size of the particle set
    ///
    /// \return The number of particles
    std::size_t size () const
    {
        return size_;
    }

    /// \brief Read and write access to particle values
    ///
    /// \return A reference to the particle values, type (T &)
    ///
    /// \note The Particle class guarantee that during the life type of the
    /// object, the reference returned by this member will no be a dangle
    /// handler.
    T &value ()
    {
        return value_;
    }

    /// \brief Read only access to particle values
    ///
    /// \return A const reference to the particle values
    const T &value () const
    {
        return value_;
    }

    /// \brief Read only access to the weights
    ///
    /// \return A const pointer to the weights
    ///
    /// \note The Particle class guarantee that during the life type of the
    /// object, the pointer returned by this always valid and point to the
    /// same address
    const double *weight_ptr () const
    {
        return weight_.data();
    }

    /// \brief Read only access to the log weights
    ///
    /// \return A const pointer to the log weights
    const double *log_weight_ptr () const
    {
        return log_weight_.data();
    }

    const Eigen::VectorXd &weight () const
    {
        return weight_;
    }

    const Eigen::VectorXd &log_weight () const
    {
        return log_weight_;
    }

    /// \brief Set the log weights
    ///
    /// \param new_weight New log weights
    /// \param delta A multiplier appiled to new_weight
    void set_log_weight (const double *new_weight, double delta = 1)
    {
        Eigen::Map<const Eigen::VectorXd> w(new_weight, size_);
        log_weight_ = delta * w;
        set_weight();
    }

    /// \brief Add to the log weights
    ///
    /// \param inc_weight Incremental log weights
    /// \param delta A multiplier applied to inc_weight
    /// \param add_zconst Whether this incremental weights should contribute
    /// the esitmates of normalizing constants
    void add_log_weight (const double *inc_weight, double delta = 1,
            bool add_zconst = true)
    {
        Eigen::Map<const Eigen::VectorXd> w(inc_weight, size_);
        if (add_zconst) {
            inc_weight_ = (delta * w).array().exp();
            zconst_ += std::log(weight_.dot(inc_weight_));
        }
        log_weight_ += delta * w;
        set_weight();
    }

    /// \brief The ESS (Effective Sample Size)
    ///
    /// \return The value of ESS for current particle set
    double ess () const
    {
        return ess_;
    }

    /// \brief Get indicator of resampling
    ///
    /// \return A bool value, \b true if the current iteration was resampled
    bool resampled () const
    {
        return resampled_;
    }

    /// \brief Set indicator of resampling
    ///
    /// \param resampled \b true if the current iteration was resampled
    void resampled (bool resampled)
    {
        resampled_ = resampled;
    }

    /// \brief Get the value of SMC normalizing constant
    ///
    /// \return SMC normalizng constant estimate
    double zconst () const
    {
        return zconst_;
    }

    /// \brief Reset the value of SMC normalizing constant
    void reset_zconst ()
    {
        zconst_ = 0;
    }

    /// \brief Perform resampling
    ///
    /// \param scheme The resampling scheme, see ResamplingScheme
    void resample (ResampleScheme scheme)
    {
        switch (scheme) {
            case MULTINOMIAL :
                resample_multinomial();
                break;
            case RESIDUAL :
                resample_residual();
                break;
            case STRATIFIED :
                resample_stratified();
                break;
            case SYSTEMATIC :
                resample_systematic();
                break;
            case RESIDUAL_STRATIFIED :
                resample_residual_stratified ();
                break;
            default :
                throw std::runtime_error(
                        "ERROR: vSMC::Particle::resample: "
                        "Unknown Resample Scheme");
        }
    }

    rng_type &prng (std::size_t id)
    {
        return prng_[id];
    }

    private :

    std::size_t size_;
    T value_;

    Eigen::VectorXd weight_;
    Eigen::VectorXd log_weight_;
    Eigen::VectorXd inc_weight_;
    Eigen::VectorXi replication_;

    double ess_;
    bool resampled_;
    double zconst_;

    std::vector<rng_type> prng_;
    typedef boost::random::binomial_distribution<> binom_type;
    binom_type binom_;

    void set_weight ()
    {
        double max_weight = log_weight_.maxCoeff();
        log_weight_ = log_weight_.array() - max_weight;
        weight_ = log_weight_.array().exp();
        double sum = weight_.sum();
        weight_ *= 1 / sum;
        ess_ = 1 / weight_.squaredNorm();
    }

    void resample_multinomial ()
    {
        weight2replication(size_);
        resample_do();
    }

    void resample_residual ()
    {
        // Reuse weight and log_weight. weight: act as the fractional part of
        // N * weight. log_weight: act as the integral part of N * weight.
        // They all will be reset to equal weights after resampling.  So it is
        // safe to modify them here.
        for (std::size_t i = 0; i != size_; ++i)
            weight_[i] = std::modf(size_ * weight_[i], log_weight_.data() + i);
        std::size_t size = size_ - log_weight_.sum();
        weight2replication(size);
        for (std::size_t i = 0; i != size_; ++i)
            replication_[i] += log_weight_[i];
        resample_do();
    }

    void resample_stratified ()
    {
        replication_.setConstant(0);
        std::size_t j = 0;
        std::size_t k = 0;
        boost::random::uniform_01<> unif;
        double u = unif(prng_[0]);
        double cw = weight_[0];
        while (j != size_) {
            while (j < cw * size_ - u && j != size_) {
                ++replication_[k];
                u = unif(prng_[j++]);
            }
            cw += weight_[++k];
        }
        resample_do();
    }

    void resample_systematic ()
    {
        replication_.setConstant(0);
        std::size_t j = 0;
        std::size_t k = 0;
        boost::random::uniform_01<> unif;
        double u = unif(prng_[0]);
        double cw = weight_[0];
        while (j != size_) {
            while (j < cw * size_ - u && j != size_) {
                ++replication_[k];
		++j;
            }
            cw += weight_[++k];
        }
        resample_do();
    }

    void resample_residual_stratified ()
    {
        for (std::size_t i = 0; i != size_; ++i)
            weight_[i] = std::modf(size_ * weight_[i], log_weight_.data() + i);
        resample_stratified();
        for (std::size_t i = 0; i != size_; ++i)
            replication_[i] += log_weight_[i];
        resample_do();
    }

    void weight2replication (std::size_t size)
    {
        double tp = weight_.sum();
        double sum_p = 0;
        std::size_t sum_n = 0;
        replication_.setConstant(0);
        for (std::size_t i = 0; i != size_; ++i) {
            if (sum_n < size && weight_[i] > 0) {
                binom_type::param_type
                    param(size - sum_n, weight_[i] / (tp - sum_p));
                replication_[i] = binom_(prng_[i], param);
            }
            sum_p += weight_[i];
            sum_n += replication_[i];
        }
    }

    void resample_do ()
    {
	// Some times the nuemrical round error can cause the total childs
	// differ from number of particles
	std::size_t sum = replication_.sum();
	if (sum != size_) {
            Eigen::VectorXd::Index id_max;
            replication_.maxCoeff(&id_max);
            replication_[id_max] += size_ - sum;
	}

        std::size_t from = 0;
        std::size_t time = 0;
        for (std::size_t to = 0; to != size_; ++to) {
            if (!replication_[to]) {
                // replication_[to] has zero child, copy from elsewhere
                if (replication_[from] - time <= 1) {
                    // only 1 child left on replication_[from]
                    time = 0;
                    do // move from to some position with at least 2 children
                        ++from;
                    while (replication_[from] < 2);
                }
                value_.copy(from, to);
                ++time;
            }
        }

        ess_ = size_;
        weight_.setConstant(1.0 / size_);
        log_weight_.setConstant(0);
    }
}; // class Particle

} // namespace vSMC

#endif // V_SMC_CORE_PARTICLE_HPP
<|endoftext|>


<commit_before>#ifndef VSMC_CORE_PARTICLE_HPP
#define VSMC_CORE_PARTICLE_HPP

#include <vsmc/internal/common.hpp>
#include <vsmc/internal/resampling.hpp>

namespace vsmc {

/// \brief Particle class representing the whole particle set
/// \ingroup Core
///
/// \tparam T Requirement:
/// \li Constructor compatible with
/// \code T(Particle<T>::size_type N) \endcode
/// \li member function copy method compatible with
/// \code copy(Particle<T>::size_type from, Particle<T>::size_type to) \endcode
template <typename T>
class Particle
{
    public :

    /// The type of the size of the particle set
    typedef VSMC_INDEX_TYPE size_type;

    /// The type of the particle values
    typedef T value_type;

#ifdef VSMC_USE_SEQUENTIAL_RNG
    /// The type of the Counter-based random number generator C++11 engine
    typedef VSMC_SEQRNG_TYPE rng_type;
#else
    /// The type of the Counter-based random number generator
    typedef VSMC_CBRNG_TYPE cbrng_type;

    /// The type of the Counter-based random number generator C++11 engine
    typedef rng::Engine<cbrng_type> rng_type;
#endif

    /// The integer type of the seed
    typedef rng_type::result_type seed_type;

    /// The type of the weight and log weight vector
    typedef Eigen::VectorXd weight_type;

    /// \brief Construct a Particle object with a given number of particles
    ///
    /// \param N The number of particles
    /// \param seed The seed to the parallel RNG system
    ///
    /// \post All weights are initialized to be euqal to each other
    explicit Particle (size_type N, seed_type seed = VSMC_RNG_SEED) :
        size_(N), value_(N),
        weight_(N), log_weight_(N), inc_weight_(N), replication_(N),
        ess_(N), resampled_(false), zconst_(0), seed_(seed)
#ifndef VSMC_USE_SEQUENTIAL_RNG
        , prng_(N)
#endif
    {
        reset_rng();
        set_equal_weight();
    }

    /// \brief Size of the particle set
    ///
    /// \return The number of particles
    size_type size () const
    {
        return size_;
    }

    /// \brief Read and write access to particle values
    ///
    /// \return A reference to the particle values
    value_type &value ()
    {
        return value_;
    }

    /// \brief Read only access to particle values
    ///
    /// \return A const reference to the particle values
    const value_type &value () const
    {
        return value_;
    }

    /// \brief Get the weight of a single particle
    ///
    /// \param id The position of the particle, 0 to size() - 1
    ///
    /// \return The weight of the particle at position id
    double weight (size_type id) const
    {
        return weight_[id];
    }

    /// \brief Read only access to the weights through pointer
    ///
    /// \return A const pointer to the weight array
    const double *weight_ptr () const
    {
        return weight_.data();
    }

    /// \brief Read only access to the weights through Eigen vector
    ///
    /// \return A const reference to the weight vector
    const weight_type &weight () const
    {
        return weight_;
    }

    /// \brief Get the log weight of a single particle
    ///
    /// \param id The position of the particle, 0 to size() - 1
    ///
    /// \return The log weight of the particle at position id
    double log_weight (size_type id) const
    {
        return log_weight_[id];
    }

    /// \brief Read only access to the log weights through pointer
    ///
    /// \return A const pointer to the log weight array
    const double *log_weight_ptr () const
    {
        return log_weight_.data();
    }

    /// \brief Read only access to the log weights through Eigen vector
    ///
    /// \return A const reference to the log weight vector
    const weight_type &log_weight () const
    {
        return log_weight_;
    }

    /// \brief Set equal weights for all particles
    void set_equal_weight ()
    {
        ess_ = size_;
        weight_.setConstant(1.0 / size_);
        log_weight_.setConstant(0);
    }

    /// \brief Set the log weights
    ///
    /// \param new_weight New log weights
    /// \param delta A multiplier appiled to new_weight
    void set_log_weight (const double *new_weight, double delta = 1)
    {
        Eigen::Map<const weight_type> w(new_weight, size_);
        set_log_weight(w, delta);
    }

    /// \brief Set the log weights
    ///
    /// \param new_weight New log weights
    /// \param delta A multiplier appiled to new_weight
    void set_log_weight (const weight_type &new_weight, double delta = 1)
    {
        log_weight_ = delta * new_weight.head(size_);
        set_weight();
    }

    /// \brief Add to the log weights
    ///
    /// \param inc_weight Incremental log weights
    /// \param delta A multiplier applied to inc_weight
    /// \param add_zconst Whether this incremental weights should contribute
    /// the esitmates of normalizing constants
    void add_log_weight (const double *inc_weight, double delta = 1,
            bool add_zconst = true)
    {
        Eigen::Map<const weight_type> w(inc_weight, size_);
        add_log_weight(w, delta, add_zconst);
    }

    /// \brief Add to the log weights
    ///
    /// \param inc_weight Incremental log weights
    /// \param delta A multiplier applied to inc_weight
    /// \param add_zconst Whether this incremental weights should contribute
    /// the esitmates of normalizing constants
    void add_log_weight (const weight_type &inc_weight, double delta = 1,
            bool add_zconst = true)
    {
        using std::log;

        if (add_zconst) {
            inc_weight_ = (delta * inc_weight.head(size_)).array().exp();
            zconst_ += log(weight_.dot(inc_weight_.head(size_)));
        }
        log_weight_ += delta * inc_weight.head(size_);
        set_weight();
    }

    /// \brief The ESS (Effective Sample Size)
    ///
    /// \return The value of ESS for current particle set
    double ess () const
    {
        return ess_;
    }

    /// \brief Get indicator of resampling
    ///
    /// \return \b true if the current iteration was resampled
    bool resampled () const
    {
        return resampled_;
    }

    /// \brief Get the value of SMC normalizing constant
    ///
    /// \return Log of SMC normalizng constant estimate
    double zconst () const
    {
        return zconst_;
    }

    /// \brief Reset the value of SMC normalizing constant
    void reset_zconst ()
    {
        zconst_ = 0;
    }

    /// \brief Perform resampling
    ///
    /// \param scheme The resampling scheme, see ResamplingScheme
    /// \param threshold The threshold for resampling
    void resample (ResampleScheme scheme, double threshold)
    {
        resampled_ = ess_ < threshold * size_;

        if (resampled_) {
            internal::pre_resampling(value_);
            switch (scheme) {
                case MULTINOMIAL :
                    resample_multinomial();
                    break;
                case RESIDUAL :
                    resample_residual();
                    break;
                case STRATIFIED :
                    resample_stratified();
                    break;
                case SYSTEMATIC :
                    resample_systematic();
                    break;
                case RESIDUAL_STRATIFIED :
                    resample_residual_stratified ();
                    break;
                default :
                    resample_stratified();
                    break;
            }
            resample_do();
            internal::post_resampling(value_);
        }
    }

    /// \brief Get a C++11 RNG engine
    ///
    /// \param id The position of the particle, 0 to size() - 1
    ///
    /// \return A reference to a C++11 RNG engine unique to particle at
    /// position id, and independent of others if \c VSMC_USE_SEQUENTIAL_RNG is
    /// not defined. Otherwise, it is the same pseudo RNG for any value of \c
    /// id
    rng_type &rng (size_type id)
    {
#ifdef VSMC_USE_SEQUENTIAL_RNG
        return srng_;
#else
        return prng_[id];
#endif
    }

    /// \brief Reset the parallel RNG system
    ///
    /// \param seed The new seed to the system
    void reset_rng (seed_type seed)
    {
        seed_ = seed;
        reset_rng();
    }

    /// \brief Reset the parallel RNG system using last time used seed
    void reset_rng ()
    {
#ifdef VSMC_USE_SEQUENTIAL_RNG
        srng_ = rng_type(seed_);
#else
        for (size_type i = 0; i != size_; ++i)
            prng_[i] = rng_type(seed_ + i);
#endif
    }

    private :

    typedef Eigen::Matrix<size_type, Eigen::Dynamic, 1> replication_type;

    size_type size_;
    value_type value_;

    weight_type weight_;
    weight_type log_weight_;
    weight_type inc_weight_;
    replication_type replication_;

    double ess_;
    bool resampled_;
    double zconst_;

    seed_type seed_;
#ifdef VSMC_USE_SEQUENTIAL_RNG
    rng_type srng_;
#else
    std::deque<rng_type> prng_;
#endif

    void set_weight ()
    {
        double max_weight = log_weight_.maxCoeff();
        log_weight_ = log_weight_.array() - max_weight;
        weight_ = log_weight_.array().exp();
        double sum = weight_.sum();
        weight_ *= 1 / sum;
        ess_ = 1 / weight_.squaredNorm();
    }

    void resample_multinomial ()
    {
        weight2replication(size_);
    }

    void resample_residual ()
    {
        /// \internal Reuse weight and log_weight. weight: act as the
        /// fractional part of N * weight. log_weight: act as the integral
        /// part of N * weight.  They all will be reset to equal weights after
        /// resampling.  So it is safe to modify them here.
        using std::modf;

        for (size_type i = 0; i != size_; ++i)
            weight_[i] = modf(size_ * weight_[i], log_weight_.data() + i);
        weight2replication(weight_.sum());
        for (size_type i = 0; i != size_; ++i)
            replication_[i] += log_weight_[i];
    }

    void resample_stratified ()
    {
        replication_.setConstant(0);
        size_type j = 0;
        size_type k = 0;
        rng::uniform_real_distribution<double> unif(0,1);
        double u = unif(rng(0));
        double cw = weight_[0];
        while (j != size_) {
            while (j < cw * size_ - u && j != size_) {
                ++replication_[k];
                u = unif(rng(j));
                ++j;
            }
            if (k == size_ - 1)
                break;
            cw += weight_[++k];
        }
    }

    void resample_systematic ()
    {
        replication_.setConstant(0);
        size_type j = 0;
        size_type k = 0;
        rng::uniform_real_distribution<double> unif(0,1);
        double u = unif(rng(0));
        double cw = weight_[0];
        while (j != size_) {
            while (j < cw * size_ - u && j != size_) {
                ++replication_[k];
                ++j;
            }
            if (k == size_ - 1)
                break;
            cw += weight_[++k];
        }
    }

    void resample_residual_stratified ()
    {
        using std::modf;

        replication_.setConstant(0);
        for (size_type i = 0; i != size_; ++i)
            weight_[i] = modf(size_ * weight_[i], log_weight_.data() + i);
        size_type size = weight_.sum();
        weight_ /= size;
        size_type j = 0;
        size_type k = 0;
        rng::uniform_real_distribution<double> unif(0,1);
        double u = unif(rng(0));
        double cw = weight_[0];
        while (j != size) {
            while (j < cw * size - u && j != size) {
                ++replication_[k];
                u = unif(rng(j));
                ++j;
            }
            if (k == size_ - 1)
                break;
            cw += weight_[++k];
        }
        for (size_type i = 0; i != size_; ++i)
            replication_[i] += log_weight_[i];
    }

    void resample_residual_systematic ()
    {
        using std::modf;

        replication_.setConstant(0);
        for (size_type i = 0; i != size_; ++i)
            weight_[i] = modf(size_ * weight_[i], log_weight_.data() + i);
        size_type size = weight_.sum();
        weight_ /= size;
        size_type j = 0;
        size_type k = 0;
        rng::uniform_real_distribution<double> unif(0,1);
        double u = unif(rng(0));
        double cw = weight_[0];
        while (j != size) {
            while (j < cw * size - u && j != size) {
                ++replication_[k];
                ++j;
            }
            if (k == size_ - 1)
                break;
            cw += weight_[++k];
        }
        for (size_type i = 0; i != size_; ++i)
            replication_[i] += log_weight_[i];
    }

    void weight2replication (size_type size)
    {
        double tp = weight_.sum();
        double sum_p = 0;
        size_type sum_n = 0;
        replication_.setConstant(0);
        for (size_type i = 0; i != size_; ++i) {
            if (sum_n < size && weight_[i] > 0) {
                rng::binomial_distribution<size_type> binom(
                        size - sum_n, weight_[i] / (tp - sum_p));
                replication_[i] = binom(rng(i));
            }
            sum_p += weight_[i];
            sum_n += replication_[i];
        }
    }

    void resample_do ()
    {
        // Some times the nuemrical round error can cause the total childs
        // differ from number of particles
        size_type sum = replication_.sum();
        if (sum != size_) {
            size_type id_max;
            replication_.maxCoeff(&id_max);
            replication_[id_max] += size_ - sum;
        }

        size_type from = 0;
        size_type time = 0;
        for (size_type to = 0; to != size_; ++to) {
            if (!replication_[to]) {
                // replication_[to] has zero child, copy from elsewhere
                if (replication_[from] - time <= 1) {
                    // only 1 child left on replication_[from]
                    time = 0;
                    do // move from to some position with at least 2 children
                        ++from;
                    while (replication_[from] < 2);
                }
                value_.copy(from, to);
                ++time;
            }
        }
        set_equal_weight();
    }
}; // class Particle

} // namespace vsmc

#endif // VSMC_CORE_PARTICLE_HPP
<commit_msg>cache ESS and normalized weight computation<commit_after>#ifndef VSMC_CORE_PARTICLE_HPP
#define VSMC_CORE_PARTICLE_HPP

#include <vsmc/internal/common.hpp>
#include <vsmc/internal/resampling.hpp>

namespace vsmc {

/// \brief Particle class representing the whole particle set
/// \ingroup Core
///
/// \tparam T Requirement:
/// \li Constructor compatible with
/// \code T(Particle<T>::size_type N) \endcode
/// \li member function copy method compatible with
/// \code copy(Particle<T>::size_type from, Particle<T>::size_type to) \endcode
template <typename T>
class Particle
{
    public :

    /// The type of the size of the particle set
    typedef VSMC_INDEX_TYPE size_type;

    /// The type of the particle values
    typedef T value_type;

#ifdef VSMC_USE_SEQUENTIAL_RNG
    /// The type of the Counter-based random number generator C++11 engine
    typedef VSMC_SEQRNG_TYPE rng_type;
#else
    /// The type of the Counter-based random number generator
    typedef VSMC_CBRNG_TYPE cbrng_type;

    /// The type of the Counter-based random number generator C++11 engine
    typedef rng::Engine<cbrng_type> rng_type;
#endif

    /// The integer type of the seed
    typedef rng_type::result_type seed_type;

    /// The type of the weight and log weight vector
    typedef Eigen::VectorXd weight_type;

    /// \brief Construct a Particle object with a given number of particles
    ///
    /// \param N The number of particles
    /// \param seed The seed to the parallel RNG system
    ///
    /// \post All weights are initialized to be euqal to each other
    explicit Particle (size_type N, seed_type seed = VSMC_RNG_SEED) :
        size_(N), value_(N), log_weight_(N), replication_(N),
        weight_(N), weight_cached_(false), inc_weight_(N),
        ess_(N), ess_cached_(false), resampled_(false), zconst_(0), seed_(seed)
#ifndef VSMC_USE_SEQUENTIAL_RNG
        , prng_(N)
#endif
    {
        reset_rng();
        set_equal_weight();
    }

    /// \brief Size of the particle set
    ///
    /// \return The number of particles
    size_type size () const
    {
        return size_;
    }

    /// \brief Read and write access to particle values
    ///
    /// \return A reference to the particle values
    value_type &value ()
    {
        return value_;
    }

    /// \brief Read only access to particle values
    ///
    /// \return A const reference to the particle values
    const value_type &value () const
    {
        return value_;
    }

    /// \brief Get the weight of a single particle
    ///
    /// \param id The position of the particle, 0 to size() - 1
    ///
    /// \return The weight of the particle at position id
    double weight (size_type id) const
    {
        return weight()[id];
    }

    /// \brief Read only access to the weights through pointer
    ///
    /// \return A const pointer to the weight array
    const double *weight_ptr () const
    {
        return weight().data();
    }

    /// \brief Read only access to the weights through Eigen vector
    ///
    /// \return A const reference to the weight vector
    const weight_type &weight () const
    {
        if (!weight_cached_) {
            weight_ = log_weight_.array().exp();
            double sum = weight_.sum();
            weight_ *= 1 / sum;
            weight_cached_ = true;
        }

        return weight_;
    }

    /// \brief Get the log weight of a single particle
    ///
    /// \param id The position of the particle, 0 to size() - 1
    ///
    /// \return The log weight of the particle at position id
    double log_weight (size_type id) const
    {
        return log_weight()[id];
    }

    /// \brief Read only access to the log weights through pointer
    ///
    /// \return A const pointer to the log weight array
    const double *log_weight_ptr () const
    {
        return log_weight().data();
    }

    /// \brief Read only access to the log weights through Eigen vector
    ///
    /// \return A const reference to the log weight vector
    const weight_type &log_weight () const
    {
        return log_weight_;
    }

    /// \brief Set equal weights for all particles
    void set_equal_weight ()
    {
        log_weight_.setConstant(0);
        weight_.setConstant(1.0 / size_);
        weight_cached_ = true;
        ess_ = size_;
        ess_cached_ = true;
    }

    /// \brief Set the log weights
    ///
    /// \param new_weight New log weights
    /// \param delta A multiplier appiled to new_weight
    void set_log_weight (const double *new_weight, double delta = 1)
    {
        Eigen::Map<const weight_type> w(new_weight, size_);
        set_log_weight(w, delta);
    }

    /// \brief Set the log weights
    ///
    /// \param new_weight New log weights
    /// \param delta A multiplier appiled to new_weight
    void set_log_weight (const weight_type &new_weight, double delta = 1)
    {
        log_weight_ = delta * new_weight.head(size_);
        set_log_weight();
    }

    /// \brief Add to the log weights
    ///
    /// \param inc_weight Incremental log weights
    /// \param delta A multiplier applied to inc_weight
    /// \param add_zconst Whether this incremental weights should contribute
    /// the esitmates of normalizing constants
    void add_log_weight (const double *inc_weight, double delta = 1,
            bool add_zconst = true)
    {
        Eigen::Map<const weight_type> w(inc_weight, size_);
        add_log_weight(w, delta, add_zconst);
    }

    /// \brief Add to the log weights
    ///
    /// \param inc_weight Incremental log weights
    /// \param delta A multiplier applied to inc_weight
    /// \param add_zconst Whether this incremental weights should contribute
    /// the esitmates of normalizing constants
    void add_log_weight (const weight_type &inc_weight, double delta = 1,
            bool add_zconst = true)
    {
        using std::log;

        if (add_zconst) {
            inc_weight_ = (delta * inc_weight.head(size_)).array().exp();
            zconst_ += log(weight_.dot(inc_weight_.head(size_)));
        }
        log_weight_ += delta * inc_weight.head(size_);
        set_log_weight();
    }

    /// \brief The ESS (Effective Sample Size)
    ///
    /// \return The value of ESS for current particle set
    double ess () const
    {
        if (!ess_cached_) {
            ess_ = 1 / weight().squaredNorm();
            ess_cached_ = true;
        }

        return ess_;
    }

    /// \brief Get indicator of resampling
    ///
    /// \return \b true if the current iteration was resampled
    bool resampled () const
    {
        return resampled_;
    }

    /// \brief Get the value of SMC normalizing constant
    ///
    /// \return Log of SMC normalizng constant estimate
    double zconst () const
    {
        return zconst_;
    }

    /// \brief Reset the value of SMC normalizing constant
    void reset_zconst ()
    {
        zconst_ = 0;
    }

    /// \brief Perform resampling
    ///
    /// \param scheme The resampling scheme, see ResamplingScheme
    /// \param threshold The threshold for resampling
    void resample (ResampleScheme scheme, double threshold)
    {
        resampled_ = ess() < threshold * size_;

        if (resampled_) {
            internal::pre_resampling(value_);
            switch (scheme) {
                case MULTINOMIAL :
                    resample_multinomial();
                    break;
                case RESIDUAL :
                    resample_residual();
                    break;
                case STRATIFIED :
                    resample_stratified();
                    break;
                case SYSTEMATIC :
                    resample_systematic();
                    break;
                case RESIDUAL_STRATIFIED :
                    resample_residual_stratified ();
                    break;
                default :
                    resample_stratified();
                    break;
            }
            resample_do();
            internal::post_resampling(value_);
        }
    }

    /// \brief Get a C++11 RNG engine
    ///
    /// \param id The position of the particle, 0 to size() - 1
    ///
    /// \return A reference to a C++11 RNG engine unique to particle at
    /// position id, and independent of others if \c VSMC_USE_SEQUENTIAL_RNG is
    /// not defined. Otherwise, it is the same pseudo RNG for any value of \c
    /// id
    rng_type &rng (size_type id)
    {
#ifdef VSMC_USE_SEQUENTIAL_RNG
        return srng_;
#else
        return prng_[id];
#endif
    }

    /// \brief Reset the parallel RNG system
    ///
    /// \param seed The new seed to the system
    void reset_rng (seed_type seed)
    {
        seed_ = seed;
        reset_rng();
    }

    /// \brief Reset the parallel RNG system using last time used seed
    void reset_rng ()
    {
#ifdef VSMC_USE_SEQUENTIAL_RNG
        srng_ = rng_type(seed_);
#else
        for (size_type i = 0; i != size_; ++i)
            prng_[i] = rng_type(seed_ + i);
#endif
    }

    private :

    typedef Eigen::Matrix<size_type, Eigen::Dynamic, 1> replication_type;

    size_type size_;
    value_type value_;

    weight_type log_weight_;
    replication_type replication_;

    mutable weight_type weight_;
    mutable bool weight_cached_;
    weight_type inc_weight_;

    mutable double ess_;
    mutable bool ess_cached_;

    bool resampled_;
    double zconst_;

    seed_type seed_;
#ifdef VSMC_USE_SEQUENTIAL_RNG
    rng_type srng_;
#else
    std::deque<rng_type> prng_;
#endif

    void set_log_weight ()
    {
        double max_weight = log_weight_.maxCoeff();
        log_weight_ = log_weight_.array() - max_weight;
        weight_cached_ = false;
        ess_cached_ = false;
    }

    void resample_multinomial ()
    {
        weight2replication(size_);
    }

    void resample_residual ()
    {
        /// \internal Reuse weight and log_weight. weight: act as the
        /// fractional part of N * weight. log_weight: act as the integral
        /// part of N * weight.  They all will be reset to equal weights after
        /// resampling.  So it is safe to modify them here.
        using std::modf;

        for (size_type i = 0; i != size_; ++i)
            weight_[i] = modf(size_ * weight_[i], log_weight_.data() + i);
        weight2replication(weight_.sum());
        for (size_type i = 0; i != size_; ++i)
            replication_[i] += log_weight_[i];
    }

    void resample_stratified ()
    {
        replication_.setConstant(0);
        size_type j = 0;
        size_type k = 0;
        rng::uniform_real_distribution<double> unif(0,1);
        double u = unif(rng(0));
        double cw = weight_[0];
        while (j != size_) {
            while (j < cw * size_ - u && j != size_) {
                ++replication_[k];
                u = unif(rng(j));
                ++j;
            }
            if (k == size_ - 1)
                break;
            cw += weight_[++k];
        }
    }

    void resample_systematic ()
    {
        replication_.setConstant(0);
        size_type j = 0;
        size_type k = 0;
        rng::uniform_real_distribution<double> unif(0,1);
        double u = unif(rng(0));
        double cw = weight_[0];
        while (j != size_) {
            while (j < cw * size_ - u && j != size_) {
                ++replication_[k];
                ++j;
            }
            if (k == size_ - 1)
                break;
            cw += weight_[++k];
        }
    }

    void resample_residual_stratified ()
    {
        using std::modf;

        replication_.setConstant(0);
        for (size_type i = 0; i != size_; ++i)
            weight_[i] = modf(size_ * weight_[i], log_weight_.data() + i);
        size_type size = weight_.sum();
        weight_ /= size;
        size_type j = 0;
        size_type k = 0;
        rng::uniform_real_distribution<double> unif(0,1);
        double u = unif(rng(0));
        double cw = weight_[0];
        while (j != size) {
            while (j < cw * size - u && j != size) {
                ++replication_[k];
                u = unif(rng(j));
                ++j;
            }
            if (k == size_ - 1)
                break;
            cw += weight_[++k];
        }
        for (size_type i = 0; i != size_; ++i)
            replication_[i] += log_weight_[i];
    }

    void resample_residual_systematic ()
    {
        using std::modf;

        replication_.setConstant(0);
        for (size_type i = 0; i != size_; ++i)
            weight_[i] = modf(size_ * weight_[i], log_weight_.data() + i);
        size_type size = weight_.sum();
        weight_ /= size;
        size_type j = 0;
        size_type k = 0;
        rng::uniform_real_distribution<double> unif(0,1);
        double u = unif(rng(0));
        double cw = weight_[0];
        while (j != size) {
            while (j < cw * size - u && j != size) {
                ++replication_[k];
                ++j;
            }
            if (k == size_ - 1)
                break;
            cw += weight_[++k];
        }
        for (size_type i = 0; i != size_; ++i)
            replication_[i] += log_weight_[i];
    }

    void weight2replication (size_type size)
    {
        double tp = weight_.sum();
        double sum_p = 0;
        size_type sum_n = 0;
        replication_.setConstant(0);
        for (size_type i = 0; i != size_; ++i) {
            if (sum_n < size && weight_[i] > 0) {
                rng::binomial_distribution<size_type> binom(
                        size - sum_n, weight_[i] / (tp - sum_p));
                replication_[i] = binom(rng(i));
            }
            sum_p += weight_[i];
            sum_n += replication_[i];
        }
    }

    void resample_do ()
    {
        // Some times the nuemrical round error can cause the total childs
        // differ from number of particles
        size_type sum = replication_.sum();
        if (sum != size_) {
            size_type id_max;
            replication_.maxCoeff(&id_max);
            replication_[id_max] += size_ - sum;
        }

        size_type from = 0;
        size_type time = 0;
        for (size_type to = 0; to != size_; ++to) {
            if (!replication_[to]) {
                // replication_[to] has zero child, copy from elsewhere
                if (replication_[from] - time <= 1) {
                    // only 1 child left on replication_[from]
                    time = 0;
                    do // move from to some position with at least 2 children
                        ++from;
                    while (replication_[from] < 2);
                }
                value_.copy(from, to);
                ++time;
            }
        }
        set_equal_weight();
    }
}; // class Particle

} // namespace vsmc

#endif // VSMC_CORE_PARTICLE_HPP
<|endoftext|>