<commit_before>/* Copyright (c) 2008-2020 the MRtrix3 contributors.
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
 *
 * Covered Software is provided under this License on an "as is"
 * basis, without warranty of any kind, either expressed, implied, or
 * statutory, including, without limitation, warranties that the
 * Covered Software is free of defects, merchantable, fit for a
 * particular purpose or non-infringing.
 * See the Mozilla Public License v. 2.0 for more details.
 *
 * For more details, see http://www.mrtrix.org/.
 */

#include "dwi/gradient.h"
#include "file/nifti_utils.h"

namespace MR
{
  namespace DWI
  {

    using namespace App;
    using namespace Eigen;

    OptionGroup GradImportOptions()
    {
      OptionGroup group ("DW gradient table import options");

      group
        + Option ("grad",
            "Provide the diffusion-weighted gradient scheme used in the acquisition "
            "in a text file. This should be supplied as a 4xN text file with each line "
            "is in the format [ X Y Z b ], where [ X Y Z ] describe the direction of the "
            "applied gradient, and b gives the b-value in units of s/mm^2. If a diffusion "
            "gradient scheme is present in the input image header, the data provided with "
            "this option will be instead used.")
        +   Argument ("file").type_file_in()

        + Option ("fslgrad",
            "Provide the diffusion-weighted gradient scheme used in the acquisition in FSL "
            "bvecs/bvals format files. If a diffusion gradient scheme is present in the "
            "input image header, the data provided with this option will be instead used.")
        +   Argument ("bvecs").type_file_in()
        +   Argument ("bvals").type_file_in();

      return group;
    }

    OptionGroup GradExportOptions()
    {
      return OptionGroup ("DW gradient table export options")

        + Option ("export_grad_mrtrix", "export the diffusion-weighted gradient table to file in MRtrix format")
        +   Argument ("path").type_file_out()

        + Option ("export_grad_fsl", "export the diffusion-weighted gradient table to files in FSL (bvecs / bvals) format")
        +   Argument ("bvecs_path").type_file_out()
        +   Argument ("bvals_path").type_file_out();
    }

    Option bvalue_scaling_option = Option ("bvalue_scaling",
        "enable or disable scaling of diffusion b-values by the square of the "
        "corresponding DW gradient norm (see Desciption). "
        "Valid choices are yes/no, true/false, 0/1 (default: automatic).")
    +   Argument ("mode").type_bool();

    const char* const bvalue_scaling_description (
      "The -bvalue_scaling option controls an aspect of the import of "
      "diffusion gradient tables. When the input diffusion-weighting "
      "direction vectors have norms that differ substantially from unity, "
      "the b-values will be scaled by the square of their corresponding "
      "vector norm (this is how multi-shell acquisitions are frequently "
      "achieved on scanner platforms). However in some rare instances, the "
      "b-values may be correct, despite the vectors not being of unit norm "
      "(or conversely, the b-values may need to be rescaled even though the "
      "vectors are close to unit norm). This option allows the user to "
      "control this operation and override MRrtix3's automatic detection."
    );

    BValueScalingBehaviour get_cmdline_bvalue_scaling_behaviour ()
    {
      auto opt = App::get_options ("bvalue_scaling");
      if (opt.empty())
        return BValueScalingBehaviour::Auto;
      if (opt[0][0])
        return BValueScalingBehaviour::UserOn;
      return BValueScalingBehaviour::UserOff;
    }

    Eigen::MatrixXd parse_DW_scheme (const Header& header)
    {
      Eigen::MatrixXd G;
      const auto it = header.keyval().find ("dw_scheme");
      if (it != header.keyval().end()) {
        try {
          G = parse_matrix (it->second);
        } catch (Exception& e) {
          throw Exception (e, "malformed DW scheme in image \"" + header.name() + "\"");
        }
      }
      return G;
    }

    Eigen::MatrixXd load_bvecs_bvals (const Header& header, const std::string& bvecs_path, const std::string& bvals_path)
    {
      Eigen::MatrixXd bvals, bvecs;
      try {
        bvals = load_matrix<> (bvals_path);
        bvecs = load_matrix<> (bvecs_path);
      } catch (Exception& e) {
        throw Exception (e, "Unable to import files \"" + bvecs_path + "\" and \"" + bvals_path + "\" as FSL bvecs/bvals pair");
      }

      if (bvals.rows() != 1) {
        if (bvals.cols() == 1)
          bvals.transposeInPlace();  // transpose if file contains column vector
        else
          throw Exception ("bvals file must contain 1 row or column only (file \"" + bvals_path + "\" has " + str(bvals.rows()) + ")");
      }
      if (bvecs.rows() != 3) {
        if (bvecs.cols() == 3)
          bvecs.transposeInPlace();
        else
          throw Exception ("bvecs file must contain exactly 3 rows or columns (file \"" + bvecs_path + "\" has " + str(bvecs.rows()) + ")");
      }

      if (bvals.cols() != bvecs.cols())
        throw Exception ("bvecs and bvals files must have same number of diffusion directions (file \"" + bvecs_path + "\" has " + str(bvecs.cols()) + ", file \"" + bvals_path + "\" has " + str(bvals.cols()) + ")");

      const size_t num_volumes = header.ndim() < 4 ? 1 : header.size(3);
      if (size_t(bvals.cols()) != num_volumes)
        throw Exception ("bvecs and bvals files must have same number of diffusion directions as DW-image (gradients: " + str(bvecs.cols()) + ", image: " + str(num_volumes) + ")");

      // bvecs format actually assumes a LHS coordinate system even if image is
      // stored using RHS - x axis is flipped to make linear 3x3 part of
      // transform have negative determinant:
      vector<size_t> order;
      auto adjusted_transform = File::NIfTI::adjust_transform (header, order);
      if (adjusted_transform.linear().determinant() > 0.0)
        bvecs.row(0) = -bvecs.row(0);

      // account for the fact that bvecs are specified wrt original image axes,
      // which may have been re-ordered and/or inverted by MRtrix to match the
      // expected anatomical frame of reference:
      Eigen::MatrixXd G (bvecs.cols(), 3);
      for (ssize_t n = 0; n < G.rows(); ++n) {
        G(n,order[0]) = header.stride(order[0]) > 0 ? bvecs(0,n) : -bvecs(0,n);
        G(n,order[1]) = header.stride(order[1]) > 0 ? bvecs(1,n) : -bvecs(1,n);
        G(n,order[2]) = header.stride(order[2]) > 0 ? bvecs(2,n) : -bvecs(2,n);
      }

      // rotate gradients into scanner coordinate system:
      Eigen::MatrixXd grad (G.rows(), 4);

      grad.leftCols<3>().transpose() = header.transform().rotation() * G.transpose();
      grad.col(3) = bvals.row(0);

      return grad;
    }

    void save_bvecs_bvals (const Header& header, const std::string& bvecs_path, const std::string& bvals_path)
    {
      const auto grad = parse_DW_scheme (header);

      // rotate vectors from scanner space to image space
      Eigen::MatrixXd G = grad.leftCols<3>() * header.transform().rotation();

      // deal with FSL requiring gradient directions to coincide with data strides
      // also transpose matrices in preparation for file output
      vector<size_t> order;
      auto adjusted_transform = File::NIfTI::adjust_transform (header, order);
      Eigen::MatrixXd bvecs (3, grad.rows());
      Eigen::MatrixXd bvals (1, grad.rows());
      for (ssize_t n = 0; n < G.rows(); ++n) {
        bvecs(0,n) = header.stride(order[0]) > 0 ? G(n,order[0]) : -G(n,order[0]);
        bvecs(1,n) = header.stride(order[1]) > 0 ? G(n,order[1]) : -G(n,order[1]);
        bvecs(2,n) = header.stride(order[2]) > 0 ? G(n,order[2]) : -G(n,order[2]);
        bvals(0,n) = grad(n,3);
      }

      // bvecs format actually assumes a LHS coordinate system even if image is
      // stored using RHS - x axis is flipped to make linear 3x3 part of
      // transform have negative determinant:
      if (adjusted_transform.linear().determinant() > 0.0)
        bvecs.row(0) = -bvecs.row(0);

      save_matrix (bvecs, bvecs_path, KeyValues(), false);
      save_matrix (bvals, bvals_path, KeyValues(), false);
    }

    void clear_DW_scheme (Header& header)
    {
      auto it = header.keyval().find ("dw_scheme");
      if (it != header.keyval().end())
        header.keyval().erase (it);
    }

    Eigen::MatrixXd get_raw_DW_scheme (const Header& header)
    {
      DEBUG ("searching for suitable gradient encoding...");
      using namespace App;
      Eigen::MatrixXd grad;

      // check whether the DW scheme has been provided via the command-line:
      const auto opt_mrtrix = get_options ("grad");
      if (opt_mrtrix.size())
        grad = load_matrix<> (opt_mrtrix[0][0]);

      const auto opt_fsl = get_options ("fslgrad");
      if (opt_fsl.size()) {
        if (opt_mrtrix.size())
          throw Exception ("Diffusion gradient table can be provided using either -grad or -fslgrad option, but NOT both");
        grad = load_bvecs_bvals (header, opt_fsl[0][0], opt_fsl[0][1]);
      }

      // otherwise use the information from the header:
      if (!opt_mrtrix.size() && !opt_fsl.size())
        grad = parse_DW_scheme (header);

      return grad;
    }

    Eigen::MatrixXd get_DW_scheme (const Header& header, BValueScalingBehaviour bvalue_scaling)
    {
      try {
        auto grad = get_raw_DW_scheme (header);
        check_DW_scheme (header, grad);

        Eigen::Array<default_type, Eigen::Dynamic, 1> squared_norms = grad.leftCols(3).rowwise().squaredNorm();
        // ensure interpreted directions are always normalised
        // also make sure that directions of [0, 0, 0] don't affect subsequent calculations
        for (ssize_t row = 0; row != grad.rows(); ++row) {
          if (squared_norms[row])
            grad.row(row).template head<3>().array() /= std::sqrt(squared_norms[row]);
          else
            squared_norms[row] = 1.0;
        }
        // modulate verbosity of message & whether or not header is modified
        // based on magnitude of effect of normalisation
        const default_type max_log_scaling_factor = squared_norms.log().abs().maxCoeff();
        const default_type max_scaling_factor = std::exp (max_log_scaling_factor);
        const bool exceeds_single_precision = max_log_scaling_factor > 1e-5;
        const bool requires_bvalue_scaling = max_log_scaling_factor > 0.01;

        DEBUG ("b-value scaling: max scaling factor = exp("
            + str(max_log_scaling_factor) + ") = " + str(max_scaling_factor));

        if (( requires_bvalue_scaling && bvalue_scaling == BValueScalingBehaviour::Auto ) ||
            bvalue_scaling == BValueScalingBehaviour::UserOn ) {
          grad.col(3).array() *= squared_norms;
          INFO ("b-values scaled by the square of DW gradient norm "
              "(maximum scaling factor = " + str(max_scaling_factor) + ")");
        }
        else if (bvalue_scaling == BValueScalingBehaviour::UserOff ) {
          if (requires_bvalue_scaling) {
            CONSOLE ("disabling b-value scaling during normalisation of DW vectors on user request "
                "(maximum scaling factor would have been " + str(max_scaling_factor) + ")");
          } else {
            WARN ("use of -bvalue_scaling option had no effect: gradient vector norms are all within tolerance "
                "(maximum scaling factor = " + str(max_scaling_factor) + ")");
          }
        }

        // write the scheme as interpreted back into the header if:
        // - vector normalisation effect is large, regardless of whether or not b-value scaling was applied
        // - gradient information was pulled from file
        // - explicit b-value scaling is requested
        if (exceeds_single_precision || get_options ("grad").size() || get_options ("fslgrad").size() || bvalue_scaling != BValueScalingBehaviour::Auto)
          set_DW_scheme (const_cast<Header&> (header), grad);

        INFO ("found " + str (grad.rows()) + "x" + str (grad.cols()) + " diffusion gradient table");
        return grad;
      }
      catch (Exception& e) {
        clear_DW_scheme (const_cast<Header&> (header));
        throw Exception (e, "error importing diffusion gradient table for image \"" + header.name() + "\"");
      }
    }

    void export_grad_commandline (const Header& header)
    {
      auto check = [](const Header& h) -> const Header& {
        if (h.keyval().find("dw_scheme") == h.keyval().end())
          throw Exception ("no gradient information found within image \"" + h.name() + "\"");
        return h;
      };

      auto opt = get_options ("export_grad_mrtrix");
      if (opt.size())
        save_matrix (parse_DW_scheme (check (header)), opt[0][0]);

      opt = get_options ("export_grad_fsl");
      if (opt.size())
        save_bvecs_bvals (check (header), opt[0][0], opt[0][1]);
    }

  }
}

<commit_msg>preserve b-value scaling for b-zero #2231<commit_after>/* Copyright (c) 2008-2020 the MRtrix3 contributors.
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
 *
 * Covered Software is provided under this License on an "as is"
 * basis, without warranty of any kind, either expressed, implied, or
 * statutory, including, without limitation, warranties that the
 * Covered Software is free of defects, merchantable, fit for a
 * particular purpose or non-infringing.
 * See the Mozilla Public License v. 2.0 for more details.
 *
 * For more details, see http://www.mrtrix.org/.
 */

#include "dwi/gradient.h"
#include "file/nifti_utils.h"

namespace MR
{
  namespace DWI
  {

