text:
// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is licensed under the Apache License, Version 2.0 (the
//  "License"); you may not use this file except in compliance with the
//  License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

/// \file Metadata.cc
///

/* ------------------------------------------------------------------
 * MOCMetadata.cc
 *
 * General functions for reading MOC ephemeris information provided by
 * Malin Space Science System.  This file contains funcitons for reading
 * the following two types of data file:
 *
 * 1. description.tab files that contain tabulated MOC ephemeris information.
 *    This information is essentially what would be available in the PDS header.
 *
 * 2. SUP files that contain detailed information about camera position, velocity,
 *    pose, focal length, pixel summing, and image size for a given MOC image.
 *    There is also a function for generating a VRML model that depicts the
 *    location of the MGS orbiting Mars at the position and pose corresponding
 *    to two MOC-NA image shots.
 */
#include <asp/Sessions/MOC/Metadata.h>
#include <asp/Sessions/MOC/Ephemeris.h>
#include <asp/SpiceIO/SpiceUtilities.h>
#include <asp/SpiceIO/TabulatedDataReader.h>

// VisionWorkbench
#include <vw/Core/Log.h>
#include <vw/Camera/OrbitingPushbroomModel.h>

// Boost
#include <boost/algorithm/string.hpp>

// STL
#include <vector>
#include <string>
#include <iostream>                        // for WriteVizSiteFrame
#include <fstream>                         // for WriteVizSiteFrame

using namespace std;
using namespace vw;
// Circumference of Mars in meters:
static const double kMarsCircumference = 21344.0e6; // km
static const double kMarsRadius = 3397.0e3;             // km, equatorial (polar is 3395)

/* ----------------------------------------------------------------------
 *               MOCImageMetadata Class Methods
 * ----------------------------------------------------------------------*/

// Parse the MOC image number from the filename
MOCImageMetadata::MOCImageMetadata(std::string const& filename) {
  m_filename = filename;
  m_focal_length = 3.437;
  m_along_scan_pixel_size = 13e-6;
  m_across_scan_pixel_size = 13e-6;
  m_start_sample = -9999;

  // Change to upper case
  std::string name = boost::to_upper_copy(filename);

  // Remove any file suffix
  std::vector<string> suffix_split;
  boost::split( suffix_split, name, boost::is_any_of(".") );

  // Erase all seperators
  std::vector<string> vec;
  boost::split( vec, suffix_split[0], boost::is_any_of("-/") );
  if (vec.size() == 2) {
    string first = vec[0];
    string last = vec[1];

    // Fix discrepancy between FHGA and FHA
    if (first == "FHGA")
      first = "FHA";

    // Insert extra 0 if needed
    if (first.length() < 3)
      first.insert(1,"0");

    while (last.length() < 5)
      last.insert(0,"0");

    m_moc_identifier = (first + "/" + last);
  } else if (vec.size() == 1 && vec[0].size() > 5){
    int len = vec[0].size();
    m_moc_identifier = vec[0].substr(0,len-5) + "/" + vec[0].substr(len-5, len-1);
  } else {
    // In this case we assume that the
    std::cout << "WARNING: failed to parse the filename \"" << filename << "\"."
              << "Data lookup in \n";
    m_moc_identifier = filename;
  }
}

/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
 *             GENERATE CAMERA MODEL FROM FREE PARAMETERS
 * - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
vw::camera::CameraModel* MOCImageMetadata::camera_model() {

  int start_sample;
  if (m_start_sample == -9999) {
    std::cout << "WARNING: SETTING START_SAMPLE TO -1024.\n";
    start_sample = int(-1024/m_crosstrack_summing);
  } else {
    start_sample = int(m_start_sample/m_crosstrack_summing);
  }

  // Use the values that were obtained from the *.sup file to program
  // the camera model parameters.
  return new vw::camera::OrbitingPushbroomModel( rows(), // number of lines
                                                 cols(), // sampels per line
                                                 start_sample,
                                                 m_focal_length,
                                                 m_along_scan_pixel_size*m_downtrack_summing,
                                                 m_across_scan_pixel_size*m_crosstrack_summing,
                                                 scan_duration() / rows(), // line integration time
                                                 m_t0_quat, m_dt_quat,
                                                 m_t0_ephem, m_dt_ephem,
                                                 Vector3(0,0,1),  // pointing_vect
                                                 Vector3(0,1,0),  // u_vec
                                                 m_quat,   // camera poses
                                                 m_ephem); // camera positions
}

// Load satellite telemetry directly using SPICE.
void MOCImageMetadata::read_spice_data() {
  m_t0_ephem = -1.25;   m_dt_ephem = 0.25;
  m_t0_quat = -1.25;    m_dt_quat = 0.25;

  // For debugging:
  //   cout << "Scan duration " << scan_duration() << "\n";
  //   cout << "Range [" << ephemeris_time + m_t0_ephem << ", " << ephemeris_time + scan_duration() - m_t0_ephem << ", " << m_dt_ephem << "] \n";

  MOC_state(ephemeris_time() + m_t0_ephem,
            ephemeris_time() + scan_duration() - m_t0_ephem,
            m_dt_ephem, m_ephem, m_ephem_rate, m_quat);
}

/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
 *              MOC DESCRIPTION.TAB FILE MANIPULATION
 *                    AND VIZ SITE FRAME I/O
 * - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */

// --------------------------------------------------------
//                Ephemeris entry data
//            (one line of description.tab
// --------------------------------------------------------
/* Byte indices for fields in the MOC description.tab file */
enum { EPHEM_IMAGE_WIDTH = 6,           // pixels
       EPHEM_IMAGE_HEIGHT = 7,          // pixels
       EPHEM_CROSSTRACK_SUMMING = 8,    // unitless
       EPHEM_DOWNTRACK_SUMMING = 9,     // unitless
       EPHEM_ASPECT_RATIO = 11,         // unitless
       EPHEM_INCIDENCE_ANGLE = 12,      // degrees
       EPHEM_LONGITUDE = 15,            // degrees
       EPHEM_LATITUDE = 16,             // degrees
       EPHEM_SCLK_TIME = 27,            // system clock ticks
       EPHEM_ACTUAL_WIDTH = 34,         // kilometers
       EPHEM_ACTUAL_HEIGHT = 35,        // kilometers
       EPHEM_IMAGE_FLIPPED = 39,        // 'Y' or 'N'
       EPHEM_NORTH_ANGLE = 40,          // degrees
       EPHEM_SKEW_ANGLE = 49,           // degrees
       EPHEM_LINE_INTEGRATION_TIME = 29,// seconds
       EPHEM_ORBIT_NUMBER = 52          // orbits
};

void MOCImageMetadata::read_tabulated_description(std::string const& filename) {
  // Read in the information found in the description.tab file.
  try {
    TabulatedDataReader ephemeris_reader(filename, ",");
    vector<string> imagetab;
    cout << "\tFinding entries for " << m_moc_identifier << ".\n";
    if (!ephemeris_reader.find_line_with_text(m_moc_identifier, imagetab)) {
      cout << "Error: could not find an entry that matched " << m_moc_identifier << " in the description file " << filename << "\nExiting.\n\n";
      exit(0);
    }
    parse_ephemeris_entry(imagetab);
    ephemeris_reader.close();
  } catch (IOErr &e) {
    cout << "Failed to open the description file: " << filename << "\n\t";
    cout << e.what() << "\n";
    exit(0);
  }
}

void MOCImageMetadata::parse_ephemeris_entry(std::vector<std::string> ephemerisData)
{
  sscanf(ephemerisData[EPHEM_LONGITUDE].c_str(), "%lf", &m_longitude);          // Image center longitude
  sscanf(ephemerisData[EPHEM_LATITUDE].c_str(), "%lf", &m_latitude);              // Image center latitude
  sscanf(ephemerisData[EPHEM_IMAGE_WIDTH].c_str(), "%lf", &m_width_pixels);     // Image width in pixels
  sscanf(ephemerisData[EPHEM_IMAGE_HEIGHT].c_str(), "%lf", &m_height_pixels);   // Image height in pixels
  sscanf(ephemerisData[EPHEM_CROSSTRACK_SUMMING].c_str(), "%lf", &m_crosstrack_summing);
  sscanf(ephemerisData[EPHEM_DOWNTRACK_SUMMING].c_str(), "%lf", &m_downtrack_summing);
  sscanf(ephemerisData[EPHEM_ASPECT_RATIO].c_str(), "%lf", &m_aspect);
  sscanf(ephemerisData[EPHEM_ACTUAL_WIDTH].c_str(), "%lf", &m_width_meters); // Image width in kilometers
  sscanf(ephemerisData[EPHEM_ACTUAL_HEIGHT].c_str(), "%lf", &m_height_meters); // Image height in kilometers
  sscanf(ephemerisData[EPHEM_INCIDENCE_ANGLE].c_str(), "%lf", &m_angle);           // Incidence angle in degrees
  sscanf(ephemerisData[EPHEM_NORTH_ANGLE].c_str(), "%lf", &m_north_angle);
  sscanf(ephemerisData[EPHEM_SKEW_ANGLE].c_str(), "%lf", &m_skew_angle);
  sscanf(ephemerisData[EPHEM_LINE_INTEGRATION_TIME].c_str(), "%lf", &m_line_integration_time);  // Line integration time
  sscanf(ephemerisData[EPHEM_ORBIT_NUMBER].c_str(), "%d", &m_orbit_number);

  m_sclk_time = ephemerisData[EPHEM_SCLK_TIME];
  m_sclk_time.erase(0, 1);
  m_sclk_time.erase(m_sclk_time.size() - 1, 1);
  m_flip = (*(ephemerisData[EPHEM_IMAGE_FLIPPED].c_str()) == 'F');

  m_worldToLocal = GenWorldToLocalTransform(m_latitude, m_longitude, kMarsRadius);
  m_aspect = 1.0 / m_aspect;
  m_width_meters = m_width_meters * 1000.0;        // Image width in meters
  m_height_meters = m_height_meters * 1000.0;      // Image height in meters
  m_angle = m_angle * M_PI / 180;          // Incidence angle in radians
  m_north_angle = m_north_angle * M_PI / 180;
  m_skew_angle = m_skew_angle * M_PI / 180;
  m_line_integration_time /= 1000.0;              // Convert from ms to secs
}

// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
//                  MOC SUP FILE MANIPULATION
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
//
// Read a MOC *.sup data file provided by Malin Space Science Systems
// This file is used to initialize a pushbroom camera model.
void MOCImageMetadata::read_ephemeris_supplement(std::string const& filename) {

  SupplementaryEphemerisParser parser(filename);

  try {

    // First, gather the basic intrinsic camera prameters
    //
    // Note that these values may be overridden by the values in the
    // description.tab file (though they should be identical values).
    //
    m_crosstrack_summing = parser.read_double("PIXEL_SUMMING");
    m_downtrack_summing = parser.read_double("PIXEL_SUMMING");
    //    double scan_duration = parser.read_double("SCAN_DURATION");

    double downloaded_lines = parser.read_double("DOWNLOADED_LINES");
    m_height_pixels = downloaded_lines/m_downtrack_summing;

    double focal_length = parser.read_double("FOCAL_LENGTH") / 1000.0;
    if (focal_length != m_focal_length) { std::cout << "WARNING: File focal length differs from standard MOC focal length ( " << focal_length << "\n"; }

    double across_scan_pixel_size = parser.read_double("ACROSS_SCAN_PIXEL_SIZE") / 1.0e6;
    double along_scan_pixel_size = parser.read_double("ALONG_SCAN_PIXEL_SIZE") / 1.0e6;
    if (across_scan_pixel_size != m_across_scan_pixel_size) { std::cout << "WARNING: File across scan pixel size differs from standard MOC focal length.\n"; }
    if (along_scan_pixel_size != m_along_scan_pixel_size) { std::cout << "WARNING: File along scan pixel size differs from standard MOC focal length.\n"; }

    double downloaded_samples = parser.read_double("DOWNLOADED_SAMPLES");
    m_width_pixels = downloaded_samples/m_crosstrack_summing;

    // This is really the only piece of information we need from the ephemeris file.
    // (This does noet appear in the description.tab file)
    m_start_sample = parser.read_double("START_SAMPLE");

    /* Read the full ephemeris information into a Nx3 matrix*/
    double n_ephem = parser.read_double("N_EPHEM");
    m_t0_ephem = parser.read_double("T0_EPHEM");
    m_dt_ephem = parser.read_double("DT_EPHEM");
    m_ephem = parser.read_vector3s("EPHEM", (int)n_ephem, 3);
    m_ephem_rate = parser.read_vector3s("EPHEM_RATE", (int)n_ephem, 3);

    /* Next, read in the time serios of data regarding orientation */
    double n_quat = parser.read_double("NUM_QUAT");
    m_t0_quat = parser.read_double("T0_QUAT");
    m_dt_quat = parser.read_double("DT_QUAT");
    m_quat = parser.read_quaternions("QUATERNIONS", (int)n_quat, 4);

    /* For IRG generated SUP files, the quaternion already takes into account the minute
     * rotation between the MOC frame of reference and the MGS S/C frame of reference.
     *
     * However, MSSS quaternions do not (we think).
     *
     * The rotation matrix below could be used to correct the quaternions from MGS frame
     * to MOC-NA frame.
     *
     * This matrix is built from a set of Euler angles that were obtained from Malin
     * Space Science Systems.  Refer to docs/MGS_to_MOC_alignment.txt for a full explanation
     * of how this matrix is built.
     */
    //   Matrix<double> R_moc(3,3);
    //   R_moc(0,0) = 1.00000;   R_moc(0,1) = -0.0031;  R_moc(0,2) = 0.0002;
    //   R_moc(1,0) = 0.0031;    R_moc(1,1) = 1.0000;   R_moc(1,2) = 0.0012;
    //   R_moc(2,0) = -0.0002;   R_moc(2,1) = -0.0012;  R_moc(2,2) = 1.0000;

  } catch (EphemerisErr &e) {
    throw IOErr() << "An error occured while parsing the ephemeris file.\n";
  }
}

/* ----------------------------------------------------------------------
 *               VIZ Site Frame Related Methods
 * ----------------------------------------------------------------------*/
vw::Matrix<double> MOCImageMetadata::GenWorldToLocalTransform(double lat, double lon, double radius)
{
  double sinLat = sin(M_PI * lat / 180.0), cosLat = cos(M_PI * lat / 180.0);
  double xOrigin, yOrigin, zOrigin;
  vw::Vector3 localXAxis, localYAxis, localZAxis;

  // Longitude is defined positive west!
  lon = (360.0 - lon);
  // The following assumes latitude is measured from the equatorial
  // plane with north positive. This is different than normal
  // spherical coordinate conversion where the equivalent angle is
  // measured from the positive z axis.
  xOrigin = radius * cosLat * cos(M_PI * lon / 180.0);
  yOrigin = radius * cosLat * sin(M_PI * lon / 180.0);
  zOrigin = radius * sinLat;

  // Debugging:
  //   cout << "\tImage center: ("
  //        << xOrigin << "," << yOrigin << "," << zOrigin << ")" << endl;

  vw::Matrix<double> translation(4,4);
  translation.set_identity();
  translation[0][2] = xOrigin;
  translation[1][2] = yOrigin;
  translation[2][2] = zOrigin;

  vw::Matrix<double> rotation(4,4);
  rotation.set_identity();

  // Mars local level is a Z-down coordinate system
  localZAxis[0] = -xOrigin / radius;
  localZAxis[1] = -yOrigin / radius;
  localZAxis[2] = -zOrigin / radius;

  if (cosLat != 0.0)
  {
    // Z_local X Z_world  = Y_local
    localYAxis[0] =  localZAxis[1];
    localYAxis[1] = -localZAxis[0];
    localYAxis[2] =  0.0;
    localYAxis /= vw::math::norm_2(localYAxis); // Normalize

    // Y_local X Z_local = X_local
    localXAxis = vw::math::cross_prod(localYAxis, localZAxis);
    localXAxis /= vw::math::norm_2(localXAxis); // Normalize
  }
  else                                     // we're at one of the poles
  {
    if (zOrigin > 0.0)                     // north pole
      localXAxis[0] = -1.0;
    else                                   // south pole
      localXAxis[0] = 1.0;
    localXAxis[1] = 0.0; localXAxis[2] = 0.0;
    localYAxis[0] = 0.0; localYAxis[1] = 1.0; localYAxis[2] = 0.0;
  }

  // The 3x3 matrix consisting of the local level coordinate system
  // axes (in world coordinates) as rows gives the local-to-world
  // rotation:
  rotation[0][0] = localXAxis[0];
  rotation[0][1] = localXAxis[1];
  rotation[0][2] = localXAxis[2];

  rotation[1][0] = localYAxis[0];
  rotation[1][1] = localYAxis[1];
  rotation[1][2] = localYAxis[2];

  rotation[2][0] = localZAxis[0];
  rotation[2][1] = localZAxis[1];
  rotation[2][2] = localZAxis[2];

  // We do the translation followed by rotation:
  vw::Matrix<double> localToWorld = rotation * translation;
  return vw::math::inverse(localToWorld);
}

void MOCImageMetadata::write_viz_site_frame(std::string prefix)
{
  string fileName = prefix + string(".viz");
  std::ofstream vizFile(fileName.c_str());

  vw_out(0) << "MOCImage::WriteVizSiteFrame(): writing viz file..."
            << std::flush;

  vizFile << "#Inventor V2.1 ascii" << std::endl << std::endl;

  vizFile << "DEF +parent1 SoVizGroup" << std::endl;
  vizFile << "{" << std::endl;
  vizFile << "    fields [ SFEnum renderCaching, SFEnum boundingBoxCaching,"
          << std::endl;
  vizFile << "             SFEnum renderCulling, SFEnum pickCulling,"
          << std::endl;
  vizFile << "       SFString title, SFNode parent, SFNode transform,"
    " SFNode render" << std::endl;
  vizFile << "       ]" << std::endl;
  vizFile << "    renderCaching OFF" << std::endl;
  vizFile << "    boundingBoxCaching    OFF" << std::endl;
  vizFile << "    renderCulling ON" << std::endl;
  vizFile << "    pickCulling           ON" << std::endl;
  vizFile << "    title         \"" << m_moc_identifier << "\"" << std::endl;
  vizFile << "    parent                NULL" << std::endl;
  vizFile << "    transform DEF +transform1 MatrixTransform" << std::endl;
  vizFile << "                          {" << std::endl;
  vizFile << "                                  matrix" << std::endl;
  vizFile << "                                  "
          << m_worldToLocal[0][0] << " " << m_worldToLocal[0][1] << " "
          << m_worldToLocal[0][2] << " " << m_worldToLocal[0][3] << std::endl;
  vizFile << "                                  "
          << m_worldToLocal[1][0] << " " << m_worldToLocal[1][1] << " "
          << m_worldToLocal[1][2] << " " << m_worldToLocal[1][3] << std::endl;
  vizFile << "                                  "
          << m_worldToLocal[2][0] << " " << m_worldToLocal[2][1] << " "
          << m_worldToLocal[2][2] << " " << m_worldToLocal[2][3] << std::endl;
  vizFile << "                                  "
          << m_worldToLocal[3][0] << " " << m_worldToLocal[3][1] << " "
          << m_worldToLocal[3][2] << " " << m_worldToLocal[3][3] << std::endl;
  vizFile << "                          }" << std::endl;
  vizFile << "    render DEF +children1 Group" << std::endl;
  vizFile << "    {" << std::endl;
  vizFile << "    }" << std::endl;
  vizFile << "    USE +transform1" << std::endl;
  vizFile << "    USE +children1" << std::endl;
  vizFile << "}" << std::endl;

  vizFile.close();

  vw_out(0) << " done." << std::endl << std::endl;
}

ext: cc

max_stars_repo_name: AndrewAnnex/StereoPipeline

max_issues_repo_name: AndrewAnnex/StereoPipeline

max_forks_repo_name: AndrewAnnex/StereoPipeline


text:
/***********************************************************************************************************************
*  OpenStudio(R), Copyright (c) 2008-2018, Alliance for Sustainable Energy, LLC. All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
*  following conditions are met:
*
*  (1) Redistributions of source code must retain the above copyright notice, this list of conditions and the following
*  disclaimer.
*
*  (2) Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
*  disclaimer in the documentation and/or other materials provided with the distribution.
*
*  (3) Neither the name of the copyright holder nor the names of any contributors may be used to endorse or promote products
*  derived from this software without specific prior written permission from the respective party.
*
*  (4) Other than as required in clauses (1) and (2), distributions in any form of modifications or other derivative works
*  may not use the "OpenStudio" trademark, "OS", "os", or any other confusingly similar designation without specific prior
*  written permission from Alliance for Sustainable Energy, LLC.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDER(S) AND ANY CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
*  INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER(S), ANY CONTRIBUTORS, THE UNITED STATES GOVERNMENT, OR THE UNITED
*  STATES DEPARTMENT OF ENERGY, NOR ANY OF THEIR EMPLOYEES, BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
*  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
*  USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
*  STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
*  ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
***********************************************************************************************************************/

#include <algorithm>
#include <gtest/gtest.h>

#include <string>
#include <boost/optional.hpp>

#include "ModelFixture.hpp"

#include "../Model.hpp"
#include "../Model_Impl.hpp"

