<commit_before>#include "Intake.h"
#include "Commands/IntakeBall.h"
#include "Commands/PushBall.h"
#include <math.h>

/**
 * Subsystems namespace with implementation
 * of Intake class.
 */
namespace subsystems
{

using IntakeBall = commands::IntakeBall;
using PushBall = commands::PushBall;

// Constructor:
Intake::Intake() : dman::WPISystem("Intake")
{
	roller_ = NULL; // keep null until configure time
	detector_ = new DigitalInput(0);

	// Default values:
	intake_speed_ = 0.5;
	push_speed_ = -1.0;

	state_ = State_t::OFF;
	mode_ = Mode_t::VBUS;
	SetMode(Mode_t::VELOCITY);

	max_velocity_ = 1000.0;
	allowed_error_ = 100;
	shoot_velocity_ = 1.0;
}

// Destructor:
Intake::~Intake()
{
	delete roller_;
	delete detector_;
}

// Dalek Manager config functions:
void Intake::doRegister()
{
	// CAN port settings:
	auto& can_ports = GetPortSpace("CAN");
	can_ports("intake_roller").SetDefault(5);

	// Instance variable settings
	auto& settings = GetSettings();
	settings("intake_speed").SetDefault(intake_speed_);
	settings("push_speed").SetDefault(push_speed_);
	settings("max_velocity").SetDefault(max_velocity_);
	settings("allowed_error").SetDefault(allowed_error_);
	settings("shoot_velocity").SetDefault(shoot_velocity_);

	// PID Settings | TODO: Set proper default values
	auto& closed_loop_settings = settings["closed_loop_settings"];
	closed_loop_settings("use").SetDefault(false);
	closed_loop_settings("P").SetDefault(.1);
	closed_loop_settings("I").SetDefault(0.0);
	closed_loop_settings("D").SetDefault(0.0);
	closed_loop_settings("F").SetDefault(1.0);
	closed_loop_settings("I_Zone").SetDefault(2.5);
	closed_loop_settings("ramp_rate").SetDefault(10);

	// Inversion settings
	settings["intake_roller"]("invert_output").SetDefault(false);
	settings["intake_roller"]("invert_sensor").SetDefault(false);
}

bool Intake::doConfigure()
{
	Log(dman::MessageData::STATUS, "", "Subsystem") << "Configuring Intake";

	// Initialize roller to CAN port
	auto& can_ports = GetPortSpace("CAN");
	if (roller_ == NULL)
		roller_ = new CANTalon(can_ports("intake_roller").GetValueOrDefault());

	// Configure variable values
	auto& settings = GetSettings();
	SetIntakeSpeed(settings("intake_speed").GetValueOrDefault());
	SetPushSpeed(settings("push_speed").GetValueOrDefault());
	SetMaxVelocity(settings("max_velocity").GetValueOrDefault());
	SetAllowedError(settings("allowed_error").GetValueOrDefault());
	SetShootVelocity(settings("shoot_velocity").GetValueOrDefault());

	// Configure closed loop settings
	auto& closed_loop_settings = settings["closed_loop_settings"];
	if (closed_loop_settings("use").GetValueOrDefault() == true)
	{
		roller_->SetP(closed_loop_settings("P").GetValueOrDefault());
		roller_->SetI(closed_loop_settings("I").GetValueOrDefault());
		roller_->SetD(closed_loop_settings("D").GetValueOrDefault());
		roller_->SetF(closed_loop_settings("F").GetValueOrDefault());
		roller_->SetIzone(closed_loop_settings("I_Zone").GetValueOrDefault());
		roller_->SetCloseLoopRampRate(closed_loop_settings("ramp_rate").GetValueOrDefault());
	}

	// Configure roller settings
	roller_->SetInverted(settings["intake_roller"]("invert_output").GetValueOrDefault());
	roller_->SetSensorDirection(settings["intake_roller"]("invert_sensor").GetValueOrDefault());

	return true;
}

// Main functions:
void Intake::Initialize()
{
	roller_->SetFeedbackDevice(CANTalon::QuadEncoder);
	roller_->ConfigEncoderCodesPerRev(8);
	roller_->SetSensorDirection(true);
	roller_->SetInverted(true);
	roller_->SelectProfileSlot(0);
	roller_->SetVoltageRampRate(0.0);
	//roller_->SetCloseLoopRampRate(5.0);

	// Configure max and min voltage outputs
	roller_->ConfigNominalOutputVoltage(0.0, 0.0);
	roller_->ConfigPeakOutputVoltage(12.0, -12.0);
}

bool Intake::CheckSwitch() const
{
	return !(detector_->Get());
}

void Intake::TakeBall(bool check)
{
	if (CheckSwitch() && check)
		Stop();
	else
		SetSpeed(intake_speed_);
}

void Intake::OutakeBall()
{
	SetSpeed(push_speed_);
}

void Intake::ShootBall()
{
	SetSpeed(shoot_velocity_);
}

void Intake::SetSpeed(double speed)
{
	if (mode_ == Mode_t::VELOCITY)
	{
		roller_->Set(max_velocity_ * speed);
	}
	else if (mode_ == Mode_t::VBUS)
	{
		roller_->Set(speed);
	}
}

double Intake::GetSpeed() const
{
	return roller_->Get();
}

void Intake::Stop()
{
	SetSpeed(0.0);
}

void Intake::SetIntakeSpeed(double intake_speed)
{
	intake_speed_ = intake_speed;
}

void Intake::SetPushSpeed(double push_speed)
{
	push_speed_ = push_speed;
}

double Intake::GetIntakeSpeed() const
{
	return intake_speed_;
}

double Intake::GetPushSpeed() const
{
	return push_speed_;
}

double Intake::GetMaxVelocity() const
{
	return max_velocity_;
}

void Intake::SetMaxVelocity(double max_velocity)
{
	max_velocity_ = max_velocity;
}

void Intake::SetShootVelocity(double shoot_velocity)
{
	shoot_velocity_ = shoot_velocity;
}

double Intake::GetShootVelocity() const
{
	return shoot_velocity_;
}

double Intake::GetCurrentPosition() const
{
	return roller_->GetPosition();
}

// Error functions:
double Intake::GetErr() const
{
	return roller_->GetClosedLoopError();
}

void Intake::SetAllowedError(double err)
{
	allowed_error_ = err;
}

double Intake::GetAllowedError() const
{
	return allowed_error_;
}

bool Intake::IsAllowable() const
{
	return (fabs(GetErr()) > fabs(allowed_error_));
}

// State functions:
void Intake::SetState(State_t state)
{
	state_ = state;
}

Intake::State_t Intake::GetState() const
{
	return state_;
}

// Control mode functions:
void Intake::SetMode(Mode_t mode)
{
	if (mode_ != mode)
	{
		mode_ = mode;
		if (mode_ == Mode_t::VELOCITY)
			roller_->SetControlMode(CANTalon::ControlMode::kSpeed);
		else if (mode_ == Mode_t::VBUS)
			roller_->SetControlMode(CANTalon::ControlMode::kPercentVbus);
	}
}

Intake::Mode_t Intake::GetMode() const
{
	return mode_;
}

// Command functions:
IntakeBall* Intake::MakeIntakeBall()
{
	return new IntakeBall(this);
}

PushBall* Intake::MakePushBall()
{
	return new PushBall(this);
}

} // end namespace subsystems
<commit_msg>Updates Intake to implement value store functions for talon values and boulder holding state<commit_after>#include "Intake.h"
#include "Commands/IntakeBall.h"
#include "Commands/PushBall.h"
#include "Utility/FunkyGet.h"
#include <math.h>

/**
 * Subsystems namespace with implementation
 * of Intake class.
 */
namespace subsystems
{
using namespace dman;
using IntakeBall = commands::IntakeBall;
using PushBall = commands::PushBall;

// Constructor:
Intake::Intake() : dman::WPISystem("Intake")
{
	roller_ = NULL; // keep null until configure time
	detector_ = new DigitalInput(0);

	// Default values:
	intake_speed_ = 0.5;
	push_speed_ = -1.0;

	state_ = State_t::OFF;
	mode_ = Mode_t::VBUS;
	SetMode(Mode_t::VELOCITY);

	max_velocity_ = 1000.0;
	allowed_error_ = 100;
	shoot_velocity_ = 1.0;
}

// Destructor:
Intake::~Intake()
{
	delete roller_;
	delete detector_;
}

// Dalek Manager config functions:
void Intake::doRegister()
{
	// CAN port settings:
	auto& can_ports = GetPortSpace("CAN");
	can_ports("intake_roller").SetDefault(5);

	// Instance variable settings
	auto& settings = GetSettings();
	settings("intake_speed").SetDefault(intake_speed_);
	settings("push_speed").SetDefault(push_speed_);
	settings("max_velocity").SetDefault(max_velocity_);
	settings("allowed_error").SetDefault(allowed_error_);
	settings("shoot_velocity").SetDefault(shoot_velocity_);

	// PID Settings | TODO: Set proper default values
	auto& closed_loop_settings = settings["closed_loop_settings"];
	closed_loop_settings("use").SetDefault(false);
	closed_loop_settings("P").SetDefault(.1);
	closed_loop_settings("I").SetDefault(0.0);
	closed_loop_settings("D").SetDefault(0.0);
	closed_loop_settings("F").SetDefault(1.0);
	closed_loop_settings("I_Zone").SetDefault(2.5);
	closed_loop_settings("ramp_rate").SetDefault(10);

	// Inversion settings
	settings["intake_roller"]("invert_output").SetDefault(false);
	settings["intake_roller"]("invert_sensor").SetDefault(false);

	// Value store functions
	GetLocalValue<double>("front_roller_temp").Initialize(std::make_shared<FunkyGet<double> >([this]()
			{
				return roller_->GetTemperature();
			}));
	GetLocalValue<double>("front_roller_ouput_voltage").Initialize(std::make_shared<FunkyGet<double> > ([this] ()
			{
				return roller_->GetOutputVoltage();
			}));
	GetLocalValue<double>("front_roller_ouput_current").Initialize(std::make_shared<FunkyGet<double> > ([this] ()
			{
				return roller_->GetOutputCurrent();
			}));
	GetLocalValue<bool>("holding_bouler").Initialize(std::make_shared<FunkyGet<bool> > ([this] ()
			{
				return this->CheckSwitch();
			}));
}

bool Intake::doConfigure()
{
	Log(dman::MessageData::STATUS, "", "Subsystem") << "Configuring Intake";

	// Initialize roller to CAN port
	auto& can_ports = GetPortSpace("CAN");
	if (roller_ == NULL)
		roller_ = new CANTalon(can_ports("intake_roller").GetValueOrDefault());

	// Configure variable values
	auto& settings = GetSettings();
	SetIntakeSpeed(settings("intake_speed").GetValueOrDefault());
	SetPushSpeed(settings("push_speed").GetValueOrDefault());
	SetMaxVelocity(settings("max_velocity").GetValueOrDefault());
	SetAllowedError(settings("allowed_error").GetValueOrDefault());
	SetShootVelocity(settings("shoot_velocity").GetValueOrDefault());

	// Configure closed loop settings
	auto& closed_loop_settings = settings["closed_loop_settings"];
	if (closed_loop_settings("use").GetValueOrDefault() == true)
	{
		roller_->SetP(closed_loop_settings("P").GetValueOrDefault());
		roller_->SetI(closed_loop_settings("I").GetValueOrDefault());
		roller_->SetD(closed_loop_settings("D").GetValueOrDefault());
		roller_->SetF(closed_loop_settings("F").GetValueOrDefault());
		roller_->SetIzone(closed_loop_settings("I_Zone").GetValueOrDefault());
		roller_->SetCloseLoopRampRate(closed_loop_settings("ramp_rate").GetValueOrDefault());
	}

	// Configure roller settings
	roller_->SetInverted(settings["intake_roller"]("invert_output").GetValueOrDefault());
	roller_->SetSensorDirection(settings["intake_roller"]("invert_sensor").GetValueOrDefault());

	return true;
}

// Main functions:
void Intake::Initialize()
{
	roller_->SetFeedbackDevice(CANTalon::QuadEncoder);
	roller_->ConfigEncoderCodesPerRev(8);
	roller_->SetSensorDirection(true);
	roller_->SetInverted(true);
	roller_->SelectProfileSlot(0);
	roller_->SetVoltageRampRate(0.0);
	//roller_->SetCloseLoopRampRate(5.0);

	// Configure max and min voltage outputs
	roller_->ConfigNominalOutputVoltage(0.0, 0.0);
	roller_->ConfigPeakOutputVoltage(12.0, -12.0);
}

bool Intake::CheckSwitch() const
{
	return !(detector_->Get());
}

void Intake::TakeBall(bool check)
{
	if (CheckSwitch() && check)
		Stop();
	else
		SetSpeed(intake_speed_);
}

void Intake::OutakeBall()
{
	SetSpeed(push_speed_);
}

void Intake::ShootBall()
{
	SetSpeed(shoot_velocity_);
}

void Intake::SetSpeed(double speed)
{
	if (mode_ == Mode_t::VELOCITY)
	{
		roller_->Set(max_velocity_ * speed);
	}
	else if (mode_ == Mode_t::VBUS)
	{
		roller_->Set(speed);
	}
}

double Intake::GetSpeed() const
{
	return roller_->Get();
}

void Intake::Stop()
{
	SetSpeed(0.0);
}

void Intake::SetIntakeSpeed(double intake_speed)
{
	intake_speed_ = intake_speed;
}

void Intake::SetPushSpeed(double push_speed)
{
	push_speed_ = push_speed;
}

double Intake::GetIntakeSpeed() const
{
	return intake_speed_;
}

double Intake::GetPushSpeed() const
{
	return push_speed_;
}

double Intake::GetMaxVelocity() const
{
	return max_velocity_;
}

void Intake::SetMaxVelocity(double max_velocity)
{
	max_velocity_ = max_velocity;
}

void Intake::SetShootVelocity(double shoot_velocity)
{
	shoot_velocity_ = shoot_velocity;
}

double Intake::GetShootVelocity() const
{
	return shoot_velocity_;
}

double Intake::GetCurrentPosition() const
{
	return roller_->GetPosition();
}

// Error functions:
double Intake::GetErr() const
{
	return roller_->GetClosedLoopError();
}

void Intake::SetAllowedError(double err)
{
	allowed_error_ = err;
}

double Intake::GetAllowedError() const
{
	return allowed_error_;
}

bool Intake::IsAllowable() const
{
	return (fabs(GetErr()) > fabs(allowed_error_));
}

// State functions:
void Intake::SetState(State_t state)
{
	state_ = state;
}

Intake::State_t Intake::GetState() const
{
	return state_;
}

// Control mode functions:
void Intake::SetMode(Mode_t mode)
{
	if (mode_ != mode)
	{
		mode_ = mode;
		if (mode_ == Mode_t::VELOCITY)
			roller_->SetControlMode(CANTalon::ControlMode::kSpeed);
		else if (mode_ == Mode_t::VBUS)
			roller_->SetControlMode(CANTalon::ControlMode::kPercentVbus);
	}
}

Intake::Mode_t Intake::GetMode() const
{
	return mode_;
}

// Command functions:
IntakeBall* Intake::MakeIntakeBall()
{
	return new IntakeBall(this);
}

PushBall* Intake::MakePushBall()
{
	return new PushBall(this);
}

} // end namespace subsystems
<|endoftext|>


