<commit_before>#include <boost/mpi/communicator.hpp>
#include <boost/graph/use_mpi.hpp>
#include <boost/graph/distributed/mpi_process_group.hpp>
#include <boost/graph/distributed/adjacency_list.hpp>
#include <boost/graph/distributed/graphviz.hpp>
#include <iomanip>
#include <iostream>
#include "csv.hpp"
#include "sigsna_utils.hpp"

using namespace boost;
using boost::graph::distributed::mpi_process_group;

struct Person {
    Person() {}
    Person(const std::string& name) : name(name) {}

    std::string name;
    unsigned int in_degree, out_degree;

    // Serialization support is required!
    template<typename Archiver>
    void serialize(Archiver &ar, const unsigned int)
    {
        ar & name & in_degree & out_degree;
    }
};

// Enable named vertex support
namespace boost {
namespace graph {
template<>
struct internal_vertex_name<Person>
{
    typedef multi_index::member<Person, std::string, &Person::name> type;
};
template<>
struct internal_vertex_constructor<Person>
{
    typedef vertex_from_name<Person> type;
};
} // end of namespace graph
} // end of namespace boost

// Declare the graph type and vertex and edge types
typedef adjacency_list<vecS, distributedS<mpi_process_group, vecS>,
        bidirectionalS, Person> Graph;
typedef property_map<Graph, std::string Person::*>::type VertexNameMap;
typedef property_map<Graph, unsigned int Person::*>::type VertexInDegreeMap;
typedef property_map<Graph, unsigned int Person::*>::type VertexOutDegreeMap;

// custom label writer
template <class Name, class InDegree, class OutDegree>
class nd_label_writer {
public:
    nd_label_writer(Name _name, InDegree _in_degree, OutDegree _out_degree) : name(_name), in_degree(_in_degree), out_degree(_out_degree) {}
    template <class VertexOrEdge>
    void operator()(std::ostream& out, const VertexOrEdge& v) const {
        // "in_degree" and "out_degree" are not a DOT attributes, so they will be ignored by the plotting programs
        // see http://www.graphviz.org/doc/schema/attributes.xml
        out << "[label=\"" << name[v] << "\" in_degree=" << in_degree[v] << " out_degree=" << out_degree[v] << "]";
    }
private:
    Name name;
    InDegree in_degree;
    OutDegree out_degree;
};
template <class Name, class InDegree, class OutDegree>
inline nd_label_writer<Name, InDegree, OutDegree>
make_nd_label_writer(Name n, InDegree i, OutDegree o) {
    return nd_label_writer<Name, InDegree, OutDegree>(n, i, o);
}

template <class Graph, class VertexInDegreeMap, class VertexOutDegreeMap>
void degree_centrality(Graph &g, VertexInDegreeMap &in_degree_map, VertexOutDegreeMap &out_degree_map)
{
    typename graph_traits<Graph>::vertex_iterator i, end;
    for(tie(i, end) = vertices(g); i != end; ++i) {
        in_degree_map[*i] = in_degree(*i, g);
        out_degree_map[*i] = out_degree(*i, g);
    }
}

int main (int argc, char *argv[])
{
    mpi::environment env(argc, argv);
    mpi::communicator world;
    std::cout << "Process " << world.rank() << " of " << world.size() << " reporting for duty." << std::endl;

    if(argc < 2)
    {
        std::cerr << "What CSV file do you want me to load?" << std::endl;
        return EXIT_FAILURE;
    }

    Graph g;
    VertexNameMap name_map = get(&Person::name, g);
    VertexInDegreeMap in_degree_map = get(&Person::in_degree, g);
    VertexOutDegreeMap out_degree_map = get(&Person::out_degree, g);

    // Load the graph from SIGSNA's CSV
    if (process_id(g.process_group()) == 0) {
        std::cout << "Process 0 here, sir! Loading the graph... ";
        std::cout.flush();

        std::ifstream file(argv[1]);
        CSV csv(file);
        sigsna::load_graph_from_csv(csv, g);

        std::cout << "done" << std::endl;
    }
    synchronize(g.process_group());

    degree_centrality(g, in_degree_map, out_degree_map);

    write_graphviz(std::cout, g, make_nd_label_writer(name_map, in_degree_map, out_degree_map));

    return EXIT_SUCCESS;
}
<commit_msg>Use stdout only for graphviz output<commit_after>#include <boost/mpi/communicator.hpp>
#include <boost/graph/use_mpi.hpp>
#include <boost/graph/distributed/mpi_process_group.hpp>
#include <boost/graph/distributed/adjacency_list.hpp>
#include <boost/graph/distributed/graphviz.hpp>
#include <iomanip>
#include <iostream>
#include "csv.hpp"
#include "sigsna_utils.hpp"

using namespace boost;
using boost::graph::distributed::mpi_process_group;

struct Person {
    Person() {}
    Person(const std::string& name) : name(name) {}

    std::string name;
    unsigned int in_degree, out_degree;

    // Serialization support is required!
    template<typename Archiver>
    void serialize(Archiver &ar, const unsigned int)
    {
        ar & name & in_degree & out_degree;
    }
};

// Enable named vertex support
namespace boost {
namespace graph {
template<>
struct internal_vertex_name<Person>
{
    typedef multi_index::member<Person, std::string, &Person::name> type;
};
template<>
struct internal_vertex_constructor<Person>
{
    typedef vertex_from_name<Person> type;
};
} // end of namespace graph
} // end of namespace boost

// Declare the graph type and vertex and edge types
typedef adjacency_list<vecS, distributedS<mpi_process_group, vecS>,
        bidirectionalS, Person> Graph;
typedef property_map<Graph, std::string Person::*>::type VertexNameMap;
typedef property_map<Graph, unsigned int Person::*>::type VertexInDegreeMap;
typedef property_map<Graph, unsigned int Person::*>::type VertexOutDegreeMap;

// custom label writer
template <class Name, class InDegree, class OutDegree>
class nd_label_writer {
public:
    nd_label_writer(Name _name, InDegree _in_degree, OutDegree _out_degree) : name(_name), in_degree(_in_degree), out_degree(_out_degree) {}
    template <class VertexOrEdge>
    void operator()(std::ostream& out, const VertexOrEdge& v) const {
        // "in_degree" and "out_degree" are not a DOT attributes, so they will be ignored by the plotting programs
        // see http://www.graphviz.org/doc/schema/attributes.xml
        out << "[label=\"" << name[v] << "\" in_degree=" << in_degree[v] << " out_degree=" << out_degree[v] << "]";
    }
private:
    Name name;
    InDegree in_degree;
    OutDegree out_degree;
};
template <class Name, class InDegree, class OutDegree>
inline nd_label_writer<Name, InDegree, OutDegree>
make_nd_label_writer(Name n, InDegree i, OutDegree o) {
    return nd_label_writer<Name, InDegree, OutDegree>(n, i, o);
}

template <class Graph, class VertexInDegreeMap, class VertexOutDegreeMap>
void degree_centrality(Graph &g, VertexInDegreeMap &in_degree_map, VertexOutDegreeMap &out_degree_map)
{
    typename graph_traits<Graph>::vertex_iterator i, end;
    for(tie(i, end) = vertices(g); i != end; ++i) {
        in_degree_map[*i] = in_degree(*i, g);
        out_degree_map[*i] = out_degree(*i, g);
    }
}

int main (int argc, char *argv[])
{
    mpi::environment env(argc, argv);
    mpi::communicator world;
    std::cerr << "Process " << world.rank() << " of " << world.size() << " reporting for duty." << std::endl;

    if(argc < 2)
    {
        if (world.rank() == 0) {
            std::cerr << "What CSV file do you want me to load?" << std::endl;
        }
        return EXIT_FAILURE;
    }

    Graph g;
    VertexNameMap name_map = get(&Person::name, g);
    VertexInDegreeMap in_degree_map = get(&Person::in_degree, g);
    VertexOutDegreeMap out_degree_map = get(&Person::out_degree, g);

    // Load the graph from SIGSNA's CSV
    if (process_id(g.process_group()) == 0) {
        std::cerr << "Process 0 here, sir! Loading the graph... ";

        std::ifstream file(argv[1]);
        CSV csv(file);
        sigsna::load_graph_from_csv(csv, g);

        std::cerr << "done" << std::endl;
    }
    synchronize(g.process_group());

    degree_centrality(g, in_degree_map, out_degree_map);

    write_graphviz(std::cout, g, make_nd_label_writer(name_map, in_degree_map, out_degree_map));

    return EXIT_SUCCESS;
}
<|endoftext|>


<commit_before>// Copyright (c) 2012 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "ash/wm/toplevel_window_event_filter.h"

#include "ash/shell.h"
#include "ash/wm/default_window_resizer.h"
#include "ash/wm/property_util.h"
#include "ash/wm/window_resizer.h"
#include "ash/wm/window_util.h"
#include "base/message_loop.h"
#include "ui/aura/client/aura_constants.h"
#include "ui/aura/cursor.h"
#include "ui/aura/env.h"
#include "ui/aura/event.h"
#include "ui/aura/root_window.h"
#include "ui/aura/window.h"
#include "ui/aura/window_delegate.h"
#include "ui/base/hit_test.h"
#include "ui/base/ui_base_types.h"
#include "ui/gfx/screen.h"

