<commit_before>/****************** <VPR heading BEGIN do not edit this line> *****************
 *
 * VR Juggler Portable Runtime
 *
 * Original Authors:
 *   Allen Bierbaum, Patrick Hartling, Kevin Meinert, Carolina Cruz-Neira
 *
 * -----------------------------------------------------------------
 * File:          $RCSfile$
 * Date modified: $Date$
 * Version:       $Revision$
 * -----------------------------------------------------------------
 *
 ****************** <VPR heading END do not edit this line> ******************/

/*************** <auto-copyright.pl BEGIN do not edit this line> **************
 *
 * VR Juggler is (C) Copyright 1998, 1999, 2000, 2001 by Iowa State University
 *
 * Original Authors:
 *   Allen Bierbaum, Christopher Just,
 *   Patrick Hartling, Kevin Meinert,
 *   Carolina Cruz-Neira, Albert Baker
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library 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
 * Library General Public License for more details.
 *
 * You should have received a copy of the GNU Library General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 *
 *************** <auto-copyright.pl END do not edit this line> ***************/

#include <vpr/vprConfig.h>

#include <map>
#include <boost/utility.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <vpr/Util/Debug.h>
#include <vpr/Util/Assert.h>

#include <vpr/md/SIM/Network/NetworkGraph.h>

namespace vpr
{

namespace sim
{

// Crummy helper for crappy C++ I/O.
static void skipToEOL (std::istream& stream)
{
   char c;

   while ( (c = stream.get()) != '\n' )
   {
      /* Loop. */ ;
   }
}

vpr::ReturnStatus NetworkGraph::construct (const std::string& path)
{
   vpr::ReturnStatus status;
   std::ifstream input_file(path.c_str());

   if ( input_file.is_open() )
   {
      vpr::Uint32 node_count, full_edge_count, half_edge_count;
      std::map<int, boost::graph_traits<net_graph_t>::vertex_descriptor> vertex_map;
      std::string temp_str;

      input_file >> node_count;

      for ( vpr::Uint32 i = 0; i < node_count; i++ )
      {
         vpr::Uint32 index;
         vpr::Uint16 node_type;
         std::string node_ip;

         input_file >> index >> node_type >> node_ip;
         skipToEOL(input_file);

         // Now create the vertex and add it to the graph.
         NetworkNode node_prop(index, node_type, node_ip);
         vertex_map[index] = boost::add_vertex(NodeProperty(node_prop),
                                               mGraph);
      }

      input_file >> full_edge_count;

      // The file format tells us that there are twice as many edges as
      // actually listed--probably to indicate bidirectional links.
      half_edge_count = full_edge_count / 2;

      net_edge_t new_edge;
      bool added;
      boost::property_map<net_graph_t, boost::edge_weight_t>::type weight_map;
      weight_map = boost::get(boost::edge_weight_t(), mGraph);

      for ( vpr::Uint32 i = 0; i < full_edge_count; i++ )
      {
         double length, bw;
         vpr::Uint32 from_node, to_node, delay;
         std::string net_type, net_ip;
         vpr::Uint16 net_id;

         input_file >> from_node >> to_node >> length >> delay >> bw
                    >> net_type >> net_id >> net_ip;

         // This is here mainly for debugging this clunky "parser".
         vprDEBUG(vprDBG_ALL, vprDBG_HVERB_LVL)
            << "Loading edge #" << i << ": (" << from_node << " --> "
            << to_node << ", " << length << " miles, " << delay << " us, "
            << bw << " Mbps, type: " << net_type << ", ID: " << net_id << ", "
            << net_ip << ")\n" << vprDEBUG_FLUSH;

         // Now add the edge to the graph.
         NetworkLine line(length, bw, delay, net_type, net_id, net_ip);
         boost::tie(new_edge, added) = boost::add_edge(vertex_map[from_node],
                                                       vertex_map[to_node],
                                                       LineProperty(line),
                                                       mGraph);

         if ( added )
         {
            weight_map[new_edge] = line.getWeight();
         }

         // Sanity check to ensure that we do not get stuck trying to read
         // bytes that aren't there.  This probably slows things down, but I
         // don't know of a better way to do this.
         if ( input_file.peek() == EOF && i + 1 < full_edge_count )
         {
            vprDEBUG(vprDBG_ALL, vprDBG_WARNING_LVL)
               << "WARNING: Expected " << full_edge_count
               << " edges, found " << i + 1 << std::endl << vprDEBUG_FLUSH;
            break;
         }
      }

      mGraphValid = true;
   }
   else
   {
      vprDEBUG(vprDBG_ALL, vprDBG_CRITICAL_LVL)
         << "ERROR: Failed to open graph input file named " << path
         << std::endl << vprDEBUG_FLUSH;
      status.setCode(vpr::ReturnStatus::Fail);
   }

   return status;
}

NetworkLine NetworkGraph::getLineProperty (const NetworkGraph::net_edge_t& e)
   const
{
   // XXX: Need to make an assertion that e is in mGraph!
//   vprASSERT(... && "Given edge not found in the graph!");

   boost::property_map<net_graph_t, network_line_t>::const_type line_prop_map;

   line_prop_map = boost::get(network_line_t(), mGraph);

   return line_prop_map[e];
}

void NetworkGraph::setLineProperty (const NetworkGraph::net_edge_t& e,
                                    const vpr::sim::NetworkLine& prop)
{
   // XXX: Need to make an assertion that e is in mGraph!
//   vprASSERT(... && "Given edge not found in the graph!");

   boost::property_map<net_graph_t, network_line_t>::type line_prop_map;

   line_prop_map    = boost::get(network_line_t(), mGraph);
   line_prop_map[e] = prop;
}

vpr::ReturnStatus NetworkGraph::getNodeWithAddr (const vpr::Uint32 addr,
                                                 NetworkGraph::net_vertex_t& node)
{
   vprASSERT(isValid() && "Trying to use invalid graph");

   vpr::ReturnStatus status(vpr::ReturnStatus::Fail);
   boost::graph_traits<net_graph_t>::vertex_iterator vi, vi_end;
   NetworkNode node_prop;

   boost::tie(vi, vi_end) = boost::vertices(mGraph);

   for ( ; vi != vi_end; vi++ )
   {
      node_prop = getNodeProperty(*vi);

      if ( node_prop.getIpAddress() == addr )
      {
         node = *vi;
         status.setCode(vpr::ReturnStatus::Succeed);
         break;
      }
   }

   return status;
}

NetworkNode NetworkGraph::getNodeProperty (const NetworkGraph::net_vertex_t& v)
   const
{
   // XXX: Need to make an assertion that v is in mGraph!
//   vprASSERT(... && "Given vertex not found in the graph!");

   boost::property_map<net_graph_t, network_node_t>::const_type node_prop_map;

   node_prop_map = boost::get(network_node_t(), mGraph);

   return node_prop_map[v];
}

void NetworkGraph::setNodeProperty (const NetworkGraph::net_vertex_t& v,
                                    const vpr::sim::NetworkNode& prop)
{
   // XXX: Need to make an assertion that v is in mGraph!
//   vprASSERT(... && "Given vertex not found in the graph!");

   boost::property_map<net_graph_t, network_node_t>::type node_prop_map;

   node_prop_map    = boost::get(network_node_t(), mGraph);
   node_prop_map[v] = prop;
}

NetworkGraph::VertexListPtr NetworkGraph::getShortestPath (const NetworkGraph::net_vertex_t& src,
                                                           const NetworkGraph::net_vertex_t& dest)
   const
{
   NetworkGraph::VertexListPtr vlist(new NetworkGraph::VertexList);
   std::vector<int> dist(boost::num_vertices(mGraph));
   std::vector<net_vertex_t> pred(boost::num_vertices(mGraph));
   NetworkGraph::net_vertex_t p(dest), q;
   std::stack<net_vertex_t> vstack;
   boost::property_map<net_graph_t, network_node_t>::const_type node_prop_map;
   boost::property_map<net_graph_t, boost::edge_weight_t>::const_type weight_map;

   node_prop_map = boost::get(network_node_t(), mGraph);
   weight_map    = boost::get(boost::edge_weight_t(), mGraph);

//   boost::dijkstra_shortest_paths(mGraph, src,
//                                  boost::distance_map(&dist[0]).predecessor_map(&pred[0]));
   boost::dijkstra_shortest_paths(mGraph, src, &pred[0], &dist[0], weight_map,
                                  boost::get(boost::vertex_index_t(), mGraph),
                                  std::less<int>(), boost::closed_plus<int>(),
                                  std::numeric_limits<int>::max(), 0,
                                  boost::default_dijkstra_visitor());

   vprDEBUG(vprDBG_ALL, vprDBG_VERB_LVL)
      << "Path (dest <-- src): " << node_prop_map[dest].getIpAddressString()
      << vprDEBUG_FLUSH;
   vstack.push(dest);

   // Put the vertices returned in the predecessor map into a stack so that
   // the order that we can reverse the order and put them into vlist.
   while ( (q = pred[p]) != p )
   {
      vprDEBUG_CONT(vprDBG_ALL, vprDBG_VERB_LVL)
         << " <-- " << node_prop_map[q].getIpAddressString() << vprDEBUG_FLUSH;

      vstack.push(q);
      p = q;
   }

   vprDEBUG_CONT(vprDBG_ALL, vprDBG_VERB_LVL)
      << " <-- " << node_prop_map[src].getIpAddressString() << vprDEBUG_FLUSH;
   vprDEBUG_CONT_END(vprDBG_ALL, vprDBG_VERB_LVL) << "\n" << vprDEBUG_FLUSH;
   vprASSERT(p == src && "Destination is unreachable!");

   while ( vstack.size() > 0 )
   {
      vlist->push_back(vstack.top());
      vstack.pop();
   }

   return vlist;
}

NetworkGraph::VertexListPtr NetworkGraph::reversePath (NetworkGraph::VertexListPtr path)
{
   VertexListPtr new_path(new NetworkGraph::VertexList(path->size()));
   NetworkGraph::VertexList::reverse_iterator i;

   for ( i = path->rbegin(); i != path->rend(); i++ )
   {
      new_path->push_back(*i);
   }

   return new_path;
}

} // End of sim namespace

} // End of vpr namespace
<commit_msg>Removed a vprDEBUG_CONT() call that was causing the source node to be printed twice.  (This was definitely confusing me during debugging last night.)<commit_after>/****************** <VPR heading BEGIN do not edit this line> *****************
 *
 * VR Juggler Portable Runtime
 *
 * Original Authors:
 *   Allen Bierbaum, Patrick Hartling, Kevin Meinert, Carolina Cruz-Neira
 *
 * -----------------------------------------------------------------
 * File:          $RCSfile$
 * Date modified: $Date$
 * Version:       $Revision$
 * -----------------------------------------------------------------
 *
 ****************** <VPR heading END do not edit this line> ******************/

/*************** <auto-copyright.pl BEGIN do not edit this line> **************
 *
 * VR Juggler is (C) Copyright 1998, 1999, 2000, 2001 by Iowa State University
 *
 * Original Authors:
 *   Allen Bierbaum, Christopher Just,
 *   Patrick Hartling, Kevin Meinert,
 *   Carolina Cruz-Neira, Albert Baker
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library 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
 * Library General Public License for more details.
 *
 * You should have received a copy of the GNU Library General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 *
 *************** <auto-copyright.pl END do not edit this line> ***************/

