<commit_before>///////////////////////////////////////////////////////////////////////////////
// demux is a generic ROS topic demultiplexer: one input topic is fanned out
// to 1 of N output topics. A service is provided to select between the outputs
//
// Copyright (C) 2009, Morgan Quigley
//
// 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 Stanford University 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 THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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.
/////////////////////////////////////////////////////////////////////////////

#include <cstdio>
#include <vector>
#include <list>
#include "ros/console.h"
#include "std_msgs/String.h"
#include "topic_tools/MuxSelect.h"
#include "topic_tools/MuxAdd.h"
#include "topic_tools/MuxList.h"
#include "topic_tools/MuxDelete.h"
#include "topic_tools/shape_shifter.h"
#include "topic_tools/parse.h"

using std::string;
using std::vector;
using std::list;
using namespace topic_tools;

const static string g_none_topic = "__none";

static ros::NodeHandle *g_node = NULL;
static bool g_advertised = false;
static string g_output_topic;
static ros::Publisher g_pub;
static ros::Publisher g_pub_selected;

struct sub_info_t
{
  ros::Subscriber sub;
  ShapeShifter* msg;
};

static list<struct sub_info_t> g_subs;
static list<struct sub_info_t>::iterator g_selected = g_subs.end();

bool sel_srv_cb( topic_tools::MuxSelect::Request  &req,
                 topic_tools::MuxSelect::Response &res )
{
  bool ret = false;
  if (g_selected != g_subs.end())
    res.prev_topic = g_selected->sub.getTopic();
  else
    res.prev_topic = string("");
  // see if it's the magical '__none' topic, in which case we open the circuit
  if (req.topic == g_none_topic)
  {
    ROS_INFO("mux selected to no input.");
    g_selected = g_subs.end();
    ret = true;
  }
  else
  {
    ROS_INFO("trying to switch mux to %s", req.topic.c_str());
    // spin through our vector of inputs and find this guy
    for (list<struct sub_info_t>::iterator it = g_subs.begin();
	 it != g_subs.end();
	 ++it)
    {
      if (ros::names::resolve(it->sub.getTopic()) == ros::names::resolve(req.topic))
      {
	g_selected = it;
	ROS_INFO("mux selected input: [%s]", it->sub.getTopic().c_str());
	ret = true;
      }
    }
  }

  if(ret)
  {
    std_msgs::String t;
    t.data = req.topic;
    g_pub_selected.publish(t);
  }

  return ret;
}

bool sel_srv_cb_dep( topic_tools::MuxSelect::Request  &req,
		     topic_tools::MuxSelect::Response &res )
{
  ROS_WARN("the <topic>_select service is deprecated; use mux/select instead");
  return sel_srv_cb(req,res);
}

void in_cb(const boost::shared_ptr<ShapeShifter const>& msg,
           ShapeShifter* s)
{
  if (!g_advertised)
  {
    ROS_INFO("advertising");
    g_pub = msg->advertise(*g_node, g_output_topic, 10);
    g_advertised = true;
  }
  if (s == g_selected->msg)
    g_pub.publish(msg);
}

bool list_topic_cb(topic_tools::MuxList::Request& req,
	 	   topic_tools::MuxList::Response& res)
{
  for (list<struct sub_info_t>::iterator it = g_subs.begin();
       it != g_subs.end();
       ++it)
  {
    res.topics.push_back(it->sub.getTopic());
  }

  return true;
}

bool add_topic_cb(topic_tools::MuxAdd::Request& req,
		  topic_tools::MuxAdd::Response& res)
{
  // Check that it's not already in our list
  ROS_INFO("trying to add %s to mux", req.topic.c_str());

  // Can't add the __none topic
  if(req.topic == g_none_topic)
  {
    ROS_WARN("failed to add topic %s to mux, because it's reserved for special use",
	     req.topic.c_str());
    return false;
  }

  // spin through our vector of inputs and find this guy
  for (list<struct sub_info_t>::iterator it = g_subs.begin();
       it != g_subs.end();
       ++it)
  {
    if (it->sub.getTopic() == req.topic)
    {
      ROS_WARN("tried to add a topic that mux was already listening to: [%s]",
	       it->sub.getTopic().c_str());
      return false;
    }
  }

  struct sub_info_t sub_info;
  try
  {
    sub_info.sub = g_node->subscribe<ShapeShifter>(req.topic, 10, boost::bind(in_cb, _1, sub_info.msg));
  }
  catch(ros::InvalidNameException& e)
  {
    ROS_WARN("failed to add topic %s to mux, because it's an invalid name: %s",
	     req.topic.c_str(), e.what());
    return false;
  }

  sub_info.msg = new ShapeShifter;
  g_subs.push_back(sub_info);

  ROS_INFO("added %s to mux", req.topic.c_str());

  return true;
}

bool del_topic_cb(topic_tools::MuxDelete::Request& req,
		  topic_tools::MuxDelete::Response& res)
{
  // Check that it's in our list
  ROS_INFO("trying to delete %s from mux", req.topic.c_str());
  // spin through our vector of inputs and find this guy
  for (list<struct sub_info_t>::iterator it = g_subs.begin();
       it != g_subs.end();
       ++it)
  {
    if (it->sub.getTopic() == req.topic)
    {
      it->sub.shutdown();
      delete it->msg;
      g_subs.erase(it);
      ROS_INFO("deleted topic %s from mux", req.topic.c_str());
      return true;
    }
  }

  ROS_WARN("tried to delete non-subscribed topic %s from mux", req.topic.c_str());
  return false;
}

int main(int argc, char **argv)
{
  vector<string> args;
  ros::removeROSArgs(argc, (const char**)argv, args);

  if (args.size() < 3)
  {
    printf("\nusage: mux OUT_TOPIC IN_TOPIC1 [IN_TOPIC2 [...]]\n\n");
    return 1;
  }
  std::string topic_name;
  if(!getBaseName(args[1], topic_name))
    return 1;
  ros::init(argc, argv, topic_name + string("_mux"),
            ros::init_options::AnonymousName);
  vector<string> topics;
  for (unsigned int i = 2; i < args.size(); i++)
    topics.push_back(args[i]);
  ros::NodeHandle n;
  g_node = &n;
  g_output_topic = args[1];
  // Put our API into the "mux" namespace, which the user should usually remap
  ros::NodeHandle mux_nh("mux");
  // Latched publisher for selected input topic name
  g_pub_selected = mux_nh.advertise<std_msgs::String>(string("selected"), 1, true);
  // Backward compatibility
  ros::ServiceServer ss = n.advertiseService(g_output_topic + string("_select"), sel_srv_cb_dep);
  // New service
  ros::ServiceServer ss_select = mux_nh.advertiseService(string("select"), sel_srv_cb);
  ros::ServiceServer ss_add = mux_nh.advertiseService(string("add"), add_topic_cb);
  ros::ServiceServer ss_list = mux_nh.advertiseService(string("list"), list_topic_cb);
  ros::ServiceServer ss_del = mux_nh.advertiseService(string("delete"), del_topic_cb);
  for (size_t i = 0; i < topics.size(); i++)
  {
    struct sub_info_t sub_info;
    sub_info.msg = new ShapeShifter;
    sub_info.sub = n.subscribe<ShapeShifter>(topics[i], 10, boost::bind(in_cb, _1, sub_info.msg));
    g_subs.push_back(sub_info);
  }
  g_selected = g_subs.begin(); // select first topic to start
  std_msgs::String t;
  t.data = g_selected->sub.getTopic();
  g_pub_selected.publish(t);
  ros::spin();
  for (list<struct sub_info_t>::iterator it = g_subs.begin();
       it != g_subs.end();
       ++it)
  {
    it->sub.shutdown();
    delete it->msg;
  }

  g_subs.clear();
  return 0;
}

<commit_msg>applied patch for topic name resolution from #2863<commit_after>///////////////////////////////////////////////////////////////////////////////
// demux is a generic ROS topic demultiplexer: one input topic is fanned out
// to 1 of N output topics. A service is provided to select between the outputs
//
// Copyright (C) 2009, Morgan Quigley
//
// 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 Stanford University 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 THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 THE COPYRIGHT OWNER OR CONTRIBUTORS 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.
/////////////////////////////////////////////////////////////////////////////

#include <cstdio>
#include <vector>
#include <list>
#include "ros/console.h"
#include "std_msgs/String.h"
#include "topic_tools/MuxSelect.h"
#include "topic_tools/MuxAdd.h"
#include "topic_tools/MuxList.h"
#include "topic_tools/MuxDelete.h"
#include "topic_tools/shape_shifter.h"
#include "topic_tools/parse.h"

using std::string;
using std::vector;
using std::list;
using namespace topic_tools;

const static string g_none_topic = "__none";

static ros::NodeHandle *g_node = NULL;
static bool g_advertised = false;
static string g_output_topic;
static ros::Publisher g_pub;
static ros::Publisher g_pub_selected;

struct sub_info_t
{
  ros::Subscriber sub;
  ShapeShifter* msg;
};

static list<struct sub_info_t> g_subs;
static list<struct sub_info_t>::iterator g_selected = g_subs.end();

bool sel_srv_cb( topic_tools::MuxSelect::Request  &req,
                 topic_tools::MuxSelect::Response &res )
{
  bool ret = false;
  if (g_selected != g_subs.end())
    res.prev_topic = g_selected->sub.getTopic();
  else
    res.prev_topic = string("");
  // see if it's the magical '__none' topic, in which case we open the circuit
  if (req.topic == g_none_topic)
  {
    ROS_INFO("mux selected to no input.");
    g_selected = g_subs.end();
    ret = true;
  }
  else
  {
    ROS_INFO("trying to switch mux to %s", req.topic.c_str());
    // spin through our vector of inputs and find this guy
    for (list<struct sub_info_t>::iterator it = g_subs.begin();
	 it != g_subs.end();
	 ++it)
    {
      if (ros::names::resolve(it->sub.getTopic()) == ros::names::resolve(req.topic))
      {
	g_selected = it;
	ROS_INFO("mux selected input: [%s]", it->sub.getTopic().c_str());
	ret = true;
      }
    }
  }

  if(ret)
  {
    std_msgs::String t;
    t.data = req.topic;
    g_pub_selected.publish(t);
  }

  return ret;
}

bool sel_srv_cb_dep( topic_tools::MuxSelect::Request  &req,
		     topic_tools::MuxSelect::Response &res )
{
  ROS_WARN("the <topic>_select service is deprecated; use mux/select instead");
  return sel_srv_cb(req,res);
}