    using namespace App;
    using namespace Eigen;

    OptionGroup GradImportOptions()
    {
      OptionGroup group ("DW gradient table import options");

      group
        + Option ("grad",
            "Provide the diffusion-weighted gradient scheme used in the acquisition "
            "in a text file. This should be supplied as a 4xN text file with each line "
            "is in the format [ X Y Z b ], where [ X Y Z ] describe the direction of the "
            "applied gradient, and b gives the b-value in units of s/mm^2. If a diffusion "
            "gradient scheme is present in the input image header, the data provided with "
            "this option will be instead used.")
        +   Argument ("file").type_file_in()

        + Option ("fslgrad",
            "Provide the diffusion-weighted gradient scheme used in the acquisition in FSL "
            "bvecs/bvals format files. If a diffusion gradient scheme is present in the "
            "input image header, the data provided with this option will be instead used.")
        +   Argument ("bvecs").type_file_in()
        +   Argument ("bvals").type_file_in();

      return group;
    }

    OptionGroup GradExportOptions()
    {
      return OptionGroup ("DW gradient table export options")

        + Option ("export_grad_mrtrix", "export the diffusion-weighted gradient table to file in MRtrix format")
        +   Argument ("path").type_file_out()

        + Option ("export_grad_fsl", "export the diffusion-weighted gradient table to files in FSL (bvecs / bvals) format")
        +   Argument ("bvecs_path").type_file_out()
        +   Argument ("bvals_path").type_file_out();
    }

    Option bvalue_scaling_option = Option ("bvalue_scaling",
        "enable or disable scaling of diffusion b-values by the square of the "
        "corresponding DW gradient norm (see Desciption). "
        "Valid choices are yes/no, true/false, 0/1 (default: automatic).")
    +   Argument ("mode").type_bool();

    const char* const bvalue_scaling_description (
      "The -bvalue_scaling option controls an aspect of the import of "
      "diffusion gradient tables. When the input diffusion-weighting "
      "direction vectors have norms that differ substantially from unity, "
      "the b-values will be scaled by the square of their corresponding "
      "vector norm (this is how multi-shell acquisitions are frequently "
      "achieved on scanner platforms). However in some rare instances, the "
      "b-values may be correct, despite the vectors not being of unit norm "
      "(or conversely, the b-values may need to be rescaled even though the "
      "vectors are close to unit norm). This option allows the user to "
      "control this operation and override MRrtix3's automatic detection."
    );

    BValueScalingBehaviour get_cmdline_bvalue_scaling_behaviour ()
    {
      auto opt = App::get_options ("bvalue_scaling");
      if (opt.empty())
        return BValueScalingBehaviour::Auto;
      if (opt[0][0])
        return BValueScalingBehaviour::UserOn;
      return BValueScalingBehaviour::UserOff;
    }

    Eigen::MatrixXd parse_DW_scheme (const Header& header)
    {
      Eigen::MatrixXd G;
      const auto it = header.keyval().find ("dw_scheme");
      if (it != header.keyval().end()) {
        try {
          G = parse_matrix (it->second);
        } catch (Exception& e) {
          throw Exception (e, "malformed DW scheme in image \"" + header.name() + "\"");
        }
      }
      return G;
    }

    Eigen::MatrixXd load_bvecs_bvals (const Header& header, const std::string& bvecs_path, const std::string& bvals_path)
    {
      Eigen::MatrixXd bvals, bvecs;
      try {
        bvals = load_matrix<> (bvals_path);
        bvecs = load_matrix<> (bvecs_path);
      } catch (Exception& e) {
        throw Exception (e, "Unable to import files \"" + bvecs_path + "\" and \"" + bvals_path + "\" as FSL bvecs/bvals pair");
      }

      if (bvals.rows() != 1) {
        if (bvals.cols() == 1)
          bvals.transposeInPlace();  // transpose if file contains column vector
        else
          throw Exception ("bvals file must contain 1 row or column only (file \"" + bvals_path + "\" has " + str(bvals.rows()) + ")");
      }
      if (bvecs.rows() != 3) {
        if (bvecs.cols() == 3)
          bvecs.transposeInPlace();
        else
          throw Exception ("bvecs file must contain exactly 3 rows or columns (file \"" + bvecs_path + "\" has " + str(bvecs.rows()) + ")");
      }

      if (bvals.cols() != bvecs.cols())
        throw Exception ("bvecs and bvals files must have same number of diffusion directions (file \"" + bvecs_path + "\" has " + str(bvecs.cols()) + ", file \"" + bvals_path + "\" has " + str(bvals.cols()) + ")");

      const size_t num_volumes = header.ndim() < 4 ? 1 : header.size(3);
      if (size_t(bvals.cols()) != num_volumes)
        throw Exception ("bvecs and bvals files must have same number of diffusion directions as DW-image (gradients: " + str(bvecs.cols()) + ", image: " + str(num_volumes) + ")");

      // bvecs format actually assumes a LHS coordinate system even if image is
      // stored using RHS - x axis is flipped to make linear 3x3 part of
      // transform have negative determinant:
      vector<size_t> order;
      auto adjusted_transform = File::NIfTI::adjust_transform (header, order);
      if (adjusted_transform.linear().determinant() > 0.0)
        bvecs.row(0) = -bvecs.row(0);

      // account for the fact that bvecs are specified wrt original image axes,
      // which may have been re-ordered and/or inverted by MRtrix to match the
      // expected anatomical frame of reference:
      Eigen::MatrixXd G (bvecs.cols(), 3);
      for (ssize_t n = 0; n < G.rows(); ++n) {
        G(n,order[0]) = header.stride(order[0]) > 0 ? bvecs(0,n) : -bvecs(0,n);
        G(n,order[1]) = header.stride(order[1]) > 0 ? bvecs(1,n) : -bvecs(1,n);
        G(n,order[2]) = header.stride(order[2]) > 0 ? bvecs(2,n) : -bvecs(2,n);
      }

      // rotate gradients into scanner coordinate system:
      Eigen::MatrixXd grad (G.rows(), 4);

      grad.leftCols<3>().transpose() = header.transform().rotation() * G.transpose();
      grad.col(3) = bvals.row(0);

      return grad;
    }

    void save_bvecs_bvals (const Header& header, const std::string& bvecs_path, const std::string& bvals_path)
    {
      const auto grad = parse_DW_scheme (header);

      // rotate vectors from scanner space to image space
      Eigen::MatrixXd G = grad.leftCols<3>() * header.transform().rotation();

      // deal with FSL requiring gradient directions to coincide with data strides
      // also transpose matrices in preparation for file output
      vector<size_t> order;
      auto adjusted_transform = File::NIfTI::adjust_transform (header, order);
      Eigen::MatrixXd bvecs (3, grad.rows());
      Eigen::MatrixXd bvals (1, grad.rows());
      for (ssize_t n = 0; n < G.rows(); ++n) {
        bvecs(0,n) = header.stride(order[0]) > 0 ? G(n,order[0]) : -G(n,order[0]);
        bvecs(1,n) = header.stride(order[1]) > 0 ? G(n,order[1]) : -G(n,order[1]);
        bvecs(2,n) = header.stride(order[2]) > 0 ? G(n,order[2]) : -G(n,order[2]);
        bvals(0,n) = grad(n,3);
      }

      // bvecs format actually assumes a LHS coordinate system even if image is
      // stored using RHS - x axis is flipped to make linear 3x3 part of
      // transform have negative determinant:
      if (adjusted_transform.linear().determinant() > 0.0)
        bvecs.row(0) = -bvecs.row(0);

      save_matrix (bvecs, bvecs_path, KeyValues(), false);
      save_matrix (bvals, bvals_path, KeyValues(), false);
    }

    void clear_DW_scheme (Header& header)
    {
      auto it = header.keyval().find ("dw_scheme");
      if (it != header.keyval().end())
        header.keyval().erase (it);
    }

    Eigen::MatrixXd get_raw_DW_scheme (const Header& header)
    {
      DEBUG ("searching for suitable gradient encoding...");
      using namespace App;
      Eigen::MatrixXd grad;

      // check whether the DW scheme has been provided via the command-line:
      const auto opt_mrtrix = get_options ("grad");
      if (opt_mrtrix.size())
        grad = load_matrix<> (opt_mrtrix[0][0]);

      const auto opt_fsl = get_options ("fslgrad");
      if (opt_fsl.size()) {
        if (opt_mrtrix.size())
          throw Exception ("Diffusion gradient table can be provided using either -grad or -fslgrad option, but NOT both");
        grad = load_bvecs_bvals (header, opt_fsl[0][0], opt_fsl[0][1]);
      }

      // otherwise use the information from the header:
      if (!opt_mrtrix.size() && !opt_fsl.size())
        grad = parse_DW_scheme (header);

      return grad;
    }

    Eigen::MatrixXd get_DW_scheme (const Header& header, BValueScalingBehaviour bvalue_scaling)
    {
      try {
        auto grad = get_raw_DW_scheme (header);
        check_DW_scheme (header, grad);

        Eigen::Array<default_type, Eigen::Dynamic, 1> squared_norms = grad.leftCols(3).rowwise().squaredNorm();
        // ensure interpreted directions are always normalised
        // also make sure that directions of [0, 0, 0] don't affect subsequent calculations
        for (ssize_t row = 0; row != grad.rows(); ++row) {
          if (squared_norms[row])
            grad.row(row).template head<3>().array() /= std::sqrt(squared_norms[row]);
        }
        // modulate verbosity of message & whether or not header is modified
        // based on magnitude of effect of normalisation
        const default_type max_log_scaling_factor = squared_norms.log().abs().maxCoeff();
        const default_type max_scaling_factor = std::exp (max_log_scaling_factor);
        const bool exceeds_single_precision = max_log_scaling_factor > 1e-5;
        const bool requires_bvalue_scaling = max_log_scaling_factor > 0.01;

        DEBUG ("b-value scaling: max scaling factor = exp("
            + str(max_log_scaling_factor) + ") = " + str(max_scaling_factor));

        if (( requires_bvalue_scaling && bvalue_scaling == BValueScalingBehaviour::Auto ) ||
            bvalue_scaling == BValueScalingBehaviour::UserOn ) {
          grad.col(3).array() *= squared_norms;
          INFO ("b-values scaled by the square of DW gradient norm "
              "(maximum scaling factor = " + str(max_scaling_factor) + ")");
        }
        else if (bvalue_scaling == BValueScalingBehaviour::UserOff ) {
          if (requires_bvalue_scaling) {
            CONSOLE ("disabling b-value scaling during normalisation of DW vectors on user request "
                "(maximum scaling factor would have been " + str(max_scaling_factor) + ")");
          } else {
            WARN ("use of -bvalue_scaling option had no effect: gradient vector norms are all within tolerance "
                "(maximum scaling factor = " + str(max_scaling_factor) + ")");
          }
        }

        // write the scheme as interpreted back into the header if:
        // - vector normalisation effect is large, regardless of whether or not b-value scaling was applied
        // - gradient information was pulled from file
        // - explicit b-value scaling is requested
        if (exceeds_single_precision || get_options ("grad").size() || get_options ("fslgrad").size() || bvalue_scaling != BValueScalingBehaviour::Auto)
          set_DW_scheme (const_cast<Header&> (header), grad);

        INFO ("found " + str (grad.rows()) + "x" + str (grad.cols()) + " diffusion gradient table");
        return grad;
      }
      catch (Exception& e) {
        clear_DW_scheme (const_cast<Header&> (header));
        throw Exception (e, "error importing diffusion gradient table for image \"" + header.name() + "\"");
      }
    }

    void export_grad_commandline (const Header& header)
    {
      auto check = [](const Header& h) -> const Header& {
        if (h.keyval().find("dw_scheme") == h.keyval().end())
          throw Exception ("no gradient information found within image \"" + h.name() + "\"");
        return h;
      };

      auto opt = get_options ("export_grad_mrtrix");
      if (opt.size())
        save_matrix (parse_DW_scheme (check (header)), opt[0][0]);

      opt = get_options ("export_grad_fsl");
      if (opt.size())
        save_bvecs_bvals (check (header), opt[0][0], opt[0][1]);
    }

  }
}

<|endoftext|>