#include <vpr/vprConfig.h>

#include <map>
#include <boost/utility.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <vpr/Util/Debug.h>
#include <vpr/Util/Assert.h>

#include <vpr/md/SIM/Network/NetworkGraph.h>

namespace vpr
{

namespace sim
{

// Crummy helper for crappy C++ I/O.
static void skipToEOL (std::istream& stream)
{
   char c;

   while ( (c = stream.get()) != '\n' )
   {
      /* Loop. */ ;
   }
}

vpr::ReturnStatus NetworkGraph::construct (const std::string& path)
{
   vpr::ReturnStatus status;
   std::ifstream input_file(path.c_str());

   if ( input_file.is_open() )
   {
      vpr::Uint32 node_count, full_edge_count, half_edge_count;
      std::map<int, boost::graph_traits<net_graph_t>::vertex_descriptor> vertex_map;
      std::string temp_str;

      input_file >> node_count;

      for ( vpr::Uint32 i = 0; i < node_count; i++ )
      {
         vpr::Uint32 index;
         vpr::Uint16 node_type;
         std::string node_ip;

         input_file >> index >> node_type >> node_ip;
         skipToEOL(input_file);

         // Now create the vertex and add it to the graph.
         NetworkNode node_prop(index, node_type, node_ip);
         vertex_map[index] = boost::add_vertex(NodeProperty(node_prop),
                                               mGraph);
      }

      input_file >> full_edge_count;

      // The file format tells us that there are twice as many edges as
      // actually listed--probably to indicate bidirectional links.
      half_edge_count = full_edge_count / 2;

      net_edge_t new_edge;
      bool added;
      boost::property_map<net_graph_t, boost::edge_weight_t>::type weight_map;
      weight_map = boost::get(boost::edge_weight_t(), mGraph);

      for ( vpr::Uint32 i = 0; i < full_edge_count; i++ )
      {
         double length, bw;
         vpr::Uint32 from_node, to_node, delay;
         std::string net_type, net_ip;
         vpr::Uint16 net_id;

         input_file >> from_node >> to_node >> length >> delay >> bw
                    >> net_type >> net_id >> net_ip;

         // This is here mainly for debugging this clunky "parser".
         vprDEBUG(vprDBG_ALL, vprDBG_HVERB_LVL)
            << "Loading edge #" << i << ": (" << from_node << " --> "
            << to_node << ", " << length << " miles, " << delay << " us, "
            << bw << " Mbps, type: " << net_type << ", ID: " << net_id << ", "
            << net_ip << ")\n" << vprDEBUG_FLUSH;

         // Now add the edge to the graph.
         NetworkLine line(length, bw, delay, net_type, net_id, net_ip);
         boost::tie(new_edge, added) = boost::add_edge(vertex_map[from_node],
                                                       vertex_map[to_node],
                                                       LineProperty(line),
                                                       mGraph);

         if ( added )
         {
            weight_map[new_edge] = line.getWeight();
         }

         // Sanity check to ensure that we do not get stuck trying to read
         // bytes that aren't there.  This probably slows things down, but I
         // don't know of a better way to do this.
         if ( input_file.peek() == EOF && i + 1 < full_edge_count )
         {
            vprDEBUG(vprDBG_ALL, vprDBG_WARNING_LVL)
               << "WARNING: Expected " << full_edge_count
               << " edges, found " << i + 1 << std::endl << vprDEBUG_FLUSH;
            break;
         }
      }

      mGraphValid = true;
   }
   else
   {
      vprDEBUG(vprDBG_ALL, vprDBG_CRITICAL_LVL)
         << "ERROR: Failed to open graph input file named " << path
         << std::endl << vprDEBUG_FLUSH;
      status.setCode(vpr::ReturnStatus::Fail);
   }

   return status;
}

NetworkLine NetworkGraph::getLineProperty (const NetworkGraph::net_edge_t& e)
   const
{
   // XXX: Need to make an assertion that e is in mGraph!
//   vprASSERT(... && "Given edge not found in the graph!");

   boost::property_map<net_graph_t, network_line_t>::const_type line_prop_map;

   line_prop_map = boost::get(network_line_t(), mGraph);

   return line_prop_map[e];
}

void NetworkGraph::setLineProperty (const NetworkGraph::net_edge_t& e,
                                    const vpr::sim::NetworkLine& prop)
{
   // XXX: Need to make an assertion that e is in mGraph!
//   vprASSERT(... && "Given edge not found in the graph!");

   boost::property_map<net_graph_t, network_line_t>::type line_prop_map;

   line_prop_map    = boost::get(network_line_t(), mGraph);
   line_prop_map[e] = prop;
}

vpr::ReturnStatus NetworkGraph::getNodeWithAddr (const vpr::Uint32 addr,
                                                 NetworkGraph::net_vertex_t& node)
{
   vprASSERT(isValid() && "Trying to use invalid graph");

   vpr::ReturnStatus status(vpr::ReturnStatus::Fail);
   boost::graph_traits<net_graph_t>::vertex_iterator vi, vi_end;
   NetworkNode node_prop;

   boost::tie(vi, vi_end) = boost::vertices(mGraph);

   for ( ; vi != vi_end; vi++ )
   {
      node_prop = getNodeProperty(*vi);

      if ( node_prop.getIpAddress() == addr )
      {
         node = *vi;
         status.setCode(vpr::ReturnStatus::Succeed);
         break;
      }
   }

   return status;
}

NetworkNode NetworkGraph::getNodeProperty (const NetworkGraph::net_vertex_t& v)
   const
{
   // XXX: Need to make an assertion that v is in mGraph!
//   vprASSERT(... && "Given vertex not found in the graph!");

   boost::property_map<net_graph_t, network_node_t>::const_type node_prop_map;

   node_prop_map = boost::get(network_node_t(), mGraph);

   return node_prop_map[v];
}

void NetworkGraph::setNodeProperty (const NetworkGraph::net_vertex_t& v,
                                    const vpr::sim::NetworkNode& prop)
{
   // XXX: Need to make an assertion that v is in mGraph!
//   vprASSERT(... && "Given vertex not found in the graph!");

   boost::property_map<net_graph_t, network_node_t>::type node_prop_map;

   node_prop_map    = boost::get(network_node_t(), mGraph);
   node_prop_map[v] = prop;
}

NetworkGraph::VertexListPtr NetworkGraph::getShortestPath (const NetworkGraph::net_vertex_t& src,
                                                           const NetworkGraph::net_vertex_t& dest)
   const
{
   NetworkGraph::VertexListPtr vlist(new NetworkGraph::VertexList);
   std::vector<int> dist(boost::num_vertices(mGraph));
   std::vector<net_vertex_t> pred(boost::num_vertices(mGraph));
   NetworkGraph::net_vertex_t p(dest), q;
   std::stack<net_vertex_t> vstack;
   boost::property_map<net_graph_t, network_node_t>::const_type node_prop_map;
   boost::property_map<net_graph_t, boost::edge_weight_t>::const_type weight_map;

   node_prop_map = boost::get(network_node_t(), mGraph);
   weight_map    = boost::get(boost::edge_weight_t(), mGraph);

//   boost::dijkstra_shortest_paths(mGraph, src,
//                                  boost::distance_map(&dist[0]).predecessor_map(&pred[0]));
   boost::dijkstra_shortest_paths(mGraph, src, &pred[0], &dist[0], weight_map,
                                  boost::get(boost::vertex_index_t(), mGraph),
                                  std::less<int>(), boost::closed_plus<int>(),
                                  std::numeric_limits<int>::max(), 0,
                                  boost::default_dijkstra_visitor());

   vprDEBUG(vprDBG_ALL, vprDBG_VERB_LVL)
      << "Path (dest <-- src): " << node_prop_map[dest].getIpAddressString()
      << vprDEBUG_FLUSH;
   vstack.push(dest);

   // Put the vertices returned in the predecessor map into a stack so that
   // the order that we can reverse the order and put them into vlist.
   while ( (q = pred[p]) != p )
   {
      vprDEBUG_CONT(vprDBG_ALL, vprDBG_VERB_LVL)
         << " <-- " << node_prop_map[q].getIpAddressString() << vprDEBUG_FLUSH;

      vstack.push(q);
      p = q;
   }

   vprDEBUG_CONT_END(vprDBG_ALL, vprDBG_VERB_LVL) << "\n" << vprDEBUG_FLUSH;
   vprASSERT(p == src && "Destination is unreachable!");

   while ( vstack.size() > 0 )
   {
      vlist->push_back(vstack.top());
      vstack.pop();
   }

   return vlist;
}

NetworkGraph::VertexListPtr NetworkGraph::reversePath (NetworkGraph::VertexListPtr path)
{
   VertexListPtr new_path(new NetworkGraph::VertexList(path->size()));
   NetworkGraph::VertexList::reverse_iterator i;

   for ( i = path->rbegin(); i != path->rend(); i++ )
   {
      new_path->push_back(*i);
   }

   return new_path;
}

} // End of sim namespace

} // End of vpr namespace
<|endoftext|>


<commit_before>#include "cvm.h"

void DabValue::dump(FILE *file) const
{
    static const char *types[] = {"INVA", "FIXN", "BOOL", "NIL ", "CLAS", "OBJE", "ARRY",
                                  "UIN8", "UI16", "UI32", "UI64", "INT8", "IN16", "IN32",
                                  "IN64", "METH", "PTR*", "BYT*", "CSTR", "DSTR"};
    assert((int)data.type >= 0 && (int)data.type < (int)countof(types));
    fprintf(file, "%s ", types[data.type]);
    print(file, true);
}

dab_class_t DabValue::class_index() const
{
    switch (data.type)
    {
    case TYPE_FIXNUM:
        return CLASS_FIXNUM;
        break;
    case TYPE_BOOLEAN:
        return CLASS_BOOLEAN;
        break;
    case TYPE_NIL:
        return CLASS_NILCLASS;
        break;
    case TYPE_ARRAY:
        return CLASS_ARRAY;
        break;
    case TYPE_CLASS:
        return (dab_class_t)data.fixnum;
        break;
    case TYPE_OBJECT:
        return (dab_class_t)this->data.object->object->klass;
        break;
    case TYPE_UINT8:
        return CLASS_UINT8;
        break;
    case TYPE_UINT16:
        return CLASS_UINT16;
        break;
    case TYPE_UINT32:
        return CLASS_UINT32;
        break;
    case TYPE_UINT64:
        return CLASS_UINT64;
        break;
    case TYPE_INT8:
        return CLASS_INT8;
        break;
    case TYPE_INT16:
        return CLASS_INT16;
        break;
    case TYPE_INT32:
        return CLASS_INT32;
        break;
    case TYPE_INT64:
        return CLASS_INT64;
        break;
    case TYPE_METHOD:
        return CLASS_METHOD;
        break;
    case TYPE_INTPTR:
        return CLASS_INTPTR;
        break;
    case TYPE_BYTEBUFFER:
        return CLASS_BYTEBUFFER;
        break;
    case TYPE_LITERALSTRING:
        return CLASS_LITERALSTRING;
        break;
    case TYPE_DYNAMICSTRING:
        return CLASS_DYNAMICSTRING;
        break;
    default:
        fprintf(stderr, "Unknown data.type %d.\n", (int)data.type);
        assert(false);
        break;
    }
}

std::string DabValue::class_name() const
{
    return get_class().name;
}