void in_cb(const boost::shared_ptr<ShapeShifter const>& msg,
           ShapeShifter* s)
{
  if (!g_advertised)
  {
    ROS_INFO("advertising");
    g_pub = msg->advertise(*g_node, g_output_topic, 10);
    g_advertised = true;
  }
  if (s == g_selected->msg)
    g_pub.publish(msg);
}

bool list_topic_cb(topic_tools::MuxList::Request& req,
	 	   topic_tools::MuxList::Response& res)
{
  for (list<struct sub_info_t>::iterator it = g_subs.begin();
       it != g_subs.end();
       ++it)
  {
    res.topics.push_back(it->sub.getTopic());
  }

  return true;
}

bool add_topic_cb(topic_tools::MuxAdd::Request& req,
		  topic_tools::MuxAdd::Response& res)
{
  // Check that it's not already in our list
  ROS_INFO("trying to add %s to mux", req.topic.c_str());

  // Can't add the __none topic
  if(req.topic == g_none_topic)
  {
    ROS_WARN("failed to add topic %s to mux, because it's reserved for special use",
	     req.topic.c_str());
    return false;
  }

  // spin through our vector of inputs and find this guy
  for (list<struct sub_info_t>::iterator it = g_subs.begin();
       it != g_subs.end();
       ++it)
  {
    if (ros::names::resolve(it->sub.getTopic()) == ros::names::resolve(req.topic))
    {
      ROS_WARN("tried to add a topic that mux was already listening to: [%s]",
	       it->sub.getTopic().c_str());
      return false;
    }
  }

  struct sub_info_t sub_info;
  try
  {
    sub_info.sub = g_node->subscribe<ShapeShifter>(req.topic, 10, boost::bind(in_cb, _1, sub_info.msg));
  }
  catch(ros::InvalidNameException& e)
  {
    ROS_WARN("failed to add topic %s to mux, because it's an invalid name: %s",
	     req.topic.c_str(), e.what());
    return false;
  }

  sub_info.msg = new ShapeShifter;
  g_subs.push_back(sub_info);

  ROS_INFO("added %s to mux", req.topic.c_str());

  return true;
}

bool del_topic_cb(topic_tools::MuxDelete::Request& req,
		  topic_tools::MuxDelete::Response& res)
{
  // Check that it's in our list
  ROS_INFO("trying to delete %s from mux", req.topic.c_str());
  // spin through our vector of inputs and find this guy
  for (list<struct sub_info_t>::iterator it = g_subs.begin();
       it != g_subs.end();
       ++it)
  {
    if (ros::names::resolve(it->sub.getTopic()) == ros::names::resolve(req.topic))
    {
      it->sub.shutdown();
      delete it->msg;
      g_subs.erase(it);
      ROS_INFO("deleted topic %s from mux", req.topic.c_str());
      return true;
    }
  }

  ROS_WARN("tried to delete non-subscribed topic %s from mux", req.topic.c_str());
  return false;
}

int main(int argc, char **argv)
{
  vector<string> args;
  ros::removeROSArgs(argc, (const char**)argv, args);

  if (args.size() < 3)
  {
    printf("\nusage: mux OUT_TOPIC IN_TOPIC1 [IN_TOPIC2 [...]]\n\n");
    return 1;
  }
  std::string topic_name;
  if(!getBaseName(args[1], topic_name))
    return 1;
  ros::init(argc, argv, topic_name + string("_mux"),
            ros::init_options::AnonymousName);
  vector<string> topics;
  for (unsigned int i = 2; i < args.size(); i++)
    topics.push_back(args[i]);
  ros::NodeHandle n;
  g_node = &n;
  g_output_topic = args[1];
  // Put our API into the "mux" namespace, which the user should usually remap
  ros::NodeHandle mux_nh("mux");
  // Latched publisher for selected input topic name
  g_pub_selected = mux_nh.advertise<std_msgs::String>(string("selected"), 1, true);
  // Backward compatibility
  ros::ServiceServer ss = n.advertiseService(g_output_topic + string("_select"), sel_srv_cb_dep);
  // New service
  ros::ServiceServer ss_select = mux_nh.advertiseService(string("select"), sel_srv_cb);
  ros::ServiceServer ss_add = mux_nh.advertiseService(string("add"), add_topic_cb);
  ros::ServiceServer ss_list = mux_nh.advertiseService(string("list"), list_topic_cb);
  ros::ServiceServer ss_del = mux_nh.advertiseService(string("delete"), del_topic_cb);
  for (size_t i = 0; i < topics.size(); i++)
  {
    struct sub_info_t sub_info;
    sub_info.msg = new ShapeShifter;
    sub_info.sub = n.subscribe<ShapeShifter>(topics[i], 10, boost::bind(in_cb, _1, sub_info.msg));
    g_subs.push_back(sub_info);
  }
  g_selected = g_subs.begin(); // select first topic to start
  std_msgs::String t;
  t.data = g_selected->sub.getTopic();
  g_pub_selected.publish(t);
  ros::spin();
  for (list<struct sub_info_t>::iterator it = g_subs.begin();
       it != g_subs.end();
       ++it)
  {
    it->sub.shutdown();
    delete it->msg;
  }

  g_subs.clear();
  return 0;
}

<|endoftext|>


<commit_before>#include "getopt.h"

/*
* Copyright (c) 1987, 1993, 1994
*      The Regents of the University of California.  All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
* 1. Redistributions of source code must retain the above copyright
*    notice, this list of conditions and the following disclaimer.
* 2. 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.
* 3. All advertising materials mentioning features or use of this software
*    must display the following acknowledgement:
*      This product includes software developed by the University of
*      California, Berkeley and its contributors.
* 4. Neither the name of the University 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 THE REGENTS AND CONTRIBUTORS ``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 THE REGENTS OR CONTRIBUTORS 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.
*/

#include <string.h>
#include <stdio.h>

int     opterr = 1,             /* if error message should be printed */
        optind = 1,             /* index into parent argv vector */
        optopt,                 /* character checked for validity */
        optreset;               /* reset getopt */
char    *optarg;                /* argument associated with option */

#define BADCH   (int)'?'
#define BADARG  (int)':'
#define EMSG    ""

/*
 * getopt --
 *      Parse argc/argv argument vector.
 */
int getopt(int nargc, char * const nargv[], const char *ostr)
{
  static char *place = EMSG;              /* option letter processing */
  const char *oli;                        /* option letter list index */

  if (optreset || !*place) {              /* update scanning pointer */
    optreset = 0;
    if (optind >= nargc || *(place = nargv[optind]) != '-') {
      place = EMSG;
      return (-1);
    }
    if (place[1] && *++place == '-') {    /* found "--" */
      ++optind;
      place = EMSG;
      return (-1);
    }
  }                                       /* option letter okay? */
  if ((optopt = (int)*place++) == (int)':' ||
    !(oli = strchr(ostr, optopt))) {
      /*
      * if the user didn't specify '-' as an option,
      * assume it means -1.
      */
      if (optopt == (int)'-')
        return (-1);
      if (!*place)
        ++optind;
      if (opterr && *ostr != ':')
        (void)printf("illegal option -- %c\n", optopt);
      return (BADCH);
  }
  if (*++oli != ':') {                    /* don't need argument */
    optarg = NULL;
    if (!*place)
      ++optind;
  }
  else {                                  /* need an argument */
    if (*place)                           /* no white space */
      optarg = place;
    else if (nargc <= ++optind) {         /* no arg */
      place = EMSG;
      if (*ostr == ':')
        return (BADARG);
      if (opterr)
        (void)printf("option requires an argument -- %c\n", optopt);
      return (BADCH);
    }
    else                                  /* white space */
      optarg = nargv[optind];
    place = EMSG;
    ++optind;
  }
  return (optopt);                        /* dump back option letter */
}<commit_msg>add blank line<commit_after>#include "getopt.h"

/*
* Copyright (c) 1987, 1993, 1994
*      The Regents of the University of California.  All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
* 1. Redistributions of source code must retain the above copyright
*    notice, this list of conditions and the following disclaimer.
* 2. 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.
* 3. All advertising materials mentioning features or use of this software
*    must display the following acknowledgement:
*      This product includes software developed by the University of
*      California, Berkeley and its contributors.
* 4. Neither the name of the University 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 THE REGENTS AND CONTRIBUTORS ``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 THE REGENTS OR CONTRIBUTORS 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.
*/

#include <string.h>
#include <stdio.h>

int     opterr = 1,             /* if error message should be printed */
        optind = 1,             /* index into parent argv vector */
        optopt,                 /* character checked for validity */
        optreset;               /* reset getopt */
char    *optarg;                /* argument associated with option */

#define BADCH   (int)'?'
#define BADARG  (int)':'
#define EMSG    ""

/*
 * getopt --
 *      Parse argc/argv argument vector.
 */
int getopt(int nargc, char * const nargv[], const char *ostr)
{
  static char *place = EMSG;              /* option letter processing */
  const char *oli;                        /* option letter list index */

  if (optreset || !*place) {              /* update scanning pointer */
    optreset = 0;
    if (optind >= nargc || *(place = nargv[optind]) != '-') {
      place = EMSG;
      return (-1);
    }
    if (place[1] && *++place == '-') {    /* found "--" */
      ++optind;
      place = EMSG;
      return (-1);
    }
  }                                       /* option letter okay? */
  if ((optopt = (int)*place++) == (int)':' ||
    !(oli = strchr(ostr, optopt))) {
      /*
      * if the user didn't specify '-' as an option,
      * assume it means -1.
      */
      if (optopt == (int)'-')
        return (-1);
      if (!*place)
        ++optind;
      if (opterr && *ostr != ':')
        (void)printf("illegal option -- %c\n", optopt);
      return (BADCH);
  }
  if (*++oli != ':') {                    /* don't need argument */
    optarg = NULL;
    if (!*place)
      ++optind;
  }
  else {                                  /* need an argument */
    if (*place)                           /* no white space */
      optarg = place;
    else if (nargc <= ++optind) {         /* no arg */
      place = EMSG;
      if (*ostr == ':')
        return (BADARG);
      if (opterr)
        (void)printf("option requires an argument -- %c\n", optopt);
      return (BADCH);
    }
    else                                  /* white space */
      optarg = nargv[optind];
    place = EMSG;
    ++optind;
  }
  return (optopt);                        /* dump back option letter */
}
<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. Neither the name PX4 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 THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "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 THE
 * COPYRIGHT OWNER OR CONTRIBUTORS 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.
 *
 ****************************************************************************/