namespace ash {

namespace {

gfx::Point ConvertPointToParent(aura::Window* window,
                                const gfx::Point& point) {
  gfx::Point result(point);
  aura::Window::ConvertPointToWindow(window, window->parent(), &result);
  return result;
}

}

ToplevelWindowEventFilter::ToplevelWindowEventFilter(aura::Window* owner)
    : in_move_loop_(false),
      in_gesture_resize_(false),
      grid_size_(0) {
  aura::client::SetWindowMoveClient(owner, this);
}

ToplevelWindowEventFilter::~ToplevelWindowEventFilter() {
}

bool ToplevelWindowEventFilter::PreHandleKeyEvent(aura::Window* target,
                                                  aura::KeyEvent* event) {
  if (window_resizer_.get() && event->type() == ui::ET_KEY_PRESSED &&
      event->key_code() == ui::VKEY_ESCAPE) {
    CompleteDrag(DRAG_REVERT);
  }
  return false;
}

bool ToplevelWindowEventFilter::PreHandleMouseEvent(aura::Window* target,
                                                    aura::MouseEvent* event) {
  switch (event->type()) {
    case ui::ET_MOUSE_PRESSED: {
      // We also update the current window component here because for the
      // mouse-drag-release-press case, where the mouse is released and
      // pressed without mouse move event.
      int component =
          target->delegate()->GetNonClientComponent(event->location());
      if ((event->flags() &
           (ui::EF_IS_DOUBLE_CLICK | ui::EF_IS_TRIPLE_CLICK)) == 0 &&
          WindowResizer::GetBoundsChangeForWindowComponent(component)) {
        gfx::Point parent_location(
            ConvertPointToParent(target, event->location()));
        window_resizer_.reset(
            CreateWindowResizer(target, parent_location, component));
      } else {
        window_resizer_.reset();
      }
      if (component == HTCAPTION &&
          event->flags() & ui::EF_IS_DOUBLE_CLICK &&
          event->flags() & ui::EF_LEFT_MOUSE_BUTTON)
        ToggleMaximizedState(target);
      return WindowResizer::GetBoundsChangeForWindowComponent(component) != 0;
    }
    case ui::ET_MOUSE_DRAGGED:
      return HandleDrag(target, event);
    case ui::ET_MOUSE_CAPTURE_CHANGED:
    case ui::ET_MOUSE_RELEASED:
      CompleteDrag(event->type() == ui::ET_MOUSE_RELEASED ?
                   DRAG_COMPLETE : DRAG_REVERT);
      if (in_move_loop_) {
        MessageLoop::current()->Quit();
        in_move_loop_ = false;
      }
      // Completing the drag may result in hiding the window. If this happens
      // return true so no other filters/observers see the event. Otherwise they
      // see the event on a hidden window.
      if (event->type() == ui::ET_MOUSE_CAPTURE_CHANGED &&
          !target->IsVisible()) {
        return true;
      }
      break;
    default:
      break;
  }
  return false;
}

ui::TouchStatus ToplevelWindowEventFilter::PreHandleTouchEvent(
    aura::Window* target,
    aura::TouchEvent* event) {
  return ui::TOUCH_STATUS_UNKNOWN;
}

ui::GestureStatus ToplevelWindowEventFilter::PreHandleGestureEvent(
    aura::Window* target,
    aura::GestureEvent* event) {
  switch (event->type()) {
    case ui::ET_GESTURE_SCROLL_BEGIN: {
      int component =
          target->delegate()->GetNonClientComponent(event->location());
      if (WindowResizer::GetBoundsChangeForWindowComponent(component) == 0) {
        window_resizer_.reset();
        return ui::GESTURE_STATUS_UNKNOWN;
      }
      in_gesture_resize_ = true;
      gfx::Point parent_location(
          ConvertPointToParent(target, event->location()));
      window_resizer_.reset(
          CreateWindowResizer(target, parent_location, component));
      break;
    }
    case ui::ET_GESTURE_SCROLL_UPDATE: {
      if (!in_gesture_resize_)
        return ui::GESTURE_STATUS_UNKNOWN;
      HandleDrag(target, event);
      break;
    }
    case ui::ET_GESTURE_SCROLL_END: {
      if (!in_gesture_resize_)
        return ui::GESTURE_STATUS_UNKNOWN;
      CompleteDrag(DRAG_COMPLETE);
      in_gesture_resize_ = false;
      break;
    }
    default:
      return ui::GESTURE_STATUS_UNKNOWN;
  }

  return ui::GESTURE_STATUS_CONSUMED;
}

void ToplevelWindowEventFilter::RunMoveLoop(aura::Window* source) {
  DCHECK(!in_move_loop_);  // Can only handle one nested loop at a time.
  in_move_loop_ = true;
  gfx::Point parent_mouse_location(gfx::Screen::GetCursorScreenPoint());
  aura::Window::ConvertPointToWindow(
      Shell::GetRootWindow(), source->parent(), &parent_mouse_location);
  window_resizer_.reset(
      CreateWindowResizer(source, parent_mouse_location, HTCAPTION));
#if !defined(OS_MACOSX)
  MessageLoopForUI::current()->RunWithDispatcher(
      aura::Env::GetInstance()->GetDispatcher());
#endif  // !defined(OS_MACOSX)
  in_move_loop_ = false;
}

void ToplevelWindowEventFilter::EndMoveLoop() {
  if (!in_move_loop_)
    return;

  in_move_loop_ = false;
  if (window_resizer_.get()) {
    window_resizer_->RevertDrag();
    window_resizer_.reset();
  }
  MessageLoopForUI::current()->Quit();
  Shell::GetRootWindow()->PostNativeEvent(ui::CreateNoopEvent());
}

// static
WindowResizer* ToplevelWindowEventFilter::CreateWindowResizer(
    aura::Window* window,
    const gfx::Point& point,
    int window_component) {
  if (!wm::IsWindowNormal(window))
    return NULL;  // Don't allow resizing/dragging maximized/fullscreen windows.
  return DefaultWindowResizer::Create(
      window, point, window_component, grid_size_);
}

void ToplevelWindowEventFilter::CompleteDrag(DragCompletionStatus status) {
  scoped_ptr<WindowResizer> resizer(window_resizer_.release());
  if (resizer.get()) {
    if (status == DRAG_COMPLETE)
      resizer->CompleteDrag();
    else
      resizer->RevertDrag();
  }
}

bool ToplevelWindowEventFilter::HandleDrag(aura::Window* target,
                                           aura::LocatedEvent* event) {
  // This function only be triggered to move window
  // by mouse drag or touch move event.
  DCHECK(event->type() == ui::ET_MOUSE_DRAGGED ||
         event->type() == ui::ET_TOUCH_MOVED ||
         event->type() == ui::ET_GESTURE_SCROLL_UPDATE);

  if (!window_resizer_.get())
    return false;
  window_resizer_->Drag(ConvertPointToParent(target, event->location()));
  return true;
}

}  // namespace ash
<commit_msg>Redue use of Shell::GetRootWindow<commit_after>// Copyright (c) 2012 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "ash/wm/toplevel_window_event_filter.h"

#include "ash/shell.h"
#include "ash/wm/default_window_resizer.h"
#include "ash/wm/property_util.h"
#include "ash/wm/window_resizer.h"
#include "ash/wm/window_util.h"
#include "base/message_loop.h"
#include "ui/aura/client/aura_constants.h"
#include "ui/aura/cursor.h"
#include "ui/aura/env.h"
#include "ui/aura/event.h"
#include "ui/aura/root_window.h"
#include "ui/aura/window.h"
#include "ui/aura/window_delegate.h"
#include "ui/base/hit_test.h"
#include "ui/base/ui_base_types.h"
#include "ui/gfx/screen.h"

namespace ash {

namespace {

gfx::Point ConvertPointToParent(aura::Window* window,
                                const gfx::Point& point) {
  gfx::Point result(point);
  aura::Window::ConvertPointToWindow(window, window->parent(), &result);
  return result;
}

}

ToplevelWindowEventFilter::ToplevelWindowEventFilter(aura::Window* owner)
    : in_move_loop_(false),
      in_gesture_resize_(false),
      grid_size_(0) {
  aura::client::SetWindowMoveClient(owner, this);
}

ToplevelWindowEventFilter::~ToplevelWindowEventFilter() {
}

bool ToplevelWindowEventFilter::PreHandleKeyEvent(aura::Window* target,
                                                  aura::KeyEvent* event) {
  if (window_resizer_.get() && event->type() == ui::ET_KEY_PRESSED &&
      event->key_code() == ui::VKEY_ESCAPE) {
    CompleteDrag(DRAG_REVERT);
  }
  return false;
}

bool ToplevelWindowEventFilter::PreHandleMouseEvent(aura::Window* target,
                                                    aura::MouseEvent* event) {
  switch (event->type()) {
    case ui::ET_MOUSE_PRESSED: {
      // We also update the current window component here because for the
      // mouse-drag-release-press case, where the mouse is released and
      // pressed without mouse move event.
      int component =
          target->delegate()->GetNonClientComponent(event->location());
      if ((event->flags() &
           (ui::EF_IS_DOUBLE_CLICK | ui::EF_IS_TRIPLE_CLICK)) == 0 &&
          WindowResizer::GetBoundsChangeForWindowComponent(component)) {
        gfx::Point parent_location(
            ConvertPointToParent(target, event->location()));
        window_resizer_.reset(
            CreateWindowResizer(target, parent_location, component));
      } else {
        window_resizer_.reset();
      }
      if (component == HTCAPTION &&
          event->flags() & ui::EF_IS_DOUBLE_CLICK &&
          event->flags() & ui::EF_LEFT_MOUSE_BUTTON)
        ToggleMaximizedState(target);
      return WindowResizer::GetBoundsChangeForWindowComponent(component) != 0;
    }
    case ui::ET_MOUSE_DRAGGED:
      return HandleDrag(target, event);
    case ui::ET_MOUSE_CAPTURE_CHANGED:
    case ui::ET_MOUSE_RELEASED:
      CompleteDrag(event->type() == ui::ET_MOUSE_RELEASED ?
                   DRAG_COMPLETE : DRAG_REVERT);
      if (in_move_loop_) {
        MessageLoop::current()->Quit();
        in_move_loop_ = false;
      }
      // Completing the drag may result in hiding the window. If this happens
      // return true so no other filters/observers see the event. Otherwise they
      // see the event on a hidden window.
      if (event->type() == ui::ET_MOUSE_CAPTURE_CHANGED &&
          !target->IsVisible()) {
        return true;
      }
      break;
    default:
      break;
  }
  return false;
}

ui::TouchStatus ToplevelWindowEventFilter::PreHandleTouchEvent(
    aura::Window* target,
    aura::TouchEvent* event) {
  return ui::TOUCH_STATUS_UNKNOWN;
}

ui::GestureStatus ToplevelWindowEventFilter::PreHandleGestureEvent(
    aura::Window* target,
    aura::GestureEvent* event) {
  switch (event->type()) {
    case ui::ET_GESTURE_SCROLL_BEGIN: {
      int component =
          target->delegate()->GetNonClientComponent(event->location());
      if (WindowResizer::GetBoundsChangeForWindowComponent(component) == 0) {
        window_resizer_.reset();
        return ui::GESTURE_STATUS_UNKNOWN;
      }
      in_gesture_resize_ = true;
      gfx::Point parent_location(
          ConvertPointToParent(target, event->location()));
      window_resizer_.reset(
          CreateWindowResizer(target, parent_location, component));
      break;
    }
    case ui::ET_GESTURE_SCROLL_UPDATE: {
      if (!in_gesture_resize_)
        return ui::GESTURE_STATUS_UNKNOWN;
      HandleDrag(target, event);
      break;
    }
    case ui::ET_GESTURE_SCROLL_END: {
      if (!in_gesture_resize_)
        return ui::GESTURE_STATUS_UNKNOWN;
      CompleteDrag(DRAG_COMPLETE);
      in_gesture_resize_ = false;
      break;
    }
    default:
      return ui::GESTURE_STATUS_UNKNOWN;
  }

  return ui::GESTURE_STATUS_CONSUMED;
}

void ToplevelWindowEventFilter::RunMoveLoop(aura::Window* source) {
  DCHECK(!in_move_loop_);  // Can only handle one nested loop at a time.
  in_move_loop_ = true;
  gfx::Point parent_mouse_location(gfx::Screen::GetCursorScreenPoint());
  aura::RootWindow* root_window = source->GetRootWindow();
  DCHECK(root_window);
  aura::Window::ConvertPointToWindow(
      root_window, source->parent(), &parent_mouse_location);
  window_resizer_.reset(
      CreateWindowResizer(source, parent_mouse_location, HTCAPTION));
#if !defined(OS_MACOSX)
  MessageLoopForUI::current()->RunWithDispatcher(
      aura::Env::GetInstance()->GetDispatcher());
#endif  // !defined(OS_MACOSX)
  in_move_loop_ = false;
}

void ToplevelWindowEventFilter::EndMoveLoop() {
  if (!in_move_loop_)
    return;

  in_move_loop_ = false;
  if (window_resizer_.get()) {
    window_resizer_->RevertDrag();
    window_resizer_.reset();
  }
  MessageLoopForUI::current()->Quit();
}

// static
WindowResizer* ToplevelWindowEventFilter::CreateWindowResizer(
    aura::Window* window,
    const gfx::Point& point,
    int window_component) {
  if (!wm::IsWindowNormal(window))
    return NULL;  // Don't allow resizing/dragging maximized/fullscreen windows.
  return DefaultWindowResizer::Create(
      window, point, window_component, grid_size_);
}

void ToplevelWindowEventFilter::CompleteDrag(DragCompletionStatus status) {
  scoped_ptr<WindowResizer> resizer(window_resizer_.release());
  if (resizer.get()) {
    if (status == DRAG_COMPLETE)
      resizer->CompleteDrag();
    else
      resizer->RevertDrag();
  }
}

bool ToplevelWindowEventFilter::HandleDrag(aura::Window* target,
                                           aura::LocatedEvent* event) {
  // This function only be triggered to move window
  // by mouse drag or touch move event.
  DCHECK(event->type() == ui::ET_MOUSE_DRAGGED ||
         event->type() == ui::ET_TOUCH_MOVED ||
         event->type() == ui::ET_GESTURE_SCROLL_UPDATE);

  if (!window_resizer_.get())
    return false;
  window_resizer_->Drag(ConvertPointToParent(target, event->location()));
  return true;
}

}  // namespace ash
<|endoftext|>