DabClass &DabValue::get_class() const
{
    return $VM->get_class(class_index());
}

bool DabValue::is_a(const DabClass &klass) const
{
    return get_class().is_subclass_of(klass);
}

void DabValue::print(FILE *out, bool debug) const
{
    fprintf(out, "%s", print_value(debug).c_str());
}

std::string DabValue::print_value(bool debug) const
{
    char        buffer[32] = {0};
    std::string ret;
    bool        use_ret = false;
    switch (data.type)
    {
    case TYPE_FIXNUM:
        snprintf(buffer, sizeof(buffer), "%" PRId64, data.fixnum);
        break;
    case TYPE_UINT8:
        snprintf(buffer, sizeof(buffer), "%" PRIu8, data.num_uint8);
        break;
    case TYPE_UINT16:
        snprintf(buffer, sizeof(buffer), "%" PRIu16, data.num_uint16);
        break;
    case TYPE_UINT32:
        snprintf(buffer, sizeof(buffer), "%" PRIu32, data.num_uint32);
        break;
    case TYPE_UINT64:
        snprintf(buffer, sizeof(buffer), "%" PRIu64, data.num_uint64);
        break;
    case TYPE_INT8:
        snprintf(buffer, sizeof(buffer), "%" PRId8, data.num_int8);
        break;
    case TYPE_INT16:
        snprintf(buffer, sizeof(buffer), "%" PRId16, data.num_int16);
        break;
    case TYPE_INT32:
        snprintf(buffer, sizeof(buffer), "%" PRId32, data.num_int32);
        break;
    case TYPE_INT64:
        snprintf(buffer, sizeof(buffer), "%" PRId64, data.num_int64);
        break;
    case TYPE_BOOLEAN:
        snprintf(buffer, sizeof(buffer), "%s", data.boolean ? "true" : "false");
        break;
    case TYPE_NIL:
        snprintf(buffer, sizeof(buffer), "nil");
        break;
    case TYPE_CLASS:
        snprintf(buffer, sizeof(buffer), "%s", class_name().c_str());
        break;
    case TYPE_OBJECT:
        snprintf(buffer, sizeof(buffer), "#%s", class_name().c_str());
        break;
    case TYPE_INTPTR:
        snprintf(buffer, sizeof(buffer), "%p", data.intptr);
        break;
    case TYPE_ARRAY:
    {
        use_ret  = true;
        ret      = "[";
        size_t i = 0;
        for (auto &item : array())
        {
            if (i)
                ret += ", ";
            ret += item.print_value(debug);
            i++;
        }
        ret += "]";
    }
    break;
    case TYPE_LITERALSTRING:
    case TYPE_DYNAMICSTRING:
    {
        use_ret = true;
        ret     = string();
        if (debug)
        {
            ret = "\"" + ret + "\"";
        }
    }
    break;
    default:
        snprintf(buffer, sizeof(buffer), "?");
        break;
    }
    if (!use_ret)
    {
        ret = buffer;
    }
    if (debug && is_object())
    {
        char extra[64] = {0};
        snprintf(extra, sizeof(extra), " [%d strong]", (int)data.object->count_strong);
        ret += extra;
    }
    return ret;
}

std::vector<DabValue> &DabValue::array() const
{
    assert(data.type == TYPE_ARRAY);
    auto *obj = (DabArray *)data.object->object;
    return obj->array;
}

std::vector<uint8_t> &DabValue::bytebuffer() const
{
    assert(data.type == TYPE_BYTEBUFFER);
    auto *obj = (DabByteBuffer *)data.object->object;
    return obj->bytebuffer;
}

std::string DabValue::literal_string() const
{
    assert(data.type == TYPE_LITERALSTRING);
    auto *obj = (DabLiteralString *)data.object->object;
    return std::string(obj->pointer, obj->length);
}

std::string DabValue::dynamic_string() const
{
    assert(data.type == TYPE_DYNAMICSTRING);
    auto *obj = (DabDynamicString *)data.object->object;
    return obj->value;
}

std::string DabValue::string() const
{
    bool dynamic = data.type == TYPE_DYNAMICSTRING;
    bool literal = data.type == TYPE_LITERALSTRING;
    bool method  = data.type == TYPE_METHOD;
    assert(literal || method || dynamic);
    if (method)
    {
        return $VM->get_symbol(data.fixnum);
    }
    else if (dynamic)
    {
        return dynamic_string();
    }
    else
    {
        return literal_string();
    }
}

bool DabValue::truthy() const
{
    switch (data.type)
    {
    case TYPE_FIXNUM:
        return data.fixnum;
    case TYPE_UINT8:
        return data.num_uint8;
    case TYPE_UINT32:
        return data.num_uint32;
    case TYPE_UINT64:
        return data.num_uint64;
    case TYPE_INT32:
        return data.num_int32;
    case TYPE_BOOLEAN:
        return data.boolean;
    case TYPE_INTPTR:
        return data.intptr;
    case TYPE_NIL:
        return false;
    case TYPE_ARRAY:
        return array().size() > 0;
    case TYPE_LITERALSTRING:
        return literal_string().length();
    case TYPE_DYNAMICSTRING:
        return dynamic_string().length();
    default:
        return true;
    }
}

DabValue DabValue::create_instance() const
{
    assert(data.type == TYPE_CLASS);

    DabBaseObject *object = nullptr;
    auto           type   = TYPE_OBJECT;
    if (data.fixnum == CLASS_ARRAY)
    {
        object = new DabArray;
        type   = TYPE_ARRAY;
    }
    else if (data.fixnum == CLASS_BYTEBUFFER)
    {
        object = new DabByteBuffer;
        type   = TYPE_BYTEBUFFER;
    }
    else if (data.fixnum == CLASS_LITERALSTRING)
    {
        object = new DabLiteralString;
        type   = TYPE_LITERALSTRING;
    }
    else if (data.fixnum == CLASS_DYNAMICSTRING)
    {
        object = new DabDynamicString;
        type   = TYPE_DYNAMICSTRING;
    }
    else
    {
        object = new DabObject;
    }

    DabObjectProxy *proxy = new DabObjectProxy;
    proxy->object         = object;
    proxy->count_strong   = 1;
    proxy->object->klass  = this->data.fixnum;

    DabValue ret;
    ret.data.type   = type;
    ret.data.object = proxy;

    if ($VM->options.verbose)
    {
        fprintf(stderr, "vm: proxy %p A (strong %3d): ! created : ", proxy,
                (int)proxy->count_strong);
        ret.dump(stderr);
        fprintf(stderr, "\n");
    }

    return ret;
}

DabValue DabValue::allocate_dynstr(const char *str)
{
    DabValue klass = $VM->get_class(CLASS_DYNAMICSTRING);

    auto ret = klass.create_instance();

    auto *obj = (DabDynamicString *)ret.data.object->object;

    obj->value = str;
    return ret;
}

DabValue DabValue::_get_instvar(dab_symbol_t symbol)
{
    assert(this->data.type == TYPE_OBJECT);
    assert(this->data.object);

    if (!this->data.object->object)
    {
        return DabValue(nullptr);
    }

    auto  object   = (DabObject *)this->data.object->object;
    auto &instvars = object->instvars;

    if (!instvars.count(symbol))
    {
        return DabValue(nullptr);
    }
    return instvars[symbol];
}

DabValue DabValue::get_instvar(dab_symbol_t symbol)
{
    auto ret = _get_instvar(symbol);
    if ($VM->options.verbose)
    {
        auto name = $VM->get_symbol(symbol);

        fprintf(stderr, "vm: proxy %p (strong %d): Get instvar <%s> -> ", this->data.object,
                (int)this->data.object->count_strong, name.c_str());
        ret.print(stderr);
        fprintf(stderr, "\n");
    }
    return ret;
}

void DabValue::set_instvar(dab_symbol_t symbol, const DabValue &value)
{
    assert(this->data.type == TYPE_OBJECT);
    assert(this->data.object);

    if ($VM->options.verbose)
    {
        auto name = $VM->get_symbol(symbol);

        fprintf(stderr, "vm: proxy %p (strong %d): Set instvar <%s> to ", this->data.object,
                (int)this->data.object->count_strong, name.c_str());
        value.print(stderr);
        fprintf(stderr, "\n");
    }

    if (!this->data.object->object)
    {
        return;
    }

    auto  object     = (DabObject *)this->data.object->object;
    auto &instvars   = object->instvars;
    instvars[symbol] = value;
}

void DabValue::set_data(const DabValueData &other_data)
{
    data = other_data;
    if ($VM->options.autorelease)
    {
        retain();
    }
}

DabValue::DabValue(const DabValue &other)
{
    set_data(other.data);
}

DabValue &DabValue::operator=(const DabValue &other)
{
    set_data(other.data);
    return *this;
}

DabValue::~DabValue()
{
    if ($VM->options.autorelease)
    {
        release();
    }
}

void DabValue::release()
{
    if (is_object())
    {
        this->data.object->release(this);
        this->data.object = nullptr;
    }
    this->data.type        = TYPE_NIL;
    this->data._initialize = 0;
}

void DabValue::retain()
{
    if (is_object())
    {
        data.object->retain(this);
    }
}

//

void DabObjectProxy::retain(DabValue *value)
{
    (void)value;
    if (this->destroying)
        return;
    count_strong += 1;
    if ($VM->options.verbose)
    {
        fprintf(stderr, "vm: proxy %p B (strong %3d): + retained: ", this, (int)this->count_strong);
        value->dump(stderr);
        fprintf(stderr, "\n");
    }
}

void DabObjectProxy::release(DabValue *value)
{
    if (this->destroying)
        return;
    count_strong -= 1;
    if ($VM->options.verbose)
    {
        fprintf(stderr, "vm: proxy %p B (strong %3d): - released: ", this, (int)this->count_strong);
        value->dump(stderr);
        fprintf(stderr, "\n");
    }
    if (count_strong == 0)
    {
        destroy(value);
    }
}

void DabObjectProxy::destroy(DabValue *value)
{
    (void)value;
    this->destroying = true;
    if ($VM->options.verbose)
    {
        fprintf(stderr, "vm: proxy %p C (strong %3d): X destroy\n", this, (int)this->count_strong);
    }
    delete object;
    delete this;
}

size_t DabValue::use_count() const
{
    if (is_object())
    {
        return data.object->count_strong;
    }
    else
    {
        return 65535;
    }
}
<commit_msg>vm: dab_value throws on unknown type instead of failing<commit_after>#include "cvm.h"

void DabValue::dump(FILE *file) const
{
    static const char *types[] = {"INVA", "FIXN", "BOOL", "NIL ", "CLAS", "OBJE", "ARRY",
                                  "UIN8", "UI16", "UI32", "UI64", "INT8", "IN16", "IN32",
                                  "IN64", "METH", "PTR*", "BYT*", "CSTR", "DSTR"};
    assert((int)data.type >= 0 && (int)data.type < (int)countof(types));
    fprintf(file, "%s ", types[data.type]);
    print(file, true);
}