/**
 * @file battery.cpp
 *
 * Library calls for battery functionality.
 *
 * @author Julian Oes <julian@oes.ch>
 */

#include "battery.h"
#include <mathlib/mathlib.h>

Battery::Battery() :
	SuperBlock(nullptr, "BAT"),
	_param_v_empty(this, "V_EMPTY"),
	_param_v_full(this, "V_CHARGED"),
	_param_n_cells(this, "N_CELLS"),
	_param_capacity(this, "CAPACITY"),
	_param_v_load_drop(this, "V_LOAD_DROP"),
	_param_r_internal(this, "R_INTERNAL"),
	_param_low_thr(this, "LOW_THR"),
	_param_crit_thr(this, "CRIT_THR"),
	_param_emergency_thr(this, "EMERGEN_THR"),
	_voltage_filtered_v(-1.0f),
	_current_filtered_a(-1.0f),
	_discharged_mah(0.0f),
	_remaining_voltage(1.0f),
	_remaining_capacity(1.0f),
	_remaining(1.0f),
	_scale(1.0f),
	_warning(battery_status_s::BATTERY_WARNING_NONE),
	_last_timestamp(0)
{
	/* load initial params */
	updateParams();
}

Battery::~Battery()
{
}

void
Battery::reset(battery_status_s *battery_status)
{
	memset(battery_status, 0, sizeof(*battery_status));
	battery_status->current_a = -1.0f;
	battery_status->remaining = 1.0f;
	battery_status->scale = 1.0f;
	battery_status->cell_count = _param_n_cells.get();
	// TODO: check if it is sane to reset warning to NONE
	battery_status->warning = battery_status_s::BATTERY_WARNING_NONE;
	battery_status->connected = false;
}

void
Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a,
			     bool connected, bool selected_source, int priority,
			     float throttle_normalized,
			     bool armed, battery_status_s *battery_status)
{
	reset(battery_status);
	battery_status->timestamp = timestamp;
	filterVoltage(voltage_v);
	filterCurrent(current_a);
	sumDischarged(timestamp, current_a);
	estimateRemaining(voltage_v, current_a, throttle_normalized, armed);
	determineWarning(connected);
	computeScale();

	if (_voltage_filtered_v > 2.1f) {
		battery_status->voltage_v = voltage_v;
		battery_status->voltage_filtered_v = _voltage_filtered_v;
		battery_status->scale = _scale;
		battery_status->current_a = current_a;
		battery_status->current_filtered_a = _current_filtered_a;
		battery_status->discharged_mah = _discharged_mah;
		battery_status->warning = _warning;
		battery_status->remaining = _remaining;
		battery_status->connected = connected;
		battery_status->system_source = selected_source;
		battery_status->priority = priority;
	}
}

void
Battery::filterVoltage(float voltage_v)
{
	if (_voltage_filtered_v < 0.0f) {
		_voltage_filtered_v = voltage_v;
	}

	// TODO: inspect that filter performance
	const float filtered_next = _voltage_filtered_v * 0.99f + voltage_v * 0.01f;

	if (PX4_ISFINITE(filtered_next)) {
		_voltage_filtered_v = filtered_next;
	}
}

void
Battery::filterCurrent(float current_a)
{
	if (_current_filtered_a < 0.0f) {
		_current_filtered_a = current_a;
	}

	// ADC poll is at 100Hz, this will perform a low pass over approx 500ms
	const float filtered_next = _current_filtered_a * 0.98f + current_a * 0.02f;

	if (PX4_ISFINITE(filtered_next)) {
		_current_filtered_a = filtered_next;
	}
}

void
Battery::sumDischarged(hrt_abstime timestamp, float current_a)
{
	// Not a valid measurement
	if (current_a < 0.0f) {
		// Because the measurement was invalid we need to stop integration
		// and re-initialize with the next valid measurement
		_last_timestamp = 0;
		return;
	}

	// Ignore first update because we don't know dT.
	if (_last_timestamp != 0) {
		_discharged_mah += current_a * ((float)(timestamp - _last_timestamp)) / 1e3f / 3600.0f;
	}

	_last_timestamp = timestamp;
}

void
Battery::estimateRemaining(float voltage_v, float current_a, float throttle_normalized, bool armed)
{
	// correct battery voltage locally for load drop to avoid estimation fluctuations
	const float bat_r = _param_r_internal.get();

	if (bat_r >= 0.0f) {
		voltage_v += bat_r * current_a;

	} else {
		// assume quadratic relation between throttle and current
		// good assumption if throttle represents RPM
		voltage_v += throttle_normalized * throttle_normalized * _param_v_load_drop.get();
	}

	// remaining battery capacity based on voltage
	const float cell_voltage = voltage_v / _param_n_cells.get();
	const float rvoltage = math::gradual(cell_voltage, _param_v_empty.get(), _param_v_full.get(), 0.f, 1.f);

	const float rvoltage_filt = _remaining_voltage * 0.99f + rvoltage * 0.01f;

	if (PX4_ISFINITE(rvoltage_filt)) {
		_remaining_voltage = rvoltage_filt;
	}

	// remaining battery capacity based on used current integrated time
	const float rcap = 1.0f - _discharged_mah / _param_capacity.get();
	const float rcap_filt = _remaining_capacity * 0.99f + rcap * 0.01f;

	if (PX4_ISFINITE(rcap_filt)) {
		_remaining_capacity = rcap_filt;
	}

	// limit to sane values
	_remaining_voltage = (_remaining_voltage < 0.0f) ? 0.0f : _remaining_voltage;
	_remaining_voltage = (_remaining_voltage > 1.0f) ? 1.0f : _remaining_voltage;

	_remaining_capacity = (_remaining_capacity < 0.0f) ? 0.0f : _remaining_capacity;
	_remaining_capacity = (_remaining_capacity > 1.0f) ? 1.0f : _remaining_capacity;

	// choose which quantity we're using for final reporting
	if (_param_capacity.get() > 0.0f) {
		// if battery capacity is known, use discharged current for estimate,
		// but don't show more than voltage estimate
		_remaining = fminf(_remaining_voltage, _remaining_capacity);

	} else {
		// else use voltage
		_remaining = _remaining_voltage;
	}
}

void
Battery::determineWarning(bool connected)
{
	if (connected) {
		// propagate warning state only if the state is higher, otherwise remain in current warning state
		if (_remaining < _param_emergency_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_EMERGENCY)) {
			_warning = battery_status_s::BATTERY_WARNING_EMERGENCY;

		} else if (_remaining < _param_crit_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
			_warning = battery_status_s::BATTERY_WARNING_CRITICAL;

		} else if (_remaining < _param_low_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_LOW)) {
			_warning = battery_status_s::BATTERY_WARNING_LOW;
		}
	}
}

void
Battery::computeScale()
{
	const float voltage_range = (_param_v_full.get() - _param_v_empty.get());

	// reusing capacity calculation to get single cell voltage before drop
	const float bat_v = _param_v_empty.get() + (voltage_range * _remaining_voltage);

	_scale = _param_v_full.get() / bat_v;

	if (_scale > 1.3f) { // Allow at most 30% compensation
		_scale = 1.3f;

	} else if (!PX4_ISFINITE(_scale) || _scale < 1.0f) { // Shouldn't ever be more than the power at full battery
		_scale = 1.0f;
	}
}
<commit_msg>Battery: get rid of the duplicate filtering, just use the already existing methods<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. Neither the name PX4 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 THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "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 THE
 * COPYRIGHT OWNER OR CONTRIBUTORS 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.
 *
 ****************************************************************************/

/**
 * @file battery.cpp
 *
 * Library calls for battery functionality.
 *
 * @author Julian Oes <julian@oes.ch>
 */

#include "battery.h"
#include <mathlib/mathlib.h>

Battery::Battery() :
	SuperBlock(nullptr, "BAT"),
	_param_v_empty(this, "V_EMPTY"),
	_param_v_full(this, "V_CHARGED"),
	_param_n_cells(this, "N_CELLS"),
	_param_capacity(this, "CAPACITY"),
	_param_v_load_drop(this, "V_LOAD_DROP"),
	_param_r_internal(this, "R_INTERNAL"),
	_param_low_thr(this, "LOW_THR"),
	_param_crit_thr(this, "CRIT_THR"),
	_param_emergency_thr(this, "EMERGEN_THR"),
	_voltage_filtered_v(-1.0f),
	_current_filtered_a(-1.0f),
	_discharged_mah(0.0f),
	_remaining_voltage(1.0f),
	_remaining_capacity(1.0f),
	_remaining(1.0f),
	_scale(1.0f),
	_warning(battery_status_s::BATTERY_WARNING_NONE),
	_last_timestamp(0)
{
	/* load initial params */
	updateParams();
}

Battery::~Battery()
{
}

void
Battery::reset(battery_status_s *battery_status)
{
	memset(battery_status, 0, sizeof(*battery_status));
	battery_status->current_a = -1.0f;
	battery_status->remaining = 1.0f;
	battery_status->scale = 1.0f;
	battery_status->cell_count = _param_n_cells.get();
	// TODO: check if it is sane to reset warning to NONE
	battery_status->warning = battery_status_s::BATTERY_WARNING_NONE;
	battery_status->connected = false;
}

void
Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a,
			     bool connected, bool selected_source, int priority,
			     float throttle_normalized,
			     bool armed, battery_status_s *battery_status)
{
	reset(battery_status);
	battery_status->timestamp = timestamp;
	filterVoltage(voltage_v);
	filterCurrent(current_a);
	sumDischarged(timestamp, current_a);
	estimateRemaining(_voltage_filtered_v, _current_filtered_a, throttle_normalized, armed);
	determineWarning(connected);
	computeScale();

	if (_voltage_filtered_v > 2.1f) {
		battery_status->voltage_v = voltage_v;
		battery_status->voltage_filtered_v = _voltage_filtered_v;
		battery_status->scale = _scale;
		battery_status->current_a = current_a;
		battery_status->current_filtered_a = _current_filtered_a;
		battery_status->discharged_mah = _discharged_mah;
		battery_status->warning = _warning;
		battery_status->remaining = _remaining;
		battery_status->connected = connected;
		battery_status->system_source = selected_source;
		battery_status->priority = priority;
	}
}