<commit_before>/**
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you 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 <signal.h> // For strsignal.
#include <stdio.h>  // For freopen.
#include <string.h> // For strlen, strerror.
#include <unistd.h> // For sysconf.

#include <sys/wait.h> // For wait (and associated macros).

#include <string>
#include <vector>

#include <stout/check.hpp>
#include <stout/os.hpp>
#include <stout/path.hpp>
#include <stout/strings.hpp>

#include "tests/environment.hpp"
#include "tests/flags.hpp"
#include "tests/mesos.hpp"
#include "tests/script.hpp"

using std::string;
using std::vector;

namespace mesos {
namespace internal {
namespace tests {

void execute(const string& script, const vector<string>& arguments)
{
  static long maximumArguments = sysconf(_SC_ARG_MAX);
  PCHECK(maximumArguments != -1);

  CHECK_LT(arguments.size(), static_cast<size_t>(maximumArguments));

  // Create a temporary directory for the test.
  Try<string> directory = environment->mkdtemp();

  CHECK_SOME(directory) << "Failed to create temporary directory";

  if (flags.verbose) {
    std::cerr << "Using temporary directory '"
              << directory.get() << "'" << std::endl;
  }

  // Determine the path for the script.
  Try<string> path =
    os::realpath(path::join(flags.source_dir, "src", "tests", script));

  if (path.isError()) {
    FAIL() << "Failed to locate script: " << path.error();
  }

  // Fork a process to change directory and run the test.
  pid_t pid;
  if ((pid = fork()) == -1) {
    FAIL() << "Failed to fork to launch script";
  }

  if (pid > 0) {
    // In parent process.
    int status;
    while (wait(&status) != pid || WIFSTOPPED(status));
    CHECK(WIFEXITED(status) || WIFSIGNALED(status));

    if (WIFEXITED(status)) {
      if (WEXITSTATUS(status) != 0) {
        FAIL() << script << " exited with status " << WEXITSTATUS(status);
      }
    } else {
      FAIL() << script << " terminated with signal '"
             << strsignal(WTERMSIG(status)) << "'";
    }
  } else {
    // In child process. DO NOT USE GLOG!

    // Start by cd'ing into the temporary directory.
    if (!os::chdir(directory.get())) {
      std::cerr << "Failed to chdir to '" << directory.get() << "'" << std::endl;
      abort();
    }

    // Redirect output to /dev/null unless the test is verbose.
    if (!flags.verbose) {
      if (freopen("/dev/null", "w", stdout) == NULL ||
          freopen("/dev/null", "w", stderr) == NULL) {
        std::cerr << "Failed to redirect stdout/stderr to /dev/null:"
                  << strerror(errno) << std::endl;
        abort();
      }
    }

    // Set up the environment for executing the script.
    os::setenv("MESOS_SOURCE_DIR", flags.source_dir);
    os::setenv("MESOS_BUILD_DIR", flags.build_dir);
    os::setenv("MESOS_WEBUI_DIR", path::join(flags.source_dir, "src", "webui"));
    os::setenv("MESOS_LAUNCHER_DIR", path::join(flags.build_dir, "src"));

    // Enable authentication.
    os::setenv("MESOS_AUTHENTICATE", "true");

    // Create test credentials.
    const std::string& credentialsPath =
      path::join(directory.get(), "credentials");

    const std::string& credentials =
      DEFAULT_CREDENTIAL.principal() + " " + DEFAULT_CREDENTIAL.secret();

    CHECK_SOME(os::write(credentialsPath, credentials))
      << "Failed to write credentials to '" << credentialsPath << "'";

    os::setenv("MESOS_CREDENTIALS", "file://" + credentialsPath);

    // We set test credentials here for example frameworks to use.
    os::setenv("DEFAULT_PRINCIPAL", DEFAULT_CREDENTIAL.principal());
    os::setenv("DEFAULT_SECRET", DEFAULT_CREDENTIAL.secret());

    // Construct the argument array.
    const char** args = (const char**) new char*[arguments.size() + 1];
    args[0] = path.get().c_str();
    size_t index = 1;
    foreach (const string& argument, arguments) {
      args[index++] = argument.c_str();
    }
    args[arguments.size() + 1] = NULL;

    // Now execute the script.
    execv(path.get().c_str(), (char* const*) args);

    std::cerr << "Failed to execute '" << script << "': "
              << strerror(errno) << std::endl;
    abort();
  }
}

} // namespace tests {
} // namespace internal {
} // namespace mesos {
<commit_msg>Revert "Fixed ARG_MAX compilation issue on some POSIX systems."<commit_after>/**
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you 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 <limits.h> // For ARG_MAX.
#include <signal.h> // For strsignal.
#include <stdio.h>  // For freopen.
#include <string.h> // For strlen, strerror.

#include <sys/wait.h> // For wait (and associated macros).

#include <string>
#include <vector>

#include <stout/check.hpp>
#include <stout/os.hpp>
#include <stout/path.hpp>
#include <stout/strings.hpp>

#include "tests/environment.hpp"
#include "tests/flags.hpp"
#include "tests/mesos.hpp"
#include "tests/script.hpp"

using std::string;
using std::vector;

namespace mesos {
namespace internal {
namespace tests {

void execute(const string& script, const vector<string>& arguments)
{
  CHECK_LT(arguments.size(), static_cast<size_t>(ARG_MAX));

  // Create a temporary directory for the test.
  Try<string> directory = environment->mkdtemp();

  CHECK_SOME(directory) << "Failed to create temporary directory";

  if (flags.verbose) {
    std::cerr << "Using temporary directory '"
              << directory.get() << "'" << std::endl;
  }

  // Determine the path for the script.
  Try<string> path =
    os::realpath(path::join(flags.source_dir, "src", "tests", script));

  if (path.isError()) {
    FAIL() << "Failed to locate script: " << path.error();
  }

  // Fork a process to change directory and run the test.
  pid_t pid;
  if ((pid = fork()) == -1) {
    FAIL() << "Failed to fork to launch script";
  }

  if (pid > 0) {
    // In parent process.
    int status;
    while (wait(&status) != pid || WIFSTOPPED(status));
    CHECK(WIFEXITED(status) || WIFSIGNALED(status));

    if (WIFEXITED(status)) {
      if (WEXITSTATUS(status) != 0) {
        FAIL() << script << " exited with status " << WEXITSTATUS(status);
      }
    } else {
      FAIL() << script << " terminated with signal '"
             << strsignal(WTERMSIG(status)) << "'";
    }
  } else {
    // In child process. DO NOT USE GLOG!

    // Start by cd'ing into the temporary directory.
    if (!os::chdir(directory.get())) {
      std::cerr << "Failed to chdir to '" << directory.get() << "'" << std::endl;
      abort();
    }

    // Redirect output to /dev/null unless the test is verbose.
    if (!flags.verbose) {
      if (freopen("/dev/null", "w", stdout) == NULL ||
          freopen("/dev/null", "w", stderr) == NULL) {
        std::cerr << "Failed to redirect stdout/stderr to /dev/null:"
                  << strerror(errno) << std::endl;
        abort();
      }
    }

    // Set up the environment for executing the script.
    os::setenv("MESOS_SOURCE_DIR", flags.source_dir);
    os::setenv("MESOS_BUILD_DIR", flags.build_dir);
    os::setenv("MESOS_WEBUI_DIR", path::join(flags.source_dir, "src", "webui"));
    os::setenv("MESOS_LAUNCHER_DIR", path::join(flags.build_dir, "src"));

    // Enable authentication.
    os::setenv("MESOS_AUTHENTICATE", "true");

    // Create test credentials.
    const std::string& credentialsPath =
      path::join(directory.get(), "credentials");

    const std::string& credentials =
      DEFAULT_CREDENTIAL.principal() + " " + DEFAULT_CREDENTIAL.secret();

    CHECK_SOME(os::write(credentialsPath, credentials))
      << "Failed to write credentials to '" << credentialsPath << "'";

    os::setenv("MESOS_CREDENTIALS", "file://" + credentialsPath);

    // We set test credentials here for example frameworks to use.
    os::setenv("DEFAULT_PRINCIPAL", DEFAULT_CREDENTIAL.principal());
    os::setenv("DEFAULT_SECRET", DEFAULT_CREDENTIAL.secret());

    // Construct the argument array.
    const char** args = (const char**) new char*[arguments.size() + 1];
    args[0] = path.get().c_str();
    size_t index = 1;
    foreach (const string& argument, arguments) {
      args[index++] = argument.c_str();
    }
    args[arguments.size() + 1] = NULL;

    // Now execute the script.
    execv(path.get().c_str(), (char* const*) args);

    std::cerr << "Failed to execute '" << script << "': "
              << strerror(errno) << std::endl;
    abort();
  }
}

} // namespace tests {
} // namespace internal {
} // namespace mesos {
<|endoftext|>


<commit_before>#line 2 "togo/utility.hpp"
/**
@copyright MIT license; see @ref index or the accompanying LICENSE file.

@file utility.hpp
@brief Utilities.
@ingroup utility
*/

#pragma once

#include <togo/config.hpp>
#include <togo/types.hpp>
#include <togo/debug_constraints.hpp>

#include <type_traits>