#include "../AdditionalProperties.hpp"
#include "../AdditionalProperties_Impl.hpp"
#include "../Construction.hpp"
#include "../Construction_Impl.hpp"
#include "../StandardOpaqueMaterial.hpp"
#include "../StandardOpaqueMaterial_Impl.hpp"
#include "../Lights.hpp"
#include "../LightsDefinition.hpp"

#include <utilities/idd/OS_AdditionalProperties_FieldEnums.hxx>

#include "../utilities/geometry/Geometry.hpp"
#include "../../utilities/units/Quantity.hpp"
#include "../../utilities/units/Unit.hpp"

using namespace openstudio;
using namespace openstudio::model;

TEST_F(ModelFixture, AdditionalProperties_AdditionalProperties) {
  ::testing::FLAGS_gtest_death_test_style = "threadsafe";

  ASSERT_EXIT(
  {
    // create a model to use
    Model model;

    // create a material object to use
    StandardOpaqueMaterial material(model);

    exit(0);
  },
    ::testing::ExitedWithCode(0),
    ""
    );

  // create a model to use
  Model model;

  // create a material object to use
  StandardOpaqueMaterial material(model);
  EXPECT_EQ(1, model.modelObjects().size());

  // create an additional properties object to use
  AdditionalProperties props = material.additionalProperties();
  EXPECT_EQ(2, model.modelObjects().size());
  EXPECT_EQ(1, model.getModelObjects<AdditionalProperties>().size());

  // return, instead of create, additional properies when it already exists
  AdditionalProperties props2 = material.additionalProperties();
  EXPECT_EQ(2, model.modelObjects().size());
  EXPECT_EQ(1, model.getModelObjects<AdditionalProperties>().size());
  EXPECT_EQ(props, props2);

}

// test setting and getting
TEST_F(ModelFixture, AdditionalProperties_Features) {

  Model model;
  Construction construction(model);
  AdditionalProperties props = construction.additionalProperties();

  // Test initially setting a string value
  EXPECT_TRUE(props.setFeature("NumberOfBedrooms", "five"));
  boost::optional<std::string> dataType = props.getFeatureDataType("NumberOfBedrooms");
  ASSERT_TRUE(dataType);
  EXPECT_EQ(*dataType, "String");
  boost::optional<std::string> strValue = props.getFeatureAsString("NumberOfBedrooms");
  ASSERT_TRUE(strValue);
  EXPECT_EQ(*strValue, "five");

  // Test changing a string value
  EXPECT_TRUE(props.setFeature("NumberOfBedrooms", "six"));
  dataType = props.getFeatureDataType("NumberOfBedrooms");
  ASSERT_TRUE(dataType);
  EXPECT_EQ(*dataType, "String");
  strValue = props.getFeatureAsString("NumberOfBedrooms");
  ASSERT_TRUE(strValue);
  EXPECT_EQ(*strValue, "six");

  // Test setting a boolean value
  EXPECT_TRUE(props.setFeature("isNicePlace", false));
  dataType = props.getFeatureDataType("isNicePlace");
  ASSERT_TRUE(dataType);
  EXPECT_EQ(*dataType, "Boolean");
  boost::optional<bool> boolValue = props.getFeatureAsBoolean("isNicePlace");
  ASSERT_TRUE(boolValue);
  ASSERT_FALSE(*boolValue);

  // Test changing a boolean value
  EXPECT_TRUE(props.setFeature("isNicePlace", true));
  dataType = props.getFeatureDataType("isNicePlace");
  ASSERT_TRUE(dataType);
  EXPECT_EQ(*dataType, "Boolean");
  boolValue = props.getFeatureAsBoolean("isNicePlace");
  ASSERT_TRUE(boolValue);
  ASSERT_TRUE(*boolValue);

  // Test Feature Name getter
  const std::vector<std::string> featureNames(props.featureNames());
  EXPECT_EQ(featureNames.size(), 2);
  EXPECT_EQ(featureNames[0], "NumberOfBedrooms");
  EXPECT_EQ(featureNames[1], "isNicePlace");

  // Test setting a double value
  EXPECT_TRUE(props.setFeature("NumberOfBathrooms", 1.5));
  dataType = props.getFeatureDataType("NumberOfBathrooms");
  ASSERT_TRUE(dataType);
  EXPECT_EQ(*dataType, "Double");
  boost::optional<double> dblValue = props.getFeatureAsDouble("NumberOfBathrooms");
  ASSERT_TRUE(dblValue);
  boost::optional<int> intValue = props.getFeatureAsInteger("NumberOfBathrooms");
  ASSERT_FALSE(intValue);
  ASSERT_FLOAT_EQ(*dblValue, 1.5);

  // Test setting a feature to a value of a different type
  EXPECT_TRUE(props.setFeature("NumberOfBedrooms", 3));
  dataType = props.getFeatureDataType("NumberOfBedrooms");
  ASSERT_TRUE(dataType);
  EXPECT_EQ(*dataType, "Integer");
  dblValue = props.getFeatureAsDouble("NumberOfBedrooms");
  ASSERT_FALSE(dblValue);
  intValue = props.getFeatureAsInteger("NumberOfBedrooms");
  ASSERT_TRUE(intValue);
  ASSERT_EQ(*intValue, 3);

  // Test resetting a feature
  EXPECT_TRUE(props.resetFeature("isNicePlace"));
  dataType = props.getFeatureDataType("isNicePlace");
  ASSERT_FALSE(dataType);
  EXPECT_FALSE(props.resetFeature("bogusName"));

  // Test suggested feature names
  EXPECT_TRUE(props.setFeature("MyUniqueFeature", 5));
  Construction construction2(model);
  AdditionalProperties props2 = construction2.additionalProperties();
  std::vector<std::string> suggestedFeatureNames(props2.suggestedFeatureNames());
  ASSERT_EQ(suggestedFeatureNames.size(), 3);
  ASSERT_NE(std::find(suggestedFeatureNames.begin(), suggestedFeatureNames.end(), "MyUniqueFeature"), suggestedFeatureNames.end());

}

// check returning model object pointed to
TEST_F(ModelFixture, AdditionalProperties_ModelObject) {
  Model model;
  StandardOpaqueMaterial material(model);
  EXPECT_EQ(0, model.getConcreteModelObjects<AdditionalProperties>().size());
  AdditionalProperties props = material.additionalProperties();
  EXPECT_EQ(1u, model.getConcreteModelObjects<AdditionalProperties>().size());
  AdditionalProperties props2 = material.additionalProperties();
  EXPECT_EQ(1u, model.getConcreteModelObjects<AdditionalProperties>().size());
  EXPECT_EQ(props.handle(), props2.handle());

  ModelObject modelObject = props.modelObject();
  StandardOpaqueMaterial material2 = modelObject.cast<StandardOpaqueMaterial>();
  EXPECT_EQ(material, material2);
}

// check that remove works on ParentObject and ModelObject derived classes
TEST_F(ModelFixture, AdditionalProperties_NonParentRemove) {
  Model model;
  auto size = model.modelObjects().size();
  LightsDefinition def(model);
  AdditionalProperties defProps = def.additionalProperties();
  Lights lights1(def);
  AdditionalProperties props1 = lights1.additionalProperties();
  Lights lights2(def);
  AdditionalProperties props2 = lights2.additionalProperties();
  EXPECT_EQ(2u, lights2.remove().size());
  EXPECT_FALSE(defProps.handle().isNull());
  EXPECT_FALSE(props1.handle().isNull());
  EXPECT_TRUE(props2.handle().isNull());

  EXPECT_EQ(4u, def.remove().size());
  EXPECT_TRUE(defProps.handle().isNull());
  EXPECT_TRUE(props1.handle().isNull());
  EXPECT_TRUE(props2.handle().isNull());

  EXPECT_EQ(size, model.modelObjects().size()) << model;
}

// check that remove works on the object
TEST_F(ModelFixture, AdditionalProperties_ParentRemove) {
  Model model;
  EXPECT_EQ(0u, model.getConcreteModelObjects<AdditionalProperties>().size());
  auto size = model.modelObjects().size();
  StandardOpaqueMaterial material(model);
  AdditionalProperties props = material.additionalProperties();
  EXPECT_EQ(1u, model.getConcreteModelObjects<AdditionalProperties>().size());
  EXPECT_EQ(2u, material.remove().size());
  EXPECT_EQ(size, model.modelObjects().size());
  EXPECT_EQ(0u, model.getConcreteModelObjects<AdditionalProperties>().size());
}

// check that remove works on the props
TEST_F(ModelFixture, AdditionalProperties_ChildRemove) {
  Model model;
  EXPECT_EQ(0u, model.getConcreteModelObjects<AdditionalProperties>().size());
  StandardOpaqueMaterial material(model);
  auto size = model.modelObjects().size();
  AdditionalProperties props = material.additionalProperties();
  EXPECT_EQ(1u, model.getConcreteModelObjects<AdditionalProperties>().size());
  EXPECT_FALSE(props.remove().empty());
  EXPECT_EQ(size, model.modelObjects().size());
  EXPECT_EQ(0u, model.getConcreteModelObjects<AdditionalProperties>().size());
}

// test that parent clone works
TEST_F(ModelFixture, AdditionalProperties_ParentClone) {
  Model model;
  EXPECT_EQ(0u, model.getConcreteModelObjects<AdditionalProperties>().size());
  StandardOpaqueMaterial material(model);
  AdditionalProperties props = material.additionalProperties();
  EXPECT_EQ(1u, model.getConcreteModelObjects<AdditionalProperties>().size());
  EXPECT_EQ(2, model.modelObjects().size());
  StandardOpaqueMaterial materialClone = material.clone(model).cast<StandardOpaqueMaterial>();
  EXPECT_EQ(4, model.modelObjects().size());
  EXPECT_EQ(2u, model.getConcreteModelObjects<AdditionalProperties>().size());
  EXPECT_NE(material.additionalProperties().handle(), materialClone.additionalProperties().handle());
  EXPECT_EQ(4, model.modelObjects().size());
  EXPECT_EQ(2u, model.getConcreteModelObjects<AdditionalProperties>().size());

  Model model2;
  StandardOpaqueMaterial materialClone2 = material.clone(model2).cast<StandardOpaqueMaterial>();
  EXPECT_EQ(2, model2.modelObjects().size());
  EXPECT_EQ(1u, model2.getConcreteModelObjects<AdditionalProperties>().size());
  EXPECT_NE(material.additionalProperties().handle(), materialClone2.additionalProperties().handle());
  EXPECT_EQ(2, model2.modelObjects().size());
  EXPECT_EQ(1u, model2.getConcreteModelObjects<AdditionalProperties>().size());
}