void
Battery::filterVoltage(float voltage_v)
{
	if (_voltage_filtered_v < 0.0f) {
		_voltage_filtered_v = voltage_v;
	}

	// TODO: inspect that filter performance
	const float filtered_next = _voltage_filtered_v * 0.99f + voltage_v * 0.01f;

	if (PX4_ISFINITE(filtered_next)) {
		_voltage_filtered_v = filtered_next;
	}
}

void
Battery::filterCurrent(float current_a)
{
	if (_current_filtered_a < 0.0f) {
		_current_filtered_a = current_a;
	}

	// ADC poll is at 100Hz, this will perform a low pass over approx 500ms
	const float filtered_next = _current_filtered_a * 0.98f + current_a * 0.02f;

	if (PX4_ISFINITE(filtered_next)) {
		_current_filtered_a = filtered_next;
	}
}

void
Battery::sumDischarged(hrt_abstime timestamp, float current_a)
{
	// Not a valid measurement
	if (current_a < 0.0f) {
		// Because the measurement was invalid we need to stop integration
		// and re-initialize with the next valid measurement
		_last_timestamp = 0;
		return;
	}

	// Ignore first update because we don't know dT.
	if (_last_timestamp != 0) {
		_discharged_mah += current_a * ((float)(timestamp - _last_timestamp)) / 1e3f / 3600.0f;
	}

	_last_timestamp = timestamp;
}

void
Battery::estimateRemaining(float voltage_v, float current_a, float throttle_normalized, bool armed)
{
	// correct battery voltage locally for load drop to avoid estimation fluctuations
	const float bat_r = _param_r_internal.get();

	if (bat_r >= 0.0f) {
		voltage_v += bat_r * current_a;

	} else {
		// assume quadratic relation between throttle and current
		// good assumption if throttle represents RPM
		voltage_v += throttle_normalized * throttle_normalized * _param_v_load_drop.get();
	}

	// remaining battery capacity based on voltage
	const float cell_voltage = voltage_v / _param_n_cells.get();
	_remaining_voltage = math::gradual(cell_voltage, _param_v_empty.get(), _param_v_full.get(), 0.f, 1.f);

	// remaining battery capacity based on used current integrated time
	_remaining_capacity = 1.0f - _discharged_mah / _param_capacity.get();

	// limit to sane values
	_remaining_voltage = (_remaining_voltage < 0.0f) ? 0.0f : _remaining_voltage;
	_remaining_voltage = (_remaining_voltage > 1.0f) ? 1.0f : _remaining_voltage;

	_remaining_capacity = (_remaining_capacity < 0.0f) ? 0.0f : _remaining_capacity;
	_remaining_capacity = (_remaining_capacity > 1.0f) ? 1.0f : _remaining_capacity;

	// choose which quantity we're using for final reporting
	if (_param_capacity.get() > 0.0f) {
		// if battery capacity is known, use discharged current for estimate,
		// but don't show more than voltage estimate
		_remaining = fminf(_remaining_voltage, _remaining_capacity);

	} else {
		// else use voltage
		_remaining = _remaining_voltage;
	}
}

void
Battery::determineWarning(bool connected)
{
	if (connected) {
		// propagate warning state only if the state is higher, otherwise remain in current warning state
		if (_remaining < _param_emergency_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_EMERGENCY)) {
			_warning = battery_status_s::BATTERY_WARNING_EMERGENCY;

		} else if (_remaining < _param_crit_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
			_warning = battery_status_s::BATTERY_WARNING_CRITICAL;

		} else if (_remaining < _param_low_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_LOW)) {
			_warning = battery_status_s::BATTERY_WARNING_LOW;
		}
	}
}

void
Battery::computeScale()
{
	const float voltage_range = (_param_v_full.get() - _param_v_empty.get());

	// reusing capacity calculation to get single cell voltage before drop
	const float bat_v = _param_v_empty.get() + (voltage_range * _remaining_voltage);

	_scale = _param_v_full.get() / bat_v;

	if (_scale > 1.3f) { // Allow at most 30% compensation
		_scale = 1.3f;

	} else if (!PX4_ISFINITE(_scale) || _scale < 1.0f) { // Shouldn't ever be more than the power at full battery
		_scale = 1.0f;
	}
}
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2015 Cloudius Systems, Ltd.
 */

#pragma once

#include "bytes.hh"
#include "timestamp.hh"
#include "tombstone.hh"
#include "gc_clock.hh"
#include "net/byteorder.hh"
#include <cstdint>
#include <iostream>

template<typename T>
static inline
void set_field(bytes& v, unsigned offset, T val) {
    reinterpret_cast<net::packed<T>*>(v.begin() + offset)->raw = net::hton(val);
}

template<typename T>
static inline
T get_field(const bytes_view& v, unsigned offset) {
    return net::ntoh(*reinterpret_cast<const net::packed<T>*>(v.begin() + offset));
}

class atomic_cell_or_collection;

/*
 * Represents atomic cell layout. Works on serialized form.
 *
 * Layout:
 *
 *  <live>  := <int8_t:flags><int64_t:timestamp>(<int32_t:expiry><int32_t:ttl>)?<value>
 *  <dead>  := <int8_t:    0><int64_t:timestamp><int32_t:deletion_time>
 */
class atomic_cell_type final {
private:
    static constexpr int8_t DEAD_FLAGS = 0;
    static constexpr int8_t LIVE_FLAG = 0x01;
    static constexpr int8_t EXPIRY_FLAG = 0x02; // When present, expiry field is present. Set only for live cells
    static constexpr unsigned flags_size = 1;
    static constexpr unsigned timestamp_offset = flags_size;
    static constexpr unsigned timestamp_size = 8;
    static constexpr unsigned expiry_offset = timestamp_offset + timestamp_size;
    static constexpr unsigned expiry_size = 4;
    static constexpr unsigned deletion_time_offset = timestamp_offset + timestamp_size;
    static constexpr unsigned deletion_time_size = 4;
    static constexpr unsigned ttl_offset = expiry_offset + expiry_size;
    static constexpr unsigned ttl_size = 4;
private:
    static bool is_live(const bytes_view& cell) {
        return cell[0] != DEAD_FLAGS;
    }
    static bool is_live_and_has_ttl(const bytes_view& cell) {
        return cell[0] & EXPIRY_FLAG;
    }
    static bool is_dead(const bytes_view& cell) {
        return cell[0] == DEAD_FLAGS;
    }
    // Can be called on live and dead cells
    static api::timestamp_type timestamp(const bytes_view& cell) {
        return get_field<api::timestamp_type>(cell, timestamp_offset);
    }
    // Can be called on live cells only
    static bytes_view value(bytes_view cell) {
        auto expiry_field_size = bool(cell[0] & EXPIRY_FLAG) * (expiry_size + ttl_size);
        auto value_offset = flags_size + timestamp_size + expiry_field_size;
        cell.remove_prefix(value_offset);
        return cell;
    }
    // Can be called only when is_dead() is true.
    static gc_clock::time_point deletion_time(const bytes_view& cell) {
        assert(is_dead(cell));
        return gc_clock::time_point(gc_clock::duration(
            get_field<int32_t>(cell, deletion_time_offset)));
    }
    // Can be called only when is_live_and_has_ttl() is true.
    static gc_clock::time_point expiry(const bytes_view& cell) {
        assert(is_live_and_has_ttl(cell));
        auto expiry = get_field<int32_t>(cell, expiry_offset);
        return gc_clock::time_point(gc_clock::duration(expiry));
    }
    // Can be called only when is_live_and_has_ttl() is true.
    static gc_clock::duration ttl(const bytes_view& cell) {
        assert(is_live_and_has_ttl(cell));
        return gc_clock::duration(get_field<int32_t>(cell, ttl_offset));
    }
    static bytes make_dead(api::timestamp_type timestamp, gc_clock::time_point deletion_time) {
        bytes b(bytes::initialized_later(), flags_size + timestamp_size + deletion_time_size);
        b[0] = DEAD_FLAGS;
        set_field(b, timestamp_offset, timestamp);
        set_field(b, deletion_time_offset, deletion_time.time_since_epoch().count());
        return b;
    }
    static bytes make_live(api::timestamp_type timestamp, bytes_view value) {
        auto value_offset = flags_size + timestamp_size;
        bytes b(bytes::initialized_later(), value_offset + value.size());
        b[0] = LIVE_FLAG;
        set_field(b, timestamp_offset, timestamp);
        std::copy_n(value.begin(), value.size(), b.begin() + value_offset);
        return b;
    }
    static bytes make_live(api::timestamp_type timestamp, bytes_view value, gc_clock::time_point expiry, gc_clock::duration ttl) {
        auto value_offset = flags_size + timestamp_size + expiry_size + ttl_size;
        bytes b(bytes::initialized_later(), value_offset + value.size());
        b[0] = EXPIRY_FLAG | LIVE_FLAG;
        set_field(b, timestamp_offset, timestamp);
        set_field(b, expiry_offset, expiry.time_since_epoch().count());
        set_field(b, ttl_offset, ttl.count());
        std::copy_n(value.begin(), value.size(), b.begin() + value_offset);
        return b;
    }
    template<typename ByteContainer>
    friend class atomic_cell_base;
    friend class atomic_cell;
};

template<typename ByteContainer>
class atomic_cell_base {
protected:
    ByteContainer _data;
protected:
    atomic_cell_base(ByteContainer&& data) : _data(std::forward<ByteContainer>(data)) { }
    atomic_cell_base(const ByteContainer& data) : _data(data) { }
public:
    bool is_live() const {
        return atomic_cell_type::is_live(_data);
    }
    bool is_live(tombstone t) const {
        return is_live() && !is_covered_by(t);
    }
    bool is_live(tombstone t, gc_clock::time_point now) const {
        return is_live() && !is_covered_by(t) && !has_expired(now);
    }
    bool is_live_and_has_ttl() const {
        return atomic_cell_type::is_live_and_has_ttl(_data);
    }
    bool is_dead(gc_clock::time_point now) const {
        return atomic_cell_type::is_dead(_data) || has_expired(now);
    }
    bool is_covered_by(tombstone t) const {
        return timestamp() <= t.timestamp;
    }
    // Can be called on live and dead cells
    api::timestamp_type timestamp() const {
        return atomic_cell_type::timestamp(_data);
    }
    // Can be called on live cells only
    bytes_view value() const {
        return atomic_cell_type::value(_data);
    }
    // Can be called only when is_dead(gc_clock::time_point)
    gc_clock::time_point deletion_time() const {
        return !is_live() ? atomic_cell_type::deletion_time(_data) : expiry() - ttl();
    }
    // Can be called only when is_live_and_has_ttl()
    gc_clock::time_point expiry() const {
        return atomic_cell_type::expiry(_data);
    }
    // Can be called only when is_live_and_has_ttl()
    gc_clock::duration ttl() const {
        return atomic_cell_type::ttl(_data);
    }
    // Can be called on live and dead cells
    bool has_expired(gc_clock::time_point now) const {
        return is_live_and_has_ttl() && expiry() < now;
    }
    bytes_view serialize() const {
        return _data;
    }
};