namespace togo {

/**
	@addtogroup utility
	@{
*/

/** @name Misc utilities */ /// @{

/// Swap the values of two references.
template<class T>
inline void swap(T& x, T& y) {
	T temp = x;
	x = y;
	y = temp;
}

/// Less-than comparison operator wrapper.
template<class T>
inline bool less(T const& x, T const& y) {
	return x < y;
}

/// Greater-than comparison operator wrapper.
template<class T>
inline bool greater(T const& x, T const& y) {
	return x > y;
}

/// @}

/** @name Type utilities */ /// @{

/// Cast unsigned integral to signed integral.
template<class T>
inline constexpr typename std::make_signed<T>::type
signed_cast(T const value) noexcept {
	return static_cast<typename std::make_signed<T>::type>(value);
}

/// Cast signed integral to unsigned integral.
template<class T>
inline constexpr typename std::make_unsigned<T>::type
unsigned_cast(T const value) noexcept {
	return static_cast<typename std::make_unsigned<T>::type>(value);
}

/// Get number of elements in bounded array.
template<class T, unsigned N>
inline constexpr unsigned
array_extent(T (&)[N]) noexcept {
	return N;
}

/// @}

/** @name Arithmetic utilities */ /// @{

/// Get the smallest of two values.
template<class T>
inline T
min(T const x, T const y) noexcept {
	TOGO_CONSTRAIN_INTEGRAL(T);
	TOGO_CONSTRAIN_ARITHMETIC(T);
	return x < y ? x : y;
}

/// Get the largest of two values.
template<class T>
inline T
max(T const x, T const y) noexcept {
	TOGO_CONSTRAIN_INTEGRAL(T);
	TOGO_CONSTRAIN_ARITHMETIC(T);
	return x > y ? x : y;
}

/// @}

/** @name Memory utilities */ /// @{

/// Advance pointer by bytes.
/// @warning This advances by bytes, not sizeof(T).
template<class T>
inline T* pointer_add(T* p, u32 const bytes) noexcept {
	return static_cast<T*>(static_cast<char*>(p) + bytes);
}

/// Aligns a pointer by moving it forward.
template<class T>
inline T* pointer_align(T* p, u32 const align) noexcept {
	u32 const m = reinterpret_cast<std::uintptr_t>(p) % align;
	if (m) {
		p = pointer_add(p, align - m);
	}
	return p;
}

/// @}

/** @} */ // end of doc-group utility

} // namespace togo
<commit_msg>utility: added data member pointer array_extent() variant.¹<commit_after>#line 2 "togo/utility.hpp"
/**
@copyright MIT license; see @ref index or the accompanying LICENSE file.

@file utility.hpp
@brief Utilities.
@ingroup utility
*/

#pragma once

#include <togo/config.hpp>
#include <togo/types.hpp>
#include <togo/debug_constraints.hpp>

#include <type_traits>

namespace togo {

/**
	@addtogroup utility
	@{
*/

/** @name Misc utilities */ /// @{

/// Swap the values of two references.
template<class T>
inline void swap(T& x, T& y) {
	T temp = x;
	x = y;
	y = temp;
}

/// Less-than comparison operator wrapper.
template<class T>
inline bool less(T const& x, T const& y) {
	return x < y;
}

/// Greater-than comparison operator wrapper.
template<class T>
inline bool greater(T const& x, T const& y) {
	return x > y;
}

/// @}

/** @name Type utilities */ /// @{

/// Cast unsigned integral to signed integral.
template<class T>
inline constexpr typename std::make_signed<T>::type
signed_cast(T const value) noexcept {
	return static_cast<typename std::make_signed<T>::type>(value);
}

/// Cast signed integral to unsigned integral.
template<class T>
inline constexpr typename std::make_unsigned<T>::type
unsigned_cast(T const value) noexcept {
	return static_cast<typename std::make_unsigned<T>::type>(value);
}

/// Get number of elements in bounded array.
template<class T, unsigned N>
inline constexpr unsigned
array_extent(T const (&)[N]) noexcept {
	return N;
}

/// Get number of elements in bounded array.
template<class T, class U, unsigned N>
inline constexpr unsigned
array_extent(T const (U::* const)[N]) noexcept {
	return N;
}

/// @}

/** @name Arithmetic utilities */ /// @{

/// Get the smallest of two values.
template<class T>
inline T
min(T const x, T const y) noexcept {
	TOGO_CONSTRAIN_INTEGRAL(T);
	TOGO_CONSTRAIN_ARITHMETIC(T);
	return x < y ? x : y;
}

/// Get the largest of two values.
template<class T>
inline T
max(T const x, T const y) noexcept {
	TOGO_CONSTRAIN_INTEGRAL(T);
	TOGO_CONSTRAIN_ARITHMETIC(T);
	return x > y ? x : y;
}

/// @}

/** @name Memory utilities */ /// @{

/// Advance pointer by bytes.
/// @warning This advances by bytes, not sizeof(T).
template<class T>
inline T* pointer_add(T* p, u32 const bytes) noexcept {
	return static_cast<T*>(static_cast<char*>(p) + bytes);
}

/// Aligns a pointer by moving it forward.
template<class T>
inline T* pointer_align(T* p, u32 const align) noexcept {
	u32 const m = reinterpret_cast<std::uintptr_t>(p) % align;
	if (m) {
		p = pointer_add(p, align - m);
	}
	return p;
}

/// @}

/** @} */ // end of doc-group utility

} // namespace togo
<|endoftext|>


<commit_before>/*
 * Copyright 2010,
 * Nicolas Mansard, Olivier Stasse, François Bleibel, Florent Lamiraux
 *
 * CNRS
 *
 * This file is part of sot-core.
 * sot-core is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 * sot-core is distributed in the hope that it will be
 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with sot-core.  If not, see <http://www.gnu.org/licenses/>.
 */

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* SOT */
#include "sot/core/device.hh"
#include <sot/core/debug.hh>
using namespace std;

#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
#include <Eigen/Geometry>
#include <dynamic-graph/linear-algebra.h>
#include <sot/core/matrix-geometry.hh>

using namespace dynamicgraph::sot;
using namespace dynamicgraph;

const std::string Device::CLASS_NAME = "Device";

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */

void Device::integrateRollPitchYaw(Vector& state, const Vector& control,
                                   double dt)
{
  Eigen::Vector3d omega;
  //TODO: EIGEN OPTIMISE
  // Translation part
  for (unsigned int i=0; i<3; i++) {
    state(i) += control(i)*dt;
    ffPose_(i,3) = state(i);
    omega(i) = control(i+3);
  }
  // Rotation part
  double roll = state(3);
  double pitch = state(4);
  double yaw = state(5);
  Eigen::Vector3d column [3];

  // Build rotation matrix as a vector of colums
  column [0](0) = cos(pitch)*cos(yaw);
  column [0](1) = cos(pitch)*sin(yaw);
  column [0](2) = -sin(pitch);

  column [1](0) = sin(roll)*sin(pitch)*cos(yaw) - cos(roll)*sin(yaw);
  column [1](1) = sin(roll)*sin(pitch)*sin(yaw) + cos(roll)*cos(yaw);
  column [1](2) = sin(roll)*cos(pitch);

  column [2](0) = cos(roll)*sin(pitch)*cos(yaw) + sin(roll)*sin(yaw);
  column [2](1) = cos(roll)*sin(pitch)*sin(yaw) - sin(roll)*cos(yaw);
  column [2](2) = cos(roll)*cos(pitch);

  // Apply Rodrigues (1795–1851) formula for rotation about omega vector
  double angle = dt*omega.norm();
  if (angle == 0) {
    return;
  }
  Eigen::Vector3d k = omega/omega.norm();
  // ei <- ei cos(angle) + sin(angle)(k ^ ei) + (k.ei)(1-cos(angle))k
  for (unsigned int i=0; i<3; i++) {
    Eigen::Vector3d ei = column[i];
    column[i] = ei*cos(angle) + (k.cross(ei))*sin(angle) + k*((k.dot(ei))*(1-cos(angle)));
  }
  // Store new position if ffPose_ member.
  for (unsigned int r = 0; r < 3; r++) {
    for (unsigned int c = 0; c < 3; c++) {
      ffPose_(r,c) = column[c](r);
    }
  }
  const double & nx = column[2](2);
  const double & ny = column[1](2);

  state(3) = atan2(ny,nx);
  state(4) = atan2(-column[0](2),
                   sqrt(ny*ny+nx*nx));
  state(5) = atan2(column[0](1),column[0](0));

}

const MatrixHomogeneous& Device::freeFlyerPose() const
{
  return ffPose_;
}

Device::
~Device( )
{
  for( unsigned int i=0; i<4; ++i ) {
    delete forcesSOUT[i];
  }
}

Device::
Device( const std::string& n )
  :Entity(n)
  ,state_(6)
  ,robotState_("Device(" + n + ")::output(vector)::robotState")
  ,robotVelocity_("Device(" + n + ")::output(vector)::robotVelocity")
  ,vel_controlInit_(false)
  ,controlInputType_(CONTROL_INPUT_ONE_INTEGRATION)
  ,controlSIN( NULL,"Device("+n+")::input(double)::control" )
  //,attitudeSIN(NULL,"Device::input(matrixRot)::attitudeIN")
  ,attitudeSIN(NULL,"Device::input(vector3)::attitudeIN")
  ,zmpSIN(NULL,"Device::input(vector3)::zmp")
  ,stateSOUT( "Device("+n+")::output(vector)::state" )
  ,velocitySOUT( "Device("+n+")::output(vector)::velocity"  )
  ,attitudeSOUT( "Device("+n+")::output(matrixRot)::attitude" )
  ,pseudoTorqueSOUT( "Device::output(vector)::ptorque" )
  ,previousControlSOUT( "Device("+n+")::output(vector)::previousControl" )
  ,motorcontrolSOUT( "Device("+n+")::output(vector)::motorcontrol" )
  ,ZMPPreviousControllerSOUT( "Device("+n+")::output(vector)::zmppreviouscontroller" ), ffPose_(),
    forceZero6 (6)
{
  forceZero6.fill (0);
  /* --- SIGNALS --- */
  for( int i=0;i<4;++i ){ withForceSignals[i] = false; }
  forcesSOUT[0] =
      new Signal<Vector, int>("OpenHRP::output(vector6)::forceRLEG");
  forcesSOUT[1] =
      new Signal<Vector, int>("OpenHRP::output(vector6)::forceLLEG");
  forcesSOUT[2] =
      new Signal<Vector, int>("OpenHRP::output(vector6)::forceRARM");
  forcesSOUT[3] =
      new Signal<Vector, int>("OpenHRP::output(vector6)::forceLARM");

  signalRegistration( controlSIN<<stateSOUT<<robotState_<<robotVelocity_
                      <<velocitySOUT<<attitudeSOUT
                      <<attitudeSIN<<zmpSIN <<*forcesSOUT[0]<<*forcesSOUT[1]
                      <<*forcesSOUT[2]<<*forcesSOUT[3] <<previousControlSOUT
                      <<pseudoTorqueSOUT << motorcontrolSOUT << ZMPPreviousControllerSOUT );
  state_.fill(.0); stateSOUT.setConstant( state_ );

  velocity_.resize(state_.size()); velocity_.setZero();
  velocitySOUT.setConstant( velocity_ );

  /* --- Commands --- */
  {
    std::string docstring;
    /* Command setStateSize. */
    docstring =
        "\n"
        "    Set size of state vector\n"
        "\n";
    addCommand("resize",
               new command::Setter<Device, unsigned int>
               (*this, &Device::setStateSize, docstring));
    docstring =
        "\n"
        "    Set state vector value\n"
        "\n";
    addCommand("set",
               new command::Setter<Device, Vector>
               (*this, &Device::setState, docstring));

    docstring =
        "\n"
        "    Set velocity vector value\n"
        "\n";
    addCommand("setVelocity",
               new command::Setter<Device, Vector>
               (*this, &Device::setVelocity, docstring));

    void(Device::*setRootPtr)(const Matrix&) = &Device::setRoot;
    docstring
        = command::docCommandVoid1("Set the root position.",
                                   "matrix homogeneous");
    addCommand("setRoot",
               command::makeCommandVoid1(*this,setRootPtr,
                                         docstring));

    /* Second Order Integration set. */
    docstring =
        "\n"
        "    Set the position calculous starting from  \n"
        "    acceleration measure instead of velocity \n"
        "\n";

    addCommand("setSecondOrderIntegration",
               command::makeCommandVoid0(*this,&Device::setSecondOrderIntegration,
                                         docstring));

    /* SET of control input type. */
    docstring =
        "\n"
        "    Set the type of control input which can be  \n"
        "    acceleration, velocity, or position\n"
        "\n";

    addCommand("setControlInputType",
               new command::Setter<Device,string>
               (*this, &Device::setControlInputType, docstring));

    // Handle commands and signals called in a synchronous way.
    periodicCallBefore_.addSpecificCommands(*this, commandMap, "before.");
    periodicCallAfter_.addSpecificCommands(*this, commandMap, "after.");

  }
}