<commit_before>/*
Copyright (c) 2013 Daniele Bartolini, Michele Rossi
Copyright (c) 2012 Daniele Bartolini, Simone Boscaratto

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

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

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

#include "Unit.h"
#include "World.h"
#include "Allocator.h"
#include "Log.h"
#include "UnitResource.h"
#include "SceneGraphManager.h"
#include "Actor.h"
#include "Controller.h"
#include "PhysicsResource.h"
#include "Device.h"
#include "ResourceManager.h"

namespace crown
{

typedef Id CameraId;
typedef Id SpriteId;

//-----------------------------------------------------------------------------
Unit::Unit(World& w, const UnitResource* ur, const Matrix4x4& pose)
	: m_world(w)
	, m_scene_graph(*w.scene_graph_manager()->create_scene_graph())
	, m_resource(ur)
	, m_num_cameras(0)
	, m_num_meshes(0)
	, m_num_sprites(0)
	, m_num_actors(0)
{
	m_controller.component.id = INVALID_ID;
	create_objects(pose);
}

//-----------------------------------------------------------------------------
Unit::~Unit()
{
	destroy_objects();
	m_world.scene_graph_manager()->destroy_scene_graph(&m_scene_graph);
}

//-----------------------------------------------------------------------------
void Unit::set_id(const UnitId id)
{
	m_id = id;
}

//-----------------------------------------------------------------------------
UnitId Unit::id()
{
	return m_id;
}

//-----------------------------------------------------------------------------
const UnitResource*	Unit::resource() const
{
	return m_resource;
}

//-----------------------------------------------------------------------------
void Unit::create_objects(const Matrix4x4& pose)
{
	// Create the scene graph
	m_scene_graph.create(m_resource->num_scene_graph_nodes(), m_resource->scene_graph_names(),
							m_resource->scene_graph_poses(), m_resource->scene_graph_parents());
	// Set root node's pose
	m_scene_graph.set_local_pose(0, pose);

	create_camera_objects();
	create_renderable_objects();
	create_physics_objects();
}

//-----------------------------------------------------------------------------
void Unit::destroy_objects()
{
	// Destroy cameras
	for (uint32_t i = 0; i < m_num_cameras; i++)
	{
		m_world.destroy_camera(m_cameras[i].component);
	}
	m_num_cameras = 0;

	// Destroy meshes
	for (uint32_t i = 0; i < m_num_meshes; i++)
	{
		m_world.render_world()->destroy_mesh(m_meshes[i].component);
	}
	m_num_meshes = 0;

	// Destroy sprites
	for (uint32_t i = 0; i < m_num_sprites; i++)
	{
		m_world.render_world()->destroy_sprite(m_sprites[i].component);
	}
	m_num_sprites = 0;

	// Destroy actors
	for (uint32_t i = 0; i < m_num_actors; i++)
	{
		m_world.physics_world()->destroy_actor(m_actors[i].component);
	}
	m_num_actors = 0;

	// Destroy controller
	if (m_controller.component.id != INVALID_ID)
	{
		m_world.physics_world()->destroy_controller(m_controller.component);
		m_controller.component.id = INVALID_ID;
	}

	// Destroy scene graph
	m_scene_graph.destroy();
}

//-----------------------------------------------------------------------------
void Unit::create_camera_objects()
{
	for (uint32_t i = 0; i < m_resource->num_cameras(); i++)
	{
		const UnitCamera camera = m_resource->get_camera(i);
		const CameraId cam = m_world.create_camera(m_scene_graph, camera.node);
		add_camera(camera.name, cam);
	}
}

//-----------------------------------------------------------------------------
void Unit::create_renderable_objects()
{
	for (uint32_t i = 0; i < m_resource->num_renderables(); i++)
	{
		const UnitRenderable renderable = m_resource->get_renderable(i);

		switch (renderable.type)
		{
			case UnitRenderable::MESH: add_mesh(renderable.name, m_world.render_world()->create_mesh(renderable.resource, m_scene_graph, renderable.node)); break;
			case UnitRenderable::SPRITE: add_sprite(renderable.name, m_world.render_world()->create_sprite(renderable.resource, m_scene_graph, renderable.node)); break;
			default: CE_FATAL("Oops, bad renderable type"); break;
		}
	}
}

//-----------------------------------------------------------------------------
void Unit::create_physics_objects()
{
	if (m_resource->physics_resource().id != 0)
	{
		const PhysicsResource* pr = (PhysicsResource*) device()->resource_manager()->data(m_resource->physics_resource());

		// Create controller if any
		if (pr->has_controller())
		{
			set_controller(pr->controller().name, m_world.physics_world()->create_controller(pr, m_scene_graph, 0));
		}

		// Create actors if any
		for (uint32_t i = 0; i < pr->num_actors(); i++)
		{
			const PhysicsActor actor = pr->actor(i);
			add_actor(actor.name, m_world.physics_world()->create_actor(m_scene_graph, m_scene_graph.node(actor.node), ActorType::DYNAMIC_PHYSICAL));
		}
	}
}

//-----------------------------------------------------------------------------
int32_t Unit::node(const char* name) const
{
	return m_scene_graph.node(name);
}

//-----------------------------------------------------------------------------
bool Unit::has_node(const char* name) const
{
	return m_scene_graph.has_node(name);
}

//-----------------------------------------------------------------------------
uint32_t Unit::num_nodes() const
{
	return m_scene_graph.num_nodes();
}

//-----------------------------------------------------------------------------
Vector3 Unit::local_position(int32_t node) const
{
	return m_scene_graph.local_position(node);
}

//-----------------------------------------------------------------------------
Quaternion Unit::local_rotation(int32_t node) const
{
	return m_scene_graph.local_rotation(node);
}

//-----------------------------------------------------------------------------
Matrix4x4 Unit::local_pose(int32_t node) const
{
	return m_scene_graph.local_pose(node);
}

//-----------------------------------------------------------------------------
Vector3 Unit::world_position(int32_t node) const
{
	return m_scene_graph.world_position(node);
}

//-----------------------------------------------------------------------------
Quaternion Unit::world_rotation(int32_t node) const
{
	return m_scene_graph.world_rotation(node);
}

//-----------------------------------------------------------------------------
Matrix4x4 Unit::world_pose(int32_t node) const
{
	return m_scene_graph.world_pose(node);
}

//-----------------------------------------------------------------------------
void Unit::set_local_position(int32_t node, const Vector3& pos)
{
	m_scene_graph.set_local_position(node, pos);
}

//-----------------------------------------------------------------------------
void Unit::set_local_rotation(int32_t node, const Quaternion& rot)
{
	m_scene_graph.set_local_rotation(node, rot);
}

//-----------------------------------------------------------------------------
void Unit::set_local_pose(int32_t node, const Matrix4x4& pose)
{
	m_scene_graph.set_local_pose(node, pose);
}

//-----------------------------------------------------------------------------
void Unit::link_node(int32_t child, int32_t parent)
{
	m_scene_graph.link(child, parent);
}

//-----------------------------------------------------------------------------
void Unit::unlink_node(int32_t child)
{
	m_scene_graph.unlink(child);
}

//-----------------------------------------------------------------------------
void Unit::update()
{
}

//-----------------------------------------------------------------------------
void Unit::reload(UnitResource* new_ur)
{
	Matrix4x4 m = m_scene_graph.world_pose(0);
	Log::d("Destroying objects");
	destroy_objects();
	m_resource = new_ur;
	Log::d("Creating object");
	create_objects(m);
}

//-----------------------------------------------------------------------------
void Unit::add_component(StringId32 name, Id component, uint32_t& size, Component* array)
{
	Component comp;
	comp.name = name;
	comp.component = component;

	array[size] = comp;
	size++;
}

//-----------------------------------------------------------------------------
Id Unit::find_component(const char* name, uint32_t size, Component* array)
{
	uint32_t name_hash = hash::murmur2_32(name, string::strlen(name), 0);

	Id comp;
	comp.id = INVALID_ID;

	for (uint32_t i = 0; i < size; i++)
	{
		if (name_hash == array[i].name)
		{
			comp = array[i].component;
		}
	}

	return comp;
}

//-----------------------------------------------------------------------------
Id Unit::find_component(uint32_t index, uint32_t size, Component* array)
{
	Id comp;
	comp.id = INVALID_ID;

	if (index < size)
	{
		comp = array[index].component;
	}

	return comp;
}

//-----------------------------------------------------------------------------
void Unit::add_camera(StringId32 name, CameraId camera)
{
	CE_ASSERT(m_num_cameras < MAX_CAMERA_COMPONENTS, "Max camera number reached");

	add_component(name, camera, m_num_cameras, m_cameras);
}

//-----------------------------------------------------------------------------
void Unit::add_mesh(StringId32 name, MeshId mesh)
{
	CE_ASSERT(m_num_meshes < MAX_MESH_COMPONENTS, "Max mesh number reached");

	add_component(name, mesh, m_num_meshes, m_meshes);
}

//-----------------------------------------------------------------------------
void Unit::add_sprite(StringId32 name, SpriteId sprite)
{
	CE_ASSERT(m_num_sprites < MAX_SPRITE_COMPONENTS, "Max sprite number reached");

	add_component(name, sprite, m_num_sprites, m_sprites);
}

//-----------------------------------------------------------------------------
void Unit::add_actor(StringId32 name, ActorId actor)
{
	CE_ASSERT(m_num_actors < MAX_ACTOR_COMPONENTS, "Max actor number reached");

	add_component(name, actor, m_num_actors, m_actors);
}

//-----------------------------------------------------------------------------
void Unit::set_controller(StringId32 name, ControllerId controller)
{
	m_controller.name = name;
	m_controller.component = controller;
}

//-----------------------------------------------------------------------------
Camera* Unit::camera(const char* name)
{
	CameraId cam = find_component(name, m_num_cameras, m_cameras);

	CE_ASSERT(cam.id != INVALID_ID, "Unit does not have camera with name '%s'", name);

	return m_world.lookup_camera(cam);
}

//-----------------------------------------------------------------------------
Camera* Unit::camera(uint32_t i)
{
	CameraId cam = find_component(i, m_num_cameras, m_cameras);

	CE_ASSERT(cam.id != INVALID_ID, "Unit does not have camera with index '%d'", i);

	return m_world.lookup_camera(cam);
}

//-----------------------------------------------------------------------------
Mesh* Unit::mesh(const char* name)
{
	MeshId mesh = find_component(name, m_num_meshes, m_meshes);

	CE_ASSERT(mesh.id != INVALID_ID, "Unit does not have mesh with name '%s'", name);

	return m_world.render_world()->lookup_mesh(mesh);
}

//-----------------------------------------------------------------------------
Mesh* Unit::mesh(uint32_t i)
{
	MeshId mesh = find_component(i, m_num_meshes, m_meshes);

	CE_ASSERT(mesh.id != INVALID_ID, "Unit does not have mesh with index '%d'", i);

	return m_world.render_world()->lookup_mesh(mesh);
}

//-----------------------------------------------------------------------------
Sprite*	Unit::sprite(const char* name)
{
	SpriteId sprite = find_component(name, m_num_sprites, m_sprites);

	CE_ASSERT(sprite.id != INVALID_ID, "Unit does not have sprite with name '%s'", name);

	return m_world.render_world()->lookup_sprite(sprite);
}

//-----------------------------------------------------------------------------
Sprite*	Unit::sprite(uint32_t i)
{
	SpriteId sprite = find_component(i, m_num_sprites, m_sprites);

	CE_ASSERT(sprite.id != INVALID_ID, "Unit does not have sprite with index '%d'", i);

	return m_world.render_world()->lookup_sprite(sprite);
}

//-----------------------------------------------------------------------------
Actor* Unit::actor(const char* name)
{
	ActorId actor = find_component(name, m_num_actors, m_actors);

	CE_ASSERT(actor.id != INVALID_ID, "Unit does not have actor with name '%s'", name);

	return m_world.physics_world()->lookup_actor(actor);
}

//-----------------------------------------------------------------------------
Actor* Unit::actor(uint32_t i)
{
	ActorId actor = find_component(i, m_num_actors, m_actors);

	CE_ASSERT(actor.id != INVALID_ID, "Unit does not have actor with name '%d'", i);

	return m_world.physics_world()->lookup_actor(actor);
}

//-----------------------------------------------------------------------------
Controller* Unit::controller()
{
	if (m_controller.component.id != INVALID_ID)
	{
		return m_world.physics_world()->lookup_controller(m_controller.component);
	}

	return NULL;
}

} // namespace crown
<commit_msg>Remove Logs<commit_after>/*
Copyright (c) 2013 Daniele Bartolini, Michele Rossi
Copyright (c) 2012 Daniele Bartolini, Simone Boscaratto

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

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

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

#include "Unit.h"
#include "World.h"
#include "Allocator.h"
#include "Log.h"
#include "UnitResource.h"
#include "SceneGraphManager.h"
#include "Actor.h"
#include "Controller.h"
#include "PhysicsResource.h"
#include "Device.h"
#include "ResourceManager.h"

namespace crown
{

typedef Id CameraId;
typedef Id SpriteId;

//-----------------------------------------------------------------------------
Unit::Unit(World& w, const UnitResource* ur, const Matrix4x4& pose)
	: m_world(w)
	, m_scene_graph(*w.scene_graph_manager()->create_scene_graph())
	, m_resource(ur)
	, m_num_cameras(0)
	, m_num_meshes(0)
	, m_num_sprites(0)
	, m_num_actors(0)
{
	m_controller.component.id = INVALID_ID;
	create_objects(pose);
}

//-----------------------------------------------------------------------------
Unit::~Unit()
{
	destroy_objects();
	m_world.scene_graph_manager()->destroy_scene_graph(&m_scene_graph);
}

//-----------------------------------------------------------------------------
void Unit::set_id(const UnitId id)
{
	m_id = id;
}

//-----------------------------------------------------------------------------
UnitId Unit::id()
{
	return m_id;
}

//-----------------------------------------------------------------------------
const UnitResource*	Unit::resource() const
{
	return m_resource;
}

//-----------------------------------------------------------------------------
void Unit::create_objects(const Matrix4x4& pose)
{
	// Create the scene graph
	m_scene_graph.create(m_resource->num_scene_graph_nodes(), m_resource->scene_graph_names(),
							m_resource->scene_graph_poses(), m_resource->scene_graph_parents());
	// Set root node's pose
	m_scene_graph.set_local_pose(0, pose);

	create_camera_objects();
	create_renderable_objects();
	create_physics_objects();
}

//-----------------------------------------------------------------------------
void Unit::destroy_objects()
{
	// Destroy cameras
	for (uint32_t i = 0; i < m_num_cameras; i++)
	{
		m_world.destroy_camera(m_cameras[i].component);
	}
	m_num_cameras = 0;

	// Destroy meshes
	for (uint32_t i = 0; i < m_num_meshes; i++)
	{
		m_world.render_world()->destroy_mesh(m_meshes[i].component);
	}
	m_num_meshes = 0;

	// Destroy sprites
	for (uint32_t i = 0; i < m_num_sprites; i++)
	{
		m_world.render_world()->destroy_sprite(m_sprites[i].component);
	}
	m_num_sprites = 0;

	// Destroy actors
	for (uint32_t i = 0; i < m_num_actors; i++)
	{
		m_world.physics_world()->destroy_actor(m_actors[i].component);
	}
	m_num_actors = 0;

	// Destroy controller
	if (m_controller.component.id != INVALID_ID)
	{
		m_world.physics_world()->destroy_controller(m_controller.component);
		m_controller.component.id = INVALID_ID;
	}

	// Destroy scene graph
	m_scene_graph.destroy();
}

//-----------------------------------------------------------------------------
void Unit::create_camera_objects()
{
	for (uint32_t i = 0; i < m_resource->num_cameras(); i++)
	{
		const UnitCamera camera = m_resource->get_camera(i);
		const CameraId cam = m_world.create_camera(m_scene_graph, camera.node);
		add_camera(camera.name, cam);
	}
}

//-----------------------------------------------------------------------------
void Unit::create_renderable_objects()
{
	for (uint32_t i = 0; i < m_resource->num_renderables(); i++)
	{
		const UnitRenderable renderable = m_resource->get_renderable(i);

		switch (renderable.type)
		{
			case UnitRenderable::MESH: add_mesh(renderable.name, m_world.render_world()->create_mesh(renderable.resource, m_scene_graph, renderable.node)); break;
			case UnitRenderable::SPRITE: add_sprite(renderable.name, m_world.render_world()->create_sprite(renderable.resource, m_scene_graph, renderable.node)); break;
			default: CE_FATAL("Oops, bad renderable type"); break;
		}
	}
}

//-----------------------------------------------------------------------------
void Unit::create_physics_objects()
{
	if (m_resource->physics_resource().id != 0)
	{
		const PhysicsResource* pr = (PhysicsResource*) device()->resource_manager()->data(m_resource->physics_resource());

		// Create controller if any
		if (pr->has_controller())
		{
			set_controller(pr->controller().name, m_world.physics_world()->create_controller(pr, m_scene_graph, 0));
		}

		// Create actors if any
		for (uint32_t i = 0; i < pr->num_actors(); i++)
		{
			const PhysicsActor actor = pr->actor(i);
			add_actor(actor.name, m_world.physics_world()->create_actor(m_scene_graph, m_scene_graph.node(actor.node), ActorType::DYNAMIC_PHYSICAL));
		}
	}
}

//-----------------------------------------------------------------------------
int32_t Unit::node(const char* name) const
{
	return m_scene_graph.node(name);
}

//-----------------------------------------------------------------------------
bool Unit::has_node(const char* name) const
{
	return m_scene_graph.has_node(name);
}

//-----------------------------------------------------------------------------
uint32_t Unit::num_nodes() const
{
	return m_scene_graph.num_nodes();
}

//-----------------------------------------------------------------------------
Vector3 Unit::local_position(int32_t node) const
{
	return m_scene_graph.local_position(node);
}

//-----------------------------------------------------------------------------
Quaternion Unit::local_rotation(int32_t node) const
{
	return m_scene_graph.local_rotation(node);
}

//-----------------------------------------------------------------------------
Matrix4x4 Unit::local_pose(int32_t node) const
{
	return m_scene_graph.local_pose(node);
}

//-----------------------------------------------------------------------------
Vector3 Unit::world_position(int32_t node) const
{
	return m_scene_graph.world_position(node);
}

//-----------------------------------------------------------------------------
Quaternion Unit::world_rotation(int32_t node) const
{
	return m_scene_graph.world_rotation(node);
}

//-----------------------------------------------------------------------------
Matrix4x4 Unit::world_pose(int32_t node) const
{
	return m_scene_graph.world_pose(node);
}

//-----------------------------------------------------------------------------
void Unit::set_local_position(int32_t node, const Vector3& pos)
{
	m_scene_graph.set_local_position(node, pos);
}

//-----------------------------------------------------------------------------
void Unit::set_local_rotation(int32_t node, const Quaternion& rot)
{
	m_scene_graph.set_local_rotation(node, rot);
}

//-----------------------------------------------------------------------------
void Unit::set_local_pose(int32_t node, const Matrix4x4& pose)
{
	m_scene_graph.set_local_pose(node, pose);
}

//-----------------------------------------------------------------------------
void Unit::link_node(int32_t child, int32_t parent)
{
	m_scene_graph.link(child, parent);
}

//-----------------------------------------------------------------------------
void Unit::unlink_node(int32_t child)
{
	m_scene_graph.unlink(child);
}

//-----------------------------------------------------------------------------
void Unit::update()
{
}

//-----------------------------------------------------------------------------
void Unit::reload(UnitResource* new_ur)
{
	Matrix4x4 m = m_scene_graph.world_pose(0);
	destroy_objects();
	m_resource = new_ur;
	create_objects(m);
}

//-----------------------------------------------------------------------------
void Unit::add_component(StringId32 name, Id component, uint32_t& size, Component* array)
{
	Component comp;
	comp.name = name;
	comp.component = component;

	array[size] = comp;
	size++;
}

//-----------------------------------------------------------------------------
Id Unit::find_component(const char* name, uint32_t size, Component* array)
{
	uint32_t name_hash = hash::murmur2_32(name, string::strlen(name), 0);

	Id comp;
	comp.id = INVALID_ID;

	for (uint32_t i = 0; i < size; i++)
	{
		if (name_hash == array[i].name)
		{
			comp = array[i].component;
		}
	}

	return comp;
}

//-----------------------------------------------------------------------------
Id Unit::find_component(uint32_t index, uint32_t size, Component* array)
{
	Id comp;
	comp.id = INVALID_ID;

	if (index < size)
	{
		comp = array[index].component;
	}

	return comp;
}

//-----------------------------------------------------------------------------
void Unit::add_camera(StringId32 name, CameraId camera)
{
	CE_ASSERT(m_num_cameras < MAX_CAMERA_COMPONENTS, "Max camera number reached");

	add_component(name, camera, m_num_cameras, m_cameras);
}

//-----------------------------------------------------------------------------
void Unit::add_mesh(StringId32 name, MeshId mesh)
{
	CE_ASSERT(m_num_meshes < MAX_MESH_COMPONENTS, "Max mesh number reached");

	add_component(name, mesh, m_num_meshes, m_meshes);
}

//-----------------------------------------------------------------------------
void Unit::add_sprite(StringId32 name, SpriteId sprite)
{
	CE_ASSERT(m_num_sprites < MAX_SPRITE_COMPONENTS, "Max sprite number reached");

	add_component(name, sprite, m_num_sprites, m_sprites);
}

//-----------------------------------------------------------------------------
void Unit::add_actor(StringId32 name, ActorId actor)
{
	CE_ASSERT(m_num_actors < MAX_ACTOR_COMPONENTS, "Max actor number reached");

	add_component(name, actor, m_num_actors, m_actors);
}

//-----------------------------------------------------------------------------
void Unit::set_controller(StringId32 name, ControllerId controller)
{
	m_controller.name = name;
	m_controller.component = controller;
}

//-----------------------------------------------------------------------------
Camera* Unit::camera(const char* name)
{
	CameraId cam = find_component(name, m_num_cameras, m_cameras);

	CE_ASSERT(cam.id != INVALID_ID, "Unit does not have camera with name '%s'", name);

	return m_world.lookup_camera(cam);
}

//-----------------------------------------------------------------------------
Camera* Unit::camera(uint32_t i)
{
	CameraId cam = find_component(i, m_num_cameras, m_cameras);

	CE_ASSERT(cam.id != INVALID_ID, "Unit does not have camera with index '%d'", i);

	return m_world.lookup_camera(cam);
}

//-----------------------------------------------------------------------------
Mesh* Unit::mesh(const char* name)
{
	MeshId mesh = find_component(name, m_num_meshes, m_meshes);

	CE_ASSERT(mesh.id != INVALID_ID, "Unit does not have mesh with name '%s'", name);

	return m_world.render_world()->lookup_mesh(mesh);
}

//-----------------------------------------------------------------------------
Mesh* Unit::mesh(uint32_t i)
{
	MeshId mesh = find_component(i, m_num_meshes, m_meshes);

	CE_ASSERT(mesh.id != INVALID_ID, "Unit does not have mesh with index '%d'", i);

	return m_world.render_world()->lookup_mesh(mesh);
}

//-----------------------------------------------------------------------------
Sprite*	Unit::sprite(const char* name)
{
	SpriteId sprite = find_component(name, m_num_sprites, m_sprites);

	CE_ASSERT(sprite.id != INVALID_ID, "Unit does not have sprite with name '%s'", name);

	return m_world.render_world()->lookup_sprite(sprite);
}

//-----------------------------------------------------------------------------
Sprite*	Unit::sprite(uint32_t i)
{
	SpriteId sprite = find_component(i, m_num_sprites, m_sprites);

	CE_ASSERT(sprite.id != INVALID_ID, "Unit does not have sprite with index '%d'", i);

	return m_world.render_world()->lookup_sprite(sprite);
}

//-----------------------------------------------------------------------------
Actor* Unit::actor(const char* name)
{
	ActorId actor = find_component(name, m_num_actors, m_actors);

	CE_ASSERT(actor.id != INVALID_ID, "Unit does not have actor with name '%s'", name);

	return m_world.physics_world()->lookup_actor(actor);
}

//-----------------------------------------------------------------------------
Actor* Unit::actor(uint32_t i)
{
	ActorId actor = find_component(i, m_num_actors, m_actors);

	CE_ASSERT(actor.id != INVALID_ID, "Unit does not have actor with name '%d'", i);

	return m_world.physics_world()->lookup_actor(actor);
}

//-----------------------------------------------------------------------------
Controller* Unit::controller()
{
	if (m_controller.component.id != INVALID_ID)
	{
		return m_world.physics_world()->lookup_controller(m_controller.component);
	}

	return NULL;
}

} // namespace crown
<|endoftext|>


<commit_before>#include "dbeacon.h"
#include "protocol.h"
#include <math.h>
#include <netinet/in.h>

using namespace std;

static inline bool write_tlv_string(uint8_t *buf, int maxlen, int &pointer, uint8_t type, const char *str) {
	int len = strlen(str);
	if ((pointer + 2 + len) > maxlen)
		return false;
	buf[pointer + 0] = type;
	buf[pointer + 1] = len;
	memcpy(buf + pointer + 2, str, len);
	pointer += len + 2;
	return true;
}

static bool write_tlv_start(uint8_t *buff, int maxlen, int &ptr, uint8_t type, int len) {
	if ((ptr + len + 2) > maxlen)
		return false;

	buff[ptr + 0] = type;
	buff[ptr + 1] = len;

	ptr += 2;

	return true;
}

static bool write_tlv_stats(uint8_t *buff, int maxlen, int &ptr, uint8_t type, uint32_t age, int sttl, const beaconMcastState &st) {
	if (!write_tlv_start(buff, maxlen, ptr, type, 20))
		return false;

	uint8_t *b = buff + ptr;

	uint32_t val;

	val = htonl((uint32_t)st.s.timestamp);
	memcpy(b + 0, &val, sizeof(val));
	val = htonl(age);
	memcpy(b + 4, &val, sizeof(val));

	// *((uint32_t *)(b + 0)) = htonl((uint32_t)st.s.timestamp);
	// *((uint32_t *)(b + 4)) = htonl(age);

	b[8] = (sttl ? sttl : defaultTTL) - st.s.rttl;

	uint32_t *stats = (uint32_t *)(b + 9);

	val = htonl(*((uint32_t *)&st.s.avgdelay));
	memcpy(stats + 0, &val, sizeof(val));
	val = htonl(*((uint32_t *)&st.s.avgjitter));
	memcpy(stats + 1, &val, sizeof(val));

	// stats[0] = htonl(*((uint32_t *)&st.s.avgdelay));
	// stats[1] = htonl(*((uint32_t *)&st.s.avgjitter));

	b[17] = (uint8_t)(st.s.avgloss * 0xff);
	b[18] = st.s.avgdup > 10. ? 0xff : ((uint8_t)ceil(st.s.avgdup * 25.5));
	b[19] = (uint8_t)(st.s.avgooo * 0xff);

	ptr += 20;

	return true;
}

int build_report(uint8_t *buff, int maxlen, int type, bool publishsources) {
	if (maxlen < 4)
		return -1;

	// 0-2 magic
	*((uint16_t *)buff) = htons(0xbeac);

	// 3 version
	buff[2] = PROTO_VER;

	// 4 packet type
	buff[3] = 1; // Report

	buff[4] = defaultTTL; // Original Hop Limit

	int ptr = 5;

	if (!write_tlv_string(buff, maxlen, ptr, T_BEAC_NAME, beaconName.c_str()))
		return -1;
	if (!write_tlv_string(buff, maxlen, ptr, T_ADMIN_CONTACT, adminContact.c_str()))
		return -1;

	if (type == WEBSITE_REPORT) {
		for (WebSites::const_iterator j = webSites.begin(); j != webSites.end(); j++)
			if (!write_tlv_string(buff, maxlen, ptr, j->first, j->second.c_str()))
				return -1;
		if (!twoLetterCC.empty()) {
			if (!write_tlv_string(buff, maxlen, ptr, T_CC, twoLetterCC.c_str()))
				return -1;
		}
		return ptr;
	} else if (type == LEAVE_REPORT) {
		if (!write_tlv_start(buff, maxlen, ptr, T_LEAVE, 0))
			return -1;
		return ptr;
	}

	if (publishsources) {
		uint64_t now = get_timestamp();

		for (Sources::const_iterator i = sources.begin(); i != sources.end(); i++) {
			if (!i->second.identified)
				continue;

			if (!i->second.ASM.s.valid && !i->second.SSM.s.valid)
				continue;

			int len = 18;

			if (i->first.family() == AF_INET)
				len = 6;

			if (type == MAP_REPORT) {
				int namelen = i->second.name.size();
				int contactlen = i->second.adminContact.size();
				len += 2 + namelen + 2 + contactlen;
			} else {
				len += (i->second.ASM.s.valid ? 22 : 0) + (i->second.SSM.s.valid ? 22 : 0);
			}

			if (!write_tlv_start(buff, maxlen, ptr, i->first.family() == AF_INET6 ? T_SOURCE_INFO : T_SOURCE_INFO_IPv4, len))
				break;

			if (i->first.family() == AF_INET6) {
				const sockaddr_in6 *addr = i->first.v6();

				memcpy(buff + ptr, &addr->sin6_addr, sizeof(in6_addr));
				memcpy(buff + ptr + 16, &addr->sin6_port, sizeof(uint16_t));

				ptr += 18;
			} else {
				const sockaddr_in *addr = i->first.v4();

				memcpy(buff + ptr, &addr->sin_addr, sizeof(in_addr));
				memcpy(buff + ptr + 4, &addr->sin_port, sizeof(uint16_t));

				ptr += 6;
			}

			if (type == MAP_REPORT) {
				write_tlv_string(buff, maxlen, ptr, T_BEAC_NAME, i->second.name.c_str());
				write_tlv_string(buff, maxlen, ptr, T_ADMIN_CONTACT, i->second.adminContact.c_str());
			} else {
				uint32_t age = (now - i->second.creation) / 1000;

				if (i->second.ASM.s.valid)
					write_tlv_stats(buff, maxlen, ptr, T_ASM_STATS, age, i->second.sttl, i->second.ASM);
				if (i->second.SSM.s.valid)
					write_tlv_stats(buff, maxlen, ptr, T_SSM_STATS, age, i->second.sttl, i->second.SSM);
			}
		}
	}

	return ptr;
}

int build_probe(uint8_t *buff, int maxlen, uint32_t sn, uint64_t ts) {
	if (maxlen < (int)(4 + 4 + 4))
		return -1;

	// 0-2 magic
	*((uint16_t *)buff) = htons(0xbeac);

	// 3 version
	buff[2] = PROTO_VER;

	// 4 packet type
	buff[3] = 0; // Probe

	*((uint32_t *)(buff + 4)) = htonl(sn);
	*((uint32_t *)(buff + 8)) = htonl((uint32_t)ts);

	return 4 + 4 + 4;
}

static inline uint8_t *tlv_begin(uint8_t *hd, int &len) {
	if (len < 2 || hd[1] > len)
		return 0;
	return hd;
}

static inline uint8_t *tlv_next(uint8_t *hd, int &len) {
	len -= hd[1] + 2;
	return tlv_begin(hd + hd[1] + 2, len);
}

static bool read_tlv_stats(uint8_t *tlv, beaconExternalStats &extb, Stats &st) {
	if (tlv[1] != 20)
		return false;

	uint32_t tmp;

	memcpy(&tmp, tlv + 2, sizeof(tmp));
	st.timestamp = ntohl(tmp);
	memcpy(&tmp, tlv + 6, sizeof(tmp));
	extb.age = tmp;

	// st.timestamp = ntohl(*(uint32_t *)(tlv + 2));
	// extb.age = ntohl(*(uint32_t *)(tlv + 6));

	st.rttl = tlv[10];

	memcpy(&tmp, tlv + 11, sizeof(tmp));
	tmp = ntohl(tmp);

	// tmp = ntohl(*(uint32_t *)(tlv + 11));
	st.avgdelay = *(float *)&tmp;

	memcpy(&tmp, tlv + 15, sizeof(tmp));
	tmp = ntohl(tmp);

	// tmp = ntohl(*(uint32_t *)(tlv + 15));
	st.avgjitter = *(float *)&tmp;

	st.avgloss = tlv[19] / 255.;
	st.avgdup = tlv[20] == 0xff ? 1e10 : tlv[20] / 25.5;
	st.avgooo = tlv[21] / 255.;

	st.valid = true;

	return true;
}

static inline bool check_string(char *hd, int len, string &result) {
	for (int i = 0; i < len; i++) {
		if (hd[i] <= 0)
			return false;
	}
	result = string(hd, len);
	return true;
}

void handle_nmsg(const address &from, uint64_t recvdts, int ttl, uint8_t *buff, int len, bool ssm) {
	if (len < 4)
		return;

	if (ntohs(*((uint16_t *)buff)) != 0xbeac)
		return;

	if (buff[2] != PROTO_VER)
		return;

	if (buff[3] == 0) {
		if (len == 12) {
			uint32_t seq = ntohl(*((uint32_t *)(buff + 4)));
			uint32_t ts = ntohl(*((uint32_t *)(buff + 8)));
			getSource(from, 0, recvdts, true).update(ttl, seq, ts, recvdts, ssm);
		}
		return;
	} else if (buff[3] == 1) {
		if (len < 5)
			return;

		beaconSource &src = getSource(from, 0, recvdts, true);

		src.sttl = buff[4];

		len -= 5;

		for (uint8_t *hd = tlv_begin(buff + 5, len); hd; hd = tlv_next(hd, len)) {
			if (verbose > 4) {
				char tmp[64];
				from.print(tmp, sizeof(tmp));
				fprintf(stderr, "Parsing TLV (%i, %i) for %s [len is now %i]\n", (int)hd[0], (int)hd[1], tmp, len);
			}

			if (hd[0] == T_BEAC_NAME) {
				string name;
				if (check_string((char *)hd + 2, hd[1], name))
					src.setName(name);
			} else if (hd[0] == T_ADMIN_CONTACT) {
				check_string((char *)hd + 2, hd[1], src.adminContact);
			} else if (hd[0] == T_SOURCE_INFO || hd[0] == T_SOURCE_INFO_IPv4) {
				int blen = hd[0] == T_SOURCE_INFO ? 18 : 6;

				if (hd[1] < blen)
					continue;

				address addr;

				if (hd[0] == T_SOURCE_INFO) {
					sockaddr_in6 *a6 = (sockaddr_in6 *)&addr;

					a6->sin6_family = AF_INET6;

					memcpy(&a6->sin6_addr, hd + 2, sizeof(in6_addr));
					memcpy(&a6->sin6_port, hd + 18, sizeof(uint16_t));
				} else {
					sockaddr_in *a4 = (sockaddr_in *)&addr;

					a4->sin_family = AF_INET;

					memcpy(&a4->sin_addr, hd + 2, sizeof(in_addr));
					memcpy(&a4->sin_port, hd + 6, sizeof(uint16_t));
				}

				beaconExternalStats &stats = src.getExternal(addr, recvdts);

				int plen = hd[1] - blen;
				for (uint8_t *pd = tlv_begin(hd + 2 + blen, plen); pd; pd = tlv_next(pd, plen)) {
					if (pd[0] == T_BEAC_NAME) {
						if (check_string((char *)pd + 2, pd[1], stats.name)) {
							stats.identified = !stats.name.empty();
						}
					} else if (pd[0] == T_ADMIN_CONTACT) {
						check_string((char *)pd + 2, pd[1], stats.contact);
					} else if (pd[0] == T_ASM_STATS || pd[0] == T_SSM_STATS) {
						Stats *st = (pd[0] == T_ASM_STATS ? &stats.ASM : &stats.SSM);

						if (!read_tlv_stats(pd, stats, *st))
							break;
						st->lastupdate = recvdts;
					}
				}

				// trigger local SSM join
				if (!addr.is_equal(beaconUnicastAddr)) {
					beaconSource &t = getSource(addr, stats.identified ? stats.name.c_str() : 0, recvdts, false);
					if (t.adminContact.empty())
						t.adminContact = stats.contact;
				}
			} else if (hd[0] == T_WEBSITE_GENERIC || hd[0] == T_WEBSITE_LG || hd[0] == T_WEBSITE_MATRIX) {
				string url;
				if (check_string((char *)hd + 2, hd[1], url)) {
					src.webSites[hd[0]] = url;
				}
			} else if (hd[0] == T_CC) {
				if (hd[1] == 2) {
					src.CC = string((char *)hd + 2, 2);
				}
			} else if (hd[0] == T_LEAVE) {
				removeSource(from, false);
				break;
			}
		}
	}
}

<commit_msg>include stdio.h otherwhise gcc 2.95 complains on linux 2.4<commit_after>#include "dbeacon.h"
#include "protocol.h"
#include <math.h>
#include <netinet/in.h>
#include <stdio.h>

using namespace std;

static inline bool write_tlv_string(uint8_t *buf, int maxlen, int &pointer, uint8_t type, const char *str) {
	int len = strlen(str);
	if ((pointer + 2 + len) > maxlen)
		return false;
	buf[pointer + 0] = type;
	buf[pointer + 1] = len;
	memcpy(buf + pointer + 2, str, len);
	pointer += len + 2;
	return true;
}

static bool write_tlv_start(uint8_t *buff, int maxlen, int &ptr, uint8_t type, int len) {
	if ((ptr + len + 2) > maxlen)
		return false;

	buff[ptr + 0] = type;
	buff[ptr + 1] = len;

	ptr += 2;

	return true;
}

static bool write_tlv_stats(uint8_t *buff, int maxlen, int &ptr, uint8_t type, uint32_t age, int sttl, const beaconMcastState &st) {
	if (!write_tlv_start(buff, maxlen, ptr, type, 20))
		return false;

	uint8_t *b = buff + ptr;

	uint32_t val;

	val = htonl((uint32_t)st.s.timestamp);
	memcpy(b + 0, &val, sizeof(val));
	val = htonl(age);
	memcpy(b + 4, &val, sizeof(val));

	// *((uint32_t *)(b + 0)) = htonl((uint32_t)st.s.timestamp);
	// *((uint32_t *)(b + 4)) = htonl(age);

	b[8] = (sttl ? sttl : defaultTTL) - st.s.rttl;

	uint32_t *stats = (uint32_t *)(b + 9);

	val = htonl(*((uint32_t *)&st.s.avgdelay));
	memcpy(stats + 0, &val, sizeof(val));
	val = htonl(*((uint32_t *)&st.s.avgjitter));
	memcpy(stats + 1, &val, sizeof(val));

	// stats[0] = htonl(*((uint32_t *)&st.s.avgdelay));
	// stats[1] = htonl(*((uint32_t *)&st.s.avgjitter));

	b[17] = (uint8_t)(st.s.avgloss * 0xff);
	b[18] = st.s.avgdup > 10. ? 0xff : ((uint8_t)ceil(st.s.avgdup * 25.5));
	b[19] = (uint8_t)(st.s.avgooo * 0xff);

	ptr += 20;

	return true;
}

int build_report(uint8_t *buff, int maxlen, int type, bool publishsources) {
	if (maxlen < 4)
		return -1;

	// 0-2 magic
	*((uint16_t *)buff) = htons(0xbeac);

	// 3 version
	buff[2] = PROTO_VER;

	// 4 packet type
	buff[3] = 1; // Report

	buff[4] = defaultTTL; // Original Hop Limit

	int ptr = 5;

	if (!write_tlv_string(buff, maxlen, ptr, T_BEAC_NAME, beaconName.c_str()))
		return -1;
	if (!write_tlv_string(buff, maxlen, ptr, T_ADMIN_CONTACT, adminContact.c_str()))
		return -1;

	if (type == WEBSITE_REPORT) {
		for (WebSites::const_iterator j = webSites.begin(); j != webSites.end(); j++)
			if (!write_tlv_string(buff, maxlen, ptr, j->first, j->second.c_str()))
				return -1;
		if (!twoLetterCC.empty()) {
			if (!write_tlv_string(buff, maxlen, ptr, T_CC, twoLetterCC.c_str()))
				return -1;
		}
		return ptr;
	} else if (type == LEAVE_REPORT) {
		if (!write_tlv_start(buff, maxlen, ptr, T_LEAVE, 0))
			return -1;
		return ptr;
	}

	if (publishsources) {
		uint64_t now = get_timestamp();

		for (Sources::const_iterator i = sources.begin(); i != sources.end(); i++) {
			if (!i->second.identified)
				continue;

			if (!i->second.ASM.s.valid && !i->second.SSM.s.valid)
				continue;

			int len = 18;

			if (i->first.family() == AF_INET)
				len = 6;

			if (type == MAP_REPORT) {
				int namelen = i->second.name.size();
				int contactlen = i->second.adminContact.size();
				len += 2 + namelen + 2 + contactlen;
			} else {
				len += (i->second.ASM.s.valid ? 22 : 0) + (i->second.SSM.s.valid ? 22 : 0);
			}

			if (!write_tlv_start(buff, maxlen, ptr, i->first.family() == AF_INET6 ? T_SOURCE_INFO : T_SOURCE_INFO_IPv4, len))
				break;

			if (i->first.family() == AF_INET6) {
				const sockaddr_in6 *addr = i->first.v6();

				memcpy(buff + ptr, &addr->sin6_addr, sizeof(in6_addr));
				memcpy(buff + ptr + 16, &addr->sin6_port, sizeof(uint16_t));

				ptr += 18;
			} else {
				const sockaddr_in *addr = i->first.v4();

				memcpy(buff + ptr, &addr->sin_addr, sizeof(in_addr));
				memcpy(buff + ptr + 4, &addr->sin_port, sizeof(uint16_t));

				ptr += 6;
			}

			if (type == MAP_REPORT) {
				write_tlv_string(buff, maxlen, ptr, T_BEAC_NAME, i->second.name.c_str());
				write_tlv_string(buff, maxlen, ptr, T_ADMIN_CONTACT, i->second.adminContact.c_str());
			} else {
				uint32_t age = (now - i->second.creation) / 1000;

				if (i->second.ASM.s.valid)
					write_tlv_stats(buff, maxlen, ptr, T_ASM_STATS, age, i->second.sttl, i->second.ASM);
				if (i->second.SSM.s.valid)
					write_tlv_stats(buff, maxlen, ptr, T_SSM_STATS, age, i->second.sttl, i->second.SSM);
			}
		}
	}

	return ptr;
}

int build_probe(uint8_t *buff, int maxlen, uint32_t sn, uint64_t ts) {
	if (maxlen < (int)(4 + 4 + 4))
		return -1;

	// 0-2 magic
	*((uint16_t *)buff) = htons(0xbeac);

	// 3 version
	buff[2] = PROTO_VER;

	// 4 packet type
	buff[3] = 0; // Probe

	*((uint32_t *)(buff + 4)) = htonl(sn);
	*((uint32_t *)(buff + 8)) = htonl((uint32_t)ts);

	return 4 + 4 + 4;
}

static inline uint8_t *tlv_begin(uint8_t *hd, int &len) {
	if (len < 2 || hd[1] > len)
		return 0;
	return hd;
}

static inline uint8_t *tlv_next(uint8_t *hd, int &len) {
	len -= hd[1] + 2;
	return tlv_begin(hd + hd[1] + 2, len);
}

static bool read_tlv_stats(uint8_t *tlv, beaconExternalStats &extb, Stats &st) {
	if (tlv[1] != 20)
		return false;

	uint32_t tmp;

	memcpy(&tmp, tlv + 2, sizeof(tmp));
	st.timestamp = ntohl(tmp);
	memcpy(&tmp, tlv + 6, sizeof(tmp));
	extb.age = tmp;

	// st.timestamp = ntohl(*(uint32_t *)(tlv + 2));
	// extb.age = ntohl(*(uint32_t *)(tlv + 6));

	st.rttl = tlv[10];

	memcpy(&tmp, tlv + 11, sizeof(tmp));
	tmp = ntohl(tmp);

	// tmp = ntohl(*(uint32_t *)(tlv + 11));
	st.avgdelay = *(float *)&tmp;

	memcpy(&tmp, tlv + 15, sizeof(tmp));
	tmp = ntohl(tmp);

	// tmp = ntohl(*(uint32_t *)(tlv + 15));
	st.avgjitter = *(float *)&tmp;

	st.avgloss = tlv[19] / 255.;
	st.avgdup = tlv[20] == 0xff ? 1e10 : tlv[20] / 25.5;
	st.avgooo = tlv[21] / 255.;

	st.valid = true;

	return true;
}

static inline bool check_string(char *hd, int len, string &result) {
	for (int i = 0; i < len; i++) {
		if (hd[i] <= 0)
			return false;
	}
	result = string(hd, len);
	return true;
}

void handle_nmsg(const address &from, uint64_t recvdts, int ttl, uint8_t *buff, int len, bool ssm) {
	if (len < 4)
		return;

	if (ntohs(*((uint16_t *)buff)) != 0xbeac)
		return;

	if (buff[2] != PROTO_VER)
		return;

	if (buff[3] == 0) {
		if (len == 12) {
			uint32_t seq = ntohl(*((uint32_t *)(buff + 4)));
			uint32_t ts = ntohl(*((uint32_t *)(buff + 8)));
			getSource(from, 0, recvdts, true).update(ttl, seq, ts, recvdts, ssm);
		}
		return;
	} else if (buff[3] == 1) {
		if (len < 5)
			return;

		beaconSource &src = getSource(from, 0, recvdts, true);

		src.sttl = buff[4];

		len -= 5;

		for (uint8_t *hd = tlv_begin(buff + 5, len); hd; hd = tlv_next(hd, len)) {
			if (verbose > 4) {
				char tmp[64];
				from.print(tmp, sizeof(tmp));
				fprintf(stderr, "Parsing TLV (%i, %i) for %s [len is now %i]\n", (int)hd[0], (int)hd[1], tmp, len);
			}

			if (hd[0] == T_BEAC_NAME) {
				string name;
				if (check_string((char *)hd + 2, hd[1], name))
					src.setName(name);
			} else if (hd[0] == T_ADMIN_CONTACT) {
				check_string((char *)hd + 2, hd[1], src.adminContact);
			} else if (hd[0] == T_SOURCE_INFO || hd[0] == T_SOURCE_INFO_IPv4) {
				int blen = hd[0] == T_SOURCE_INFO ? 18 : 6;

				if (hd[1] < blen)
					continue;

				address addr;

				if (hd[0] == T_SOURCE_INFO) {
					sockaddr_in6 *a6 = (sockaddr_in6 *)&addr;

					a6->sin6_family = AF_INET6;

					memcpy(&a6->sin6_addr, hd + 2, sizeof(in6_addr));
					memcpy(&a6->sin6_port, hd + 18, sizeof(uint16_t));
				} else {
					sockaddr_in *a4 = (sockaddr_in *)&addr;

					a4->sin_family = AF_INET;

					memcpy(&a4->sin_addr, hd + 2, sizeof(in_addr));
					memcpy(&a4->sin_port, hd + 6, sizeof(uint16_t));
				}

				beaconExternalStats &stats = src.getExternal(addr, recvdts);

				int plen = hd[1] - blen;
				for (uint8_t *pd = tlv_begin(hd + 2 + blen, plen); pd; pd = tlv_next(pd, plen)) {
					if (pd[0] == T_BEAC_NAME) {
						if (check_string((char *)pd + 2, pd[1], stats.name)) {
							stats.identified = !stats.name.empty();
						}
					} else if (pd[0] == T_ADMIN_CONTACT) {
						check_string((char *)pd + 2, pd[1], stats.contact);
					} else if (pd[0] == T_ASM_STATS || pd[0] == T_SSM_STATS) {
						Stats *st = (pd[0] == T_ASM_STATS ? &stats.ASM : &stats.SSM);

						if (!read_tlv_stats(pd, stats, *st))
							break;
						st->lastupdate = recvdts;
					}
				}

				// trigger local SSM join
				if (!addr.is_equal(beaconUnicastAddr)) {
					beaconSource &t = getSource(addr, stats.identified ? stats.name.c_str() : 0, recvdts, false);
					if (t.adminContact.empty())
						t.adminContact = stats.contact;
				}
			} else if (hd[0] == T_WEBSITE_GENERIC || hd[0] == T_WEBSITE_LG || hd[0] == T_WEBSITE_MATRIX) {
				string url;
				if (check_string((char *)hd + 2, hd[1], url)) {
					src.webSites[hd[0]] = url;
				}
			} else if (hd[0] == T_CC) {
				if (hd[1] == 2) {
					src.CC = string((char *)hd + 2, 2);
				}
			} else if (hd[0] == T_LEAVE) {
				removeSource(from, false);
				break;
			}
		}
	}
}

<|endoftext|>


<commit_before>#include "ShaderFactory.h"

#include <vector>

#include "..\util\fileutil.h"

std::shared_ptr<ShaderProgram> ShaderFactory::makeShaderProgram(const std::string &shaderName)
{
	if (shaderName == "phongLighting" || true) // or true to always be true *for testing purposes
	{
		std::shared_ptr<ShaderProgram> program = std::make_shared<ShaderProgram>();
		program->shaderProgram = initGLProgram(shaderName);

		if (program->shaderProgram == 0)
		{
			printf("Error: ShaderFactory::makeShaderProgram failed to initialize shader program (%s).\n", shaderName.c_str());
			return nullptr;
		}

		program->initAttribLocations();
		//program->attribLocations["vertex"] = glGetAttribLocation(program->shaderProgram, "vertex");
		//program->attribLocations["normal"] = glGetAttribLocation(program->shaderProgram, "normal");

		program->initMaterialUniforms();
		program->initLightUniforms();
		program->initModelViewProjectionUniforms();

		GLState nullState;
		program->updateAllUniforms(nullState);

		return program;
	}

	return nullptr;
}

GLuint ShaderFactory::initGLProgram(const std::string &shaderProgram)
{
	GLuint glProgram = glCreateProgram();
	if (glProgram == 0)
	{
		printf("Error: ShaderFactory::initGLProgram cannot create shader program (%s).\n", shaderProgram.c_str());
		return 0;
	}

	GLuint vertShader = glCreateShader(GL_VERTEX_SHADER);
	GLuint fragShader = glCreateShader(GL_FRAGMENT_SHADER);
	if (vertShader == 0 || fragShader == 0)
	{
		printf("Error: ShaderFacctory::initGLProgram unable to create shader objects.");
		return 0;
	}

	std::string vertexShaderSourceCodePath = "shader//" + shaderProgram + ".v.glsl";
	std::string vertexShaderSourceCode = readTextFile(vertexShaderSourceCodePath);
	const char* vertexShaderSource = vertexShaderSourceCode.c_str();
	int vsLength = static_cast<int>(vertexShaderSourceCode.length());
	if (vsLength == 0)
	{
		printf("Error: ShaderFactory::initGLProgram received empty vertex shader.\n");
		return 0;
	}

	std::string fragmentShaderSourceCode = readTextFile("shader//" + shaderProgram + ".f.glsl");
	const char* fragmentShaderSource = fragmentShaderSourceCode.c_str();
	int fsLength = static_cast<int>(fragmentShaderSourceCode.length());
	if (fsLength == 0)
	{
		printf("Error: ShaderFactory::initGLProgram received empty fragment shader.\n");
		return 0;
	}

	glShaderSource(vertShader, 1, &vertexShaderSource, &vsLength);
	glShaderSource(fragShader, 1, &fragmentShaderSource, &fsLength);

	glCompileShader(vertShader);
	glCompileShader(fragShader);

	GLint success = 0;
	glGetShaderiv(vertShader, GL_COMPILE_STATUS, &success);
	if (success == GL_FALSE)
	{
		// get length of InfoLog, create vector with length and put error into vector
		GLint infoLength = 0;
		glGetShaderiv(vertShader, GL_INFO_LOG_LENGTH, &infoLength);
		std::vector<GLchar> errorLog(infoLength);
		glGetShaderInfoLog(vertShader, infoLength, &infoLength, errorLog.data());

		// Construct string and output error
		std::string errorInfoLog(errorLog.begin(), errorLog.end());
		printf("Error compiling vertex shadeer: \n %s \n", errorInfoLog.c_str());

		// don't leak shaders
		glDeleteShader(vertShader);
		glDeleteShader(fragShader);

		return 0;
	}

	glGetShaderiv(fragShader, GL_COMPILE_STATUS, &success);
	if (success == GL_FALSE)
	{
		// Get length of InfoLog, create vector with length and put errorLog into vector
		GLint infoLength = 0;
		glGetShaderiv(fragShader, GL_INFO_LOG_LENGTH, &infoLength);
		std::vector<GLchar> errorLog(infoLength);
		glGetShaderInfoLog(fragShader, infoLength, &infoLength, errorLog.data());

		// Construct string and output error
		std::string errorInfoLog(errorLog.begin(), errorLog.end());
		printf("Error compiling fragment shader:\n %s \n", errorInfoLog.c_str());

		// Don't leak shaders
		glDeleteShader(vertShader);
		glDeleteShader(fragShader);

		return 0;
	}

	// Attach shaders to program and link it
	glAttachShader(glProgram, vertShader);
	glAttachShader(glProgram, fragShader);
	glLinkProgram(glProgram);

	// Check for linking error
	glGetProgramiv(glProgram, GL_LINK_STATUS, &success);
	if (success == GL_FALSE)
	{
		GLint infoLength = 0;
		glGetProgramiv(glProgram, GL_INFO_LOG_LENGTH, &infoLength);
		std::vector<GLchar> errorLog(infoLength);
		glGetProgramInfoLog(glProgram, infoLength, &infoLength, errorLog.data());

		// Construct string and output error
		std::string errorInfoLog(errorLog.begin(), errorLog.end());
		printf("Error compiling fragment shader: \n %s \n", errorInfoLog.c_str());

		// Don't leak program or shaders
		glDeleteProgram(glProgram);
		glDeleteShader(vertShader);
		glDeleteShader(fragShader);

		return 0;
	}

	// Successful linking, now detach and delete shaders
	glDetachShader(glProgram, vertShader);
	glDetachShader(glProgram, fragShader);

	glDeleteShader(vertShader);
	glDeleteShader(fragShader);

	// Validate program
	glValidateProgram(glProgram);

	// Check for validation errors
	glGetProgramiv(glProgram, GL_VALIDATE_STATUS, &success);
	if (success == GL_FALSE)
	{
		GLint infoLength = 0;
		glGetProgramiv(glProgram, GL_INFO_LOG_LENGTH, &infoLength);
		std::vector<GLchar> errorLog(infoLength);
		glGetProgramInfoLog(glProgram, infoLength, &infoLength, errorLog.data());

		// Construct string and output error
		std::string errorInfoLog(errorLog.begin(), errorLog.end());
		printf("Error: Invalid shaderprogram: \n %s \n", errorInfoLog.c_str());

		return 0;
	}

	// Program was successfully linked and validated!
	return glProgram;
}<commit_msg>Employing initTextureUniforms(). Changing error output messages to make it easier to understand.<commit_after>#include "ShaderFactory.h"

#include <vector>

#include "..\util\fileutil.h"

std::shared_ptr<ShaderProgram> ShaderFactory::makeShaderProgram(const std::string &shaderName)
{
	if (shaderName == "phongLighting" || true) // or true to always be true *for testing purposes
	{
		std::shared_ptr<ShaderProgram> program = std::make_shared<ShaderProgram>();
		program->shaderProgram = initGLProgram(shaderName);

		if (program->shaderProgram == 0)
		{
			printf("Warning: ShaderFactory failed to initialize shader: (%s).\n", shaderName.c_str());
			return nullptr;
		}

		program->initAttribLocations();
		//program->attribLocations["vertex"] = glGetAttribLocation(program->shaderProgram, "vertex");
		//program->attribLocations["normal"] = glGetAttribLocation(program->shaderProgram, "normal");

		program->initTextureUniforms();
		program->initMaterialUniforms();
		program->initLightUniforms();
		program->initModelViewProjectionUniforms();

		GLState nullState;
		program->updateAllUniforms(nullState);

		printf("%s has been loaded as a proper ShaderProgram.\n", shaderName.c_str());

		return program;
	}

	return nullptr;
}

GLuint ShaderFactory::initGLProgram(const std::string &shaderProgram)
{
	GLuint glProgram = glCreateProgram();
	if (glProgram == 0)
	{
		GLenum error = glGetError();

		std::string errorMsg = "Could not create a new shader program. ";

		switch (error)
		{
			case GL_NO_ERROR:
				errorMsg += "No Error\n";
				break;
			case GL_INVALID_ENUM:
				errorMsg += "An unacceptable value is specified for an enumerated argument.\n";
				break;
			case GL_INVALID_VALUE:
				errorMsg += "A numeric argument is out of range.\n";
				break;
			case GL_INVALID_OPERATION:
				errorMsg += "The specified operation has been performed out of context, or is not allowed in the current state.\n";
				break;
			case GL_INVALID_FRAMEBUFFER_OPERATION:
				errorMsg += "The command is trying to render to or read from the framebuffer while the currently bound framebuffer is not framebuffer complete (i.e. the return value from glCheckFramebufferStatus is not GL_FRAMEBUFFER_COMPLETE).\n";
				break;
			case GL_OUT_OF_MEMORY:
				errorMsg += "There is not enough memory left to execute the command.\n";
				break;
			case GL_STACK_UNDERFLOW:
				errorMsg += "An attempt has been made to perform an operation that would cause an internal stack to underflow.\n";
				break;
			case GL_STACK_OVERFLOW:
				errorMsg += "An attempt has been made to perform an operation that would cause an internal stack to overflow.\n";
				break;
			default:
				errorMsg += "Reason unknown (default).\n";
		}

		printf("Error: ShaderFactory: Program cannot be created (%s).\n", shaderProgram.c_str());
		printf("%s\n", errorMsg.c_str());
		return 0;
	}

	GLuint vertShader = glCreateShader(GL_VERTEX_SHADER);
	GLuint fragShader = glCreateShader(GL_FRAGMENT_SHADER);
	if (vertShader == 0 || fragShader == 0)
	{
		printf("Error: ShaderFacctory: Unable to create shader objects.");
		return 0;
	}

	std::string vertexShaderSourceCodePath = "shader//" + shaderProgram + ".v.glsl";
	std::string vertexShaderSourceCode = readTextFile(vertexShaderSourceCodePath);
	const char* vertexShaderSource = vertexShaderSourceCode.c_str();
	int vsLength = static_cast<int>(vertexShaderSourceCode.length());
	if (vsLength == 0)
	{
		printf("Error: ShaderFactory: Received empty vertex shader.\n");
		return 0;
	}

	std::string fragmentShaderSourceCode = readTextFile("shader//" + shaderProgram + ".f.glsl");
	const char* fragmentShaderSource = fragmentShaderSourceCode.c_str();
	int fsLength = static_cast<int>(fragmentShaderSourceCode.length());
	if (fsLength == 0)
	{
		printf("Error: ShaderFactory::initGLProgram received empty fragment shader.\n");
		return 0;
	}

	glShaderSource(vertShader, 1, &vertexShaderSource, &vsLength);
	glShaderSource(fragShader, 1, &fragmentShaderSource, &fsLength);

	glCompileShader(vertShader);
	glCompileShader(fragShader);

	GLint success = 0;
	glGetShaderiv(vertShader, GL_COMPILE_STATUS, &success);
	if (success == GL_FALSE)
	{
		// get length of InfoLog, create vector with length and put error into vector
		GLint infoLength = 0;
		glGetShaderiv(vertShader, GL_INFO_LOG_LENGTH, &infoLength);
		std::vector<GLchar> errorLog(infoLength);
		glGetShaderInfoLog(vertShader, infoLength, &infoLength, errorLog.data());

		// Construct string and output error
		std::string errorInfoLog(errorLog.begin(), errorLog.end());
		printf("Error compiling vertex shadeer: \n %s \n", errorInfoLog.c_str());

		// don't leak shaders
		glDeleteShader(vertShader);
		glDeleteShader(fragShader);

		return 0;
	}

	glGetShaderiv(fragShader, GL_COMPILE_STATUS, &success);
	if (success == GL_FALSE)
	{
		// Get length of InfoLog, create vector with length and put errorLog into vector
		GLint infoLength = 0;
		glGetShaderiv(fragShader, GL_INFO_LOG_LENGTH, &infoLength);
		std::vector<GLchar> errorLog(infoLength);
		glGetShaderInfoLog(fragShader, infoLength, &infoLength, errorLog.data());

		// Construct string and output error
		std::string errorInfoLog(errorLog.begin(), errorLog.end());
		printf("Error compiling fragment shader:\n %s \n", errorInfoLog.c_str());

		// Don't leak shaders
		glDeleteShader(vertShader);
		glDeleteShader(fragShader);

		return 0;
	}

	// Attach shaders to program and link it
	glAttachShader(glProgram, vertShader);
	glAttachShader(glProgram, fragShader);
	glLinkProgram(glProgram);

	// Check for linking error
	glGetProgramiv(glProgram, GL_LINK_STATUS, &success);
	if (success == GL_FALSE)
	{
		GLint infoLength = 0;
		glGetProgramiv(glProgram, GL_INFO_LOG_LENGTH, &infoLength);
		std::vector<GLchar> errorLog(infoLength);
		glGetProgramInfoLog(glProgram, infoLength, &infoLength, errorLog.data());

		// Construct string and output error
		std::string errorInfoLog(errorLog.begin(), errorLog.end());
		printf("Error compiling fragment shader: \n %s \n", errorInfoLog.c_str());

		// Don't leak program or shaders
		glDeleteProgram(glProgram);
		glDeleteShader(vertShader);
		glDeleteShader(fragShader);

		return 0;
	}

	// Successful linking, now detach and delete shaders
	glDetachShader(glProgram, vertShader);
	glDetachShader(glProgram, fragShader);

	glDeleteShader(vertShader);
	glDeleteShader(fragShader);

	// Validate program
	glValidateProgram(glProgram);

	// Check for validation errors
	glGetProgramiv(glProgram, GL_VALIDATE_STATUS, &success);
	if (success == GL_FALSE)
	{
		GLint infoLength = 0;
		glGetProgramiv(glProgram, GL_INFO_LOG_LENGTH, &infoLength);
		std::vector<GLchar> errorLog(infoLength);
		glGetProgramInfoLog(glProgram, infoLength, &infoLength, errorLog.data());

		// Construct string and output error
		std::string errorInfoLog(errorLog.begin(), errorLog.end());
		printf("Error: Invalid shaderprogram: \n %s \n", errorInfoLog.c_str());

		return 0;
	}

	// Program was successfully linked and validated!
	return glProgram;
}<|endoftext|>


<commit_before>/*
 * Copyright (c) 2014 - 2017 Stephen F. Booth <me@sbooth.org>
 * See https://github.com/sbooth/SFBAudioEngine/blob/master/LICENSE.txt for license information
 */