<commit_before>/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2011, Willow Garage, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id$
 *
 */

#ifndef PCL_SEARCH_IMPL_ORGANIZED_NEIGHBOR_SEARCH_H_
#define PCL_SEARCH_IMPL_ORGANIZED_NEIGHBOR_SEARCH_H_

#include <pcl/search/organized.h>
#include <pcl/common/eigen.h>
#include <pcl/common/time.h>
#include <Eigen/Eigenvalues>

//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> int
pcl::search::OrganizedNeighbor<PointT>::radiusSearch (const               PointT &query,
                                                      const double        radius,
                                                      std::vector<int>    &k_indices,
                                                      std::vector<float>  &k_sqr_distances,
                                                      unsigned int        max_nn) const
{
  // NAN test
  assert (isFinite (query) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");

  // search window
  unsigned left, right, top, bottom;
  //unsigned x, y, idx;
  float squared_distance;
  double squared_radius;

  k_indices.clear ();
  k_sqr_distances.clear ();

  squared_radius = radius * radius;

  this->getProjectedRadiusSearchBox (query, static_cast<float> (squared_radius), left, right, top, bottom);

  // iterate over search box
  if (max_nn == 0 || max_nn >= static_cast<unsigned int> (input_->points.size ()))
    max_nn = static_cast<unsigned int> (input_->points.size ());

  k_indices.reserve (max_nn);
  k_sqr_distances.reserve (max_nn);

  unsigned yEnd  = (bottom + 1) * input_->width + right + 1;
  register unsigned idx  = top * input_->width + left;
  unsigned skip = input_->width - right + left - 1;
  unsigned xEnd = idx - left + right + 1;

  for (; xEnd != yEnd; idx += skip, xEnd += input_->width)
  {
    for (; idx < xEnd; ++idx)
    {
      if (!mask_[idx] || !isFinite (input_->points[idx]))
        continue;

      squared_distance = (input_->points[idx].getVector3fMap () - query.getVector3fMap ()).squaredNorm ();
      if (squared_distance <= squared_radius)
      {
        k_indices.push_back (idx);
        k_sqr_distances.push_back (squared_distance);
        // already done ?
        if (k_indices.size () == max_nn)
        {
          if (sorted_results_)
            this->sortResults (k_indices, k_sqr_distances);
          return (max_nn);
        }
      }
    }
  }
  if (sorted_results_)
    this->sortResults (k_indices, k_sqr_distances);
  return (static_cast<int> (k_indices.size ()));
}

//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> int
pcl::search::OrganizedNeighbor<PointT>::nearestKSearch (const PointT &query,
                                                        int k,
                                                        std::vector<int> &k_indices,
                                                        std::vector<float> &k_sqr_distances) const
{
  assert (isFinite (query) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
  if (k < 1)
  {
    k_indices.clear ();
    k_sqr_distances.clear ();
    return (0);
  }

  // project query point on the image plane
  Eigen::Vector3f q = KR_ * query.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);
  int xBegin = int(q [0] / q [2] + 0.5f);
  int yBegin = int(q [1] / q [2] + 0.5f);
  int xEnd   = xBegin + 1; // end is the pixel that is not used anymore, like in iterators
  int yEnd   = yBegin + 1;

  // the search window. This is supposed to shrink within the iterations
  unsigned left = 0;
  unsigned right = input_->width - 1;
  unsigned top = 0;
  unsigned bottom = input_->height - 1;

  std::priority_queue <Entry> results;
  //std::vector<Entry> k_results;
  //k_results.reserve (k);
  // add point laying on the projection of the query point.
  if (xBegin >= 0 &&
      xBegin < static_cast<int> (input_->width) &&
      yBegin >= 0 &&
      yBegin < static_cast<int> (input_->height))
    testPoint (query, k, results, yBegin * input_->width + xBegin);
  else // point lys
  {
    // find the box that touches the image border -> dont waste time evaluating boxes that are completely outside the image!
    int dist = std::numeric_limits<int>::max ();

    if (xBegin < 0)
      dist = -xBegin;
    else if (xBegin >= static_cast<int> (input_->width))
      dist = xBegin - static_cast<int> (input_->width);

    if (yBegin < 0)
      dist = std::min (dist, -yBegin);
    else if (yBegin >= static_cast<int> (input_->height))
      dist = std::min (dist, yBegin - static_cast<int> (input_->height));

    xBegin -= dist;
    xEnd   += dist;

    yBegin -= dist;
    yEnd   += dist;
  }

  // stop used as isChanged as well as stop.
  bool stop = false;
  do
  {
    // increment box size
    --xBegin;
    ++xEnd;
    --yBegin;
    ++yEnd;

    // the range in x-direction which intersects with the image width
    int xFrom = xBegin;
    int xTo   = xEnd;
    clipRange (xFrom, xTo, 0, input_->width);

    // if x-extend is not 0
    if (xTo > xFrom)
    {
      // if upper line of the rectangle is visible and x-extend is not 0
      if (yBegin >= 0 && yBegin < static_cast<int> (input_->height))
      {
        int idx   = yBegin * input_->width + xFrom;
        int idxTo = idx + xTo - xFrom;
        for (; idx < idxTo; ++idx)
          stop = testPoint (query, k, results, idx) || stop;
      }

      // the row yEnd does NOT belong to the box -> last row = yEnd - 1
      // if lower line of the rectangle is visible
      if (yEnd > 0 && yEnd <= static_cast<int> (input_->height))
      {
        int idx   = (yEnd - 1) * input_->width + xFrom;
        int idxTo = idx + xTo - xFrom;

        for (; idx < idxTo; ++idx)
          stop = testPoint (query, k, results, idx) || stop;
      }

      // skip first row and last row (already handled above)
      int yFrom = yBegin + 1;
      int yTo   = yEnd - 1;
      clipRange (yFrom, yTo, 0, input_->height);

      // if we have lines in between that are also visible
      if (yFrom < yTo)
      {
        if (xBegin >= 0 && xBegin < static_cast<int> (input_->width))
        {
          int idx   = yFrom * input_->width + xBegin;
          int idxTo = yTo * input_->width + xBegin;

          for (; idx < idxTo; idx += input_->width)
            stop = testPoint (query, k, results, idx) || stop;
        }

        if (xEnd > 0 && xEnd <= static_cast<int> (input_->width))
        {
          int idx   = yFrom * input_->width + xEnd - 1;
          int idxTo = yTo * input_->width + xEnd - 1;

          for (; idx < idxTo; idx += input_->width)
            stop = testPoint (query, k, results, idx) || stop;
        }

      }
      // stop here means that the k-nearest neighbor changed -> recalculate bounding box of ellipse.
      if (stop)
        getProjectedRadiusSearchBox (query, results.top ().distance, left, right, top, bottom);

    }
    // now we use it as stop flag -> if bounding box is completely within the already examined search box were done!
    stop = (static_cast<int> (left)   >= xBegin && static_cast<int> (left)   < xEnd &&
            static_cast<int> (right)  >= xBegin && static_cast<int> (right)  < xEnd &&
            static_cast<int> (top)    >= yBegin && static_cast<int> (top)    < yEnd &&
            static_cast<int> (bottom) >= yBegin && static_cast<int> (bottom) < yEnd);

  } while (!stop);

  k_indices.resize (results.size ());
  k_sqr_distances.resize (results.size ());
  size_t idx = results.size () - 1;
  while (!results.empty ())
  {
    k_indices [idx] = results.top ().index;
    k_sqr_distances [idx] = results.top ().distance;
    results.pop ();
    --idx;
  }

  return (static_cast<int> (k_indices.size ()));
}

////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> void
pcl::search::OrganizedNeighbor<PointT>::getProjectedRadiusSearchBox (const PointT& point,
                                                                     float squared_radius,
                                                                     unsigned &minX,
                                                                     unsigned &maxX,
                                                                     unsigned &minY,
                                                                     unsigned &maxY) const
{
  Eigen::Vector3f q = KR_ * point.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);

  float a = squared_radius * KR_KRT_.coeff (8) - q [2] * q [2];
  float b = squared_radius * KR_KRT_.coeff (7) - q [1] * q [2];
  float c = squared_radius * KR_KRT_.coeff (4) - q [1] * q [1];
  int min, max;
  // a and c are multiplied by two already => - 4ac -> - ac
  float det = b * b - a * c;
  if (det < 0)
  {
    minY = 0;
    maxY = input_->height - 1;
  }
  else
  {
    float y1 = (b - sqrt (det)) / a;
    float y2 = (b + sqrt (det)) / a;

    min = std::min (static_cast<int> (floor (y1)), static_cast<int> (floor (y2)));
    max = std::max (static_cast<int> (ceil (y1)), static_cast<int> (ceil (y2)));
    minY = static_cast<unsigned> (std::min (static_cast<int> (input_->height) - 1, std::max (0, min)));
    maxY = static_cast<unsigned> (std::max (std::min (static_cast<int> (input_->height) - 1, max), 0));
  }

  b = squared_radius * KR_KRT_.coeff (6) - q [0] * q [2];
  c = squared_radius * KR_KRT_.coeff (0) - q [0] * q [0];

  det = b * b - a * c;
  if (det < 0)
  {
    minX = 0;
    maxX = input_->width - 1;
  }
  else
  {
    float x1 = (b - sqrt (det)) / a;
    float x2 = (b + sqrt (det)) / a;

    min = std::min (static_cast<int> (floor (x1)), static_cast<int> (floor (x2)));
    max = std::max (static_cast<int> (ceil (x1)), static_cast<int> (ceil (x2)));
    minX = static_cast<unsigned> (std::min (static_cast<int> (input_->width)- 1, std::max (0, min)));
    maxX = static_cast<unsigned> (std::max (std::min (static_cast<int> (input_->width) - 1, max), 0));
  }
}