void Device::
setStateSize( const unsigned int& size )
{
  state_.resize(size); state_.fill( .0 );
  stateSOUT .setConstant( state_ );
  previousControlSOUT.setConstant( state_ );
  pseudoTorqueSOUT.setConstant( state_ );
  motorcontrolSOUT .setConstant( state_ );

  Device::setVelocitySize(size);

  Vector zmp(3); zmp.fill( .0 );
  ZMPPreviousControllerSOUT .setConstant( zmp );
}

void Device::
setVelocitySize( const unsigned int& size )
{
  velocity_.resize(size);
  velocity_.fill(.0);
  velocitySOUT.setConstant( velocity_ );
}

void Device::
setState( const Vector& st )
{
  state_ = st;
  stateSOUT .setConstant( state_ );
  motorcontrolSOUT .setConstant( state_ );
}

void Device::
setVelocity( const Vector& vel )
{
  velocity_ = vel;
  velocitySOUT .setConstant( velocity_ );
}

void Device::
setRoot( const Matrix & root )
{
  Eigen::Matrix4d _matrix4d(root);
  MatrixHomogeneous _root(_matrix4d);
  setRoot( _root );
}

void Device::
setRoot( const MatrixHomogeneous & worldMwaist )
{
  VectorRollPitchYaw r = (worldMwaist.linear().eulerAngles(2,1,0)).reverse();
  Vector q = state_;
  q = worldMwaist.translation(); // abusive ... but working.
  for( unsigned int i=0;i<3;++i ) q(i+3) = r(i);
}

void Device::
setSecondOrderIntegration()
{
  controlInputType_ = CONTROL_INPUT_TWO_INTEGRATION;
  velocity_.resize(state_.size());
  velocity_.setZero();
  velocitySOUT.setConstant( velocity_ );
}

void Device::
setNoIntegration()
{
  controlInputType_ = CONTROL_INPUT_NO_INTEGRATION;
  velocity_.resize(state_.size());
  velocity_.setZero();
  velocitySOUT.setConstant( velocity_ );
}

void Device::
setControlInputType(const std::string& cit)
{
  for(int i=0; i<CONTROL_INPUT_SIZE; i++)
    if(cit==ControlInput_s[i])
    {
      controlInputType_ = (ControlInput)i;
      sotDEBUG(25)<<"Control input type: "<<ControlInput_s[i]<<endl;
      return;
    }
  sotDEBUG(25)<<"Unrecognized control input type: "<<cit<<endl;
}

void Device::
increment( const double & dt )
{
  int time = stateSOUT.getTime();
  sotDEBUG(25) << "Time : " << time << std::endl;

  // Run Synchronous commands and evaluate signals outside the main
  // connected component of the graph.
  try
  {
    periodicCallBefore_.run(time+1);
  }
  catch (std::exception& e)
  {
    std::cerr
        << "exception caught while running periodical commands (before): "
        << e.what () << std::endl;
  }
  catch (const char* str)
  {
    std::cerr
        << "exception caught while running periodical commands (before): "
        << str << std::endl;
  }
  catch (...)
  {
    std::cerr
        << "unknown exception caught while"
        << " running periodical commands (before)" << std::endl;
  }

  /* Force the recomputation of the control. */
  controlSIN( time );
  sotDEBUG(25) << "u" <<time<<" = " << controlSIN.accessCopy() << endl;

  /* Integration of numerical values. This function is virtual. */
  integrate( dt );
  sotDEBUG(25) << "q" << time << " = " << state_ << endl;

  /* Position the signals corresponding to sensors. */
  stateSOUT .setConstant( state_ ); stateSOUT.setTime( time+1 );
  //computation of the velocity signal
  if( controlInputType_==CONTROL_INPUT_TWO_INTEGRATION )
  {
    velocitySOUT.setConstant( velocity_ );
    velocitySOUT.setTime( time+1 );
  }
  else if (controlInputType_==CONTROL_INPUT_ONE_INTEGRATION)
  {
    velocitySOUT.setConstant( controlSIN.accessCopy() );
    velocitySOUT.setTime( time+1 );
  }
  for( int i=0;i<4;++i ){
    if(  !withForceSignals[i] ) forcesSOUT[i]->setConstant(forceZero6);
  }
  Vector zmp(3); zmp.fill( .0 );
  ZMPPreviousControllerSOUT .setConstant( zmp );

  // Run Synchronous commands and evaluate signals outside the main
  // connected component of the graph.
  try
  {
    periodicCallAfter_.run(time+1);
  }
  catch (std::exception& e)
  {
    std::cerr
        << "exception caught while running periodical commands (after): "
        << e.what () << std::endl;
  }
  catch (const char* str)
  {
    std::cerr
        << "exception caught while running periodical commands (after): "
        << str << std::endl;
  }
  catch (...)
  {
    std::cerr
        << "unknown exception caught while"
        << " running periodical commands (after)" << std::endl;
  }

  // Others signals.
  motorcontrolSOUT .setConstant( state_ );
}

void Device::integrate( const double & dt )
{
  const Vector & controlIN = controlSIN.accessCopy();

  if (controlInputType_==CONTROL_INPUT_NO_INTEGRATION)
  {
    assert(state_.size()==controlIN.size()+6);
    for( int i=0;i<controlIN.size();++i )
      state_(i+6) = controlIN(i);
    return;
  }

  if( !vel_controlInit_ )
  {
    vel_control_ = Vector(controlIN.size());
    vel_control_.setZero();
    vel_controlInit_ = true;
  }

  // If control size is state size - 6, integrate joint angles,
  // if control and state are of same size, integrate 6 first degrees of
  // freedom as a translation and roll pitch yaw.
  unsigned int offset = 6;

  if (controlInputType_==CONTROL_INPUT_TWO_INTEGRATION)
  {
    if(controlIN.size() == velocity_.size()) offset = 0;
    for( int i=0;i<controlIN.size();++i )
    {
      vel_control_(i) = velocity_(i+offset) + controlIN(i)*dt*0.5;
      velocity_(i+offset) = velocity_(i+offset) + controlIN(i)*dt;
    }
  }
  else
  {
    vel_control_ = controlIN;
  }

  if (vel_control_.size() == state_.size()) {
    offset = 0;
    integrateRollPitchYaw(state_, vel_control_, dt);
  }

  for( int i=6;i<state_.size();++i )
  { state_(i) += (vel_control_(i-offset)*dt); }
}

/* --- DISPLAY ------------------------------------------------------------ */

void Device::display ( std::ostream& os ) const
{os <<name<<": "<<state_<<endl; }
<commit_msg>Corrected freeflyer pose initialization<commit_after>/*
 * Copyright 2010,
 * Nicolas Mansard, Olivier Stasse, François Bleibel, Florent Lamiraux
 *
 * CNRS
 *
 * This file is part of sot-core.
 * sot-core is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 * sot-core is distributed in the hope that it will be
 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with sot-core.  If not, see <http://www.gnu.org/licenses/>.
 */

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* SOT */
#include "sot/core/device.hh"
#include <sot/core/debug.hh>
using namespace std;

#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
#include <Eigen/Geometry>
#include <dynamic-graph/linear-algebra.h>
#include <sot/core/matrix-geometry.hh>

using namespace dynamicgraph::sot;
using namespace dynamicgraph;

const std::string Device::CLASS_NAME = "Device";

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */

void Device::integrateRollPitchYaw(Vector& state, const Vector& control,
                                   double dt)
{
  Eigen::Vector3d omega;
  //TODO: EIGEN OPTIMISE
  // Translation part
  for (unsigned int i=0; i<3; i++) {
    state(i) += control(i)*dt;
    ffPose_(i,3) = state(i);
    omega(i) = control(i+3);
  }
  // Rotation part
  double roll = state(3);
  double pitch = state(4);
  double yaw = state(5);
  Eigen::Vector3d column [3];

  // Build rotation matrix as a vector of colums
  column [0](0) = cos(pitch)*cos(yaw);
  column [0](1) = cos(pitch)*sin(yaw);
  column [0](2) = -sin(pitch);

  column [1](0) = sin(roll)*sin(pitch)*cos(yaw) - cos(roll)*sin(yaw);
  column [1](1) = sin(roll)*sin(pitch)*sin(yaw) + cos(roll)*cos(yaw);
  column [1](2) = sin(roll)*cos(pitch);

  column [2](0) = cos(roll)*sin(pitch)*cos(yaw) + sin(roll)*sin(yaw);
  column [2](1) = cos(roll)*sin(pitch)*sin(yaw) - sin(roll)*cos(yaw);
  column [2](2) = cos(roll)*cos(pitch);

  // Apply Rodrigues (1795–1851) formula for rotation about omega vector
  double angle = dt*omega.norm();
  if (angle == 0) {
    for (unsigned int r = 0; r < 3; r++) {
      for (unsigned int c = 0; c < 3; c++) {
        ffPose_(r,c) = column[c](r);
      }
    }
    return;
  }
  Eigen::Vector3d k = omega/omega.norm();
  // ei <- ei cos(angle) + sin(angle)(k ^ ei) + (k.ei)(1-cos(angle))k
  for (unsigned int i=0; i<3; i++) {
    Eigen::Vector3d ei = column[i];
    column[i] = ei*cos(angle) + (k.cross(ei))*sin(angle) + k*((k.dot(ei))*(1-cos(angle)));
  }
  // Store new position if ffPose_ member.
  for (unsigned int r = 0; r < 3; r++) {
    for (unsigned int c = 0; c < 3; c++) {
      ffPose_(r,c) = column[c](r);
    }
  }
  const double & nx = column[2](2);
  const double & ny = column[1](2);

  state(3) = atan2(ny,nx);
  state(4) = atan2(-column[0](2),
                   sqrt(ny*ny+nx*nx));
  state(5) = atan2(column[0](1),column[0](0));

}

const MatrixHomogeneous& Device::freeFlyerPose() const
{
  return ffPose_;
}

Device::
~Device( )
{
  for( unsigned int i=0; i<4; ++i ) {
    delete forcesSOUT[i];
  }
}

