<commit_before>#include <boost/test/unit_test.hpp>

#include <string>
#include <vector>

#include "netbase.h"

using namespace std;

BOOST_AUTO_TEST_SUITE(netbase_tests)

BOOST_AUTO_TEST_CASE(netbase_networks)
{
    BOOST_CHECK(CNetAddr("127.0.0.1").GetNetwork()                              == NET_UNROUTABLE);
    BOOST_CHECK(CNetAddr("::1").GetNetwork()                                    == NET_UNROUTABLE);
    BOOST_CHECK(CNetAddr("8.8.8.8").GetNetwork()                                == NET_IPV4);
    BOOST_CHECK(CNetAddr("2001::8888").GetNetwork()                             == NET_IPV6);
    BOOST_CHECK(CNetAddr("FD87:D87E:EB43:edb1:8e4:3588:e546:35ca").GetNetwork() == NET_TOR);
}

BOOST_AUTO_TEST_CASE(netbase_properties)
{
    BOOST_CHECK(CNetAddr("127.0.0.1").IsIPv4());
    BOOST_CHECK(CNetAddr("::FFFF:192.168.1.1").IsIPv4());
    BOOST_CHECK(CNetAddr("::1").IsIPv6());
    BOOST_CHECK(CNetAddr("10.0.0.1").IsRFC1918());
    BOOST_CHECK(CNetAddr("192.168.1.1").IsRFC1918());
    BOOST_CHECK(CNetAddr("172.31.255.255").IsRFC1918());
    BOOST_CHECK(CNetAddr("2001:0DB8::").IsRFC3849());
    BOOST_CHECK(CNetAddr("169.254.1.1").IsRFC3927());
    BOOST_CHECK(CNetAddr("2002::1").IsRFC3964());
    BOOST_CHECK(CNetAddr("FC00::").IsRFC4193());
    BOOST_CHECK(CNetAddr("2001::2").IsRFC4380());
    BOOST_CHECK(CNetAddr("2001:10::").IsRFC4843());
    BOOST_CHECK(CNetAddr("FE80::").IsRFC4862());
    BOOST_CHECK(CNetAddr("64:FF9B::").IsRFC6052());
    BOOST_CHECK(CNetAddr("FD87:D87E:EB43:edb1:8e4:3588:e546:35ca").IsTor());
    BOOST_CHECK(CNetAddr("127.0.0.1").IsLocal());
    BOOST_CHECK(CNetAddr("::1").IsLocal());
    BOOST_CHECK(CNetAddr("8.8.8.8").IsRoutable());
    BOOST_CHECK(CNetAddr("2001::1").IsRoutable());
    BOOST_CHECK(CNetAddr("127.0.0.1").IsValid());
}

bool static TestSplitHost(string test, string host, int port)
{
    string hostOut;
    int portOut = -1;
    SplitHostPort(test, portOut, hostOut);
    return hostOut == host && port == portOut;
}

BOOST_AUTO_TEST_CASE(netbase_splithost)
{
    BOOST_CHECK(TestSplitHost("www.coingen.io", "www.coingen.io", -1));
    BOOST_CHECK(TestSplitHost("[www.coingen.io]", "www.coingen.io", -1));
    BOOST_CHECK(TestSplitHost("www.coingen.io:80", "www.coingen.io", 80));
    BOOST_CHECK(TestSplitHost("[www.coingen.io]:80", "www.coingen.io", 80));
    BOOST_CHECK(TestSplitHost("127.0.0.1", "127.0.0.1", -1));
    BOOST_CHECK(TestSplitHost("127.0.0.1:8945", "127.0.0.1", 8945));
    BOOST_CHECK(TestSplitHost("[127.0.0.1]", "127.0.0.1", -1));
    BOOST_CHECK(TestSplitHost("[127.0.0.1]:8945", "127.0.0.1", 8945));
    BOOST_CHECK(TestSplitHost("::ffff:127.0.0.1", "::ffff:127.0.0.1", -1));
    BOOST_CHECK(TestSplitHost("[::ffff:127.0.0.1]:8945", "::ffff:127.0.0.1", 8945));
    BOOST_CHECK(TestSplitHost("[::]:8945", "::", 8945));
    BOOST_CHECK(TestSplitHost("::8945", "::8945", -1));
    BOOST_CHECK(TestSplitHost(":8945", "", 8945));
    BOOST_CHECK(TestSplitHost("[]:8945", "", 8945));
    BOOST_CHECK(TestSplitHost("", "", -1));
}

bool static TestParse(string src, string canon)
{
    CService addr;
    if (!LookupNumeric(src.c_str(), addr, 65535))
        return canon == "";
    return canon == addr.ToString();
}

BOOST_AUTO_TEST_CASE(netbase_lookupnumeric)
{
    BOOST_CHECK(TestParse("127.0.0.1", "127.0.0.1:65535"));
    BOOST_CHECK(TestParse("127.0.0.1:8945", "127.0.0.1:8945"));
    BOOST_CHECK(TestParse("::ffff:127.0.0.1", "127.0.0.1:65535"));
    BOOST_CHECK(TestParse("::", "[::]:65535"));
    BOOST_CHECK(TestParse("[::]:8945", "[::]:8945"));
    BOOST_CHECK(TestParse("[127.0.0.1]", "127.0.0.1:65535"));
    BOOST_CHECK(TestParse(":::", ""));
}

BOOST_AUTO_TEST_CASE(onioncat_test)
{
    // values from http://www.cypherpunk.at/onioncat/wiki/OnionCat
    CNetAddr addr1("5wyqrzbvrdsumnok.onion");
    CNetAddr addr2("FD87:D87E:EB43:edb1:8e4:3588:e546:35ca");
    BOOST_CHECK(addr1 == addr2);
    BOOST_CHECK(addr1.IsTor());
    BOOST_CHECK(addr1.ToStringIP() == "5wyqrzbvrdsumnok.onion");
    BOOST_CHECK(addr1.IsRoutable());
}

BOOST_AUTO_TEST_SUITE_END()
<commit_msg>New port<commit_after>#include <boost/test/unit_test.hpp>

#include <string>
#include <vector>

#include "netbase.h"

using namespace std;

BOOST_AUTO_TEST_SUITE(netbase_tests)

BOOST_AUTO_TEST_CASE(netbase_networks)
{
    BOOST_CHECK(CNetAddr("127.0.0.1").GetNetwork()                              == NET_UNROUTABLE);
    BOOST_CHECK(CNetAddr("::1").GetNetwork()                                    == NET_UNROUTABLE);
    BOOST_CHECK(CNetAddr("8.8.8.8").GetNetwork()                                == NET_IPV4);
    BOOST_CHECK(CNetAddr("2001::8888").GetNetwork()                             == NET_IPV6);
    BOOST_CHECK(CNetAddr("FD87:D87E:EB43:edb1:8e4:3588:e546:35ca").GetNetwork() == NET_TOR);
}

BOOST_AUTO_TEST_CASE(netbase_properties)
{
    BOOST_CHECK(CNetAddr("127.0.0.1").IsIPv4());
    BOOST_CHECK(CNetAddr("::FFFF:192.168.1.1").IsIPv4());
    BOOST_CHECK(CNetAddr("::1").IsIPv6());
    BOOST_CHECK(CNetAddr("10.0.0.1").IsRFC1918());
    BOOST_CHECK(CNetAddr("192.168.1.1").IsRFC1918());
    BOOST_CHECK(CNetAddr("172.31.255.255").IsRFC1918());
    BOOST_CHECK(CNetAddr("2001:0DB8::").IsRFC3849());
    BOOST_CHECK(CNetAddr("169.254.1.1").IsRFC3927());
    BOOST_CHECK(CNetAddr("2002::1").IsRFC3964());
    BOOST_CHECK(CNetAddr("FC00::").IsRFC4193());
    BOOST_CHECK(CNetAddr("2001::2").IsRFC4380());
    BOOST_CHECK(CNetAddr("2001:10::").IsRFC4843());
    BOOST_CHECK(CNetAddr("FE80::").IsRFC4862());
    BOOST_CHECK(CNetAddr("64:FF9B::").IsRFC6052());
    BOOST_CHECK(CNetAddr("FD87:D87E:EB43:edb1:8e4:3588:e546:35ca").IsTor());
    BOOST_CHECK(CNetAddr("127.0.0.1").IsLocal());
    BOOST_CHECK(CNetAddr("::1").IsLocal());
    BOOST_CHECK(CNetAddr("8.8.8.8").IsRoutable());
    BOOST_CHECK(CNetAddr("2001::1").IsRoutable());
    BOOST_CHECK(CNetAddr("127.0.0.1").IsValid());
}

bool static TestSplitHost(string test, string host, int port)
{
    string hostOut;
    int portOut = -1;
    SplitHostPort(test, portOut, hostOut);
    return hostOut == host && port == portOut;
}

BOOST_AUTO_TEST_CASE(netbase_splithost)
{
    BOOST_CHECK(TestSplitHost("www.coingen.io", "www.coingen.io", -1));
    BOOST_CHECK(TestSplitHost("[www.coingen.io]", "www.coingen.io", -1));
    BOOST_CHECK(TestSplitHost("www.coingen.io:80", "www.coingen.io", 80));
    BOOST_CHECK(TestSplitHost("[www.coingen.io]:80", "www.coingen.io", 80));
    BOOST_CHECK(TestSplitHost("127.0.0.1", "127.0.0.1", -1));
    BOOST_CHECK(TestSplitHost("127.0.0.1:8915", "127.0.0.1", 8915));
    BOOST_CHECK(TestSplitHost("[127.0.0.1]", "127.0.0.1", -1));
    BOOST_CHECK(TestSplitHost("[127.0.0.1]:8915", "127.0.0.1", 8915));
    BOOST_CHECK(TestSplitHost("::ffff:127.0.0.1", "::ffff:127.0.0.1", -1));
    BOOST_CHECK(TestSplitHost("[::ffff:127.0.0.1]:8915", "::ffff:127.0.0.1", 8915));
    BOOST_CHECK(TestSplitHost("[::]:8915", "::", 8915));
    BOOST_CHECK(TestSplitHost("::8915", "::8915", -1));
    BOOST_CHECK(TestSplitHost(":8915", "", 8915));
    BOOST_CHECK(TestSplitHost("[]:8915", "", 8915));
    BOOST_CHECK(TestSplitHost("", "", -1));
}

bool static TestParse(string src, string canon)
{
    CService addr;
    if (!LookupNumeric(src.c_str(), addr, 65535))
        return canon == "";
    return canon == addr.ToString();
}

BOOST_AUTO_TEST_CASE(netbase_lookupnumeric)
{
    BOOST_CHECK(TestParse("127.0.0.1", "127.0.0.1:65535"));
    BOOST_CHECK(TestParse("127.0.0.1:8915", "127.0.0.1:8915"));
    BOOST_CHECK(TestParse("::ffff:127.0.0.1", "127.0.0.1:65535"));
    BOOST_CHECK(TestParse("::", "[::]:65535"));
    BOOST_CHECK(TestParse("[::]:8915", "[::]:8915"));
    BOOST_CHECK(TestParse("[127.0.0.1]", "127.0.0.1:65535"));
    BOOST_CHECK(TestParse(":::", ""));
}

BOOST_AUTO_TEST_CASE(onioncat_test)
{
    // values from http://www.cypherpunk.at/onioncat/wiki/OnionCat
    CNetAddr addr1("5wyqrzbvrdsumnok.onion");
    CNetAddr addr2("FD87:D87E:EB43:edb1:8e4:3588:e546:35ca");
    BOOST_CHECK(addr1 == addr2);
    BOOST_CHECK(addr1.IsTor());
    BOOST_CHECK(addr1.ToStringIP() == "5wyqrzbvrdsumnok.onion");
    BOOST_CHECK(addr1.IsRoutable());
}

BOOST_AUTO_TEST_SUITE_END()
<|endoftext|>


<commit_before>#include "openvdb_distance_field.h"
#include <openvdb/tools/LevelSetSphere.h>
#include <openvdb/tools/Composite.h>
#include <ros/assert.h>
#include <algorithm>
#include <math.h>

distance_field::OpenVDBDistanceField::OpenVDBDistanceField(float voxel_size, float background) :
  voxel_size_(voxel_size),
  background_(background)
{
  openvdb::initialize();
  transform_ = openvdb::math::Transform::createLinearTransform(voxel_size_);
}

openvdb::FloatGrid::Ptr distance_field::OpenVDBDistanceField::getGrid() const
{
  return grid_;
}