#include <algorithm>
#include <array>

#include "DoPDecoder.h"
#include "CFErrorUtilities.h"
#include "Logger.h"

#define DSD_FRAMES_PER_DOP_FRAME 16

namespace {
	// Bit reversal lookup table from http://graphics.stanford.edu/~seander/bithacks.html#BitReverseTable
	static const uint8_t sBitReverseTable256 [256] =
	{
#   define R2(n)     n,     n + 2*64,     n + 1*64,     n + 3*64
#   define R4(n) R2(n), R2(n + 2*16), R2(n + 1*16), R2(n + 3*16)
#   define R6(n) R4(n), R4(n + 2*4 ), R4(n + 1*4 ), R4(n + 3*4 )
		R6(0), R6(2), R6(1), R6(3)
	};

	// Support DSD64, DSD128, and DSD256 (64x, 128x, and 256x the CD sample rate of 44.1 KHz)
	// as well as the 48.0 KHz variants 6.144 MHz and 12.288 MHz
	static const std::array<Float64, 5> sSupportedSampleRates = { {2822400, 5644800, 11289600, 6144000, 12288000} };
}

#pragma mark Factory Methods

SFB::Audio::Decoder::unique_ptr SFB::Audio::DoPDecoder::CreateForURL(CFURLRef url, CFErrorRef *error)
{
	return CreateForInputSource(InputSource::CreateForURL(url, 0, error), error);
}