dab_class_t DabValue::class_index() const
{
    switch (data.type)
    {
    case TYPE_FIXNUM:
        return CLASS_FIXNUM;
        break;
    case TYPE_BOOLEAN:
        return CLASS_BOOLEAN;
        break;
    case TYPE_NIL:
        return CLASS_NILCLASS;
        break;
    case TYPE_ARRAY:
        return CLASS_ARRAY;
        break;
    case TYPE_CLASS:
        return (dab_class_t)data.fixnum;
        break;
    case TYPE_OBJECT:
        return (dab_class_t)this->data.object->object->klass;
        break;
    case TYPE_UINT8:
        return CLASS_UINT8;
        break;
    case TYPE_UINT16:
        return CLASS_UINT16;
        break;
    case TYPE_UINT32:
        return CLASS_UINT32;
        break;
    case TYPE_UINT64:
        return CLASS_UINT64;
        break;
    case TYPE_INT8:
        return CLASS_INT8;
        break;
    case TYPE_INT16:
        return CLASS_INT16;
        break;
    case TYPE_INT32:
        return CLASS_INT32;
        break;
    case TYPE_INT64:
        return CLASS_INT64;
        break;
    case TYPE_METHOD:
        return CLASS_METHOD;
        break;
    case TYPE_INTPTR:
        return CLASS_INTPTR;
        break;
    case TYPE_BYTEBUFFER:
        return CLASS_BYTEBUFFER;
        break;
    case TYPE_LITERALSTRING:
        return CLASS_LITERALSTRING;
        break;
    case TYPE_DYNAMICSTRING:
        return CLASS_DYNAMICSTRING;
        break;
    default:
        char description[256];
        sprintf(description, "Unknown data.type %d.\n", (int)data.type);
        throw DabRuntimeError(description);
    }
}

std::string DabValue::class_name() const
{
    return get_class().name;
}

DabClass &DabValue::get_class() const
{
    return $VM->get_class(class_index());
}

bool DabValue::is_a(const DabClass &klass) const
{
    return get_class().is_subclass_of(klass);
}

void DabValue::print(FILE *out, bool debug) const
{
    fprintf(out, "%s", print_value(debug).c_str());
}

std::string DabValue::print_value(bool debug) const
{
    char        buffer[32] = {0};
    std::string ret;
    bool        use_ret = false;
    switch (data.type)
    {
    case TYPE_FIXNUM:
        snprintf(buffer, sizeof(buffer), "%" PRId64, data.fixnum);
        break;
    case TYPE_UINT8:
        snprintf(buffer, sizeof(buffer), "%" PRIu8, data.num_uint8);
        break;
    case TYPE_UINT16:
        snprintf(buffer, sizeof(buffer), "%" PRIu16, data.num_uint16);
        break;
    case TYPE_UINT32:
        snprintf(buffer, sizeof(buffer), "%" PRIu32, data.num_uint32);
        break;
    case TYPE_UINT64:
        snprintf(buffer, sizeof(buffer), "%" PRIu64, data.num_uint64);
        break;
    case TYPE_INT8:
        snprintf(buffer, sizeof(buffer), "%" PRId8, data.num_int8);
        break;
    case TYPE_INT16:
        snprintf(buffer, sizeof(buffer), "%" PRId16, data.num_int16);
        break;
    case TYPE_INT32:
        snprintf(buffer, sizeof(buffer), "%" PRId32, data.num_int32);
        break;
    case TYPE_INT64:
        snprintf(buffer, sizeof(buffer), "%" PRId64, data.num_int64);
        break;
    case TYPE_BOOLEAN:
        snprintf(buffer, sizeof(buffer), "%s", data.boolean ? "true" : "false");
        break;
    case TYPE_NIL:
        snprintf(buffer, sizeof(buffer), "nil");
        break;
    case TYPE_CLASS:
        snprintf(buffer, sizeof(buffer), "%s", class_name().c_str());
        break;
    case TYPE_OBJECT:
        snprintf(buffer, sizeof(buffer), "#%s", class_name().c_str());
        break;
    case TYPE_INTPTR:
        snprintf(buffer, sizeof(buffer), "%p", data.intptr);
        break;
    case TYPE_ARRAY:
    {
        use_ret  = true;
        ret      = "[";
        size_t i = 0;
        for (auto &item : array())
        {
            if (i)
                ret += ", ";
            ret += item.print_value(debug);
            i++;
        }
        ret += "]";
    }
    break;
    case TYPE_LITERALSTRING:
    case TYPE_DYNAMICSTRING:
    {
        use_ret = true;
        ret     = string();
        if (debug)
        {
            ret = "\"" + ret + "\"";
        }
    }
    break;
    default:
        snprintf(buffer, sizeof(buffer), "?");
        break;
    }
    if (!use_ret)
    {
        ret = buffer;
    }
    if (debug && is_object())
    {
        char extra[64] = {0};
        snprintf(extra, sizeof(extra), " [%d strong]", (int)data.object->count_strong);
        ret += extra;
    }
    return ret;
}

std::vector<DabValue> &DabValue::array() const
{
    assert(data.type == TYPE_ARRAY);
    auto *obj = (DabArray *)data.object->object;
    return obj->array;
}

std::vector<uint8_t> &DabValue::bytebuffer() const
{
    assert(data.type == TYPE_BYTEBUFFER);
    auto *obj = (DabByteBuffer *)data.object->object;
    return obj->bytebuffer;
}

std::string DabValue::literal_string() const
{
    assert(data.type == TYPE_LITERALSTRING);
    auto *obj = (DabLiteralString *)data.object->object;
    return std::string(obj->pointer, obj->length);
}

std::string DabValue::dynamic_string() const
{
    assert(data.type == TYPE_DYNAMICSTRING);
    auto *obj = (DabDynamicString *)data.object->object;
    return obj->value;
}

std::string DabValue::string() const
{
    bool dynamic = data.type == TYPE_DYNAMICSTRING;
    bool literal = data.type == TYPE_LITERALSTRING;
    bool method  = data.type == TYPE_METHOD;
    assert(literal || method || dynamic);
    if (method)
    {
        return $VM->get_symbol(data.fixnum);
    }
    else if (dynamic)
    {
        return dynamic_string();
    }
    else
    {
        return literal_string();
    }
}

bool DabValue::truthy() const
{
    switch (data.type)
    {
    case TYPE_FIXNUM:
        return data.fixnum;
    case TYPE_UINT8:
        return data.num_uint8;
    case TYPE_UINT32:
        return data.num_uint32;
    case TYPE_UINT64:
        return data.num_uint64;
    case TYPE_INT32:
        return data.num_int32;
    case TYPE_BOOLEAN:
        return data.boolean;
    case TYPE_INTPTR:
        return data.intptr;
    case TYPE_NIL:
        return false;
    case TYPE_ARRAY:
        return array().size() > 0;
    case TYPE_LITERALSTRING:
        return literal_string().length();
    case TYPE_DYNAMICSTRING:
        return dynamic_string().length();
    default:
        return true;
    }
}

DabValue DabValue::create_instance() const
{
    assert(data.type == TYPE_CLASS);

    DabBaseObject *object = nullptr;
    auto           type   = TYPE_OBJECT;
    if (data.fixnum == CLASS_ARRAY)
    {
        object = new DabArray;
        type   = TYPE_ARRAY;
    }
    else if (data.fixnum == CLASS_BYTEBUFFER)
    {
        object = new DabByteBuffer;
        type   = TYPE_BYTEBUFFER;
    }
    else if (data.fixnum == CLASS_LITERALSTRING)
    {
        object = new DabLiteralString;
        type   = TYPE_LITERALSTRING;
    }
    else if (data.fixnum == CLASS_DYNAMICSTRING)
    {
        object = new DabDynamicString;
        type   = TYPE_DYNAMICSTRING;
    }
    else
    {
        object = new DabObject;
    }

    DabObjectProxy *proxy = new DabObjectProxy;
    proxy->object         = object;
    proxy->count_strong   = 1;
    proxy->object->klass  = this->data.fixnum;

    DabValue ret;
    ret.data.type   = type;
    ret.data.object = proxy;

    if ($VM->options.verbose)
    {
        fprintf(stderr, "vm: proxy %p A (strong %3d): ! created : ", proxy,
                (int)proxy->count_strong);
        ret.dump(stderr);
        fprintf(stderr, "\n");
    }

    return ret;
}

DabValue DabValue::allocate_dynstr(const char *str)
{
    DabValue klass = $VM->get_class(CLASS_DYNAMICSTRING);

    auto ret = klass.create_instance();

    auto *obj = (DabDynamicString *)ret.data.object->object;

    obj->value = str;
    return ret;
}

DabValue DabValue::_get_instvar(dab_symbol_t symbol)
{
    assert(this->data.type == TYPE_OBJECT);
    assert(this->data.object);

    if (!this->data.object->object)
    {
        return DabValue(nullptr);
    }

    auto  object   = (DabObject *)this->data.object->object;
    auto &instvars = object->instvars;

    if (!instvars.count(symbol))
    {
        return DabValue(nullptr);
    }
    return instvars[symbol];
}

DabValue DabValue::get_instvar(dab_symbol_t symbol)
{
    auto ret = _get_instvar(symbol);
    if ($VM->options.verbose)
    {
        auto name = $VM->get_symbol(symbol);

        fprintf(stderr, "vm: proxy %p (strong %d): Get instvar <%s> -> ", this->data.object,
                (int)this->data.object->count_strong, name.c_str());
        ret.print(stderr);
        fprintf(stderr, "\n");
    }
    return ret;
}

void DabValue::set_instvar(dab_symbol_t symbol, const DabValue &value)
{
    assert(this->data.type == TYPE_OBJECT);
    assert(this->data.object);

    if ($VM->options.verbose)
    {
        auto name = $VM->get_symbol(symbol);

        fprintf(stderr, "vm: proxy %p (strong %d): Set instvar <%s> to ", this->data.object,
                (int)this->data.object->count_strong, name.c_str());
        value.print(stderr);
        fprintf(stderr, "\n");
    }

    if (!this->data.object->object)
    {
        return;
    }

    auto  object     = (DabObject *)this->data.object->object;
    auto &instvars   = object->instvars;
    instvars[symbol] = value;
}

void DabValue::set_data(const DabValueData &other_data)
{
    data = other_data;
    if ($VM->options.autorelease)
    {
        retain();
    }
}

DabValue::DabValue(const DabValue &other)
{
    set_data(other.data);
}

DabValue &DabValue::operator=(const DabValue &other)
{
    set_data(other.data);
    return *this;
}

DabValue::~DabValue()
{
    if ($VM->options.autorelease)
    {
        release();
    }
}

void DabValue::release()
{
    if (is_object())
    {
        this->data.object->release(this);
        this->data.object = nullptr;
    }
    this->data.type        = TYPE_NIL;
    this->data._initialize = 0;
}

void DabValue::retain()
{
    if (is_object())
    {
        data.object->retain(this);
    }
}

//

void DabObjectProxy::retain(DabValue *value)
{
    (void)value;
    if (this->destroying)
        return;
    count_strong += 1;
    if ($VM->options.verbose)
    {
        fprintf(stderr, "vm: proxy %p B (strong %3d): + retained: ", this, (int)this->count_strong);
        value->dump(stderr);
        fprintf(stderr, "\n");
    }
}

void DabObjectProxy::release(DabValue *value)
{
    if (this->destroying)
        return;
    count_strong -= 1;
    if ($VM->options.verbose)
    {
        fprintf(stderr, "vm: proxy %p B (strong %3d): - released: ", this, (int)this->count_strong);
        value->dump(stderr);
        fprintf(stderr, "\n");
    }
    if (count_strong == 0)
    {
        destroy(value);
    }
}