double distance_field::OpenVDBDistanceField::getDistance(Eigen::Vector3f &point)
{
  openvdb::math::Vec3s xyz(point[0], point[1], point[2]);
  xyz = transform_->worldToIndex(xyz);
  openvdb::Coord c = openvdb::util::nearestCoord(xyz);
  return grid_->getAccessor().getValue(c);
}

void distance_field::OpenVDBDistanceField::addRobotToField(const moveit::core::RobotStatePtr robot_state, const float exBandWidth, const float inBandWidth)
{
  std::vector<const moveit::core::LinkModel*> links = robot_state->getRobotModel()->getLinkModelsWithCollisionGeometry();
  for (int i = 0; i < links.size(); ++i)
  {
    std::vector<shapes::ShapeConstPtr> shapes = links[i]->getShapes();
    EigenSTL::vector_Affine3d shape_poses = links[i]->getCollisionOriginTransforms();

    Eigen::Affine3d link_pose = robot_state->getFrameTransform(links[i]->getName());

    for (int j = 0; j < shapes.size(); ++j)
    {
      addShapeToField(shapes[j].get(), link_pose * shape_poses[j], exBandWidth, inBandWidth);
    }
  }
}

void distance_field::OpenVDBDistanceField::addShapeToField(const shapes::Shape *shape, const Eigen::Affine3d &pose, const float exBandWidth, const float inBandWidth)
{
  openvdb::FloatGrid::Ptr grid;

  switch(shape->type)
  {
    case shapes::BOX:
    {
      return;
      const shapes::Box *box = static_cast<const shapes::Box *>(shape);
      const openvdb::math::Vec3s pmax = openvdb::math::Vec3s(std::abs(box->size[0])/2.0, std::abs(box->size[1])/2.0, std::abs(box->size[2])/2.0);
      const openvdb::math::Vec3s pmin = -1.0*pmax;

      std::vector<openvdb::math::Vec3s> points(8);
      points[0] = openvdb::math::Vec3s(pmin[0], pmin[1], pmin[2]);
      points[1] = openvdb::math::Vec3s(pmin[0], pmin[1], pmax[2]);
      points[2] = openvdb::math::Vec3s(pmax[0], pmin[1], pmax[2]);
      points[3] = openvdb::math::Vec3s(pmax[0], pmin[1], pmin[2]);
      points[4] = openvdb::math::Vec3s(pmin[0], pmax[1], pmin[2]);
      points[5] = openvdb::math::Vec3s(pmin[0], pmax[1], pmax[2]);
      points[6] = openvdb::math::Vec3s(pmax[0], pmax[1], pmax[2]);
      points[7] = openvdb::math::Vec3s(pmax[0], pmax[1], pmin[2]);

      std::vector<openvdb::Vec4I> quads(6);
      quads[0] = openvdb::Vec4I(0, 1, 2, 3); // bottom
      quads[1] = openvdb::Vec4I(7, 6, 5, 4); // top
      quads[2] = openvdb::Vec4I(4, 5, 1, 0); // front
      quads[3] = openvdb::Vec4I(6, 7, 3, 2); // back
      quads[4] = openvdb::Vec4I(0, 3, 7, 4); // left
      quads[5] = openvdb::Vec4I(1, 5, 6, 2); // right

      // Tranform point location
      TransformVec3s(pose, points);

      // Convert data from world to index
      WorldToIndex(transform_, points);

      openvdb::tools::MeshToVolume<openvdb::FloatGrid> voxelizer(transform_);

      voxelizer.convertToLevelSet(points, quads, exBandWidth, inBandWidth);

      grid = voxelizer.distGridPtr()->deepCopy();

      break;
    }

    case shapes::CONE:
    {
      const shapes::Cone *cone = static_cast<const shapes::Cone *>(shape);

      // Number of sides
      int sides = 30;

      // Cone Data
      double dtheta = 2 * M_PI/sides;
      double dh = cone->length/2.0;
      std::vector<openvdb::math::Vec3s> points(sides+2);
      std::vector<openvdb::Vec4I> quads(2*sides);

      // Create Vertices
      double theta = 0;
      for(int i = 0; i < sides; ++i)
      {
        double x = cone->radius * std::cos(theta);
        double y = cone->radius * std::sin(theta);
        points[i] = openvdb::math::Vec3s(x, y, -dh);
        theta += dtheta;
      }
      int top_pdx = sides;
      int bot_pdx = sides + 1;
      points[top_pdx] = openvdb::math::Vec3s(0.0, 0.0, dh);
      points[bot_pdx] = openvdb::math::Vec3s(0.0, 0.0, -dh);

      // Create polygons for wall and bottom
      for(int i = 0; i < sides; ++i)
      {
        int d = i;
        int d1 = i + 1;
        bool last = (i == (sides-1));

        if (last)
          d1 = 0;

        // Create wall
        quads[i] = openvdb::Vec4I(top_pdx, d, d1, openvdb::util::INVALID_IDX);

        // Create bottom cap
        quads[sides+i] = openvdb::Vec4I(bot_pdx, d1, d, openvdb::util::INVALID_IDX);
      }

      // Transform point location
      TransformVec3s(pose, points);

      // Convert data from world to index
      WorldToIndex(transform_, points);

      openvdb::tools::MeshToVolume<openvdb::FloatGrid> voxelizer(transform_);

      voxelizer.convertToLevelSet(points, quads, exBandWidth, inBandWidth);

      grid = voxelizer.distGridPtr()->deepCopy();

      break;
    }

    case shapes::CYLINDER:
    {
      const shapes::Cylinder *cylinder = static_cast<const shapes::Cylinder *>(shape);

      // Number of sides
      int sides = 30;

      // Cylinder Precision
      double dtheta = 2 * M_PI/sides;

      double dh = cylinder->length/2.0;
      std::vector<openvdb::math::Vec3s> points(2*(sides+1));
      std::vector<openvdb::Vec4I> quads(3*sides);

      // Create Vertices
      double theta = 0;
      int sign = 1;
      for(int i = 0; i < sides; ++i)
      {
        double x = cylinder->radius * std::cos(theta);
        double y = cylinder->radius * std::sin(theta);
        points[2 * i] = openvdb::math::Vec3s(x, y, sign * dh);
        points[(2 * i)+1] = openvdb::math::Vec3s(x, y, -sign * dh);
        theta += dtheta;
        sign = -1 * sign;
      }
      int top_pdx = (2 * sides);
      int bot_pdx = (2 * sides) + 1;
      points[top_pdx] = openvdb::math::Vec3s(0.0, 0.0, dh);
      points[bot_pdx] = openvdb::math::Vec3s(0.0, 0.0, -dh);

      // Create polygons for wall and caps
      for(int i = 0; i < sides; ++i)
      {
        int d = i * 2;
        int d1 = d + 1;
        int d2 = d + 2;
        int d3 = d + 3;

        bool last = (i == (sides-1));
        if( i % 2 == 0)
        {
          if (last)
          {
            d2 = 1;
            d3 = 0;
          }

          // Create wall
          quads[i] = openvdb::Vec4I(d, d1, d2, d3);

          // Create top cap
          quads[sides+i] = openvdb::Vec4I(top_pdx, d, d3, openvdb::util::INVALID_IDX);

          // Create bottom cap
          quads[(2*sides)+i] = openvdb::Vec4I(bot_pdx, d1, d2, openvdb::util::INVALID_IDX);
        }
        else
        {
          if (last)
          {
            d2 = 0;
            d3 = 1;
          }

          // Create wall
          quads[i] = openvdb::Vec4I(d3, d2, d1, d);

          // Create top cap
          quads[sides+i] = openvdb::Vec4I(top_pdx, d1, d2, openvdb::util::INVALID_IDX);

          // Create bottom cap
          quads[(2*sides)+i] = openvdb::Vec4I(bot_pdx, d, d3, openvdb::util::INVALID_IDX);
        }
      }

      // Tranform point location
      TransformVec3s(pose, points);

      // Convert data from world to index
      WorldToIndex(transform_, points);

      openvdb::tools::MeshToVolume<openvdb::FloatGrid> voxelizer(transform_);

      voxelizer.convertToLevelSet(points, quads, exBandWidth, inBandWidth);

      grid = voxelizer.distGridPtr()->deepCopy();

      break;
    }

    case shapes::OCTREE:
    {
      ROS_ERROR("OpenVDB Distance Field: Shape Type OCTREE is not implemented.");
      return;
    }

    case shapes::MESH:
    {
      shapes::Mesh *mesh = static_cast<shapes::Mesh *>(shape->clone());

      // Now need to clean verticies
      mesh->mergeVertices(0.0001);

      // Get points and triangles from Shape
      MeshData mesh_data = ShapeMeshToOpenVDB(mesh, pose);

      // Convert data from world to index
      WorldToIndex(transform_, mesh_data.points);

      openvdb::tools::MeshToVolume<openvdb::FloatGrid> voxelizer(transform_);

      voxelizer.convertToLevelSet(mesh_data.points, mesh_data.quads, exBandWidth, inBandWidth);

      grid = voxelizer.distGridPtr()->deepCopy();

      delete mesh;
      break;
    }

    case shapes::PLANE:
    {
      ROS_ERROR("OpenVDB Distance Field: Shape Type PLANE is not implemented.");
      return;
    }

    case shapes::SPHERE:
    {
      const shapes::Sphere *sphere = static_cast<const shapes::Sphere *>(shape);
      grid = openvdb::tools::createLevelSetSphere<openvdb::FloatGrid>(sphere->radius,  openvdb::Vec3f(pose.translation()(0), pose.translation()(1), pose.translation()(2)), voxel_size_, exBandWidth);
      break;
    }

    case shapes::UNKNOWN_SHAPE:
    {
      ROS_ERROR("OpenVDB Distance Field: Unknown Shape Type");
      return;
    }

  }

  // Set the default background distance
  grid->setBackground(background_);
  if (!grid_)
  {
    grid_ = grid;
  }
  else
  {
    // openvdb::tools::csgUnion(grid_, grid, true); This has compiler errors, need to find out why.

    // This was the only way to get the correct distance values when combining two grids
    grid_->tree().combine(grid->tree(), distance_field::diff);

    // Clean up the grid after merge to conserve memory
    grid_->pruneGrid();
  }
}

void distance_field::OpenVDBDistanceField::writeToFile(const std::string file_path)
{
  // Create a VDB file object.
  openvdb::io::File vdbFile(file_path);

  // Add the grid pointer to a container.
  openvdb::GridPtrVec grids;
  grids.push_back(grid_);

  // Write out the contents of the container.
  vdbFile.write(grids);
  vdbFile.close();
}

distance_field::MeshData distance_field::ShapeMeshToOpenVDB(const shapes::Mesh *mesh, const Eigen::Affine3d &pose)
{
  MeshData output;
  openvdb::math::Mat4d tf;

  output.points.resize(mesh->vertex_count);
  output.triangles.resize(mesh->triangle_count);
  output.quads.resize(mesh->triangle_count);

  // Convert Eigen to OpenVDB
  Affine3dToMat4d(pose, tf);

  // Populate Verticies/Triangles/Quads
  for (unsigned int i = 0 ; i < mesh->triangle_count ; ++i)
  {
    unsigned int i3 = i * 3;
    unsigned int v1 = mesh->triangles[i3];
    unsigned int v2 = mesh->triangles[i3 + 1];
    unsigned int v3 = mesh->triangles[i3 + 2];
    openvdb::math::Vec3s p1(mesh->vertices[v1 * 3], mesh->vertices[v1 * 3 + 1], mesh->vertices[v1 * 3 + 2]);
    openvdb::math::Vec3s p2(mesh->vertices[v2 * 3], mesh->vertices[v2 * 3 + 1], mesh->vertices[v2 * 3 + 2]);
    openvdb::math::Vec3s p3(mesh->vertices[v3 * 3], mesh->vertices[v3 * 3 + 1], mesh->vertices[v3 * 3 + 2]);

    output.points[v1] = tf*p1;
    output.points[v2] = tf*p2;
    output.points[v3] = tf*p3;

    openvdb::Vec3I triangle(v1, v2, v3);
    output.triangles[i] = triangle;

    openvdb::Vec4I quad(v1, v2, v3, openvdb::util::INVALID_IDX);
    output.quads[i] = quad;
  }

  ROS_ASSERT(mesh->vertex_count == output.points.size());
  ROS_ASSERT(mesh->triangle_count == output.triangles.size());
  ROS_ASSERT(mesh->triangle_count == output.quads.size());

  return output;
}

void distance_field::Affine3dToMat4d(const Eigen::Affine3d &input, openvdb::math::Mat4d &output)
{
  for (int i = 0; i < 4; ++i)
    for (int j = 0; j < 4; ++j)
      output(i, j) = input(i, j);
}