Device::
Device( const std::string& n )
  :Entity(n)
  ,state_(6)
  ,robotState_("Device(" + n + ")::output(vector)::robotState")
  ,robotVelocity_("Device(" + n + ")::output(vector)::robotVelocity")
  ,vel_controlInit_(false)
  ,controlInputType_(CONTROL_INPUT_ONE_INTEGRATION)
  ,controlSIN( NULL,"Device("+n+")::input(double)::control" )
  //,attitudeSIN(NULL,"Device::input(matrixRot)::attitudeIN")
  ,attitudeSIN(NULL,"Device::input(vector3)::attitudeIN")
  ,zmpSIN(NULL,"Device::input(vector3)::zmp")
  ,stateSOUT( "Device("+n+")::output(vector)::state" )
  ,velocitySOUT( "Device("+n+")::output(vector)::velocity"  )
  ,attitudeSOUT( "Device("+n+")::output(matrixRot)::attitude" )
  ,pseudoTorqueSOUT( "Device::output(vector)::ptorque" )
  ,previousControlSOUT( "Device("+n+")::output(vector)::previousControl" )
  ,motorcontrolSOUT( "Device("+n+")::output(vector)::motorcontrol" )
  ,ZMPPreviousControllerSOUT( "Device("+n+")::output(vector)::zmppreviouscontroller" ), ffPose_(),
    forceZero6 (6)
{
  forceZero6.fill (0);
  /* --- SIGNALS --- */
  for( int i=0;i<4;++i ){ withForceSignals[i] = false; }
  forcesSOUT[0] =
      new Signal<Vector, int>("OpenHRP::output(vector6)::forceRLEG");
  forcesSOUT[1] =
      new Signal<Vector, int>("OpenHRP::output(vector6)::forceLLEG");
  forcesSOUT[2] =
      new Signal<Vector, int>("OpenHRP::output(vector6)::forceRARM");
  forcesSOUT[3] =
      new Signal<Vector, int>("OpenHRP::output(vector6)::forceLARM");

  signalRegistration( controlSIN<<stateSOUT<<robotState_<<robotVelocity_
                      <<velocitySOUT<<attitudeSOUT
                      <<attitudeSIN<<zmpSIN <<*forcesSOUT[0]<<*forcesSOUT[1]
                      <<*forcesSOUT[2]<<*forcesSOUT[3] <<previousControlSOUT
                      <<pseudoTorqueSOUT << motorcontrolSOUT << ZMPPreviousControllerSOUT );
  state_.fill(.0); stateSOUT.setConstant( state_ );

  velocity_.resize(state_.size()); velocity_.setZero();
  velocitySOUT.setConstant( velocity_ );

  /* --- Commands --- */
  {
    std::string docstring;
    /* Command setStateSize. */
    docstring =
        "\n"
        "    Set size of state vector\n"
        "\n";
    addCommand("resize",
               new command::Setter<Device, unsigned int>
               (*this, &Device::setStateSize, docstring));
    docstring =
        "\n"
        "    Set state vector value\n"
        "\n";
    addCommand("set",
               new command::Setter<Device, Vector>
               (*this, &Device::setState, docstring));

    docstring =
        "\n"
        "    Set velocity vector value\n"
        "\n";
    addCommand("setVelocity",
               new command::Setter<Device, Vector>
               (*this, &Device::setVelocity, docstring));

    void(Device::*setRootPtr)(const Matrix&) = &Device::setRoot;
    docstring
        = command::docCommandVoid1("Set the root position.",
                                   "matrix homogeneous");
    addCommand("setRoot",
               command::makeCommandVoid1(*this,setRootPtr,
                                         docstring));

    /* Second Order Integration set. */
    docstring =
        "\n"
        "    Set the position calculous starting from  \n"
        "    acceleration measure instead of velocity \n"
        "\n";

    addCommand("setSecondOrderIntegration",
               command::makeCommandVoid0(*this,&Device::setSecondOrderIntegration,
                                         docstring));

    /* SET of control input type. */
    docstring =
        "\n"
        "    Set the type of control input which can be  \n"
        "    acceleration, velocity, or position\n"
        "\n";

    addCommand("setControlInputType",
               new command::Setter<Device,string>
               (*this, &Device::setControlInputType, docstring));

    // Handle commands and signals called in a synchronous way.
    periodicCallBefore_.addSpecificCommands(*this, commandMap, "before.");
    periodicCallAfter_.addSpecificCommands(*this, commandMap, "after.");

  }
}

void Device::
setStateSize( const unsigned int& size )
{
  state_.resize(size); state_.fill( .0 );
  stateSOUT .setConstant( state_ );
  previousControlSOUT.setConstant( state_ );
  pseudoTorqueSOUT.setConstant( state_ );
  motorcontrolSOUT .setConstant( state_ );

  Device::setVelocitySize(size);

  Vector zmp(3); zmp.fill( .0 );
  ZMPPreviousControllerSOUT .setConstant( zmp );
}

void Device::
setVelocitySize( const unsigned int& size )
{
  velocity_.resize(size);
  velocity_.fill(.0);
  velocitySOUT.setConstant( velocity_ );
}

void Device::
setState( const Vector& st )
{
  state_ = st;
  stateSOUT .setConstant( state_ );
  motorcontrolSOUT .setConstant( state_ );
}

void Device::
setVelocity( const Vector& vel )
{
  velocity_ = vel;
  velocitySOUT .setConstant( velocity_ );
}

void Device::
setRoot( const Matrix & root )
{
  Eigen::Matrix4d _matrix4d(root);
  MatrixHomogeneous _root(_matrix4d);
  setRoot( _root );
}

void Device::
setRoot( const MatrixHomogeneous & worldMwaist )
{
  VectorRollPitchYaw r = (worldMwaist.linear().eulerAngles(2,1,0)).reverse();
  Vector q = state_;
  q = worldMwaist.translation(); // abusive ... but working.
  for( unsigned int i=0;i<3;++i ) q(i+3) = r(i);
}

void Device::
setSecondOrderIntegration()
{
  controlInputType_ = CONTROL_INPUT_TWO_INTEGRATION;
  velocity_.resize(state_.size());
  velocity_.setZero();
  velocitySOUT.setConstant( velocity_ );
}

void Device::
setNoIntegration()
{
  controlInputType_ = CONTROL_INPUT_NO_INTEGRATION;
  velocity_.resize(state_.size());
  velocity_.setZero();
  velocitySOUT.setConstant( velocity_ );
}

void Device::
setControlInputType(const std::string& cit)
{
  for(int i=0; i<CONTROL_INPUT_SIZE; i++)
    if(cit==ControlInput_s[i])
    {
      controlInputType_ = (ControlInput)i;
      sotDEBUG(25)<<"Control input type: "<<ControlInput_s[i]<<endl;
      return;
    }
  sotDEBUG(25)<<"Unrecognized control input type: "<<cit<<endl;
}

void Device::
increment( const double & dt )
{
  int time = stateSOUT.getTime();
  sotDEBUG(25) << "Time : " << time << std::endl;

  // Run Synchronous commands and evaluate signals outside the main
  // connected component of the graph.
  try
  {
    periodicCallBefore_.run(time+1);
  }
  catch (std::exception& e)
  {
    std::cerr
        << "exception caught while running periodical commands (before): "
        << e.what () << std::endl;
  }
  catch (const char* str)
  {
    std::cerr
        << "exception caught while running periodical commands (before): "
        << str << std::endl;
  }
  catch (...)
  {
    std::cerr
        << "unknown exception caught while"
        << " running periodical commands (before)" << std::endl;
  }

  /* Force the recomputation of the control. */
  controlSIN( time );
  sotDEBUG(25) << "u" <<time<<" = " << controlSIN.accessCopy() << endl;

  /* Integration of numerical values. This function is virtual. */
  integrate( dt );
  sotDEBUG(25) << "q" << time << " = " << state_ << endl;

  /* Position the signals corresponding to sensors. */
  stateSOUT .setConstant( state_ ); stateSOUT.setTime( time+1 );
  //computation of the velocity signal
  if( controlInputType_==CONTROL_INPUT_TWO_INTEGRATION )
  {
    velocitySOUT.setConstant( velocity_ );
    velocitySOUT.setTime( time+1 );
  }
  else if (controlInputType_==CONTROL_INPUT_ONE_INTEGRATION)
  {
    velocitySOUT.setConstant( controlSIN.accessCopy() );
    velocitySOUT.setTime( time+1 );
  }
  for( int i=0;i<4;++i ){
    if(  !withForceSignals[i] ) forcesSOUT[i]->setConstant(forceZero6);
  }
  Vector zmp(3); zmp.fill( .0 );
  ZMPPreviousControllerSOUT .setConstant( zmp );

  // Run Synchronous commands and evaluate signals outside the main
  // connected component of the graph.
  try
  {
    periodicCallAfter_.run(time+1);
  }
  catch (std::exception& e)
  {
    std::cerr
        << "exception caught while running periodical commands (after): "
        << e.what () << std::endl;
  }
  catch (const char* str)
  {
    std::cerr
        << "exception caught while running periodical commands (after): "
        << str << std::endl;
  }
  catch (...)
  {
    std::cerr
        << "unknown exception caught while"
        << " running periodical commands (after)" << std::endl;
  }

  // Others signals.
  motorcontrolSOUT .setConstant( state_ );
}

void Device::integrate( const double & dt )
{
  const Vector & controlIN = controlSIN.accessCopy();

  if (controlInputType_==CONTROL_INPUT_NO_INTEGRATION)
  {
    assert(state_.size()==controlIN.size()+6);
    for( int i=0;i<controlIN.size();++i )
      state_(i+6) = controlIN(i);
    return;
  }

  if( !vel_controlInit_ )
  {
    vel_control_ = Vector(controlIN.size());
    vel_control_.setZero();
    vel_controlInit_ = true;
  }

  // If control size is state size - 6, integrate joint angles,
  // if control and state are of same size, integrate 6 first degrees of
  // freedom as a translation and roll pitch yaw.
  unsigned int offset = 6;

  if (controlInputType_==CONTROL_INPUT_TWO_INTEGRATION)
  {
    if(controlIN.size() == velocity_.size()) offset = 0;
    for( int i=0;i<controlIN.size();++i )
    {
      vel_control_(i) = velocity_(i+offset) + controlIN(i)*dt*0.5;
      velocity_(i+offset) = velocity_(i+offset) + controlIN(i)*dt;
    }
  }
  else
  {
    vel_control_ = controlIN;
  }

  if (vel_control_.size() == state_.size()) {
    offset = 0;
    integrateRollPitchYaw(state_, vel_control_, dt);
  }

  for( int i=6;i<state_.size();++i )
  { state_(i) += (vel_control_(i-offset)*dt); }
}

/* --- DISPLAY ------------------------------------------------------------ */

void Device::display ( std::ostream& os ) const
{os <<name<<": "<<state_<<endl; }
<|endoftext|>


<commit_before>#ifndef STAN_MATH_GPU_BASIC_MATRIX_GPU_HPP
#define STAN_MATH_GPU_BASIC_MATRIX_GPU_HPP
#ifdef STAN_OPENCL
#include <stan/math/gpu/matrix_gpu.hpp>
#include <stan/math/gpu/err/check_matrix_gpu.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/scal/err/check_size_match.hpp>
#include <CL/cl.hpp>