SFB::Audio::Decoder::unique_ptr SFB::Audio::DoPDecoder::CreateForInputSource(InputSource::unique_ptr inputSource, CFErrorRef *error)
{
	if(!inputSource)
		return nullptr;

	return CreateForDecoder(Decoder::CreateForInputSource(std::move(inputSource), error), error);
}

SFB::Audio::Decoder::unique_ptr SFB::Audio::DoPDecoder::CreateForDecoder(unique_ptr decoder, CFErrorRef *error)
{
#pragma unused(error)

	if(!decoder)
		return nullptr;

	return unique_ptr(new DoPDecoder(std::move(decoder)));
}

SFB::Audio::DoPDecoder::DoPDecoder(Decoder::unique_ptr decoder)
	: mDecoder(std::move(decoder)), mMarker(0x05), mReverseBits(false)
{
	assert(nullptr != mDecoder);
}

bool SFB::Audio::DoPDecoder::_Open(CFErrorRef *error)
{
	if(!mDecoder->IsOpen() && !mDecoder->Open(error))
		return false;

	const auto& decoderFormat = mDecoder->GetFormat();

	if(!decoderFormat.IsDSD()) {
		if(error) {
			SFB::CFString description(CFCopyLocalizedString(CFSTR("The file “%@” is not a valid DSD file."), ""));
			SFB::CFString failureReason(CFCopyLocalizedString(CFSTR("Not a DSD file"), ""));
			SFB::CFString recoverySuggestion(CFCopyLocalizedString(CFSTR("The file's extension may not match the file's type."), ""));

			*error = CreateErrorForURL(Decoder::ErrorDomain, Decoder::InputOutputError, description, GetURL(), failureReason, recoverySuggestion);
		}

		return false;
	}

	if(std::end(sSupportedSampleRates) == std::find(std::begin(sSupportedSampleRates), std::end(sSupportedSampleRates), decoderFormat.mSampleRate)) {
		LOGGER_ERR("org.sbooth.AudioEngine.Decoder.DOP", "Unsupported sample rate: " << decoderFormat.mSampleRate);

		if(error) {
			SFB::CFString description(CFCopyLocalizedString(CFSTR("The file “%@” is not supported."), ""));
			SFB::CFString failureReason(CFCopyLocalizedString(CFSTR("Unsupported DSD sample rate"), ""));
			SFB::CFString recoverySuggestion(CFCopyLocalizedString(CFSTR("The file's sample rate is not supported for DSD over PCM."), ""));

			*error = CreateErrorForURL(Decoder::ErrorDomain, Decoder::InputOutputError, description, GetURL(), failureReason, recoverySuggestion);
		}

		return false;
	}

	mBufferList.Allocate(decoderFormat, 4096);

	// Generate non-interleaved 24-bit big endian output
	mFormat.mFormatID			= kAudioFormatDoP;
	mFormat.mFormatFlags		= kAudioFormatFlagIsBigEndian | kAudioFormatFlagIsPacked | kAudioFormatFlagIsSignedInteger | kAudioFormatFlagIsNonInterleaved;

	mFormat.mSampleRate			= decoderFormat.mSampleRate / DSD_FRAMES_PER_DOP_FRAME;
	mFormat.mChannelsPerFrame	= decoderFormat.mChannelsPerFrame;
	mFormat.mBitsPerChannel		= 24;

	mFormat.mBytesPerPacket		= mFormat.mBitsPerChannel / 8;
	mFormat.mFramesPerPacket	= 1;
	mFormat.mBytesPerFrame		= mFormat.mBytesPerPacket * mFormat.mFramesPerPacket;

	mFormat.mReserved			= 0;

	mReverseBits = !(kAudioFormatFlagIsBigEndian & decoderFormat.mFormatFlags);

	return true;
}