void distance_field::WorldToIndex(const openvdb::math::Transform::Ptr transform, std::vector<openvdb::math::Vec3s> &points)
{
  std::transform(points.begin(), points.end(), points.begin(),
                 [&transform](openvdb::math::Vec3s point){ return transform->worldToIndex(point); });
}

void distance_field::TransformVec3s(const Eigen::Affine3d &pose, std::vector<openvdb::v2_1::math::Vec3s> &points)
{
  openvdb::math::Mat4d tf;
  Affine3dToMat4d(pose, tf);
  std::transform(points.begin(), points.end(), points.begin(),
                 [&tf](openvdb::math::Vec3s point){ return tf * point; });
}
<commit_msg>add the use of csgUnion for joining grids<commit_after>#include "openvdb_distance_field.h"
#include <openvdb/tools/LevelSetSphere.h>
#include <openvdb/tools/Composite.h>
#include <openvdb/tools/GridOperators.h>
#include <openvdb/tools/VolumeToSpheres.h>
#include <ros/assert.h>
#include <algorithm>
#include <math.h>

distance_field::OpenVDBDistanceField::OpenVDBDistanceField(float voxel_size, float background) :
  voxel_size_(voxel_size),
  background_(background)
{
  openvdb::initialize();
  transform_ = openvdb::math::Transform::createLinearTransform(voxel_size_);
}

openvdb::FloatGrid::Ptr distance_field::OpenVDBDistanceField::getGrid() const
{
  return grid_;
}

double distance_field::OpenVDBDistanceField::getDistance(Eigen::Vector3f &point)
{
  openvdb::math::Vec3s xyz(point[0], point[1], point[2]);
  xyz = transform_->worldToIndex(xyz);
  openvdb::Coord c = openvdb::util::nearestCoord(xyz);
  return grid_->getAccessor().getValue(c);
}

void distance_field::OpenVDBDistanceField::addRobotToField(const moveit::core::RobotStatePtr robot_state, const float exBandWidth, const float inBandWidth)
{
  std::vector<const moveit::core::LinkModel*> links = robot_state->getRobotModel()->getLinkModelsWithCollisionGeometry();
  for (int i = 0; i < links.size(); ++i)
  {
    std::vector<shapes::ShapeConstPtr> shapes = links[i]->getShapes();
    EigenSTL::vector_Affine3d shape_poses = links[i]->getCollisionOriginTransforms();

    Eigen::Affine3d link_pose = robot_state->getFrameTransform(links[i]->getName());

    for (int j = 0; j < shapes.size(); ++j)
    {
      addShapeToField(shapes[j].get(), link_pose * shape_poses[j], exBandWidth, inBandWidth);
    }
  }
}

void distance_field::OpenVDBDistanceField::addShapeToField(const shapes::Shape *shape, const Eigen::Affine3d &pose, const float exBandWidth, const float inBandWidth)
{
  openvdb::FloatGrid::Ptr grid;

  switch(shape->type)
  {
    case shapes::BOX:
    {
      return;
      const shapes::Box *box = static_cast<const shapes::Box *>(shape);
      const openvdb::math::Vec3s pmax = openvdb::math::Vec3s(std::abs(box->size[0])/2.0, std::abs(box->size[1])/2.0, std::abs(box->size[2])/2.0);
      const openvdb::math::Vec3s pmin = -1.0*pmax;

      std::vector<openvdb::math::Vec3s> points(8);
      points[0] = openvdb::math::Vec3s(pmin[0], pmin[1], pmin[2]);
      points[1] = openvdb::math::Vec3s(pmin[0], pmin[1], pmax[2]);
      points[2] = openvdb::math::Vec3s(pmax[0], pmin[1], pmax[2]);
      points[3] = openvdb::math::Vec3s(pmax[0], pmin[1], pmin[2]);
      points[4] = openvdb::math::Vec3s(pmin[0], pmax[1], pmin[2]);
      points[5] = openvdb::math::Vec3s(pmin[0], pmax[1], pmax[2]);
      points[6] = openvdb::math::Vec3s(pmax[0], pmax[1], pmax[2]);
      points[7] = openvdb::math::Vec3s(pmax[0], pmax[1], pmin[2]);

      std::vector<openvdb::Vec4I> quads(6);
      quads[0] = openvdb::Vec4I(0, 1, 2, 3); // bottom
      quads[1] = openvdb::Vec4I(7, 6, 5, 4); // top
      quads[2] = openvdb::Vec4I(4, 5, 1, 0); // front
      quads[3] = openvdb::Vec4I(6, 7, 3, 2); // back
      quads[4] = openvdb::Vec4I(0, 3, 7, 4); // left
      quads[5] = openvdb::Vec4I(1, 5, 6, 2); // right

      // Tranform point location
      TransformVec3s(pose, points);

      // Convert data from world to index
      WorldToIndex(transform_, points);

      openvdb::tools::MeshToVolume<openvdb::FloatGrid> voxelizer(transform_);

      voxelizer.convertToLevelSet(points, quads, exBandWidth, inBandWidth);

      grid = voxelizer.distGridPtr()->deepCopy();

      break;
    }

    case shapes::CONE:
    {
      const shapes::Cone *cone = static_cast<const shapes::Cone *>(shape);

      // Number of sides
      int sides = 30;

      // Cone Data
      double dtheta = 2 * M_PI/sides;
      double dh = cone->length/2.0;
      std::vector<openvdb::math::Vec3s> points(sides+2);
      std::vector<openvdb::Vec4I> quads(2*sides);

      // Create Vertices
      double theta = 0;
      for(int i = 0; i < sides; ++i)
      {
        double x = cone->radius * std::cos(theta);
        double y = cone->radius * std::sin(theta);
        points[i] = openvdb::math::Vec3s(x, y, -dh);
        theta += dtheta;
      }
      int top_pdx = sides;
      int bot_pdx = sides + 1;
      points[top_pdx] = openvdb::math::Vec3s(0.0, 0.0, dh);
      points[bot_pdx] = openvdb::math::Vec3s(0.0, 0.0, -dh);

      // Create polygons for wall and bottom
      for(int i = 0; i < sides; ++i)
      {
        int d = i;
        int d1 = i + 1;
        bool last = (i == (sides-1));

        if (last)
          d1 = 0;

        // Create wall
        quads[i] = openvdb::Vec4I(top_pdx, d, d1, openvdb::util::INVALID_IDX);

        // Create bottom cap
        quads[sides+i] = openvdb::Vec4I(bot_pdx, d1, d, openvdb::util::INVALID_IDX);
      }

      // Transform point location
      TransformVec3s(pose, points);

      // Convert data from world to index
      WorldToIndex(transform_, points);

      openvdb::tools::MeshToVolume<openvdb::FloatGrid> voxelizer(transform_);

      voxelizer.convertToLevelSet(points, quads, exBandWidth, inBandWidth);

      grid = voxelizer.distGridPtr()->deepCopy();

      break;
    }

    case shapes::CYLINDER:
    {
      const shapes::Cylinder *cylinder = static_cast<const shapes::Cylinder *>(shape);

      // Number of sides
      int sides = 30;

      // Cylinder Precision
      double dtheta = 2 * M_PI/sides;

      double dh = cylinder->length/2.0;
      std::vector<openvdb::math::Vec3s> points(2*(sides+1));
      std::vector<openvdb::Vec4I> quads(3*sides);

      // Create Vertices
      double theta = 0;
      int sign = 1;
      for(int i = 0; i < sides; ++i)
      {
        double x = cylinder->radius * std::cos(theta);
        double y = cylinder->radius * std::sin(theta);
        points[2 * i] = openvdb::math::Vec3s(x, y, sign * dh);
        points[(2 * i)+1] = openvdb::math::Vec3s(x, y, -sign * dh);
        theta += dtheta;
        sign = -1 * sign;
      }
      int top_pdx = (2 * sides);
      int bot_pdx = (2 * sides) + 1;
      points[top_pdx] = openvdb::math::Vec3s(0.0, 0.0, dh);
      points[bot_pdx] = openvdb::math::Vec3s(0.0, 0.0, -dh);

      // Create polygons for wall and caps
      for(int i = 0; i < sides; ++i)
      {
        int d = i * 2;
        int d1 = d + 1;
        int d2 = d + 2;
        int d3 = d + 3;

        bool last = (i == (sides-1));
        if( i % 2 == 0)
        {
          if (last)
          {
            d2 = 1;
            d3 = 0;
          }

          // Create wall
          quads[i] = openvdb::Vec4I(d, d1, d2, d3);

          // Create top cap
          quads[sides+i] = openvdb::Vec4I(top_pdx, d, d3, openvdb::util::INVALID_IDX);

          // Create bottom cap
          quads[(2*sides)+i] = openvdb::Vec4I(bot_pdx, d1, d2, openvdb::util::INVALID_IDX);
        }
        else
        {
          if (last)
          {
            d2 = 0;
            d3 = 1;
          }

          // Create wall
          quads[i] = openvdb::Vec4I(d3, d2, d1, d);

          // Create top cap
          quads[sides+i] = openvdb::Vec4I(top_pdx, d1, d2, openvdb::util::INVALID_IDX);

          // Create bottom cap
          quads[(2*sides)+i] = openvdb::Vec4I(bot_pdx, d, d3, openvdb::util::INVALID_IDX);
        }
      }

      // Tranform point location
      TransformVec3s(pose, points);

      // Convert data from world to index
      WorldToIndex(transform_, points);

      openvdb::tools::MeshToVolume<openvdb::FloatGrid> voxelizer(transform_);

      voxelizer.convertToLevelSet(points, quads, exBandWidth, inBandWidth);

      grid = voxelizer.distGridPtr()->deepCopy();

      break;
    }

    case shapes::OCTREE:
    {
      ROS_ERROR("OpenVDB Distance Field: Shape Type OCTREE is not implemented.");
      return;
    }

    case shapes::MESH:
    {
      shapes::Mesh *mesh = static_cast<shapes::Mesh *>(shape->clone());

      // Now need to clean verticies
      mesh->mergeVertices(0.0001);

      // Get points and triangles from Shape
      MeshData mesh_data = ShapeMeshToOpenVDB(mesh, pose);

      // Convert data from world to index
      WorldToIndex(transform_, mesh_data.points);

      openvdb::tools::MeshToVolume<openvdb::FloatGrid> voxelizer(transform_);

      voxelizer.convertToLevelSet(mesh_data.points, mesh_data.quads, exBandWidth, inBandWidth);

      grid = voxelizer.distGridPtr()->deepCopy();

      delete mesh;
      break;
    }

    case shapes::PLANE:
    {
      ROS_ERROR("OpenVDB Distance Field: Shape Type PLANE is not implemented.");
      return;
    }

    case shapes::SPHERE:
    {
      const shapes::Sphere *sphere = static_cast<const shapes::Sphere *>(shape);
      grid = openvdb::tools::createLevelSetSphere<openvdb::FloatGrid>(sphere->radius,  openvdb::Vec3f(pose.translation()(0), pose.translation()(1), pose.translation()(2)), voxel_size_, exBandWidth);
      break;
    }

    case shapes::UNKNOWN_SHAPE:
    {
      ROS_ERROR("OpenVDB Distance Field: Unknown Shape Type");
      return;
    }

  }

  // Set the default background distance
  grid->setBackground(background_);
  if (!grid_)
  {
    grid_ = grid;
  }
  else
  {
    openvdb::tools::csgUnion(*grid_, *grid, true);
  }
}

void distance_field::OpenVDBDistanceField::writeToFile(const std::string file_path)
{
  // Create a VDB file object.
  openvdb::io::File vdbFile(file_path);

  // Add the grid pointer to a container.
  openvdb::GridPtrVec grids;
  grids.push_back(grid_);

  // Write out the contents of the container.
  vdbFile.write(grids);
  vdbFile.close();
}

