<commit_before>/*
 # Copyright (c) 2011, Georgia Tech Research Corporation
 # All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions are met:
 #     * Redistributions of source code must retain the above copyright
 #       notice, this list of conditions and the following disclaimer.
 #     * Redistributions in binary form must reproduce the above copyright
 #       notice, this list of conditions and the following disclaimer in the
 #       documentation and/or other materials provided with the distribution.
 #     * Neither the name of the Georgia Tech Research Corporation nor the
 #       names of its contributors may be used to endorse or promote products
 #       derived from this software without specific prior written permission.
 #
 # THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND
 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 # DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT,
 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #

 ## author Advait Jain (Healthcare Robotics Lab, Georgia Tech.)
 ## author Chih-Hung Aaron King (Healthcare Robotics Lab, Georgia Tech.)
 */

//== This application listens for a rigid body named 'Tracker' on a remote machine
//== and publishes & tf it's position and orientation through ROS.
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <iostream>
#include <nav_msgs/Odometry.h>
#include <stdio.h>
#include <math.h>

#include <vrpn_Connection.h>
#include <vrpn_Tracker.h>
#include <Eigen/Geometry>

#include "vicon_odometry_estimator.h"

void VRPN_CALLBACK track_target(void *, const vrpn_TRACKERCB t);

// TODO(millanea@ethz.ch): The following callbacks should be implemented in the case that
//                         the tracking system supports them. In this case a flag should be
//                         added to the code indicating the whether or not the vicon estimator
//                         should be run.
//void VRPN_CALLBACK track_target_velocity(void *, const vrpn_TRACKERVELCB tv);
//void VRPN_CALLBACK track_target_acceleration(void *, const vrpn_TRACKERACCCB ta);

class TargetState
{
 public:
  geometry_msgs::TransformStamped target;
  nav_msgs::Odometry odometry;
};

TargetState *target_state;
std::string object_name;
std::string coordinate_system_string;

enum CoordinateSystem
{
  vicon,
  optitrack
} coordinate_system;

// Global indicating the availability of new VRPN callback function.
bool fresh_data = false;
vrpn_TRACKERCB prev_vrpn_data;

// Pointer to the vicon estimator. Global such that it can be accessed from the callback
vicon_estimator::ViconOdometryEstimator* vicon_odometry_estimator = NULL;

class Rigid_Body
{
 private:
  ros::Publisher target_pub;
  ros::Publisher odometry_pub;
  tf::TransformBroadcaster br;
  vrpn_Connection *connection;
  vrpn_Tracker_Remote *tracker;

 public:
  Rigid_Body(ros::NodeHandle& nh, std::string server_ip, int port, const std::string& object_name)
  {
    target_pub = nh.advertise<geometry_msgs::TransformStamped>("pose", 100);
    odometry_pub = nh.advertise<nav_msgs::Odometry>("odometry", 100);
    std::string connection_name = server_ip + ":" + boost::lexical_cast<std::string>(port);
    connection = vrpn_get_connection_by_name(connection_name.c_str());
    tracker = new vrpn_Tracker_Remote(object_name.c_str(), connection);

    tracker->print_latest_report();
    this->tracker->register_change_handler(NULL, track_target);

    // TODO(millanea@ethz.ch): The following callbacks should be added if they're available.
    //                         See detailed note above.
    //this->tracker->register_change_handler(NULL, track_target_velocity);
    //this->tracker->register_change_handler(NULL, track_target_acceleration);

    tracker->print_latest_report();
  }

  void publish_target_state(TargetState *target_state)
  {
    br.sendTransform(target_state->target);
    target_pub.publish(target_state->target);
  }

  void publish_odometry(TargetState *target_state)
  {
    odometry_pub.publish(target_state->odometry);
  }

  void step_vrpn()
  {
    this->tracker->mainloop();
    this->connection->mainloop();
  }
};

// TODO(millanea@ethz.ch): The following callbacks should be implemented if they're required.
//                         See detailed note above.
//void VRPN_CALLBACK track_target_acceleration(void *, const vrpn_TRACKERACCCB ta) {
//  std::cout<<"acceleration_callback"<<std::endl;
//  std::cout<<"ta.vel[0]"<<ta.acc[0]<<std::endl;
//}
//
//void VRPN_CALLBACK track_target_velocity(void *, const vrpn_TRACKERVELCB tv) {
//  std::cout<<"velocity_callback"<<std::endl;
//  std::cout<<"tv.vel[0]"<<tv.vel[0]<<std::endl;
//}

//== Tracker Position/Orientation Callback ==--
void VRPN_CALLBACK track_target(void *, const vrpn_TRACKERCB t)
{
  Eigen::Quaterniond qOrig(t.quat[3], t.quat[0], t.quat[1], t.quat[2]);
  Eigen::Quaterniond qFix(0.70710678, 0.70710678, 0., 0.);

  Eigen::Quaterniond orientation_measured_B_W;
  Eigen::Vector3d position_measured_W;
  switch (coordinate_system)
  {
    case optitrack:
    {
      // Here we rotate the Optitrack measured quaternion by qFix, a
      // Pi/2 rotation around the x-axis. By doing so we convert from
      // NED to ENU (I think).
      orientation_measured_B_W = qFix * qOrig * qFix.inverse();
      position_measured_W = Eigen::Vector3d(t.pos[0], -t.pos[2], t.pos[1]);
      break;
    }
    case vicon:
    {
      orientation_measured_B_W = qOrig;
      position_measured_W = Eigen::Vector3d(t.pos[0], t.pos[1], t.pos[2]);
      break;
    }
    default:
    {
      ROS_FATAL("Coordinate system not defined!");
      break;
    }
  }

  // verifying that each callback indeed gives fresh data.
  if (prev_vrpn_data.quat[0] == t.quat[0] and prev_vrpn_data.quat[1] == t.quat[1]
      and prev_vrpn_data.quat[2] == t.quat[2] and prev_vrpn_data.quat[3] == t.quat[3]
      and prev_vrpn_data.pos[0] == t.pos[0] and prev_vrpn_data.pos[1] == t.pos[1]
      and prev_vrpn_data.pos[2] == t.pos[2])
    ROS_WARN("Repeated Values");
  prev_vrpn_data = t;

  // Somehow the vrpn msgs are in a different time zone.
  const int kMicroSecToNanoSec = 1000;
  ros::Time timestamp_local = ros::Time::now();
  int timediff_sec = std::round(double(timestamp_local.sec - t.msg_time.tv_sec) / 3600) * 3600;

  int timestamp_nsec = t.msg_time.tv_usec * kMicroSecToNanoSec;
  ros::Time timestamp = ros::Time(t.msg_time.tv_sec + timediff_sec, timestamp_nsec);

  ros::Duration time_diff = ros::Time::now() - timestamp;
  if (std::abs(time_diff.toSec()) > 0.1) {
    ROS_WARN_STREAM_THROTTLE(1, "Time delay: " << time_diff.toSec());
  }

  // Updating the estimates with the new measurements
  vicon_odometry_estimator->updateEstimate(position_measured_W, orientation_measured_B_W);
  vicon_odometry_estimator->publishResults(timestamp);
  Eigen::Vector3d position_estimate_W = vicon_odometry_estimator->getEstimatedPosition();
  Eigen::Vector3d velocity_estimate_W = vicon_odometry_estimator->getEstimatedVelocity();
  Eigen::Quaterniond orientation_estimate_B_W = vicon_odometry_estimator->getEstimatedOrientation();
  Eigen::Vector3d rate_estimate_B = vicon_odometry_estimator->getEstimatedAngularVelocity();

  // Rotating the estimated global frame velocity into the body frame
  Eigen::Vector3d velocity_estimate_B = orientation_estimate_B_W.toRotationMatrix() * velocity_estimate_W;

  // Populating topic contents. Published in main loop
  target_state->target.header.stamp = timestamp;
  target_state->target.header.frame_id = coordinate_system_string;
  target_state->target.child_frame_id = object_name;
  target_state->target.transform.translation.x = position_estimate_W.x();
  target_state->target.transform.translation.y = position_estimate_W.y();
  target_state->target.transform.translation.z = position_estimate_W.z();
  target_state->target.transform.rotation.x = orientation_estimate_B_W.x();
  target_state->target.transform.rotation.y = orientation_estimate_B_W.y();
  target_state->target.transform.rotation.z = orientation_estimate_B_W.z();
  target_state->target.transform.rotation.w = orientation_estimate_B_W.w();

  // Assemble odometry message.
  target_state->odometry.header.stamp = timestamp;
  target_state->odometry.header.frame_id = coordinate_system_string;
  target_state->odometry.child_frame_id = object_name;
  target_state->odometry.pose.pose.position.x = position_estimate_W.x();
  target_state->odometry.pose.pose.position.y = position_estimate_W.y();
  target_state->odometry.pose.pose.position.z = position_estimate_W.z();
  target_state->odometry.pose.pose.orientation.w = orientation_estimate_B_W.w();
  target_state->odometry.pose.pose.orientation.x = orientation_estimate_B_W.x();
  target_state->odometry.pose.pose.orientation.y = orientation_estimate_B_W.y();
  target_state->odometry.pose.pose.orientation.z = orientation_estimate_B_W.z();
  target_state->odometry.twist.twist.linear.x = velocity_estimate_B.x();
  target_state->odometry.twist.twist.linear.y = velocity_estimate_B.y();
  target_state->odometry.twist.twist.linear.z = velocity_estimate_B.z();
  target_state->odometry.twist.twist.angular.x = rate_estimate_B.x();
  target_state->odometry.twist.twist.angular.y = rate_estimate_B.y();
  target_state->odometry.twist.twist.angular.z = rate_estimate_B.z();

  // Indicating to the main loop the data is ready for publishing
  fresh_data = true;
}