class atomic_cell_view final : public atomic_cell_base<bytes_view> {
    atomic_cell_view(bytes_view data) : atomic_cell_base(data) {}
public:
    static atomic_cell_view from_bytes(bytes_view data) { return atomic_cell_view(data); }

    friend class atomic_cell;
    friend std::ostream& operator<<(std::ostream& os, const atomic_cell_view& acv);
};

class atomic_cell final : public atomic_cell_base<bytes> {
    atomic_cell(bytes b) : atomic_cell_base(std::move(b)) {}
public:
    atomic_cell(const atomic_cell&) = default;
    atomic_cell(atomic_cell&&) = default;
    atomic_cell& operator=(const atomic_cell&) = default;
    atomic_cell& operator=(atomic_cell&&) = default;
    static atomic_cell from_bytes(bytes b) {
        return atomic_cell(std::move(b));
    }
    atomic_cell(atomic_cell_view other) : atomic_cell_base(bytes { other._data.begin(), other._data.end() }) {}
    operator atomic_cell_view() const {
        return atomic_cell_view(_data);
    }
    static atomic_cell make_dead(api::timestamp_type timestamp, gc_clock::time_point expiry) {
        return atomic_cell_type::make_dead(timestamp, expiry);
    }
    static atomic_cell make_live(api::timestamp_type timestamp, bytes_view value) {
        return atomic_cell_type::make_live(timestamp, value);
    }
    static atomic_cell make_live(api::timestamp_type timestamp, bytes_view value,
        gc_clock::time_point expiry, gc_clock::duration ttl)
    {
        return atomic_cell_type::make_live(timestamp, value, expiry, ttl);
    }
    static atomic_cell make_live(api::timestamp_type timestamp, bytes_view value, ttl_opt ttl) {
        if (!ttl) {
            return atomic_cell_type::make_live(timestamp, value);
        } else {
            return atomic_cell_type::make_live(timestamp, value, gc_clock::now() + *ttl, *ttl);
        }
    }
    friend class atomic_cell_or_collection;
    friend std::ostream& operator<<(std::ostream& os, const atomic_cell& ac);
};

// Represents a mutation of a collection.  Actual format is determined by collection type,
// and is:
//   set:  list of atomic_cell
//   map:  list of pair<atomic_cell, bytes> (for key/value)
//   list: tbd, probably ugly
class collection_mutation {
public:
    struct view {
        bytes_view data;
        bytes_view serialize() const { return data; }
        static view from_bytes(bytes_view v) { return { v }; }
    };
    struct one {
        bytes data;
        one() {}
        one(bytes b) : data(std::move(b)) {}
        one(view v) : data(v.data.begin(), v.data.end()) {}
        operator view() const { return { data }; }
    };
};

namespace db {
template<typename T>
class serializer;
}

// A variant type that can hold either an atomic_cell, or a serialized collection.
// Which type is stored is determinied by the schema.
class atomic_cell_or_collection final {
    bytes _data;

    template<typename T>
    friend class db::serializer;
private:
    atomic_cell_or_collection() = default;
    atomic_cell_or_collection(bytes&& data) : _data(std::move(data)) {}
public:
    atomic_cell_or_collection(atomic_cell ac) : _data(std::move(ac._data)) {}
    static atomic_cell_or_collection from_atomic_cell(atomic_cell data) { return { std::move(data._data) }; }
    atomic_cell_view as_atomic_cell() const { return atomic_cell_view::from_bytes(_data); }
    atomic_cell_or_collection(collection_mutation::one cm) : _data(std::move(cm.data)) {}
    static atomic_cell_or_collection from_collection_mutation(collection_mutation::one data) {
        return std::move(data.data);
    }
    collection_mutation::view as_collection_mutation() const {
        return collection_mutation::view{_data};
    }
    bytes_view serialize() const {
        return _data;
    }
    friend std::ostream& operator<<(std::ostream&, const atomic_cell_or_collection&);
};

class column_definition;

int compare_atomic_cell_for_merge(atomic_cell_view left, atomic_cell_view right);
void merge_column(const column_definition& def,
        atomic_cell_or_collection& old,
        const atomic_cell_or_collection& neww);
<commit_msg>db: atomic_cell: Fix misnamed parameter<commit_after>/*
 * Copyright (C) 2015 Cloudius Systems, Ltd.
 */

#pragma once

#include "bytes.hh"
#include "timestamp.hh"
#include "tombstone.hh"
#include "gc_clock.hh"
#include "net/byteorder.hh"
#include <cstdint>
#include <iostream>

template<typename T>
static inline
void set_field(bytes& v, unsigned offset, T val) {
    reinterpret_cast<net::packed<T>*>(v.begin() + offset)->raw = net::hton(val);
}

template<typename T>
static inline
T get_field(const bytes_view& v, unsigned offset) {
    return net::ntoh(*reinterpret_cast<const net::packed<T>*>(v.begin() + offset));
}

class atomic_cell_or_collection;

/*
 * Represents atomic cell layout. Works on serialized form.
 *
 * Layout:
 *
 *  <live>  := <int8_t:flags><int64_t:timestamp>(<int32_t:expiry><int32_t:ttl>)?<value>
 *  <dead>  := <int8_t:    0><int64_t:timestamp><int32_t:deletion_time>
 */
class atomic_cell_type final {
private:
    static constexpr int8_t DEAD_FLAGS = 0;
    static constexpr int8_t LIVE_FLAG = 0x01;
    static constexpr int8_t EXPIRY_FLAG = 0x02; // When present, expiry field is present. Set only for live cells
    static constexpr unsigned flags_size = 1;
    static constexpr unsigned timestamp_offset = flags_size;
    static constexpr unsigned timestamp_size = 8;
    static constexpr unsigned expiry_offset = timestamp_offset + timestamp_size;
    static constexpr unsigned expiry_size = 4;
    static constexpr unsigned deletion_time_offset = timestamp_offset + timestamp_size;
    static constexpr unsigned deletion_time_size = 4;
    static constexpr unsigned ttl_offset = expiry_offset + expiry_size;
    static constexpr unsigned ttl_size = 4;
private:
    static bool is_live(const bytes_view& cell) {
        return cell[0] != DEAD_FLAGS;
    }
    static bool is_live_and_has_ttl(const bytes_view& cell) {
        return cell[0] & EXPIRY_FLAG;
    }
    static bool is_dead(const bytes_view& cell) {
        return cell[0] == DEAD_FLAGS;
    }
    // Can be called on live and dead cells
    static api::timestamp_type timestamp(const bytes_view& cell) {
        return get_field<api::timestamp_type>(cell, timestamp_offset);
    }
    // Can be called on live cells only
    static bytes_view value(bytes_view cell) {
        auto expiry_field_size = bool(cell[0] & EXPIRY_FLAG) * (expiry_size + ttl_size);
        auto value_offset = flags_size + timestamp_size + expiry_field_size;
        cell.remove_prefix(value_offset);
        return cell;
    }
    // Can be called only when is_dead() is true.
    static gc_clock::time_point deletion_time(const bytes_view& cell) {
        assert(is_dead(cell));
        return gc_clock::time_point(gc_clock::duration(
            get_field<int32_t>(cell, deletion_time_offset)));
    }
    // Can be called only when is_live_and_has_ttl() is true.
    static gc_clock::time_point expiry(const bytes_view& cell) {
        assert(is_live_and_has_ttl(cell));
        auto expiry = get_field<int32_t>(cell, expiry_offset);
        return gc_clock::time_point(gc_clock::duration(expiry));
    }
    // Can be called only when is_live_and_has_ttl() is true.
    static gc_clock::duration ttl(const bytes_view& cell) {
        assert(is_live_and_has_ttl(cell));
        return gc_clock::duration(get_field<int32_t>(cell, ttl_offset));
    }
    static bytes make_dead(api::timestamp_type timestamp, gc_clock::time_point deletion_time) {
        bytes b(bytes::initialized_later(), flags_size + timestamp_size + deletion_time_size);
        b[0] = DEAD_FLAGS;
        set_field(b, timestamp_offset, timestamp);
        set_field(b, deletion_time_offset, deletion_time.time_since_epoch().count());
        return b;
    }
    static bytes make_live(api::timestamp_type timestamp, bytes_view value) {
        auto value_offset = flags_size + timestamp_size;
        bytes b(bytes::initialized_later(), value_offset + value.size());
        b[0] = LIVE_FLAG;
        set_field(b, timestamp_offset, timestamp);
        std::copy_n(value.begin(), value.size(), b.begin() + value_offset);
        return b;
    }
    static bytes make_live(api::timestamp_type timestamp, bytes_view value, gc_clock::time_point expiry, gc_clock::duration ttl) {
        auto value_offset = flags_size + timestamp_size + expiry_size + ttl_size;
        bytes b(bytes::initialized_later(), value_offset + value.size());
        b[0] = EXPIRY_FLAG | LIVE_FLAG;
        set_field(b, timestamp_offset, timestamp);
        set_field(b, expiry_offset, expiry.time_since_epoch().count());
        set_field(b, ttl_offset, ttl.count());
        std::copy_n(value.begin(), value.size(), b.begin() + value_offset);
        return b;
    }
    template<typename ByteContainer>
    friend class atomic_cell_base;
    friend class atomic_cell;
};