distance_field::MeshData distance_field::ShapeMeshToOpenVDB(const shapes::Mesh *mesh, const Eigen::Affine3d &pose)
{
  MeshData output;
  openvdb::math::Mat4d tf;

  output.points.resize(mesh->vertex_count);
  output.triangles.resize(mesh->triangle_count);
  output.quads.resize(mesh->triangle_count);

  // Convert Eigen to OpenVDB
  Affine3dToMat4d(pose, tf);

  // Populate Verticies/Triangles/Quads
  for (unsigned int i = 0 ; i < mesh->triangle_count ; ++i)
  {
    unsigned int i3 = i * 3;
    unsigned int v1 = mesh->triangles[i3];
    unsigned int v2 = mesh->triangles[i3 + 1];
    unsigned int v3 = mesh->triangles[i3 + 2];
    openvdb::math::Vec3s p1(mesh->vertices[v1 * 3], mesh->vertices[v1 * 3 + 1], mesh->vertices[v1 * 3 + 2]);
    openvdb::math::Vec3s p2(mesh->vertices[v2 * 3], mesh->vertices[v2 * 3 + 1], mesh->vertices[v2 * 3 + 2]);
    openvdb::math::Vec3s p3(mesh->vertices[v3 * 3], mesh->vertices[v3 * 3 + 1], mesh->vertices[v3 * 3 + 2]);

    output.points[v1] = tf*p1;
    output.points[v2] = tf*p2;
    output.points[v3] = tf*p3;

    openvdb::Vec3I triangle(v1, v2, v3);
    output.triangles[i] = triangle;

    openvdb::Vec4I quad(v1, v2, v3, openvdb::util::INVALID_IDX);
    output.quads[i] = quad;
  }

  ROS_ASSERT(mesh->vertex_count == output.points.size());
  ROS_ASSERT(mesh->triangle_count == output.triangles.size());
  ROS_ASSERT(mesh->triangle_count == output.quads.size());

  return output;
}

void distance_field::Affine3dToMat4d(const Eigen::Affine3d &input, openvdb::math::Mat4d &output)
{
  for (int i = 0; i < 4; ++i)
    for (int j = 0; j < 4; ++j)
      output(i, j) = input(i, j);
}

void distance_field::WorldToIndex(const openvdb::math::Transform::Ptr transform, std::vector<openvdb::math::Vec3s> &points)
{
  std::transform(points.begin(), points.end(), points.begin(),
                 [&transform](openvdb::math::Vec3s point){ return transform->worldToIndex(point); });
}

void distance_field::TransformVec3s(const Eigen::Affine3d &pose, std::vector<openvdb::v2_1::math::Vec3s> &points)
{
  openvdb::math::Mat4d tf;
  Affine3dToMat4d(pose, tf);
  std::transform(points.begin(), points.end(), points.begin(),
                 [&tf](openvdb::math::Vec3s point){ return tf * point; });
}
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  $RCSfile: filterdetect.cxx,v $
 *
 *  $Revision: 1.3 $
 *
 *  last change: $Author: hr $ $Date: 2003-06-30 15:38:53 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  the BSD license.
 *
 *  Copyright (c) 2003 by Sun Microsystems, 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:
 *  1. Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *  2. Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *  3. Neither the name of Sun Microsystems, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 *  OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 *  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
 *  TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
 *  USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *************************************************************************/

#include <iostream.h>
#include <stdlib.h>
#include <ctype.h>
#include <stdio.h>
#ifndef _FILTERDETECT_HXX
#include "filterdetect.hxx"
#endif
#ifndef _OSL_DIAGNOSE_H_
#include <osl/diagnose.h>
#endif
#ifndef _COM_SUN_STAR_LANG_XMULTISERVICEFACTORY_HPP_
#include <com/sun/star/lang/XMultiServiceFactory.hpp>
#endif

#ifndef _COM_SUN_STAR_IO_XACTIVEDATASOURCE_HPP_
#include <com/sun/star/io/XActiveDataSource.hpp>
#endif
#ifndef _COM_SUN_STAR_IO_XOUTPUTSTREAM_HPP_
#include <com/sun/star/io/XOutputStream.hpp>
#endif
#ifndef _COM_SUN_STAR_IO_XINPUTSTREAM_HPP_
#include <com/sun/star/io/XInputStream.hpp>
#endif
#ifndef _COM_SUN_STAR_XML_SAX_XDOCUMENTHANDLER_HPP_
#include <com/sun/star/xml/sax/XDocumentHandler.hpp>
#endif

#ifndef _COM_SUN_STAR_XML_SAX_INPUTSOURCE_HPP_
#include <com/sun/star/xml/sax/InputSource.hpp>
#endif
#ifndef _COM_SUN_STAR_XML_SAX_XPARSER_HPP_
#include <com/sun/star/xml/sax/XParser.hpp>
#endif

#ifndef _COM_SUN_STAR_XML_XIMPORTFILTER_HPP_
#include <com/sun/star/xml/XImportFilter.hpp>
#endif
#ifndef _COM_SUN_STAR_XML_XEXPORTFILTER_HPP_
#include <com/sun/star/xml/XExportFilter.hpp>
#endif
#ifndef _COM_SUN_STAR_FRAME_XMODEL_HPP_
#include <com/sun/star/frame/XModel.hpp>
#endif
#ifndef _COM_SUN_STAR_FRAME_XCONTROLLER_HPP_
#include <com/sun/star/frame/XController.hpp>
#endif
#ifndef  _COM_SUN_STAR_TASK_XSTATUSINDICATOR_HPP_
#include <com/sun/star/task/XStatusIndicator.hpp>
#endif
#ifndef _COM_SUN_STAR_TASK_XSTATUSINDICATORFACTORY_HPP_
#include <com/sun/star/task/XStatusIndicatorFactory.hpp>
#endif
#ifndef _COM_SUN_STAR_STYLE_XSTYLEFAMILIESSUPPLIER_HPP_
#include <com/sun/star/style/XStyleFamiliesSupplier.hpp>
#endif
#ifndef _COM_SUN_STAR_STYLE_XSTYLELOADER_HPP_
#include <com/sun/star/style/XStyleLoader.hpp>
#endif

#ifndef _COM_SUN_STAR_IO_XINPUTSTREAM_HPP_
#include <com/sun/star/io/XInputStream.hpp>
#endif

#ifndef _COM_SUN_STAR_DOCUMENT_XEXTENDEDFILTERDETECTION_HPP_
#include <com/sun/star/document/XExtendedFilterDetection.hpp>
#endif

#ifndef _COM_SUN_STAR_CONTAINER_XNAMEACCESS_HPP_
#include <com/sun/star/container/XNamesAccess.hpp>
#endif

#ifndef _COM_SUN_STAR_BEANS_PROPERTYSTATE_HPP_
#include <com/sun/star/beans/PropertyState.hpp>
#endif

#ifndef _COM_SUN_STAR_UCB_XSIMPLEFILEACCESS_HPP_
#include <com/sun/star/ucb/XSimpleFileAccess.hpp>
#endif
#ifndef _COM_SUN_STAR_BEANS_XPROPERTYSET_HPP_
#include <com/sun/star/beans/XPropertySet.hpp>
#endif

using rtl::OUString;
using com::sun::star::uno::Sequence;
using com::sun::star::uno::Reference;
using com::sun::star::uno::Any;
using com::sun::star::uno::UNO_QUERY;
using com::sun::star::uno::XInterface;
using com::sun::star::uno::Exception;
using com::sun::star::uno::RuntimeException;
using com::sun::star::lang::XMultiServiceFactory;
using com::sun::star::io::XActiveDataSource;
using com::sun::star::io::XOutputStream;
using com::sun::star::beans::PropertyValue;
using com::sun::star::document::XExporter;
using com::sun::star::document::XFilter;
using com::sun::star::document::XExtendedFilterDetection;

using com::sun::star::io::XInputStream;
using com::sun::star::document::XImporter;
using com::sun::star::xml::sax::InputSource;
using com::sun::star::xml::sax::XDocumentHandler;
using com::sun::star::xml::sax::XParser;
using com::sun::star::task::XInteractionHandler;

using namespace ::com::sun::star::frame;
using namespace ::com::sun::star;
using namespace com::sun::star::container;
using namespace com::sun::star::uno;
using namespace com::sun::star::beans;
using namespace com::sun::star::ucb;

Reference< com::sun::star::frame::XModel > xModel;

::rtl::OUString SAL_CALL supportedByType( const ::rtl::OUString clipBoardFormat  ,  const ::rtl::OString resultString, const ::rtl::OUString checkType);

::rtl::OUString SAL_CALL FilterDetect::detect( com::sun::star::uno::Sequence< com::sun::star::beans::PropertyValue >& aArguments ) throw( com::sun::star::uno::RuntimeException )
{
    ::rtl::OUString sTypeName = OUString::createFromAscii("");
    ::rtl::OUString sUrl;
    ::rtl::OUString originalTypeName;
    Sequence<PropertyValue > lProps ;

    com::sun::star::uno::Reference< com::sun::star::io::XInputStream > xInStream;
    ::rtl::OUString temp;
    OSL_ENSURE( sal_False, "starting Detect" );
    const PropertyValue * pValue = aArguments.getConstArray();
    sal_Int32 nLength;
    ::rtl::OString resultString;
    sal_Int32 location=0;

    nLength = aArguments.getLength();
    for ( sal_Int32 i = 0 ; i < nLength; i++)
    {
        if ( pValue[i].Name.equalsAsciiL ( RTL_CONSTASCII_STRINGPARAM ( "TypeName" ) ) )
        {
            //pValue[i].Value >>= originalTypeName;
            location=i;
            // OSL_ENSURE( sal_False, ::rtl::OUStringToOString(sTypeName,RTL_TEXTENCODING_ASCII_US).getStr() );
        }
        else if ( pValue[i].Name.equalsAsciiL ( RTL_CONSTASCII_STRINGPARAM ( "URL" ) ) )
        {

            pValue[i].Value >>= sUrl;
            //OSL_ENSURE( sal_False, ::rtl::OUStringToOString(sUrl,RTL_TEXTENCODING_ASCII_US).getStr() );
        }
    }
    try{
    Reference< XSimpleFileAccess > xSFI( mxMSF->createInstance
                ( OUString::createFromAscii( "com.sun.star.ucb.SimpleFileAccess" ) ), UNO_QUERY );
        //OSL_ENSURE( sal_False, "Opening file" );
      try
      {
          xInStream = xSFI->openFileRead( sUrl);
      }
      catch( Exception& )
               //catch( Exception& e )
      {
        OSL_ENSURE( sal_False, " No InputStream" );
            return sTypeName;
      }
         //OSL_ENSURE( sal_False, "file open" );
        com::sun::star::uno::Sequence< sal_Int8 > aData;
        if (!xInStream.is())
        {
        OSL_ENSURE( sal_False, " No InputStream" );
            return sTypeName;
        }
       // OSL_ENSURE( sal_False, "file read" );
        long nBytesToRead=xInStream->available();
        xInStream->skipBytes (0);
        long bytestRead =xInStream->readBytes (aData,  1000);
        resultString=::rtl::OString((const sal_Char *)aData.getConstArray(),bytestRead) ;
        //OSL_ENSURE( sal_False, "file read finished" );
        // test typedetect code
        xInStream->closeInput();
        Reference <XNameAccess> xTypeCont(mxMSF->createInstance(OUString::createFromAscii("com.sun.star.document.TypeDetection")),UNO_QUERY);
        Sequence < ::rtl::OUString > myTypes= xTypeCont->getElementNames();
        nLength = myTypes.getLength();
        sal_Int32 new_nlength=0;
        sal_Int32 i = 0 ;
        while(  (i < nLength) && (sTypeName.equalsAscii("")))
        {
            Any elem = xTypeCont->getByName(myTypes[i]);
            elem >>=lProps;
            new_nlength = lProps.getLength();
            sal_Int32 j =0;

            while( j < new_nlength && sTypeName.equalsAscii(""))
            {
                ::rtl::OUString tmpStr =OUString::createFromAscii("");
                lProps[j].Value >>=tmpStr;
                //OSL_ENSURE( sal_False,::rtl::OUStringToOString(tmpStr,RTL_TEXTENCODING_ASCII_US).getStr());
                if((lProps[j].Name.equalsAscii("ClipboardFormat")) && (!tmpStr.equalsAscii("")) )
                {
                    //OSL_ENSURE( sal_False, "found" );
                    sTypeName = supportedByType(tmpStr,resultString, myTypes[i]);
                }
               j++;
            }
            i++;
        }
        //end test
    }
    catch(Exception &)
    {
        OSL_ENSURE( sal_False, "An Exception occured while opening File stream" );
    }

    if(sTypeName.equalsAscii(""))
    {
        //sTypeName=::rtl::OUString::createFromAscii("devguide_FlatXML_Cpp");
    }
    else
    {
        aArguments[location].Value <<=sTypeName;
    }
    //OSL_ENSURE( sal_False, ::rtl::OUStringToOString(sTypeName,RTL_TEXTENCODING_ASCII_US).getStr() );
    return sTypeName;
}