//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> void
pcl::search::OrganizedNeighbor<PointT>::computeCameraMatrix (Eigen::Matrix3f& camera_matrix) const
{
  pcl::getCameraMatrixFromProjectionMatrix (projection_matrix_, camera_matrix);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> void
pcl::search::OrganizedNeighbor<PointT>::estimateProjectionMatrix ()
{
  // internally we calculate with double but store the result into float matrices.
  typedef double Scalar;
  projection_matrix_.setZero ();
  if (input_->height == 1 || input_->width == 1)
  {
    PCL_ERROR ("[pcl::%s::estimateProjectionMatrix] Input dataset is not organized!\n", this->getName ().c_str ());
    return;
  }

  const unsigned ySkip = (input_->height >> pyramid_level_);
  const unsigned xSkip = (input_->width >> pyramid_level_);

  std::vector<int> indices;
  indices.reserve (input_->size () >> (pyramid_level_ << 1));

  for (unsigned yIdx = 0, idx = 0; yIdx < input_->height; yIdx += ySkip, idx += input_->width * (ySkip - 1))
  {
    for (unsigned xIdx = 0; xIdx < input_->width; xIdx += xSkip, idx += xSkip)
    {
      if (!mask_ [idx])
        continue;

      indices.push_back (idx);
    }
  }

  double residual_sqr = pcl::estimateProjectionMatrix<PointT> (input_, projection_matrix_, indices);

  if ( fabs (residual_sqr) > eps_ * indices.size ())
  {
    PCL_ERROR ("[pcl::%s::radiusSearch] Input dataset is not from a projective device!\nResidual (MSE) %f, using %d valid points\n", this->getName ().c_str (), residual_sqr / double (indices.size()), indices.size ());
    return;
  }

  // get left 3x3 sub matrix, which contains K * R, with K = camera matrix = [[fx s cx] [0 fy cy] [0 0 1]]
  // and R being the rotation matrix
  KR_ = projection_matrix_.topLeftCorner <3, 3> ();

  // precalculate KR * KR^T needed by calculations during nn-search
  KR_KRT_ = KR_ * KR_.transpose ();
}

//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> bool
pcl::search::OrganizedNeighbor<PointT>::projectPoint (const PointT& point, pcl::PointXY& q) const
{
  Eigen::Vector3f projected = KR_ * point.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);
  q.x = projected [0] / projected [2];
  q.y = projected [1] / projected [2];
  return (projected[2] != 0);
}
#define PCL_INSTANTIATE_OrganizedNeighbor(T) template class PCL_EXPORTS pcl::search::OrganizedNeighbor<T>;

#endif
<commit_msg>fixed a compiler warning<commit_after>/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2011, Willow Garage, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id$
 *
 */

#ifndef PCL_SEARCH_IMPL_ORGANIZED_NEIGHBOR_SEARCH_H_
#define PCL_SEARCH_IMPL_ORGANIZED_NEIGHBOR_SEARCH_H_

#include <pcl/search/organized.h>
#include <pcl/common/eigen.h>
#include <pcl/common/time.h>
#include <Eigen/Eigenvalues>

//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> int
pcl::search::OrganizedNeighbor<PointT>::radiusSearch (const               PointT &query,
                                                      const double        radius,
                                                      std::vector<int>    &k_indices,
                                                      std::vector<float>  &k_sqr_distances,
                                                      unsigned int        max_nn) const
{
  // NAN test
  assert (isFinite (query) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");

  // search window
  unsigned left, right, top, bottom;
  //unsigned x, y, idx;
  float squared_distance;
  double squared_radius;

  k_indices.clear ();
  k_sqr_distances.clear ();

  squared_radius = radius * radius;

  this->getProjectedRadiusSearchBox (query, static_cast<float> (squared_radius), left, right, top, bottom);

  // iterate over search box
  if (max_nn == 0 || max_nn >= static_cast<unsigned int> (input_->points.size ()))
    max_nn = static_cast<unsigned int> (input_->points.size ());

  k_indices.reserve (max_nn);
  k_sqr_distances.reserve (max_nn);

  unsigned yEnd  = (bottom + 1) * input_->width + right + 1;
  register unsigned idx  = top * input_->width + left;
  unsigned skip = input_->width - right + left - 1;
  unsigned xEnd = idx - left + right + 1;

  for (; xEnd != yEnd; idx += skip, xEnd += input_->width)
  {
    for (; idx < xEnd; ++idx)
    {
      if (!mask_[idx] || !isFinite (input_->points[idx]))
        continue;

      squared_distance = (input_->points[idx].getVector3fMap () - query.getVector3fMap ()).squaredNorm ();
      if (squared_distance <= squared_radius)
      {
        k_indices.push_back (idx);
        k_sqr_distances.push_back (squared_distance);
        // already done ?
        if (k_indices.size () == max_nn)
        {
          if (sorted_results_)
            this->sortResults (k_indices, k_sqr_distances);
          return (max_nn);
        }
      }
    }
  }
  if (sorted_results_)
    this->sortResults (k_indices, k_sqr_distances);
  return (static_cast<int> (k_indices.size ()));
}

//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> int
pcl::search::OrganizedNeighbor<PointT>::nearestKSearch (const PointT &query,
                                                        int k,
                                                        std::vector<int> &k_indices,
                                                        std::vector<float> &k_sqr_distances) const
{
  assert (isFinite (query) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
  if (k < 1)
  {
    k_indices.clear ();
    k_sqr_distances.clear ();
    return (0);
  }

  // project query point on the image plane
  Eigen::Vector3f q = KR_ * query.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);
  int xBegin = int(q [0] / q [2] + 0.5f);
  int yBegin = int(q [1] / q [2] + 0.5f);
  int xEnd   = xBegin + 1; // end is the pixel that is not used anymore, like in iterators
  int yEnd   = yBegin + 1;

  // the search window. This is supposed to shrink within the iterations
  unsigned left = 0;
  unsigned right = input_->width - 1;
  unsigned top = 0;
  unsigned bottom = input_->height - 1;

  std::priority_queue <Entry> results;
  //std::vector<Entry> k_results;
  //k_results.reserve (k);
  // add point laying on the projection of the query point.
  if (xBegin >= 0 &&
      xBegin < static_cast<int> (input_->width) &&
      yBegin >= 0 &&
      yBegin < static_cast<int> (input_->height))
    testPoint (query, k, results, yBegin * input_->width + xBegin);
  else // point lys
  {
    // find the box that touches the image border -> dont waste time evaluating boxes that are completely outside the image!
    int dist = std::numeric_limits<int>::max ();

    if (xBegin < 0)
      dist = -xBegin;
    else if (xBegin >= static_cast<int> (input_->width))
      dist = xBegin - static_cast<int> (input_->width);

    if (yBegin < 0)
      dist = std::min (dist, -yBegin);
    else if (yBegin >= static_cast<int> (input_->height))
      dist = std::min (dist, yBegin - static_cast<int> (input_->height));

    xBegin -= dist;
    xEnd   += dist;

    yBegin -= dist;
    yEnd   += dist;
  }

  // stop used as isChanged as well as stop.
  bool stop = false;
  do
  {
    // increment box size
    --xBegin;
    ++xEnd;
    --yBegin;
    ++yEnd;

    // the range in x-direction which intersects with the image width
    int xFrom = xBegin;
    int xTo   = xEnd;
    clipRange (xFrom, xTo, 0, input_->width);

    // if x-extend is not 0
    if (xTo > xFrom)
    {
      // if upper line of the rectangle is visible and x-extend is not 0
      if (yBegin >= 0 && yBegin < static_cast<int> (input_->height))
      {
        int idx   = yBegin * input_->width + xFrom;
        int idxTo = idx + xTo - xFrom;
        for (; idx < idxTo; ++idx)
          stop = testPoint (query, k, results, idx) || stop;
      }

      // the row yEnd does NOT belong to the box -> last row = yEnd - 1
      // if lower line of the rectangle is visible
      if (yEnd > 0 && yEnd <= static_cast<int> (input_->height))
      {
        int idx   = (yEnd - 1) * input_->width + xFrom;
        int idxTo = idx + xTo - xFrom;

        for (; idx < idxTo; ++idx)
          stop = testPoint (query, k, results, idx) || stop;
      }

      // skip first row and last row (already handled above)
      int yFrom = yBegin + 1;
      int yTo   = yEnd - 1;
      clipRange (yFrom, yTo, 0, input_->height);

      // if we have lines in between that are also visible
      if (yFrom < yTo)
      {
        if (xBegin >= 0 && xBegin < static_cast<int> (input_->width))
        {
          int idx   = yFrom * input_->width + xBegin;
          int idxTo = yTo * input_->width + xBegin;

          for (; idx < idxTo; idx += input_->width)
            stop = testPoint (query, k, results, idx) || stop;
        }

        if (xEnd > 0 && xEnd <= static_cast<int> (input_->width))
        {
          int idx   = yFrom * input_->width + xEnd - 1;
          int idxTo = yTo * input_->width + xEnd - 1;

          for (; idx < idxTo; idx += input_->width)
            stop = testPoint (query, k, results, idx) || stop;
        }

      }
      // stop here means that the k-nearest neighbor changed -> recalculate bounding box of ellipse.
      if (stop)
        getProjectedRadiusSearchBox (query, results.top ().distance, left, right, top, bottom);

    }
    // now we use it as stop flag -> if bounding box is completely within the already examined search box were done!
    stop = (static_cast<int> (left)   >= xBegin && static_cast<int> (left)   < xEnd &&
            static_cast<int> (right)  >= xBegin && static_cast<int> (right)  < xEnd &&
            static_cast<int> (top)    >= yBegin && static_cast<int> (top)    < yEnd &&
            static_cast<int> (bottom) >= yBegin && static_cast<int> (bottom) < yEnd);

  } while (!stop);

  k_indices.resize (results.size ());
  k_sqr_distances.resize (results.size ());
  size_t idx = results.size () - 1;
  while (!results.empty ())
  {
    k_indices [idx] = results.top ().index;
    k_sqr_distances [idx] = results.top ().distance;
    results.pop ();
    --idx;
  }

  return (static_cast<int> (k_indices.size ()));
}

////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> void
pcl::search::OrganizedNeighbor<PointT>::getProjectedRadiusSearchBox (const PointT& point,
                                                                     float squared_radius,
                                                                     unsigned &minX,
                                                                     unsigned &maxX,
                                                                     unsigned &minY,
                                                                     unsigned &maxY) const
{
  Eigen::Vector3f q = KR_ * point.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);

  float a = squared_radius * KR_KRT_.coeff (8) - q [2] * q [2];
  float b = squared_radius * KR_KRT_.coeff (7) - q [1] * q [2];
  float c = squared_radius * KR_KRT_.coeff (4) - q [1] * q [1];
  int min, max;
  // a and c are multiplied by two already => - 4ac -> - ac
  float det = b * b - a * c;
  if (det < 0)
  {
    minY = 0;
    maxY = input_->height - 1;
  }
  else
  {
    float y1 = (b - sqrt (det)) / a;
    float y2 = (b + sqrt (det)) / a;

    min = std::min (static_cast<int> (floor (y1)), static_cast<int> (floor (y2)));
    max = std::max (static_cast<int> (ceil (y1)), static_cast<int> (ceil (y2)));
    minY = static_cast<unsigned> (std::min (static_cast<int> (input_->height) - 1, std::max (0, min)));
    maxY = static_cast<unsigned> (std::max (std::min (static_cast<int> (input_->height) - 1, max), 0));
  }

  b = squared_radius * KR_KRT_.coeff (6) - q [0] * q [2];
  c = squared_radius * KR_KRT_.coeff (0) - q [0] * q [0];

  det = b * b - a * c;
  if (det < 0)
  {
    minX = 0;
    maxX = input_->width - 1;
  }
  else
  {
    float x1 = (b - sqrt (det)) / a;
    float x2 = (b + sqrt (det)) / a;

    min = std::min (static_cast<int> (floor (x1)), static_cast<int> (floor (x2)));
    max = std::max (static_cast<int> (ceil (x1)), static_cast<int> (ceil (x2)));
    minX = static_cast<unsigned> (std::min (static_cast<int> (input_->width)- 1, std::max (0, min)));
    maxX = static_cast<unsigned> (std::max (std::min (static_cast<int> (input_->width) - 1, max), 0));
  }
}