int main(int argc, char* argv[])
{
  ros::init(argc, argv, "ros_vrpn_client");
  ros::NodeHandle nh("~");

  target_state = new TargetState;

  std::string vrpn_server_ip;
  int vrpn_port;
  std::string trackedObjectName;
  std::string object_name;

  nh.param<std::string>("vrpn_server_ip", vrpn_server_ip, std::string());
  nh.param<int>("vrpn_port", vrpn_port, 3883);
  nh.param<std::string>("vrpn_coordinate_system", coordinate_system_string, "vicon");
  nh.param<std::string>("object_name", object_name, "bluebird");

  std::cout << "vrpn_server_ip:" << vrpn_server_ip << std::endl;
  std::cout << "vrpn_port:" << vrpn_port << std::endl;
  std::cout << "vrpn_coordinate_system:" << coordinate_system_string << std::endl;
  std::cout << "object_name:" << object_name << std::endl;

  if (coordinate_system_string == std::string("vicon"))
  {

  }
  else if (coordinate_system_string == std::string("optitrack"))
  {

  }
  else
  {
    ROS_FATAL("ROS param vrpn_coordinate_system should be either 'vicon' or 'optitrack'!");
  }

  // Creating the estimator
  vicon_odometry_estimator = new vicon_estimator::ViconOdometryEstimator(nh);
  vicon_odometry_estimator->initializeParameters(nh);
  vicon_odometry_estimator->reset();

  // Creating object which handles data publishing
  Rigid_Body tool(nh, vrpn_server_ip, vrpn_port, object_name);

  ros::Rate loop_rate(1000);  //TODO(gohlp): fix this

  while (ros::ok())
  {
    tool.step_vrpn();

    // only publish when receive data over VRPN.
    if (fresh_data == true)
    {
      tool.publish_target_state(target_state);
      tool.publish_odometry(target_state);
      fresh_data = false;
    }
    //ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}
<commit_msg>removed boost cast and added anonymous node name to ros init<commit_after>/*
 # Copyright (c) 2011, Georgia Tech Research Corporation
 # All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions are met:
 #     * Redistributions of source code must retain the above copyright
 #       notice, this list of conditions and the following disclaimer.
 #     * Redistributions in binary form must reproduce the above copyright
 #       notice, this list of conditions and the following disclaimer in the
 #       documentation and/or other materials provided with the distribution.
 #     * Neither the name of the Georgia Tech Research Corporation nor the
 #       names of its contributors may be used to endorse or promote products
 #       derived from this software without specific prior written permission.
 #
 # THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND
 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 # DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT,
 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #

 ## author Advait Jain (Healthcare Robotics Lab, Georgia Tech.)
 ## author Chih-Hung Aaron King (Healthcare Robotics Lab, Georgia Tech.)
 */

//== This application listens for a rigid body named 'Tracker' on a remote machine
//== and publishes & tf it's position and orientation through ROS.
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <iostream>
#include <nav_msgs/Odometry.h>
#include <stdio.h>
#include <math.h>
#include <iostream>

#include <vrpn_Connection.h>
#include <vrpn_Tracker.h>
#include <Eigen/Geometry>

#include "vicon_odometry_estimator.h"

void VRPN_CALLBACK track_target(void *, const vrpn_TRACKERCB t);

// TODO(millanea@ethz.ch): The following callbacks should be implemented in the case that
//                         the tracking system supports them. In this case a flag should be
//                         added to the code indicating the whether or not the vicon estimator
//                         should be run.
//void VRPN_CALLBACK track_target_velocity(void *, const vrpn_TRACKERVELCB tv);
//void VRPN_CALLBACK track_target_acceleration(void *, const vrpn_TRACKERACCCB ta);

class TargetState
{
 public:
  geometry_msgs::TransformStamped target;
  nav_msgs::Odometry odometry;
};

TargetState *target_state;
std::string object_name;
std::string coordinate_system_string;

enum CoordinateSystem
{
  vicon,
  optitrack
} coordinate_system;

// Global indicating the availability of new VRPN callback function.
bool fresh_data = false;
vrpn_TRACKERCB prev_vrpn_data;

// Pointer to the vicon estimator. Global such that it can be accessed from the callback
vicon_estimator::ViconOdometryEstimator* vicon_odometry_estimator = NULL;

class Rigid_Body
{
 private:
  ros::Publisher target_pub;
  ros::Publisher odometry_pub;
  tf::TransformBroadcaster br;
  vrpn_Connection *connection;
  vrpn_Tracker_Remote *tracker;

 public:
  Rigid_Body(ros::NodeHandle& nh, std::string server_ip, int port, const std::string& object_name)
  {
    target_pub = nh.advertise<geometry_msgs::TransformStamped>("pose", 100);
    odometry_pub = nh.advertise<nav_msgs::Odometry>("odometry", 100);
    std::stringstream connection_name;
    connection_name << server_ip << ":" << port;
    connection = vrpn_get_connection_by_name(connection_name.str().c_str());
    tracker = new vrpn_Tracker_Remote(object_name.c_str(), connection);

    tracker->print_latest_report();
    this->tracker->register_change_handler(NULL, track_target);

    // TODO(millanea@ethz.ch): The following callbacks should be added if they're available.
    //                         See detailed note above.
    //this->tracker->register_change_handler(NULL, track_target_velocity);
    //this->tracker->register_change_handler(NULL, track_target_acceleration);

    tracker->print_latest_report();
  }

  void publish_target_state(TargetState *target_state)
  {
    br.sendTransform(target_state->target);
    target_pub.publish(target_state->target);
  }

  void publish_odometry(TargetState *target_state)
  {
    odometry_pub.publish(target_state->odometry);
  }

  void step_vrpn()
  {
    this->tracker->mainloop();
    this->connection->mainloop();
  }
};

// TODO(millanea@ethz.ch): The following callbacks should be implemented if they're required.
//                         See detailed note above.
//void VRPN_CALLBACK track_target_acceleration(void *, const vrpn_TRACKERACCCB ta) {
//  std::cout<<"acceleration_callback"<<std::endl;
//  std::cout<<"ta.vel[0]"<<ta.acc[0]<<std::endl;
//}
//
//void VRPN_CALLBACK track_target_velocity(void *, const vrpn_TRACKERVELCB tv) {
//  std::cout<<"velocity_callback"<<std::endl;
//  std::cout<<"tv.vel[0]"<<tv.vel[0]<<std::endl;
//}

//== Tracker Position/Orientation Callback ==--
void VRPN_CALLBACK track_target(void *, const vrpn_TRACKERCB t)
{
  Eigen::Quaterniond qOrig(t.quat[3], t.quat[0], t.quat[1], t.quat[2]);
  Eigen::Quaterniond qFix(0.70710678, 0.70710678, 0., 0.);

  Eigen::Quaterniond orientation_measured_B_W;
  Eigen::Vector3d position_measured_W;
  switch (coordinate_system)
  {
    case optitrack:
    {
      // Here we rotate the Optitrack measured quaternion by qFix, a
      // Pi/2 rotation around the x-axis. By doing so we convert from
      // NED to ENU (I think).
      orientation_measured_B_W = qFix * qOrig * qFix.inverse();
      position_measured_W = Eigen::Vector3d(t.pos[0], -t.pos[2], t.pos[1]);
      break;
    }
    case vicon:
    {
      orientation_measured_B_W = qOrig;
      position_measured_W = Eigen::Vector3d(t.pos[0], t.pos[1], t.pos[2]);
      break;
    }
    default:
    {
      ROS_FATAL("Coordinate system not defined!");
      break;
    }
  }

  // verifying that each callback indeed gives fresh data.
  if (prev_vrpn_data.quat[0] == t.quat[0] and prev_vrpn_data.quat[1] == t.quat[1]
      and prev_vrpn_data.quat[2] == t.quat[2] and prev_vrpn_data.quat[3] == t.quat[3]
      and prev_vrpn_data.pos[0] == t.pos[0] and prev_vrpn_data.pos[1] == t.pos[1]
      and prev_vrpn_data.pos[2] == t.pos[2])
    ROS_WARN("Repeated Values");
  prev_vrpn_data = t;

  // Somehow the vrpn msgs are in a different time zone.
  const int kMicroSecToNanoSec = 1000;
  ros::Time timestamp_local = ros::Time::now();
  int timediff_sec = std::round(double(timestamp_local.sec - t.msg_time.tv_sec) / 3600) * 3600;

  int timestamp_nsec = t.msg_time.tv_usec * kMicroSecToNanoSec;
  ros::Time timestamp = ros::Time(t.msg_time.tv_sec + timediff_sec, timestamp_nsec);

  ros::Duration time_diff = ros::Time::now() - timestamp;
  if (std::abs(time_diff.toSec()) > 0.1) {
    ROS_WARN_STREAM_THROTTLE(1, "Time delay: " << time_diff.toSec());
  }

  // Updating the estimates with the new measurements
  vicon_odometry_estimator->updateEstimate(position_measured_W, orientation_measured_B_W);
  vicon_odometry_estimator->publishResults(timestamp);
  Eigen::Vector3d position_estimate_W = vicon_odometry_estimator->getEstimatedPosition();
  Eigen::Vector3d velocity_estimate_W = vicon_odometry_estimator->getEstimatedVelocity();
  Eigen::Quaterniond orientation_estimate_B_W = vicon_odometry_estimator->getEstimatedOrientation();
  Eigen::Vector3d rate_estimate_B = vicon_odometry_estimator->getEstimatedAngularVelocity();

  // Rotating the estimated global frame velocity into the body frame
  Eigen::Vector3d velocity_estimate_B = orientation_estimate_B_W.toRotationMatrix() * velocity_estimate_W;

  // Populating topic contents. Published in main loop
  target_state->target.header.stamp = timestamp;
  target_state->target.header.frame_id = coordinate_system_string;
  target_state->target.child_frame_id = object_name;
  target_state->target.transform.translation.x = position_estimate_W.x();
  target_state->target.transform.translation.y = position_estimate_W.y();
  target_state->target.transform.translation.z = position_estimate_W.z();
  target_state->target.transform.rotation.x = orientation_estimate_B_W.x();
  target_state->target.transform.rotation.y = orientation_estimate_B_W.y();
  target_state->target.transform.rotation.z = orientation_estimate_B_W.z();
  target_state->target.transform.rotation.w = orientation_estimate_B_W.w();

  // Assemble odometry message.
  target_state->odometry.header.stamp = timestamp;
  target_state->odometry.header.frame_id = coordinate_system_string;
  target_state->odometry.child_frame_id = object_name;
  target_state->odometry.pose.pose.position.x = position_estimate_W.x();
  target_state->odometry.pose.pose.position.y = position_estimate_W.y();
  target_state->odometry.pose.pose.position.z = position_estimate_W.z();
  target_state->odometry.pose.pose.orientation.w = orientation_estimate_B_W.w();
  target_state->odometry.pose.pose.orientation.x = orientation_estimate_B_W.x();
  target_state->odometry.pose.pose.orientation.y = orientation_estimate_B_W.y();
  target_state->odometry.pose.pose.orientation.z = orientation_estimate_B_W.z();
  target_state->odometry.twist.twist.linear.x = velocity_estimate_B.x();
  target_state->odometry.twist.twist.linear.y = velocity_estimate_B.y();
  target_state->odometry.twist.twist.linear.z = velocity_estimate_B.z();
  target_state->odometry.twist.twist.angular.x = rate_estimate_B.x();
  target_state->odometry.twist.twist.angular.y = rate_estimate_B.y();
  target_state->odometry.twist.twist.angular.z = rate_estimate_B.z();

  // Indicating to the main loop the data is ready for publishing
  fresh_data = true;
}

int main(int argc, char* argv[])
{
  ros::init(argc, argv, "ros_vrpn_client", ros::init_options::AnonymousName);
  ros::NodeHandle nh("~");

  target_state = new TargetState;

  std::string vrpn_server_ip;
  int vrpn_port;
  std::string trackedObjectName;

  nh.param<std::string>("vrpn_server_ip", vrpn_server_ip, std::string());
  nh.param<int>("vrpn_port", vrpn_port, 3883);
  nh.param<std::string>("vrpn_coordinate_system", coordinate_system_string, "vicon");
  nh.param<std::string>("object_name", object_name, "bluebird");

  std::cout << "vrpn_server_ip:" << vrpn_server_ip << std::endl;
  std::cout << "vrpn_port:" << vrpn_port << std::endl;
  std::cout << "vrpn_coordinate_system:" << coordinate_system_string << std::endl;
  std::cout << "object_name:" << object_name << std::endl;

  if (coordinate_system_string == std::string("vicon"))
  {

  }
  else if (coordinate_system_string == std::string("optitrack"))
  {

  }
  else
  {
    ROS_FATAL("ROS param vrpn_coordinate_system should be either 'vicon' or 'optitrack'!");
  }

  // Creating the estimator
  vicon_odometry_estimator = new vicon_estimator::ViconOdometryEstimator(nh);
  vicon_odometry_estimator->initializeParameters(nh);
  vicon_odometry_estimator->reset();

  // Creating object which handles data publishing
  Rigid_Body tool(nh, vrpn_server_ip, vrpn_port, object_name);

  ros::Rate loop_rate(1000);  //TODO(gohlp): fix this

  while (ros::ok())
  {
    tool.step_vrpn();

    // only publish when receive data over VRPN.
    if (fresh_data == true)
    {
      tool.publish_target_state(target_state);
      tool.publish_odometry(target_state);
      fresh_data = false;
    }
    //ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}
<|endoftext|>


<commit_before>// Copyright 2016 Tom Barthel-Steer
// http://www.tomsteer.net
//
// 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 "sacnsocket.h"

#include <QNetworkAddressEntry>
#include <QDebug>
#include <QThread>
#include "ipaddr.h"
#include "streamcommon.h"
#include "streamingacn.h"
#include "sacnlistener.h"
#include "preferences.h"

sACNRxSocket::sACNRxSocket(QNetworkInterface iface, QObject *parent) :
    QUdpSocket(parent),
    m_interface(iface)
{}

sACNRxSocket::sBindStatus sACNRxSocket::bind(quint16 universe)
{
    sACNRxSocket::sBindStatus status;
    m_universe = universe;
    bool ok = false;

    CIPAddr multicastAddr;
    GetUniverseAddress(universe, multicastAddr);

    QHostAddress listenAddr = QHostAddress::AnyIPv4;
#ifdef TARGET_WINXP
    #pragma message("Unicast listener unavailable for this platform")
    listenAddr = multicastAddr.ToQHostAddress();
#endif
    ok = QUdpSocket::bind(listenAddr,
              STREAM_IP_PORT,
              QAbstractSocket::ShareAddress | QAbstractSocket::ReuseAddressHint);

    status.unicast = ok ? BIND_OK : BIND_FAILED;

    // Join multicast on selected NIC
    if (ok) {
        #if (QT_VERSION <= QT_VERSION_CHECK(5, 8, 0))
            #ifdef Q_OS_WIN
                #pragma message("setMulticastInterface() fails to bind to correct interface on systems running IPV4 and IPv6 with QT <= 5.8.0")
            #else
                #error setMulticastInterface() fails to bind to correct interface on systems running IPV4 and IPv6 with QT <= 5.8.0
            #endif
        #endif
        setMulticastInterface(m_interface);
        ok |= joinMulticastGroup(QHostAddress(multicastAddr.GetV4Address()), m_interface);
        status.multicast = ok ? BIND_OK : BIND_FAILED;
    }

    if(ok) {
#ifdef TARGET_WINXP
        qDebug() << "sACNRxSocket " << QThread::currentThreadId() << ": Bound to IP:" << listenAddr.toString();
#else
        qDebug() << "sACNRxSocket " << QThread::currentThreadId() << ": Bound to interface:" << multicastInterface().name();
#endif
        qDebug() << "sACNRxSocket " << QThread::currentThreadId() << ": Joining Multicast Group:" << QHostAddress(multicastAddr.GetV4Address()).toString();
    } else {
        close();
        qDebug() << "sACNRxSocket " << QThread::currentThreadId() << ": Failed to bind RX socket";
    }

    return status;
}

sACNTxSocket::sACNTxSocket(QNetworkInterface iface, QObject *parent) :
    QUdpSocket(parent),
    m_interface(iface)
{}

bool sACNTxSocket::bind()
{
    bool ok = false;

    // Bind to first IPv4 address on selected NIC
    foreach (QNetworkAddressEntry ifaceAddr, m_interface.addressEntries()) {
        if (ifaceAddr.ip().protocol() == QAbstractSocket::IPv4Protocol) {
            ok = QUdpSocket::bind(ifaceAddr.ip());
            if (ok) {
                qDebug() << "sACNTxSocket " << QThread::currentThreadId() << ": Bound to IP:" << ifaceAddr.ip().toString();

#ifndef Q_OS_WIN
                // Don't send to self, this will cause issues on multihomed systems
                setSocketOption(QAbstractSocket::MulticastLoopbackOption, QVariant(0));
#endif

                setSocketOption(QAbstractSocket::MulticastTtlOption, QVariant(Preferences::getInstance()->GetMulticastTtl()));

                setMulticastInterface(m_interface);
                qDebug() << "sACNTxSocket " << QThread::currentThreadId() << ": Bound to interface:" << multicastInterface().name();
            }
            break;
        }
    }

    if (!ok) qDebug() << "sACNTxSocket " << QThread::currentThreadId() << ": Failed to bind TX socket";

    return ok;
}

qint64 sACNTxSocket::writeDatagram(const char *data, qint64 len, const QHostAddress &host, quint16 port)
{
#ifndef Q_OS_WIN
    // If destination multicast (and we're not in offline mode)
    // Copy to correct internal listener(s) for local display
    if (host.isMulticast() && !localAddress().isLoopback()) {
        CIPAddr addr;
        for (auto &weakListener : sACNManager::getInstance()->getListenerList()) {
            sACNManager::tListener listener(weakListener);
            if (listener) {
                GetUniverseAddress(listener->universe(), addr);
                if (QHostAddress(addr.GetV4Address()) == host) {
                    listener->processDatagram(
                                QByteArray(data, len),
                                QHostAddress(host),
                                localAddress());
                }
            }
        }
    }
#endif

    // Send to world
    return QUdpSocket::writeDatagram(data, len, host, port);
}
<commit_msg>Fix MacOS loopback<commit_after>// Copyright 2016 Tom Barthel-Steer
// http://www.tomsteer.net
//
// 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 "sacnsocket.h"

#include <QNetworkAddressEntry>
#include <QDebug>
#include <QThread>
#include "ipaddr.h"
#include "streamcommon.h"
#include "streamingacn.h"
#include "sacnlistener.h"
#include "preferences.h"

sACNRxSocket::sACNRxSocket(QNetworkInterface iface, QObject *parent) :
    QUdpSocket(parent),
    m_interface(iface)
{}

sACNRxSocket::sBindStatus sACNRxSocket::bind(quint16 universe)
{
    sACNRxSocket::sBindStatus status;
    m_universe = universe;
    bool ok = false;

    CIPAddr multicastAddr;
    GetUniverseAddress(universe, multicastAddr);

    QHostAddress listenAddr = QHostAddress::AnyIPv4;
#ifdef TARGET_WINXP
    #pragma message("Unicast listener unavailable for this platform")
    listenAddr = multicastAddr.ToQHostAddress();
#endif
    ok = QUdpSocket::bind(listenAddr,
              STREAM_IP_PORT,
              QAbstractSocket::ShareAddress | QAbstractSocket::ReuseAddressHint);

    status.unicast = ok ? BIND_OK : BIND_FAILED;

    // Join multicast on selected NIC
    if (ok) {
        #if (QT_VERSION <= QT_VERSION_CHECK(5, 8, 0))
            #ifdef Q_OS_WIN
                #pragma message("setMulticastInterface() fails to bind to correct interface on systems running IPV4 and IPv6 with QT <= 5.8.0")
            #else
                #error setMulticastInterface() fails to bind to correct interface on systems running IPV4 and IPv6 with QT <= 5.8.0
            #endif
        #endif
        setMulticastInterface(m_interface);
        ok |= joinMulticastGroup(QHostAddress(multicastAddr.GetV4Address()), m_interface);
        status.multicast = ok ? BIND_OK : BIND_FAILED;
    }

    if(ok) {
#ifdef TARGET_WINXP
        qDebug() << "sACNRxSocket " << QThread::currentThreadId() << ": Bound to IP:" << listenAddr.toString();
#else
        qDebug() << "sACNRxSocket " << QThread::currentThreadId() << ": Bound to interface:" << multicastInterface().name();
#endif
        qDebug() << "sACNRxSocket " << QThread::currentThreadId() << ": Joining Multicast Group:" << QHostAddress(multicastAddr.GetV4Address()).toString();
    } else {
        close();
        qDebug() << "sACNRxSocket " << QThread::currentThreadId() << ": Failed to bind RX socket";
    }

    return status;
}

sACNTxSocket::sACNTxSocket(QNetworkInterface iface, QObject *parent) :
    QUdpSocket(parent),
    m_interface(iface)
{}

bool sACNTxSocket::bind()
{
    bool ok = false;

    // Bind to first IPv4 address on selected NIC
    foreach (QNetworkAddressEntry ifaceAddr, m_interface.addressEntries()) {
        if (ifaceAddr.ip().protocol() == QAbstractSocket::IPv4Protocol) {
            ok = QUdpSocket::bind(ifaceAddr.ip());
            if (ok) {
                qDebug() << "sACNTxSocket " << QThread::currentThreadId() << ": Bound to IP:" << ifaceAddr.ip().toString();

#ifndef Q_OS_WIN
                // Don't send to self, this will cause issues on multihomed systems
                setSocketOption(QAbstractSocket::MulticastLoopbackOption, QVariant(0));
#endif

                setSocketOption(QAbstractSocket::MulticastTtlOption, QVariant(Preferences::getInstance()->GetMulticastTtl()));

                setMulticastInterface(m_interface);
                qDebug() << "sACNTxSocket " << QThread::currentThreadId() << ": Bound to interface:" << multicastInterface().name();
            }
            break;
        }
    }

    if (!ok) qDebug() << "sACNTxSocket " << QThread::currentThreadId() << ": Failed to bind TX socket";

    return ok;
}

bool writeDatagramLoopbackTest(const QHostAddress &dest, QHostAddress localAddress) {
#ifdef Q_OS_WIN
    return false;
#endif

#ifdef Q_OS_LINUX
    return (dest.isMulticast() && !localAddress.isLoopback());
#endif

#ifdef Q_OS_MACOS
    return (dest.isMulticast());
#endif
}

qint64 sACNTxSocket::writeDatagram(const char *data, qint64 len, const QHostAddress &host, quint16 port)
{
    // Copy to correct internal listener(s) for local display, if required by platform
    if (writeDatagramLoopbackTest(host, localAddress())) {
        CIPAddr addr;
        for (auto &weakListener : sACNManager::getInstance()->getListenerList()) {
            sACNManager::tListener listener(weakListener);
            if (listener) {
                GetUniverseAddress(listener->universe(), addr);
                if (QHostAddress(addr.GetV4Address()) == host) {
                    listener->processDatagram(
                                QByteArray(data, len),
                                QHostAddress(host),
                                localAddress());
                }
            }
        }
    }

    // Send to world
    return QUdpSocket::writeDatagram(data, len, host, port);
}
<|endoftext|>


<commit_before>#pragma once

#include <CoreFoundation/CoreFoundation.h>
#include <chrono>
#include <cstdlib>
#include <filesystem>
#include <pqrs/osx/launchctl.hpp>
#include <spdlog/fmt/fmt.h>
#include <string>
#include <string_view>
#include <thread>

namespace krbn {
class constants final {
public:
  static const char* get_version_file_path(void) {
    return "/Library/Application Support/org.pqrs/Karabiner-Elements/version";
  }

  static const char* get_tmp_directory(void) {
    return "/Library/Application Support/org.pqrs/tmp";
  }

  static std::string get_pid_directory(void) {
    return "/Library/Application Support/org.pqrs/tmp/pid";
  }

  static std::string get_rootonly_directory(void) {
    return "/Library/Application Support/org.pqrs/tmp/rootonly";
  }

  static std::filesystem::path get_system_user_directory(void) {
    return "/Library/Application Support/org.pqrs/tmp/user";
  }

  static std::filesystem::path get_system_user_directory(uid_t uid) {
    return fmt::format("{0}/{1}", get_system_user_directory().string(), uid);
  }

  static const char* get_observer_state_json_file_path(void) {
    return "/Library/Application Support/org.pqrs/tmp/karabiner_observer_state.json";
  }

  static const char* get_grabber_state_json_file_path(void) {
    return "/Library/Application Support/org.pqrs/tmp/karabiner_grabber_state.json";
  }

  static const char* get_grabber_socket_file_path(void) {
    return "/Library/Application Support/org.pqrs/tmp/karabiner_grabber_receiver";
  }

  static std::string get_grabber_session_monitor_receiver_socket_file_path(void) {
    return get_rootonly_directory() + "/karabiner_grabber_session_monitor_receiver";
  }

  static const char* get_devices_json_file_path(void) {
    return "/Library/Application Support/org.pqrs/tmp/karabiner_grabber_devices.json";
  }

  static const char* get_device_details_json_file_path(void) {
    return "/Library/Application Support/org.pqrs/tmp/karabiner_grabber_device_details.json";
  }

  static const char* get_manipulator_environment_json_file_path(void) {
    return "/Library/Application Support/org.pqrs/tmp/karabiner_grabber_manipulator_environment.json";
  }

  static const char* get_notification_message_file_path(void) {
    return "/Library/Application Support/org.pqrs/tmp/karabiner_notification_message.json";
  }

  static std::string get_session_monitor_receiver_socket_file_path(uid_t uid) {
    return fmt::format("{0}/karabiner_session_monitor_receiver.{1}", get_rootonly_directory(), uid);
  }

  static const char* get_system_configuration_directory(void) {
    return "/Library/Application Support/org.pqrs/config";
  }

  static const char* get_system_core_configuration_file_path(void) {
    return "/Library/Application Support/org.pqrs/config/karabiner.json";
  }

  static const std::string& get_user_configuration_directory(void) {
    static std::mutex mutex;
    std::lock_guard<std::mutex> guard(mutex);

    static bool once = false;
    static std::string directory;

    if (!once) {
      once = true;
      if (auto xdg_config_home = std::getenv("XDG_CONFIG_HOME")) {
        directory = xdg_config_home;
      } else {
        if (auto home = std::getenv("HOME")) {
          directory = home;
          directory += "/.config";
        }
      }

      if (!directory.empty()) {
        directory += "/karabiner";
      }
    }

    return directory;
  }

  static const std::string& get_user_data_directory(void) {
    static std::mutex mutex;
    std::lock_guard<std::mutex> guard(mutex);

    static bool once = false;
    static std::string directory;

    if (!once) {
      once = true;
      if (auto xdg_config_home = std::getenv("XDG_DATA_HOME")) {
        directory = xdg_config_home;
      } else {
        if (auto home = std::getenv("HOME")) {
          directory = home;
          directory += "/.local/share";
        }
      }

      if (!directory.empty()) {
        directory += "/karabiner";
      }
    }

    return directory;
  }

  static const std::string& get_user_core_configuration_file_path(void) {
    static std::mutex mutex;
    std::lock_guard<std::mutex> guard(mutex);

    static bool once = false;
    static std::string file_path;

    if (!once) {
      once = true;
      auto directory = get_user_configuration_directory();
      if (!directory.empty()) {
        file_path = directory + "/karabiner.json";
      }
    }

    return file_path;
  }

  static const std::string& get_user_core_configuration_automatic_backups_directory(void) {
    static std::mutex mutex;
    std::lock_guard<std::mutex> guard(mutex);

    static bool once = false;
    static std::string directory;

    if (!once) {
      once = true;
      auto d = get_user_configuration_directory();
      if (!d.empty()) {
        directory = d + "/automatic_backups";
      }
    }

    return directory;
  }

  static const std::string& get_user_complex_modifications_assets_directory(void) {
    static std::mutex mutex;
    std::lock_guard<std::mutex> guard(mutex);

    static bool once = false;
    static std::string directory;

    if (!once) {
      once = true;
      auto d = get_user_configuration_directory();
      if (!d.empty()) {
        directory = d + "/assets/complex_modifications";
      }
    }

    return directory;
  }

  static const std::string& get_user_log_directory(void) {
    static std::mutex mutex;
    std::lock_guard<std::mutex> guard(mutex);

    static bool once = false;
    static std::string directory;

    if (!once) {
      once = true;
      auto d = get_user_data_directory();
      if (!d.empty()) {
        directory = d + "/log";
      }
    }

    return directory;
  }

  static const std::string& get_user_pid_directory(void) {
    static std::mutex mutex;
    std::lock_guard<std::mutex> guard(mutex);

    static bool once = false;
    static std::string directory;

    if (!once) {
      once = true;
      auto d = get_user_data_directory();
      if (!d.empty()) {
        directory = d + "/pid";
      }
    }

    return directory;
  }

  static const char* get_distributed_notification_observed_object(void) {
    return "org.pqrs.karabiner";
  }

  static const char* get_distributed_notification_console_user_server_is_disabled(void) {
    return "console_user_server_is_disabled";
  }

  static const size_t get_local_datagram_buffer_size(void) {
    return 32 * 1024;
  }

  static pqrs::osx::launchctl::service_name get_observer_agent_launchctl_service_name(void) {
    return pqrs::osx::launchctl::service_name("org.pqrs.karabiner.agent.karabiner_observer");
  }

  static pqrs::osx::launchctl::service_path get_observer_agent_launchctl_service_path(void) {
    return pqrs::osx::launchctl::service_path("/Library/LaunchAgents/org.pqrs.karabiner.agent.karabiner_observer.plist");
  }

  static pqrs::osx::launchctl::service_name get_grabber_agent_launchctl_service_name(void) {
    return pqrs::osx::launchctl::service_name("org.pqrs.karabiner.agent.karabiner_grabber");
  }

  static pqrs::osx::launchctl::service_path get_grabber_agent_launchctl_service_path(void) {
    return pqrs::osx::launchctl::service_path("/Library/LaunchAgents/org.pqrs.karabiner.agent.karabiner_grabber.plist");
  }

  static pqrs::osx::launchctl::service_name get_session_monitor_launchctl_service_name(void) {
    return pqrs::osx::launchctl::service_name("org.pqrs.karabiner.karabiner_session_monitor");
  }

  static pqrs::osx::launchctl::service_path get_session_monitor_launchctl_service_path(void) {
    return pqrs::osx::launchctl::service_path("/Library/LaunchAgents/org.pqrs.karabiner.karabiner_session_monitor.plist");
  }

  static pqrs::osx::launchctl::service_name get_console_user_server_launchctl_service_name(void) {
    return pqrs::osx::launchctl::service_name("org.pqrs.karabiner.karabiner_console_user_server");
  }

  static pqrs::osx::launchctl::service_path get_console_user_server_launchctl_service_path(void) {
    return pqrs::osx::launchctl::service_path("/Library/LaunchAgents/org.pqrs.karabiner.karabiner_console_user_server.plist");
  }

  static pqrs::osx::launchctl::service_name get_notification_window_launchctl_service_name(void) {
    return pqrs::osx::launchctl::service_name("org.pqrs.karabiner.NotificationWindow");
  }

  static pqrs::osx::launchctl::service_path get_notification_window_launchctl_service_path(void) {
    return pqrs::osx::launchctl::service_path("/Library/LaunchAgents/org.pqrs.karabiner.NotificationWindow.plist");
  }
};
} // namespace krbn
<commit_msg>Add constants::get_grabber_daemon_launchctl_service_name, get_grabber_daemon_launchctl_service_path<commit_after>#pragma once

#include <CoreFoundation/CoreFoundation.h>
#include <chrono>
#include <cstdlib>
#include <filesystem>
#include <pqrs/osx/launchctl.hpp>
#include <spdlog/fmt/fmt.h>
#include <string>
#include <string_view>
#include <thread>

namespace krbn {
class constants final {
public:
  static const char* get_version_file_path(void) {
    return "/Library/Application Support/org.pqrs/Karabiner-Elements/version";
  }

  static const char* get_tmp_directory(void) {
    return "/Library/Application Support/org.pqrs/tmp";
  }

  static std::string get_pid_directory(void) {
    return "/Library/Application Support/org.pqrs/tmp/pid";
  }

  static std::string get_rootonly_directory(void) {
    return "/Library/Application Support/org.pqrs/tmp/rootonly";
  }

  static std::filesystem::path get_system_user_directory(void) {
    return "/Library/Application Support/org.pqrs/tmp/user";
  }

  static std::filesystem::path get_system_user_directory(uid_t uid) {
    return fmt::format("{0}/{1}", get_system_user_directory().string(), uid);
  }

  static const char* get_observer_state_json_file_path(void) {
    return "/Library/Application Support/org.pqrs/tmp/karabiner_observer_state.json";
  }

  static const char* get_grabber_state_json_file_path(void) {
    return "/Library/Application Support/org.pqrs/tmp/karabiner_grabber_state.json";
  }

  static const char* get_grabber_socket_file_path(void) {
    return "/Library/Application Support/org.pqrs/tmp/karabiner_grabber_receiver";
  }

  static std::string get_grabber_session_monitor_receiver_socket_file_path(void) {
    return get_rootonly_directory() + "/karabiner_grabber_session_monitor_receiver";
  }

  static const char* get_devices_json_file_path(void) {
    return "/Library/Application Support/org.pqrs/tmp/karabiner_grabber_devices.json";
  }

  static const char* get_device_details_json_file_path(void) {
    return "/Library/Application Support/org.pqrs/tmp/karabiner_grabber_device_details.json";
  }

  static const char* get_manipulator_environment_json_file_path(void) {
    return "/Library/Application Support/org.pqrs/tmp/karabiner_grabber_manipulator_environment.json";
  }

  static const char* get_notification_message_file_path(void) {
    return "/Library/Application Support/org.pqrs/tmp/karabiner_notification_message.json";
  }

  static std::string get_session_monitor_receiver_socket_file_path(uid_t uid) {
    return fmt::format("{0}/karabiner_session_monitor_receiver.{1}", get_rootonly_directory(), uid);
  }

  static const char* get_system_configuration_directory(void) {
    return "/Library/Application Support/org.pqrs/config";
  }

  static const char* get_system_core_configuration_file_path(void) {
    return "/Library/Application Support/org.pqrs/config/karabiner.json";
  }

  static const std::string& get_user_configuration_directory(void) {
    static std::mutex mutex;
    std::lock_guard<std::mutex> guard(mutex);

    static bool once = false;
    static std::string directory;

    if (!once) {
      once = true;
      if (auto xdg_config_home = std::getenv("XDG_CONFIG_HOME")) {
        directory = xdg_config_home;
      } else {
        if (auto home = std::getenv("HOME")) {
          directory = home;
          directory += "/.config";
        }
      }

      if (!directory.empty()) {
        directory += "/karabiner";
      }
    }

    return directory;
  }

  static const std::string& get_user_data_directory(void) {
    static std::mutex mutex;
    std::lock_guard<std::mutex> guard(mutex);

    static bool once = false;
    static std::string directory;

    if (!once) {
      once = true;
      if (auto xdg_config_home = std::getenv("XDG_DATA_HOME")) {
        directory = xdg_config_home;
      } else {
        if (auto home = std::getenv("HOME")) {
          directory = home;
          directory += "/.local/share";
        }
      }

      if (!directory.empty()) {
        directory += "/karabiner";
      }
    }

    return directory;
  }

  static const std::string& get_user_core_configuration_file_path(void) {
    static std::mutex mutex;
    std::lock_guard<std::mutex> guard(mutex);

    static bool once = false;
    static std::string file_path;

    if (!once) {
      once = true;
      auto directory = get_user_configuration_directory();
      if (!directory.empty()) {
        file_path = directory + "/karabiner.json";
      }
    }

    return file_path;
  }

  static const std::string& get_user_core_configuration_automatic_backups_directory(void) {
    static std::mutex mutex;
    std::lock_guard<std::mutex> guard(mutex);

    static bool once = false;
    static std::string directory;

    if (!once) {
      once = true;
      auto d = get_user_configuration_directory();
      if (!d.empty()) {
        directory = d + "/automatic_backups";
      }
    }

    return directory;
  }

  static const std::string& get_user_complex_modifications_assets_directory(void) {
    static std::mutex mutex;
    std::lock_guard<std::mutex> guard(mutex);

    static bool once = false;
    static std::string directory;

    if (!once) {
      once = true;
      auto d = get_user_configuration_directory();
      if (!d.empty()) {
        directory = d + "/assets/complex_modifications";
      }
    }

    return directory;
  }

  static const std::string& get_user_log_directory(void) {
    static std::mutex mutex;
    std::lock_guard<std::mutex> guard(mutex);

    static bool once = false;
    static std::string directory;

    if (!once) {
      once = true;
      auto d = get_user_data_directory();
      if (!d.empty()) {
        directory = d + "/log";
      }
    }

    return directory;
  }

  static const std::string& get_user_pid_directory(void) {
    static std::mutex mutex;
    std::lock_guard<std::mutex> guard(mutex);

    static bool once = false;
    static std::string directory;

    if (!once) {
      once = true;
      auto d = get_user_data_directory();
      if (!d.empty()) {
        directory = d + "/pid";
      }
    }

    return directory;
  }

  static const char* get_distributed_notification_observed_object(void) {
    return "org.pqrs.karabiner";
  }

  static const char* get_distributed_notification_console_user_server_is_disabled(void) {
    return "console_user_server_is_disabled";
  }

  static const size_t get_local_datagram_buffer_size(void) {
    return 32 * 1024;
  }

  static pqrs::osx::launchctl::service_name get_grabber_daemon_launchctl_service_name(void) {
    return pqrs::osx::launchctl::service_name("org.pqrs.karabiner.karabiner_grabber");
  }

  static pqrs::osx::launchctl::service_path get_grabber_daemon_launchctl_service_path(void) {
    return pqrs::osx::launchctl::service_path("/Library/LaunchDaemons/org.pqrs.karabiner.karabiner_grabber.plist");
  }

  static pqrs::osx::launchctl::service_name get_observer_agent_launchctl_service_name(void) {
    return pqrs::osx::launchctl::service_name("org.pqrs.karabiner.agent.karabiner_observer");
  }

  static pqrs::osx::launchctl::service_path get_observer_agent_launchctl_service_path(void) {
    return pqrs::osx::launchctl::service_path("/Library/LaunchAgents/org.pqrs.karabiner.agent.karabiner_observer.plist");
  }

  static pqrs::osx::launchctl::service_name get_grabber_agent_launchctl_service_name(void) {
    return pqrs::osx::launchctl::service_name("org.pqrs.karabiner.agent.karabiner_grabber");
  }

  static pqrs::osx::launchctl::service_path get_grabber_agent_launchctl_service_path(void) {
    return pqrs::osx::launchctl::service_path("/Library/LaunchAgents/org.pqrs.karabiner.agent.karabiner_grabber.plist");
  }

  static pqrs::osx::launchctl::service_name get_session_monitor_launchctl_service_name(void) {
    return pqrs::osx::launchctl::service_name("org.pqrs.karabiner.karabiner_session_monitor");
  }

  static pqrs::osx::launchctl::service_path get_session_monitor_launchctl_service_path(void) {
    return pqrs::osx::launchctl::service_path("/Library/LaunchAgents/org.pqrs.karabiner.karabiner_session_monitor.plist");
  }

  static pqrs::osx::launchctl::service_name get_console_user_server_launchctl_service_name(void) {
    return pqrs::osx::launchctl::service_name("org.pqrs.karabiner.karabiner_console_user_server");
  }

  static pqrs::osx::launchctl::service_path get_console_user_server_launchctl_service_path(void) {
    return pqrs::osx::launchctl::service_path("/Library/LaunchAgents/org.pqrs.karabiner.karabiner_console_user_server.plist");
  }

  static pqrs::osx::launchctl::service_name get_notification_window_launchctl_service_name(void) {
    return pqrs::osx::launchctl::service_name("org.pqrs.karabiner.NotificationWindow");
  }

  static pqrs::osx::launchctl::service_path get_notification_window_launchctl_service_path(void) {
    return pqrs::osx::launchctl::service_path("/Library/LaunchAgents/org.pqrs.karabiner.NotificationWindow.plist");
  }
};
} // namespace krbn
<|endoftext|>


<commit_before><commit_msg>modified shession_client to use request_type strings instead of hardcoded strings<commit_after><|endoftext|>


<commit_before>
#include <ugdk/system/engine.h>
#include <ugdk/action/3D/camera.h>
#include <ugdk/action/3D/scene3d.h>
#include <ugdk/action/3D/component/body.h>

#include <shipsbattle/objects/ship.h>
#include <shipsbattle/components/spacedust.h>
#include <shipsbattle/components/playercontroller.h>

#include <shipsbattle/components/hull.h>
#include <shipsbattle/components/subsystems/subhull.h>

#include <btBulletDynamicsCommon.h>
#include <OgreSceneManager.h>
#include <OgreOverlaySystem.h>

#include <memory>
#include <iostream>
#include <sstream>

#define AREA_RANGE 200.0

using std::weak_ptr;
using std::shared_ptr;
using std::unique_ptr;
using std::make_shared;
using std::stringstream;
using std::cout;
using std::endl;
using shipsbattle::objects::Ship;
using shipsbattle::components::SpaceDust;
using shipsbattle::components::PlayerController;

ugdk::action::mode3d::Scene3D *ourscene;

void CreateHUD(Ship& pla, Ship& enemy) {
    Ogre::OverlayManager* overlay_mgr = Ogre::OverlayManager::getSingletonPtr();
    std::string over_name = "SBHUD";
    Ogre::Overlay* hud = overlay_mgr->create(over_name);
    hud->setZOrder(600);

    Ogre::TextAreaOverlayElement* plaHullStats = static_cast<Ogre::TextAreaOverlayElement*>(overlay_mgr->createOverlayElement("TextArea", over_name + "/PlaHullStats"));
    plaHullStats->setMetricsMode(Ogre::GMM_PIXELS);
    plaHullStats->setPosition(10, 0);
    plaHullStats->setDimensions(350, 30);
    plaHullStats->setFontName("testeFont");
    plaHullStats->setCharHeight(16);
    plaHullStats->setColour(Ogre::ColourValue::Black);
    plaHullStats->setCaption("PLAYER HULL STATS");
    Ogre::TextAreaOverlayElement* plaArmorStats = static_cast<Ogre::TextAreaOverlayElement*>(overlay_mgr->createOverlayElement("TextArea", over_name + "/PlaArmorStats"));
    plaArmorStats->setMetricsMode(Ogre::GMM_PIXELS);
    plaArmorStats->setPosition(10, 30);
    plaArmorStats->setDimensions(350, 30);
    plaArmorStats->setFontName("testeFont");
    plaArmorStats->setCharHeight(16);
    plaArmorStats->setColour(Ogre::ColourValue::Blue);
    plaArmorStats->setCaption("PLAYER ARMOR STATS");

    Ogre::TextAreaOverlayElement* enemyHullStats = static_cast<Ogre::TextAreaOverlayElement*>(overlay_mgr->createOverlayElement("TextArea", over_name + "/EnemyHullStats"));
    enemyHullStats->setMetricsMode(Ogre::GMM_PIXELS);
    enemyHullStats->setPosition(10, 60);
    enemyHullStats->setDimensions(350, 30);
    enemyHullStats->setFontName("testeFont");
    enemyHullStats->setCharHeight(16);
    enemyHullStats->setColour(Ogre::ColourValue::Black);
    enemyHullStats->setCaption("ENEMY HULL STATS");
    Ogre::TextAreaOverlayElement* enemyArmorStats = static_cast<Ogre::TextAreaOverlayElement*>(overlay_mgr->createOverlayElement("TextArea", over_name + "/EnemyArmorStats"));
    enemyArmorStats->setMetricsMode(Ogre::GMM_PIXELS);
    enemyArmorStats->setPosition(10, 90);
    enemyArmorStats->setDimensions(350, 30);
    enemyArmorStats->setFontName("testeFont");
    enemyArmorStats->setCharHeight(16);
    enemyArmorStats->setColour(Ogre::ColourValue::Blue);
    enemyArmorStats->setCaption("ENEMY ARMOR STATS");

    Ogre::OverlayContainer* panel = static_cast<Ogre::OverlayContainer*>(overlay_mgr->createOverlayElement("Panel", over_name + "/Panel"));
    panel->setMetricsMode(Ogre::GMM_PIXELS);
    panel->setPosition(450, 0.0);
    panel->setDimensions(350, 120);
    panel->setMaterialName("BaseWhite");
    panel->addChild(plaHullStats);
    panel->addChild(plaArmorStats);
    panel->addChild(enemyHullStats);
    panel->addChild(enemyArmorStats);
    hud->add2D(panel);
    hud->show();

    ourscene->AddTask(ugdk::system::Task(
    [&pla, &enemy, plaHullStats, plaArmorStats, enemyHullStats, enemyArmorStats](double dt) {

        auto func = [](Ship& ship, Ogre::TextAreaOverlayElement* hullStats, Ogre::TextAreaOverlayElement* armorStats) {
            auto hull = ship.hull()->GetSubHull("MainHull");
            double hullPerc = 100.0 * hull->hitpoints() / hull->max_hitpoints();
            double armorPerc = 100.0 * hull->armor_rating() / hull->max_armor_rating();
            stringstream ssh;
            ssh.precision(2);
            ssh.setf(std::ios::fixed, std::ios::floatfield);
            ssh << ship->name() << " HULL: " << hullPerc << "% (" << hull->hitpoints() << "/" << hull->max_hitpoints() << ")";
            hullStats->setCaption(ssh.str());
            stringstream ssa;
            ssa.precision(2);
            ssa.setf(std::ios::fixed, std::ios::floatfield);
            ssa << ship->name() << " ARMOR: " << armorPerc << "% (" << hull->armor_rating() << "/" << hull->max_armor_rating() << ")";
            armorStats->setCaption(ssa.str());
        };

        func(pla, plaHullStats, plaArmorStats);
        func(enemy, enemyHullStats, enemyArmorStats);
    }));
}

Ship createShip(const std::string& name) {
    return Ship(*ourscene, name, "AerOmar.mesh");
}

void AddSubHull(Ship& ship, const std::string& name, double radius, double x, double y, double z) {
    shipsbattle::components::Hull* hull = ship.hull();
    shipsbattle::components::subsystems::SubHull* sh = new shipsbattle::components::subsystems::SubHull(name);
    sh->SetVolume(radius, btVector3(x, y, z));
    hull->AddSubHull(std::shared_ptr<shipsbattle::components::subsystems::SubHull>(sh));
}

int main(int argc, char* argv[]) {
    ugdk::system::Configuration config;
    config.base_path = "assets/";
    config.windows_list.front().title = "Ships Battle";
    config.ogre_plugins.push_back("Plugin_ParticleFX");
    ugdk::system::Initialize(config);

    // create 3D scene
    ourscene = new ugdk::action::mode3d::Scene3D(btVector3(0.0, 0.0, 0.0));
    ourscene->manager()->setAmbientLight(Ogre::ColourValue(.7, .7, .7));
    // set some basic stuff our "space" scenes will need.
    ourscene->ShowFrameStats();
    ourscene->manager()->setSkyBox(true, "Backgrounds/Nebula1");

    {
        // create Player ship
        Ship player = createShip("Player");
        player->AddComponent(make_shared<SpaceDust>());
        player->AddComponent(make_shared<PlayerController>());
        ourscene->camera()->AttachTo(*player);
        ourscene->camera()->SetParameters(Ogre::Vector3::ZERO, 100);
        ourscene->camera()->SetDistance(10);

        //AerOmar bounding box size: (3.07287, 1.06726, 6.79639) - bounding sphere radius: 3.88078
        AddSubHull(player, "BallOrigin", 0.5, 0, 0, 0);
        //AddSubHull(player, "BallUno", 1.0, 0, 0, 1.0);
        //AddSubHull(player, "BallBottom", 0.1, 0, 0, 0.25);
        AddSubHull(player, "BallFront", 0.2, 0, 0, 3.7);

        ourscene->event_handler().AddListener<ugdk::input::KeyPressedEvent>(
        [&player](const ugdk::input::KeyPressedEvent& ev) -> void {
            if (ev.scancode == ugdk::input::Scancode::I) {
                player.hull()->TakeDamage(-100, 0.2, 5.0, btVector3(0, 0, 3.9));
            }
            else if (ev.scancode == ugdk::input::Scancode::O) {
                player.hull()->TakeDamage(100, 0.2, 1.0, btVector3(0, 0, 3.9));
            }
            else if (ev.scancode == ugdk::input::Scancode::P) {
                player.hull()->TakeDamage(100, 0.2, 0.0, btVector3(0, 0, 3.9));
            }
        });

        // create Enemy ship
        Ship enemy = createShip("Enemy");
        enemy.body()->Translate(0, 0, 80);

        CreateHUD(player, enemy);

        // push scene
        ugdk::system::PushScene(unique_ptr<ugdk::action::Scene>(ourscene));
    }

    ugdk::system::Run();
    ugdk::system::Release();
    return 0;
}
<commit_msg>Updated main so that: -HUD checks if ship no longer exists -Fixed problem with scope of Ship variables; -Implemented method to create a sphere mesh with attached material -Added a useless subhull to enemy for damage testing purposes<commit_after>
#include <ugdk/system/engine.h>
#include <ugdk/action/3D/camera.h>
#include <ugdk/action/3D/scene3d.h>
#include <ugdk/action/3D/component/body.h>

#include <shipsbattle/objects/ship.h>
#include <shipsbattle/components/spacedust.h>
#include <shipsbattle/components/playercontroller.h>
#include <shipsbattle/components/subsystems/typedefs.h>
#include <shipsbattle/components/hull.h>
#include <shipsbattle/components/subsystems/subhull.h>

#include <shipsbattle/components/timedlife.h>

#include <btBulletDynamicsCommon.h>
#include <OgreSceneManager.h>
#include <OgreOverlaySystem.h>

#include <OgreMeshManager.h>
#include <OgreHardwareBufferManager.h>
#include <OgreSubMesh.h>

#include <memory>
#include <iostream>
#include <sstream>

#define AREA_RANGE 200.0

using std::weak_ptr;
using std::shared_ptr;
using std::unique_ptr;
using std::make_shared;
using std::stringstream;
using std::cout;
using std::endl;
using shipsbattle::objects::Ship;
using shipsbattle::components::SpaceDust;
using shipsbattle::components::PlayerController;
using namespace shipsbattle::components::subsystems::DecaymentFunctions;

ugdk::action::mode3d::Scene3D *ourscene;

void CreateHUD(Ship& pla, Ship& enemy) {
    Ogre::OverlayManager* overlay_mgr = Ogre::OverlayManager::getSingletonPtr();
    std::string over_name = "SBHUD";
    Ogre::Overlay* hud = overlay_mgr->create(over_name);
    hud->setZOrder(600);

    Ogre::TextAreaOverlayElement* plaHullStats = static_cast<Ogre::TextAreaOverlayElement*>(overlay_mgr->createOverlayElement("TextArea", over_name + "/PlaHullStats"));
    plaHullStats->setMetricsMode(Ogre::GMM_PIXELS);
    plaHullStats->setPosition(10, 0);
    plaHullStats->setDimensions(350, 30);
    plaHullStats->setFontName("testeFont");
    plaHullStats->setCharHeight(16);
    plaHullStats->setColour(Ogre::ColourValue::Black);
    plaHullStats->setCaption("PLAYER HULL STATS");
    Ogre::TextAreaOverlayElement* plaArmorStats = static_cast<Ogre::TextAreaOverlayElement*>(overlay_mgr->createOverlayElement("TextArea", over_name + "/PlaArmorStats"));
    plaArmorStats->setMetricsMode(Ogre::GMM_PIXELS);
    plaArmorStats->setPosition(10, 30);
    plaArmorStats->setDimensions(350, 30);
    plaArmorStats->setFontName("testeFont");
    plaArmorStats->setCharHeight(16);
    plaArmorStats->setColour(Ogre::ColourValue::Blue);
    plaArmorStats->setCaption("PLAYER ARMOR STATS");

    Ogre::TextAreaOverlayElement* enemyHullStats = static_cast<Ogre::TextAreaOverlayElement*>(overlay_mgr->createOverlayElement("TextArea", over_name + "/EnemyHullStats"));
    enemyHullStats->setMetricsMode(Ogre::GMM_PIXELS);
    enemyHullStats->setPosition(10, 60);
    enemyHullStats->setDimensions(350, 30);
    enemyHullStats->setFontName("testeFont");
    enemyHullStats->setCharHeight(16);
    enemyHullStats->setColour(Ogre::ColourValue::Black);
    enemyHullStats->setCaption("ENEMY HULL STATS");
    Ogre::TextAreaOverlayElement* enemyArmorStats = static_cast<Ogre::TextAreaOverlayElement*>(overlay_mgr->createOverlayElement("TextArea", over_name + "/EnemyArmorStats"));
    enemyArmorStats->setMetricsMode(Ogre::GMM_PIXELS);
    enemyArmorStats->setPosition(10, 90);
    enemyArmorStats->setDimensions(350, 30);
    enemyArmorStats->setFontName("testeFont");
    enemyArmorStats->setCharHeight(16);
    enemyArmorStats->setColour(Ogre::ColourValue::Blue);
    enemyArmorStats->setCaption("ENEMY ARMOR STATS");

    Ogre::OverlayContainer* panel = static_cast<Ogre::OverlayContainer*>(overlay_mgr->createOverlayElement("Panel", over_name + "/Panel"));
    panel->setMetricsMode(Ogre::GMM_PIXELS);
    panel->setPosition(450, 0.0);
    panel->setDimensions(350, 120);
    panel->setMaterialName("BaseWhite");
    panel->addChild(plaHullStats);
    panel->addChild(plaArmorStats);
    panel->addChild(enemyHullStats);
    panel->addChild(enemyArmorStats);
    hud->add2D(panel);
    hud->show();

    ourscene->AddTask(ugdk::system::Task(
    [&pla, &enemy, plaHullStats, plaArmorStats, enemyHullStats, enemyArmorStats](double dt) {

        auto func = [](Ship& ship, Ogre::TextAreaOverlayElement* hullStats, Ogre::TextAreaOverlayElement* armorStats) {
            if (ship.valid()) {
                auto hull = ship.hull()->GetSubHull("MainHull");
                double hullPerc = 100.0 * hull->hitpoints() / hull->max_hitpoints();
                double armorPerc = 100.0 * hull->armor_rating() / hull->max_armor_rating();
                stringstream ssh;
                ssh.precision(2);
                ssh.setf(std::ios::fixed, std::ios::floatfield);
                ssh << ship->name() << " HULL: " << hullPerc << "% (" << hull->hitpoints() << "/" << hull->max_hitpoints() << ")";
                hullStats->setCaption(ssh.str());
                stringstream ssa;
                ssa.precision(2);
                ssa.setf(std::ios::fixed, std::ios::floatfield);
                ssa << ship->name() << " ARMOR: " << armorPerc << "% (" << hull->armor_rating() << "/" << hull->max_armor_rating() << ")";
                armorStats->setCaption(ssa.str());
            }
            else {
                hullStats->setCaption("HULL: DESTROYED");
                armorStats->setCaption("ARMOR: DESTROYED");
            }
        };

        func(pla, plaHullStats, plaArmorStats);
        func(enemy, enemyHullStats, enemyArmorStats);
    }));
}

void createSphere(const std::string& name, const float r, const int nRings=16, const int nSegments=16) {
    Ogre::MeshPtr pSphere = Ogre::MeshManager::getSingleton().createManual(Ogre::String(name), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
    Ogre::SubMesh *pSphereVertex = pSphere->createSubMesh();

    pSphere->sharedVertexData = new Ogre::VertexData();
    Ogre::VertexData* vertexData = pSphere->sharedVertexData;

    // define the vertex format
    Ogre::VertexDeclaration* vertexDecl = vertexData->vertexDeclaration;
    size_t currOffset = 0;
    // positions
    vertexDecl->addElement(0, currOffset, Ogre::VET_FLOAT3, Ogre::VES_POSITION);
    currOffset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);
    // normals
    vertexDecl->addElement(0, currOffset, Ogre::VET_FLOAT3, Ogre::VES_NORMAL);
    currOffset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);
    // two dimensional texture coordinates
    vertexDecl->addElement(0, currOffset, Ogre::VET_FLOAT2, Ogre::VES_TEXTURE_COORDINATES, 0);
    currOffset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT2);

    // allocate the vertex buffer
    vertexData->vertexCount = (nRings + 1) * (nSegments + 1);
    Ogre::HardwareVertexBufferSharedPtr vBuf = Ogre::HardwareBufferManager::getSingleton().createVertexBuffer(vertexDecl->getVertexSize(0), vertexData->vertexCount, Ogre::HardwareBuffer::HBU_STATIC_WRITE_ONLY, false);
    Ogre::VertexBufferBinding* binding = vertexData->vertexBufferBinding;
    binding->setBinding(0, vBuf);
    float* pVertex = static_cast<float*>(vBuf->lock(Ogre::HardwareBuffer::HBL_DISCARD));

    // allocate index buffer
    pSphereVertex->indexData->indexCount = 6 * nRings * (nSegments + 1);
    pSphereVertex->indexData->indexBuffer = Ogre::HardwareBufferManager::getSingleton().createIndexBuffer(Ogre::HardwareIndexBuffer::IT_16BIT, pSphereVertex->indexData->indexCount, Ogre::HardwareBuffer::HBU_STATIC_WRITE_ONLY, false);
    Ogre::HardwareIndexBufferSharedPtr iBuf = pSphereVertex->indexData->indexBuffer;
    unsigned short* pIndices = static_cast<unsigned short*>(iBuf->lock(Ogre::HardwareBuffer::HBL_DISCARD));

    float fDeltaRingAngle = (Ogre::Math::PI / nRings);
    float fDeltaSegAngle = (2 * Ogre::Math::PI / nSegments);
    unsigned short wVerticeIndex = 0;

    // Generate the group of rings for the sphere
    for (int ring = 0; ring <= nRings; ring++) {
        float r0 = r * sinf(ring * fDeltaRingAngle);
        float y0 = r * cosf(ring * fDeltaRingAngle);

        // Generate the group of segments for the current ring
        for (int seg = 0; seg <= nSegments; seg++) {
            float x0 = r0 * sinf(seg * fDeltaSegAngle);
            float z0 = r0 * cosf(seg * fDeltaSegAngle);

            // Add one vertex to the strip which makes up the sphere
            *pVertex++ = x0;
            *pVertex++ = y0;
            *pVertex++ = z0;

            Ogre::Vector3 vNormal = Ogre::Vector3(x0, y0, z0).normalisedCopy();
            *pVertex++ = vNormal.x;
            *pVertex++ = vNormal.y;
            *pVertex++ = vNormal.z;

            *pVertex++ = (float)seg / (float)nSegments;
            *pVertex++ = (float)ring / (float)nRings;

            if (ring != nRings) {
                // each vertex (except the last) has six indices pointing to it
                *pIndices++ = wVerticeIndex + nSegments + 1;
                *pIndices++ = wVerticeIndex;
                *pIndices++ = wVerticeIndex + nSegments;
                *pIndices++ = wVerticeIndex + nSegments + 1;
                *pIndices++ = wVerticeIndex + 1;
                *pIndices++ = wVerticeIndex;
                wVerticeIndex++;
            }
        }; // end for seg
    } // end for ring

    // Unlock
    vBuf->unlock();
    iBuf->unlock();
    // Generate face list
    pSphereVertex->useSharedVertices = true;

    // the original code was missing this line:
    pSphere->_setBounds(Ogre::AxisAlignedBox(Ogre::Vector3(-r, -r, -r), Ogre::Vector3(r, r, r)), false);
    pSphere->_setBoundingSphereRadius(r);
    // this line makes clear the mesh is loaded (avoids memory leaks)
    pSphere->load();

    //material
    Ogre::MaterialPtr mMat = Ogre::MaterialManager::getSingleton().create("AmmoSphere", "General", true);
    mMat->getTechnique(0)->getPass(0)->setDiffuse(Ogre::ColourValue(1.0, 0.1, 0));
    mMat->getTechnique(0)->getPass(0)->setEmissive(Ogre::ColourValue(1.0, 0.1, 0));
    pSphereVertex->setMaterialName("AmmoSphere");
}

Ship createShip(const std::string& name) {
    return Ship(*ourscene, name, "AerOmar.mesh");
}

void AddSubHull(Ship& ship, const std::string& name, double radius, double x, double y, double z) {
    shipsbattle::components::Hull* hull = ship.hull();
    shipsbattle::components::subsystems::SubHull* sh = new shipsbattle::components::subsystems::SubHull(name);
    sh->SetVolume(radius, btVector3(x, y, z));
    hull->AddSubHull(std::shared_ptr<shipsbattle::components::subsystems::SubHull>(sh));
}

int main(int argc, char* argv[]) {
    ugdk::system::Configuration config;
    config.base_path = "assets/";
    config.windows_list.front().title = "Ships Battle";
    config.ogre_plugins.push_back("Plugin_ParticleFX");
    ugdk::system::Initialize(config);

    // create 3D scene
    ourscene = new ugdk::action::mode3d::Scene3D(btVector3(0.0, 0.0, 0.0));
    ourscene->manager()->setAmbientLight(Ogre::ColourValue(.7, .7, .7));
    // set some basic stuff our "space" scenes will need.
    ourscene->ShowFrameStats();
    ourscene->manager()->setSkyBox(true, "Backgrounds/Nebula1");

    // prepare our simple test mesh for projectiles
    createSphere("Ammo", 0.1);

    // create Player ship
    Ship player = createShip("Player");
    player->AddComponent(make_shared<SpaceDust>());
    player->AddComponent(make_shared<PlayerController>());
    ourscene->camera()->AttachTo(*player);
    ourscene->camera()->SetParameters(Ogre::Vector3::ZERO, 100);
    ourscene->camera()->SetDistance(10);

    //AerOmar bounding box size: (3.07287, 1.06726, 6.79639) - bounding sphere radius: 3.88078
    AddSubHull(player, "BallOrigin", 0.5, 0, 0, 0);
    //AddSubHull(player, "BallUno", 1.0, 0, 0, 1.0);
    //AddSubHull(player, "BallBottom", 0.1, 0, 0, 0.25);
    AddSubHull(player, "BallFront", 0.2, 0, 0, 3.7);

    ourscene->event_handler().AddListener<ugdk::input::KeyPressedEvent>(
    [&player](const ugdk::input::KeyPressedEvent& ev) -> void {
        if (ev.scancode == ugdk::input::Scancode::I) {
            player.hull()->TakeDamage(-100, 0.2, 5.0, btVector3(0, 0, 3.9), CONSTANT);
        }
        else if (ev.scancode == ugdk::input::Scancode::O) {
            player.hull()->TakeDamage(100, 0.2, 1.0, btVector3(0, 0, 3.9), CONSTANT);
        }
        else if (ev.scancode == ugdk::input::Scancode::P) {
            player.hull()->TakeDamage(100, 0.2, 0.0, btVector3(0, 0, 3.9), CONSTANT);
        }
    });

    // create Enemy ship
    Ship enemy = createShip("Enemy");
    enemy.body()->Translate(0, 0, 80);
    AddSubHull(enemy, "Butt", 0.2, 0.0, 0.0, -3.6);
    player->component<PlayerController>()->set_target( enemy.hull()->GetSubHull("Butt").get() );
    //enemy->AddComponent(std::make_shared<shipsbattle::components::TimedLife>(15.0));

    CreateHUD(player, enemy);

    // push scene
    ugdk::system::PushScene(unique_ptr<ugdk::action::Scene>(ourscene));

    ugdk::system::Run();
    ugdk::system::Release();
    return 0;
}
<|endoftext|>


<commit_before>/*
Flexisip, a flexible SIP proxy server with media capabilities.
Copyright (C) 2012  Belledonne Communications SARL.

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

This program 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 Affero General Public License for more details.

You should have received a copy of the GNU Affero General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "sipattrextractor.hh"
#include <string>
#include <sstream>
#include <sofia-sip/sip.h>
#include <sofia-sip/sip_protos.h>
#include <stdexcept>

using namespace std;

static string subKey(const string &key, size_t *pos) {
	size_t dot=key.find('.', *pos)-*pos;
	if (dot != string::npos) {
		string id=key.substr(*pos, dot);
		//			LOGD("SUBKEY '%s' from '%s'", id.c_str(), key.c_str());
		*pos+=dot+1;
		return id;
	}

	if (key.size() != *pos) {
		//			LOGD("SUBKEY '%s'", key.c_str());
		*pos=key.size();
		return key;
	}
	return "";
}

inline static string cstring_get(const string &key, size_t pos, const char *str) {
	if (!str) throw new invalid_argument("Null string found in sip msg for " + key);
	return std::string(str);
}

inline static string int_get(const string &key, size_t pos, int value) {
	ostringstream oss;
	oss << value;
	return oss.str();
}

static string url_get(const string &key, size_t pos, const url_t *url) {
	string id=subKey(key, &pos);
	if (!url) throw new invalid_argument("No url found in sip msg for " + key);
	if (id == "domain") return cstring_get(key, pos, url->url_host);
	if (id == "user") return cstring_get(key, pos, url->url_user);
	if (id == "params") return cstring_get(key, pos, url->url_params);
	throw new runtime_error("url_get: unhandled arg '" + id + "' in " + key);
}

static string addr_get(const string &key, size_t pos, const sip_addr_s *addr) {
	string id=subKey(key, &pos);
	if (!addr) throw new invalid_argument("No address found in sip msg for " + key);
	if (id == "uri") return url_get(key, pos, addr->a_url);
	throw new runtime_error("addr_get: unhandled arg '" + id + "' in " + key);
}

static string request_get(const string &key, size_t pos, const sip_request_t *req) {
	string id=subKey(key, &pos);
	if (!req) throw new invalid_argument("No request found in sip msg for " + key);
	if (id == "uri") return url_get(key, pos, req->rq_url);
	if (id == "mn" || id == "method-name") return cstring_get(key, pos, req->rq_method_name);
	throw new runtime_error("request_get: unhandled arg '" + id + "' in " + key);
}

static string ua_get(const string &key, size_t pos, const sip_user_agent_t *ua)  {
	if (!ua) throw new invalid_argument("No user-agent found in sip msg for " + key);
	return cstring_get(key, pos, ua->g_string);
}

static string status_get(const string &key, size_t pos, const sip_status_s *status) {
	string id=subKey(key, &pos);
	if (!status) throw new invalid_argument("No status found in sip msg for " + key);
	if (id == "phrase") return cstring_get(key, pos, status->st_phrase);
	if (id == "code") return int_get(key, pos, status->st_status);
	throw new runtime_error("status_get: unhandled arg '" + id + "' in " + key);
}

static string callid_get(const string &key, size_t pos, const sip_call_id_s *callid) {
	string id=subKey(key, &pos);
	if (id.empty()) return cstring_get(key, pos, callid->i_id);
	if (id == "hash") return int_get(key, pos, callid->i_hash);
	throw new runtime_error("callid_get: unhandled arg '" + id + "' in " + key);
}

static bool is_request(sip_t *sip) {
	return sip_is_request((sip_header_t *)sip->sip_request);
}

std::string SipAttributes::get(const std::string &key) const {
		size_t pos=0;
		string id=subKey(key, &pos);

		if (id == "from") return addr_get(key, pos, (sip_addr_s *)sip->sip_from);
		if (id == "to") return addr_get(key, pos, (sip_addr_s *)sip->sip_to);
		if (id == "request") return request_get(key, pos, sip->sip_request);
		if (id == "direction") return is_request(sip) ? "request" : "response";
		if (id == "status") return status_get(key, pos, (sip_status_s *) sip->sip_status);
		if (id == "ua" || id == "user-agent") return ua_get(key, pos, sip->sip_user_agent);
		if (id == "callid" ) return callid_get(key, pos, sip->sip_call_id);

		throw new runtime_error("unhandled arg '" + id + "' in '" + key+ "'");
	}

bool SipAttributes::isTrue(const string &key) const {
		if (key == "is_request") {
			return is_request(sip);
		} else if (key == "is_response") {
			return !is_request(sip);
		}
		throw new runtime_error("unhandled true/false " + key);
};
<commit_msg>Allow empty url params in filters.<commit_after>/*
Flexisip, a flexible SIP proxy server with media capabilities.
Copyright (C) 2012  Belledonne Communications SARL.

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

This program 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 Affero General Public License for more details.

You should have received a copy of the GNU Affero General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "sipattrextractor.hh"
#include <string>
#include <sstream>
#include <sofia-sip/sip.h>
#include <sofia-sip/sip_protos.h>
#include <stdexcept>

using namespace std;

static string subKey(const string &key, size_t *pos) {
	size_t dot=key.find('.', *pos)-*pos;
	if (dot != string::npos) {
		string id=key.substr(*pos, dot);
		//			LOGD("SUBKEY '%s' from '%s'", id.c_str(), key.c_str());
		*pos+=dot+1;
		return id;
	}

	if (key.size() != *pos) {
		//			LOGD("SUBKEY '%s'", key.c_str());
		*pos=key.size();
		return key;
	}
	return "";
}

inline static string cstring_get(const string &key, size_t pos, const char *str) {
	if (!str) throw new invalid_argument("Null string found in sip msg for " + key);
	return str;
}
inline static string cstring_or_empty_get(const string &key, size_t pos, const char *str) {
	return str? str : "";
}

inline static string int_get(const string &key, size_t pos, int value) {
	ostringstream oss;
	oss << value;
	return oss.str();
}

static string url_get(const string &key, size_t pos, const url_t *url) {
	string id=subKey(key, &pos);
	if (!url) throw new invalid_argument("No url found in sip msg for " + key);
	if (id == "domain") return cstring_get(key, pos, url->url_host);
	if (id == "user") return cstring_get(key, pos, url->url_user);
	if (id == "params") return cstring_or_empty_get(key, pos, url->url_params);
	throw new runtime_error("url_get: unhandled arg '" + id + "' in " + key);
}

static string addr_get(const string &key, size_t pos, const sip_addr_s *addr) {
	string id=subKey(key, &pos);
	if (!addr) throw new invalid_argument("No address found in sip msg for " + key);
	if (id == "uri") return url_get(key, pos, addr->a_url);
	throw new runtime_error("addr_get: unhandled arg '" + id + "' in " + key);
}

static string request_get(const string &key, size_t pos, const sip_request_t *req) {
	string id=subKey(key, &pos);
	if (!req) throw new invalid_argument("No request found in sip msg for " + key);
	if (id == "uri") return url_get(key, pos, req->rq_url);
	if (id == "mn" || id == "method-name") return cstring_get(key, pos, req->rq_method_name);
	throw new runtime_error("request_get: unhandled arg '" + id + "' in " + key);
}

static string ua_get(const string &key, size_t pos, const sip_user_agent_t *ua)  {
	if (!ua) throw new invalid_argument("No user-agent found in sip msg for " + key);
	return cstring_get(key, pos, ua->g_string);
}

static string status_get(const string &key, size_t pos, const sip_status_s *status) {
	string id=subKey(key, &pos);
	if (!status) throw new invalid_argument("No status found in sip msg for " + key);
	if (id == "phrase") return cstring_get(key, pos, status->st_phrase);
	if (id == "code") return int_get(key, pos, status->st_status);
	throw new runtime_error("status_get: unhandled arg '" + id + "' in " + key);
}

static string callid_get(const string &key, size_t pos, const sip_call_id_s *callid) {
	string id=subKey(key, &pos);
	if (id.empty()) return cstring_get(key, pos, callid->i_id);
	if (id == "hash") return int_get(key, pos, callid->i_hash);
	throw new runtime_error("callid_get: unhandled arg '" + id + "' in " + key);
}

static bool is_request(sip_t *sip) {
	return sip_is_request((sip_header_t *)sip->sip_request);
}

std::string SipAttributes::get(const std::string &key) const {
		size_t pos=0;
		string id=subKey(key, &pos);

		if (id == "from") return addr_get(key, pos, (sip_addr_s *)sip->sip_from);
		if (id == "to") return addr_get(key, pos, (sip_addr_s *)sip->sip_to);
		if (id == "request") return request_get(key, pos, sip->sip_request);
		if (id == "direction") return is_request(sip) ? "request" : "response";
		if (id == "status") return status_get(key, pos, (sip_status_s *) sip->sip_status);
		if (id == "ua" || id == "user-agent") return ua_get(key, pos, sip->sip_user_agent);
		if (id == "callid" ) return callid_get(key, pos, sip->sip_call_id);

		throw new runtime_error("unhandled arg '" + id + "' in '" + key+ "'");
	}

bool SipAttributes::isTrue(const string &key) const {
		if (key == "is_request") {
			return is_request(sip);
		} else if (key == "is_response") {
			return !is_request(sip);
		}
		throw new runtime_error("unhandled true/false " + key);
};
<|endoftext|>


<commit_before>/*
 * Copyright 2011 Esrille 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 "StackingContext.h"

#include <new>
#include <iostream>

namespace org { namespace w3c { namespace dom { namespace bootstrap {

StackingContext* StackingContext::removeChild(StackingContext* item)
{
    StackingContext* next = item->nextSibling;
    StackingContext* prev = item->previousSibling;
    if (!next)
        lastChild = prev;
    else
        next->previousSibling = prev;
    if (!prev)
        firstChild = next;
    else
        prev->nextSibling = next;
    item->parent = 0;
    --childCount;
    return item;
}

StackingContext* StackingContext::insertBefore(StackingContext* item, StackingContext* after)
{
    if (!after)
        return appendChild(item);
    item->previousSibling = after->previousSibling;
    item->nextSibling = after;
    after->previousSibling = item;
    if (!item->previousSibling)
        firstChild = item;
    else
        item->previousSibling->nextSibling = item;
    item->parent = this;
    ++childCount;
    return item;
}

StackingContext* StackingContext::appendChild(StackingContext* item)
{
    StackingContext* prev = lastChild;
    if (!prev)
        firstChild = item;
    else
        prev->nextSibling = item;
    item->previousSibling = prev;
    item->nextSibling = 0;
    lastChild = item;
    item->parent = this;
    ++childCount;
    return item;
}

StackingContext::StackingContext(int zIndex) :
    zIndex(zIndex),
    z1(0.0f),
    z3(0.0f),
    parent(0),
    firstChild(0),
    lastChild(0),
    previousSibling(0),
    nextSibling(0),
    childCount(0),
    zero(0)
{
}

StackingContext::~StackingContext()
{
    if (parent)
        parent->removeChild(this);
    while (0 < childCount) {
        StackingContext* child = removeChild(firstChild);
        delete child;
    }
}

StackingContext* StackingContext::getAuto()
{
    if (zIndex == 0)
        return this;
    if (!zero)
        zero = addContext(0);
    return zero;
}

StackingContext* StackingContext::addContext(int zIndex)
{
    if (isAuto())
        return parent->addContext(zIndex);

    StackingContext* after = 0;
    for (auto i = getFirstChild(); i; i = i->getNextSibling()) {
        if (zIndex < i->zIndex) {
            after = i;
            break;
        }
    }
    StackingContext* item = new(std::nothrow) StackingContext(zIndex);
    if (item)
        insertBefore(item, after);
    return item;

}

float StackingContext::eval(float z)
{
    z += 1.0f;
    z1 = z3 = z;
    for (auto i = getFirstChild(); i; i = i->getNextSibling()) {
        z = i->eval(z);
        if (i->zIndex < 0) {
            z += 1.0f;
            z3 = z;
        }
    }
    return z;
}

void StackingContext::dump(std::string indent)
{
    std::cout << indent << "z-index: " << zIndex << " " << z1 << " " << z3 << "\n";
    indent += "    ";
    for (auto child = getFirstChild(); child; child = child->getNextSibling())
        child->dump(indent);
}

}}}}  // org::w3c::dom::bootstrap<commit_msg>(StackingContext::getAuto) : Fix a bug.<commit_after>/*
 * Copyright 2011 Esrille 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 "StackingContext.h"

#include <new>
#include <iostream>

namespace org { namespace w3c { namespace dom { namespace bootstrap {

StackingContext* StackingContext::removeChild(StackingContext* item)
{
    StackingContext* next = item->nextSibling;
    StackingContext* prev = item->previousSibling;
    if (!next)
        lastChild = prev;
    else
        next->previousSibling = prev;
    if (!prev)
        firstChild = next;
    else
        prev->nextSibling = next;
    item->parent = 0;
    --childCount;
    return item;
}

StackingContext* StackingContext::insertBefore(StackingContext* item, StackingContext* after)
{
    if (!after)
        return appendChild(item);
    item->previousSibling = after->previousSibling;
    item->nextSibling = after;
    after->previousSibling = item;
    if (!item->previousSibling)
        firstChild = item;
    else
        item->previousSibling->nextSibling = item;
    item->parent = this;
    ++childCount;
    return item;
}

StackingContext* StackingContext::appendChild(StackingContext* item)
{
    StackingContext* prev = lastChild;
    if (!prev)
        firstChild = item;
    else
        prev->nextSibling = item;
    item->previousSibling = prev;
    item->nextSibling = 0;
    lastChild = item;
    item->parent = this;
    ++childCount;
    return item;
}

StackingContext::StackingContext(int zIndex) :
    zIndex(zIndex),
    z1(0.0f),
    z3(0.0f),
    parent(0),
    firstChild(0),
    lastChild(0),
    previousSibling(0),
    nextSibling(0),
    childCount(0),
    zero(0)
{
}

StackingContext::~StackingContext()
{
    if (parent)
        parent->removeChild(this);
    while (0 < childCount) {
        StackingContext* child = removeChild(firstChild);
        delete child;
    }
}

StackingContext* StackingContext::getAuto()
{
    if (isAuto())
        return this;
    if (!zero)
        zero = addContext(0);
    return zero;
}

StackingContext* StackingContext::addContext(int zIndex)
{
    if (isAuto())
        return parent->addContext(zIndex);

    StackingContext* after = 0;
    for (auto i = getFirstChild(); i; i = i->getNextSibling()) {
        if (zIndex < i->zIndex) {
            after = i;
            break;
        }
    }
    StackingContext* item = new(std::nothrow) StackingContext(zIndex);
    if (item)
        insertBefore(item, after);
    return item;

}

float StackingContext::eval(float z)
{
    z += 1.0f;
    z1 = z3 = z;
    for (auto i = getFirstChild(); i; i = i->getNextSibling()) {
        z = i->eval(z);
        if (i->zIndex < 0) {
            z += 1.0f;
            z3 = z;
        }
    }
    return z;
}

void StackingContext::dump(std::string indent)
{
    std::cout << indent << "z-index: " << zIndex << " " << z1 << " " << z3 << "\n";
    indent += "    ";
    for (auto child = getFirstChild(); child; child = child->getNextSibling())
        child->dump(indent);
}

}}}}  // org::w3c::dom::bootstrap<|endoftext|>


<commit_before>#ifndef AMGCL_COARSENING_AS_SCALAR_HPP
#define AMGCL_COARSENING_AS_SCALAR_HPP

/*
The MIT License

Copyright (c) 2012-2022 Denis Demidov <dennis.demidov@gmail.com>

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

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

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

/**
 * \file   amgcl/coarsening/as_scalar.hpp
 * \author Denis Demidov <dennis.demidov@gmail.com>
 * \brief  Scalar coarsening for block matrices.
 */

#include <amgcl/adapter/block_matrix.hpp>
#include <amgcl/backend/interface.hpp>

namespace amgcl {
namespace coarsening {

// Takes a block matrix as input,
// converts it to the scalar format,
// applies the base coarsening,
// converts the results back to block format.
template <class ScalarTemplate, template <class> class Coarsening>
struct as_scalar {
    typedef Coarsening<ScalarTemplate> Base;

    template <class Backend>
    struct type {
        typedef typename Base::params params;
        Base base;

        type(const params &prm = params()) : base(prm) {};

        template <class Matrix>
        std::tuple<
            std::shared_ptr<Matrix>,
            std::shared_ptr<Matrix>
            >
        transfer_operators(const Matrix &B) {
            typedef typename backend::value_type<Matrix>::type Block;
            auto T = base.transfer_operators(*adapter::unblock_matrix(B));

            auto &P = *std::get<0>(T);
            auto &R = *std::get<1>(T);

            backend::sort_rows(P);
            backend::sort_rows(R);

            return std::make_tuple(
                    std::make_shared<Matrix>(adapter::block_matrix<Block>(P)),
                    std::make_shared<Matrix>(adapter::block_matrix<Block>(R))
                    );
        }

        template <class Matrix>
        std::shared_ptr<Matrix>
        coarse_operator(const Matrix &A, const Matrix &P, const Matrix &R) const {
            return base.coarse_operator(A, P, R);
        }
    };
};

} // namespace coarsening
} // namespace amgcl

#endif
<commit_msg>No need to specify scalar backend for coarsening::as_scalar<commit_after>#ifndef AMGCL_COARSENING_AS_SCALAR_HPP
#define AMGCL_COARSENING_AS_SCALAR_HPP

/*
The MIT License

Copyright (c) 2012-2022 Denis Demidov <dennis.demidov@gmail.com>

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

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

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

/**
 * \file   amgcl/coarsening/as_scalar.hpp
 * \author Denis Demidov <dennis.demidov@gmail.com>
 * \brief  Scalar coarsening for block matrices.
 */

#include <amgcl/adapter/block_matrix.hpp>
#include <amgcl/backend/builtin.hpp>
#include <amgcl/value_type/interface.hpp>

namespace amgcl {
namespace coarsening {

// Takes a block matrix as input,
// converts it to the scalar format,
// applies the base coarsening,
// converts the results back to block format.
template <template <class> class Coarsening>
struct as_scalar {
    template <class Backend>
    struct type {
        typedef typename math::scalar_of<typename Backend::value_type>::type Scalar;
        typedef Coarsening< backend::builtin<Scalar> > Base;

        typedef typename Base::params params;
        Base base;

        type(const params &prm = params()) : base(prm) {};

        template <class Matrix>
        std::tuple<
            std::shared_ptr<Matrix>,
            std::shared_ptr<Matrix>
            >
        transfer_operators(const Matrix &B) {
            typedef typename backend::value_type<Matrix>::type Block;
            auto T = base.transfer_operators(*adapter::unblock_matrix(B));

            auto &P = *std::get<0>(T);
            auto &R = *std::get<1>(T);

            backend::sort_rows(P);
            backend::sort_rows(R);

            return std::make_tuple(
                    std::make_shared<Matrix>(adapter::block_matrix<Block>(P)),
                    std::make_shared<Matrix>(adapter::block_matrix<Block>(R))
                    );
        }

        template <class Matrix>
        std::shared_ptr<Matrix>
        coarse_operator(const Matrix &A, const Matrix &P, const Matrix &R) const {
            return base.coarse_operator(A, P, R);
        }
    };
};

} // namespace coarsening
} // namespace amgcl

#endif
<|endoftext|>