::rtl::OUString SAL_CALL supportedByType( const ::rtl::OUString clipBoardFormat ,  const ::rtl::OString resultString, const ::rtl::OUString checkType)
{
    sal_Int32 i=0;
    sal_Int32 checked =0;
    ::rtl::OUString sTypeName= OUString::createFromAscii("");
    if((clipBoardFormat.match(OUString::createFromAscii("doctype:"))))
    {
            ::rtl::OString tryStr = ::rtl::OUStringToOString(clipBoardFormat.copy(8),RTL_TEXTENCODING_ASCII_US).getStr();
            // OSL_ENSURE( sal_False, tryStr);
            while((checked <=resultString.getLength())&& (sTypeName.equalsAscii("")))
            {
                if( resultString.match(tryStr,checked))
                {
                    sTypeName = checkType;
                    break;
                }
                checked++;
            }
    }
    return sTypeName;
}

// XInitialization
void SAL_CALL FilterDetect::initialize( const Sequence< Any >& aArguments )
    throw (Exception, RuntimeException)
{
    Sequence < PropertyValue > aAnySeq;
    sal_Int32 nLength = aArguments.getLength();
    if ( nLength && ( aArguments[0] >>= aAnySeq ) )
    {
        const PropertyValue * pValue = aAnySeq.getConstArray();
        nLength = aAnySeq.getLength();
        for ( sal_Int32 i = 0 ; i < nLength; i++)
        {
            if ( pValue[i].Name.equalsAsciiL ( RTL_CONSTASCII_STRINGPARAM ( "Type" ) ) )
            {
                pValue[i].Value >>= msFilterName;
            }
            else if ( pValue[i].Name.equalsAsciiL ( RTL_CONSTASCII_STRINGPARAM ( "UserData" ) ) )
            {
                pValue[i].Value >>= msUserData;
            }
            else if ( pValue[i].Name.equalsAsciiL ( RTL_CONSTASCII_STRINGPARAM ( "TemplateName" ) ) )
            {
                pValue[i].Value>>=msTemplateName;
            }
        }
    }
}

OUString FilterDetect_getImplementationName ()
    throw (RuntimeException)
{
    return OUString ( RTL_CONSTASCII_USTRINGPARAM ( "com.sun.star.comp.filters.FlatXMLFilterDetect" ) );
}

#define SERVICE_NAME1 "com.sun.star.document.ExtendedTypeDetection"

sal_Bool SAL_CALL FilterDetect_supportsService( const OUString& ServiceName )
    throw (RuntimeException)
{
    return ServiceName.equalsAsciiL( RTL_CONSTASCII_STRINGPARAM ( SERVICE_NAME1 ) );
}

Sequence< OUString > SAL_CALL FilterDetect_getSupportedServiceNames(  )
    throw (RuntimeException)
{
    Sequence < OUString > aRet(2);
    OUString* pArray = aRet.getArray();
    pArray[0] =  OUString ( RTL_CONSTASCII_USTRINGPARAM ( SERVICE_NAME1 ) );
    return aRet;
}
#undef SERVICE_NAME1
#undef SERVICE_NAME2

Reference< XInterface > SAL_CALL FilterDetect_createInstance( const Reference< XMultiServiceFactory > & rSMgr)
    throw( Exception )
{
    return (cppu::OWeakObject*) new FilterDetect( rSMgr );
}

// XServiceInfo
OUString SAL_CALL FilterDetect::getImplementationName(  )
    throw (RuntimeException)
{
    return FilterDetect_getImplementationName();
}

sal_Bool SAL_CALL FilterDetect::supportsService( const OUString& rServiceName )
    throw (RuntimeException)
{
    return FilterDetect_supportsService( rServiceName );
}

Sequence< OUString > SAL_CALL FilterDetect::getSupportedServiceNames(  )
    throw (RuntimeException)
{
    return FilterDetect_getSupportedServiceNames();
}
<commit_msg>INTEGRATION: CWS fwkmerge1 (1.3.62); FILE MERGED 2004/02/03 15:47:05 lo 1.3.62.1: #115238# merging up #111125, #111126 and #111127 from fwk01pp1 (flatxml filter)<commit_after>/*************************************************************************
 *
 *  $RCSfile: filterdetect.cxx,v $
 *
 *  $Revision: 1.4 $
 *
 *  last change: $Author: hr $ $Date: 2004-03-09 10:10:54 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  the BSD license.
 *
 *  Copyright (c) 2003 by Sun Microsystems, 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:
 *  1. Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *  2. Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *  3. Neither the name of Sun Microsystems, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 *  OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 *  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
 *  TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
 *  USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *************************************************************************/

#include "filterdetect.hxx"

#ifndef _OSL_DIAGNOSE_H_
#include <osl/diagnose.h>
#endif
#ifndef _COM_SUN_STAR_LANG_XMULTISERVICEFACTORY_HPP_
#include <com/sun/star/lang/XMultiServiceFactory.hpp>
#endif
#ifndef _COM_SUN_STAR_IO_XACTIVEDATASOURCE_HPP_
#include <com/sun/star/io/XActiveDataSource.hpp>
#endif
#ifndef _COM_SUN_STAR_IO_XOUTPUTSTREAM_HPP_
#include <com/sun/star/io/XOutputStream.hpp>
#endif
#ifndef _COM_SUN_STAR_IO_XINPUTSTREAM_HPP_
#include <com/sun/star/io/XInputStream.hpp>
#endif
#ifndef _COM_SUN_STAR_XML_SAX_XDOCUMENTHANDLER_HPP_
#include <com/sun/star/xml/sax/XDocumentHandler.hpp>
#endif
#ifndef _COM_SUN_STAR_XML_SAX_INPUTSOURCE_HPP_
#include <com/sun/star/xml/sax/InputSource.hpp>
#endif
#ifndef _COM_SUN_STAR_XML_SAX_XPARSER_HPP_
#include <com/sun/star/xml/sax/XParser.hpp>
#endif
#ifndef _COM_SUN_STAR_XML_XIMPORTFILTER_HPP_
#include <com/sun/star/xml/XImportFilter.hpp>
#endif
#ifndef _COM_SUN_STAR_XML_XEXPORTFILTER_HPP_
#include <com/sun/star/xml/XExportFilter.hpp>
#endif
#ifndef _COM_SUN_STAR_FRAME_XMODEL_HPP_
#include <com/sun/star/frame/XModel.hpp>
#endif
#ifndef _COM_SUN_STAR_FRAME_XCONTROLLER_HPP_
#include <com/sun/star/frame/XController.hpp>
#endif
#ifndef  _COM_SUN_STAR_TASK_XSTATUSINDICATOR_HPP_
#include <com/sun/star/task/XStatusIndicator.hpp>
#endif
#ifndef _COM_SUN_STAR_TASK_XSTATUSINDICATORFACTORY_HPP_
#include <com/sun/star/task/XStatusIndicatorFactory.hpp>
#endif
#ifndef _COM_SUN_STAR_STYLE_XSTYLEFAMILIESSUPPLIER_HPP_
#include <com/sun/star/style/XStyleFamiliesSupplier.hpp>
#endif
#ifndef _COM_SUN_STAR_STYLE_XSTYLELOADER_HPP_
#include <com/sun/star/style/XStyleLoader.hpp>
#endif
#ifndef _COM_SUN_STAR_IO_XINPUTSTREAM_HPP_
#include <com/sun/star/io/XInputStream.hpp>
#endif
#ifndef _COM_SUN_STAR_DOCUMENT_XEXTENDEDFILTERDETECTION_HPP_
#include <com/sun/star/document/XExtendedFilterDetection.hpp>
#endif
#ifndef _COM_SUN_STAR_CONTAINER_XNAMEACCESS_HPP_
#include <com/sun/star/container/XNamesAccess.hpp>
#endif
#ifndef _COM_SUN_STAR_BEANS_PROPERTYSTATE_HPP_
#include <com/sun/star/beans/PropertyState.hpp>
#endif
#ifndef _COM_SUN_STAR_UCB_XSIMPLEFILEACCESS_HPP_
#include <com/sun/star/ucb/XSimpleFileAccess.hpp>
#endif
#ifndef _COM_SUN_STAR_BEANS_XPROPERTYSET_HPP_
#include <com/sun/star/beans/XPropertySet.hpp>
#endif

using namespace rtl;
using namespace com::sun::star::uno;
using namespace com::sun::star::document;
using namespace com::sun::star::lang;
using namespace com::sun::star::io;
using namespace com::sun::star::beans;
using namespace com::sun::star::xml::sax;
using namespace com::sun::star::xml;
using namespace com::sun::star::task;
using namespace com::sun::star::frame;
using namespace com::sun::star::container;
using namespace com::sun::star::ucb;

OUString SAL_CALL FilterDetect::detect(Sequence< PropertyValue >& aArguments )
    throw( RuntimeException )
{
    // type name to return
    OUString sOriginalTypeName;
    OUString sTypeName;
    OUString sURL;
    // stream of the document to be detected
    Reference< XInputStream > xInStream;
    for ( sal_Int32 i = 0 ; i < aArguments.getLength(); i++)
    {
        OUString aName = aArguments[i].Name;
        if (aName.equalsAscii("TypeName" ) )
            aArguments[i].Value >>= sOriginalTypeName;
        if (aName.equalsAscii("URL" ) )
            aArguments[i].Value >>= sURL;
        if (aName.equalsAscii("InputStream" ) )
            aArguments[i].Value >>= xInStream;
    }

    if (!xInStream.is())
    {
        // open the stream if it was not suplied by the framework
        Reference< XSimpleFileAccess > xSFI(mxMSF->createInstance(
            OUString::createFromAscii("com.sun.star.ucb.SimpleFileAccess" )), UNO_QUERY);
        if (sURL.getLength() > 0 && xSFI.is())
        {
            try
            {
                xInStream = xSFI->openFileRead( sURL);
            }
            catch( Exception& )
            {
                return sTypeName;
            }
        } else {
            // failed to access UCB
            return sTypeName;
        }
    }

    // flatxml starts with an office:document element. this element
    // conatains a clas="..." attribut by which we can deduct the
    // type of document that is to be loaded
    //
    // WARNING:
    // parsing the plain text of the document is an easy way to do this
    // but not the purest solution, since namespaces and other xml details
    // may lead to another syntactic expression of the same document.
    // this example works for the way the office serializes it's XML stream
    // but might need extension for other data sources...
    static OString aDocToken("office:document");
    static OString aClassToken("office:class=\"");

    sal_Int32 nHeadSize = 4096;
    Sequence< sal_Int8 > aHeadData(nHeadSize);
    long bytestRead = xInStream->readBytes(aHeadData, nHeadSize);

    OString aHead = OString((const sal_Char *)aHeadData.getConstArray(), bytestRead).toAsciiLowerCase();

    // check for document element of flatxml format
    if (aHead.indexOf(aDocToken) >= 0)
    {
        // read document class
        sal_Int32 n = aHead.indexOf(aClassToken);
        if (n >= 0)
        {
            n += aClassToken.getLength();
            OString aClass = aHead.copy(n, aHead.indexOf('\"', n) - n);
            // return type for class found
            if (aClass.equals("text"))
                sTypeName = OUString::createFromAscii("devguide_FlatXML_Cpp_writer");
            else if (aClass.equals("spreadsheet"))
                sTypeName = OUString::createFromAscii("devguide_FlatXML_Cpp_calc");
            else if (aClass.equals("drawing"))
                sTypeName = OUString::createFromAscii("devguide_FlatXML_Cpp_draw");
            else if (aClass.equals("presentation"))
                sTypeName = OUString::createFromAscii("devguide_FlatXML_Cpp_impress");
        }
    }
    return sTypeName;
}

// XInitialization
void SAL_CALL FilterDetect::initialize( const Sequence< Any >& aArguments )
    throw (Exception, RuntimeException)
{
    Sequence < PropertyValue > aAnySeq;
    sal_Int32 nLength = aArguments.getLength();
    if ( nLength && ( aArguments[0] >>= aAnySeq ) )
    {
        const PropertyValue * pValue = aAnySeq.getConstArray();
        nLength = aAnySeq.getLength();
        for ( sal_Int32 i = 0 ; i < nLength; i++)
        {
            if ( pValue[i].Name.equalsAsciiL ( RTL_CONSTASCII_STRINGPARAM ( "Type" ) ) )
            {
                pValue[i].Value >>= msFilterName;
            }
            else if ( pValue[i].Name.equalsAsciiL ( RTL_CONSTASCII_STRINGPARAM ( "UserData" ) ) )
            {
                pValue[i].Value >>= msUserData;
            }
            else if ( pValue[i].Name.equalsAsciiL ( RTL_CONSTASCII_STRINGPARAM ( "TemplateName" ) ) )
            {
                pValue[i].Value>>=msTemplateName;
            }
        }
    }
}