// test that child clone does not work
TEST_F(ModelFixture, AdditionalProperties_ChildClone) {
  Model model;
  EXPECT_EQ(0u, model.getConcreteModelObjects<AdditionalProperties>().size());
  StandardOpaqueMaterial material(model);
  AdditionalProperties props = material.additionalProperties();
  EXPECT_EQ(1u, model.getConcreteModelObjects<AdditionalProperties>().size());
  EXPECT_EQ(2, model.modelObjects().size());

  EXPECT_EQ(0, props.resources().size());

  // DLM: do we want to allow this clone?
  AdditionalProperties propsClone = props.clone(model).cast<AdditionalProperties>();
  EXPECT_EQ(3, model.modelObjects().size());
  EXPECT_EQ(2u, model.getConcreteModelObjects<AdditionalProperties>().size());
  EXPECT_EQ(props.modelObject().handle(), propsClone.modelObject().handle());

  Model model2;

  // DLM: do we want to allow this clone?
  AdditionalProperties propsClone2 = props.clone(model2).cast<AdditionalProperties>();
  EXPECT_EQ(1u, model2.modelObjects().size());
  EXPECT_EQ(1u, model2.getConcreteModelObjects<AdditionalProperties>().size());

  // DLM: no way to clean up this invalid object?
  EXPECT_TRUE(propsClone2.isEmpty(OS_AdditionalPropertiesFields::ObjectName));
  EXPECT_THROW(propsClone2.modelObject(), openstudio::Exception);

  // go back to first model

  // calling additionalProperties again will remove one of the props,
  // no telling which one, so do this test last
  material.additionalProperties();
  EXPECT_EQ(1u, model.getConcreteModelObjects<AdditionalProperties>().size());
}

// check that can't add properties to a properties
TEST_F(ModelFixture, AdditionalProperties_AdditionalProperties2) {
  Model model;
  EXPECT_EQ(0u, model.getConcreteModelObjects<AdditionalProperties>().size());
  auto size = model.modelObjects().size();
  StandardOpaqueMaterial material(model);
  AdditionalProperties props = material.additionalProperties();
  EXPECT_EQ(1u, model.getConcreteModelObjects<AdditionalProperties>().size());
  EXPECT_THROW(props.additionalProperties(), openstudio::Exception);
  EXPECT_EQ(1u, model.getConcreteModelObjects<AdditionalProperties>().size());
}

// test that setters work
TEST_F(ModelFixture, AdditionalProperties_Setters) {
  Model model;
  EXPECT_EQ(0u, model.getConcreteModelObjects<AdditionalProperties>().size());
  StandardOpaqueMaterial material(model);
  AdditionalProperties props = material.additionalProperties();
  EXPECT_EQ(1u, model.getConcreteModelObjects<AdditionalProperties>().size());

  EXPECT_EQ(0, props.featureNames().size());
  EXPECT_FALSE(props.getFeatureDataType("Feature"));

  EXPECT_TRUE(props.setFeature("Feature", 1.0));
  ASSERT_EQ(1u, props.featureNames().size());
  EXPECT_EQ("Feature", props.featureNames()[0]);
  ASSERT_TRUE(props.getFeatureDataType("Feature"));
  EXPECT_EQ("Double", props.getFeatureDataType("Feature").get());
  ASSERT_TRUE(props.getFeatureAsDouble("Feature"));
  EXPECT_EQ(1.0, props.getFeatureAsDouble("Feature").get());

  EXPECT_TRUE(props.setFeature("Feature", 2.0));
  ASSERT_EQ(1u, props.featureNames().size());
  EXPECT_EQ("Feature", props.featureNames()[0]);
  ASSERT_TRUE(props.getFeatureDataType("Feature"));
  EXPECT_EQ("Double", props.getFeatureDataType("Feature").get());
  ASSERT_TRUE(props.getFeatureAsDouble("Feature"));
  EXPECT_EQ(2.0, props.getFeatureAsDouble("Feature").get());

  EXPECT_TRUE(props.setFeature("Feature2", -2.0));
  ASSERT_EQ(2u, props.featureNames().size());
  EXPECT_EQ("Feature", props.featureNames()[0]);
  EXPECT_EQ("Feature2", props.featureNames()[1]);
  ASSERT_TRUE(props.getFeatureDataType("Feature2"));
  EXPECT_EQ("Double", props.getFeatureDataType("Feature2").get());
  ASSERT_TRUE(props.getFeatureAsDouble("Feature2"));
  EXPECT_EQ(-2.0, props.getFeatureAsDouble("Feature2").get());

  EXPECT_TRUE(props.setFeature("Feature", "Yay"));
  ASSERT_EQ(2u, props.featureNames().size());
  EXPECT_EQ("Feature", props.featureNames()[0]);
  EXPECT_EQ("Feature2", props.featureNames()[1]);
  ASSERT_TRUE(props.getFeatureDataType("Feature"));
  EXPECT_EQ("String", props.getFeatureDataType("Feature").get());
  ASSERT_TRUE(props.getFeatureAsString("Feature"));
  EXPECT_EQ("Yay", props.getFeatureAsString("Feature").get());

  EXPECT_TRUE(props.resetFeature("Feature"));
  ASSERT_EQ(1u, props.featureNames().size());
  EXPECT_EQ("Feature2", props.featureNames()[0]);
  ASSERT_TRUE(props.getFeatureDataType("Feature2"));
  EXPECT_EQ("Double", props.getFeatureDataType("Feature2").get());
  ASSERT_TRUE(props.getFeatureAsDouble("Feature2"));
  EXPECT_EQ(-2.0, props.getFeatureAsDouble("Feature2").get());

}

ext: cpp


text:
/**
 * IMPLEMENTATION NOTES
 *
 * Parsing an incomplete block with the sizeinator may go off the end of the
 * block. However, any reasonable block size <16k will simply be allocated by
 * the memory manager, so we won't go off the page, and we'll detect the size
 * mismatch at the next item or at the end.
 *
 * Valgrind may complain though.
 */
#include "serializing.h"

#include "leaf_node.h"
#include "internal_node.h"
#include "overflow_node.h"

#include <cmath>
#include <boost/lexical_cast.hpp>

#define to_string boost::lexical_cast<std::string>

#define VALIDATE_OFFSET \
    if (m_offset >= m_input.size()) \
        throw std::runtime_error((std::string("End of block while parsing node data: ") + to_string(m_offset) + " >= " + to_string(m_input.size())).c_str())

namespace libbruce {

struct NodeParser
{
    NodeParser(const mempage &input, const tree_functions &fns)
        : m_input(input), fns(fns), m_offset(sizeof(flags_t) + sizeof(keycount_t)) { }

    leafnode_ptr parseLeafNode()
    {
        keycount_t count = keyCount();

        std::vector<kv_pair> items;
        items.reserve(count);

        // Read N keys
        for (keycount_t i = 0; i < count; i++)
        {
            VALIDATE_OFFSET;
            uint32_t size = fns.keySize(m_input.at<const char>(m_offset));

            // Push back
            items.push_back(kv_pair(m_input.slice(m_offset, size), memslice()));

            m_offset += size;
        }

        // Read N values
        for (keycount_t i = 0; i < count; i++)
        {
            VALIDATE_OFFSET;
            uint32_t size = fns.valueSize(m_input.at<const char>(m_offset));

            // Couple value to key
            items[i].second = m_input.slice(m_offset, size);

            m_offset += size;
        }

        leafnode_ptr ret = boost::make_shared<LeafNode>(&items, fns);

        VALIDATE_OFFSET;
        ret->overflow.count = *m_input.at<itemcount_t>(m_offset);
        m_offset += sizeof(itemcount_t);

        VALIDATE_OFFSET;
        ret->overflow.nodeID = *m_input.at<nodeid_t>(m_offset);
        m_offset += sizeof(nodeid_t);

        validateAtEnd();

        return ret;
    }

    overflownode_ptr parseOverflowNode()
    {
        overflownode_ptr ret = boost::make_shared<OverflowNode>(keyCount());

        // Read N values
        for (keycount_t i = 0; i < keyCount(); i++)
        {
            VALIDATE_OFFSET;
            uint32_t size = fns.valueSize(m_input.at<const char>(m_offset));

            // Couple value to key
            ret->values.push_back(m_input.slice(m_offset, size));

            m_offset += size;
        }

        VALIDATE_OFFSET;
        ret->next.count = *m_input.at<itemcount_t>(m_offset);
        m_offset += sizeof(itemcount_t);

        VALIDATE_OFFSET;
        ret->next.nodeID = *m_input.at<nodeid_t>(m_offset);
        m_offset += sizeof(nodeid_t);

        validateAtEnd();

        return ret;
    }

    internalnode_ptr parseInternalNode()
    {
        internalnode_ptr ret = boost::make_shared<InternalNode>(keyCount());

        keycount_t editCount = *m_input.at<keycount_t>(m_offset);
        m_offset += sizeof(keycount_t);

        //------------------------------------------------
        //  Read branches

        // Read N-1 keys, starting at 1
        ret->branches.push_back(node_branch(memslice(), nodeid_t(), 0));
        for (keycount_t i = 1; i < keyCount(); i++)
        {
            VALIDATE_OFFSET;
            uint32_t size = fns.keySize(m_input.at<const char>(m_offset));

            ret->branches.push_back(node_branch(m_input.slice(m_offset, size), nodeid_t(), 0));

            m_offset += size;
        }

        // Read N node identifiers
        for (keycount_t i = 0; i < keyCount(); i++)
        {
            VALIDATE_OFFSET;
            uint32_t size = sizeof(nodeid_t);

            ret->branches[i].nodeID = *m_input.at<nodeid_t>(m_offset);

            m_offset += size;
        }

        // Read N item counts
        for (keycount_t i = 0; i < keyCount(); i++)
        {
            VALIDATE_OFFSET;
            uint32_t size = sizeof(itemcount_t);

            ret->branches[i].itemCount = *m_input.at<itemcount_t>(m_offset);

            m_offset += size;
        }

        //------------------------------------------------
        //  Read pending edits
        std::vector<edit_t> editTypes;
        std::vector<memslice> editKeys;
        editTypes.reserve(editCount);
        editKeys.reserve(editCount);

        // Edit types
        for (keycount_t i = 0; i < editCount; i++)
        {
            VALIDATE_OFFSET;
            editTypes.push_back((edit_t)*m_input.at<uint8_t>(m_offset));
            m_offset += sizeof(uint8_t);
        }

        // Keys
        for (keycount_t i = 0; i < editCount; i++)
        {
            VALIDATE_OFFSET;

            uint32_t size = fns.keySize(m_input.at<const char>(m_offset));
            editKeys.push_back(m_input.slice(m_offset, size));

            m_offset += size;
        }

        // Values
        for (keycount_t i = 0; i < editCount; i++)
        {
            memslice value;

            if (editTypes[i] != REMOVE_KEY) // Doesn't have a value
            {
                VALIDATE_OFFSET;

                uint32_t size = fns.valueSize(m_input.at<const char>(m_offset));
                value = m_input.slice(m_offset, size);
            }

            ret->editQueue.push_back(pending_edit(editTypes[i], editKeys[i], value, false));

            m_offset += value.size();
        }

        validateAtEnd();

        return ret;
    }

private:
    keycount_t keyCount()
    {
        return *m_input.at<keycount_t>(sizeof(flags_t));
    }