bool SFB::Audio::DoPDecoder::_Close(CFErrorRef *error)
{
	if(!mDecoder->Close(error))
		return false;

	mBufferList.Deallocate();

	return true;
}

SFB::CFString SFB::Audio::DoPDecoder::_GetSourceFormatDescription() const
{
	return CFString(mDecoder->CreateSourceFormatDescription());
}

#pragma mark Functionality

UInt32 SFB::Audio::DoPDecoder::_ReadAudio(AudioBufferList *bufferList, UInt32 frameCount)
{
	// Only multiples of 16 frames can be read (16 frames equals two bytes)
	if(bufferList->mNumberBuffers != mFormat.mChannelsPerFrame || 0 != frameCount % 16) {
		LOGGER_WARNING("org.sbooth.AudioEngine.Decoder.DOP", "_ReadAudio() called with invalid parameters");
		return 0;
	}

	UInt32 framesRead = 0;

	// Reset output buffer data size
	for(UInt32 i = 0; i < bufferList->mNumberBuffers; ++i)
		bufferList->mBuffers[i].mDataByteSize = 0;

	for(;;) {
		// Grab the DSD audio
		UInt32 framesRemaining = frameCount - framesRead;
		UInt32 dsdFramesRemaining = DSD_FRAMES_PER_DOP_FRAME * framesRemaining;
		UInt32 dsdFramesDecoded = mDecoder->ReadAudio(mBufferList, std::min(mBufferList.GetCapacityFrames(), dsdFramesRemaining));
		if(0 == dsdFramesDecoded)
			break;

		UInt32 framesDecoded = dsdFramesDecoded / DSD_FRAMES_PER_DOP_FRAME;

		// Convert to DoP
		for(UInt32 i = 0; i < mBufferList->mNumberBuffers; ++i) {
			const unsigned char *src = (const unsigned char *)mBufferList->mBuffers[i].mData;
			unsigned char *dst = (unsigned char *)bufferList->mBuffers[i].mData + bufferList->mBuffers[i].mDataByteSize;

			for(UInt32 j = 0; j < framesDecoded; ++j) {
				// Insert the DSD marker
				*dst++ = mMarker;

				// Copy the DSD bits
				if(mReverseBits) {
					*dst++ = sBitReverseTable256[*src++];
					*dst++ = sBitReverseTable256[*src++];
				}
				else {
					*dst++ = *src++;
					*dst++ = *src++;
				}

				mMarker = (uint8_t)0x05 == mMarker ? (uint8_t)0xfa : (uint8_t)0x05;
			}

			bufferList->mBuffers[i].mDataByteSize += mFormat.FrameCountToByteCount(framesDecoded);
		}

		framesRead += framesDecoded;

		// All requested frames were read
		if(framesRead == frameCount)
			break;
	}

	return framesRead;
}