OUString FilterDetect_getImplementationName ()
    throw (RuntimeException)
{
    return OUString ( RTL_CONSTASCII_USTRINGPARAM ( "devguide.officedev.samples.filter.FlatXmlDetect" ) );
}

#define SERVICE_NAME1 "com.sun.star.document.ExtendedTypeDetection"

sal_Bool SAL_CALL FilterDetect_supportsService( const OUString& ServiceName )
    throw (RuntimeException)
{
    return ServiceName.equalsAsciiL( RTL_CONSTASCII_STRINGPARAM ( SERVICE_NAME1 ) );
}

Sequence< OUString > SAL_CALL FilterDetect_getSupportedServiceNames(  )
    throw (RuntimeException)
{
    Sequence < OUString > aRet(2);
    OUString* pArray = aRet.getArray();
    pArray[0] =  OUString ( RTL_CONSTASCII_USTRINGPARAM ( SERVICE_NAME1 ) );
    return aRet;
}
#undef SERVICE_NAME1
#undef SERVICE_NAME2

Reference< XInterface > SAL_CALL FilterDetect_createInstance( const Reference< XMultiServiceFactory > & rSMgr)
    throw( Exception )
{
    return (cppu::OWeakObject*) new FilterDetect( rSMgr );
}

// XServiceInfo
OUString SAL_CALL FilterDetect::getImplementationName(  )
    throw (RuntimeException)
{
    return FilterDetect_getImplementationName();
}

sal_Bool SAL_CALL FilterDetect::supportsService( const OUString& rServiceName )
    throw (RuntimeException)
{
    return FilterDetect_supportsService( rServiceName );
}

Sequence< OUString > SAL_CALL FilterDetect::getSupportedServiceNames(  )
    throw (RuntimeException)
{
    return FilterDetect_getSupportedServiceNames();
}
<|endoftext|>


<commit_before>// Test for __lsan_(un)register_root_region().
// RUN: LSAN_BASE="use_stacks=0:use_registers=0"
// RUN: %clangxx_lsan %s -o %t
// RUN: LSAN_OPTIONS=$LSAN_BASE %run %t
// RUN: LSAN_OPTIONS=$LSAN_BASE not %run %t foo 2>&1 | FileCheck %s
// RUN: LSAN_OPTIONS=$LSAN_BASE:use_root_regions=0 not %run %t 2>&1 | FileCheck %s

#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/mman.h>
#include <unistd.h>

#include <sanitizer/lsan_interface.h>

int main(int argc, char *argv[]) {
  size_t size = getpagesize() * 2;
  void *p =
      mmap(0, size, PROT_READ | PROT_WRITE, MAP_PRIVATE | MAP_ANONYMOUS, -1, 0);
  assert(p);
  // Make half of the memory inaccessible. LSan must not crash trying to read it.
  assert(0 == mprotect((char *)p + size / 2, size / 2, PROT_NONE));

  __lsan_register_root_region(p, size);
  *((void **)p) = malloc(1337);
  fprintf(stderr, "Test alloc: %p.\n", p);
  if (argc > 1)
    __lsan_unregister_root_region(p, size);
  return 0;
}
// CHECK: Test alloc: [[ADDR:.*]].
// CHECK: SUMMARY: {{(Leak|Address)}}Sanitizer: 1337 byte(s) leaked in 1 allocation(s)
<commit_msg>[lsan] Define MAP_ANONYMOUS as MAP_ANON for OSX in the test.<commit_after>// Test for __lsan_(un)register_root_region().
// RUN: LSAN_BASE="use_stacks=0:use_registers=0"
// RUN: %clangxx_lsan %s -o %t
// RUN: LSAN_OPTIONS=$LSAN_BASE %run %t
// RUN: LSAN_OPTIONS=$LSAN_BASE not %run %t foo 2>&1 | FileCheck %s
// RUN: LSAN_OPTIONS=$LSAN_BASE:use_root_regions=0 not %run %t 2>&1 | FileCheck %s

#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/mman.h>
#include <unistd.h>

#include <sanitizer/lsan_interface.h>

// OSX doesn't have MAP_ANONYMOUS.
#ifndef MAP_ANONYMOUS
#  define MAP_ANONYMOUS MAP_ANON
#endif

int main(int argc, char *argv[]) {
  size_t size = getpagesize() * 2;
  void *p =
      mmap(0, size, PROT_READ | PROT_WRITE, MAP_PRIVATE | MAP_ANONYMOUS, -1, 0);
  assert(p);
  // Make half of the memory inaccessible. LSan must not crash trying to read it.
  assert(0 == mprotect((char *)p + size / 2, size / 2, PROT_NONE));

  __lsan_register_root_region(p, size);
  *((void **)p) = malloc(1337);
  fprintf(stderr, "Test alloc: %p.\n", p);
  if (argc > 1)
    __lsan_unregister_root_region(p, size);
  return 0;
}
// CHECK: Test alloc: [[ADDR:.*]].
// CHECK: SUMMARY: {{(Leak|Address)}}Sanitizer: 1337 byte(s) leaked in 1 allocation(s)
<|endoftext|>


<commit_before>/*
   DDS, a bridge double dummy solver.

   Copyright (C) 2006-2014 by Bo Haglund /
   2014-2018 by Bo Haglund & Soren Hein.

   See LICENSE and README.
*/

/*
   ABstats is a simple object for AB statistics and return values.
*/

#include "dds.h"
#include "ABstats.h"

ABstats::ABstats()
{
  fname = "";
  fileSet = false;
  fp = stdout;

  for (int p = 0; p < DDS_AB_POS; p++)
    sprintf(name[p], "Position %4d", p);

  ABstats::Reset();
}

ABstats::~ABstats()
{
  // Nothing to do
  if (fp != stdout && fp != nullptr)
    fclose(fp);
}

void ABstats::Reset()
{
  for (int p = 0; p < DDS_AB_POS; p++)
  {
    sumNew[p] = 0;
    psumNew[p] = 0;

    for (int depth = 0; depth < DDS_MAXDEPTH; depth++)
      counter[p][depth] = 0;
  }

  for (int side = 0; side < 2; side++)
  {
    for (int depth = 0; depth < DDS_MAXDEPTH; depth++)
      score[side][depth] = 0;
  }

  for (int depth = 0; depth < DDS_MAXDEPTH; depth++)
    nodes[depth] = 0;

  allnodes = 0;

  for (int side = 0; side < 2; side++)
  {
    for (int depth = 0; depth < DDS_MAXDEPTH; depth++)
      ABsides[side].list[depth] = 0;

    ABsides[side].sum = 0;
    ABsides[side].sumWeighted = 0;
  }

  for (int place = 0; place < DDS_AB_POS; place++)
  {
    for (int depth = 0; depth < DDS_MAXDEPTH; depth++)
      ABplaces[place].list[depth] = 0;

    ABplaces[place].sum = 0;
    ABplaces[place].sumWeighted = 0;
  }
}

void ABstats::ResetCum()
{
  for (int depth = 0; depth < DDS_MAXDEPTH; depth++)
    nodesCum[depth] = 0;

  allnodesCum = 0;

  scoreCum[1] = 0;
  scoreCum[0] = 0;

  pscoreCum[1] = 0;
  pscoreCum[0] = 0;

  for (int p = 0; p < DDS_AB_POS; p++)
  {
    counterCum[p] = 0;
    pcounterCum[p] = 0;
  }

  for (int side = 0; side < 2; side++)
  {
    ABsides[side].sumCum = 0;
    ABsides[side].sumCumWeighted = 0;
  }

  for (int place = 0; place < DDS_AB_POS; place++)
  {
    ABplaces[place].sumCum = 0;
    ABplaces[place].sumCumWeighted = 0;
  }
}

void ABstats::SetFile(const string& fnameIn)
{
  fname = fnameIn;
}

void ABstats::SetName(int no, char * ourName)
{
  if (no < 0 || no >= DDS_AB_POS)
    return;

  sprintf(name[no], "%s", ourName);
}

void ABstats::IncrPos(
  int no,
  bool side,
  int depth)
{
  if (no < 0 || no >= DDS_AB_POS)
    return;

  counter[no][depth]++;
  sumNew[no]++;
  psumNew[no] += depth;

  ABplaces[no].list[depth]++;
  ABplaces[no].sum++;
  ABplaces[no].sumWeighted += depth;
  ABplaces[no].sumCum++;
  ABplaces[no].sumCumWeighted += depth;

  const int iside = (side ? 1 : 0);

  score[iside][depth]++;

  ABsides[iside].list[depth]++;
  ABsides[iside].sum++;
  ABsides[iside].sumWeighted += depth;
  ABsides[iside].sumCum++;
  ABsides[iside].sumCumWeighted += depth;

}

void ABstats::IncrNode(int depth)
{
  nodes[depth]++;
  nodesCum[depth]++;
  allnodes++;
}

int ABstats::GetNodes() const
{
  return allnodes;
}

#include "../include/portab.h"
void ABstats::PrintStatsPosition(FILE * fpl) const
{
  UNUSED(fpl);
}

void ABstats::PrintStats()
{
  if (! fileSet)
  {
    if (fname != "")
    {
      fp = fopen(fname.c_str(), "w");
      if (! fp)
        fp = stdout;
    }
    fileSet = true;
  }

  allnodesCum += allnodes;

  // sumNew[p], ABplaces[p].sum
  // psumNew[p], ABplaces[p].sumWeighted
  // counterCum[p], ABplaces[p].sumCum
  // pcounterCum[p], ABplaces[p].sumCumWeighted

  for (int p = 0; p < DDS_AB_POS; p++)
  {
    counterCum[p] += sumNew[p];
    pcounterCum[p] += psumNew[p];
  }

for (int p = 0; p < DDS_AB_POS; p++)
{
  if (ABplaces[p].sum != sumNew[p])
    fprintf(fp, "sum %d: %d %d\n", p, ABplaces[p].sum, sumNew[p]);

  if (ABplaces[p].sumWeighted != psumNew[p])
    fprintf(fp, "sumw %d: %d %d\n", p, ABplaces[p].sumWeighted, psumNew[p]);

  if (ABplaces[p].sumCum != counterCum[p])
    fprintf(fp, "sumc %d: %d %d\n", p, ABplaces[p].sumCum, counterCum[p]);

  if (ABplaces[p].sumCumWeighted != pcounterCum[p])
    fprintf(fp, "sumcw %d: %d %d\n", p, ABplaces[p].sumCumWeighted, pcounterCum[p]);

}

  int s = ABsides[1].sum + ABsides[0].sum;
  int cs = ABsides[1].sumCum + ABsides[0].sumCum;
  if (s)
  {
    fprintf(fp, "%2s %-20s %8s %5s %5s %8s %5s %5s\n",
            "No",
            "Return",
            "Count",
            "%",
            "d_avg",
            "Cumul",
            "%",
            "d_avg");

    fprintf(fp, "-----------------------------------");
    fprintf(fp, "------------------------------\n");

    fprintf(fp, "%2s %-20s %8d %5.1f %5.1f %8d %5.1f %5.1f\n",
            "",
            "Side1",
            ABsides[1].sum,
            100. * ABsides[1].sum / static_cast<double>(s),
            ABsides[1].sumWeighted / static_cast<double>(s),
            ABsides[1].sumCum,
            100. * ABsides[1].sumCum / static_cast<double>(cs),
            ABsides[1].sumCumWeighted / static_cast<double>(cs));

    fprintf(fp, "%2s %-20s %8d %5.1f %5.1f %8d %5.1f %5.1f\n\n",
            "",
            "Side0",
            ABsides[0].sum,
            100. * ABsides[0].sum / static_cast<double>(s),
            ABsides[0].sumWeighted / static_cast<double>(s),
            ABsides[0].sumCum,
            100. * ABsides[0].sumCum / static_cast<double>(cs),
            ABsides[0].sumCumWeighted / static_cast<double>(cs));

    for (int p = 0; p < DDS_AB_POS; p++)
    {
      if (sumNew[p])
      {
        fprintf(fp, "%2d %-20s %8d %5.1f %5.1f %8d %5.1f %5.1f\n",
                p,
                name[p],
                sumNew[p],
                100. * sumNew[p] / static_cast<double>(s),
                psumNew[p] / static_cast<double>(sumNew[p]),
                counterCum[p],
                100. * counterCum[p] / static_cast<double>(cs),
                pcounterCum[p] / static_cast<double>(counterCum[p]));
      }
      else if (counterCum[p])
      {
        fprintf(fp, "%2d %-20s %8d %5.1f %5s %8d %5.1f %5.1f\n",
                p,
                name[p],
                sumNew[p],
                100. * sumNew[p] / static_cast<double>(s),
                "",
                counterCum[p],
                100. * counterCum[p] / static_cast<double>(cs),
                pcounterCum[p] / static_cast<double>(counterCum[p]));
      }
    }
  }

  fprintf(fp, "\n%5s %6s %6s %5s %5s %6s\n",
          "Depth",
          "Nodes",
          "Cumul",
          "Cum%",
          "Cumc%",
          "Branch");

  fprintf(fp, "------------------------------------------\n");

  int c = 0;
  double np = 0., ncp = 0.;
  for (int d = DDS_MAXDEPTH - 1; d >= 0; d--)
  {
    if (nodesCum[d] == 0)
      continue;

    c += nodesCum[d];
    np += d * nodes[d];
    ncp += d * nodesCum[d];

    fprintf(fp, "%5d %6d %6d %5.1f %5.1f",
            d,
            nodes[d],
            nodesCum[d],
            100. * nodesCum[d] / static_cast<double>(allnodesCum),
            100. * c / static_cast<double>(allnodesCum));

    // "Branching factor" from end of one trick to end of
    // the previous trick.
    if ((d % 4 == 1) &&
        (d < DDS_MAXDEPTH - 4) &&
        (nodesCum[d + 4] > 0))
      fprintf(fp, " %5.2f",
              nodesCum[d] / static_cast<double>(nodesCum[d + 4]));
    fprintf(fp, "\n");
  }

  fprintf(fp, "\n%-5s %6d %6d\n",
          "Total", allnodes, allnodesCum);

  if (allnodes)
  {
    fprintf(fp, "%-5s %6.1f %6.1f\n",
            "d_avg",
            np / static_cast<double>(allnodes),
            ncp / static_cast<double>(allnodesCum));
  }
  else if (allnodesCum)
  {
    fprintf(fp, "\n%-5s %6s %6.1f\n",
            "Avg",
            "-",
            ncp / static_cast<double>(allnodesCum));
  }

  fprintf(fp, "%-5s %6d\n\n\n", "Diff",
          allnodes - ABsides[1].sum - ABsides[0].sum);

#ifdef DDS_AB_DETAILS
  fprintf(fp, "%2s %6s %6s",
          "d",
          "Side1",
          "Side0");

  for (int p = 0; p < DDS_AB_POS; p++)
    fprintf(fp, " %5d", p);
  fprintf(fp, "\n------------------------------");
  fprintf(fp, "-----------------------------\n");

  for (int d = DDS_MAXDEPTH - 1; d >= 0; d--)
  {
    if (score[1][d] == 0 && score[0][d] == 0)
      continue;

    fprintf(fp, "%2d %6d %6d",
            d, score[1][d], score[0][d]);

    for (int p = 0; p < DDS_AB_POS; p++)
      fprintf(fp, " %5d", counter[p][d]);
    fprintf(fp, "\n");
  }

  fprintf(fp, "--------------------------------");
  fprintf(fp, "---------------------------\n");

  fprintf(fp, "%2s %6d %6d",
          "S", ABsides[1].sum, ABsides[0].sum);

  for (int p = 0; p < DDS_AB_POS; p++)
    fprintf(fp, " %5d", sumNew[p]);
  fprintf(fp, "\n\n");
#endif

}
<commit_msg>Further step<commit_after>/*
   DDS, a bridge double dummy solver.

   Copyright (C) 2006-2014 by Bo Haglund /
   2014-2018 by Bo Haglund & Soren Hein.

   See LICENSE and README.
*/