    void validateAtEnd()
    {
        if (m_offset != m_input.size())
            throw std::runtime_error((std::string("Trailing bytes found: ") +
                                     boost::lexical_cast<std::string>(m_input.size()
                                                                      -
                                                                      m_offset)
                                     + " bytes left").c_str());
    }

    size_t m_offset;
    const mempage &m_input;
    const tree_functions &fns;
};

//----------------------------------------------------------------------

node_ptr ParseNode(const mempage &input, const tree_functions &fns)
{
    NodeParser parser(input, fns);

    switch (*input.at<flags_t>(0))
    {
        case TYPE_INTERNAL:
            return parser.parseInternalNode();
        case TYPE_LEAF:
            return parser.parseLeafNode();
        case TYPE_OVERFLOW:
            return parser.parseOverflowNode();
    }

    throw std::runtime_error("Unknown node type");
}

//----------------------------------------------------------------------

NodeSize::NodeSize(uint32_t blockSize)
    : m_blockSize(blockSize), m_size(sizeof(flags_t) + sizeof(keycount_t))
{
}

bool NodeSize::shouldSplit()
{
    return m_blockSize && m_blockSize < m_size;
}

//----------------------------------------------------------------------

LeafNodeSize::LeafNodeSize(const leafnode_ptr &node, uint32_t blockSize)
    : NodeSize(blockSize)
{
    m_size += sizeof(itemcount_t) + sizeof(nodeid_t);  // For the chained overflow block
    uint32_t splitSize = m_size; // Header

    m_size += node->elementsSize();

    if (shouldSplit() && !node->pairs.empty())
    {
        uint32_t pieceSize = std::ceil(m_blockSize / 2.0);

        pairlist_t::const_iterator here;
        pairlist_t::const_iterator startOfThisKey = node->pairs.begin();

        for (pairlist_t::const_iterator it = node->pairs.begin(); it != node->pairs.end(); ++it)
        {
            if (!(it->first == startOfThisKey->first))
                startOfThisKey = it;

            splitSize += it->first.size() + it->second.size();
            if (splitSize > pieceSize)
            {
                here = it;
                break;
            }
        }

        // 'here' is in the middle of the highest key range that should still be included.
        //
        // [K][K............K)[L]
        //    |     |         |
        //    |    here      splitStart
        //   overflowstart

        // Move the split index forwards while we're on the same key
        m_splitStart = here;
        while (m_splitStart != node->pairs.end() && m_splitStart->first == here->first)
            ++m_splitStart;

        // Move the overflow start back while there is still a key before it that is the same
        m_overflowStart = startOfThisKey;
        ++m_overflowStart;
    }
}

//----------------------------------------------------------------------

OverflowNodeSize::OverflowNodeSize(const overflownode_ptr &node, uint32_t blockSize)
    : NodeSize(blockSize)
{
    m_size += sizeof(itemcount_t) + sizeof(nodeid_t);  // For the chained overflow block
    uint32_t baseSize = m_size;

    for (valuelist_t::const_iterator it = node->values.begin(); it != node->values.end(); ++it)
    {
        m_size += it->size();
    }

    if (shouldSplit())
    {
        uint32_t pieceSize = m_blockSize;
        uint32_t splitSize = baseSize;

        // Find the split index
        for (m_splitIndex = 0; m_splitIndex < node->valueCount(); m_splitIndex++)
        {
            splitSize += node->values[m_splitIndex].size();
            if (splitSize > pieceSize)
                break;
        }
    }
}

InternalNodeSize::InternalNodeSize(const internalnode_ptr &node, uint32_t blockSize, uint32_t maxEditQueueSize)
    : NodeSize(blockSize), m_maxEditQueueSize(maxEditQueueSize), m_editQueueSize(0)
{
    m_blockSize = blockSize - maxEditQueueSize; // The "block size" excludes the queue area

    m_size += sizeof(keycount_t); // Size of edit queue

    for (branchlist_t::const_iterator it = node->branches.begin(); it != node->branches.end(); ++it)
    {
        // We never store the first key
        if (it != node->branches.begin())
            m_size += it->minKey.size();
        m_size += sizeof(nodeid_t) + sizeof(itemcount_t);
    }

    for (editlist_t::const_iterator it = node->editQueue.begin(); it != node->editQueue.end(); ++it)
    {
        m_editQueueSize += sizeof(uint8_t); // 1 byte for the edit type
        m_editQueueSize += it->key.size() + it->value.size();
    }

    if (shouldSplit())
    {
        uint32_t splitSize = m_size;
        uint32_t pieceSize = std::ceil(m_size / 2.0);

        for (m_splitIndex = 1; m_splitIndex < node->branchCount(); m_splitIndex++)
        {
            if (m_splitIndex != 1) splitSize += node->branch(m_splitIndex-1).minKey.size();
            splitSize += sizeof(nodeid_t) + sizeof(itemcount_t);

            if (splitSize > pieceSize)
                return;
        }
    }
}

bool InternalNodeSize::shouldApplyEditQueue() const
{
    return m_editQueueSize > m_maxEditQueueSize;
}

//----------------------------------------------------------------------

mempage SerializeLeafNode(const leafnode_ptr &node)
{
    LeafNodeSize size(node, 0);
    mempage mem(size.size());

    uint32_t offset = 0;

    // Flags
    *mem.at<flags_t>(offset) = node->nodeType();
    offset += sizeof(flags_t);

    // Count
    *mem.at<keycount_t>(offset) = node->pairCount();
    offset += sizeof(keycount_t);

    // Keys
    for (pairlist_t::const_iterator it = node->pairs.begin(); it != node->pairs.end(); ++it)
    {
        memcpy(mem.at<char>(offset), it->first.ptr(), it->first.size());
        offset += it->first.size();
    }

    // Values
    for (pairlist_t::const_iterator it = node->pairs.begin(); it != node->pairs.end(); ++it)
    {
        memcpy(mem.at<char>(offset), it->second.ptr(), it->second.size());
        offset += it->second.size();
    }

    // Overflow block
    *mem.at<itemcount_t>(offset) = node->overflow.count;
    offset += sizeof(itemcount_t);

    *mem.at<nodeid_t>(offset) = node->overflow.nodeID;
    offset += sizeof(nodeid_t);

    return mem;
}

mempage SerializeOverflowNode(const overflownode_ptr &node)
{
    OverflowNodeSize size(node, 0);
    mempage mem(size.size());

    uint32_t offset = 0;

    // Flags
    *mem.at<flags_t>(offset) = node->nodeType();
    offset += sizeof(flags_t);

    // Count
    *mem.at<keycount_t>(offset) = node->valueCount();
    offset += sizeof(keycount_t);

    // Values
    for (valuelist_t::const_iterator it = node->values.begin(); it != node->values.end(); ++it)
    {
        memcpy(mem.at<char>(offset), it->ptr(), it->size());
        offset += it->size();
    }

    // Next overflow block
    *mem.at<itemcount_t>(offset) = node->next.count;
    offset += sizeof(itemcount_t);

    *mem.at<nodeid_t>(offset) = node->next.nodeID;
    offset += sizeof(nodeid_t);

    return mem;
}

mempage SerializeInternalNode(const internalnode_ptr &node)
{
    InternalNodeSize size(node, 0, 0);
    mempage mem(size.size() + size.editQueueSize());

    uint32_t offset = 0;

    // Flags
    *mem.at<flags_t>(offset) = node->nodeType();
    offset += sizeof(flags_t);

    // Count
    *mem.at<keycount_t>(offset) = node->branchCount();
    offset += sizeof(keycount_t);

    // Edit queue count
    *mem.at<keycount_t>(offset) = node->editQueue.size();
    offset += sizeof(keycount_t);

    // Keys (except the 1st one)
    bool first = true;
    for (branchlist_t::const_iterator it = node->branches.begin(); it != node->branches.end(); ++it)
    {
        if (!first)
        {
            memcpy(mem.at<char>(offset), it->minKey.ptr(), it->minKey.size());
            offset += it->minKey.size();
        }
        first = false;
    }

    // IDs
    for (branchlist_t::const_iterator it = node->branches.begin(); it != node->branches.end(); ++it)
    {
        *mem.at<nodeid_t>(offset) = it->nodeID;
        offset += sizeof(nodeid_t);
    }

    // Item counts
    for (branchlist_t::const_iterator it = node->branches.begin(); it != node->branches.end(); ++it)
    {
        *mem.at<itemcount_t>(offset) = it->itemCount;
        offset += sizeof(itemcount_t);
    }

    // Edit types
    for (editlist_t::const_iterator it = node->editQueue.begin(); it != node->editQueue.end(); ++it)
    {
        *mem.at<uint8_t>(offset) = it->edit;
        offset += sizeof(uint8_t);
    }

    // Edit keys
    for (editlist_t::const_iterator it = node->editQueue.begin(); it != node->editQueue.end(); ++it)
    {
        memcpy(mem.at<char>(offset), it->key.ptr(), it->key.size());
        offset += it->key.size();
    }

    // Edit values
    for (editlist_t::const_iterator it = node->editQueue.begin(); it != node->editQueue.end(); ++it)
    {
        if (it->edit != REMOVE_KEY) // Doesn't have a value
        {
            memcpy(mem.at<char>(offset), it->value.ptr(), it->value.size());
            offset += it->value.size();
        }
    }

    assert(offset == mem.size());

    return mem;
}

mempage SerializeNode(const node_ptr &node)
{
    switch (node->nodeType())
    {
        case TYPE_LEAF:
            return SerializeLeafNode(boost::static_pointer_cast<LeafNode>(node));
        case TYPE_INTERNAL:
            return SerializeInternalNode(boost::static_pointer_cast<InternalNode>(node));
        case TYPE_OVERFLOW:
            return SerializeOverflowNode(boost::static_pointer_cast<OverflowNode>(node));
    }
}

}


text:
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "pcl_ros/point_cloud.h"
#include <pcl/point_types.h>
#include <boost/foreach.hpp>
#include "sensor_msgs/PointCloud2.h"
#include "brushfire.cpp"
#include "math.h"
#include "nav_msgs/Path.h"
#include "geometry_msgs/PointStamped.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/Point.h"
#include "geometry_msgs/Quaternion.h"
#include <iostream>
#include "Eigen/Dense"
#include "QP.cpp"
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_listener.h>
#include "pcl/octree/octree.h"
/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */

float LF[] = {0.5,1,1.5,2,3.08,3.1,3.11,2.2,2.6,2.8,3,3.69,4.24,5.13,6.5};
ros::Subscriber sub;
ros::Publisher wayPointPub;
ros::Publisher pathPub;
nav_msgs::Path globalPath;
nav_msgs::Path localPointPath;
nav_msgs::Path localWayPath;
bool isUpdate;
MatrixXd optim_polyX;
MatrixXd optim_polyY;
MatrixXd optim_polyZ;
vector< double >VAX;
vector< double >VAY;
vector< double >VAZ;
MatrixXd time_kfBK;

using namespace Eigen;

int compareIndex(float num,vector<float>LFinter)
{
  int i = 0;
  while(true )
  {
    if(i < LFinter.size()){

      if(num <= LFinter[i]){

	return i;
      }
      else{
	i++;
      }

    }
    else{
      return LFinter.size()-1;
    }
  }
}
float att(float factor,Position one,Position two){
    return factor*((one.x-two.x)*(one.x-two.x)+(one.y-two.y)*(one.y-two.y))/2;
};
float rep(float factor,float dist){
    return factor*pow(30/dist,2)/2;
};

void globalPathProcess(const nav_msgs::Path msg)
{
    globalPath = msg;
}

void chatterCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& msg)
{
  vector< vector< vector< int > > >matries;
  vector< vector< Position > >obstalePointsList(14);
  vector <Position> lastPointList;
  vector< Position> metaData;
  printf ("Cloud: width = %d, height = %d\n", msg->width, msg->height);
  int halfMaxHeight = 0;
  int halfMaxWidth = 0;
  int maxD = 0;
  bool isSafe = false;
  nav_msgs::Path thePath;
  nav_msgs::Path theWayPath;
  vector< float > theCollision;
  vector< float > Xdist;

  for(int i = 0; i<4 ;i++){
    Xdist.push_back(LF[i]);
  }

	  float resolution = 0.1f;
	  pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree (resolution);
	  octree.setInputCloud (msg);
	  octree.addPointsFromInputCloud ();

  ros::Time begin_time = ros::Time::now();

  while(isSafe != true){

     // for(int i = 0;i<(theCollision.size());i++){
      if(theCollision.size() != 0){
	while((theCollision[0]-0.1)<Xdist[Xdist.size()-1]){
	  Xdist.pop_back();
	}
	Xdist.push_back(theCollision[0]-0.1);
      }
     // }
      theCollision.clear();
    //  for(int i =0 ;i <Xdist.size();i++){
	//printf("%f\n",Xdist[i]);
    //  }
     // sort(Xdist.begin(), Xdist.end());
    //printf("it's ok here\n");
      BOOST_FOREACH (const pcl::PointXYZ& pt, msg->points){

	//printf("%f,%f,%f\n",pt.x,pt.y,pt.z);
	if(!isnan(pt.x)&&!isnan(pt.y)&&!isnan(pt.z)){
	    int i = compareIndex(pt.x,Xdist);
	    int theZ = pt.z*10;
	    int theY = pt.y*10;
	    int z;
	    int y;
	    if(i>maxD){
	      maxD = i;
	    }
	    //printf("%d,%d",maxD,i);
	    if(z!=theZ||y!=theY){
	      z = theZ;
	      y = theY;
	      Position point;
	      point.x = z;
	      point.y = y;
	      if(abs(point.x)>halfMaxWidth){
		halfMaxWidth = abs(point.x);
	      }
	      if(abs(point.y)>halfMaxHeight){
		halfMaxHeight = abs(point.y);
	      }
	//printf ("%d,%d,%d\n",i,point.x,point.y);

	      for(int j = i;j<Xdist.size();j++){
	  //printf("test:");
	      obstalePointsList.at(j).push_back(point);
	      }

	    }
	}
    //printf("it's complete\n");
      }

    //printf ("%d,%d\n",halfMaxWidth,halfMaxHeight);
    nav_msgs::Path path;
    path.header = std_msgs::Header();
    path.header.frame_id = "robot";
    path.header.stamp = ros::Time::now();
    geometry_msgs::PoseStamped poseStamped;
    poseStamped.pose.position.x = 0;
    poseStamped.pose.position.y = 0;
    poseStamped.pose.position.z = 0;
    path.poses.push_back(poseStamped);

    vector <Position> PointList;
    static Position prevPosition = {0,0};
    for(int i = 0; i<maxD+1; i++){
      if(obstalePointsList.at(i).size()!=0){
	vector< vector<int> >theMaries(halfMaxWidth*2+1, vector<int>(halfMaxHeight*2+1, -1));
	Position x = brushfire(theMaries,obstalePointsList.at(i));
	//displayGrid(theMaries);

	  float xx = Xdist[i] - path.poses[i].pose.position.x;
	  float yy = (float)x.y/10 - path.poses[i].pose.position.y;
	  float zz = (float)x.x/10 - path.poses[i].pose.position.z;

	  pcl::PointXYZ searchPoint;
	  float radius = 0.2;
	  std::vector<int> pointIdxRadiusSearch;
	  std::vector<float> pointRadiusSquaredDistance;
	  for(float j = 0.0; j <= 1.0 ; j+=0.1){
	      searchPoint.x = path.poses[i].pose.position.x + j*xx;
	      searchPoint.y = path.poses[i].pose.position.y + j*yy;
	      searchPoint.z = path.poses[i].pose.position.z + j*zz;

	      //printf("%f,%f,%f\n",j*xx,j*yy,j*zz);
	      if (octree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
	      {
		 // printf("%f,%f,%f\n",searchPoint.x,searchPoint.y,searchPoint.z);
		  theCollision.push_back(searchPoint.x);
		  break;
	      }
	  }
	  if(theCollision.size() != 0){
	    if(searchPoint.x == 0) break;
	    geometry_msgs::PoseStamped poseStamped;
	    poseStamped.pose.position.x = searchPoint.x;
	    poseStamped.pose.position.y = searchPoint.y;
	    poseStamped.pose.position.z = searchPoint.z;
	//printf("%f,%f,%f\n",poseStamped.pose.position.x,poseStamped.pose.position.y,poseStamped.pose.position.z);
	    path.poses.push_back(poseStamped);
	      break;
	  }
	  else{
	    //printf("get:%d,%d\n",x.x,x.y);
	    geometry_msgs::PoseStamped poseStamped;
	    poseStamped.pose.position.x = Xdist[i];
	    poseStamped.pose.position.y = (float)x.y/10;
	    poseStamped.pose.position.z = (float)x.x/10;
	//printf("%f,%f,%f\n",poseStamped.pose.position.x,poseStamped.pose.position.y,poseStamped.pose.position.z);
	    path.poses.push_back(poseStamped);
	//lastPointList.push_back(x);;
	  }

     }

   }
   if(theCollision.size() == 0){
      isSafe = true;
   }
   isSafe = true;
   thePath = path;
  }

   if(thePath.poses.size() == 1){
     return;
   }

  int num = 0;
  isSafe = false;
  theCollision.clear();
  theCollision.push_back(0);
  while(isSafe != true){
    nav_msgs::Path waypath;
    waypath.header = std_msgs::Header();
    waypath.header.frame_id = "robot";
    waypath.header.stamp = ros::Time::now();

    MatrixXd listX(1,thePath.poses.size());
    MatrixXd listY(1,thePath.poses.size());
    MatrixXd listZ(1,thePath.poses.size());
    MatrixXd time_kf(1,thePath.poses.size());
    int bc = 0;
    for(int i = 0; i < thePath.poses.size(); i++){
	listX(0,i) = thePath.poses[i].pose.position.x;
	listY(0,i) = thePath.poses[i].pose.position.y;
	listZ(0,i) = thePath.poses[i].pose.position.z;
	time_kf(0,i) = i*2;

    }
    time_kfBK = time_kf;
    std::cout<<"X:" << listX <<std::endl<< "Y:" << listY <<std::endl<< "Z:" << listZ <<std::endl;

    optim_polyX = QP(listX,time_kf,VAX[0],VAX[1]);
    vector< double >trajX = getTraj(optim_polyX,time_kf);

    optim_polyY = QP(listY,time_kf,VAY[0],VAY[1]);
    vector< double >trajY = getTraj(optim_polyY,time_kf);

    optim_polyZ = QP(listZ,time_kf,VAZ[0],VAZ[1]);
    vector< double >trajZ = getTraj(optim_polyZ,time_kf);

    pcl::PointXYZ searchPoint;
    float radius = 0.1;
    for (int i = 0; i< trajX.size(); i++) {
        searchPoint.x = trajX[i];
	searchPoint.y = trajY[i];
	searchPoint.z = trajZ[i];

	std::vector<int> pointIdxRadiusSearch;
	std::vector<float> pointRadiusSquaredDistance;

	if (octree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
	{
	  theCollision.clear();
	  theCollision.push_back(trajX[i]);

	  break;
	}

    	geometry_msgs::PoseStamped poseStamped;
	poseStamped.pose.position.x = trajX[i];
	poseStamped.pose.position.y = trajY[i];
	poseStamped.pose.position.z = trajZ[i];
	waypath.poses.push_back(poseStamped);
    }
    if(theCollision.size() == 0){
      isSafe = true;
    }
    isSafe = true;
   // num++;
    theWayPath = waypath;

  }
    ros::Time end_time = ros::Time::now();
    ROS_INFO("It use %f",(end_time-begin_time).toSec());
    localPointPath = thePath;
    localWayPath = theWayPath;
    wayPointPub.publish(thePath);
    pathPub.publish(theWayPath);
    isUpdate = true;
  //ROS_INFO("I heard: %d and %f and %d",msg.height,*x,msg.data[0]);

}

int main(int argc, char **argv)
{

  ros::init(argc, argv, "path_build");
  ros::NodeHandle n;
  ROS_INFO("It's beginning");

  sub = n.subscribe("/cloud_source", 10, chatterCallback);
  ros::Subscriber globalpathsub = n.subscribe("/globe_path", 10, globalPathProcess);
  wayPointPub = n.advertise<nav_msgs::Path>("way_point",10);
  pathPub = n.advertise<nav_msgs::Path>("way_path",10);
  ros::Publisher real_path_pub = n.advertise<nav_msgs::Path>("real_path", 10);
  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 10);
  tf::TransformBroadcaster odom_broadcaster;
  tf::TransformListener listener;
  nav_msgs::Path real_pathway;
  real_pathway.header.frame_id = "world";
  ros::Time current_time, last_time;
  current_time = ros::Time::now();
  last_time = ros::Time::now();
  double xxxz = 0;
  double xyzz = 0;
  double x = 0.2;
  double y = -6;
  double z = 0.0;
  double thy = 0.0;
  double thz = atan(40.0);
  double t = 0;
  double vx = 0.0;
  double vy = 0.0;
  double vz = 0.0;
  double vthy = 0.0;
  double vthz = 0.0;
  VAX.push_back(0);
  VAX.push_back(0);
  VAY.push_back(0);
  VAY.push_back(0);
  VAZ.push_back(0);
  VAZ.push_back(0);
  isUpdate = false;
  bool isComplete = false;
  int globalWayPointNum = 0;
  ros::Rate loop_rate(20);

  int count = 0;
  while (ros::ok())
  {
    ros::spinOnce();

    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = ros::Time::now();;
    odom_trans.header.frame_id = "world";
    odom_trans.child_frame_id = "robot";

    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromRollPitchYaw(0,0,thz);
    odom_trans.transform.translation.x = x;
    odom_trans.transform.translation.y = y;
    odom_trans.transform.translation.z = z;
    odom_trans.transform.rotation = odom_quat;
    odom_broadcaster.sendTransform(odom_trans);

    current_time = ros::Time::now();
    double dt = (current_time - last_time).toSec();
    if(isUpdate == true){
      t = dt;
      isUpdate = false;
    }
    else{
      t += dt;
    }
    if(globalPath.poses.size()==0 || localWayPath.poses.size() == 0 ||isComplete){

	geometry_msgs::TransformStamped odom_trans;
	odom_trans.header.stamp = current_time;
	odom_trans.header.frame_id = "world";
	odom_trans.child_frame_id = "robot";

	geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromRollPitchYaw(0,0,thz);
	odom_trans.transform.translation.x = x;
	odom_trans.transform.translation.y = y;
	odom_trans.transform.translation.z = z;
	odom_trans.transform.rotation = odom_quat;
	odom_broadcaster.sendTransform(odom_trans);

	nav_msgs::Odometry odom;
	odom.header.stamp = current_time;
	odom.header.frame_id = "world";

	odom.pose.pose.position.x = x;
	odom.pose.pose.position.y = y;
	odom.pose.pose.position.z = z;
	odom.pose.pose.orientation = odom_quat;

	odom.child_frame_id = "robot";

	odom_pub.publish(odom);

    }
    else{
	listener.waitForTransform("/robot", "/world",
                              localWayPath.header.stamp, ros::Duration(3.0));
        geometry_msgs::PointStamped tfpoint;
	tfpoint.header.frame_id = localWayPath.header.frame_id;
	tfpoint.header.stamp = localWayPath.header.stamp;
	double scale = 0.005;
	tfpoint.point.x = localWayPath.poses[(int)(t/scale)].pose.position.x;
	tfpoint.point.y = localWayPath.poses[(int)(t/scale)].pose.position.y;
	tfpoint.point.z = localWayPath.poses[(int)(t/scale)].pose.position.z;
	geometry_msgs::PointStamped afTfPoint;
        listener.transformPoint("world", tfpoint, afTfPoint);

        x = afTfPoint.point.x;
	y = afTfPoint.point.y;
	z = afTfPoint.point.z;

	for(int i = (int)(t/scale);i<=(int)((t+dt)/scale);i++){
	    geometry_msgs::PointStamped smooth_point;
	    smooth_point.header.frame_id = localWayPath.header.frame_id;
	    smooth_point.header.stamp = localWayPath.header.stamp;
	    smooth_point.point.x = localWayPath.poses[i].pose.position.x;
	    smooth_point.point.y = localWayPath.poses[i].pose.position.y;
	    smooth_point.point.z = localWayPath.poses[i].pose.position.z;
	    geometry_msgs::PointStamped next_smooth_Point;
	    listener.transformPoint("world", smooth_point, next_smooth_Point);

	    geometry_msgs::PoseStamped poseStamped;
	    poseStamped.pose.position.x = next_smooth_Point.point.x;
	    poseStamped.pose.position.y = next_smooth_Point.point.y;
	    poseStamped.pose.position.z = next_smooth_Point.point.z;
	    real_pathway.poses.push_back(poseStamped);
	}
	real_pathway.header.stamp = current_time;
	real_path_pub.publish(real_pathway);

	geometry_msgs::PointStamped nexttfpoint;
	nexttfpoint.header.frame_id = localWayPath.header.frame_id;
	nexttfpoint.header.stamp = localWayPath.header.stamp;
	nexttfpoint.point.x = localWayPath.poses[(int)((t+dt)/scale)].pose.position.x;
	nexttfpoint.point.y = localWayPath.poses[(int)((t+dt)/scale)].pose.position.y;
	nexttfpoint.point.z = localWayPath.poses[(int)((t+dt)/scale)].pose.position.z;
	geometry_msgs::PointStamped nextafTfPoint;
        listener.transformPoint("world", nexttfpoint, nextafTfPoint);

	VAX = getVandA(optim_polyX,time_kfBK,t);
	VAY = getVandA(optim_polyY,time_kfBK,t);
	VAZ = getVandA(optim_polyZ,time_kfBK,t);
	geometry_msgs::Vector3Stamped vectorAcc;
	vectorAcc.header.frame_id = localWayPath.header.frame_id;
	vectorAcc.header.stamp = localWayPath.header.stamp;
	vectorAcc.vector.x = VAX[1];
	vectorAcc.vector.y = VAY[1];
	vectorAcc.vector.z = VAZ[1];
	geometry_msgs::Vector3Stamped vectorAccAns;
	geometry_msgs::Vector3Stamped vectorVco;
	vectorVco.header.frame_id = localWayPath.header.frame_id;
	vectorVco.header.stamp = localWayPath.header.stamp;
	vectorVco.vector.x = VAX[0];
	vectorVco.vector.y = VAY[0];
	vectorVco.vector.z = VAZ[0];
	geometry_msgs::Vector3Stamped vectorVcoAns;
	listener.transformVector("world",vectorAcc,vectorAccAns);
	listener.transformVector("world",vectorVco,vectorVcoAns);
	//printf("%f\n",atan2((nextafTfPoint.point.y-y),(nextafTfPoint.point.x-x))/3.14*180);
        //thy = atan2((nextafTfPoint.point.z-z),sqrt((nextafTfPoint.point.x-x)*(nextafTfPoint.point.x-x)+(nextafTfPoint.point.z-z)*(nextafTfPoint.point.z-z)));

	//printf("after:%f,%f\n",thy,thz);
	//if(nextafTfPoint.point.y != y||nextafTfPoint.point.x!=x){
	//  thz = atan2((nextafTfPoint.point.y-y),(nextafTfPoint.point.x-x));
	//}
	while((x-globalPath.poses[globalWayPointNum].pose.position.x)*(x-globalPath.poses[globalWayPointNum].pose.position.x)
	  +(y-globalPath.poses[globalWayPointNum].pose.position.y)*(y-globalPath.poses[globalWayPointNum].pose.position.y)
	  +(z-globalPath.poses[globalWayPointNum].pose.position.z)*(z-globalPath.poses[globalWayPointNum].pose.position.z)
	  < 1){
	  globalWayPointNum++;
	  if(globalPath.poses.size()==globalWayPointNum){
	    globalWayPointNum--;
	    if((x-globalPath.poses[globalWayPointNum].pose.position.x)*(x-globalPath.poses[globalWayPointNum].pose.position.x)
	  +(y-globalPath.poses[globalWayPointNum].pose.position.y)*(y-globalPath.poses[globalWayPointNum].pose.position.y)
	  +(z-globalPath.poses[globalWayPointNum].pose.position.z)*(z-globalPath.poses[globalWayPointNum].pose.position.z)
	  < 0.3)
	    isComplete = true;
	    break;
	  }
	}
	//thz = atan2((globalPath.poses[globalWayPointNum].pose.position.y-y),(globalPath.poses[globalWayPointNum].pose.position.x-x));

	thz = atan2((globalPath.poses[globalWayPointNum].pose.position.y-globalPath.poses[globalWayPointNum-1].pose.position.y),(globalPath.poses[globalWayPointNum].pose.position.x-globalPath.poses[globalWayPointNum-1].pose.position.x));

	geometry_msgs::TransformStamped odom_trans;
	odom_trans.header.stamp = current_time;
	odom_trans.header.frame_id = "world";
	odom_trans.child_frame_id = "robot";

	geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromRollPitchYaw(0,0,thz);
	odom_trans.transform.translation.x = x;
	odom_trans.transform.translation.y = y;
	odom_trans.transform.translation.z = z;
	odom_trans.transform.rotation = odom_quat;
	odom_broadcaster.sendTransform(odom_trans);

	nav_msgs::Odometry odom;
	odom.header.stamp = current_time;
	odom.header.frame_id = "world";

	if(nextafTfPoint.point.y != y||nextafTfPoint.point.x!=x){
	   xxxz = atan2((nextafTfPoint.point.y-y),(nextafTfPoint.point.x-x));
	   xyzz =  -1*atan2((nextafTfPoint.point.z-z),sqrt((nextafTfPoint.point.y-y)*(nextafTfPoint.point.y-y)+(nextafTfPoint.point.x-x)*(nextafTfPoint.point.x-x)));
	}
	odom.pose.pose.position.x = x;
	odom.pose.pose.position.y = y;
	odom.pose.pose.position.z = z;
	odom.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0,xyzz,xxxz);;

	odom.child_frame_id = "robot";
	odom.twist.twist.linear.x = vectorVco.vector.x;
	odom.twist.twist.linear.y = vectorVco.vector.y;
	odom.twist.twist.linear.z = vectorVco.vector.z;
	//odom.twist.twist.angular.y = atan2((nextafTfPoint.point.z-z),(nextafTfPoint.point.x-x))/dt;
	//odom.twist.twist.angular.z = atan2((nextafTfPoint.point.y-y),(nextafTfPoint.point.x-x))/dt;

        odom_pub.publish(odom);

	//double ang = atan2(VAY[0],VAX[0])-atan2(VAY[1],VAX[1]);
	//double val = sqrt( VAX[1]*VAX[1] + VAY[1]*VAY[1]);
	//VAX[0] = sqrt( VAX[0]*VAX[0] + VAY[0]*VAY[0]);
	//VAX[1] = val*sin(ang);
	//VAY[0] = 0;
	//VAY[1] = val*sin(ang);
	printf("%f,%f,thz:=%f\n",vectorVcoAns.vector.x,vectorVcoAns.vector.y,thz);
	double ang0 = atan2(vectorVcoAns.vector.y,vectorVcoAns.vector.x)- thz;
	double ang1 = atan2(vectorAccAns.vector.y,vectorAccAns.vector.x) - thz;
	double valA0 = sqrt( vectorVcoAns.vector.y*vectorVcoAns.vector.y + vectorVcoAns.vector.x*vectorVcoAns.vector.x);
	double valA1 = sqrt( vectorAccAns.vector.y*vectorAccAns.vector.y + vectorAccAns.vector.x*vectorAccAns.vector.x);
	printf("%f,%f,%f,%f\n",ang0,ang1,valA0,valA1);
	VAX[0] = valA0*cos(ang0);
	VAX[1] = valA1*cos(ang1);
	VAY[0] = valA0*sin(ang0);
	VAY[1] = valA1*sin(ang1);
	printf("%f,%f\n",VAX[0],VAY[0]);
    }

    last_time = current_time;
    loop_rate.sleep();
    ++count;
  }

  return 0;
}