SInt64 SFB::Audio::DoPDecoder::_GetTotalFrames() const
{
	return mDecoder->GetTotalFrames() / DSD_FRAMES_PER_DOP_FRAME;
}

SInt64 SFB::Audio::DoPDecoder::_GetCurrentFrame() const
{
	return mDecoder->GetCurrentFrame() / DSD_FRAMES_PER_DOP_FRAME;
}

SInt64 SFB::Audio::DoPDecoder::_SeekToFrame(SInt64 frame)
{
	if(-1 == mDecoder->SeekToFrame(DSD_FRAMES_PER_DOP_FRAME * frame))
		return -1;

	for(UInt32 i = 0; i < mBufferList->mNumberBuffers; ++i)
		mBufferList->mBuffers[i].mDataByteSize = 0;

	return _GetCurrentFrame();
}
<commit_msg>Check for allocation failure<commit_after>/*
 * Copyright (c) 2014 - 2017 Stephen F. Booth <me@sbooth.org>
 * See https://github.com/sbooth/SFBAudioEngine/blob/master/LICENSE.txt for license information
 */

#include <algorithm>
#include <array>

#include "DoPDecoder.h"
#include "CFErrorUtilities.h"
#include "Logger.h"

#define DSD_FRAMES_PER_DOP_FRAME 16