/*
   ABstats is a simple object for AB statistics and return values.
*/

#include "dds.h"
#include "ABstats.h"

ABstats::ABstats()
{
  fname = "";
  fileSet = false;
  fp = stdout;

  for (int p = 0; p < DDS_AB_POS; p++)
    sprintf(name[p], "Position %4d", p);

  ABstats::Reset();
}

ABstats::~ABstats()
{
  // Nothing to do
  if (fp != stdout && fp != nullptr)
    fclose(fp);
}

void ABstats::Reset()
{
  for (int p = 0; p < DDS_AB_POS; p++)
  {
    sumNew[p] = 0;
    psumNew[p] = 0;

    for (int depth = 0; depth < DDS_MAXDEPTH; depth++)
      counter[p][depth] = 0;
  }

  for (int side = 0; side < 2; side++)
  {
    for (int depth = 0; depth < DDS_MAXDEPTH; depth++)
      score[side][depth] = 0;
  }

  for (int depth = 0; depth < DDS_MAXDEPTH; depth++)
    nodes[depth] = 0;

  allnodes = 0;

  for (int side = 0; side < 2; side++)
  {
    for (int depth = 0; depth < DDS_MAXDEPTH; depth++)
      ABsides[side].list[depth] = 0;

    ABsides[side].sum = 0;
    ABsides[side].sumWeighted = 0;
  }

  for (int place = 0; place < DDS_AB_POS; place++)
  {
    for (int depth = 0; depth < DDS_MAXDEPTH; depth++)
      ABplaces[place].list[depth] = 0;

    ABplaces[place].sum = 0;
    ABplaces[place].sumWeighted = 0;
  }
}

void ABstats::ResetCum()
{
  for (int depth = 0; depth < DDS_MAXDEPTH; depth++)
    nodesCum[depth] = 0;

  allnodesCum = 0;

  scoreCum[1] = 0;
  scoreCum[0] = 0;

  pscoreCum[1] = 0;
  pscoreCum[0] = 0;

  for (int p = 0; p < DDS_AB_POS; p++)
  {
    counterCum[p] = 0;
    pcounterCum[p] = 0;
  }

  for (int side = 0; side < 2; side++)
  {
    ABsides[side].sumCum = 0;
    ABsides[side].sumCumWeighted = 0;
  }

  for (int place = 0; place < DDS_AB_POS; place++)
  {
    ABplaces[place].sumCum = 0;
    ABplaces[place].sumCumWeighted = 0;
  }
}

void ABstats::SetFile(const string& fnameIn)
{
  fname = fnameIn;
}

void ABstats::SetName(int no, char * ourName)
{
  if (no < 0 || no >= DDS_AB_POS)
    return;

  sprintf(name[no], "%s", ourName);
}

void ABstats::IncrPos(
  int no,
  bool side,
  int depth)
{
  if (no < 0 || no >= DDS_AB_POS)
    return;

  counter[no][depth]++;
  sumNew[no]++;
  psumNew[no] += depth;

  ABplaces[no].list[depth]++;
  ABplaces[no].sum++;
  ABplaces[no].sumWeighted += depth;
  ABplaces[no].sumCum++;
  ABplaces[no].sumCumWeighted += depth;

  const int iside = (side ? 1 : 0);

  score[iside][depth]++;

  ABsides[iside].list[depth]++;
  ABsides[iside].sum++;
  ABsides[iside].sumWeighted += depth;
  ABsides[iside].sumCum++;
  ABsides[iside].sumCumWeighted += depth;

}

void ABstats::IncrNode(int depth)
{
  nodes[depth]++;
  nodesCum[depth]++;
  allnodes++;
}

int ABstats::GetNodes() const
{
  return allnodes;
}

#include "../include/portab.h"
void ABstats::PrintStatsPosition(FILE * fpl) const
{
  UNUSED(fpl);
}

void ABstats::PrintStats()
{
  if (! fileSet)
  {
    if (fname != "")
    {
      fp = fopen(fname.c_str(), "w");
      if (! fp)
        fp = stdout;
    }
    fileSet = true;
  }

  allnodesCum += allnodes;

  int s = ABsides[1].sum + ABsides[0].sum;
  int cs = ABsides[1].sumCum + ABsides[0].sumCum;
  if (s)
  {
    fprintf(fp, "%2s %-20s %8s %5s %5s %8s %5s %5s\n",
            "No",
            "Return",
            "Count",
            "%",
            "d_avg",
            "Cumul",
            "%",
            "d_avg");

    fprintf(fp, "-----------------------------------");
    fprintf(fp, "------------------------------\n");

    fprintf(fp, "%2s %-20s %8d %5.1f %5.1f %8d %5.1f %5.1f\n",
            "",
            "Side1",
            ABsides[1].sum,
            100. * ABsides[1].sum / static_cast<double>(s),
            ABsides[1].sumWeighted / static_cast<double>(s),
            ABsides[1].sumCum,
            100. * ABsides[1].sumCum / static_cast<double>(cs),
            ABsides[1].sumCumWeighted / static_cast<double>(cs));

    fprintf(fp, "%2s %-20s %8d %5.1f %5.1f %8d %5.1f %5.1f\n\n",
            "",
            "Side0",
            ABsides[0].sum,
            100. * ABsides[0].sum / static_cast<double>(s),
            ABsides[0].sumWeighted / static_cast<double>(s),
            ABsides[0].sumCum,
            100. * ABsides[0].sumCum / static_cast<double>(cs),
            ABsides[0].sumCumWeighted / static_cast<double>(cs));

    for (int p = 0; p < DDS_AB_POS; p++)
    {
      if (ABplaces[p].sum)
      {
        fprintf(fp, "%2d %-20s %8d %5.1f %5.1f %8d %5.1f %5.1f\n",
                p,
                name[p],
                ABplaces[p].sum,
                100. * ABplaces[p].sum / static_cast<double>(s),
                ABplaces[p].sumWeighted / static_cast<double>(ABplaces[p].sum),
                ABplaces[p].sumCum,
                100. * ABplaces[p].sumCum / static_cast<double>(cs),
                ABplaces[p].sumCumWeighted / static_cast<double>(ABplaces[p].sumCum));
      }
      else if (ABplaces[p].sumCum)
      {
        fprintf(fp, "%2d %-20s %8d %5.1f %5s %8d %5.1f %5.1f\n",
                p,
                name[p],
                ABplaces[p].sum,
                100. * ABplaces[p].sum / static_cast<double>(s),
                "",
                ABplaces[p].sumCum,
                100. * ABplaces[p].sumCum / static_cast<double>(cs),
                ABplaces[p].sumCumWeighted / static_cast<double>(ABplaces[p].sumCum));
      }
    }
  }

  fprintf(fp, "\n%5s %6s %6s %5s %5s %6s\n",
          "Depth",
          "Nodes",
          "Cumul",
          "Cum%",
          "Cumc%",
          "Branch");

  fprintf(fp, "------------------------------------------\n");

  int c = 0;
  double np = 0., ncp = 0.;
  for (int d = DDS_MAXDEPTH - 1; d >= 0; d--)
  {
    if (nodesCum[d] == 0)
      continue;

    c += nodesCum[d];
    np += d * nodes[d];
    ncp += d * nodesCum[d];

    fprintf(fp, "%5d %6d %6d %5.1f %5.1f",
            d,
            nodes[d],
            nodesCum[d],
            100. * nodesCum[d] / static_cast<double>(allnodesCum),
            100. * c / static_cast<double>(allnodesCum));

    // "Branching factor" from end of one trick to end of
    // the previous trick.
    if ((d % 4 == 1) &&
        (d < DDS_MAXDEPTH - 4) &&
        (nodesCum[d + 4] > 0))
      fprintf(fp, " %5.2f",
              nodesCum[d] / static_cast<double>(nodesCum[d + 4]));
    fprintf(fp, "\n");
  }

  fprintf(fp, "\n%-5s %6d %6d\n",
          "Total", allnodes, allnodesCum);

  if (allnodes)
  {
    fprintf(fp, "%-5s %6.1f %6.1f\n",
            "d_avg",
            np / static_cast<double>(allnodes),
            ncp / static_cast<double>(allnodesCum));
  }
  else if (allnodesCum)
  {
    fprintf(fp, "\n%-5s %6s %6.1f\n",
            "Avg",
            "-",
            ncp / static_cast<double>(allnodesCum));
  }

  fprintf(fp, "%-5s %6d\n\n\n", "Diff",
          allnodes - ABsides[1].sum - ABsides[0].sum);

#ifdef DDS_AB_DETAILS
  fprintf(fp, "%2s %6s %6s",
          "d",
          "Side1",
          "Side0");

  for (int p = 0; p < DDS_AB_POS; p++)
    fprintf(fp, " %5d", p);
  fprintf(fp, "\n------------------------------");
  fprintf(fp, "-----------------------------\n");

  for (int d = DDS_MAXDEPTH - 1; d >= 0; d--)
  {
    if (score[1][d] == 0 && score[0][d] == 0)
      continue;

    fprintf(fp, "%2d %6d %6d",
            d, score[1][d], score[0][d]);

    for (int p = 0; p < DDS_AB_POS; p++)
      fprintf(fp, " %5d", counter[p][d]);
    fprintf(fp, "\n");
  }

  fprintf(fp, "--------------------------------");
  fprintf(fp, "---------------------------\n");

  fprintf(fp, "%2s %6d %6d",
          "S", ABsides[1].sum, ABsides[0].sum);

  for (int p = 0; p < DDS_AB_POS; p++)
    fprintf(fp, " %5d", ABplaces[p].sum);
  fprintf(fp, "\n\n");
#endif

}
<|endoftext|>