max_stars_repo_name: weichnn/avoidancebyprojection

max_issues_repo_name: weichnn/avoidancebyprojection

max_forks_repo_name: weichnn/avoidancebyprojection


text:
/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2020 Scientific Computing and Imaging Institute,
   University of Utah.

   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 <gtest/gtest.h>

#include <Core/Datatypes/Legacy/Field/VField.h>
#include <Core/Datatypes/Legacy/Field/FieldInformation.h>
#include <Core/Algorithms/Base/AlgorithmPreconditions.h>
#include <Core/Algorithms/BrainStimulator/SetupRHSforTDCSandTMSAlgorithm.h>
#include <Testing/Utils/SCIRunUnitTests.h>
#include <Testing/Utils/MatrixTestUtilities.h>
#include <boost/lexical_cast.hpp>
#include <vector>
#include <iostream>
#include <Core/Math/MiscMath.h>

using namespace SCIRun;
using namespace SCIRun::Core::Geometry;
using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Algorithms::BrainStimulator;
using namespace SCIRun::Core::Algorithms;
using namespace SCIRun::TestUtils;

namespace
{
  FieldHandle LoadColin27ScalpSurfMesh()
  {
    return loadFieldFromFile(TestResources::rootDir() / "Fields/setuptdcs/colin27_skin_surf.fld");
  }
  FieldHandle LoadColin27ElecSurfMesh()
  {
    return loadFieldFromFile(TestResources::rootDir() / "Fields/setuptdcs/colin27_elc_surf.fld");
  }
  FieldHandle LoadColin27TetMesh()
  {
    return loadFieldFromFile(TestResources::rootDir() / "Fields/setuptdcs/elc_tet.fld");
  }