namespace {
	// Bit reversal lookup table from http://graphics.stanford.edu/~seander/bithacks.html#BitReverseTable
	static const uint8_t sBitReverseTable256 [256] =
	{
#   define R2(n)     n,     n + 2*64,     n + 1*64,     n + 3*64
#   define R4(n) R2(n), R2(n + 2*16), R2(n + 1*16), R2(n + 3*16)
#   define R6(n) R4(n), R4(n + 2*4 ), R4(n + 1*4 ), R4(n + 3*4 )
		R6(0), R6(2), R6(1), R6(3)
	};

	// Support DSD64, DSD128, and DSD256 (64x, 128x, and 256x the CD sample rate of 44.1 KHz)
	// as well as the 48.0 KHz variants 6.144 MHz and 12.288 MHz
	static const std::array<Float64, 5> sSupportedSampleRates = { {2822400, 5644800, 11289600, 6144000, 12288000} };
}

#pragma mark Factory Methods

SFB::Audio::Decoder::unique_ptr SFB::Audio::DoPDecoder::CreateForURL(CFURLRef url, CFErrorRef *error)
{
	return CreateForInputSource(InputSource::CreateForURL(url, 0, error), error);
}

SFB::Audio::Decoder::unique_ptr SFB::Audio::DoPDecoder::CreateForInputSource(InputSource::unique_ptr inputSource, CFErrorRef *error)
{
	if(!inputSource)
		return nullptr;

	return CreateForDecoder(Decoder::CreateForInputSource(std::move(inputSource), error), error);
}

SFB::Audio::Decoder::unique_ptr SFB::Audio::DoPDecoder::CreateForDecoder(unique_ptr decoder, CFErrorRef *error)
{
#pragma unused(error)

	if(!decoder)
		return nullptr;

	return unique_ptr(new DoPDecoder(std::move(decoder)));
}

SFB::Audio::DoPDecoder::DoPDecoder(Decoder::unique_ptr decoder)
	: mDecoder(std::move(decoder)), mMarker(0x05), mReverseBits(false)
{
	assert(nullptr != mDecoder);
}

bool SFB::Audio::DoPDecoder::_Open(CFErrorRef *error)
{
	if(!mDecoder->IsOpen() && !mDecoder->Open(error))
		return false;

	const auto& decoderFormat = mDecoder->GetFormat();

	if(!decoderFormat.IsDSD()) {
		if(error) {
			SFB::CFString description(CFCopyLocalizedString(CFSTR("The file “%@” is not a valid DSD file."), ""));
			SFB::CFString failureReason(CFCopyLocalizedString(CFSTR("Not a DSD file"), ""));
			SFB::CFString recoverySuggestion(CFCopyLocalizedString(CFSTR("The file's extension may not match the file's type."), ""));

			*error = CreateErrorForURL(Decoder::ErrorDomain, Decoder::InputOutputError, description, GetURL(), failureReason, recoverySuggestion);
		}

		return false;
	}

	if(std::end(sSupportedSampleRates) == std::find(std::begin(sSupportedSampleRates), std::end(sSupportedSampleRates), decoderFormat.mSampleRate)) {
		LOGGER_ERR("org.sbooth.AudioEngine.Decoder.DOP", "Unsupported sample rate: " << decoderFormat.mSampleRate);

		if(error) {
			SFB::CFString description(CFCopyLocalizedString(CFSTR("The file “%@” is not supported."), ""));
			SFB::CFString failureReason(CFCopyLocalizedString(CFSTR("Unsupported DSD sample rate"), ""));
			SFB::CFString recoverySuggestion(CFCopyLocalizedString(CFSTR("The file's sample rate is not supported for DSD over PCM."), ""));

			*error = CreateErrorForURL(Decoder::ErrorDomain, Decoder::InputOutputError, description, GetURL(), failureReason, recoverySuggestion);
		}

		return false;
	}

	if(!mBufferList.Allocate(decoderFormat, 4096)) {
		LOGGER_CRIT("org.sbooth.AudioEngine.Decoder.DoP", "Unable to allocate memory")

		if(error)
			*error = CFErrorCreate(kCFAllocatorDefault, kCFErrorDomainPOSIX, ENOMEM, nullptr);

		return false;
	}

	// Generate non-interleaved 24-bit big endian output
	mFormat.mFormatID			= kAudioFormatDoP;
	mFormat.mFormatFlags		= kAudioFormatFlagIsBigEndian | kAudioFormatFlagIsPacked | kAudioFormatFlagIsSignedInteger | kAudioFormatFlagIsNonInterleaved;

	mFormat.mSampleRate			= decoderFormat.mSampleRate / DSD_FRAMES_PER_DOP_FRAME;
	mFormat.mChannelsPerFrame	= decoderFormat.mChannelsPerFrame;
	mFormat.mBitsPerChannel		= 24;

	mFormat.mBytesPerPacket		= mFormat.mBitsPerChannel / 8;
	mFormat.mFramesPerPacket	= 1;
	mFormat.mBytesPerFrame		= mFormat.mBytesPerPacket * mFormat.mFramesPerPacket;

	mFormat.mReserved			= 0;

	mReverseBits = !(kAudioFormatFlagIsBigEndian & decoderFormat.mFormatFlags);

	return true;
}

bool SFB::Audio::DoPDecoder::_Close(CFErrorRef *error)
{
	if(!mDecoder->Close(error))
		return false;

	mBufferList.Deallocate();

	return true;
}

SFB::CFString SFB::Audio::DoPDecoder::_GetSourceFormatDescription() const
{
	return CFString(mDecoder->CreateSourceFormatDescription());
}

#pragma mark Functionality

UInt32 SFB::Audio::DoPDecoder::_ReadAudio(AudioBufferList *bufferList, UInt32 frameCount)
{
	// Only multiples of 16 frames can be read (16 frames equals two bytes)
	if(bufferList->mNumberBuffers != mFormat.mChannelsPerFrame || 0 != frameCount % 16) {
		LOGGER_WARNING("org.sbooth.AudioEngine.Decoder.DOP", "_ReadAudio() called with invalid parameters");
		return 0;
	}

	UInt32 framesRead = 0;

	// Reset output buffer data size
	for(UInt32 i = 0; i < bufferList->mNumberBuffers; ++i)
		bufferList->mBuffers[i].mDataByteSize = 0;

	for(;;) {
		// Grab the DSD audio
		UInt32 framesRemaining = frameCount - framesRead;
		UInt32 dsdFramesRemaining = DSD_FRAMES_PER_DOP_FRAME * framesRemaining;
		UInt32 dsdFramesDecoded = mDecoder->ReadAudio(mBufferList, std::min(mBufferList.GetCapacityFrames(), dsdFramesRemaining));
		if(0 == dsdFramesDecoded)
			break;

		UInt32 framesDecoded = dsdFramesDecoded / DSD_FRAMES_PER_DOP_FRAME;

		// Convert to DoP
		// NB: Currently DSDIFFDecoder and DSFDecoder only produce non-interleaved output
		for(UInt32 i = 0; i < mBufferList->mNumberBuffers; ++i) {
			const unsigned char *src = (const unsigned char *)mBufferList->mBuffers[i].mData;
			unsigned char *dst = (unsigned char *)bufferList->mBuffers[i].mData + bufferList->mBuffers[i].mDataByteSize;

			for(UInt32 j = 0; j < framesDecoded; ++j) {
				// Insert the DSD marker
				*dst++ = mMarker;

				// Copy the DSD bits
				if(mReverseBits) {
					*dst++ = sBitReverseTable256[*src++];
					*dst++ = sBitReverseTable256[*src++];
				}
				else {
					*dst++ = *src++;
					*dst++ = *src++;
				}

				mMarker = (uint8_t)0x05 == mMarker ? (uint8_t)0xfa : (uint8_t)0x05;
			}

			bufferList->mBuffers[i].mDataByteSize += mFormat.FrameCountToByteCount(framesDecoded);
		}

		framesRead += framesDecoded;

		// All requested frames were read
		if(framesRead == frameCount)
			break;
	}

	return framesRead;
}

SInt64 SFB::Audio::DoPDecoder::_GetTotalFrames() const
{
	return mDecoder->GetTotalFrames() / DSD_FRAMES_PER_DOP_FRAME;
}

SInt64 SFB::Audio::DoPDecoder::_GetCurrentFrame() const
{
	return mDecoder->GetCurrentFrame() / DSD_FRAMES_PER_DOP_FRAME;
}

SInt64 SFB::Audio::DoPDecoder::_SeekToFrame(SInt64 frame)
{
	if(-1 == mDecoder->SeekToFrame(DSD_FRAMES_PER_DOP_FRAME * frame))
		return -1;

	for(UInt32 i = 0; i < mBufferList->mNumberBuffers; ++i)
		mBufferList->mBuffers[i].mDataByteSize = 0;

	return _GetCurrentFrame();
}
<|endoftext|>