<commit_before>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <iostream>
#include <stdexcept>

#include "soundio.hpp"

// TODO those defines should be runtime variables, dependant of the stream

// file dependant
#define SAMPLE_RATE 44100
// file dependant (now : uint16_t)
#define BYTES_PER_SAMPLE 2
// file dependent (now : stereo)
#define CHANNELS_COUNT 2

namespace ORCore {
    // The global soundio instance
    struct SoundIo      *soundio = NULL;
    // The default device instance
    struct SoundIoDevice *device = NULL;

    int soundio_initialize() {
        if (soundio) // Already initialized
            return 0;

        soundio = soundio_create();
        if (!soundio) {
            m_logger->error("SoundIO_create: out of memory");
            throw std::runtime_error("SoundIO_create: out of memory");
        }

        soundio->app_name = APP_NAME;

        int err = soundio_connect(soundio);
        if (err) {
            m_logger->error("SoundIO_connect error: {}", soundio_strerror(err));
            throw std::runtime_error(std::string("SoundIO_connect error: ") + soundio_strerror(err));
        }

        soundio_flush_events(soundio);
        return 0;
    }

    int soundio_quit() {
        soundio_destroy(soundio);
        return 0;
    }

    int soundio_connect_default_output_device() {
        if (device) // Already initialized
            return 0;

        int default_out_device_index = soundio_default_output_device_index(soundio);
        if (default_out_device_index < 0) {
            m_logger->error("SoundIO: No output device found");
            throw std::runtime_error("SoundIO: No output device found");
        }

        device = soundio_get_output_device(soundio, default_out_device_index);
        if (!device) {
            m_logger->error("SoundIO: out of memory");
            throw std::runtime_error("SoundIO: out of memory");
        }

        return 0;
    }

    int soundio_disconnect_device() {
        soundio_device_unref(device);
        return 0;
    }

    void SoundIoOStream::write_callback_static(
        struct SoundIoOutStream *outstream,
        int frame_count_min, int frame_count_max) {
        return ((SoundIoOStream*)outstream->userdata)
            ->write_callback(outstream, frame_count_min, frame_count_max);
    }

    void SoundIoOStream::write_callback(
        struct SoundIoOutStream *outstream,
        int frame_count_min, int frame_count_max) {
        const struct SoundIoChannelLayout *layout = &outstream->layout;
        struct SoundIoChannelArea *areas;
        int err;

        if ((err = soundio_outstream_begin_write(outstream, &areas, &frame_count_max))) {
            m_logger->error("{}", soundio_strerror(err));
            throw runtime_error("SoundIO begin write failed");
        }

        int byte_count_max = frame_count_max* BYTES_PER_SAMPLE*CHANNELS_COUNT;
        int16_t data[byte_count_max];

        for (int bytes_decoded = 0; bytes_decoded < byte_count_max; ) {
            // Most decoders won't decode all bytes we need at once,
            // so we need multiple invocations !
            bytes_decoded += theSong->readBuffer(
                ((char*)data)+ bytes_decoded, byte_count_max - bytes_decoded);
        }

        // Now we copy the data into the outstream !
        for (int i = 0; i < frame_count_max; ++i) {
            for (int channel = 0; channel < layout->channel_count; ++channel) {
                int16_t sample = data[channel + 2*i];
                int16_t *ptr = (int16_t*)(areas[channel].ptr + areas[channel].step * i);
                *ptr = sample;
            }
        }

        if ((err = soundio_outstream_end_write(outstream))) {
            m_logger->error("soundio error : {}\n", soundio_strerror(err));
            throw runtime_error("SoundIO end write failed");
        }
    }

    void SoundIoOStream::underflow_callback_static(struct SoundIoOutStream *outstream) {
        return ((SoundIoOStream*)outstream->userdata)->underflow_callback(outstream);
    }
    void SoundIoOStream::underflow_callback(struct SoundIoOutStream *outstream) {
        static int count = 0;
        m_logger->error("underflow {}\n", count++);
    }

    int SoundIoOStream::setInput(Codec *theSong) {
        this->theSong = theSong;
        return 0;
    }

    int SoundIoOStream::open(double latency) {
        SoundIoFormat format;
        switch(theSong->getBitDepth()) {
            case  8: format = SoundIoFormatS8;
            case 16: format = SoundIoFormatS16NE;
            case 24: format = SoundIoFormatS24NE;
            case 32: format = SoundIoFormatS32NE;
            default: format = SoundIoFormatS16NE;

        }
        return open(format, theSong->getSampleRate(), latency);
    }

    int SoundIoOStream::open(SoundIoFormat format, int sample_rate, double latency) {
        outstream = soundio_outstream_create(device);
        outstream->format           = format;
        outstream->sample_rate      = sample_rate;
        outstream->software_latency = latency;

        outstream->userdata             = this;
        outstream->write_callback       = &SoundIoOStream::write_callback_static;
        outstream->underflow_callback   = &SoundIoOStream::underflow_callback_static;

        int err = soundio_outstream_open(outstream);
        if (err) {
            m_logger->error("unable to open device: {}", soundio_strerror(err));
            throw std::runtime_error(std::string("unable to open device: ") + soundio_strerror(err));
        }

        err = outstream->layout_error;
        if (err) {
            m_logger->error("unable to set channel layout: ", soundio_strerror(err));
            throw std::runtime_error(std::string("unable to set channel layout: ") + soundio_strerror(err));
        }

        err = soundio_outstream_start(outstream);
        if (err) {
            m_logger->error("unable to start device: ", soundio_strerror(err));
            throw std::runtime_error(std::string("unable to start device: ") + soundio_strerror(err));
        }
        return 0;
    }

    void SoundIoOStream::close() {
        soundio_outstream_destroy(outstream);
    }

    int soundio_main(Codec *thesong) {

        soundio_initialize();
        soundio_connect_default_output_device();
        SoundIoOStream *mysoundioostream = new SoundIoOStream();
        mysoundioostream->setInput(thesong);
        mysoundioostream->open(0.01);

        for (;;)
            soundio_wait_events(soundio);

        delete(mysoundioostream);
        soundio_disconnect_device();
        soundio_quit();
        return 0;
    }

} // namespace ORCore
<commit_msg>Stupid fix about std:: missing<commit_after>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <iostream>
#include <stdexcept>

#include "soundio.hpp"

// TODO those defines should be runtime variables, dependant of the stream

// file dependant
#define SAMPLE_RATE 44100
// file dependant (now : uint16_t)
#define BYTES_PER_SAMPLE 2
// file dependent (now : stereo)
#define CHANNELS_COUNT 2

namespace ORCore {
    // The global soundio instance
    struct SoundIo      *soundio = NULL;
    // The default device instance
    struct SoundIoDevice *device = NULL;

    int soundio_initialize() {
        if (soundio) // Already initialized
            return 0;

        soundio = soundio_create();
        if (!soundio) {
            m_logger->error("SoundIO_create: out of memory");
            throw std::runtime_error("SoundIO_create: out of memory");
        }

        soundio->app_name = APP_NAME;

        int err = soundio_connect(soundio);
        if (err) {
            m_logger->error("SoundIO_connect error: {}", soundio_strerror(err));
            throw std::runtime_error(std::string("SoundIO_connect error: ") + soundio_strerror(err));
        }

        soundio_flush_events(soundio);
        return 0;
    }

    int soundio_quit() {
        soundio_destroy(soundio);
        return 0;
    }

    int soundio_connect_default_output_device() {
        if (device) // Already initialized
            return 0;

        int default_out_device_index = soundio_default_output_device_index(soundio);
        if (default_out_device_index < 0) {
            m_logger->error("SoundIO: No output device found");
            throw std::runtime_error("SoundIO: No output device found");
        }

        device = soundio_get_output_device(soundio, default_out_device_index);
        if (!device) {
            m_logger->error("SoundIO: out of memory");
            throw std::runtime_error("SoundIO: out of memory");
        }

        return 0;
    }

    int soundio_disconnect_device() {
        soundio_device_unref(device);
        return 0;
    }

    void SoundIoOStream::write_callback_static(
        struct SoundIoOutStream *outstream,
        int frame_count_min, int frame_count_max) {
        return ((SoundIoOStream*)outstream->userdata)
            ->write_callback(outstream, frame_count_min, frame_count_max);
    }

    void SoundIoOStream::write_callback(
        struct SoundIoOutStream *outstream,
        int frame_count_min, int frame_count_max) {
        const struct SoundIoChannelLayout *layout = &outstream->layout;
        struct SoundIoChannelArea *areas;
        int err;

        if ((err = soundio_outstream_begin_write(outstream, &areas, &frame_count_max))) {
            m_logger->error("{}", soundio_strerror(err));
            throw std::runtime_error("SoundIO begin write failed");
        }

        int byte_count_max = frame_count_max* BYTES_PER_SAMPLE*CHANNELS_COUNT;
        int16_t data[byte_count_max];

        for (int bytes_decoded = 0; bytes_decoded < byte_count_max; ) {
            // Most decoders won't decode all bytes we need at once,
            // so we need multiple invocations !
            bytes_decoded += theSong->readBuffer(
                ((char*)data)+ bytes_decoded, byte_count_max - bytes_decoded);
        }

        // Now we copy the data into the outstream !
        for (int i = 0; i < frame_count_max; ++i) {
            for (int channel = 0; channel < layout->channel_count; ++channel) {
                int16_t sample = data[channel + 2*i];
                int16_t *ptr = (int16_t*)(areas[channel].ptr + areas[channel].step * i);
                *ptr = sample;
            }
        }

        if ((err = soundio_outstream_end_write(outstream))) {
            m_logger->error("soundio error : {}\n", soundio_strerror(err));
            throw std::runtime_error("SoundIO end write failed");
        }
    }

    void SoundIoOStream::underflow_callback_static(struct SoundIoOutStream *outstream) {
        return ((SoundIoOStream*)outstream->userdata)->underflow_callback(outstream);
    }
    void SoundIoOStream::underflow_callback(struct SoundIoOutStream *outstream) {
        static int count = 0;
        m_logger->error("underflow {}\n", count++);
    }

    int SoundIoOStream::setInput(Codec *theSong) {
        this->theSong = theSong;
        return 0;
    }

    int SoundIoOStream::open(double latency) {
        SoundIoFormat format;
        switch(theSong->getBitDepth()) {
            case  8: format = SoundIoFormatS8;
            case 16: format = SoundIoFormatS16NE;
            case 24: format = SoundIoFormatS24NE;
            case 32: format = SoundIoFormatS32NE;
            default: format = SoundIoFormatS16NE;

        }
        return open(format, theSong->getSampleRate(), latency);
    }

    int SoundIoOStream::open(SoundIoFormat format, int sample_rate, double latency) {
        outstream = soundio_outstream_create(device);
        outstream->format           = format;
        outstream->sample_rate      = sample_rate;
        outstream->software_latency = latency;

        outstream->userdata             = this;
        outstream->write_callback       = &SoundIoOStream::write_callback_static;
        outstream->underflow_callback   = &SoundIoOStream::underflow_callback_static;

        int err = soundio_outstream_open(outstream);
        if (err) {
            m_logger->error("unable to open device: {}", soundio_strerror(err));
            throw std::runtime_error(std::string("unable to open device: ") + soundio_strerror(err));
        }

        err = outstream->layout_error;
        if (err) {
            m_logger->error("unable to set channel layout: ", soundio_strerror(err));
            throw std::runtime_error(std::string("unable to set channel layout: ") + soundio_strerror(err));
        }

        err = soundio_outstream_start(outstream);
        if (err) {
            m_logger->error("unable to start device: ", soundio_strerror(err));
            throw std::runtime_error(std::string("unable to start device: ") + soundio_strerror(err));
        }
        return 0;
    }

    void SoundIoOStream::close() {
        soundio_outstream_destroy(outstream);
    }

    int soundio_main(Codec *thesong) {

        soundio_initialize();
        soundio_connect_default_output_device();
        SoundIoOStream *mysoundioostream = new SoundIoOStream();
        mysoundioostream->setInput(thesong);
        mysoundioostream->open(0.01);

        for (;;)
            soundio_wait_events(soundio);

        delete(mysoundioostream);
        soundio_disconnect_device();
        soundio_quit();
        return 0;
    }

} // namespace ORCore
<|endoftext|>