  DenseMatrixHandle ElectrodeSpongeLocationAndThickness()
  {
   DenseMatrixHandle m(makeShared<DenseMatrix>(2,4));
   (*m)(0,0) = 59.5343;
   (*m)(0,1) = 15.5240;
   (*m)(0,2) = 84.5013;
   (*m)(0,3) = 4.0;
   (*m)(1,0) = 122.798;
   (*m)(1,1) = 32.2019;
   (*m)(1,2) = 128.798;
   (*m)(1,3) = 4.0;

   return m;
  }
}

TEST(SetupRHSforTDCSandTMSAlgorithm, Correct)
{
  SetupTDCSAlgorithm algo;
  int m=2; //2 patch electrodes for colin27
  int ref_node = 1000;
  double impedance = 5000;
  DenseMatrixHandle contact_impedance_times_elc_surface(makeShared<DenseMatrix>(2,1));
  (*contact_impedance_times_elc_surface)(1,0) = 16.408535;
  (*contact_impedance_times_elc_surface)(0,0) = 15.326115;
  std::vector<Variable> elc;

  elc.push_back(AlgorithmParameter(Name("elc" + boost::lexical_cast<std::string>(0)), -1.0));
  elc.push_back(AlgorithmParameter(Name("elc" + boost::lexical_cast<std::string>(1)),  1.0));

  std::vector<Variable> impelc;
  for (int i=0; i<m; i++)
  {
     AlgorithmParameter imp_elc_i(Name("imp_elc" + boost::lexical_cast<std::string>(i)), impedance);
     impelc.push_back(imp_elc_i);
  }
  FieldHandle mesh(LoadColin27TetMesh());
  FieldHandle scalp_tri_surf(LoadColin27ScalpSurfMesh());
  FieldHandle elc_tri_surf(LoadColin27ElecSurfMesh());
  DenseMatrixHandle elc_sponge_location(ElectrodeSpongeLocationAndThickness());

  algo.set(Parameters::refnode, ref_node);
  auto result = algo.run(mesh,elc,impelc,m,scalp_tri_surf,elc_tri_surf,elc_sponge_location);
  DenseMatrixHandle lhs_knowns = result.get<0>();
  DenseMatrixHandle elc_element= result.get<1>();
  DenseMatrixHandle elc_element_typ = result.get<2>();
  DenseMatrixHandle elc_element_def = result.get<3>();
  DenseMatrixHandle elc_contact_imp= result.get<4>();
  DenseMatrixHandle rhs = result.get<5>();
  FieldHandle elec_sponge_surf = result.get<6>();
  DenseMatrixHandle selectmatrixind = result.get<7>();

  EXPECT_EQ(lhs_knowns->nrows(),84350);
  EXPECT_EQ(lhs_knowns->ncols(),1);

  EXPECT_EQ(elc_element->nrows(),27743);
  EXPECT_EQ(elc_element->ncols(),1);

  EXPECT_EQ(elc_element_typ->nrows(),27743);
  EXPECT_EQ(elc_element_typ->ncols(),1);

  EXPECT_EQ(elc_element_def->nrows(),27743);
  EXPECT_EQ(elc_element_def->ncols(),4);

  EXPECT_EQ(elc_contact_imp->nrows(),27743);
  EXPECT_EQ(elc_contact_imp->ncols(),1);

  for (int i=0; i<lhs_knowns->nrows(); i++)
  {
   if (i!=ref_node)
    EXPECT_TRUE(IsNan((*lhs_knowns)(i,0)));
     else
        EXPECT_EQ((*lhs_knowns)(i), 0.0);
  }

  for (int i=0; i<elc_element_typ->nrows(); i++)
      EXPECT_EQ((*elc_element_typ)(i,0), 2.0);

  for (int i=0; i<selectmatrixind->nrows(); i++)
      EXPECT_EQ((*selectmatrixind)(i,0), i);

  VMesh* elec_sponge_surf_vmesh = elec_sponge_surf->vmesh();

  for (int i=0; i<elc_element->nrows(); i++)
  {
    for (int j=0; j<m; j++)
    {
     if ((*elc_element)(i,0)==j)
       EXPECT_NEAR((*elc_contact_imp)(i,0), (*contact_impedance_times_elc_surface)(j,0),0.0001);
    }

    if ((*elc_element)(i,0)>=m)
       EXPECT_EQ(1,0);   //intended throw
  }

  EXPECT_EQ(elec_sponge_surf_vmesh->num_nodes(),14343);
  EXPECT_EQ(elec_sponge_surf_vmesh->num_elems(),27743);
}

TEST(SetupRHSforTDCSandTMSAlgorithm, Correct_scalpIsElectrodeContact)
{
  SetupTDCSAlgorithm algo;
  algo.set(Parameters::GetContactSurface, true);  //determine the surface that joins scalp and electrode spoge
  int m=2; //2 patch electrodes for colin27
  int ref_node = 1050;
  double impedance = 10000; // 100000 [Ohm] !!!
  DenseMatrixHandle contact_impedance_times_elc_surface(makeShared<DenseMatrix>(2,1));
  (*contact_impedance_times_elc_surface)(0,0) = 28.882450711537267; /// BrainStimulator expects Ohm * m^2
  (*contact_impedance_times_elc_surface)(1,0) = 30.353635349846389;
  std::vector<Variable> elc;

  elc.push_back(AlgorithmParameter(Name("elc" + boost::lexical_cast<std::string>(0)), -1.0));
  elc.push_back(AlgorithmParameter(Name("elc" + boost::lexical_cast<std::string>(1)),  1.0));

  std::vector<Variable> impelc;
  for (int i=0; i<m; i++)
  {
     AlgorithmParameter imp_elc_i(Name("imp_elc" + boost::lexical_cast<std::string>(i)), impedance);
     impelc.push_back(imp_elc_i);
  }
  FieldHandle mesh(LoadColin27TetMesh());
  FieldHandle scalp_tri_surf(LoadColin27ScalpSurfMesh());
  FieldHandle elc_tri_surf(LoadColin27ElecSurfMesh());
  DenseMatrixHandle elc_sponge_location(ElectrodeSpongeLocationAndThickness());

  algo.set(Parameters::refnode, ref_node);
  auto result = algo.run(mesh,elc,impelc,m,scalp_tri_surf,elc_tri_surf,elc_sponge_location);
  DenseMatrixHandle lhs_knowns = result.get<0>();
  DenseMatrixHandle elc_element= result.get<1>();
  DenseMatrixHandle elc_element_typ = result.get<2>();
  DenseMatrixHandle elc_element_def = result.get<3>();
  DenseMatrixHandle elc_contact_imp= result.get<4>();
  DenseMatrixHandle rhs = result.get<5>();
  FieldHandle elec_sponge_surf = result.get<6>();
  DenseMatrixHandle selectmatrixind = result.get<7>();

  EXPECT_EQ(lhs_knowns->nrows(),84350);
  EXPECT_EQ(lhs_knowns->ncols(),1);

  EXPECT_EQ(elc_element->nrows(),25865);
  EXPECT_EQ(elc_element->ncols(),1);

  EXPECT_EQ(elc_element_typ->nrows(),25865);
  EXPECT_EQ(elc_element_typ->ncols(),1);

  EXPECT_EQ(elc_element_def->nrows(),25865);
  EXPECT_EQ(elc_element_def->ncols(),4);

  EXPECT_EQ(elc_contact_imp->nrows(),25865);
  EXPECT_EQ(elc_contact_imp->ncols(),1);

  for (int i=0; i<lhs_knowns->nrows(); i++)
  {
   if (i!=ref_node)
    EXPECT_TRUE(IsNan((*lhs_knowns)(i,0)));
     else
        EXPECT_EQ((*lhs_knowns)(i), 0.0);
  }

  for (int i=0; i<elc_element_typ->nrows(); i++)
      EXPECT_EQ((*elc_element_typ)(i,0), 2.0);

  for (int i=0; i<selectmatrixind->nrows(); i++)
      EXPECT_EQ((*selectmatrixind)(i,0), i);

  VMesh* elec_sponge_surf_vmesh = elec_sponge_surf->vmesh();

  for (int i=0; i<elc_element->nrows(); i++)
  {
    for (int j=0; j<m; j++)
    {
     if ((*elc_element)(i,0)==j)
       EXPECT_NEAR((*elc_contact_imp)(i,0), (*contact_impedance_times_elc_surface)(j,0),0.001);
    }

    if ((*elc_element)(i,0)>=m)
       EXPECT_EQ(1,0);   //intended throw
  }

  EXPECT_EQ(elec_sponge_surf_vmesh->num_nodes(),77595);
  EXPECT_EQ(elec_sponge_surf_vmesh->num_elems(),25865);
}