void DabObjectProxy::destroy(DabValue *value)
{
    (void)value;
    this->destroying = true;
    if ($VM->options.verbose)
    {
        fprintf(stderr, "vm: proxy %p C (strong %3d): X destroy\n", this, (int)this->count_strong);
    }
    delete object;
    delete this;
}

size_t DabValue::use_count() const
{
    if (is_object())
    {
        return data.object->count_strong;
    }
    else
    {
        return 65535;
    }
}
<|endoftext|>


<commit_before>/*************** <auto-copyright.pl BEGIN do not edit this line> **************
 *
 * VR Juggler is (C) Copyright 1998-2010 by Iowa State University
 *
 * Original Authors:
 *   Allen Bierbaum, Christopher Just,
 *   Patrick Hartling, Kevin Meinert,
 *   Carolina Cruz-Neira, Albert Baker
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library 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
 * Library General Public License for more details.
 *
 * You should have received a copy of the GNU Library General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 *
 *************** <auto-copyright.pl END do not edit this line> ***************/

#include <vrj/vrjConfig.h>

#include <string>
#include <gmtl/Math.h>
#include <gmtl/Vec.h>
#include <gmtl/Matrix.h>
#include <gmtl/MatrixOps.h>
#include <gmtl/Generate.h>
#include <gmtl/Xforms.h>

#include <jccl/Config/ConfigElement.h>
#include <gadget/Type/Position/PositionUnitConversion.h>

#include <vrj/Kernel/User.h>
#include <vrj/Util/Debug.h>
#include <vrj/Display/SurfaceProjection.h>
#include <vrj/Display/TrackedSurfaceProjection.h>
#include <vrj/Display/DisplayExceptions.h>
#include <vrj/Display/SurfaceViewport.h>

namespace vrj
{

SurfaceViewport::SurfaceViewport()
   : Viewport()
   , mTracked(false)
{
   /* Do nothing. */ ;
}

ViewportPtr SurfaceViewport::create()
{
   return ViewportPtr(new SurfaceViewport());
}

SurfaceViewport::~SurfaceViewport()
{
   /* Do nothing. */ ;
}

bool SurfaceViewport::config(jccl::ConfigElementPtr element)
{
   vprASSERT(element.get() != NULL);
   vprASSERT(element->getID() == "surface_viewport");

   // Call base class config
   if ( ! Viewport::config(element) )
   {
      return false;
   }

   bool result(true);

   mType = SURFACE;

   // Read in the corners
   mLLCorner.set(element->getProperty<float>("lower_left_corner", 0),
                 element->getProperty<float>("lower_left_corner", 1),
                 element->getProperty<float>("lower_left_corner", 2));
   mLRCorner.set(element->getProperty<float>("lower_right_corner", 0),
                 element->getProperty<float>("lower_right_corner", 1),
                 element->getProperty<float>("lower_right_corner", 2));
   mURCorner.set(element->getProperty<float>("upper_right_corner", 0),
                 element->getProperty<float>("upper_right_corner", 1),
                 element->getProperty<float>("upper_right_corner", 2));
   mULCorner.set(element->getProperty<float>("upper_left_corner", 0),
                 element->getProperty<float>("upper_left_corner", 1),
                 element->getProperty<float>("upper_left_corner", 2));

   // Calculate the rotation and the pts
//   calculateSurfaceRotation();
//   calculateCornersInBaseFrame();

   // Get info about being tracked
   mTracked = element->getProperty<bool>("tracked");
   if(mTracked)
   {
      mTrackerProxyName = element->getProperty<std::string>("tracker_proxy");
   }

   // Create Projection objects
   // NOTE: The -'s are because we are measuring distance to
   //  the left(bottom) which is opposite the normal axis direction
   //vjMatrix rot_inv;
   //rot_inv.invert(mSurfaceRotation);
   SurfaceProjectionPtr left_proj;
   SurfaceProjectionPtr right_proj;

   if(!mTracked)
   {
      left_proj = SurfaceProjection::create(mLLCorner, mLRCorner, mURCorner,
                                            mULCorner);
      right_proj = SurfaceProjection::create(mLLCorner, mLRCorner, mURCorner,
                                             mULCorner);
   }
   else
   {
      left_proj = TrackedSurfaceProjection::create(mLLCorner, mLRCorner,
                                                   mURCorner, mULCorner,
                                                   mTrackerProxyName);
      right_proj = TrackedSurfaceProjection::create(mLLCorner, mLRCorner,
                                                    mURCorner, mULCorner,
                                                    mTrackerProxyName);
   }

   try
   {
      left_proj->validateCorners();
      right_proj->validateCorners();

      // NOTE: Even if the corner validation above failed, we still proceed with
      // setting up mLeftProj and mRightProj. This is because other code is not
      // written to handle the case of a viewport having no projections. This
      // could definitely be improved.
      mLeftProj  = left_proj;
      mRightProj = right_proj;

      // Configure the projections
      mLeftProj->config(element);
      mLeftProj->setEye(Projection::LEFT);
      mLeftProj->setViewport(shared_from_this());

      mRightProj->config(element);
      mRightProj->setEye(Projection::RIGHT);
      mRightProj->setViewport(shared_from_this());
   }
   catch (InvalidSurfaceException& ex)
   {
      vprDEBUG(vrjDBG_DISP_MGR, vprDBG_CRITICAL_LVL)
         << clrOutBOLD(clrRED, "ERROR")
         << ": The surface defined by the viewport named\n" << vprDEBUG_FLUSH;
      vprDEBUG_NEXT(vrjDBG_DISP_MGR, vprDBG_CRITICAL_LVL)
         << "       '" << element->getName() << "' is invalid!\n"
         << vprDEBUG_FLUSH;
      vprDEBUG_NEXT(vrjDBG_DISP_MGR, vprDBG_CRITICAL_LVL)
         << ex.what() << std::endl << vprDEBUG_FLUSH;

      result = false;
   }

   return result;
}

void SurfaceViewport::updateProjections(const float positionScale)
{
   // -- Calculate Eye Positions -- //
   const gmtl::Matrix44f cur_head_pos(
      mUser->getHeadPosProxy()->getData(positionScale)
   );
   /*
   Coord  head_coord(cur_head_pos);       // Create a user readable version

   vprDEBUG(vprDBG_ALL, vprDBG_HVERB_LVL)
      << "vjDisplay::updateProjections: Getting head position" << std::endl
      << vprDEBUG_FLUSH;
   vprDEBUG(vprDBG_ALL, vprDBG_HVERB_LVL) << "\tHeadPos:" << head_coord.pos << "\tHeadOr:"
                        << head_coord.orient << std::endl << vprDEBUG_FLUSH;
                        */

   // Compute location of left and right eyes
   //float interocularDist = 2.75f/12.0f;
   float interocular_dist = mUser->getInterocularDistance();
   interocular_dist *= positionScale;              // Scale eye separation

   // Distance to move eye.
   const float eye_offset(interocular_dist * 0.5f);

   // NOTE: Eye coord system is -z forward, x-right, y-up

   if (Viewport::LEFT_EYE == mView || Viewport::STEREO == mView)
   {
      const gmtl::Matrix44f left_eye_pos(
         cur_head_pos *
            gmtl::makeTrans<gmtl::Matrix44f>(gmtl::Vec3f(-eye_offset, 0, 0))
      );
      mLeftProj->calcViewMatrix(left_eye_pos, positionScale);
   }

   if (Viewport::RIGHT_EYE == mView || Viewport::STEREO == mView)
   {
      const gmtl::Matrix44f right_eye_pos(
         cur_head_pos *
            gmtl::makeTrans<gmtl::Matrix44f>(gmtl::Vec3f(eye_offset, 0, 0))
      );
      mRightProj->calcViewMatrix(right_eye_pos, positionScale);
   }
}

std::ostream& SurfaceViewport::outStream(std::ostream& out,
                                         const unsigned int indentLevel)
{
   Viewport::outStream(out, indentLevel);
   out << std::endl;

   const std::string indent_text(indentLevel, ' ');

   /*
   out << "LL: " << mLLCorner << ", LR: " << mLRCorner
       << ", UR: " << mURCorner << ", UL:" << mULCorner << std::endl;
   out << "surfRot: \n" << mSurfaceRotation << std::endl;
   */
   if ( mView == vrj::Viewport::LEFT_EYE || mView == vrj::Viewport::STEREO )
   {
      out << indent_text << "Left projection:\n";
      mLeftProj->outStream(out, indentLevel + 2);
      out << std::endl;
   }
   if ( mView == vrj::Viewport::RIGHT_EYE || mView == vrj::Viewport::STEREO )
   {
      out << indent_text << "Right projection:\n";
      mRightProj->outStream(out, indentLevel + 2);
      out << std::endl;
   }

   return out;
}

}
<commit_msg>Removed code that was commented out 8 to 9 years ago.<commit_after>/*************** <auto-copyright.pl BEGIN do not edit this line> **************
 *
 * VR Juggler is (C) Copyright 1998-2010 by Iowa State University
 *
 * Original Authors:
 *   Allen Bierbaum, Christopher Just,
 *   Patrick Hartling, Kevin Meinert,
 *   Carolina Cruz-Neira, Albert Baker
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library 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
 * Library General Public License for more details.
 *
 * You should have received a copy of the GNU Library General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 *
 *************** <auto-copyright.pl END do not edit this line> ***************/

#include <vrj/vrjConfig.h>

#include <string>
#include <gmtl/Math.h>
#include <gmtl/Vec.h>
#include <gmtl/Matrix.h>
#include <gmtl/MatrixOps.h>
#include <gmtl/Generate.h>
#include <gmtl/Xforms.h>

#include <jccl/Config/ConfigElement.h>
#include <gadget/Type/Position/PositionUnitConversion.h>

#include <vrj/Kernel/User.h>
#include <vrj/Util/Debug.h>
#include <vrj/Display/SurfaceProjection.h>
#include <vrj/Display/TrackedSurfaceProjection.h>
#include <vrj/Display/DisplayExceptions.h>
#include <vrj/Display/SurfaceViewport.h>