template<typename ByteContainer>
class atomic_cell_base {
protected:
    ByteContainer _data;
protected:
    atomic_cell_base(ByteContainer&& data) : _data(std::forward<ByteContainer>(data)) { }
    atomic_cell_base(const ByteContainer& data) : _data(data) { }
public:
    bool is_live() const {
        return atomic_cell_type::is_live(_data);
    }
    bool is_live(tombstone t) const {
        return is_live() && !is_covered_by(t);
    }
    bool is_live(tombstone t, gc_clock::time_point now) const {
        return is_live() && !is_covered_by(t) && !has_expired(now);
    }
    bool is_live_and_has_ttl() const {
        return atomic_cell_type::is_live_and_has_ttl(_data);
    }
    bool is_dead(gc_clock::time_point now) const {
        return atomic_cell_type::is_dead(_data) || has_expired(now);
    }
    bool is_covered_by(tombstone t) const {
        return timestamp() <= t.timestamp;
    }
    // Can be called on live and dead cells
    api::timestamp_type timestamp() const {
        return atomic_cell_type::timestamp(_data);
    }
    // Can be called on live cells only
    bytes_view value() const {
        return atomic_cell_type::value(_data);
    }
    // Can be called only when is_dead(gc_clock::time_point)
    gc_clock::time_point deletion_time() const {
        return !is_live() ? atomic_cell_type::deletion_time(_data) : expiry() - ttl();
    }
    // Can be called only when is_live_and_has_ttl()
    gc_clock::time_point expiry() const {
        return atomic_cell_type::expiry(_data);
    }
    // Can be called only when is_live_and_has_ttl()
    gc_clock::duration ttl() const {
        return atomic_cell_type::ttl(_data);
    }
    // Can be called on live and dead cells
    bool has_expired(gc_clock::time_point now) const {
        return is_live_and_has_ttl() && expiry() < now;
    }
    bytes_view serialize() const {
        return _data;
    }
};

class atomic_cell_view final : public atomic_cell_base<bytes_view> {
    atomic_cell_view(bytes_view data) : atomic_cell_base(data) {}
public:
    static atomic_cell_view from_bytes(bytes_view data) { return atomic_cell_view(data); }

    friend class atomic_cell;
    friend std::ostream& operator<<(std::ostream& os, const atomic_cell_view& acv);
};

class atomic_cell final : public atomic_cell_base<bytes> {
    atomic_cell(bytes b) : atomic_cell_base(std::move(b)) {}
public:
    atomic_cell(const atomic_cell&) = default;
    atomic_cell(atomic_cell&&) = default;
    atomic_cell& operator=(const atomic_cell&) = default;
    atomic_cell& operator=(atomic_cell&&) = default;
    static atomic_cell from_bytes(bytes b) {
        return atomic_cell(std::move(b));
    }
    atomic_cell(atomic_cell_view other) : atomic_cell_base(bytes { other._data.begin(), other._data.end() }) {}
    operator atomic_cell_view() const {
        return atomic_cell_view(_data);
    }
    static atomic_cell make_dead(api::timestamp_type timestamp, gc_clock::time_point deletion_time) {
        return atomic_cell_type::make_dead(timestamp, deletion_time);
    }
    static atomic_cell make_live(api::timestamp_type timestamp, bytes_view value) {
        return atomic_cell_type::make_live(timestamp, value);
    }
    static atomic_cell make_live(api::timestamp_type timestamp, bytes_view value,
        gc_clock::time_point expiry, gc_clock::duration ttl)
    {
        return atomic_cell_type::make_live(timestamp, value, expiry, ttl);
    }
    static atomic_cell make_live(api::timestamp_type timestamp, bytes_view value, ttl_opt ttl) {
        if (!ttl) {
            return atomic_cell_type::make_live(timestamp, value);
        } else {
            return atomic_cell_type::make_live(timestamp, value, gc_clock::now() + *ttl, *ttl);
        }
    }
    friend class atomic_cell_or_collection;
    friend std::ostream& operator<<(std::ostream& os, const atomic_cell& ac);
};

// Represents a mutation of a collection.  Actual format is determined by collection type,
// and is:
//   set:  list of atomic_cell
//   map:  list of pair<atomic_cell, bytes> (for key/value)
//   list: tbd, probably ugly
class collection_mutation {
public:
    struct view {
        bytes_view data;
        bytes_view serialize() const { return data; }
        static view from_bytes(bytes_view v) { return { v }; }
    };
    struct one {
        bytes data;
        one() {}
        one(bytes b) : data(std::move(b)) {}
        one(view v) : data(v.data.begin(), v.data.end()) {}
        operator view() const { return { data }; }
    };
};

namespace db {
template<typename T>
class serializer;
}

// A variant type that can hold either an atomic_cell, or a serialized collection.
// Which type is stored is determinied by the schema.
class atomic_cell_or_collection final {
    bytes _data;

    template<typename T>
    friend class db::serializer;
private:
    atomic_cell_or_collection() = default;
    atomic_cell_or_collection(bytes&& data) : _data(std::move(data)) {}
public:
    atomic_cell_or_collection(atomic_cell ac) : _data(std::move(ac._data)) {}
    static atomic_cell_or_collection from_atomic_cell(atomic_cell data) { return { std::move(data._data) }; }
    atomic_cell_view as_atomic_cell() const { return atomic_cell_view::from_bytes(_data); }
    atomic_cell_or_collection(collection_mutation::one cm) : _data(std::move(cm.data)) {}
    static atomic_cell_or_collection from_collection_mutation(collection_mutation::one data) {
        return std::move(data.data);
    }
    collection_mutation::view as_collection_mutation() const {
        return collection_mutation::view{_data};
    }
    bytes_view serialize() const {
        return _data;
    }
    friend std::ostream& operator<<(std::ostream&, const atomic_cell_or_collection&);
};

class column_definition;

int compare_atomic_cell_for_merge(atomic_cell_view left, atomic_cell_view right);
void merge_column(const column_definition& def,
        atomic_cell_or_collection& old,
        const atomic_cell_or_collection& neww);
<|endoftext|>


<commit_before>#include "path/FollowablePath3D.h"

#include "utils/Utils.h"
#include "math/Utils.h"

using namespace std;
using namespace chr;

namespace chr
{
  namespace path
  {
    FollowablePath3D::FollowablePath3D(size_t capacity)
    {
      if (capacity > 0)
      {
        points.reserve(capacity);
        lengths.reserve(capacity);
      }
    }

    void FollowablePath3D::clear()
    {
      points.clear();
      lengths.clear();
    }

    void FollowablePath3D::reserve(size_t n)
    {
      points.reserve(n);
      lengths.reserve(n);
    }

    size_t FollowablePath3D::size() const
    {
      return points.size();
    }

    bool FollowablePath3D::empty() const
    {
      return points.empty();
    }

    float FollowablePath3D::getLength() const
    {
      if (!empty())
      {
        return lengths.back();
      }
      else
      {
        return 0;
      }
    }

    const vector<FollowablePath3D::Point>& FollowablePath3D::getPoints() const
    {
      return points;
    }

    const vector<float>& FollowablePath3D::getLengths() const
    {
      return lengths;
    }

    FollowablePath3D& FollowablePath3D::setMode(Mode mode)
    {
      this->mode = mode;
      return *this;
    }

    FollowablePath3D& FollowablePath3D::setSampling(Sampling sampling)
    {
      this->sampling = sampling;
      return *this;
    }

    FollowablePath3D& FollowablePath3D::begin()
    {
      points.clear();
      lengths.clear();

      return *this;
    }

    FollowablePath3D& FollowablePath3D::end()
    {
      auto end = size();

      if (end > 1)
      {
        points[end - 1].forward = points[end - 2].forward;
      }

      return *this;
    }

    FollowablePath3D& FollowablePath3D::add(const glm::vec3 &position, const glm::vec3 &left)
    {
      if (!empty())
      {
        auto delta = position - points.back().position;
        auto length = glm::length(delta); // ASSERT: length > EPSILON

        lengths.push_back(lengths.back() + length);
        points.back().forward = delta / length;
      }
      else
      {
        lengths.push_back(0);
      }

      points.emplace_back(position, left);
      return *this;
    }

    glm::quat FollowablePath3D::offsetToQuat(float offset, float sampleSize) const
    {
      float length = getLength();

      if (length > 0)
      {
        if (sampleSize > 0)
        {
          if ((mode == MODE_LOOP) || (mode == MODE_MODULO))
          {
            offset = math::boundf(offset, length);
          }

          float offset0 = offset - sampleSize * 0.5f;
          float offset1 = offset + sampleSize * 0.5f;

          if ((offset1 <= 0) || ((offset0 < 0) && (offset1 > 0)))
          {
            return points.front().toQuat();
          }
          else if ((offset0 >= length) || ((offset1 > length) && (offset0 < length)))
          {
            return points.back().toQuat();
          }

          auto index0 = utils::search(lengths, offset0, 1, size());
          auto index1 = utils::search(lengths, offset1, 1, size());

          if (index1 < index0)
          {
            return points[index0].toQuat();
          }
          else if (index0 > index1)
          {
            return points[index1].toQuat();
          }
          else
          {
            if (sampling == SAMPLING_CONTINUOUS)
            {
              float u0 = (offset0 - lengths[index0]) / (lengths[index0 + 1] - lengths[index0]);
              auto q0 = glm::slerp(points[index0].toQuat(), points[index0 + 1].toQuat(), u0);

              float u1 = (offset1 - lengths[index1]) / (lengths[index1 + 1] - lengths[index1]);
              auto q1 = glm::slerp(points[index1].toQuat(), points[index1 + 1].toQuat(), u1);

              return glm::slerp(q0, q1, 0.5f);
            }
            else if (sampling == SAMPLING_CORNERS)
            {
              float limit0 = lengths[index0 + 1] - sampleSize * 0.5f;
              float limit1 = lengths[index1] + sampleSize * 0.5f;

              if (offset < limit0)
              {
                return points[index0].toQuat();
              }
              else if (offset > limit1)
              {
                return points[index1].toQuat();
              }
              else
              {
                auto q0 = points[index0].toQuat();
                auto q1 = points[index1].toQuat();

                float ratio = (offset - limit0) / (limit1 - limit0);
                return glm::slerp(q0, q1, ratio);
              }
            }
          }
        }
        else
        {
          if ((mode == MODE_LOOP) || (mode == MODE_MODULO))
          {
            offset = math::boundf(offset, length);
          }
          else if (offset <= 0)
          {
            if (mode == MODE_BOUNDED)
            {
              return points.front().toQuat();
            }
          }
          else if (offset >= length)
          {
            if (mode == MODE_BOUNDED)
            {
              return points.back().toQuat();
            }
          }

          auto index = utils::search(lengths, offset, 1, size());
          return points[index].toQuat();
        }
      }

      return glm::quat();
    }