//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> void
pcl::search::OrganizedNeighbor<PointT>::computeCameraMatrix (Eigen::Matrix3f& camera_matrix) const
{
  pcl::getCameraMatrixFromProjectionMatrix (projection_matrix_, camera_matrix);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> void
pcl::search::OrganizedNeighbor<PointT>::estimateProjectionMatrix ()
{
  // internally we calculate with double but store the result into float matrices.
  typedef double Scalar;
  projection_matrix_.setZero ();
  if (input_->height == 1 || input_->width == 1)
  {
    PCL_ERROR ("[pcl::%s::estimateProjectionMatrix] Input dataset is not organized!\n", this->getName ().c_str ());
    return;
  }

  const unsigned ySkip = (input_->height >> pyramid_level_);
  const unsigned xSkip = (input_->width >> pyramid_level_);

  std::vector<int> indices;
  indices.reserve (input_->size () >> (pyramid_level_ << 1));

  for (unsigned yIdx = 0, idx = 0; yIdx < input_->height; yIdx += ySkip, idx += input_->width * (ySkip - 1))
  {
    for (unsigned xIdx = 0; xIdx < input_->width; xIdx += xSkip, idx += xSkip)
    {
      if (!mask_ [idx])
        continue;

      indices.push_back (idx);
    }
  }

  double residual_sqr = pcl::estimateProjectionMatrix<PointT> (input_, projection_matrix_, indices);

  if (fabs (residual_sqr) > eps_ * float (indices.size ()))
  {
    PCL_ERROR ("[pcl::%s::radiusSearch] Input dataset is not from a projective device!\nResidual (MSE) %f, using %d valid points\n", this->getName ().c_str (), residual_sqr / double (indices.size()), indices.size ());
    return;
  }

  // get left 3x3 sub matrix, which contains K * R, with K = camera matrix = [[fx s cx] [0 fy cy] [0 0 1]]
  // and R being the rotation matrix
  KR_ = projection_matrix_.topLeftCorner <3, 3> ();

  // precalculate KR * KR^T needed by calculations during nn-search
  KR_KRT_ = KR_ * KR_.transpose ();
}

//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> bool
pcl::search::OrganizedNeighbor<PointT>::projectPoint (const PointT& point, pcl::PointXY& q) const
{
  Eigen::Vector3f projected = KR_ * point.getVector3fMap () + projection_matrix_.block <3, 1> (0, 3);
  q.x = projected [0] / projected [2];
  q.y = projected [1] / projected [2];
  return (projected[2] != 0);
}
#define PCL_INSTANTIATE_OrganizedNeighbor(T) template class PCL_EXPORTS pcl::search::OrganizedNeighbor<T>;

#endif
<|endoftext|>


<commit_before>/*
 * The Apache Software License, Version 1.1
 *
 *
 * Copyright (c) 1999 The Apache Software Foundation.  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. The end-user documentation included with the redistribution,
 *    if any, must include the following acknowledgment:
 *       "This product includes software developed by the
 *        Apache Software Foundation (http://www.apache.org/)."
 *    Alternately, this acknowledgment may appear in the software itself,
 *    if and wherever such third-party acknowledgments normally appear.
 *
 * 4. The names "Xalan" and "Apache Software Foundation" must
 *    not be used to endorse or promote products derived from this
 *    software without prior written permission. For written
 *    permission, please contact apache@apache.org.
 *
 * 5. Products derived from this software may not be called "Apache",
 *    nor may "Apache" appear in their name, without prior written
 *    permission of the Apache Software Foundation.
 *
 * THIS SOFTWARE IS PROVIDED ``AS IS'' AND ANY EXPRESSED 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 APACHE SOFTWARE FOUNDATION OR
 * ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 * ====================================================================
 *
 * This software consists of voluntary contributions made by many
 * individuals on behalf of the Apache Software Foundation and was
 * originally based on software copyright (c) 1999, International
 * Business Machines, Inc., http://www.ibm.com.  For more
 * information on the Apache Software Foundation, please see
 * <http://www.apache.org/>.
 */
#if !defined(FUNCTIONDOC_HEADER_GUARD_1357924680)
#define FUNCTIONDOC_HEADER_GUARD_1357924680

// Base header file.  Must be first.
#include <XSLT/XSLTDefinitions.hpp>

#include <vector>

#include <PlatformSupport/DOMStringHelper.hpp>

// Base class header file...
#include <XPath/Function.hpp>

/**
 * XPath implementation of "document" function.
 */
//
// These are all inline, even though
// there are virtual functions, because we expect that they will only be
// needed by the XSLT class.
class XALAN_XSLT_EXPORT FunctionDoc : public Function
{
public:

	// These methods are inherited from Function ...

	virtual XObject*
	execute(
			XPathExecutionContext&			executionContext,
			const DOM_Node&					context,
			int								/* opPos */,
			const XObjectArgVectorType&		args)
	{
		executionContext.error("Document() function implementation has been replaced by xslt/FunctionDocument!",
							   context);

		return executionContext.getXObjectFactory().createNull();
	}

#if defined(XALAN_NO_COVARIANT_RETURN_TYPE)
	virtual Function*
#else
	virtual FunctionDoc*
#endif
	clone() const
	{
		return new FunctionDoc(*this);
	}

private:

	// Not implemented...
	FunctionDoc&
	operator=(const FunctionDoc&);

	bool
	operator==(const FunctionDoc&) const;
};

#endif	// FUNCTIONDOC_HEADER_GUARD_1357924680
<commit_msg>Obsolete file.<commit_after><|endoftext|>


<commit_before>
#include <iomanip>
#include <iostream>
#include <random>

#include <mxpfit/esprit.hpp>
#include <mxpfit/fast_esprit.hpp>

#include "constants.hpp"
#include "timer.hpp"

using Index   = Eigen::Index;
using Real    = double;
using Complex = std::complex<Real>;

using RealArray      = Eigen::Array<Real, Eigen::Dynamic, 1>;
using ComplexArray   = Eigen::Array<Complex, Eigen::Dynamic, 1>;
using ExponentialSum = mxpfit::ExponentialSum<Complex, Complex>;

constexpr static const Real pi = math::constant<Real>::pi;

void print_exponential_sum(const ExponentialSum& ret)
{
    std::cout << "# Parameters: real(a[i]), imag(a[i]), real(w[i]), "
                 "imag(w[i]), [n = "
              << ret.size() << "]\n";
    for (Index i = 0; i < ret.size(); ++i)
    {
        const auto ai = ret.exponent(i);
        const auto wi = ret.weight(i);

        std::cout << std::setw(24) << std::real(ai) << '\t'  // real part
                  << std::setw(24) << std::imag(ai) << '\t'  // imaginary part
                  << std::setw(24) << std::real(wi) << '\t'  // real part
                  << std::setw(24) << std::imag(wi) << '\n'; // imaginary part
    }

    std::cout << std::endl;
}

void fit(Index N, Real eps, const ExponentialSum& orig, Real noise_magnitude)
{
    const Index L    = N / 2;
    const Index M    = orig.size(); // upper bound of # of terms
    const auto delta = Real(1);     // step width is unity

    std::random_device seed_gen;
    std::mt19937 rnd(seed_gen());
    std::uniform_real_distribution<Real> noise(-noise_magnitude,
                                               noise_magnitude);
    // Prepare sampling data
    // ComplexArray h_exact(N); // data without noise
    // ComplexArray h(N);
    // for (Index i = 0; i < N; ++i)
    // {
    //     const auto val = orig(Real(i));
    //     h_exact(i)     = val;
    //     h(i)           = val + noise(rnd);
    // }
    RealArray h_exact(N); // data without noise
    RealArray h(N);
    for (Index i = 0; i < N; ++i)
    {
        const auto val = std::real(orig(Real(i)));
        h_exact(i)     = val;
        h(i)           = val + noise(rnd);
    }

    {
        std::cout << "# --- Fit by ESPRIT (N = " << N << ", L = " << L
                  << ", M = " << M << ", eps = " << eps << ")\n";
        Timer time;
        mxpfit::ESPRIT<Complex> esprit(N, L, M);
        ExponentialSum ret = esprit.compute(h.matrix(), Real(), delta, eps);

        std::cout << "# --- done (elapsed time: " << time.elapsed().count()
                  << " us)\n";

        print_exponential_sum(ret);
    }
    {
        std::cout << "# --- Fit by FastESPRIT (N = " << N << ", L = " << L
                  << ", M = " << M << ", eps = " << eps << ")\n";
        Timer time;
        mxpfit::FastESPRIT<Real> esprit(N, L, 2 * M);
        ExponentialSum ret = esprit.compute(h.matrix(), Real(), delta, eps);

        std::cout << "# --- done (elapsed time: " << time.elapsed().count()
                  << " us)\n";

        print_exponential_sum(ret);
    }
}

//-----------------------------------------------------------------------------
// Main
//-----------------------------------------------------------------------------
int main()
{
    std::cout.precision(15);
    std::cout.setf(std::ios::scientific);

    ExponentialSum orig(5);
    orig.exponent(0) = Complex();
    orig.exponent(1) = Complex(Real(), pi / 4);
    orig.exponent(2) = Complex(Real(), -pi / 4);
    orig.exponent(3) = Complex(Real(), pi / 2);
    orig.exponent(4) = Complex(Real(), -pi / 2);

    orig.weight(0) = Complex(34);
    orig.weight(1) = Complex(300);
    orig.weight(2) = Complex(300);
    orig.weight(3) = Complex(1);
    orig.weight(4) = Complex(1);

    std::cout << "# Exact exponential sum\n";
    print_exponential_sum(orig);

    // const Index nsamples[] = {1 << 8,  1 << 9,  1 << 10, 1 << 11,
    //                           1 << 12, 1 << 13, 1 << 14, 1 << 15,
    //                           1 << 16, 1 << 17, 1 << 18};
    const Index N              = 1024;
    const Real eps             = 1.0e-7;
    const Real noise_magnitude = 3.0;

    fit(N, eps, orig, noise_magnitude);

    return 0;
}
<commit_msg>Compare ESPRIT and FastESPRIT<commit_after>#include <iomanip>
#include <iostream>
#include <random>

#include <mxpfit/esprit.hpp>
#include <mxpfit/fast_esprit.hpp>

#include "constants.hpp"
#include "timer.hpp"

using Index   = Eigen::Index;
using Real    = double;
using Complex = std::complex<Real>;

using RealArray      = Eigen::Array<Real, Eigen::Dynamic, 1>;
using ComplexArray   = Eigen::Array<Complex, Eigen::Dynamic, 1>;
using ExponentialSum = mxpfit::ExponentialSum<Complex, Complex>;

constexpr static const Real pi = math::constant<Real>::pi;

void print_exponential_sum(const ExponentialSum& ret)
{
    std::cout << "# Parameters: real(a[i]), imag(a[i]), real(w[i]), "
                 "imag(w[i]), [n = "
              << ret.size() << "]\n";
    for (Index i = 0; i < ret.size(); ++i)
    {
        const auto ai = ret.exponent(i);
        const auto wi = ret.weight(i);

        std::cout << std::setw(24) << std::real(ai) << '\t'  // real part
                  << std::setw(24) << std::imag(ai) << '\t'  // imaginary part
                  << std::setw(24) << std::real(wi) << '\t'  // real part
                  << std::setw(24) << std::imag(wi) << '\n'; // imaginary part
    }

    std::cout << std::endl;
}

//
// Make sequence of sampling data h[k] = fn(k) + e(k) for k = 0,1,2...,N
// where e(k) is random noise uniformly distributed in
// -noise_magnitude <= e(k) <= noise_magnitude
//
template <typename Functor>
void make_sampling_data(Functor fn, Real noise_magnitude,
                        Eigen::Ref<RealArray> h_exact,
                        Eigen::Ref<RealArray> h_noise)
{
    assert(h_exact.size() == h_noise.size());

    const Index n_samples = h_noise.size();
    std::random_device seed_gen;
    std::mt19937 rnd(seed_gen());
    std::uniform_real_distribution<Real> noise(-noise_magnitude,
                                               noise_magnitude);

    for (Index i = 0; i < n_samples; ++i)
    {
        const auto val = fn(Real(i));
        h_exact(i)     = val;
        h_noise(i)     = val + noise(rnd);
    }
}

//
// Fit sampling data by original ESPRIT algorithm
//
std::pair<ExponentialSum, std::chrono::microseconds>
fit_by_original_esprit(const Eigen::Ref<const RealArray>& h, Index n_terms,
                       Real eps)
{
    const Index N    = h.size(); // number of sampling data
    const Index L    = N / 2;    // window size of Hankel matrix
    const auto delta = Real(1);  // step width is unity

    Timer time;
    mxpfit::ESPRIT<Complex> esprit(N, L, n_terms);
    ExponentialSum ret = esprit.compute(h.matrix(), Real(), delta, eps);
    return {ret, time.elapsed()};
}

//
// Fit sampling data by fast ESPRIT algorithm
//
std::pair<ExponentialSum, std::chrono::microseconds>
fit_by_fast_esprit(const Eigen::Ref<const RealArray>& h, Index n_terms,
                   Real eps)
{
    const Index N    = h.size(); // number of sampling data
    const Index L    = N / 2;    // window size of Hankel matrix
    const auto delta = Real(1);  // step width is unity

    Timer time;
    mxpfit::FastESPRIT<Real> esprit(N, L, n_terms);
    ExponentialSum ret = esprit.compute(h.matrix(), Real(), delta, eps);

    return {ret, time.elapsed()};
}

//-----------------------------------------------------------------------------
// Main
//-----------------------------------------------------------------------------
int main()
{
    std::cout.precision(15);
    std::cout.setf(std::ios::scientific);

    ExponentialSum orig(5);
    orig.exponent(0) = Complex();
    orig.exponent(1) = Complex(Real(), pi / 4);
    orig.exponent(2) = Complex(Real(), -pi / 4);
    orig.exponent(3) = Complex(Real(), pi / 2);
    orig.exponent(4) = Complex(Real(), -pi / 2);

    orig.weight(0) = Complex(34);
    orig.weight(1) = Complex(300);
    orig.weight(2) = Complex(300);
    orig.weight(3) = Complex(1);
    orig.weight(4) = Complex(1);

    const Index nsamples[] = {128, 256, 512, 1024, 2048};

    const Index n_terms        = orig.size();
    const Index n_trial        = orig.size();
    const Real eps             = 1.0e-5;
    const Real noise_magnitude = 3.0;

    std::cout << "# Exact exponential sum\n";
    print_exponential_sum(orig);

    for (auto N : nsamples)
    {
        RealArray h_exact(N);
        RealArray h_noise(N);
        ExponentialSum ret;
        auto total_time_orig = std::chrono::microseconds();
        auto total_time_fast = std::chrono::microseconds();
        auto elapsed         = std::chrono::microseconds();

        std::cout << "# Fitting by ESPRIT N = " << N << ", L = " << N / 2
                  << ", M = " << n_terms << ", eps = " << eps << ")"
                  << std::endl;

        for (Index iter = 0; iter < n_trial; ++iter)
        {
            make_sampling_data([&](Real x) { return std::real(orig(x)); },
                               noise_magnitude, h_exact, h_noise);

            std::cout << "# --- Trial " << iter + 1 << '\n';
            std::tie(ret, elapsed) =
                fit_by_original_esprit(h_noise, n_terms, eps);
            total_time_orig += elapsed;
            std::cout << "#     Original ESPRIT (elapsed time: " << elapsed.count()
                      << " microseconds)\n";
            print_exponential_sum(ret);

            std::tie(ret, elapsed) = fit_by_fast_esprit(h_noise, n_terms, eps);
            total_time_fast += elapsed;
            std::cout << "#     Fast ESPRIT     (elapsed time: " << elapsed.count()
                      << " microseconds)\n";
            print_exponential_sum(ret);
            std::cout << std::endl;
        }

        std::cout << "#$ Averaged running time:\n"
                  << "   [original]: " << total_time_orig.count() / n_trial
                  << " microseconds\n"
                  << "   [fast]:     " << total_time_fast.count() / n_trial
                  << " microseconds\n"
                  << std::endl;
    }

    return 0;
}
<|endoftext|>


<commit_before>/**************************************************************************
**
** This file is part of Qt Creator
**
** Copyright (c) 2010 Nokia Corporation and/or its subsidiary(-ies).
**
** Contact: Nokia Corporation (qt-info@nokia.com)
**
** Commercial Usage
**
** Licensees holding valid Qt Commercial licenses may use this file in
** accordance with the Qt Commercial License Agreement provided with the
** Software or, alternatively, in accordance with the terms contained in
** a written agreement between you and Nokia.
**
** GNU Lesser General Public License Usage
**
** Alternatively, this file may be used under the terms of the GNU Lesser
** General Public License version 2.1 as published by the Free Software
** Foundation and appearing in the file LICENSE.LGPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU Lesser General Public License version 2.1 requirements
** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** If you are unsure which license is appropriate for your use, please
** contact the sales department at http://qt.nokia.com/contact.
**
**************************************************************************/

#include "Preprocessor.h"
#include <AST.h>
#include <ASTVisitor.h>
#include <Control.h>
#include <Scope.h>
#include <TranslationUnit.h>
#include <Literals.h>
#include <Symbols.h>
#include <Names.h>
#include <CoreTypes.h>
#include <Bind.h>

#include <string>
#include <cstdlib>
#include <sstream>

using namespace CPlusPlus;

enum { BLOCK_SIZE = 4 * 1024};

void parse(const char *fileName, const char *source, unsigned size);
int runWithSystemPreprocessor(int argc, char *argv[]);
int runWithNewPreprocessor(int argc, char *argv[]);

int main(int argc, char *argv[])
{
    if (getenv("CPLUSPLUS_WITH_NEW_PREPROCESSOR"))
        return runWithNewPreprocessor(argc, argv);

    return runWithSystemPreprocessor(argc, argv);
}

int runWithSystemPreprocessor(int argc, char *argv[])
{
    std::string cmdline;
    cmdline += "gcc -E -xc++ -U__BLOCKS__ -D__restrict= -D__extension__=";

    for (int i = 1; i < argc; ++i) {
        cmdline += ' ';
        cmdline += argv[i];
    }

    char block[BLOCK_SIZE];

    std::string preprocessedCode;

    if (FILE *fp = popen(cmdline.c_str(), "r")) {
        while (size_t sz = fread(block, 1, BLOCK_SIZE, fp))
            preprocessedCode.append(block, sz);

        pclose(fp);

    } else {
        fprintf(stderr, "c++: No such file or directory\n");
        return EXIT_FAILURE;
    }

    parse("<stdin>", preprocessedCode.c_str(), preprocessedCode.size());
    return EXIT_SUCCESS;
}

int runWithNewPreprocessor(int argc, char *argv[])
{
    if (argc == 1) {
        fprintf(stderr, "c++: No such file or directory\n");
        return EXIT_FAILURE;
    }

    char block[BLOCK_SIZE];

    std::string source;

    if (FILE *fp = fopen(argv[1], "r")) {
        while (size_t sz = fread(block, 1, BLOCK_SIZE, fp))
            source.append(block, sz);

        fclose(fp);

    } else {
        fprintf(stderr, "c++: No such file or directory\n");
        return EXIT_FAILURE;
    }

    std::ostringstream out;
    Preprocessor pp(out);
    pp(source.c_str(), source.size(), StringRef(argv[1]));

    const std::string preprocessedCode = out.str();
    parse(argv[1], preprocessedCode.c_str(), preprocessedCode.size());
    return EXIT_SUCCESS;
}

void parse(const char *fileName, const char *source, unsigned size)
{
    Control control;
    TranslationUnit unit(&control, control.stringLiteral(fileName));
    unit.setSource(source, size);
    unit.parse();

    Namespace *globalNamespace = control.newNamespace(0);
    Bind bind(&unit);
    bind(unit.ast()->asTranslationUnit(), globalNamespace);
}
<commit_msg>Include CPlusPlus.h<commit_after>/**************************************************************************
**
** This file is part of Qt Creator
**
** Copyright (c) 2010 Nokia Corporation and/or its subsidiary(-ies).
**
** Contact: Nokia Corporation (qt-info@nokia.com)
**
** Commercial Usage
**
** Licensees holding valid Qt Commercial licenses may use this file in
** accordance with the Qt Commercial License Agreement provided with the
** Software or, alternatively, in accordance with the terms contained in
** a written agreement between you and Nokia.
**
** GNU Lesser General Public License Usage
**
** Alternatively, this file may be used under the terms of the GNU Lesser
** General Public License version 2.1 as published by the Free Software
** Foundation and appearing in the file LICENSE.LGPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU Lesser General Public License version 2.1 requirements
** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** If you are unsure which license is appropriate for your use, please
** contact the sales department at http://qt.nokia.com/contact.
**
**************************************************************************/

#include "Preprocessor.h"
#include <CPlusPlus.h>

#include <string>
#include <cstdlib>
#include <sstream>

using namespace CPlusPlus;

enum { BLOCK_SIZE = 4 * 1024};

void parse(const char *fileName, const char *source, unsigned size);
int runWithSystemPreprocessor(int argc, char *argv[]);
int runWithNewPreprocessor(int argc, char *argv[]);

int main(int argc, char *argv[])
{
    if (getenv("CPLUSPLUS_WITH_NEW_PREPROCESSOR"))
        return runWithNewPreprocessor(argc, argv);

    return runWithSystemPreprocessor(argc, argv);
}

int runWithSystemPreprocessor(int argc, char *argv[])
{
    std::string cmdline;
    cmdline += "gcc -E -xc++ -U__BLOCKS__ -D__restrict= -D__extension__=";

    for (int i = 1; i < argc; ++i) {
        cmdline += ' ';
        cmdline += argv[i];
    }

    char block[BLOCK_SIZE];

    std::string preprocessedCode;

    if (FILE *fp = popen(cmdline.c_str(), "r")) {
        while (size_t sz = fread(block, 1, BLOCK_SIZE, fp))
            preprocessedCode.append(block, sz);

        pclose(fp);

    } else {
        fprintf(stderr, "c++: No such file or directory\n");
        return EXIT_FAILURE;
    }

    parse("<stdin>", preprocessedCode.c_str(), preprocessedCode.size());
    return EXIT_SUCCESS;
}

int runWithNewPreprocessor(int argc, char *argv[])
{
    if (argc == 1) {
        fprintf(stderr, "c++: No such file or directory\n");
        return EXIT_FAILURE;
    }

    char block[BLOCK_SIZE];

    std::string source;

    if (FILE *fp = fopen(argv[1], "r")) {
        while (size_t sz = fread(block, 1, BLOCK_SIZE, fp))
            source.append(block, sz);

        fclose(fp);

    } else {
        fprintf(stderr, "c++: No such file or directory\n");
        return EXIT_FAILURE;
    }

    std::ostringstream out;
    Preprocessor pp(out);
    pp(source.c_str(), source.size(), StringRef(argv[1]));

    const std::string preprocessedCode = out.str();
    parse(argv[1], preprocessedCode.c_str(), preprocessedCode.size());
    return EXIT_SUCCESS;
}

void parse(const char *fileName, const char *source, unsigned size)
{
    Control control;
    TranslationUnit unit(&control, control.stringLiteral(fileName));
    unit.setSource(source, size);
    unit.parse();

    Namespace *globalNamespace = control.newNamespace(0);
    Bind bind(&unit);
    bind(unit.ast()->asTranslationUnit(), globalNamespace);
}
<|endoftext|>


<commit_before>// AboutBox.cpp
// Copyright (c) 2009, Dan Heeks
// This program is released under the BSD license. See the file COPYING for details.

#include "stdafx.h"
#include "AboutBox.h"
#include "HeeksFrame.h"

BEGIN_EVENT_TABLE( CAboutBox, wxDialog )
EVT_BUTTON( wxID_OK, CAboutBox::OnButtonOK )
END_EVENT_TABLE()

CAboutBox::CAboutBox(wxWindow *parent):wxDialog(parent, wxID_ANY, _T(""), wxDefaultPosition, wxDefaultSize, wxDEFAULT_DIALOG_STYLE)
{
    wxPanel *panel = new wxPanel(this, wxID_ANY, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL);

    wxButton *ok_button = new wxButton(panel, wxID_OK, _("OK"));

    wxBoxSizer *mainsizer = new wxBoxSizer( wxVERTICAL );

	wxTextCtrl* text_ctrl = new wxTextCtrl( this, wxID_ANY, _T(""),	wxDefaultPosition, wxSize(800,600), wxTE_READONLY | wxTE_MULTILINE);

	wxString str = wxString(_T("HeeksCAD\n see http://heeks.net, or for source code: http://code.google.com/p/heekscad/\n\nusing Open CASCADE solid modeller - http://www.opencascade.org"))
		+ wxString(_T("\n\nwindows made with wxWidgets 2.8.9 - http://wxwidgets.org"))
		+ wxString(_T("\n\ntext uses glFont Copyright (c) 1998 Brad Fish E-mail: bhf5@email.byu.edu Web: http://students.cs.byu.edu/~bfish/"))
		+ wxString(_T("\n\nWritten by:\n Dan Heeks\n Jon Pry\n Jonathan George\n David Nicholls"))
		+ wxString(_T("\n\nWith contributions from:\n Hirutso Enni\n Perttu Ahola\n Dave ( the archivist )\n mpictor\n fenn\n fungunner2\n andrea ( openSUSE )\n g_code\n Luigi Barbati (Italian translation)\n André Pascual (French translation)"))
		+ wxString(_T("\n\nThis is free, open source software."));

	wxString version_str = wxGetApp().m_version_number;
	version_str.Replace(_T(" "), _T("."));

	this->SetTitle(version_str);

	text_ctrl->WriteText(str + wxGetApp().m_frame->m_extra_about_box_str);

    //mainsizer->Add( text_ctrl, wxSizerFlags().Align(wxALIGN_CENTER).Border(wxALL, 10).Expand() );
    //mainsizer->Add( ok_button, wxSizerFlags().Align(wxALIGN_CENTER) );
	mainsizer->Add( text_ctrl );
	mainsizer->Add( ok_button );
	mainsizer->RecalcSizes();

    // tell frame to make use of sizer (or constraints, if any)
    panel->SetAutoLayout( true );
    panel->SetSizer( mainsizer );

#ifndef __WXWINCE__
    // don't allow frame to get smaller than what the sizers tell ye
    mainsizer->SetSizeHints( this );
#endif

    Show(true);
}

void CAboutBox::OnButtonOK(wxCommandEvent& event)
{
	EndModal(wxID_OK);
}
<commit_msg>problems with the accent on the e. I changed it to a normal "e"<commit_after>// AboutBox.cpp
// Copyright (c) 2009, Dan Heeks
// This program is released under the BSD license. See the file COPYING for details.

#include "stdafx.h"
#include "AboutBox.h"
#include "HeeksFrame.h"

BEGIN_EVENT_TABLE( CAboutBox, wxDialog )
EVT_BUTTON( wxID_OK, CAboutBox::OnButtonOK )
END_EVENT_TABLE()

CAboutBox::CAboutBox(wxWindow *parent):wxDialog(parent, wxID_ANY, _T(""), wxDefaultPosition, wxDefaultSize, wxDEFAULT_DIALOG_STYLE)
{
    wxPanel *panel = new wxPanel(this, wxID_ANY, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL);

    wxButton *ok_button = new wxButton(panel, wxID_OK, _("OK"));

    wxBoxSizer *mainsizer = new wxBoxSizer( wxVERTICAL );

	wxTextCtrl* text_ctrl = new wxTextCtrl( this, wxID_ANY, _T(""),	wxDefaultPosition, wxSize(800,600), wxTE_READONLY | wxTE_MULTILINE);

	wxString str = wxString(_T("HeeksCAD\n see http://heeks.net, or for source code: http://code.google.com/p/heekscad/\n\nusing Open CASCADE solid modeller - http://www.opencascade.org"))
		+ wxString(_T("\n\nwindows made with wxWidgets 2.8.9 - http://wxwidgets.org"))
		+ wxString(_T("\n\ntext uses glFont Copyright (c) 1998 Brad Fish E-mail: bhf5@email.byu.edu Web: http://students.cs.byu.edu/~bfish/"))
		+ wxString(_T("\n\nWritten by:\n Dan Heeks\n Jon Pry\n Jonathan George\n David Nicholls"))
		+ wxString(_T("\n\nWith contributions from:\n Hirutso Enni\n Perttu Ahola\n Dave ( the archivist )\n mpictor\n fenn\n fungunner2\n andrea ( openSUSE )\n g_code\n Luigi Barbati (Italian translation)\n Andre Pascual (French translation)"))
		+ wxString(_T("\n\nThis is free, open source software."));

	wxString version_str = wxGetApp().m_version_number;
	version_str.Replace(_T(" "), _T("."));

	this->SetTitle(version_str);

	text_ctrl->WriteText(str + wxGetApp().m_frame->m_extra_about_box_str);

    //mainsizer->Add( text_ctrl, wxSizerFlags().Align(wxALIGN_CENTER).Border(wxALL, 10).Expand() );
    //mainsizer->Add( ok_button, wxSizerFlags().Align(wxALIGN_CENTER) );
	mainsizer->Add( text_ctrl );
	mainsizer->Add( ok_button );
	mainsizer->RecalcSizes();

    // tell frame to make use of sizer (or constraints, if any)
    panel->SetAutoLayout( true );
    panel->SetSizer( mainsizer );

#ifndef __WXWINCE__
    // don't allow frame to get smaller than what the sizers tell ye
    mainsizer->SetSizeHints( this );
#endif

    Show(true);
}

void CAboutBox::OnButtonOK(wxCommandEvent& event)
{
	EndModal(wxID_OK);
}
<|endoftext|>


<commit_before>/***************************************************************************
 *   Copyright (c) Juergen Riegel          (juergen.riegel@web.de) 2014    *
 *                                                                         *
 *   This file is part of the FreeCAD CAx development system.              *
 *                                                                         *
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Library General Public           *
 *   License as published by the Free Software Foundation; either          *
 *   version 2 of the License, or (at your option) any later version.      *
 *                                                                         *
 *   This library  is distributed in the hope that it will be useful,      *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU Library General Public License for more details.                  *
 *                                                                         *
 *   You should have received a copy of the GNU Library General Public     *
 *   License along with this library; see the file COPYING.LIB. If not,    *
 *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
 *   Suite 330, Boston, MA  02111-1307, USA                                *
 *                                                                         *
 ***************************************************************************/

#include "PreCompiled.h"

#ifndef _PreComp_
#endif

#include <App/Document.h>
#include <App/Plane.h>

#include "Part.h"
#include "PartPy.h"

using namespace App;

PROPERTY_SOURCE(App::Part, App::GeoFeatureGroup)

//===========================================================================
// Feature
//===========================================================================

const char* Part::BaseplaneTypes[3] = {"XY-Plane", "XZ-Plane", "YZ-Plane"};

Part::Part(void)
{
    ADD_PROPERTY(Type,(""));
    ADD_PROPERTY_TYPE(Material, (), 0, App::Prop_None, "Map with material properties");
    ADD_PROPERTY_TYPE(Meta, (), 0, App::Prop_None, "Map with additional meta information");

    // create the uuid for the document
    Base::Uuid id;
    ADD_PROPERTY_TYPE(Id, (""), 0, App::Prop_None, "ID (Part-Number) of the Item");
    ADD_PROPERTY_TYPE(Uid, (id), 0, App::Prop_None, "UUID of the Item");

    // license stuff
    ADD_PROPERTY_TYPE(License, ("CC BY 3.0"), 0, App::Prop_None, "License string of the Item");
    ADD_PROPERTY_TYPE(LicenseURL, ("http://creativecommons.org/licenses/by/3.0/"), 0, App::Prop_None, "URL to the license text/contract");
    // color and apperance
    ADD_PROPERTY(Color, (1.0, 1.0, 1.0, 1.0)); // set transparent -> not used

}

Part::~Part(void)
{
}

PyObject *Part::getPyObject()
{
    if (PythonObject.is(Py::_None())){
        // ref counter is set to 1
        PythonObject = Py::Object(new PartPy(this),true);
    }
    return Py::new_reference_to(PythonObject);
}

// Python feature ---------------------------------------------------------

// Not quit sure yet makeing Part derivable in Python is good Idea!
// JR 2014

//namespace App {
///// @cond DOXERR
//PROPERTY_SOURCE_TEMPLATE(App::PartPython, App::Part)
//template<> const char* App::PartPython::getViewProviderName(void) const {
//    return "Gui::ViewProviderPartPython";
//}
//template<> PyObject* App::PartPython::getPyObject(void) {
//    if (PythonObject.is(Py::_None())) {
//        // ref counter is set to 1
//        PythonObject = Py::Object(new FeaturePythonPyT<App::PartPy>(this),true);
//    }
//    return Py::new_reference_to(PythonObject);
//}
///// @endcond
//
//// explicit template instantiation
//template class AppExport FeaturePythonT<App::Part>;
//}
<commit_msg>App: Part: rename base planes.<commit_after>/***************************************************************************
 *   Copyright (c) Juergen Riegel          (juergen.riegel@web.de) 2014    *
 *                                                                         *
 *   This file is part of the FreeCAD CAx development system.              *
 *                                                                         *
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Library General Public           *
 *   License as published by the Free Software Foundation; either          *
 *   version 2 of the License, or (at your option) any later version.      *
 *                                                                         *
 *   This library  is distributed in the hope that it will be useful,      *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU Library General Public License for more details.                  *
 *                                                                         *
 *   You should have received a copy of the GNU Library General Public     *
 *   License along with this library; see the file COPYING.LIB. If not,    *
 *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
 *   Suite 330, Boston, MA  02111-1307, USA                                *
 *                                                                         *
 ***************************************************************************/

#include "PreCompiled.h"

#ifndef _PreComp_
#endif

#include <App/Document.h>
#include <App/Plane.h>

#include "Part.h"
#include "PartPy.h"

using namespace App;

PROPERTY_SOURCE(App::Part, App::GeoFeatureGroup)

//===========================================================================
// Feature
//===========================================================================

const char* Part::BaseplaneTypes[3] = {"XY_Plane", "XZ_Plane", "YZ_Plane"};

Part::Part(void)
{
    ADD_PROPERTY(Type,(""));
    ADD_PROPERTY_TYPE(Material, (), 0, App::Prop_None, "Map with material properties");
    ADD_PROPERTY_TYPE(Meta, (), 0, App::Prop_None, "Map with additional meta information");

    // create the uuid for the document
    Base::Uuid id;
    ADD_PROPERTY_TYPE(Id, (""), 0, App::Prop_None, "ID (Part-Number) of the Item");
    ADD_PROPERTY_TYPE(Uid, (id), 0, App::Prop_None, "UUID of the Item");

    // license stuff
    ADD_PROPERTY_TYPE(License, ("CC BY 3.0"), 0, App::Prop_None, "License string of the Item");
    ADD_PROPERTY_TYPE(LicenseURL, ("http://creativecommons.org/licenses/by/3.0/"), 0, App::Prop_None, "URL to the license text/contract");
    // color and apperance
    ADD_PROPERTY(Color, (1.0, 1.0, 1.0, 1.0)); // set transparent -> not used

}

Part::~Part(void)
{
}

PyObject *Part::getPyObject()
{
    if (PythonObject.is(Py::_None())){
        // ref counter is set to 1
        PythonObject = Py::Object(new PartPy(this),true);
    }
    return Py::new_reference_to(PythonObject);
}

// Python feature ---------------------------------------------------------

// Not quit sure yet makeing Part derivable in Python is good Idea!
// JR 2014

//namespace App {
///// @cond DOXERR
//PROPERTY_SOURCE_TEMPLATE(App::PartPython, App::Part)
//template<> const char* App::PartPython::getViewProviderName(void) const {
//    return "Gui::ViewProviderPartPython";
//}
//template<> PyObject* App::PartPython::getPyObject(void) {
//    if (PythonObject.is(Py::_None())) {
//        // ref counter is set to 1
//        PythonObject = Py::Object(new FeaturePythonPyT<App::PartPy>(this),true);
//    }
//    return Py::new_reference_to(PythonObject);
//}
///// @endcond
//
//// explicit template instantiation
//template class AppExport FeaturePythonT<App::Part>;
//}
<|endoftext|>


<commit_before>/*
	This file is part of the MinSG library.
	Copyright (C) 2012 Ralf Petring <ralf@petring.net>

	This library is subject to the terms of the Mozilla Public License, v. 2.0.
	You should have received a copy of the MPL along with this library; see the
	file LICENSE. If not, you can obtain one at http://mozilla.org/MPL/2.0/.
*/
#include "DebugCamera.h"
#include <Rendering/Mesh/Mesh.h>
#include <Rendering/RenderingContext/ParameterStructs.h>
#include <Rendering/RenderingContext/RenderingContext.h>
#include <Rendering/FBO.h>

#include <functional>

using namespace std::placeholders;
using namespace Rendering;

namespace MinSG {

DebugCamera::DebugCamera() = default;
DebugCamera::~DebugCamera() = default;

void DebugCamera::displayMesh(RenderingContext & rc, Mesh * mesh) {

	mesh->_display(rc, 0, mesh->isUsingIndexData() ? mesh->getIndexCount() : mesh->getVertexCount());

	if(rc.getColorBufferParameters().isAnyWritingEnabled() || rc.getDepthBufferParameters().isWritingEnabled()) {

		rc.pushFBO();
		rc.pushProjectionMatrix();
		rc.pushMatrix();
		rc.pushViewport();
		rc.pushScissor();

		Geometry::Matrix4x4 mod = rc.getMatrix();

		rc.setFBO(fbo.get());
		rc.setProjectionMatrix(debug->getFrustum().getProjectionMatrix());
		rc.setInverseCameraMatrix(debug->getWorldMatrix());
		rc.setMatrix(conversionMatrix * mod);
		rc.setViewport(debug->getViewport());
		if(debug->isScissorEnabled()) {
			rc.setScissor(Rendering::ScissorParameters(debug->getScissor()));
		} else {
			rc.setScissor(Rendering::ScissorParameters());
		}

		mesh->_display(rc, 0, mesh->isUsingIndexData() ? mesh->getIndexCount() : mesh->getVertexCount());

		rc.setInverseCameraMatrix(original->getWorldMatrix());

		rc.popFBO();
		rc.popProjectionMatrix();
		rc.popMatrix();
		rc.popViewport();
		rc.popScissor();

	}
}

void DebugCamera::enable(RenderingContext & rc, AbstractCameraNode * _debug, AbstractCameraNode * _original, FBO * _fbo) {
	debug = _debug;
	original = _original;
	fbo = _fbo;

	debug->updateFrustum();
	original->updateFrustum();

	conversionMatrix = debug->getWorldMatrix().inverse() * original->getWorldMatrix();

	rc.setDisplayMeshFn(std::bind(std::mem_fn(&DebugCamera::displayMesh), this, _1, _2));
}

void DebugCamera::disable(RenderingContext & rc) {
	rc.resetDisplayMeshFn();
	debug = nullptr;
	original = nullptr;
	fbo = nullptr;
	conversionMatrix.setIdentity();
}

}
<commit_msg>Add comment why _display is called twice<commit_after>/*
	This file is part of the MinSG library.
	Copyright (C) 2012 Ralf Petring <ralf@petring.net>

	This library is subject to the terms of the Mozilla Public License, v. 2.0.
	You should have received a copy of the MPL along with this library; see the
	file LICENSE. If not, you can obtain one at http://mozilla.org/MPL/2.0/.
*/
#include "DebugCamera.h"
#include <Rendering/Mesh/Mesh.h>
#include <Rendering/RenderingContext/ParameterStructs.h>
#include <Rendering/RenderingContext/RenderingContext.h>
#include <Rendering/FBO.h>

#include <functional>

using namespace std::placeholders;
using namespace Rendering;

namespace MinSG {

DebugCamera::DebugCamera() = default;
DebugCamera::~DebugCamera() = default;

void DebugCamera::displayMesh(RenderingContext & rc, Mesh * mesh) {

	// Display for the "normal" camera.
	mesh->_display(rc, 0, mesh->isUsingIndexData() ? mesh->getIndexCount() : mesh->getVertexCount());

	if(rc.getColorBufferParameters().isAnyWritingEnabled() || rc.getDepthBufferParameters().isWritingEnabled()) {

		rc.pushFBO();
		rc.pushProjectionMatrix();
		rc.pushMatrix();
		rc.pushViewport();
		rc.pushScissor();

		Geometry::Matrix4x4 mod = rc.getMatrix();

		rc.setFBO(fbo.get());
		rc.setProjectionMatrix(debug->getFrustum().getProjectionMatrix());
		rc.setInverseCameraMatrix(debug->getWorldMatrix());
		rc.setMatrix(conversionMatrix * mod);
		rc.setViewport(debug->getViewport());
		if(debug->isScissorEnabled()) {
			rc.setScissor(Rendering::ScissorParameters(debug->getScissor()));
		} else {
			rc.setScissor(Rendering::ScissorParameters());
		}

		// Display for the "debug" camera.
		mesh->_display(rc, 0, mesh->isUsingIndexData() ? mesh->getIndexCount() : mesh->getVertexCount());

		rc.setInverseCameraMatrix(original->getWorldMatrix());

		rc.popFBO();
		rc.popProjectionMatrix();
		rc.popMatrix();
		rc.popViewport();
		rc.popScissor();

	}
}

void DebugCamera::enable(RenderingContext & rc, AbstractCameraNode * _debug, AbstractCameraNode * _original, FBO * _fbo) {
	debug = _debug;
	original = _original;
	fbo = _fbo;

	debug->updateFrustum();
	original->updateFrustum();

	conversionMatrix = debug->getWorldMatrix().inverse() * original->getWorldMatrix();

	rc.setDisplayMeshFn(std::bind(std::mem_fn(&DebugCamera::displayMesh), this, _1, _2));
}

void DebugCamera::disable(RenderingContext & rc) {
	rc.resetDisplayMeshFn();
	debug = nullptr;
	original = nullptr;
	fbo = nullptr;
	conversionMatrix.setIdentity();
}

}
<|endoftext|>


<commit_before>#include <iostream>

int main() {

  int sum, n1, n2;

  std::cout << "Enter n1: ";
  std::cin >> n1;

  std::cout << "Enter n2: ";
  std::cin >> n2;

  sum = n1+n2;

  std::cout << std::endl;
  std::cout << "The sum is: " << sum << std::endl;

  return 0;
}
<commit_msg>Delete 2.sum.cpp<commit_after><|endoftext|>


<commit_before>#include "BallTree.h"
#include "Utility.h"
#include "Page.h"
#include "PagePool.h"
#include <algorithm>
#include <ctime>
#include <tuple>
#include <cstdint>

using namespace std;

Page* getPage(const string &indexPath, int pid);
tuple<bool, int, int> readIndex(Page* indexPage, int id);

BallTree::BallTree(): root(nullptr), size(0), dimension(-1), leafNum(0), indexPage(nullptr) {
}

BallTree::~BallTree() {
	if (root) delete root;
}

bool BallTree::buildTree(int n, int d, float** data) {
	int* id = new int[n];
	for (int i = 0; i < n; i++) id[i] = i + 1;	//ڵÿӦid

	size = n;
	dimension = d;
	root = BallTreeNode::build(n, d, data, id);		//ýڵ㺯

	//countNode();

	delete[] id;
	return true;
}

int BallTree::mipSearch(int d, float* query) {
	visitedLeafNum = 0;
	auto startTime = clock();
	auto tmp = _mipSearch(root, query);
	printf("done in %d ms. leaf counted: %d. Total leaves: %d. MIP: %10lf ID: %d\n", clock() - startTime, visitedLeafNum, leafNum, tmp.second, tmp.first);
	return tmp.first;
}

pair<int, float> BallTree::_mipSearch(BallTreeNode* root, float* query) {
	if (root->isLeaf()) {	//ȵҶӽڵ
		// leaf
		visitedLeafNum++;
		float maxProd = innerProduct(query, *root->data->begin(), root->dimension);	//Utilityڻ
		int maxi = *root->id->begin();
		auto itData = ++root->data->begin();
		auto itId = ++root->id->begin();
		while (itData != root->data->end()) {
			float prod = innerProduct(query, *itData, root->dimension);
			if (prod > maxProd) {
				maxProd = prod;
				maxi = *itId;
			}
			itData++;
			itId++;
		}

		return make_pair(maxi, maxProd);	//Ҷӽڵ㲢㷵صǰڻ
	}
	else {	//ӽڵ
		if (!root->left) root->left = restoreNode(root->leftId);
		if (!root->right) root->right = restoreNode(root->rightId);
		float leftBound = root->left->getBound(query), rightBound = root->right->getBound(query);	//ڻ
		//ݿڻСжϽȥĸ
		if (leftBound > rightBound) {
			auto leftRes = _mipSearch(root->left, query);	//ѰұȽϺõĵǰڻ
			if (leftRes.second >= rightBound)	//ݵǰڻĳڵڻֱ̭ýڵ
				return leftRes;
			auto rightRes = _mipSearch(root->right, query);
			return leftRes.second > rightRes.second ? leftRes : rightRes;	//ݹҪڵڻ
		}
		else {
			auto rightRes = _mipSearch(root->right, query);
			if (rightRes.second >= leftBound)
				return rightRes;
			auto leftRes = _mipSearch(root->left, query);
			return leftRes.second > rightRes.second ? leftRes : rightRes;
		}
	}
}

bool BallTree::storeTree(const string& indexPath) {
	if (!indexPage) indexPage = pagePool->create(4);	// δindexҳӲм
														// pid, sid: ҳŲۺŸ2ֽ
	auto nonLeafPage = pagePool->create(21 + dimension * 4);
														// ڱҶ
	auto leafPage = pagePool->create(17 + N0 * (4 + dimension * 4) + dimension * 4);
														// ڱҶӽ
	int leafCount = 0, nonLeafCount = 0;				// ͳҳļѴitem
	char* buffer = new char[255];
	traverse(root, [this, buffer, &leafCount, &leafPage, &nonLeafCount, &nonLeafPage, indexPath](BallTreeNode *node) {
		bool leaf = node->isLeaf();
		if (node->isLeaf()) {
			if (leafCount == leafPage->getCapacity()) { // Ҷҳ½һҳ
				sprintf(buffer, "%s/%d.page", indexPath.c_str(), leafPage->getId());
				leafPage->writeBack(buffer);
				pagePool->remove(leafPage->getId());
				leafPage = pagePool->create(17 + N0 * (4 + dimension * 4) + dimension * 4);
				leafCount = 0;
			}
			int16_t pid = leafPage->getId(), sid = leafCount;	// 16λ棬ʡռ
			memcpy(buffer, &pid, 2);
			memcpy(buffer + 2, &sid, 2);
			indexPage->setBySlot(node->getId(), buffer);		// id -> <pid,sid>
			leafPage->setBySlot(leafCount++, node->serialize().first);
		}
		else {
			if (nonLeafCount == nonLeafPage->getCapacity()) {	// Ҷҳ½
				sprintf(buffer, "%s/%d.page", indexPath.c_str(), nonLeafPage->getId());
				nonLeafPage->writeBack(buffer);
				pagePool->remove(nonLeafPage->getId());
				nonLeafPage = pagePool->create(21 + dimension * 4);
				nonLeafCount = 0;
			}
			int16_t pid = nonLeafPage->getId(), sid = nonLeafCount;
			memcpy(buffer, &pid, 2);
			memcpy(buffer + 2, &sid, 2);
			indexPage->setBySlot(node->getId(), buffer);
			nonLeafPage->setBySlot(nonLeafCount++, node->serialize().first);
		}
	});
	sprintf(buffer, "%s/%d.page", indexPath.c_str(), indexPage->getId());
	indexPage->writeBack(buffer);					// дҳ
	delete[] buffer;
	printf("storage done.\n");
	return true;
}

bool BallTree::restoreTree(const string& indexPath) {
	pagePool->setBaseDir(indexPath);
	indexPage = pagePool->createFromFile(0);		// 0ҳļΪҳ
	root = restoreNode(0);
	return true;
}

void BallTree::traverse(BallTreeNode *root, function<void(BallTreeNode *node)> func) {
	if (!root) return;
	func(root);
	traverse(root->left, func);
	traverse(root->right, func);
}

void BallTree::countNode() {
	int nodeNum = 0, leafNum = 0;
	traverse(root, [&nodeNum, &leafNum](BallTreeNode *node) {
		node++;
		if (node->isLeaf()) leafNum++;
	});
	this->leafNum = leafNum;
}

BallTreeNode* BallTree::restoreNode(int tid) {
	auto info = readIndex(indexPage, tid);
	auto page = pagePool->get(get<1>(info));
	if (!page) page = pagePool->createFromFile(get<1>(info));
	auto node = BallTreeNode::deserialize(page->getBySlot(get<2>(info)));
	if (dimension == -1) dimension = node->dimension;
	return node;
}

Page* getPage(const string &indexPath, int pid) {
	auto p = pagePool->get(pid);
	if (p) return p;
	return pagePool->createFromFile(indexPath, pid);
}

tuple<bool, int, int> readIndex(Page* indexPage, int id) {
	auto buffer = indexPage->getBySlot(id);
	bool isLeaf = false;	// not used anymore
	int16_t pid, sid;
	memcpy(&pid, buffer, 2);
	memcpy(&sid, buffer + 2, 2);
	return make_tuple(isLeaf, int(pid), int(sid));
}
<commit_msg>Searchcomment<commit_after>#include "BallTree.h"
#include "Utility.h"
#include "Page.h"
#include "PagePool.h"
#include <algorithm>
#include <ctime>
#include <tuple>
#include <cstdint>

using namespace std;

Page* getPage(const string &indexPath, int pid);
tuple<bool, int, int> readIndex(Page* indexPage, int id);

BallTree::BallTree(): root(nullptr), size(0), dimension(-1), leafNum(0), indexPage(nullptr) {
}

BallTree::~BallTree() {
	if (root) delete root;
}

bool BallTree::buildTree(int n, int d, float** data) {
	int* id = new int[n];
	for (int i = 0; i < n; i++) id[i] = i + 1;	//ڵÿӦid

	size = n;
	dimension = d;
	root = BallTreeNode::build(n, d, data, id);		//ýڵ㺯

	//countNode();

	delete[] id;
	return true;
}

int BallTree::mipSearch(int d, float* query) {
	visitedLeafNum = 0;
	auto startTime = clock();	//ڼѯʱ
	auto tmp = _mipSearch(root, query);
	printf("done in %d ms. leaf counted: %d. Total leaves: %d. MIP: %10lf ID: %d\n", clock() - startTime, visitedLeafNum, leafNum, tmp.second, tmp.first);
	return tmp.first;
}

pair<int, float> BallTree::_mipSearch(BallTreeNode* root, float* query) {
	if (root->isLeaf()) {	//ȵҶӽڵ
		// leaf
		visitedLeafNum++;
		float maxProd = innerProduct(query, *root->data->begin(), root->dimension);	//Utilityڻ
		int maxi = *root->id->begin();
		auto itData = ++root->data->begin();
		auto itId = ++root->id->begin();
		while (itData != root->data->end()) {	//ҵееڻĵ
			float prod = innerProduct(query, *itData, root->dimension);
			if (prod > maxProd) {
				maxProd = prod;
				maxi = *itId;
			}
			itData++;
			itId++;
		}

		return make_pair(maxi, maxProd);	//صǰڻĵ
	}
	else {	//ӽڵ
		if (!root->left) root->left = restoreNode(root->leftId);
		if (!root->right) root->right = restoreNode(root->rightId);
		float leftBound = root->left->getBound(query), rightBound = root->right->getBound(query);	//ڻ
		//ݿڻСжϽȥĸ
		if (leftBound > rightBound) {
			auto leftRes = _mipSearch(root->left, query);	//ѰұȽϺõĵǰڻ
			if (leftRes.second >= rightBound)	//ݵǰڻĳڵڻֱ̭ýڵ
				return leftRes;
			auto rightRes = _mipSearch(root->right, query);
			return leftRes.second > rightRes.second ? leftRes : rightRes;	//ݹҪڵڻ
		}
		else {
			auto rightRes = _mipSearch(root->right, query);
			if (rightRes.second >= leftBound)
				return rightRes;
			auto leftRes = _mipSearch(root->left, query);
			return leftRes.second > rightRes.second ? leftRes : rightRes;
		}
	}
}

bool BallTree::storeTree(const string& indexPath) {
	if (!indexPage) indexPage = pagePool->create(4);	// δindexҳӲм
														// pid, sid: ҳŲۺŸ2ֽ
	auto nonLeafPage = pagePool->create(21 + dimension * 4);
														// ڱҶ
	auto leafPage = pagePool->create(17 + N0 * (4 + dimension * 4) + dimension * 4);
														// ڱҶӽ
	int leafCount = 0, nonLeafCount = 0;				// ͳҳļѴitem
	char* buffer = new char[255];
	traverse(root, [this, buffer, &leafCount, &leafPage, &nonLeafCount, &nonLeafPage, indexPath](BallTreeNode *node) {
		bool leaf = node->isLeaf();
		if (node->isLeaf()) {
			if (leafCount == leafPage->getCapacity()) { // Ҷҳ½һҳ
				sprintf(buffer, "%s/%d.page", indexPath.c_str(), leafPage->getId());
				leafPage->writeBack(buffer);
				pagePool->remove(leafPage->getId());
				leafPage = pagePool->create(17 + N0 * (4 + dimension * 4) + dimension * 4);
				leafCount = 0;
			}
			int16_t pid = leafPage->getId(), sid = leafCount;	// 16λ棬ʡռ
			memcpy(buffer, &pid, 2);
			memcpy(buffer + 2, &sid, 2);
			indexPage->setBySlot(node->getId(), buffer);		// id -> <pid,sid>
			leafPage->setBySlot(leafCount++, node->serialize().first);
		}
		else {
			if (nonLeafCount == nonLeafPage->getCapacity()) {	// Ҷҳ½
				sprintf(buffer, "%s/%d.page", indexPath.c_str(), nonLeafPage->getId());
				nonLeafPage->writeBack(buffer);
				pagePool->remove(nonLeafPage->getId());
				nonLeafPage = pagePool->create(21 + dimension * 4);
				nonLeafCount = 0;
			}
			int16_t pid = nonLeafPage->getId(), sid = nonLeafCount;
			memcpy(buffer, &pid, 2);
			memcpy(buffer + 2, &sid, 2);
			indexPage->setBySlot(node->getId(), buffer);
			nonLeafPage->setBySlot(nonLeafCount++, node->serialize().first);
		}
	});
	sprintf(buffer, "%s/%d.page", indexPath.c_str(), indexPage->getId());
	indexPage->writeBack(buffer);					// дҳ
	delete[] buffer;
	printf("storage done.\n");
	return true;
}

bool BallTree::restoreTree(const string& indexPath) {
	pagePool->setBaseDir(indexPath);
	indexPage = pagePool->createFromFile(0);		// 0ҳļΪҳ
	root = restoreNode(0);
	return true;
}

void BallTree::traverse(BallTreeNode *root, function<void(BallTreeNode *node)> func) {
	if (!root) return;
	func(root);
	traverse(root->left, func);
	traverse(root->right, func);
}

void BallTree::countNode() {
	int nodeNum = 0, leafNum = 0;
	traverse(root, [&nodeNum, &leafNum](BallTreeNode *node) {
		node++;
		if (node->isLeaf()) leafNum++;
	});
	this->leafNum = leafNum;
}

BallTreeNode* BallTree::restoreNode(int tid) {
	auto info = readIndex(indexPage, tid);
	auto page = pagePool->get(get<1>(info));
	if (!page) page = pagePool->createFromFile(get<1>(info));
	auto node = BallTreeNode::deserialize(page->getBySlot(get<2>(info)));
	if (dimension == -1) dimension = node->dimension;
	return node;
}

Page* getPage(const string &indexPath, int pid) {
	auto p = pagePool->get(pid);
	if (p) return p;
	return pagePool->createFromFile(indexPath, pid);
}

tuple<bool, int, int> readIndex(Page* indexPage, int id) {
	auto buffer = indexPage->getBySlot(id);
	bool isLeaf = false;	// not used anymore
	int16_t pid, sid;
	memcpy(&pid, buffer, 2);
	memcpy(&sid, buffer + 2, 2);
	return make_tuple(isLeaf, int(pid), int(sid));
}
<|endoftext|>