namespace vrj
{

SurfaceViewport::SurfaceViewport()
   : Viewport()
   , mTracked(false)
{
   /* Do nothing. */ ;
}

ViewportPtr SurfaceViewport::create()
{
   return ViewportPtr(new SurfaceViewport());
}

SurfaceViewport::~SurfaceViewport()
{
   /* Do nothing. */ ;
}

bool SurfaceViewport::config(jccl::ConfigElementPtr element)
{
   vprASSERT(element.get() != NULL);
   vprASSERT(element->getID() == "surface_viewport");

   // Call base class config
   if ( ! Viewport::config(element) )
   {
      return false;
   }

   bool result(true);

   mType = SURFACE;

   // Read in the corners
   mLLCorner.set(element->getProperty<float>("lower_left_corner", 0),
                 element->getProperty<float>("lower_left_corner", 1),
                 element->getProperty<float>("lower_left_corner", 2));
   mLRCorner.set(element->getProperty<float>("lower_right_corner", 0),
                 element->getProperty<float>("lower_right_corner", 1),
                 element->getProperty<float>("lower_right_corner", 2));
   mURCorner.set(element->getProperty<float>("upper_right_corner", 0),
                 element->getProperty<float>("upper_right_corner", 1),
                 element->getProperty<float>("upper_right_corner", 2));
   mULCorner.set(element->getProperty<float>("upper_left_corner", 0),
                 element->getProperty<float>("upper_left_corner", 1),
                 element->getProperty<float>("upper_left_corner", 2));

   // Calculate the rotation and the pts
//   calculateSurfaceRotation();
//   calculateCornersInBaseFrame();

   // Get info about being tracked
   mTracked = element->getProperty<bool>("tracked");
   if(mTracked)
   {
      mTrackerProxyName = element->getProperty<std::string>("tracker_proxy");
   }

   // Create Projection objects
   // NOTE: The -'s are because we are measuring distance to
   //  the left(bottom) which is opposite the normal axis direction
   //vjMatrix rot_inv;
   //rot_inv.invert(mSurfaceRotation);
   SurfaceProjectionPtr left_proj;
   SurfaceProjectionPtr right_proj;

   if(!mTracked)
   {
      left_proj = SurfaceProjection::create(mLLCorner, mLRCorner, mURCorner,
                                            mULCorner);
      right_proj = SurfaceProjection::create(mLLCorner, mLRCorner, mURCorner,
                                             mULCorner);
   }
   else
   {
      left_proj = TrackedSurfaceProjection::create(mLLCorner, mLRCorner,
                                                   mURCorner, mULCorner,
                                                   mTrackerProxyName);
      right_proj = TrackedSurfaceProjection::create(mLLCorner, mLRCorner,
                                                    mURCorner, mULCorner,
                                                    mTrackerProxyName);
   }

   try
   {
      left_proj->validateCorners();
      right_proj->validateCorners();

      // NOTE: Even if the corner validation above failed, we still proceed with
      // setting up mLeftProj and mRightProj. This is because other code is not
      // written to handle the case of a viewport having no projections. This
      // could definitely be improved.
      mLeftProj  = left_proj;
      mRightProj = right_proj;

      // Configure the projections
      mLeftProj->config(element);
      mLeftProj->setEye(Projection::LEFT);
      mLeftProj->setViewport(shared_from_this());

      mRightProj->config(element);
      mRightProj->setEye(Projection::RIGHT);
      mRightProj->setViewport(shared_from_this());
   }
   catch (InvalidSurfaceException& ex)
   {
      vprDEBUG(vrjDBG_DISP_MGR, vprDBG_CRITICAL_LVL)
         << clrOutBOLD(clrRED, "ERROR")
         << ": The surface defined by the viewport named\n" << vprDEBUG_FLUSH;
      vprDEBUG_NEXT(vrjDBG_DISP_MGR, vprDBG_CRITICAL_LVL)
         << "       '" << element->getName() << "' is invalid!\n"
         << vprDEBUG_FLUSH;
      vprDEBUG_NEXT(vrjDBG_DISP_MGR, vprDBG_CRITICAL_LVL)
         << ex.what() << std::endl << vprDEBUG_FLUSH;

      result = false;
   }

   return result;
}

void SurfaceViewport::updateProjections(const float positionScale)
{
   // -- Calculate Eye Positions -- //
   const gmtl::Matrix44f cur_head_pos(
      mUser->getHeadPosProxy()->getData(positionScale)
   );

   // Compute location of left and right eyes
   float interocular_dist = mUser->getInterocularDistance();
   interocular_dist *= positionScale;              // Scale eye separation

   // Distance to move eye.
   const float eye_offset(interocular_dist * 0.5f);

   // NOTE: Eye coord system is -z forward, x-right, y-up

   if (Viewport::LEFT_EYE == mView || Viewport::STEREO == mView)
   {
      const gmtl::Matrix44f left_eye_pos(
         cur_head_pos *
            gmtl::makeTrans<gmtl::Matrix44f>(gmtl::Vec3f(-eye_offset, 0, 0))
      );
      mLeftProj->calcViewMatrix(left_eye_pos, positionScale);
   }

   if (Viewport::RIGHT_EYE == mView || Viewport::STEREO == mView)
   {
      const gmtl::Matrix44f right_eye_pos(
         cur_head_pos *
            gmtl::makeTrans<gmtl::Matrix44f>(gmtl::Vec3f(eye_offset, 0, 0))
      );
      mRightProj->calcViewMatrix(right_eye_pos, positionScale);
   }
}

std::ostream& SurfaceViewport::outStream(std::ostream& out,
                                         const unsigned int indentLevel)
{
   Viewport::outStream(out, indentLevel);
   out << std::endl;

   const std::string indent_text(indentLevel, ' ');

   /*
   out << "LL: " << mLLCorner << ", LR: " << mLRCorner
       << ", UR: " << mURCorner << ", UL:" << mULCorner << std::endl;
   out << "surfRot: \n" << mSurfaceRotation << std::endl;
   */
   if ( mView == vrj::Viewport::LEFT_EYE || mView == vrj::Viewport::STEREO )
   {
      out << indent_text << "Left projection:\n";
      mLeftProj->outStream(out, indentLevel + 2);
      out << std::endl;
   }
   if ( mView == vrj::Viewport::RIGHT_EYE || mView == vrj::Viewport::STEREO )
   {
      out << indent_text << "Right projection:\n";
      mRightProj->outStream(out, indentLevel + 2);
      out << std::endl;
   }

   return out;
}

}
<|endoftext|>


<commit_before>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2011, Willow Garage, Inc.
*  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 Willow Garage 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.
*********************************************************************/

/* Author: Ioan Sucan, Sachin Chitta */

#include "ompl_interface/ompl_interface.h"
#include <planning_models/conversions.h>
#include <ompl/tools/debug/Profiler.h>

ompl_interface::OMPLInterface::OMPLInterface(const planning_models::KinematicModelConstPtr &kmodel) :
  kmodel_(kmodel), context_manager_(kmodel), constraints_library_(context_manager_), use_constraints_approximations_(true)
{
}

ompl_interface::OMPLInterface::~OMPLInterface(void)
{
}

ompl_interface::ModelBasedPlanningContextPtr ompl_interface::OMPLInterface::prepareForSolve(const moveit_msgs::MotionPlanRequest &req,
                                                                                const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                                                moveit_msgs::MoveItErrorCodes &error_code,
                                                                                unsigned int &attempts, double &timeout) const
{
  ModelBasedPlanningContextPtr context = getPlanningContext(req);
  if (!context)
  {
    error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
    return context;
  }

  timeout = req.allowed_planning_time.toSec();
  if (timeout <= 0.0)
  {
    error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ALLOWED_PLANNING_TIME;
    ROS_INFO("The timeout for planning must be positive (%lf specified). Assuming one second instead.", timeout);
    timeout = 1.0;
  }

  attempts = 1;
  if (req.num_planning_attempts > 0)
    attempts = req.num_planning_attempts;
  else
    if (req.num_planning_attempts < 0)
      ROS_ERROR("The number of desired planning attempts should be positive. Assuming one attempt.");

  if (context)
  {
    context->clear();

    // get the starting state
    context->setPlanningScene(planning_scene);
    planning_models::KinematicState start_state = planning_scene->getCurrentState();
    planning_models::robotStateToKinematicState(*planning_scene->getTransforms(), req.start_state, start_state);
    context->setStartState(start_state);
    context->setPlanningVolume(req.workspace_parameters);
    if (!context->setPathConstraints(req.path_constraints, &error_code))
      return ModelBasedPlanningContextPtr();
    if (!context->setGoalConstraints(req.goal_constraints, req.path_constraints, &error_code))
      return ModelBasedPlanningContextPtr();
    context->configure();
    ROS_DEBUG("%s: New planning context is set.", context->getName().c_str());
    error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
  }

  return context;
}

bool ompl_interface::OMPLInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                          const moveit_msgs::GetMotionPlan::Request &req, moveit_msgs::GetMotionPlan::Response &res) const
{
  ompl::tools::Profiler::ScopedStart pslv;
  ot::Profiler::ScopedBlock sblock("OMPLInterfaceSolve");

  unsigned int attempts = 1;
  double timeout = 0.0;
  ModelBasedPlanningContextPtr context = prepareForSolve(req.motion_plan_request, planning_scene, res.error_code, attempts, timeout);
  if (!context)
    return false;

  if (context->solve(timeout, attempts))
  {
    double ptime = context->getLastPlanTime();
    if (ptime < timeout)
    {
      context->simplifySolution(timeout - ptime);
      ptime += context->getLastSimplifyTime();
    }
    context->interpolateSolution();

    // fill the response
    ROS_DEBUG("%s: Returning successful solution with %lu states", context->getName().c_str(),
	      context->getOMPLSimpleSetup().getSolutionPath().getStateCount());

    pm::kinematicStateToRobotState(context->getCompleteInitialRobotState(), res.trajectory_start);
    res.planning_time = ros::Duration(ptime);
    context->getSolutionPath(res.trajectory);

    return true;
  }
  else
  {
    ROS_INFO("Unable to solve the planning problem");
    return false;
  }
}

bool ompl_interface::OMPLInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
					  const moveit_msgs::GetMotionPlan::Request &req, moveit_msgs::MotionPlanDetailedResponse &res) const
{
  ompl::tools::Profiler::ScopedStart pslv;
  ot::Profiler::ScopedBlock sblock("OMPLInterfaceSolve");

  unsigned int attempts = 1;
  double timeout = 0.0;
  moveit_msgs::MoveItErrorCodes error_code;
  ModelBasedPlanningContextPtr context = prepareForSolve(req.motion_plan_request, planning_scene, error_code, attempts, timeout);
  if (!context)
    return false;

  if (context->solve(timeout, attempts))
  {
    res.trajectory.reserve(3);
    res.trajectory.resize(1);

    pm::kinematicStateToRobotState(context->getCompleteInitialRobotState(), res.trajectory_start);

    res.processing_time.push_back(ros::Duration(context->getLastPlanTime()));
    res.description.push_back("plan");
    context->getSolutionPath(res.trajectory[0]);

    double ptime = context->getLastPlanTime();
    if (ptime < timeout)
    {
      context->simplifySolution(timeout - ptime);
      res.trajectory.resize(2);
      res.processing_time.push_back(ros::Duration(context->getLastSimplifyTime()));
      res.description.push_back("simplify");
      context->getSolutionPath(res.trajectory[1]);
    }

    ros::WallTime start_interpolate = ros::WallTime::now();
    res.trajectory.resize(res.trajectory.size() + 1);
    context->interpolateSolution();
    res.processing_time.push_back(ros::Duration((ros::WallTime::now() - start_interpolate).toSec()));
    res.description.push_back("interpolate");
    context->getSolutionPath(res.trajectory.back());

    // fill the response
    ROS_DEBUG("%s: Returning successful solution with %lu states", context->getName().c_str(),
	      context->getOMPLSimpleSetup().getSolutionPath().getStateCount());
    return true;
  }
  else
  {
    ROS_INFO("Unable to solve the planning problem");
    return false;
  }
}

bool ompl_interface::OMPLInterface::benchmark(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                              const moveit_msgs::ComputePlanningBenchmark::Request &req,
                                              moveit_msgs::ComputePlanningBenchmark::Response &res) const
{
  unsigned int attempts = 1;
  double timeout = 0.0;
  ModelBasedPlanningContextPtr context = prepareForSolve(req.motion_plan_request, planning_scene, res.error_code, attempts, timeout);
  if (!context)
    return false;
  return context->benchmark(timeout, std::max(1u, req.average_count), req.filename);
}