    FollowablePath3D::Value FollowablePath3D::offsetToValue(float offset, float sampleSize) const
    {
      Value value;

      float length = getLength();

      if (length > 0)
      {
        if ((mode == MODE_LOOP) || (mode == MODE_MODULO))
        {
          offset = math::boundf(offset, length);
        }
        else if (offset <= 0)
        {
          if (mode == MODE_BOUNDED)
          {
            value.position = points.front().position;
            value.left = points.front().left;
            value.forward = points.front().forward;
            value.up = glm::cross(-value.left, value.forward);
            value.offset = 0;

            goto exit;
          }
          else if (mode == MODE_CLAMPED)
          {
            goto exit;
          }
        }
        else if (offset >= length)
        {
          if (mode == MODE_BOUNDED)
          {
            value.position = points.back().position;
            value.left = points.front().left;
            value.forward = points.back().forward;
            value.up = glm::cross(-value.left, value.forward);
            value.offset = length;

            goto exit;
          }
          else if (mode == MODE_CLAMPED)
          {
            goto exit;
          }
        }

        auto index = utils::search(lengths, offset, 1, size());
        auto &p0 = points[index];
        auto &p1 = points[index + 1];

        float u = (offset - lengths[index]) / (lengths[index + 1] - lengths[index]);

        value.position = p0.position * (1 - u) + p1.position * u;
        value.offset = offset;

        if (sampleSize > 0)
        {
          value.fromQuat(offsetToQuat(offset, sampleSize));
        }
        else
        {
          value.forward = p0.forward;
          value.left = p0.left;
          value.up = glm::cross(-value.left, value.forward);
        }
      }

      exit:
      return value;
    }

    // ---

    glm::quat FollowablePath3D::Point::toQuat() const
    {
      auto up(glm::cross(-left, forward));

      glm::mat4 m;
      m[0][0] = forward.x; m[1][0] = left.x; m[2][0] = up.x;
      m[0][1] = forward.y; m[1][1] = left.y; m[2][1] = up.y;
      m[0][2] = forward.z; m[1][2] = left.z; m[2][2] = up.z;

      return glm::quat_cast(m);
    }

    void FollowablePath3D::Value::applyToMatrix(glm::mat4 &m) const
    {
      m[0][0] = forward.x; m[1][0] = left.x; m[2][0] = up.x; m[3][0] = position.x;
      m[0][1] = forward.y; m[1][1] = left.y; m[2][1] = up.y; m[3][1] = position.y;
      m[0][2] = forward.z; m[1][2] = left.z; m[2][2] = up.z; m[3][2] = position.z;
      m[0][3] = 0;         m[1][3] = 0;      m[2][3] = 0;    m[3][3] = 1;
    }

    void FollowablePath3D::Value::applyToMatrix(array<float, 16> &m) const
    {
      m[0] = forward.x; m[4] = left.x; m[ 8] = up.x; m[12] = position.x;
      m[1] = forward.y; m[5] = left.y; m[ 9] = up.y; m[13] = position.y;
      m[2] = forward.z; m[6] = left.z; m[10] = up.z; m[14] = position.z;
      m[3] = 0;         m[7] = 0;      m[11] = 0;    m[15] = 1;
    }

    void FollowablePath3D::Value::fromQuat(const glm::quat &q)
    {
      float xx = q.x * q.x;
      float xy = q.x * q.y;
      float xz = q.x * q.z;
      float xw = q.x * q.w;

      float yy = q.y * q.y;
      float yz = q.y * q.z;
      float yw = q.y * q.w;

      float zz = q.z * q.z;
      float zw = q.z * q.w;

      forward.x = 1 - 2 * (yy + zz);
      left.x = 2 * (xy - zw);
      up.x = 2 * (xz + yw);

      forward.y = 2 * (xy + zw);
      left.y = 1 - 2 * (xx + zz);
      up.y = 2 * (yz - xw);

      forward.z = 2 * (xz - yw);
      left.z = 2 * (yz + xw);
      up.z = 1 - 2 * (xx + yy);
    }
  }
}
<commit_msg>TWEAK<commit_after>#include "path/FollowablePath3D.h"

#include "utils/Utils.h"
#include "math/Utils.h"

using namespace std;
using namespace chr;

namespace chr
{
  namespace path
  {
    FollowablePath3D::FollowablePath3D(size_t capacity)
    {
      if (capacity > 0)
      {
        points.reserve(capacity);
        lengths.reserve(capacity);
      }
    }

    void FollowablePath3D::clear()
    {
      points.clear();
      lengths.clear();
    }

    void FollowablePath3D::reserve(size_t n)
    {
      points.reserve(n);
      lengths.reserve(n);
    }

    size_t FollowablePath3D::size() const
    {
      return points.size();
    }

    bool FollowablePath3D::empty() const
    {
      return points.empty();
    }

    float FollowablePath3D::getLength() const
    {
      if (!empty())
      {
        return lengths.back();
      }
      else
      {
        return 0;
      }
    }

    const vector<FollowablePath3D::Point>& FollowablePath3D::getPoints() const
    {
      return points;
    }

    const vector<float>& FollowablePath3D::getLengths() const
    {
      return lengths;
    }

    FollowablePath3D& FollowablePath3D::setMode(Mode mode)
    {
      this->mode = mode;
      return *this;
    }

    FollowablePath3D& FollowablePath3D::setSampling(Sampling sampling)
    {
      this->sampling = sampling;
      return *this;
    }

    FollowablePath3D& FollowablePath3D::begin()
    {
      points.clear();
      lengths.clear();

      return *this;
    }

    FollowablePath3D& FollowablePath3D::end()
    {
      auto end = size();

      if (end > 1)
      {
        points[end - 1].forward = points[end - 2].forward;
      }

      return *this;
    }

    FollowablePath3D& FollowablePath3D::add(const glm::vec3 &position, const glm::vec3 &left)
    {
      if (!empty())
      {
        auto delta = position - points.back().position;
        auto length = glm::length(delta); // ASSERT: length > EPSILON

        lengths.push_back(lengths.back() + length);
        points.back().forward = delta / length;
      }
      else
      {
        lengths.push_back(0);
      }

      points.emplace_back(position, left);
      return *this;
    }

    glm::quat FollowablePath3D::offsetToQuat(float offset, float sampleSize) const
    {
      float length = getLength();

      if (length > 0)
      {
        if (sampleSize > 0)
        {
          if ((mode == MODE_LOOP) || (mode == MODE_MODULO))
          {
            offset = math::boundf(offset, length);
          }

          float offset0 = offset - sampleSize * 0.5f;
          float offset1 = offset + sampleSize * 0.5f;

          if (mode == MODE_BOUNDED)
          {
            if ((offset1 <= 0) || ((offset0 < 0) && (offset1 > 0)))
            {
              return points.front().toQuat();
            }
            else if ((offset0 >= length) || ((offset1 > length) && (offset0 < length)))
            {
              return points.back().toQuat();
            }
          }

          auto index0 = utils::search(lengths, offset0, 1, size());
          auto index1 = utils::search(lengths, offset1, 1, size());

          if (index1 < index0)
          {
            return points[index0].toQuat();
          }
          else if (index0 > index1)
          {
            return points[index1].toQuat();
          }
          else
          {
            if (sampling == SAMPLING_CONTINUOUS)
            {
              float u0 = (offset0 - lengths[index0]) / (lengths[index0 + 1] - lengths[index0]);
              auto q0 = glm::slerp(points[index0].toQuat(), points[index0 + 1].toQuat(), u0);

              float u1 = (offset1 - lengths[index1]) / (lengths[index1 + 1] - lengths[index1]);
              auto q1 = glm::slerp(points[index1].toQuat(), points[index1 + 1].toQuat(), u1);

              return glm::slerp(q0, q1, 0.5f);
            }
            else if (sampling == SAMPLING_CORNERS)
            {
              float limit0 = lengths[index0 + 1] - sampleSize * 0.5f;
              float limit1 = lengths[index1] + sampleSize * 0.5f;

              if (offset < limit0)
              {
                return points[index0].toQuat();
              }
              else if (offset > limit1)
              {
                return points[index1].toQuat();
              }
              else
              {
                auto q0 = points[index0].toQuat();
                auto q1 = points[index1].toQuat();

                float ratio = (offset - limit0) / (limit1 - limit0);
                return glm::slerp(q0, q1, ratio);
              }
            }
          }
        }
        else
        {
          if ((mode == MODE_LOOP) || (mode == MODE_MODULO))
          {
            offset = math::boundf(offset, length);
          }
          else if (offset <= 0)
          {
            if (mode == MODE_BOUNDED)
            {
              return points.front().toQuat();
            }
          }
          else if (offset >= length)
          {
            if (mode == MODE_BOUNDED)
            {
              return points.back().toQuat();
            }
          }

          auto index = utils::search(lengths, offset, 1, size());
          return points[index].toQuat();
        }
      }

      return glm::quat();
    }

    FollowablePath3D::Value FollowablePath3D::offsetToValue(float offset, float sampleSize) const
    {
      Value value;

      float length = getLength();

      if (length > 0)
      {
        if ((mode == MODE_LOOP) || (mode == MODE_MODULO))
        {
          offset = math::boundf(offset, length);
        }
        else if (offset <= 0)
        {
          if (mode == MODE_BOUNDED)
          {
            value.position = points.front().position;
            value.left = points.front().left;
            value.forward = points.front().forward;
            value.up = glm::cross(-value.left, value.forward);
            value.offset = 0;

            goto exit;
          }
          else if (mode == MODE_CLAMPED)
          {
            goto exit;
          }
        }
        else if (offset >= length)
        {
          if (mode == MODE_BOUNDED)
          {
            value.position = points.back().position;
            value.left = points.front().left;
            value.forward = points.back().forward;
            value.up = glm::cross(-value.left, value.forward);
            value.offset = length;

            goto exit;
          }
          else if (mode == MODE_CLAMPED)
          {
            goto exit;
          }
        }

        auto index = utils::search(lengths, offset, 1, size());
        auto &p0 = points[index];
        auto &p1 = points[index + 1];

        float u = (offset - lengths[index]) / (lengths[index + 1] - lengths[index]);

        value.position = p0.position * (1 - u) + p1.position * u;
        value.offset = offset;

        if (sampleSize > 0)
        {
          value.fromQuat(offsetToQuat(offset, sampleSize));
        }
        else
        {
          value.forward = p0.forward;
          value.left = p0.left;
          value.up = glm::cross(-value.left, value.forward);
        }
      }

      exit:
      return value;
    }

    // ---