/** @file stan/math/gpu/basic_matrix_gpu.hpp
*    @brief basic_matrix_gpu - basic matrix operations:
*    copy, copy lower/upper triangular, copy triangular transposed,
*    copy submatrix, init matrix with zeros, create an identity matrix,
*    add, subtract, transpose
*/
namespace stan {
  namespace math {
    enum triangularity {LOWER = 0, UPPER = 1, NONE = 2 };
    enum copy_transposed_triangular {LOWER_TO_UPPER_TRIANGULAR = 0,
      UPPER_TO_LOWER_TRIANGULAR = 1};
    /**
     * Stores the tranpose of the second matrix
     * to the first matrix. Both matrices
     * are on the  GPU.
     *
     * @param src the input matrix
     *
     * @return transposed input matrix
     *
     */
    inline matrix_gpu transpose(matrix_gpu & src) {
      matrix_gpu dst(src.cols(), src.rows());
      if ( dst.size() == 0 ) return dst;
      cl::Kernel kernel = opencl_context.get_kernel("transpose");
      cl::CommandQueue cmdQueue = opencl_context.queue();
      try {
        kernel.setArg(0, dst.buffer());
        kernel.setArg(1, src.buffer());
        kernel.setArg(2, src.rows());
        kernel.setArg(3, src.cols());
        cmdQueue.enqueueNDRangeKernel(
          kernel,
          cl::NullRange,
          cl::NDRange(src.rows(), src.cols()),
          cl::NullRange,
          NULL,
          NULL);
      } catch (const cl::Error& e) {
        check_opencl_error("transpose", e);
      }
      return dst;
    }

    /**
     * Stores zeros in the matrix on the GPU.
     * It support writing the zeros to lower
     * triangular, upper triangular or the
     * whole matrix.
     *
     * @param A matrix
     * @param part optional parameter
     * that describes where to assign zeros:
     * LOWER - lower triangular
     * UPPER - upper triangular
     * if the parameter is not specified,
     * zeros are assigned to the whole matrix.
     *
     */
    inline void zeros(matrix_gpu & A, triangularity part = NONE) {
      if ( A.size() == 0 ) return;
      cl::Kernel kernel = opencl_context.get_kernel("zeros");
      cl::CommandQueue cmdQueue = opencl_context.queue();
      try {
        kernel.setArg(0, A.buffer());
        kernel.setArg(1, A.rows());
        kernel.setArg(2, A.cols());
        kernel.setArg(3, part);
        cmdQueue.enqueueNDRangeKernel(
          kernel,
          cl::NullRange,
          cl::NDRange(A.rows(), A.cols()),
          cl::NullRange,
          NULL,
          NULL);
      } catch (const cl::Error& e) {
        check_opencl_error("zeros", e);
      }
    }

    /**
     * Returns the identity matrix stored on the GPU
     *
     * @param rows_cols the number of rows and columns
     *
     * @return the identity matrix
     *
     */
    inline matrix_gpu identity(int rows_cols) {
      matrix_gpu A(rows_cols, rows_cols);
      if (rows_cols == 0) {
        return A;
      }
      cl::Kernel kernel = opencl_context.get_kernel("identity");
      cl::CommandQueue cmdQueue = opencl_context.queue();

      try {
        kernel.setArg(0, A.buffer());
        kernel.setArg(1, A.rows());
        kernel.setArg(2, A.cols());
        cmdQueue.enqueueNDRangeKernel(
          kernel,
          cl::NullRange,
          cl::NDRange(A.rows(), A.cols()),
          cl::NullRange,
          NULL,
          NULL);
      } catch (const cl::Error& e) {
        check_opencl_error("identity", e);
      }
      return A;
    }

    /**
     * Copies the lower or upper
     * triangular of the source matrix to
     * the destination matrix.
     * Both matrices are stored on the GPU.
     *
     * @param src the source matrix
     * @param lower_upper enum to describe
     * which part of the matrix to copy:
     * LOWER - copies the lower triangular
     * UPPER - copes the upper triangular
     *
     * @return the matrix with the copied content
     *
     */
    inline matrix_gpu copy_triangular(matrix_gpu & src,
     triangularity lower_upper) {
      if (src.size() == 0) {
        return src;
      }
      if (src.size() == 1) {
        return src;
      }
      matrix_gpu dst(src.rows(), src.cols());
      cl::Kernel kernel = opencl_context.get_kernel("copy_triangular");
      cl::CommandQueue cmdQueue = opencl_context.queue();
      try {
        kernel.setArg(0, dst.buffer());
        kernel.setArg(1, src.buffer());
        kernel.setArg(2, dst.rows());
        kernel.setArg(3, dst.cols());
        kernel.setArg(4, lower_upper);
        cmdQueue.enqueueNDRangeKernel(
          kernel,
          cl::NullRange,
          cl::NDRange(dst.rows(), dst.cols()),
          cl::NullRange,
          NULL,
          NULL);
      } catch (const cl::Error& e) {
        check_opencl_error("copy_triangular", e);
      }
      return dst;
    }
     /**
     * Copies a submatrix of the source matrix to
     * the destination matrix. The submatrix to copy
     * starts at (src_offset_rows, src_offset_cols)
     * and is of size size_rows x size_cols.
     * The submatrix is copied to the
     * destination matrix starting at
     * (dst_offset_rows, dst_offset_cols)
     *
     * @param src the source matrix
     * @param dst the destination submatrix
     * @param src_offset_rows the offset row in src
     * @param src_offset_cols the offset column in src
     * @param dst_offset_rows the offset row in dst
     * @param dst_offset_cols the offset column in dst
     * @param size_rows the number of rows in the submatrix
     * @param size_cols the number of columns in the submatrix
     *
     * @throw std::invalid_argument if
     *
     *
     */
    inline void copy_submatrix(matrix_gpu & src,
     matrix_gpu & dst, int src_offset_rows, int src_offset_cols,
     int dst_offset_rows, int dst_offset_cols, int size_rows, int size_cols) {
      if (size_rows == 0 || size_cols == 0)
        return;
      if ( (src_offset_rows+size_rows) > src.rows() ||
           (src_offset_cols+size_cols) > src.cols() ) {
        domain_error("copy_submatrix", "submatrix in src" ,
          " is out of bounds", "");
      }
      if ((dst_offset_rows+size_rows) > dst.rows() ||
           (dst_offset_cols+size_cols) > dst.cols() ) {
        domain_error("copy_submatrix", "submatrix in src" ,
         " is out of bounds", "");
      }
      cl::Kernel kernel = opencl_context.get_kernel("copy_submatrix");
      cl::CommandQueue cmdQueue = opencl_context.queue();
      try {
        kernel.setArg(0, src.buffer());
        kernel.setArg(1, dst.buffer());

        kernel.setArg(2, src_offset_rows);
        kernel.setArg(3, src_offset_cols);
        kernel.setArg(4, dst_offset_rows);
        kernel.setArg(5, dst_offset_cols);

        kernel.setArg(6, size_rows);
        kernel.setArg(7, size_cols);

        kernel.setArg(8, src.rows());
        kernel.setArg(9, src.cols());
        kernel.setArg(10, dst.rows());
        kernel.setArg(11, dst.cols());

        cmdQueue.enqueueNDRangeKernel(
          kernel,
          cl::NullRange,
          cl::NDRange(size_rows, size_cols),
          cl::NullRange,
          NULL,
          NULL);
      } catch (const cl::Error& e) {
        check_opencl_error("copy_submatrix", e);
      }
    }

    /**
     * Copies the lower triangular
     * to the upper triangular of the
     * matrix on the GPU.
     *
     * @param A matrix
     * @param lower_upper enum to describe
     * which copy operation to perform
     * LOWER_TO_UPPER_TRIANGULAR - copies the lower
     *   triangular to the upper triangular
     * UPPER_TO_LOWER_TRIANGULAR - copes the upper
     *  triangular to the lower triangular
     *
     * @throw std::invalid_argument if the
     * matrices do not have matching dimensions
     *
     */
    inline void copy_triangular_transposed(matrix_gpu & A,
     copy_transposed_triangular lower_upper) {
      if (A.size() == 0) {
        return;
      }
      if (A.size() == 1) {
        return;
      }
      check_square("copy_triangular_transposed (GPU)", "A", A);
      cl::Kernel kernel =
        opencl_context.get_kernel("copy_triangular_transposed");
      cl::CommandQueue cmdQueue = opencl_context.queue();
      try {
        kernel.setArg(0, A.buffer());
        kernel.setArg(1, A.rows());
        kernel.setArg(2, A.cols());
        kernel.setArg(3, lower_upper);
        cmdQueue.enqueueNDRangeKernel(
          kernel,
          cl::NullRange,
          cl::NDRange(A.rows(), A.cols()),
          cl::NullRange,
          NULL,
          NULL);
      } catch (const cl::Error& e) {
        check_opencl_error("copy_triangular_transposed", e);
      }
    }
    /**
     * Matrix addition on the GPU
     * Adds the first two matrices and stores
     * the result in the third matrix (C=A+B)
     *
     * @param A first matrix
     * @param B second matrix
     *
     * @return sum of A and B
     *
     * @throw std::invalid_argument if the
     * input matrices do not have matching dimensions
     *
     */
    inline matrix_gpu add(matrix_gpu& A,
     matrix_gpu& B) {
      check_matching_dims("add (GPU)", "A", A, "B", B);
      matrix_gpu C(A.rows(), A.cols());
      if (C.size() == 0) {
        return C;
      }
      cl::Kernel kernel = opencl_context.get_kernel("add");
      cl::CommandQueue cmdQueue = opencl_context.queue();
      try {
        kernel.setArg(0, C.buffer());
        kernel.setArg(1, A.buffer());
        kernel.setArg(2, B.buffer());
        kernel.setArg(3, A.rows());
        kernel.setArg(4, A.cols());
        cmdQueue.enqueueNDRangeKernel(
          kernel,
          cl::NullRange,
          cl::NDRange(A.rows(), A.cols()),
          cl::NullRange,
          NULL,
          NULL);
      } catch (const cl::Error& e) {
        check_opencl_error("add", e);
      }
      return C;
    }

    /**
     * Matrix subtraction on the GPU
     * Subtracts the second matrix
     * from the first matrix and stores
     * the result in the third matrix (C=A-B)
     *
     * @param A first matrix
     * @param B second matrix
     *
     * @return subtraction result matrix
     *
     * @throw std::invalid_argument if the
     * input matrices do not have matching dimensions
     *
     */
    inline matrix_gpu subtract(matrix_gpu & A, matrix_gpu & B) {
      check_matching_dims("subtract (GPU)", "A", A, "B", B);
      matrix_gpu C(A.rows(), A.cols());
      if (A.size() == 0) {
        return C;
      }
      cl::Kernel kernel = opencl_context.get_kernel("subtract");
      cl::CommandQueue cmdQueue = opencl_context.queue();

      try {
        kernel.setArg(0, C.buffer());
        kernel.setArg(1, A.buffer());
        kernel.setArg(2, B.buffer());
        kernel.setArg(3, A.rows());
        kernel.setArg(4, A.cols());
        cmdQueue.enqueueNDRangeKernel(
          kernel,
          cl::NullRange,
          cl::NDRange(A.rows(), A.cols()),
          cl::NullRange,
          NULL,
          NULL);
      } catch (cl::Error& e) {
        check_opencl_error("subtract", e);
      }
      return C;
    }
  }
}

#endif
#endif
<commit_msg>check headers updated<commit_after>#ifndef STAN_MATH_GPU_BASIC_MATRIX_GPU_HPP
#define STAN_MATH_GPU_BASIC_MATRIX_GPU_HPP
#ifdef STAN_OPENCL
#include <stan/math/gpu/matrix_gpu.hpp>
#include <stan/math/gpu/err/check_matching_dims.hpp>
#include <stan/math/gpu/err/check_square.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/scal/err/check_size_match.hpp>
#include <CL/cl.hpp>