ompl::base::PathPtr ompl_interface::OMPLInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                         const std::string &config, const planning_models::KinematicState &start_state,
                                                         const moveit_msgs::Constraints &goal_constraints, double timeout,
                                                         const std::string &factory_type) const
{
  moveit_msgs::Constraints empty;
  return solve(planning_scene, config, start_state, goal_constraints, empty, timeout, factory_type);
}

ompl::base::PathPtr ompl_interface::OMPLInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                         const std::string &config, const planning_models::KinematicState &start_state,
                                                         const moveit_msgs::Constraints &goal_constraints,
                                                         const moveit_msgs::Constraints &path_constraints, double timeout,
                                                         const std::string &factory_type) const
{
  ompl::tools::Profiler::ScopedStart pslv;

  ModelBasedPlanningContextPtr context = getPlanningContext(config, factory_type);
  if (!context)
    return ob::PathPtr();

  std::vector<moveit_msgs::Constraints> goal_constraints_v(1, goal_constraints);
  context->setPlanningScene(planning_scene);
  context->setStartState(start_state);
  context->setPathConstraints(path_constraints, NULL);
  context->setGoalConstraints(goal_constraints_v, path_constraints, NULL);
  context->configure();

  // solve the planning problem
  if (context->solve(timeout, 1))
  {
    double ptime = context->getLastPlanTime();
    if (ptime < timeout)
      context->simplifySolution(timeout - ptime);
    context->interpolateSolution();
    return ob::PathPtr(new og::PathGeometric(context->getOMPLSimpleSetup().getSolutionPath()));
  }

  return ob::PathPtr();
}

void ompl_interface::OMPLInterface::terminateSolve(void)
{
  const ModelBasedPlanningContextPtr &context = getLastPlanningContext();
  if (context)
    context->terminateSolve();
}
<commit_msg>minor update needed for benchmark message change<commit_after>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2011, Willow Garage, Inc.
*  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 Willow Garage 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.
*********************************************************************/

/* Author: Ioan Sucan, Sachin Chitta */

#include "ompl_interface/ompl_interface.h"
#include <planning_models/conversions.h>
#include <ompl/tools/debug/Profiler.h>

ompl_interface::OMPLInterface::OMPLInterface(const planning_models::KinematicModelConstPtr &kmodel) :
  kmodel_(kmodel), context_manager_(kmodel), constraints_library_(context_manager_), use_constraints_approximations_(true)
{
}

ompl_interface::OMPLInterface::~OMPLInterface(void)
{
}

ompl_interface::ModelBasedPlanningContextPtr ompl_interface::OMPLInterface::prepareForSolve(const moveit_msgs::MotionPlanRequest &req,
                                                                                const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                                                moveit_msgs::MoveItErrorCodes &error_code,
                                                                                unsigned int &attempts, double &timeout) const
{
  ModelBasedPlanningContextPtr context = getPlanningContext(req);
  if (!context)
  {
    error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
    return context;
  }

  timeout = req.allowed_planning_time.toSec();
  if (timeout <= 0.0)
  {
    error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ALLOWED_PLANNING_TIME;
    ROS_INFO("The timeout for planning must be positive (%lf specified). Assuming one second instead.", timeout);
    timeout = 1.0;
  }

  attempts = 1;
  if (req.num_planning_attempts > 0)
    attempts = req.num_planning_attempts;
  else
    if (req.num_planning_attempts < 0)
      ROS_ERROR("The number of desired planning attempts should be positive. Assuming one attempt.");

  if (context)
  {
    context->clear();

    // get the starting state
    context->setPlanningScene(planning_scene);
    planning_models::KinematicState start_state = planning_scene->getCurrentState();
    planning_models::robotStateToKinematicState(*planning_scene->getTransforms(), req.start_state, start_state);
    context->setStartState(start_state);
    context->setPlanningVolume(req.workspace_parameters);
    if (!context->setPathConstraints(req.path_constraints, &error_code))
      return ModelBasedPlanningContextPtr();
    if (!context->setGoalConstraints(req.goal_constraints, req.path_constraints, &error_code))
      return ModelBasedPlanningContextPtr();
    context->configure();
    ROS_DEBUG("%s: New planning context is set.", context->getName().c_str());
    error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
  }

  return context;
}

bool ompl_interface::OMPLInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                          const moveit_msgs::GetMotionPlan::Request &req, moveit_msgs::GetMotionPlan::Response &res) const
{
  ompl::tools::Profiler::ScopedStart pslv;
  ot::Profiler::ScopedBlock sblock("OMPLInterfaceSolve");

  unsigned int attempts = 1;
  double timeout = 0.0;
  ModelBasedPlanningContextPtr context = prepareForSolve(req.motion_plan_request, planning_scene, res.error_code, attempts, timeout);
  if (!context)
    return false;

  if (context->solve(timeout, attempts))
  {
    double ptime = context->getLastPlanTime();
    if (ptime < timeout)
    {
      context->simplifySolution(timeout - ptime);
      ptime += context->getLastSimplifyTime();
    }
    context->interpolateSolution();

    // fill the response
    ROS_DEBUG("%s: Returning successful solution with %lu states", context->getName().c_str(),
	      context->getOMPLSimpleSetup().getSolutionPath().getStateCount());

    pm::kinematicStateToRobotState(context->getCompleteInitialRobotState(), res.trajectory_start);
    res.planning_time = ros::Duration(ptime);
    context->getSolutionPath(res.trajectory);

    return true;
  }
  else
  {
    ROS_INFO("Unable to solve the planning problem");
    return false;
  }
}

bool ompl_interface::OMPLInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
					  const moveit_msgs::GetMotionPlan::Request &req, moveit_msgs::MotionPlanDetailedResponse &res) const
{
  ompl::tools::Profiler::ScopedStart pslv;
  ot::Profiler::ScopedBlock sblock("OMPLInterfaceSolve");

  unsigned int attempts = 1;
  double timeout = 0.0;
  moveit_msgs::MoveItErrorCodes error_code;
  ModelBasedPlanningContextPtr context = prepareForSolve(req.motion_plan_request, planning_scene, error_code, attempts, timeout);
  if (!context)
    return false;

  if (context->solve(timeout, attempts))
  {
    res.trajectory.reserve(3);
    res.trajectory.resize(1);

    pm::kinematicStateToRobotState(context->getCompleteInitialRobotState(), res.trajectory_start);

    res.processing_time.push_back(ros::Duration(context->getLastPlanTime()));
    res.description.push_back("plan");
    context->getSolutionPath(res.trajectory[0]);

    double ptime = context->getLastPlanTime();
    if (ptime < timeout)
    {
      context->simplifySolution(timeout - ptime);
      res.trajectory.resize(2);
      res.processing_time.push_back(ros::Duration(context->getLastSimplifyTime()));
      res.description.push_back("simplify");
      context->getSolutionPath(res.trajectory[1]);
    }

    ros::WallTime start_interpolate = ros::WallTime::now();
    res.trajectory.resize(res.trajectory.size() + 1);
    context->interpolateSolution();
    res.processing_time.push_back(ros::Duration((ros::WallTime::now() - start_interpolate).toSec()));
    res.description.push_back("interpolate");
    context->getSolutionPath(res.trajectory.back());

    // fill the response
    ROS_DEBUG("%s: Returning successful solution with %lu states", context->getName().c_str(),
	      context->getOMPLSimpleSetup().getSolutionPath().getStateCount());
    return true;
  }
  else
  {
    ROS_INFO("Unable to solve the planning problem");
    return false;
  }
}

bool ompl_interface::OMPLInterface::benchmark(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                              const moveit_msgs::ComputePlanningBenchmark::Request &req,
                                              moveit_msgs::ComputePlanningBenchmark::Response &res) const
{
  unsigned int attempts = 1;
  double timeout = 0.0;
  ModelBasedPlanningContextPtr context = prepareForSolve(req.motion_plan_request, planning_scene, res.error_code, attempts, timeout);
  if (!context)
    return false;
  return context->benchmark(timeout, attempts, req.filename);
}

ompl::base::PathPtr ompl_interface::OMPLInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                         const std::string &config, const planning_models::KinematicState &start_state,
                                                         const moveit_msgs::Constraints &goal_constraints, double timeout,
                                                         const std::string &factory_type) const
{
  moveit_msgs::Constraints empty;
  return solve(planning_scene, config, start_state, goal_constraints, empty, timeout, factory_type);
}

ompl::base::PathPtr ompl_interface::OMPLInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                         const std::string &config, const planning_models::KinematicState &start_state,
                                                         const moveit_msgs::Constraints &goal_constraints,
                                                         const moveit_msgs::Constraints &path_constraints, double timeout,
                                                         const std::string &factory_type) const
{
  ompl::tools::Profiler::ScopedStart pslv;

  ModelBasedPlanningContextPtr context = getPlanningContext(config, factory_type);
  if (!context)
    return ob::PathPtr();

  std::vector<moveit_msgs::Constraints> goal_constraints_v(1, goal_constraints);
  context->setPlanningScene(planning_scene);
  context->setStartState(start_state);
  context->setPathConstraints(path_constraints, NULL);
  context->setGoalConstraints(goal_constraints_v, path_constraints, NULL);
  context->configure();

  // solve the planning problem
  if (context->solve(timeout, 1))
  {
    double ptime = context->getLastPlanTime();
    if (ptime < timeout)
      context->simplifySolution(timeout - ptime);
    context->interpolateSolution();
    return ob::PathPtr(new og::PathGeometric(context->getOMPLSimpleSetup().getSolutionPath()));
  }

  return ob::PathPtr();
}

void ompl_interface::OMPLInterface::terminateSolve(void)
{
  const ModelBasedPlanningContextPtr &context = getLastPlanningContext();
  if (context)
    context->terminateSolve();
}
<|endoftext|>


<commit_before>/*
 * DynApproxBetweenness.cpp
 *
 *  Created on: 31.07.2014
 *      Author: ebergamini
 */

#include "DynApproxBetweenness.h"
#include "../auxiliary/Random.h"
#include "../distance/Diameter.h"
#include "../graph/Sampling.h"
#include "../graph/DynDijkstra.h"
#include "../graph/DynBFS.h"
#include "../auxiliary/Log.h"
#include "../auxiliary/NumericTools.h"