    glm::quat FollowablePath3D::Point::toQuat() const
    {
      auto up(glm::cross(-left, forward));

      glm::mat4 m;
      m[0][0] = forward.x; m[1][0] = left.x; m[2][0] = up.x;
      m[0][1] = forward.y; m[1][1] = left.y; m[2][1] = up.y;
      m[0][2] = forward.z; m[1][2] = left.z; m[2][2] = up.z;

      return glm::quat_cast(m);
    }

    void FollowablePath3D::Value::applyToMatrix(glm::mat4 &m) const
    {
      m[0][0] = forward.x; m[1][0] = left.x; m[2][0] = up.x; m[3][0] = position.x;
      m[0][1] = forward.y; m[1][1] = left.y; m[2][1] = up.y; m[3][1] = position.y;
      m[0][2] = forward.z; m[1][2] = left.z; m[2][2] = up.z; m[3][2] = position.z;
      m[0][3] = 0;         m[1][3] = 0;      m[2][3] = 0;    m[3][3] = 1;
    }

    void FollowablePath3D::Value::applyToMatrix(array<float, 16> &m) const
    {
      m[0] = forward.x; m[4] = left.x; m[ 8] = up.x; m[12] = position.x;
      m[1] = forward.y; m[5] = left.y; m[ 9] = up.y; m[13] = position.y;
      m[2] = forward.z; m[6] = left.z; m[10] = up.z; m[14] = position.z;
      m[3] = 0;         m[7] = 0;      m[11] = 0;    m[15] = 1;
    }

    void FollowablePath3D::Value::fromQuat(const glm::quat &q)
    {
      float xx = q.x * q.x;
      float xy = q.x * q.y;
      float xz = q.x * q.z;
      float xw = q.x * q.w;

      float yy = q.y * q.y;
      float yz = q.y * q.z;
      float yw = q.y * q.w;

      float zz = q.z * q.z;
      float zw = q.z * q.w;

      forward.x = 1 - 2 * (yy + zz);
      left.x = 2 * (xy - zw);
      up.x = 2 * (xz + yw);

      forward.y = 2 * (xy + zw);
      left.y = 1 - 2 * (xx + zz);
      up.y = 2 * (yz - xw);

      forward.z = 2 * (xz - yw);
      left.z = 2 * (yz + xw);
      up.z = 1 - 2 * (xx + yy);
    }
  }
}
<|endoftext|>


<commit_before>/** \brief Utility for replacing old BSZ PPN's with new K10+ PPN's.
 *  \author Dr. Johannes Ruscheinski (johannes.ruscheinski@uni-tuebingen.de)
 *
 *  \copyright 2019 Universitätsbibliothek Tübingen.  All rights reserved.
 *
 *  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 <algorithm>
#include <stdexcept>
#include <unordered_map>
#include <cstdlib>
#include <cstring>
#include <kchashdb.h>
#include "JSON.h"
#include "MARC.h"
#include "Solr.h"
#include "StringUtil.h"
#include "util.h"

namespace {

[[noreturn]] void Usage() {
    ::Usage("old_ppns_to_new_ppns_map_path marc_input marc_output field_and_subfield_code1 "
            "[field_and_subfield_code2 .. field_and_subfield_codeN]\n"
            "For field_and_subfield_code an example would be 773w.");
}

void ProcessRecords(MARC::Reader * const marc_reader, MARC::Writer * const marc_writer,
                    const std::vector<std::string> &tags_and_subfield_codes, kyotocabinet::HashDB * const db)
{
    unsigned total_record_count(0), patched_record_count(0);
    while (MARC::Record record = marc_reader->read()) {
            ++total_record_count;

            bool patched_record(false);
            for (const auto tag_and_subfield_code : tags_and_subfield_codes) {
                for (auto field : record.getTagRange(tag_and_subfield_code.substr(0, MARC::Record::TAG_LENGTH))) {
                    const char SUBFIELD_CODE(tag_and_subfield_code[MARC::Record::TAG_LENGTH]);
                    MARC::Subfields subfields(field.getSubfields());
                    bool patched_field(false);
                    for (auto &subfield : subfields) {
                        if (subfield.code_ != SUBFIELD_CODE)
                            continue;

                        std::string old_ppn_candidate;
                        if (StringUtil::StartsWith(subfield.value_, "(DE-576)"))
                            old_ppn_candidate = subfield.value_.substr(__builtin_strlen("(DE-576)"));
                        else
                            old_ppn_candidate = subfield.value_;

                        std::string new_ppn;
                        if (not db->get(old_ppn_candidate, &new_ppn))
                            continue;

                        subfield.value_ = new_ppn;
                        patched_field = true;
                    }

                    if (patched_field) {
                        field.setContents(subfields, field.getIndicator1(), field.getIndicator2());
                        patched_record = true;
                    }
                }
            }
            if (patched_record)
                ++patched_record_count;

            marc_writer->write(record);
    }

    LOG_INFO("Processed " + std::to_string(total_record_count) + " records and patched " + std::to_string(patched_record_count)
             + " of them.");
}

} // unnamed namespace

int Main(int argc, char *argv[]) {
    if (argc < 4)
        Usage();

    kyotocabinet::HashDB db;
    if (not db.open(argv[1], kyotocabinet::HashDB::OREADER))
        LOG_ERROR("Failed to open database \"" + std::string(argv[1]) + "\" for reading (" + std::string(db.error().message()) + ")!");

    std::vector<std::string> tags_and_subfield_codes;
    for (int arg_no(3); arg_no < argc; ++arg_no) {
        if (std::strlen(argv[arg_no]) != MARC::Record::TAG_LENGTH + 1)
            LOG_ERROR("bad tag + subfield code: \"" + std::string(argv[arg_no]) + "\"!");
        tags_and_subfield_codes.emplace_back(argv[arg_no]);
    }
    std::sort(tags_and_subfield_codes.begin(), tags_and_subfield_codes.end());

    const auto marc_reader(MARC::Reader::Factory(argv[1]));
    const auto marc_writer(MARC::Writer::Factory(argv[2]));
    ProcessRecords(marc_reader.get(), marc_writer.get(), tags_and_subfield_codes, &db);

    return EXIT_SUCCESS;
}
<commit_msg>We noew use multiple Kyotocabinet databases.<commit_after>/** \brief Utility for replacing old BSZ PPN's with new K10+ PPN's.
 *  \author Dr. Johannes Ruscheinski (johannes.ruscheinski@uni-tuebingen.de)
 *
 *  \copyright 2019 Universitätsbibliothek Tübingen.  All rights reserved.
 *
 *  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 <algorithm>
#include <stdexcept>
#include <unordered_map>
#include <cstdlib>
#include <cstring>
#include <kchashdb.h>
#include "FileUtil.h"
#include "JSON.h"
#include "MARC.h"
#include "Solr.h"
#include "StringUtil.h"
#include "util.h"

namespace {

[[noreturn]] void Usage() {
    ::Usage("old_ppns_to_new_ppns_map_directory marc_input marc_output field_and_subfield_code1 "
            "[field_and_subfield_code2 .. field_and_subfield_codeN]\n"
            "For field_and_subfield_code an example would be 773w.");
}

void OpenAllDBs(const std::string &old_ppns_to_new_ppns_map_directory, std::vector<kyotocabinet::HashDB *> * const dbs) {
    FileUtil::Directory directory(old_ppns_to_new_ppns_map_directory, ".*\\.db");
    for (const auto &entry : directory) {
        dbs->emplace_back(new kyotocabinet::HashDB);
        if (not dbs->back()->open(entry.getName(), kyotocabinet::HashDB::OREADER))
            LOG_ERROR("Failed to open database \"" + entry.getName() + "\" for reading ("
                      + std::string(dbs->back()->error().message()) + ")!");
    }
}

void ProcessRecords(MARC::Reader * const marc_reader, MARC::Writer * const marc_writer,
                    const std::vector<std::string> &tags_and_subfield_codes, const std::vector<kyotocabinet::HashDB *> &dbs)
{
    unsigned total_record_count(0), patched_record_count(0);
    while (MARC::Record record = marc_reader->read()) {
            ++total_record_count;

            bool patched_record(false);
            for (const auto tag_and_subfield_code : tags_and_subfield_codes) {
                for (auto field : record.getTagRange(tag_and_subfield_code.substr(0, MARC::Record::TAG_LENGTH))) {
                    const char SUBFIELD_CODE(tag_and_subfield_code[MARC::Record::TAG_LENGTH]);
                    MARC::Subfields subfields(field.getSubfields());
                    bool patched_field(false);
                    for (auto &subfield : subfields) {
                        if (subfield.code_ != SUBFIELD_CODE)
                            continue;

                        std::string old_ppn_candidate;
                        if (StringUtil::StartsWith(subfield.value_, "(DE-576)"))
                            old_ppn_candidate = subfield.value_.substr(__builtin_strlen("(DE-576)"));
                        else
                            old_ppn_candidate = subfield.value_;

                        for (const auto &db : dbs) {
                            std::string new_ppn;
                            if (db->get(old_ppn_candidate, &new_ppn)) {
                                subfield.value_ = new_ppn;
                                patched_field = true;
                                break;
                            }
                        }
                    }

                    if (patched_field) {
                        field.setContents(subfields, field.getIndicator1(), field.getIndicator2());
                        patched_record = true;
                    }
                }
            }
            if (patched_record)
                ++patched_record_count;

            marc_writer->write(record);
    }

    LOG_INFO("Processed " + std::to_string(total_record_count) + " records and patched " + std::to_string(patched_record_count)
             + " of them.");
}

} // unnamed namespace

int Main(int argc, char *argv[]) {
    if (argc < 4)
        Usage();

    std::vector<kyotocabinet::HashDB *> dbs;
    OpenAllDBs(argv[1], &dbs);

    std::vector<std::string> tags_and_subfield_codes;
    for (int arg_no(3); arg_no < argc; ++arg_no) {
        if (std::strlen(argv[arg_no]) != MARC::Record::TAG_LENGTH + 1)
            LOG_ERROR("bad tag + subfield code: \"" + std::string(argv[arg_no]) + "\"!");
        tags_and_subfield_codes.emplace_back(argv[arg_no]);
    }
    std::sort(tags_and_subfield_codes.begin(), tags_and_subfield_codes.end());

    const auto marc_reader(MARC::Reader::Factory(argv[1]));
    const auto marc_writer(MARC::Writer::Factory(argv[2]));
    ProcessRecords(marc_reader.get(), marc_writer.get(), tags_and_subfield_codes, dbs);

    return EXIT_SUCCESS;
}
<|endoftext|>