/** @file stan/math/gpu/basic_matrix_gpu.hpp
*    @brief basic_matrix_gpu - basic matrix operations:
*    copy, copy lower/upper triangular, copy triangular transposed,
*    copy submatrix, init matrix with zeros, create an identity matrix,
*    add, subtract, transpose
*/
namespace stan {
  namespace math {
    enum triangularity {LOWER = 0, UPPER = 1, NONE = 2 };
    enum copy_transposed_triangular {LOWER_TO_UPPER_TRIANGULAR = 0,
      UPPER_TO_LOWER_TRIANGULAR = 1};
    /**
     * Stores the tranpose of the second matrix
     * to the first matrix. Both matrices
     * are on the  GPU.
     *
     * @param src the input matrix
     *
     * @return transposed input matrix
     *
     */
    inline matrix_gpu transpose(matrix_gpu & src) {
      matrix_gpu dst(src.cols(), src.rows());
      if ( dst.size() == 0 ) return dst;
      cl::Kernel kernel = opencl_context.get_kernel("transpose");
      cl::CommandQueue cmdQueue = opencl_context.queue();
      try {
        kernel.setArg(0, dst.buffer());
        kernel.setArg(1, src.buffer());
        kernel.setArg(2, src.rows());
        kernel.setArg(3, src.cols());
        cmdQueue.enqueueNDRangeKernel(
          kernel,
          cl::NullRange,
          cl::NDRange(src.rows(), src.cols()),
          cl::NullRange,
          NULL,
          NULL);
      } catch (const cl::Error& e) {
        check_opencl_error("transpose", e);
      }
      return dst;
    }

    /**
     * Stores zeros in the matrix on the GPU.
     * It support writing the zeros to lower
     * triangular, upper triangular or the
     * whole matrix.
     *
     * @param A matrix
     * @param part optional parameter
     * that describes where to assign zeros:
     * LOWER - lower triangular
     * UPPER - upper triangular
     * if the parameter is not specified,
     * zeros are assigned to the whole matrix.
     *
     */
    inline void zeros(matrix_gpu & A, triangularity part = NONE) {
      if ( A.size() == 0 ) return;
      cl::Kernel kernel = opencl_context.get_kernel("zeros");
      cl::CommandQueue cmdQueue = opencl_context.queue();
      try {
        kernel.setArg(0, A.buffer());
        kernel.setArg(1, A.rows());
        kernel.setArg(2, A.cols());
        kernel.setArg(3, part);
        cmdQueue.enqueueNDRangeKernel(
          kernel,
          cl::NullRange,
          cl::NDRange(A.rows(), A.cols()),
          cl::NullRange,
          NULL,
          NULL);
      } catch (const cl::Error& e) {
        check_opencl_error("zeros", e);
      }
    }

    /**
     * Returns the identity matrix stored on the GPU
     *
     * @param rows_cols the number of rows and columns
     *
     * @return the identity matrix
     *
     */
    inline matrix_gpu identity(int rows_cols) {
      matrix_gpu A(rows_cols, rows_cols);
      if (rows_cols == 0) {
        return A;
      }
      cl::Kernel kernel = opencl_context.get_kernel("identity");
      cl::CommandQueue cmdQueue = opencl_context.queue();

      try {
        kernel.setArg(0, A.buffer());
        kernel.setArg(1, A.rows());
        kernel.setArg(2, A.cols());
        cmdQueue.enqueueNDRangeKernel(
          kernel,
          cl::NullRange,
          cl::NDRange(A.rows(), A.cols()),
          cl::NullRange,
          NULL,
          NULL);
      } catch (const cl::Error& e) {
        check_opencl_error("identity", e);
      }
      return A;
    }

    /**
     * Copies the lower or upper
     * triangular of the source matrix to
     * the destination matrix.
     * Both matrices are stored on the GPU.
     *
     * @param src the source matrix
     * @param lower_upper enum to describe
     * which part of the matrix to copy:
     * LOWER - copies the lower triangular
     * UPPER - copes the upper triangular
     *
     * @return the matrix with the copied content
     *
     */
    inline matrix_gpu copy_triangular(matrix_gpu & src,
     triangularity lower_upper) {
      if (src.size() == 0) {
        return src;
      }
      if (src.size() == 1) {
        return src;
      }
      matrix_gpu dst(src.rows(), src.cols());
      cl::Kernel kernel = opencl_context.get_kernel("copy_triangular");
      cl::CommandQueue cmdQueue = opencl_context.queue();
      try {
        kernel.setArg(0, dst.buffer());
        kernel.setArg(1, src.buffer());
        kernel.setArg(2, dst.rows());
        kernel.setArg(3, dst.cols());
        kernel.setArg(4, lower_upper);
        cmdQueue.enqueueNDRangeKernel(
          kernel,
          cl::NullRange,
          cl::NDRange(dst.rows(), dst.cols()),
          cl::NullRange,
          NULL,
          NULL);
      } catch (const cl::Error& e) {
        check_opencl_error("copy_triangular", e);
      }
      return dst;
    }
     /**
     * Copies a submatrix of the source matrix to
     * the destination matrix. The submatrix to copy
     * starts at (src_offset_rows, src_offset_cols)
     * and is of size size_rows x size_cols.
     * The submatrix is copied to the
     * destination matrix starting at
     * (dst_offset_rows, dst_offset_cols)
     *
     * @param src the source matrix
     * @param dst the destination submatrix
     * @param src_offset_rows the offset row in src
     * @param src_offset_cols the offset column in src
     * @param dst_offset_rows the offset row in dst
     * @param dst_offset_cols the offset column in dst
     * @param size_rows the number of rows in the submatrix
     * @param size_cols the number of columns in the submatrix
     *
     * @throw std::invalid_argument if
     *
     *
     */
    inline void copy_submatrix(matrix_gpu & src,
     matrix_gpu & dst, int src_offset_rows, int src_offset_cols,
     int dst_offset_rows, int dst_offset_cols, int size_rows, int size_cols) {
      if (size_rows == 0 || size_cols == 0)
        return;
      if ( (src_offset_rows+size_rows) > src.rows() ||
           (src_offset_cols+size_cols) > src.cols() ) {
        domain_error("copy_submatrix", "submatrix in src" ,
          " is out of bounds", "");
      }
      if ((dst_offset_rows+size_rows) > dst.rows() ||
           (dst_offset_cols+size_cols) > dst.cols() ) {
        domain_error("copy_submatrix", "submatrix in src" ,
         " is out of bounds", "");
      }
      cl::Kernel kernel = opencl_context.get_kernel("copy_submatrix");
      cl::CommandQueue cmdQueue = opencl_context.queue();
      try {
        kernel.setArg(0, src.buffer());
        kernel.setArg(1, dst.buffer());

        kernel.setArg(2, src_offset_rows);
        kernel.setArg(3, src_offset_cols);
        kernel.setArg(4, dst_offset_rows);
        kernel.setArg(5, dst_offset_cols);

        kernel.setArg(6, size_rows);
        kernel.setArg(7, size_cols);

        kernel.setArg(8, src.rows());
        kernel.setArg(9, src.cols());
        kernel.setArg(10, dst.rows());
        kernel.setArg(11, dst.cols());

        cmdQueue.enqueueNDRangeKernel(
          kernel,
          cl::NullRange,
          cl::NDRange(size_rows, size_cols),
          cl::NullRange,
          NULL,
          NULL);
      } catch (const cl::Error& e) {
        check_opencl_error("copy_submatrix", e);
      }
    }

    /**
     * Copies the lower triangular
     * to the upper triangular of the
     * matrix on the GPU.
     *
     * @param A matrix
     * @param lower_upper enum to describe
     * which copy operation to perform
     * LOWER_TO_UPPER_TRIANGULAR - copies the lower
     *   triangular to the upper triangular
     * UPPER_TO_LOWER_TRIANGULAR - copes the upper
     *  triangular to the lower triangular
     *
     * @throw std::invalid_argument if the
     * matrices do not have matching dimensions
     *
     */
    inline void copy_triangular_transposed(matrix_gpu & A,
     copy_transposed_triangular lower_upper) {
      if (A.size() == 0) {
        return;
      }
      if (A.size() == 1) {
        return;
      }
      check_square("copy_triangular_transposed (GPU)", "A", A);
      cl::Kernel kernel =
        opencl_context.get_kernel("copy_triangular_transposed");
      cl::CommandQueue cmdQueue = opencl_context.queue();
      try {
        kernel.setArg(0, A.buffer());
        kernel.setArg(1, A.rows());
        kernel.setArg(2, A.cols());
        kernel.setArg(3, lower_upper);
        cmdQueue.enqueueNDRangeKernel(
          kernel,
          cl::NullRange,
          cl::NDRange(A.rows(), A.cols()),
          cl::NullRange,
          NULL,
          NULL);
      } catch (const cl::Error& e) {
        check_opencl_error("copy_triangular_transposed", e);
      }
    }
    /**
     * Matrix addition on the GPU
     * Adds the first two matrices and stores
     * the result in the third matrix (C=A+B)
     *
     * @param A first matrix
     * @param B second matrix
     *
     * @return sum of A and B
     *
     * @throw std::invalid_argument if the
     * input matrices do not have matching dimensions
     *
     */
    inline matrix_gpu add(matrix_gpu& A,
     matrix_gpu& B) {
      check_matching_dims("add (GPU)", "A", A, "B", B);
      matrix_gpu C(A.rows(), A.cols());
      if (C.size() == 0) {
        return C;
      }
      cl::Kernel kernel = opencl_context.get_kernel("add");
      cl::CommandQueue cmdQueue = opencl_context.queue();
      try {
        kernel.setArg(0, C.buffer());
        kernel.setArg(1, A.buffer());
        kernel.setArg(2, B.buffer());
        kernel.setArg(3, A.rows());
        kernel.setArg(4, A.cols());
        cmdQueue.enqueueNDRangeKernel(
          kernel,
          cl::NullRange,
          cl::NDRange(A.rows(), A.cols()),
          cl::NullRange,
          NULL,
          NULL);
      } catch (const cl::Error& e) {
        check_opencl_error("add", e);
      }
      return C;
    }

    /**
     * Matrix subtraction on the GPU
     * Subtracts the second matrix
     * from the first matrix and stores
     * the result in the third matrix (C=A-B)
     *
     * @param A first matrix
     * @param B second matrix
     *
     * @return subtraction result matrix
     *
     * @throw std::invalid_argument if the
     * input matrices do not have matching dimensions
     *
     */
    inline matrix_gpu subtract(matrix_gpu & A, matrix_gpu & B) {
      check_matching_dims("subtract (GPU)", "A", A, "B", B);
      matrix_gpu C(A.rows(), A.cols());
      if (A.size() == 0) {
        return C;
      }
      cl::Kernel kernel = opencl_context.get_kernel("subtract");
      cl::CommandQueue cmdQueue = opencl_context.queue();

      try {
        kernel.setArg(0, C.buffer());
        kernel.setArg(1, A.buffer());
        kernel.setArg(2, B.buffer());
        kernel.setArg(3, A.rows());
        kernel.setArg(4, A.cols());
        cmdQueue.enqueueNDRangeKernel(
          kernel,
          cl::NullRange,
          cl::NDRange(A.rows(), A.cols()),
          cl::NullRange,
          NULL,
          NULL);
      } catch (cl::Error& e) {
        check_opencl_error("subtract", e);
      }
      return C;
    }
  }
}

#endif
#endif
<|endoftext|>