namespace NetworKit {

DynApproxBetweenness::DynApproxBetweenness(const Graph& G, double epsilon, double delta, bool storePredecessors) : Centrality(G, true),
storePreds(storePredecessors), epsilon(epsilon), delta(delta) {
  INFO("Constructing DynApproxBetweenness. storePredecessors = ", storePredecessors);
}

count DynApproxBetweenness::getNumberOfSamples() {
    return r;
}

void DynApproxBetweenness::run() {
  INFO("Inside DynApproxBetweenness. storePreds = ", storePreds);
    if (G.isDirected()) {
        throw std::runtime_error("Invalid argument: G must be undirected.");
    }
    scoreData.clear();
    scoreData.resize(G.upperNodeIdBound());
    u.clear();
    v.clear();
    sampledPaths.clear();

    double c = 0.5; // universal positive constant - see reference in paper

    edgeweight vd = Diameter::estimatedVertexDiameterPedantic(G);

    INFO("estimated diameter: ", vd);
    r = ceil((c / (epsilon * epsilon)) * (floor(log(vd - 2)) + 1 + log(1 / delta)));
    INFO("taking ", r, " path samples");
    sssp.clear();
    sssp.resize(r);
    u.resize(r);
    v.resize(r);
    sampledPaths.resize(r);

    for (count i = 0; i < r; i++) {
        DEBUG("sample ", i);
        // sample random node pair
        u[i] = Sampling::randomNode(G);
        do {
            v[i] = Sampling::randomNode(G);
        } while (v[i] == u[i]);
        if (G.isWeighted()) {
            INFO("Calling DynDijkstra inside run DynApproxBet");
            sssp[i].reset(new DynDijkstra(G, u[i], storePreds));
        } else {
            INFO("Calling DynBFS inside run DynApproxBet");
            sssp[i].reset(new DynBFS(G, u[i], storePreds));
        }
        DEBUG("running shortest path algorithm for node ", u[i]);

        INFO("Calling setTargetNodeon sssp instance inside run DynApproxBet");
        sssp[i]->setTargetNode(v[i]);
        INFO("Calling run on sssp instance inside run DynApproxBet");
        sssp[i]->run();
        INFO("Ran sssp");
        if (sssp[i]->distances[v[i]] > 0) { // at least one path between {u, v} exists
            DEBUG("updating estimate for path ", u[i], " <-> ", v[i]);
            INFO("Entered if statement.");
            // random path sampling and estimation update
            sampledPaths[i].clear();
            node t = v[i];
            while (t != u[i])  {
              INFO("Entered while statement");
                // sample z in P_u(t) with probability sigma_uz / sigma_us
                std::vector<std::pair<node, double> > choices;
                if (storePreds) {
                    for (node z : sssp[i]->previous[t]) {
                        // workaround for integer overflow in large graphs
                        bigfloat tmp = sssp[i]->numberOfPaths(z) / sssp[i]->numberOfPaths(t);
                        double weight;
                        tmp.ToDouble(weight);

                        choices.emplace_back(z, weight); 	// sigma_uz / sigma_us
                    }
                }
                else {
                  INFO("Storepreds is false");
                  G.forInEdgesOf(t, [&](node t, node z, edgeweight w){
                        if (Aux::NumericTools::logically_equal(sssp[i]->distances[t], sssp[i]->distances[z] + w)) {
                            // workaround for integer overflow in large graphs
                            INFO("Calling number of paths");
                            bigfloat tmp = sssp[i]->numberOfPaths(z) / sssp[i]->numberOfPaths(t);
                            INFO("Called number of paths");
                            double weight;
                            tmp.ToDouble(weight);

                            choices.emplace_back(z, weight);
                        }

                    });
                }
                INFO("Node considered: ", t);
                INFO("Source considered: ", u[i]);
                assert (choices.size() > 0);
                node z = Aux::Random::weightedChoice(choices);
                assert (z <= G.upperNodeIdBound());
                if (z != u[i]) {
                    scoreData[z] += 1 / (double) r;
                    sampledPaths[i].push_back(z);
                }
                t = z;
            }
        }
    }

    hasRun = true;

}

void DynApproxBetweenness::update(const std::vector<GraphEvent>& batch) {
    INFO ("Updating");
    for (node i = 0; i < r; i++) {
        sssp[i]->update(batch);
        if (sssp[i]->modified()) {
            // subtract contributions to nodes in the old sampled path
            for (node z: sampledPaths[i]) {
                scoreData[z] -= 1 / (double) r;
            }
            // sample a new shortest path
            sampledPaths[i].clear();
            node t = v[i];
            while (t != u[i])  {
                // sample z in P_u(t) with probability sigma_uz / sigma_us
                std::vector<std::pair<node, double> > choices;
                if (storePreds) {
                    for (node z : sssp[i]->previous[t]) {
                        // workaround for integer overflow in large graphs
                        bigfloat tmp = sssp[i]->numberOfPaths(z) / sssp[i]->numberOfPaths(t);
                        double weight;
                        tmp.ToDouble(weight);

                        choices.emplace_back(z, weight);
                    }
                }
                else {
                    G.forInEdgesOf(t, [&](node t, node z, edgeweight w){
                        if (Aux::NumericTools::logically_equal(sssp[i]->distances[t], sssp[i]->distances[z] + w)) {
                            // workaround for integer overflow in large graphs
                            bigfloat tmp = sssp[i]->numberOfPaths(z) / sssp[i]->numberOfPaths(t);
                            double weight;
                            tmp.ToDouble(weight);

                            choices.emplace_back(z, weight);
                        }
                    });
                }
                assert (choices.size() > 0); // this should fail only if the graph is not connected
                if (choices.size() == 0) {
                    INFO ("node: ", t);
                    INFO ("source: ", u[i]);
                    INFO ("distance: ", sssp[i]->distances[t]);
                }
                node z = Aux::Random::weightedChoice(choices);
                assert (z <= G.upperNodeIdBound());
                if (z != u[i]) {
                    scoreData[z] += 1 / (double) r;
                    sampledPaths[i].push_back(z);
                }
                t = z;
            }

        }

    }
}

} /* namespace NetworKit */
<commit_msg>Correct computation of sample size. While here, const'ify a constant.<commit_after>/*
 * DynApproxBetweenness.cpp
 *
 *  Created on: 31.07.2014
 *      Author: ebergamini
 */

#include "DynApproxBetweenness.h"
#include "../auxiliary/Random.h"
#include "../distance/Diameter.h"
#include "../graph/Sampling.h"
#include "../graph/DynDijkstra.h"
#include "../graph/DynBFS.h"
#include "../auxiliary/Log.h"
#include "../auxiliary/NumericTools.h"

namespace NetworKit {

DynApproxBetweenness::DynApproxBetweenness(const Graph& G, double epsilon, double delta, bool storePredecessors) : Centrality(G, true),
storePreds(storePredecessors), epsilon(epsilon), delta(delta) {
  INFO("Constructing DynApproxBetweenness. storePredecessors = ", storePredecessors);
}

count DynApproxBetweenness::getNumberOfSamples() {
    return r;
}

void DynApproxBetweenness::run() {
  INFO("Inside DynApproxBetweenness. storePreds = ", storePreds);
    if (G.isDirected()) {
        throw std::runtime_error("Invalid argument: G must be undirected.");
    }
    scoreData.clear();
    scoreData.resize(G.upperNodeIdBound());
    u.clear();
    v.clear();
    sampledPaths.clear();

    const double c = 0.5; // universal positive constant - see reference in paper

    edgeweight vd = Diameter::estimatedVertexDiameterPedantic(G);

    INFO("estimated diameter: ", vd);
    r = ceil((c / (epsilon * epsilon)) * (floor(log2(vd - 2)) + 1 - log(delta)));
    INFO("taking ", r, " path samples");
    sssp.clear();
    sssp.resize(r);
    u.resize(r);
    v.resize(r);
    sampledPaths.resize(r);

    for (count i = 0; i < r; i++) {
        DEBUG("sample ", i);
        // sample random node pair
        u[i] = Sampling::randomNode(G);
        do {
            v[i] = Sampling::randomNode(G);
        } while (v[i] == u[i]);
        if (G.isWeighted()) {
            INFO("Calling DynDijkstra inside run DynApproxBet");
            sssp[i].reset(new DynDijkstra(G, u[i], storePreds));
        } else {
            INFO("Calling DynBFS inside run DynApproxBet");
            sssp[i].reset(new DynBFS(G, u[i], storePreds));
        }
        DEBUG("running shortest path algorithm for node ", u[i]);

        INFO("Calling setTargetNodeon sssp instance inside run DynApproxBet");
        sssp[i]->setTargetNode(v[i]);
        INFO("Calling run on sssp instance inside run DynApproxBet");
        sssp[i]->run();
        INFO("Ran sssp");
        if (sssp[i]->distances[v[i]] > 0) { // at least one path between {u, v} exists
            DEBUG("updating estimate for path ", u[i], " <-> ", v[i]);
            INFO("Entered if statement.");
            // random path sampling and estimation update
            sampledPaths[i].clear();
            node t = v[i];
            while (t != u[i])  {
              INFO("Entered while statement");
                // sample z in P_u(t) with probability sigma_uz / sigma_us
                std::vector<std::pair<node, double> > choices;
                if (storePreds) {
                    for (node z : sssp[i]->previous[t]) {
                        // workaround for integer overflow in large graphs
                        bigfloat tmp = sssp[i]->numberOfPaths(z) / sssp[i]->numberOfPaths(t);
                        double weight;
                        tmp.ToDouble(weight);

                        choices.emplace_back(z, weight); 	// sigma_uz / sigma_us
                    }
                }
                else {
                  INFO("Storepreds is false");
                  G.forInEdgesOf(t, [&](node t, node z, edgeweight w){
                        if (Aux::NumericTools::logically_equal(sssp[i]->distances[t], sssp[i]->distances[z] + w)) {
                            // workaround for integer overflow in large graphs
                            INFO("Calling number of paths");
                            bigfloat tmp = sssp[i]->numberOfPaths(z) / sssp[i]->numberOfPaths(t);
                            INFO("Called number of paths");
                            double weight;
                            tmp.ToDouble(weight);

                            choices.emplace_back(z, weight);
                        }

                    });
                }
                INFO("Node considered: ", t);
                INFO("Source considered: ", u[i]);
                assert (choices.size() > 0);
                node z = Aux::Random::weightedChoice(choices);
                assert (z <= G.upperNodeIdBound());
                if (z != u[i]) {
                    scoreData[z] += 1 / (double) r;
                    sampledPaths[i].push_back(z);
                }
                t = z;
            }
        }
    }

    hasRun = true;

}

void DynApproxBetweenness::update(const std::vector<GraphEvent>& batch) {
    INFO ("Updating");
    for (node i = 0; i < r; i++) {
        sssp[i]->update(batch);
        if (sssp[i]->modified()) {
            // subtract contributions to nodes in the old sampled path
            for (node z: sampledPaths[i]) {
                scoreData[z] -= 1 / (double) r;
            }
            // sample a new shortest path
            sampledPaths[i].clear();
            node t = v[i];
            while (t != u[i])  {
                // sample z in P_u(t) with probability sigma_uz / sigma_us
                std::vector<std::pair<node, double> > choices;
                if (storePreds) {
                    for (node z : sssp[i]->previous[t]) {
                        // workaround for integer overflow in large graphs
                        bigfloat tmp = sssp[i]->numberOfPaths(z) / sssp[i]->numberOfPaths(t);
                        double weight;
                        tmp.ToDouble(weight);

                        choices.emplace_back(z, weight);
                    }
                }
                else {
                    G.forInEdgesOf(t, [&](node t, node z, edgeweight w){
                        if (Aux::NumericTools::logically_equal(sssp[i]->distances[t], sssp[i]->distances[z] + w)) {
                            // workaround for integer overflow in large graphs
                            bigfloat tmp = sssp[i]->numberOfPaths(z) / sssp[i]->numberOfPaths(t);
                            double weight;
                            tmp.ToDouble(weight);

                            choices.emplace_back(z, weight);
                        }
                    });
                }
                assert (choices.size() > 0); // this should fail only if the graph is not connected
                if (choices.size() == 0) {
                    INFO ("node: ", t);
                    INFO ("source: ", u[i]);
                    INFO ("distance: ", sssp[i]->distances[t]);
                }
                node z = Aux::Random::weightedChoice(choices);
                assert (z <= G.upperNodeIdBound());
                if (z != u[i]) {
                    scoreData[z] += 1 / (double) r;
                    sampledPaths[i].push_back(z);
                }
                t = z;
            }

        }

    }
}

} /* namespace NetworKit */
<|endoftext|>