<commit_before>#include "base_draw_startmenue.h"

Base_draw_startmenue::Base_draw_startmenue(double *define_srsize_, int *gr_, int draw_x_, int draw_y_, int rota_cx_, int rota_cy_, double rota_radian_,
		int *menue_var_, int this_menue_var_) {
	p_define_srsize = define_srsize_;
	p_gr = gr_;
	draw_x = draw_x_;
	draw_y = draw_y_;
	rota_cx = rota_cx_;
	rota_cy = rota_cy_;
	rota_radian = rota_radian_;
	menue_var = menue_var_;
	this_menue_var = this_menue_var_;
}

void Base_draw_startmenue::Drawgr() {
	if (*menue_var == this_menue_var) {
		DrawRotaGraph2(static_cast<int>(draw_x * *p_define_srsize), static_cast<int>(draw_y * *p_define_srsize),
			static_cast<int>(rota_cx * *p_define_srsize), static_cast<int>(rota_cy * *p_define_srsize), *p_define_srsize, rota_radian, *p_gr, true, false);
	}
	else if (*menue_var == this_menue_var + 1 || (this_menue_var == 0 && *menue_var == MAX_MENUE_NUMBER - 1)) {
		SetDrawBlendMode(DX_BLENDMODE_ALPHA, 128);
		DrawRotaGraph2(static_cast<int>(draw_x * *p_define_srsize), static_cast<int>(draw_y * *p_define_srsize + MOVE_MENUE_DIST),
			static_cast<int>(rota_cx * *p_define_srsize), static_cast<int>(rota_cy * *p_define_srsize), *p_define_srsize, rota_radian, *p_gr, true, false);
		SetDrawBlendMode(DX_BLENDMODE_NOBLEND, 0);
	}
	else if (*menue_var == this_menue_var - 1 || (this_menue_var == MAX_MENUE_NUMBER - 1 && *menue_var == 0)) {
		SetDrawBlendMode(DX_BLENDMODE_ALPHA, 128);
		DrawRotaGraph2(static_cast<int>(draw_x * *p_define_srsize), static_cast<int>(draw_y * *p_define_srsize - MOVE_MENUE_DIST),
			static_cast<int>(rota_cx * *p_define_srsize), static_cast<int>(rota_cy * *p_define_srsize), *p_define_srsize, rota_radian, *p_gr, true, false);
		SetDrawBlendMode(DX_BLENDMODE_NOBLEND, 0);
	}
}<commit_msg>正常にスタートメニューが表示されるよう修正<commit_after>#include "base_draw_startmenue.h"

Base_draw_startmenue::Base_draw_startmenue(double *define_srsize_, int *gr_, int draw_x_, int draw_y_, int rota_cx_, int rota_cy_, double rota_radian_,
		int *menue_var_, int this_menue_var_) {
	p_define_srsize = define_srsize_;
	p_gr = gr_;
	draw_x = draw_x_;
	draw_y = draw_y_;
	rota_cx = rota_cx_;
	rota_cy = rota_cy_;
	rota_radian = rota_radian_;
	menue_var = menue_var_;
	this_menue_var = this_menue_var_;
}

void Base_draw_startmenue::Drawgr() {
	if (*menue_var == this_menue_var) {
		DrawRotaGraph2(static_cast<int>(draw_x * *p_define_srsize), static_cast<int>(draw_y * *p_define_srsize),
			static_cast<int>(rota_cx * *p_define_srsize), static_cast<int>(rota_cy * *p_define_srsize), *p_define_srsize, rota_radian, *p_gr, true, false);
	}
	else if (*menue_var == this_menue_var + 1 || (this_menue_var == MAX_MENUE_NUMBER - 1 && *menue_var == 0)) {
		SetDrawBlendMode(DX_BLENDMODE_ALPHA, 128);
		DrawRotaGraph2(static_cast<int>(draw_x * *p_define_srsize), static_cast<int>(draw_y * *p_define_srsize + MOVE_MENUE_DIST),
			static_cast<int>(rota_cx * *p_define_srsize), static_cast<int>(rota_cy * *p_define_srsize), *p_define_srsize, rota_radian, *p_gr, true, false);
		SetDrawBlendMode(DX_BLENDMODE_NOBLEND, 0);
	}
	else if (*menue_var == this_menue_var - 1 || (this_menue_var == 0 && *menue_var == MAX_MENUE_NUMBER - 1)) {
		SetDrawBlendMode(DX_BLENDMODE_ALPHA, 128);
		DrawRotaGraph2(static_cast<int>(draw_x * *p_define_srsize), static_cast<int>(draw_y * *p_define_srsize - MOVE_MENUE_DIST),
			static_cast<int>(rota_cx * *p_define_srsize), static_cast<int>(rota_cy * *p_define_srsize), *p_define_srsize, rota_radian, *p_gr, true, false);
		SetDrawBlendMode(DX_BLENDMODE_NOBLEND, 0);
	}
}<|endoftext|>


<commit_before><commit_msg>Fix documentation.<commit_after><|endoftext|>


<commit_before>/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, 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_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PLANE_H_
#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PLANE_H_

#include "pcl/sample_consensus/sac_model_plane.h"
#include "pcl/common/centroid.h"
#include "pcl/common/eigen.h"
#include "pcl/common/concatenate.h"

//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelPlane<PointT>::isSampleGood(const std::vector<int> &samples) const
{
  // Get the values at the two points
  pcl::Array4fMapConst p0 = input_->points[samples[0]].getArray4fMap ();
  pcl::Array4fMapConst p1 = input_->points[samples[1]].getArray4fMap ();
  pcl::Array4fMapConst p2 = input_->points[samples[2]].getArray4fMap ();

  // Compute the segment values (in 3d) between p1 and p0
  Eigen::Array4f p1p0 = p1 - p0;

  // Compute the segment values (in 3d) between p2 and p0
  Eigen::Array4f p2p0 = p2 - p0;

  Eigen::Array4f dy1dy2 = p1p0 / p2p0;

  return ( (dy1dy2[0] != dy1dy2[1]) || (dy1dy2[2] != dy1dy2[1]) );
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelPlane<PointT>::computeModelCoefficients (
      const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
{
  // Need 3 samples
  if (samples.size () != 3)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelPlane::computeModelCoefficients] Invalid set of samples given (%lu)!\n", (unsigned long)samples.size ());
    return (false);
  }

  pcl::Array4fMapConst p0 = input_->points[samples[0]].getArray4fMap ();
  pcl::Array4fMapConst p1 = input_->points[samples[1]].getArray4fMap ();
  pcl::Array4fMapConst p2 = input_->points[samples[2]].getArray4fMap ();

  // Compute the segment values (in 3d) between p1 and p0
  Eigen::Array4f p1p0 = p1 - p0;
  // Compute the segment values (in 3d) between p2 and p0
  Eigen::Array4f p2p0 = p2 - p0;

  // Avoid some crashes by checking for collinearity here
  Eigen::Array4f dy1dy2 = p1p0 / p2p0;
  if ( (dy1dy2[0] == dy1dy2[1]) && (dy1dy2[2] == dy1dy2[1]) )          // Check for collinearity
    return (false);

  // Compute the plane coefficients from the 3 given points in a straightforward manner
  // calculate the plane normal n = (p2-p1) x (p3-p1) = cross (p2-p1, p3-p1)
  model_coefficients.resize (4);
  model_coefficients[0] = p1p0[1] * p2p0[2] - p1p0[2] * p2p0[1];
  model_coefficients[1] = p1p0[2] * p2p0[0] - p1p0[0] * p2p0[2];
  model_coefficients[2] = p1p0[0] * p2p0[1] - p1p0[1] * p2p0[0];
  model_coefficients[3] = 0;

  // Normalize
  model_coefficients.normalize ();

  // ... + d = 0
  model_coefficients[3] = -1 * (model_coefficients.template head<4>().dot (p0.matrix ()));

  return (true);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelPlane<PointT>::getDistancesToModel (
      const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
  // Needs a valid set of model coefficients
  if (model_coefficients.size () != 4)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelPlane::getDistancesToModel] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ());
    return;
  }

  distances.resize (indices_->size ());

  // Iterate through the 3d points and calculate the distances from them to the plane
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    // Calculate the distance from the point to the plane normal as the dot product
    // D = (P-A).N/|N|
    /*distances[i] = fabs (model_coefficients[0] * input_->points[(*indices_)[i]].x +
                         model_coefficients[1] * input_->points[(*indices_)[i]].y +
                         model_coefficients[2] * input_->points[(*indices_)[i]].z +
                         model_coefficients[3]);*/
    Eigen::Vector4f pt (input_->points[(*indices_)[i]].x,
                        input_->points[(*indices_)[i]].y,
                        input_->points[(*indices_)[i]].z,
                        1);
    distances[i] = fabs (model_coefficients.dot (pt));
  }
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelPlane<PointT>::selectWithinDistance (
      const Eigen::VectorXf &model_coefficients, double threshold, std::vector<int> &inliers)
{
  // Needs a valid set of model coefficients
  if (model_coefficients.size () != 4)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelPlane::selectWithinDistance] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ());
    return;
  }

  int nr_p = 0;
  inliers.resize (indices_->size ());

  // Iterate through the 3d points and calculate the distances from them to the plane
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    // Calculate the distance from the point to the plane normal as the dot product
    // D = (P-A).N/|N|
    Eigen::Vector4f pt (input_->points[(*indices_)[i]].x,
                        input_->points[(*indices_)[i]].y,
                        input_->points[(*indices_)[i]].z,
                        1);
    if (fabs (model_coefficients.dot (pt)) < threshold)
    {
      // Returns the indices of the points whose distances are smaller than the threshold
      inliers[nr_p] = (*indices_)[i];
      nr_p++;
    }
  }
  inliers.resize (nr_p);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelPlane<PointT>::optimizeModelCoefficients (
      const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
{
  // Needs a valid set of model coefficients
  if (model_coefficients.size () != 4)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ());
    optimized_coefficients = model_coefficients;
    return;
  }

  // Need at least 3 points to estimate a plane
  if (inliers.size () < 4)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", (unsigned long)inliers.size ());
    optimized_coefficients = model_coefficients;
    return;
  }

  Eigen::Vector4f plane_parameters;

  // Use Least-Squares to fit the plane through all the given sample points and find out its coefficients
  EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
  Eigen::Vector4f xyz_centroid;

  // Estimate the XYZ centroid
  compute3DCentroid (*input_, inliers, xyz_centroid);
  xyz_centroid[3] = 0;

  // Compute the 3x3 covariance matrix
  computeCovarianceMatrix (*input_, inliers, xyz_centroid, covariance_matrix);

  // Compute the model coefficients
  EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
  EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
  pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);

  // Hessian form (D = nc . p_plane (centroid here) + p)
  optimized_coefficients.resize (4);
  optimized_coefficients[0] = eigen_vectors (0, 0);
  optimized_coefficients[1] = eigen_vectors (1, 0);
  optimized_coefficients[2] = eigen_vectors (2, 0);
  optimized_coefficients[3] = 0;
  optimized_coefficients[3] = -1 * optimized_coefficients.dot (xyz_centroid);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelPlane<PointT>::projectPoints (
      const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields)
{
  // Needs a valid set of model coefficients
  if (model_coefficients.size () != 4)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelPlane::projectPoints] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ());
    return;
  }

  projected_points.header = input_->header;
  projected_points.is_dense = input_->is_dense;

  Eigen::Vector4f mc (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  // Copy all the data fields from the input cloud to the projected one?
  if (copy_data_fields)
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (input_->points.size ());
    projected_points.width    = input_->width;
    projected_points.height   = input_->height;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < input_->points.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the plane
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      // Calculate the distance from the point to the plane
      Eigen::Vector4f p (input_->points[inliers[i]].x,
                         input_->points[inliers[i]].y,
                         input_->points[inliers[i]].z,
                         1);
      float distance_to_plane = model_coefficients.dot (p);

      pcl::Vector4fMap pp = projected_points.points[inliers[i]].getVector4fMap ();
      pp = p - mc * distance_to_plane;        // mc[3] = 0, therefore the 3rd coordinate is safe
    }
  }
  else
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (inliers.size ());
    projected_points.width    = inliers.size ();
    projected_points.height   = 1;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < inliers.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the plane
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      // Calculate the distance from the point to the plane
      Eigen::Vector4f p (input_->points[inliers[i]].x,
                         input_->points[inliers[i]].y,
                         input_->points[inliers[i]].z,
                         1);
      float distance_to_plane = model_coefficients.dot (p);

      pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap ();
      pp = p - mc * distance_to_plane;        // mc[3] = 0, therefore the 3rd coordinate is safe
    }
  }
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelPlane<PointT>::doSamplesVerifyModel (
      const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, double threshold)
{
  // Needs a valid set of model coefficients
  if (model_coefficients.size () != 4)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelPlane::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ());
    return (false);
  }

  for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
  {
    Eigen::Vector4f pt (input_->points[*it].x,
                        input_->points[*it].y,
                        input_->points[*it].z,
                        1);
    if (fabs (model_coefficients.dot (pt)) > threshold)
      return (false);
  }

  return (true);
}

#define PCL_INSTANTIATE_SampleConsensusModelPlane(T) template class PCL_EXPORTS pcl::SampleConsensusModelPlane<T>;

#endif    // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PLANE_H_

<commit_msg>fixed issue #192 (error in projection of pcl::ProjectInliers with pcl::SACMODEL_PLANE)<commit_after>/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, 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_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PLANE_H_
#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PLANE_H_

#include "pcl/sample_consensus/sac_model_plane.h"
#include "pcl/common/centroid.h"
#include "pcl/common/eigen.h"
#include "pcl/common/concatenate.h"

//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelPlane<PointT>::isSampleGood(const std::vector<int> &samples) const
{
  // Get the values at the two points
  pcl::Array4fMapConst p0 = input_->points[samples[0]].getArray4fMap ();
  pcl::Array4fMapConst p1 = input_->points[samples[1]].getArray4fMap ();
  pcl::Array4fMapConst p2 = input_->points[samples[2]].getArray4fMap ();

  // Compute the segment values (in 3d) between p1 and p0
  Eigen::Array4f p1p0 = p1 - p0;

  // Compute the segment values (in 3d) between p2 and p0
  Eigen::Array4f p2p0 = p2 - p0;

  Eigen::Array4f dy1dy2 = p1p0 / p2p0;

  return ( (dy1dy2[0] != dy1dy2[1]) || (dy1dy2[2] != dy1dy2[1]) );
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelPlane<PointT>::computeModelCoefficients (
      const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
{
  // Need 3 samples
  if (samples.size () != 3)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelPlane::computeModelCoefficients] Invalid set of samples given (%lu)!\n", (unsigned long)samples.size ());
    return (false);
  }

  pcl::Array4fMapConst p0 = input_->points[samples[0]].getArray4fMap ();
  pcl::Array4fMapConst p1 = input_->points[samples[1]].getArray4fMap ();
  pcl::Array4fMapConst p2 = input_->points[samples[2]].getArray4fMap ();

  // Compute the segment values (in 3d) between p1 and p0
  Eigen::Array4f p1p0 = p1 - p0;
  // Compute the segment values (in 3d) between p2 and p0
  Eigen::Array4f p2p0 = p2 - p0;

  // Avoid some crashes by checking for collinearity here
  Eigen::Array4f dy1dy2 = p1p0 / p2p0;
  if ( (dy1dy2[0] == dy1dy2[1]) && (dy1dy2[2] == dy1dy2[1]) )          // Check for collinearity
    return (false);

  // Compute the plane coefficients from the 3 given points in a straightforward manner
  // calculate the plane normal n = (p2-p1) x (p3-p1) = cross (p2-p1, p3-p1)
  model_coefficients.resize (4);
  model_coefficients[0] = p1p0[1] * p2p0[2] - p1p0[2] * p2p0[1];
  model_coefficients[1] = p1p0[2] * p2p0[0] - p1p0[0] * p2p0[2];
  model_coefficients[2] = p1p0[0] * p2p0[1] - p1p0[1] * p2p0[0];
  model_coefficients[3] = 0;

  // Normalize
  model_coefficients.normalize ();

  // ... + d = 0
  model_coefficients[3] = -1 * (model_coefficients.template head<4>().dot (p0.matrix ()));

  return (true);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelPlane<PointT>::getDistancesToModel (
      const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
  // Needs a valid set of model coefficients
  if (model_coefficients.size () != 4)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelPlane::getDistancesToModel] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ());
    return;
  }

  distances.resize (indices_->size ());

  // Iterate through the 3d points and calculate the distances from them to the plane
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    // Calculate the distance from the point to the plane normal as the dot product
    // D = (P-A).N/|N|
    /*distances[i] = fabs (model_coefficients[0] * input_->points[(*indices_)[i]].x +
                         model_coefficients[1] * input_->points[(*indices_)[i]].y +
                         model_coefficients[2] * input_->points[(*indices_)[i]].z +
                         model_coefficients[3]);*/
    Eigen::Vector4f pt (input_->points[(*indices_)[i]].x,
                        input_->points[(*indices_)[i]].y,
                        input_->points[(*indices_)[i]].z,
                        1);
    distances[i] = fabs (model_coefficients.dot (pt));
  }
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelPlane<PointT>::selectWithinDistance (
      const Eigen::VectorXf &model_coefficients, double threshold, std::vector<int> &inliers)
{
  // Needs a valid set of model coefficients
  if (model_coefficients.size () != 4)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelPlane::selectWithinDistance] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ());
    return;
  }

  int nr_p = 0;
  inliers.resize (indices_->size ());

  // Iterate through the 3d points and calculate the distances from them to the plane
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    // Calculate the distance from the point to the plane normal as the dot product
    // D = (P-A).N/|N|
    Eigen::Vector4f pt (input_->points[(*indices_)[i]].x,
                        input_->points[(*indices_)[i]].y,
                        input_->points[(*indices_)[i]].z,
                        1);
    if (fabs (model_coefficients.dot (pt)) < threshold)
    {
      // Returns the indices of the points whose distances are smaller than the threshold
      inliers[nr_p] = (*indices_)[i];
      nr_p++;
    }
  }
  inliers.resize (nr_p);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelPlane<PointT>::optimizeModelCoefficients (
      const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
{
  // Needs a valid set of model coefficients
  if (model_coefficients.size () != 4)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ());
    optimized_coefficients = model_coefficients;
    return;
  }

  // Need at least 3 points to estimate a plane
  if (inliers.size () < 4)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", (unsigned long)inliers.size ());
    optimized_coefficients = model_coefficients;
    return;
  }

  Eigen::Vector4f plane_parameters;

  // Use Least-Squares to fit the plane through all the given sample points and find out its coefficients
  EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
  Eigen::Vector4f xyz_centroid;

  // Estimate the XYZ centroid
  compute3DCentroid (*input_, inliers, xyz_centroid);
  xyz_centroid[3] = 0;

  // Compute the 3x3 covariance matrix
  computeCovarianceMatrix (*input_, inliers, xyz_centroid, covariance_matrix);

  // Compute the model coefficients
  EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
  EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
  pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);

  // Hessian form (D = nc . p_plane (centroid here) + p)
  optimized_coefficients.resize (4);
  optimized_coefficients[0] = eigen_vectors (0, 0);
  optimized_coefficients[1] = eigen_vectors (1, 0);
  optimized_coefficients[2] = eigen_vectors (2, 0);
  optimized_coefficients[3] = 0;
  optimized_coefficients[3] = -1 * optimized_coefficients.dot (xyz_centroid);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelPlane<PointT>::projectPoints (
      const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields)
{
  // Needs a valid set of model coefficients
  if (model_coefficients.size () != 4)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelPlane::projectPoints] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ());
    return;
  }

  projected_points.header = input_->header;
  projected_points.is_dense = input_->is_dense;

  Eigen::Vector4f mc (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);

  // normalize the vector perpendicular to the plane...
  mc.normalize ();
  // ... and store the resulting normal as a local copy of the model coefficients
  Eigen::Vector4f tmp_mc = model_coefficients;
  tmp_mc[0] = mc[0];
  tmp_mc[1] = mc[1];
  tmp_mc[2] = mc[2];

  // Copy all the data fields from the input cloud to the projected one?
  if (copy_data_fields)
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (input_->points.size ());
    projected_points.width    = input_->width;
    projected_points.height   = input_->height;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < input_->points.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the plane
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      // Calculate the distance from the point to the plane
      Eigen::Vector4f p (input_->points[inliers[i]].x,
                         input_->points[inliers[i]].y,
                         input_->points[inliers[i]].z,
                         1);
      // use normalized coefficients to calculate the scalar projection
      float distance_to_plane = tmp_mc.dot (p);

      pcl::Vector4fMap pp = projected_points.points[inliers[i]].getVector4fMap ();
      pp = p - mc * distance_to_plane;        // mc[3] = 0, therefore the 3rd coordinate is safe
    }
  }
  else
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (inliers.size ());
    projected_points.width    = inliers.size ();
    projected_points.height   = 1;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < inliers.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the plane
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      // Calculate the distance from the point to the plane
      Eigen::Vector4f p (input_->points[inliers[i]].x,
                         input_->points[inliers[i]].y,
                         input_->points[inliers[i]].z,
                         1);
      // use normalized coefficients to calculate the scalar projection
      float distance_to_plane = tmp_mc.dot (p);

      pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap ();
      pp = p - mc * distance_to_plane;        // mc[3] = 0, therefore the 3rd coordinate is safe
    }
  }
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelPlane<PointT>::doSamplesVerifyModel (
      const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, double threshold)
{
  // Needs a valid set of model coefficients
  if (model_coefficients.size () != 4)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelPlane::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ());
    return (false);
  }

  for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
  {
    Eigen::Vector4f pt (input_->points[*it].x,
                        input_->points[*it].y,
                        input_->points[*it].z,
                        1);
    if (fabs (model_coefficients.dot (pt)) > threshold)
      return (false);
  }

  return (true);
}

#define PCL_INSTANTIATE_SampleConsensusModelPlane(T) template class PCL_EXPORTS pcl::SampleConsensusModelPlane<T>;

#endif    // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_PLANE_H_

<|endoftext|>


<commit_before>/****************************************************************************
**
** Copyright (C) 2009 Nokia Corporation and/or its subsidiary(-ies).
** All rights reserved.
** Contact: Nokia Corporation (qt-info@nokia.com)
**
** This file is part of the Qt Mobility Components.
**
** $QT_BEGIN_LICENSE:LGPL$
** No Commercial Usage
** This file contains pre-release code and may not be distributed.
** You may use this file in accordance with the terms and conditions
** contained in the Technology Preview License Agreement accompanying
** this package.
**
** 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.
**
** In addition, as a special exception, Nokia gives you certain additional
** rights.  These rights are described in the Nokia Qt LGPL Exception
** version 1.1, included in the file LGPL_EXCEPTION.txt in this package.
**
** If you have questions regarding the use of this file, please contact
** Nokia at qt-info@nokia.com.
**
**
**
**
**
**
**
**
** $QT_END_LICENSE$
**
****************************************************************************/
#include <smsclnt.h>
#include <smuthdr.h>        // CSmsHeader
#include <rsendas.h>
#include <rsendasmessage.h>
#include <f32file.h>
#include <mmsconst.h>
#include <utf.h>            // CnvUtfConverter

#include "qmessageservice.h"
#include "qmessageservice_symbian_p.h"
#include "qmtmengine_symbian_p.h"
#include "qmessage_symbian_p.h"

QTM_BEGIN_NAMESPACE

QMessageServicePrivate::QMessageServicePrivate(QMessageService* parent)
 : q_ptr(parent),
   _state(QMessageService::InactiveState),
   _active(false)
{
}

QMessageServicePrivate::~QMessageServicePrivate()
{
}

bool QMessageServicePrivate::sendSMS(QMessage &message)
{
    return CMTMEngine::instance()->sendSMS(message);
}

bool QMessageServicePrivate::sendMMS(QMessage &message)
{
    return CMTMEngine::instance()->sendMMS(message);
}

bool QMessageServicePrivate::sendEmail(QMessage &message)
{
    return CMTMEngine::instance()->sendEmail(message);
}

bool QMessageServicePrivate::show(const QMessageId& id)
{
	return CMTMEngine::instance()->showMessage(id);
}

bool QMessageServicePrivate::compose(const QMessage &message)
{
	return CMTMEngine::instance()->composeMessage(message);
}

bool QMessageServicePrivate::queryMessages(const QMessageFilter &filter, const QMessageSortOrder &sortOrder, uint limit, uint offset) const
{
    return CMTMEngine::instance()->queryMessages((QMessageServicePrivate&)*this, filter, sortOrder, limit, offset);
}

bool QMessageServicePrivate::queryMessages(const QMessageFilter &filter, const QString &body, QMessageDataComparator::MatchFlags matchFlags, const QMessageSortOrder &sortOrder, uint limit, uint offset) const
{
    return CMTMEngine::instance()->queryMessages((QMessageServicePrivate&)*this, filter, body, matchFlags, sortOrder, limit, offset);
}

bool QMessageServicePrivate::countMessages(const QMessageFilter &filter)
{
    return CMTMEngine::instance()->countMessages((QMessageServicePrivate&)*this, filter);
}

bool QMessageServicePrivate::retrieve(const QMessageId &messageId, const QMessageContentContainerId &id)
{
	return CMTMEngine::instance()->retrieve(messageId, id);
}

bool QMessageServicePrivate::retrieveBody(const QMessageId& id)
{
	return CMTMEngine::instance()->retrieveBody(id);
}

bool QMessageServicePrivate::retrieveHeader(const QMessageId& id)
{
	return CMTMEngine::instance()->retrieveHeader(id);
}

bool QMessageServicePrivate::exportUpdates(const QMessageAccountId &id)
{
    return CMTMEngine::instance()->exportUpdates(id);
}

void QMessageServicePrivate::setFinished(bool successful)
{
    if (!successful && (_error == QMessageManager::NoError)) {
        // We must report an error of some sort
        _error = QMessageManager::RequestIncomplete;
    }

    _state = QMessageService::FinishedState;
    emit q_ptr->stateChanged(_state);
    _active = false;
}

QMessageService::QMessageService(QObject *parent)
    : QObject(parent),
    d_ptr(new QMessageServicePrivate(this))
{
	connect(d_ptr, SIGNAL(stateChanged(QMessageService::State)), this, SIGNAL(stateChanged(QMessageService::State)));
	connect(d_ptr, SIGNAL(messagesFound(const QMessageIdList&)), this, SIGNAL(messagesFound(const QMessageIdList&)));
    connect(d_ptr, SIGNAL(messagesCounted(int)), this, SIGNAL(messagesCounted(int)));
	connect(d_ptr, SIGNAL(progressChanged(uint, uint)), this, SIGNAL(progressChanged(uint, uint)));
}

QMessageService::~QMessageService()
{
}

bool QMessageService::queryMessages(const QMessageFilter &filter, const QMessageSortOrder &sortOrder, uint limit, uint offset)
{
    if (d_ptr->_active) {
        return false;
    }

    d_ptr->_active = true;
    d_ptr->_error = QMessageManager::NoError;

    if (d_ptr->queryMessages(filter, sortOrder, limit, offset)) {
        d_ptr->_state = QMessageService::ActiveState;
        emit stateChanged(d_ptr->_state);
    } else {
        d_ptr->setFinished(false);
    }

    return d_ptr->_active;
}

bool QMessageService::queryMessages(const QMessageFilter &filter, const QString &body, QMessageDataComparator::MatchFlags matchFlags, const QMessageSortOrder &sortOrder, uint limit, uint offset)
{
    if (d_ptr->_active) {
        return false;
    }

    d_ptr->_active = true;
    d_ptr->_error = QMessageManager::NoError;

    if (d_ptr->queryMessages(filter, body, matchFlags, sortOrder, limit, offset)) {
        d_ptr->_state = QMessageService::ActiveState;
        emit stateChanged(d_ptr->_state);
    } else {
        d_ptr->setFinished(false);
    }

    return d_ptr->_active;
}

bool QMessageService::countMessages(const QMessageFilter &filter)
{
    if (d_ptr->_active) {
        return false;
    }

    d_ptr->_active = true;
    d_ptr->_error = QMessageManager::NoError;

    if (d_ptr->countMessages(filter)) {
        d_ptr->_state = QMessageService::ActiveState;
        emit stateChanged(d_ptr->_state);
    } else {
        d_ptr->setFinished(false);
    }

    return d_ptr->_active;
}

bool QMessageService::send(QMessage &message)
{
	if (d_ptr->_active) {
		return false;
	}

	d_ptr->_active = true;
	d_ptr->_error = QMessageManager::NoError;

    bool retVal = true;

    d_ptr->_state = QMessageService::ActiveState;
    emit stateChanged(d_ptr->_state);

    // Check message type
    if(message.type() == QMessage::AnyType || message.type() == QMessage::NoType) {
        d_ptr->_error = QMessageManager::ConstraintFailure;
        retVal = false;
    }

    QMessageAccountId accountId = message.parentAccountId();
    if (retVal) {
        // Check account
        if (!accountId.isValid()) {
            accountId = QMessageAccount::defaultAccount(message.type());
            if (!accountId.isValid()) {
                d_ptr->_error = QMessageManager::InvalidId;
                retVal = false;
            }
        }
    }

    QMessageAccount account(accountId);
    if (retVal) {
        // Check account/message type compatibility
        if (!(account.messageTypes() & message.type())) {
            d_ptr->_error = QMessageManager::ConstraintFailure;
            retVal = false;
        }
    }

    if (retVal) {
        // Check recipients
        QMessageAddressList recipients = message.to() + message.bcc() + message.cc();
        if (recipients.isEmpty()) {
            d_ptr->_error = QMessageManager::ConstraintFailure;
            return false;
        }
    }

    QMessage outgoing(message);

    // Set default account if unset
    if (!outgoing.parentAccountId().isValid()) {
        outgoing.setParentAccountId(accountId);
    }

    if (retVal) {
        if (account.messageTypes() & QMessage::Sms) {
            retVal = d_ptr->sendSMS(outgoing);
        } else if (account.messageTypes() & QMessage::Mms) {
            retVal = d_ptr->sendMMS(outgoing);
        } else if (account.messageTypes() & QMessage::Email) {
            retVal = d_ptr->sendEmail(outgoing);
        }
    }

    d_ptr->setFinished(retVal);
    return retVal;
}

bool QMessageService::compose(const QMessage &message)
{
	if (d_ptr->_active) {
		return false;
	}

	d_ptr->_active = true;
	d_ptr->_error = QMessageManager::NoError;

	bool retVal = true;
	d_ptr->_state = QMessageService::ActiveState;
	emit stateChanged(d_ptr->_state);

	retVal = d_ptr->compose(message);

    d_ptr->setFinished(retVal);
    return retVal;
}

bool QMessageService::retrieveHeader(const QMessageId& id)
{
	if (d_ptr->_active) {
		return false;
	}

    if (!id.isValid()) {
        d_ptr->_error = QMessageManager::InvalidId;
        return false;
    }

	d_ptr->_active = true;
	d_ptr->_error = QMessageManager::NoError;

	bool retVal = true;
	d_ptr->_state = QMessageService::ActiveState;
	emit stateChanged(d_ptr->_state);

	retVal = d_ptr->retrieveHeader(id);

    d_ptr->setFinished(retVal);
    return retVal;
}

bool QMessageService::retrieveBody(const QMessageId& id)
{
	if (d_ptr->_active) {
		return false;
	}

    if (!id.isValid()) {
        d_ptr->_error = QMessageManager::InvalidId;
        return false;
    }

	d_ptr->_active = true;
	d_ptr->_error = QMessageManager::NoError;

	bool retVal = true;
	d_ptr->_state = QMessageService::ActiveState;
	emit stateChanged(d_ptr->_state);

	retVal = d_ptr->retrieveBody(id);

    d_ptr->setFinished(retVal);
    return retVal;
}

bool QMessageService::retrieve(const QMessageId &messageId, const QMessageContentContainerId& id)
{
	if (d_ptr->_active) {
		return false;
	}

    if (!messageId.isValid() || !id.isValid()) {
        d_ptr->_error = QMessageManager::InvalidId;
        return false;
    }

	d_ptr->_active = true;
	d_ptr->_error = QMessageManager::NoError;

	bool retVal = true;
	d_ptr->_state = QMessageService::ActiveState;
	emit stateChanged(d_ptr->_state);

	retVal = d_ptr->retrieve(messageId, id);

    d_ptr->setFinished(retVal);
    return retVal;
}

bool QMessageService::show(const QMessageId& id)
{
	if (d_ptr->_active) {
		return false;
	}

    if (!id.isValid()) {
        d_ptr->_error = QMessageManager::InvalidId;
        return false;
    }

	d_ptr->_active = true;
	d_ptr->_error = QMessageManager::NoError;

	bool retVal = true;
	d_ptr->_state = QMessageService::ActiveState;
	emit stateChanged(d_ptr->_state);

    retVal = d_ptr->show(id);

    d_ptr->setFinished(retVal);
    return retVal;
}

bool QMessageService::exportUpdates(const QMessageAccountId &id)
{
    if (d_ptr->_active) {
        return false;
    }

    if (!id.isValid()) {
        d_ptr->_error = QMessageManager::InvalidId;
        return false;
    }

    d_ptr->_active = true;
    d_ptr->_error = QMessageManager::NoError;

    bool retVal = true;
    d_ptr->_state = QMessageService::ActiveState;
    emit stateChanged(d_ptr->_state);

    retVal = d_ptr->exportUpdates(id);

    d_ptr->setFinished(retVal);
    return retVal;
}

QMessageService::State QMessageService::state() const
{
	return d_ptr->_state;
}

void QMessageService::cancel()
{
}

QMessageManager::Error QMessageService::error() const
{
    return d_ptr->_error;
}

#include "moc_qmessageservice_symbian_p.cpp"
QTM_END_NAMESPACE
<commit_msg>Fixed message type recognition from parentAccount when message is send<commit_after>/****************************************************************************
**
** Copyright (C) 2009 Nokia Corporation and/or its subsidiary(-ies).
** All rights reserved.
** Contact: Nokia Corporation (qt-info@nokia.com)
**
** This file is part of the Qt Mobility Components.
**
** $QT_BEGIN_LICENSE:LGPL$
** No Commercial Usage
** This file contains pre-release code and may not be distributed.
** You may use this file in accordance with the terms and conditions
** contained in the Technology Preview License Agreement accompanying
** this package.
**
** 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.
**
** In addition, as a special exception, Nokia gives you certain additional
** rights.  These rights are described in the Nokia Qt LGPL Exception
** version 1.1, included in the file LGPL_EXCEPTION.txt in this package.
**
** If you have questions regarding the use of this file, please contact
** Nokia at qt-info@nokia.com.
**
**
**
**
**
**
**
**
** $QT_END_LICENSE$
**
****************************************************************************/
#include <smsclnt.h>
#include <smuthdr.h>        // CSmsHeader
#include <rsendas.h>
#include <rsendasmessage.h>
#include <f32file.h>
#include <mmsconst.h>
#include <utf.h>            // CnvUtfConverter

#include "qmessageservice.h"
#include "qmessageservice_symbian_p.h"
#include "qmtmengine_symbian_p.h"
#include "qmessage_symbian_p.h"

QTM_BEGIN_NAMESPACE

QMessageServicePrivate::QMessageServicePrivate(QMessageService* parent)
 : q_ptr(parent),
   _state(QMessageService::InactiveState),
   _active(false)
{
}

QMessageServicePrivate::~QMessageServicePrivate()
{
}

bool QMessageServicePrivate::sendSMS(QMessage &message)
{
    return CMTMEngine::instance()->sendSMS(message);
}

bool QMessageServicePrivate::sendMMS(QMessage &message)
{
    return CMTMEngine::instance()->sendMMS(message);
}

bool QMessageServicePrivate::sendEmail(QMessage &message)
{
    return CMTMEngine::instance()->sendEmail(message);
}

bool QMessageServicePrivate::show(const QMessageId& id)
{
	return CMTMEngine::instance()->showMessage(id);
}

bool QMessageServicePrivate::compose(const QMessage &message)
{
	return CMTMEngine::instance()->composeMessage(message);
}

bool QMessageServicePrivate::queryMessages(const QMessageFilter &filter, const QMessageSortOrder &sortOrder, uint limit, uint offset) const
{
    return CMTMEngine::instance()->queryMessages((QMessageServicePrivate&)*this, filter, sortOrder, limit, offset);
}

bool QMessageServicePrivate::queryMessages(const QMessageFilter &filter, const QString &body, QMessageDataComparator::MatchFlags matchFlags, const QMessageSortOrder &sortOrder, uint limit, uint offset) const
{
    return CMTMEngine::instance()->queryMessages((QMessageServicePrivate&)*this, filter, body, matchFlags, sortOrder, limit, offset);
}

bool QMessageServicePrivate::countMessages(const QMessageFilter &filter)
{
    return CMTMEngine::instance()->countMessages((QMessageServicePrivate&)*this, filter);
}

bool QMessageServicePrivate::retrieve(const QMessageId &messageId, const QMessageContentContainerId &id)
{
	return CMTMEngine::instance()->retrieve(messageId, id);
}

bool QMessageServicePrivate::retrieveBody(const QMessageId& id)
{
	return CMTMEngine::instance()->retrieveBody(id);
}

bool QMessageServicePrivate::retrieveHeader(const QMessageId& id)
{
	return CMTMEngine::instance()->retrieveHeader(id);
}

bool QMessageServicePrivate::exportUpdates(const QMessageAccountId &id)
{
    return CMTMEngine::instance()->exportUpdates(id);
}

void QMessageServicePrivate::setFinished(bool successful)
{
    if (!successful && (_error == QMessageManager::NoError)) {
        // We must report an error of some sort
        _error = QMessageManager::RequestIncomplete;
    }

    _state = QMessageService::FinishedState;
    emit q_ptr->stateChanged(_state);
    _active = false;
}

QMessageService::QMessageService(QObject *parent)
    : QObject(parent),
    d_ptr(new QMessageServicePrivate(this))
{
	connect(d_ptr, SIGNAL(stateChanged(QMessageService::State)), this, SIGNAL(stateChanged(QMessageService::State)));
	connect(d_ptr, SIGNAL(messagesFound(const QMessageIdList&)), this, SIGNAL(messagesFound(const QMessageIdList&)));
    connect(d_ptr, SIGNAL(messagesCounted(int)), this, SIGNAL(messagesCounted(int)));
	connect(d_ptr, SIGNAL(progressChanged(uint, uint)), this, SIGNAL(progressChanged(uint, uint)));
}

QMessageService::~QMessageService()
{
}

bool QMessageService::queryMessages(const QMessageFilter &filter, const QMessageSortOrder &sortOrder, uint limit, uint offset)
{
    if (d_ptr->_active) {
        return false;
    }

    d_ptr->_active = true;
    d_ptr->_error = QMessageManager::NoError;

    if (d_ptr->queryMessages(filter, sortOrder, limit, offset)) {
        d_ptr->_state = QMessageService::ActiveState;
        emit stateChanged(d_ptr->_state);
    } else {
        d_ptr->setFinished(false);
    }

    return d_ptr->_active;
}

bool QMessageService::queryMessages(const QMessageFilter &filter, const QString &body, QMessageDataComparator::MatchFlags matchFlags, const QMessageSortOrder &sortOrder, uint limit, uint offset)
{
    if (d_ptr->_active) {
        return false;
    }

    d_ptr->_active = true;
    d_ptr->_error = QMessageManager::NoError;

    if (d_ptr->queryMessages(filter, body, matchFlags, sortOrder, limit, offset)) {
        d_ptr->_state = QMessageService::ActiveState;
        emit stateChanged(d_ptr->_state);
    } else {
        d_ptr->setFinished(false);
    }

    return d_ptr->_active;
}

bool QMessageService::countMessages(const QMessageFilter &filter)
{
    if (d_ptr->_active) {
        return false;
    }

    d_ptr->_active = true;
    d_ptr->_error = QMessageManager::NoError;

    if (d_ptr->countMessages(filter)) {
        d_ptr->_state = QMessageService::ActiveState;
        emit stateChanged(d_ptr->_state);
    } else {
        d_ptr->setFinished(false);
    }

    return d_ptr->_active;
}

bool QMessageService::send(QMessage &message)
{
	if (d_ptr->_active) {
		return false;
	}

	d_ptr->_active = true;
	d_ptr->_error = QMessageManager::NoError;

    bool retVal = true;

    d_ptr->_state = QMessageService::ActiveState;
    emit stateChanged(d_ptr->_state);

    QMessageAccountId accountId = message.parentAccountId();
    QMessage::Type msgType = QMessage::NoType;

    // Check message type
    if (message.type() == QMessage::AnyType || message.type() == QMessage::NoType) {
        QMessage::TypeFlags types = QMessage::NoType;
        if (accountId.isValid()) {
            // ParentAccountId was defined => Message type can be read
            // from parent account
            QMessageAccount account = QMessageAccount(accountId);
            QMessage::TypeFlags types = account.messageTypes();
            if (types & QMessage::Sms) {
                msgType = QMessage::Sms;
            } else if (types & QMessage::Mms) {
                msgType = QMessage::Mms;
            } else if (types & QMessage::Email) {
                msgType = QMessage::Email;
            }
        }
        if (msgType == QMessage::NoType) {
            d_ptr->_error = QMessageManager::ConstraintFailure;
            retVal = false;
        }
    }

    if (retVal) {
        // Check account
        if (!accountId.isValid()) {
            accountId = QMessageAccount::defaultAccount(message.type());
            if (!accountId.isValid()) {
                d_ptr->_error = QMessageManager::InvalidId;
                retVal = false;
            }
        }
    }

    QMessageAccount account(accountId);
    if (retVal) {
        // Check account/message type compatibility
        if (!(account.messageTypes() & message.type()) && (msgType == QMessage::NoType)) {
            d_ptr->_error = QMessageManager::ConstraintFailure;
            retVal = false;
        }
    }

    if (retVal) {
        // Check recipients
        QMessageAddressList recipients = message.to() + message.bcc() + message.cc();
        if (recipients.isEmpty()) {
            d_ptr->_error = QMessageManager::ConstraintFailure;
            return false;
        }
    }

    if (retVal) {
        QMessage outgoing(message);

        // Set default account if unset
        if (!outgoing.parentAccountId().isValid()) {
            outgoing.setParentAccountId(accountId);
        }

        if (outgoing.type() == QMessage::AnyType || outgoing.type() == QMessage::NoType) {
            outgoing.setType(msgType);
        }

        if (account.messageTypes() & QMessage::Sms) {
            retVal = d_ptr->sendSMS(outgoing);
        } else if (account.messageTypes() & QMessage::Mms) {
            retVal = d_ptr->sendMMS(outgoing);
        } else if (account.messageTypes() & QMessage::Email) {
            retVal = d_ptr->sendEmail(outgoing);
        }
    }

    d_ptr->setFinished(retVal);
    return retVal;
}

bool QMessageService::compose(const QMessage &message)
{
	if (d_ptr->_active) {
		return false;
	}

	d_ptr->_active = true;
	d_ptr->_error = QMessageManager::NoError;

	bool retVal = true;
	d_ptr->_state = QMessageService::ActiveState;
	emit stateChanged(d_ptr->_state);

	retVal = d_ptr->compose(message);

    d_ptr->setFinished(retVal);
    return retVal;
}

bool QMessageService::retrieveHeader(const QMessageId& id)
{
	if (d_ptr->_active) {
		return false;
	}

    if (!id.isValid()) {
        d_ptr->_error = QMessageManager::InvalidId;
        return false;
    }

	d_ptr->_active = true;
	d_ptr->_error = QMessageManager::NoError;

	bool retVal = true;
	d_ptr->_state = QMessageService::ActiveState;
	emit stateChanged(d_ptr->_state);

	retVal = d_ptr->retrieveHeader(id);

    d_ptr->setFinished(retVal);
    return retVal;
}

bool QMessageService::retrieveBody(const QMessageId& id)
{
	if (d_ptr->_active) {
		return false;
	}

    if (!id.isValid()) {
        d_ptr->_error = QMessageManager::InvalidId;
        return false;
    }

	d_ptr->_active = true;
	d_ptr->_error = QMessageManager::NoError;

	bool retVal = true;
	d_ptr->_state = QMessageService::ActiveState;
	emit stateChanged(d_ptr->_state);

	retVal = d_ptr->retrieveBody(id);

    d_ptr->setFinished(retVal);
    return retVal;
}

bool QMessageService::retrieve(const QMessageId &messageId, const QMessageContentContainerId& id)
{
	if (d_ptr->_active) {
		return false;
	}

    if (!messageId.isValid() || !id.isValid()) {
        d_ptr->_error = QMessageManager::InvalidId;
        return false;
    }

	d_ptr->_active = true;
	d_ptr->_error = QMessageManager::NoError;

	bool retVal = true;
	d_ptr->_state = QMessageService::ActiveState;
	emit stateChanged(d_ptr->_state);

	retVal = d_ptr->retrieve(messageId, id);

    d_ptr->setFinished(retVal);
    return retVal;
}

bool QMessageService::show(const QMessageId& id)
{
	if (d_ptr->_active) {
		return false;
	}

    if (!id.isValid()) {
        d_ptr->_error = QMessageManager::InvalidId;
        return false;
    }

	d_ptr->_active = true;
	d_ptr->_error = QMessageManager::NoError;

	bool retVal = true;
	d_ptr->_state = QMessageService::ActiveState;
	emit stateChanged(d_ptr->_state);

    retVal = d_ptr->show(id);

    d_ptr->setFinished(retVal);
    return retVal;
}

bool QMessageService::exportUpdates(const QMessageAccountId &id)
{
    if (d_ptr->_active) {
        return false;
    }

    if (!id.isValid()) {
        d_ptr->_error = QMessageManager::InvalidId;
        return false;
    }

    d_ptr->_active = true;
    d_ptr->_error = QMessageManager::NoError;

    bool retVal = true;
    d_ptr->_state = QMessageService::ActiveState;
    emit stateChanged(d_ptr->_state);

    retVal = d_ptr->exportUpdates(id);

    d_ptr->setFinished(retVal);
    return retVal;
}

QMessageService::State QMessageService::state() const
{
	return d_ptr->_state;
}

void QMessageService::cancel()
{
}

QMessageManager::Error QMessageService::error() const
{
    return d_ptr->_error;
}

#include "moc_qmessageservice_symbian_p.cpp"
QTM_END_NAMESPACE
<|endoftext|>


<commit_before>#include "State.h"
#include <iomanip>
#include <ostream>
using namespace nspace;
using namespace std;

namespace nspace{
std::ostream& operator<<(std::ostream & out, const State & state){

  int width = 16;
  out << std::setw(20)<<"index\\derivative";
  out << "||";
  for(int i=0;i < state.derivatives(); i++){
    out << std::setw(width)<<i;
    out << "|";
  }
  out <<endl;
  out << setw((width+1)* state.derivatives() + 22)<< setfill('-')<< "-";
  out << setfill(' ')<<endl;
  for(int i=0; i < state.dimension(); i++){
    out << std::setw(20)<<i;
    out << "||";
    for(int d=0; d < state.derivatives(); d++){
      out << std::setw(width) << std::setprecision(5) << state(i,d);
      out <<"|";
    }
    out << std::endl;
  }
  return out;
}

}

State::State(uint offset, uint dimension, uint derivatives, State & parent):_parent(&parent),_offset(offset),_derivatives(derivatives),_stateMatrix(parent._stateMatrix),_dimension(dimension){ }

State::State():_parent(0),_derivatives(0),_offset(0),_dimension(0),_stateMatrix(0){ }<commit_msg>int -> uint<commit_after>#include "State.h"
#include <iomanip>
#include <ostream>
using namespace nspace;
using namespace std;

namespace nspace{
std::ostream& operator<<(std::ostream & out, const State & state){

  int width = 16;
  out << std::setw(20)<<"index\\derivative";
  out << "||";
  for(int i=0;i < state.derivatives(); i++){
    out << std::setw(width)<<i;
    out << "|";
  }
  out <<endl;
  out << setw((width+1)* state.derivatives() + 22)<< setfill('-')<< "-";
  out << setfill(' ')<<endl;
  for(uint i=0; i < state.dimension(); i++){
    out << std::setw(20)<<i;
    out << "||";
    for(uint d=0; d < state.derivatives(); d++){
      out << std::setw(width) << std::setprecision(5) << state(i,d);
      out <<"|";
    }
    out << std::endl;
  }
  return out;
}

}

State::State(uint offset, uint dimension, uint derivatives, State & parent):_parent(&parent),_offset(offset),_derivatives(derivatives),_stateMatrix(parent._stateMatrix),_dimension(dimension){ }

State::State():_parent(0),_derivatives(0),_offset(0),_dimension(0),_stateMatrix(0){ }<|endoftext|>


<commit_before>///
/// @file   ParallelPrimeSieve.cpp
/// @brief  ParallelPrimeSieve sieves primes in parallel using
///         OpenMP 2.0 (2002) or later.
///
/// Copyright (C) 2013 Kim Walisch, <kim.walisch@gmail.com>
///
/// This file is distributed under the BSD License. See the COPYING
/// file in the top level directory.
///

#include <primesieve/soe/config.h>
#include <primesieve/soe/ParallelPrimeSieve.h>
#include <primesieve/soe/PrimeSieve.h>
#include <primesieve/soe/imath.h>

#include <stdint.h>
#include <cstdlib>
#include <cassert>
#include <algorithm>

#ifdef _OPENMP
  #include <omp.h>
  #include <primesieve/soe/ParallelPrimeSieve-lock.h>
#endif

using namespace soe;

ParallelPrimeSieve::ParallelPrimeSieve() :
  numThreads_(DEFAULT_NUM_THREADS),
  shm_(NULL)
{ }

void ParallelPrimeSieve::init(SharedMemory& shm)
{
  setStart(shm.start);
  setStop(shm.stop);
  setSieveSize(shm.sieveSize);
  setFlags(shm.flags);
  setNumThreads(shm.threads);
  shm_ = &shm;
}

int ParallelPrimeSieve::getNumThreads() const
{
  if (numThreads_ == DEFAULT_NUM_THREADS) {
    // Use 1 thread to generate primes in arithmetic order
    return (isPrint() || isGenerate()) ? 1 : idealNumThreads();
  }
  return numThreads_;
}

void ParallelPrimeSieve::setNumThreads(int threads)
{
  numThreads_ = getInBetween(1, threads, getMaxThreads());
}

/// Get an ideal number of threads for the current
/// set start_ and stop_ numbers.
///
int ParallelPrimeSieve::idealNumThreads() const
{
  if (start_ > stop_)
    return 1;
  uint64_t threshold = std::max(config::MIN_THREAD_INTERVAL, isqrt(stop_) / 5);
  uint64_t threads = getInterval() / threshold;
  threads = getInBetween<uint64_t>(1, threads, getMaxThreads());
  return static_cast<int>(threads);
}

/// Get an interval size that ensures a good load balance
/// when multiple threads are used.
///
uint64_t ParallelPrimeSieve::getThreadInterval(int threads) const
{
  assert(threads > 0);
  uint64_t unbalanced = getInterval() / threads;
  uint64_t balanced = isqrt(stop_) * 1000;
  uint64_t fastest = std::min(balanced, unbalanced);
  uint64_t threadInterval = getInBetween(config::MIN_THREAD_INTERVAL, fastest, config::MAX_THREAD_INTERVAL);
  uint64_t chunks = getInterval() / threadInterval;
  if (chunks < threads * 5u)
    threadInterval = std::max(config::MIN_THREAD_INTERVAL, unbalanced);
  threadInterval += 30 - threadInterval % 30;
  return threadInterval;
}

/// Align n to modulo 30 + 2 to prevent prime k-tuplet
/// (twin primes, prime triplets, ...) gaps.
///
uint64_t ParallelPrimeSieve::align(uint64_t n) const
{
  if (n == start_)
    return start_;
  n = std::min(n + 32 - n % 30, stop_);
  return n;
}

bool ParallelPrimeSieve::tooMany(int threads) const
{
  return (threads > 1 && getInterval() / threads < config::MIN_THREAD_INTERVAL);
}

#ifdef _OPENMP

int ParallelPrimeSieve::getMaxThreads()
{
  return omp_get_max_threads();
}

double ParallelPrimeSieve::getWallTime() const
{
  return omp_get_wtime();
}

/// Sieve the primes and prime k-tuplets within [start_, stop_]
/// in parallel using OpenMP multi-threading.
///
void ParallelPrimeSieve::sieve()
{
  reset();
  if (start_ > stop_)
    return;
  OmpInitLock ompInit(&lock_);

  int threads = getNumThreads();
  if (tooMany(threads))
    threads = idealNumThreads();

  if (threads == 1)
    PrimeSieve::sieve();
  else {
    uint64_t threadInterval = getThreadInterval(threads);
    uint64_t count0 = 0, count1 = 0, count2 = 0, count3 = 0, count4 = 0, count5 = 0, count6 = 0;
    double t1 = getWallTime();

#if _OPENMP >= 200800 /* OpenMP >= 3.0 (2008) */

    #pragma omp parallel for schedule(dynamic) num_threads(threads) \
      reduction(+: count0, count1, count2, count3, count4, count5, count6)
    for (uint64_t n = start_; n < stop_; n += threadInterval) {
      PrimeSieve ps(*this, omp_get_thread_num());
      uint64_t threadStart = align(n);
      uint64_t threadStop  = align(n + threadInterval);
      ps.sieve(threadStart, threadStop);
      count0 += ps.getCount(0);
      count1 += ps.getCount(1);
      count2 += ps.getCount(2);
      count3 += ps.getCount(3);
      count4 += ps.getCount(4);
      count5 += ps.getCount(5);
      count6 += ps.getCount(6);
    }

#else /* OpenMP 2.x */

    int64_t iters = 1 + (getInterval() - 1) / threadInterval;

    #pragma omp parallel for schedule(dynamic) num_threads(threads) \
      reduction(+: count0, count1, count2, count3, count4, count5, count6)
    for (int64_t i = 0; i < iters; i++) {
      PrimeSieve ps(*this, omp_get_thread_num());
      uint64_t n = start_ + i * threadInterval;
      uint64_t threadStart = align(n);
      uint64_t threadStop  = align(n + threadInterval);
      ps.sieve(threadStart, threadStop);
      count0 += ps.getCount(0);
      count1 += ps.getCount(1);
      count2 += ps.getCount(2);
      count3 += ps.getCount(3);
      count4 += ps.getCount(4);
      count5 += ps.getCount(5);
      count6 += ps.getCount(6);
    }

#endif

    seconds_ = getWallTime() - t1;
    counts_[0] = count0;
    counts_[1] = count1;
    counts_[2] = count2;
    counts_[3] = count3;
    counts_[4] = count4;
    counts_[5] = count5;
    counts_[6] = count6;
  }

  // communicate the sieving results to the
  // primesieve GUI application
  if (shm_) {
    std::copy(counts_.begin(), counts_.end(), shm_->counts);
    shm_->seconds = seconds_;
  }
}

/// Calculate the sieving status.
/// @param processed  Sum of recently processed segments.
///
bool ParallelPrimeSieve::updateStatus(uint64_t processed, bool waitForLock)
{
  OmpLockGuard lock(getLock<omp_lock_t*>(), waitForLock);
  if (lock.isSet()) {
    PrimeSieve::updateStatus(processed, false);
    if (shm_)
      shm_->status = getStatus();
  }
  return lock.isSet();
}

/// Used to synchronize threads for prime number generation

void ParallelPrimeSieve::setLock()
{
  omp_lock_t* lock = getLock<omp_lock_t*>();
  omp_set_lock(lock);
}

void ParallelPrimeSieve::unsetLock()
{
  omp_lock_t* lock = getLock<omp_lock_t*>();
  omp_unset_lock(lock);
}

#endif /* _OPENMP */

/// If OpenMP is disabled then ParallelPrimeSieve behaves like
/// the single threaded PrimeSieve.
///
#if !defined(_OPENMP)

int ParallelPrimeSieve::getMaxThreads()
{
  return 1;
}

void ParallelPrimeSieve::sieve()
{
  PrimeSieve::sieve();
}

double ParallelPrimeSieve::getWallTime() const
{
  return PrimeSieve::getWallTime();
}

bool ParallelPrimeSieve::updateStatus(uint64_t processed, bool waitForLock)
{
  return PrimeSieve::updateStatus(processed, waitForLock);
}

void ParallelPrimeSieve::setLock() { }

void ParallelPrimeSieve::unsetLock() { }

#endif
<commit_msg>setNumThreads(-1) uses default number of threads.<commit_after>///
/// @file   ParallelPrimeSieve.cpp
/// @brief  ParallelPrimeSieve sieves primes in parallel using
///         OpenMP 2.0 (2002) or later.
///
/// Copyright (C) 2013 Kim Walisch, <kim.walisch@gmail.com>
///
/// This file is distributed under the BSD License. See the COPYING
/// file in the top level directory.
///

#include <primesieve/soe/config.h>
#include <primesieve/soe/ParallelPrimeSieve.h>
#include <primesieve/soe/PrimeSieve.h>
#include <primesieve/soe/imath.h>

#include <stdint.h>
#include <cstdlib>
#include <cassert>
#include <algorithm>

#ifdef _OPENMP
  #include <omp.h>
  #include <primesieve/soe/ParallelPrimeSieve-lock.h>
#endif

using namespace soe;

ParallelPrimeSieve::ParallelPrimeSieve() :
  numThreads_(DEFAULT_NUM_THREADS),
  shm_(NULL)
{ }

void ParallelPrimeSieve::init(SharedMemory& shm)
{
  setStart(shm.start);
  setStop(shm.stop);
  setSieveSize(shm.sieveSize);
  setFlags(shm.flags);
  setNumThreads(shm.threads);
  shm_ = &shm;
}

int ParallelPrimeSieve::getNumThreads() const
{
  // Use 1 thread if arithmetic order is important
  if (numThreads_ == DEFAULT_NUM_THREADS)
    return (isPrint() || isGenerate()) ? 1 : idealNumThreads();
  return numThreads_;
}

void ParallelPrimeSieve::setNumThreads(int threads)
{
  numThreads_ = threads;
  if (numThreads_ != DEFAULT_NUM_THREADS)
  	numThreads_ = getInBetween(1, numThreads_, getMaxThreads());
}

/// Get an ideal number of threads for the current
/// set start_ and stop_ numbers.
///
int ParallelPrimeSieve::idealNumThreads() const
{
  if (start_ > stop_)
    return 1;
  uint64_t threshold = std::max(config::MIN_THREAD_INTERVAL, isqrt(stop_) / 5);
  uint64_t threads = getInterval() / threshold;
  threads = getInBetween<uint64_t>(1, threads, getMaxThreads());
  return static_cast<int>(threads);
}

/// Get an interval size that ensures a good load balance
/// when multiple threads are used.
///
uint64_t ParallelPrimeSieve::getThreadInterval(int threads) const
{
  assert(threads > 0);
  uint64_t unbalanced = getInterval() / threads;
  uint64_t balanced = isqrt(stop_) * 1000;
  uint64_t fastest = std::min(balanced, unbalanced);
  uint64_t threadInterval = getInBetween(config::MIN_THREAD_INTERVAL, fastest, config::MAX_THREAD_INTERVAL);
  uint64_t chunks = getInterval() / threadInterval;
  if (chunks < threads * 5u)
    threadInterval = std::max(config::MIN_THREAD_INTERVAL, unbalanced);
  threadInterval += 30 - threadInterval % 30;
  return threadInterval;
}

/// Align n to modulo 30 + 2 to prevent prime k-tuplet
/// (twin primes, prime triplets, ...) gaps.
///
uint64_t ParallelPrimeSieve::align(uint64_t n) const
{
  if (n == start_)
    return start_;
  n = std::min(n + 32 - n % 30, stop_);
  return n;
}

bool ParallelPrimeSieve::tooMany(int threads) const
{
  return (threads > 1 && getInterval() / threads < config::MIN_THREAD_INTERVAL);
}

#ifdef _OPENMP

int ParallelPrimeSieve::getMaxThreads()
{
  return omp_get_max_threads();
}

double ParallelPrimeSieve::getWallTime() const
{
  return omp_get_wtime();
}

/// Sieve the primes and prime k-tuplets within [start_, stop_]
/// in parallel using OpenMP multi-threading.
///
void ParallelPrimeSieve::sieve()
{
  reset();
  if (start_ > stop_)
    return;
  OmpInitLock ompInit(&lock_);

  int threads = getNumThreads();
  if (tooMany(threads))
    threads = idealNumThreads();

  if (threads == 1)
    PrimeSieve::sieve();
  else {
    uint64_t threadInterval = getThreadInterval(threads);
    uint64_t count0 = 0, count1 = 0, count2 = 0, count3 = 0, count4 = 0, count5 = 0, count6 = 0;
    double t1 = getWallTime();

#if _OPENMP >= 200800 /* OpenMP >= 3.0 (2008) */

    #pragma omp parallel for schedule(dynamic) num_threads(threads) \
      reduction(+: count0, count1, count2, count3, count4, count5, count6)
    for (uint64_t n = start_; n < stop_; n += threadInterval) {
      PrimeSieve ps(*this, omp_get_thread_num());
      uint64_t threadStart = align(n);
      uint64_t threadStop  = align(n + threadInterval);
      ps.sieve(threadStart, threadStop);
      count0 += ps.getCount(0);
      count1 += ps.getCount(1);
      count2 += ps.getCount(2);
      count3 += ps.getCount(3);
      count4 += ps.getCount(4);
      count5 += ps.getCount(5);
      count6 += ps.getCount(6);
    }

#else /* OpenMP 2.x */

    int64_t iters = 1 + (getInterval() - 1) / threadInterval;

    #pragma omp parallel for schedule(dynamic) num_threads(threads) \
      reduction(+: count0, count1, count2, count3, count4, count5, count6)
    for (int64_t i = 0; i < iters; i++) {
      PrimeSieve ps(*this, omp_get_thread_num());
      uint64_t n = start_ + i * threadInterval;
      uint64_t threadStart = align(n);
      uint64_t threadStop  = align(n + threadInterval);
      ps.sieve(threadStart, threadStop);
      count0 += ps.getCount(0);
      count1 += ps.getCount(1);
      count2 += ps.getCount(2);
      count3 += ps.getCount(3);
      count4 += ps.getCount(4);
      count5 += ps.getCount(5);
      count6 += ps.getCount(6);
    }

#endif

    seconds_ = getWallTime() - t1;
    counts_[0] = count0;
    counts_[1] = count1;
    counts_[2] = count2;
    counts_[3] = count3;
    counts_[4] = count4;
    counts_[5] = count5;
    counts_[6] = count6;
  }

  // communicate the sieving results to the
  // primesieve GUI application
  if (shm_) {
    std::copy(counts_.begin(), counts_.end(), shm_->counts);
    shm_->seconds = seconds_;
  }
}

/// Calculate the sieving status.
/// @param processed  Sum of recently processed segments.
///
bool ParallelPrimeSieve::updateStatus(uint64_t processed, bool waitForLock)
{
  OmpLockGuard lock(getLock<omp_lock_t*>(), waitForLock);
  if (lock.isSet()) {
    PrimeSieve::updateStatus(processed, false);
    if (shm_)
      shm_->status = getStatus();
  }
  return lock.isSet();
}

/// Used to synchronize threads for prime number generation

void ParallelPrimeSieve::setLock()
{
  omp_lock_t* lock = getLock<omp_lock_t*>();
  omp_set_lock(lock);
}

void ParallelPrimeSieve::unsetLock()
{
  omp_lock_t* lock = getLock<omp_lock_t*>();
  omp_unset_lock(lock);
}

#endif /* _OPENMP */

/// If OpenMP is disabled then ParallelPrimeSieve behaves like
/// the single threaded PrimeSieve.
///
#if !defined(_OPENMP)

int ParallelPrimeSieve::getMaxThreads()
{
  return 1;
}

void ParallelPrimeSieve::sieve()
{
  PrimeSieve::sieve();
}

double ParallelPrimeSieve::getWallTime() const
{
  return PrimeSieve::getWallTime();
}

bool ParallelPrimeSieve::updateStatus(uint64_t processed, bool waitForLock)
{
  return PrimeSieve::updateStatus(processed, waitForLock);
}

void ParallelPrimeSieve::setLock() { }

void ParallelPrimeSieve::unsetLock() { }

#endif
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2015 WinT 3794 <http://wint3794.org>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include <QDir>
#include <QFile>
#include <QTimer>
#include <QMessageBox>
#include <QApplication>

#include <math.h>

#include "Settings.h"
#include "GamepadManager.h"

/* Maximum axis value */
#define _MAX_VAL 32767

/* Location of community-maintained joystick mappings */
#define _CONTROLLER_DB ":/sdl/database.txt"

/* Used to create mappings from unsupported controllers */
#if defined _WIN32 || defined _WIN64
#define _GENERIC_MAPPINGS ":/sdl/generic/windows.txt"
#elif defined __APPLE__
#define _GENERIC_MAPPINGS ":/sdl/generic/mac-osx.txt"
#elif defined __gnu_linux__
#define _GENERIC_MAPPINGS ":/sdl/generic/linux.txt"
#endif

#define SDL_MAIN_HANDLED
#define _INIT_CODE SDL_INIT_JOYSTICK | SDL_INIT_GAMECONTROLLER

GamepadManager* GamepadManager::m_instance = nullptr;

//------------------------------------------------------------------------------
// Class initialization functions
//------------------------------------------------------------------------------

GamepadManager::GamepadManager()
{
    SDL_SetHint (SDL_HINT_JOYSTICK_ALLOW_BACKGROUND_EVENTS, "1");

    if (SDL_Init (_INIT_CODE) != 0) {
        QMessageBox::critical (nullptr, tr ("Fatal Error!"),
                               tr ("SDL Init Error: %1").arg (SDL_GetError()));

        exit (EXIT_FAILURE);
    }

    /* Enable event states to use them in the event loop */
    SDL_JoystickEventState (SDL_ENABLE);
    SDL_GameControllerEventState (SDL_ENABLE);

    /* Load community controller database */
    QFile db (_CONTROLLER_DB);
    if (db.open (QFile::ReadOnly)) {
        while (!db.atEnd())
            SDL_GameControllerAddMapping (
                QVariant (db.readLine()).toString().toStdString().c_str());

        db.close();
    }

    /* Load generic mapping string, used for unsupported controllers */
    QFile generic (_GENERIC_MAPPINGS);
    if (generic.open (QFile::ReadOnly)) {
        m_genericMapping = (QString)generic.readAll();
        generic.close();
    }
}

GamepadManager::~GamepadManager()
{
    for (int i = 0; i < SDL_NumJoysticks(); ++i)
        SDL_GameControllerClose (SDL_GameControllerOpen (i));

    SDL_Quit();
}

GamepadManager* GamepadManager::getInstance()
{
    if (m_instance == nullptr)
        m_instance = new GamepadManager();

    return m_instance;
}

//------------------------------------------------------------------------------
// Functions that give information about a selected joystick
//------------------------------------------------------------------------------

int GamepadManager::getNumAxes (int joystick)
{
    return SDL_JoystickNumAxes (SDL_JoystickOpen (joystick));
}

int GamepadManager::getNumButtons (int joystick)
{
    return SDL_JoystickNumButtons (SDL_JoystickOpen (joystick));
}

QString GamepadManager::getAxisName (int axis)
{
    return QString ("Axis %1").arg (axis);
}

QString GamepadManager::getButtonName (int button)
{
    return QString ("Button %1").arg (button);
}

QString GamepadManager::getJoystickName (int joystick)
{
    QString name = (QString) SDL_GameControllerNameForIndex (joystick);
    return name.isEmpty() ? SDL_JoystickNameForIndex (joystick) : name;
}

//------------------------------------------------------------------------------
// Functions that give information about all joysticks
//------------------------------------------------------------------------------

QStringList GamepadManager::joystickList()
{
    QStringList list;

    for (int i = 0; i < SDL_NumJoysticks(); ++i)
        list.append (getJoystickName (i));

    return list;
}

//------------------------------------------------------------------------------
// Functions that need to be called after initalization of UI
//------------------------------------------------------------------------------

void GamepadManager::init()
{
    m_time = 20;
    m_tracker = -1;
    QTimer::singleShot (500, this, SLOT (readSdlEvents()));
}

void GamepadManager::setUpdateInterval (int time)
{
    if (time >= 0)
        m_time = time;
}

//------------------------------------------------------------------------------
// Functions that control behaviour of a specific joystick
//------------------------------------------------------------------------------

void GamepadManager::rumble (int joystick, int time)
{
    SDL_InitSubSystem (SDL_INIT_HAPTIC);
    SDL_Haptic* haptic = SDL_HapticOpen (joystick);

    if (haptic != nullptr) {
        SDL_HapticRumbleInit (haptic);
        SDL_HapticRumblePlay (haptic, 1, time);
    }
}

//------------------------------------------------------------------------------
// Functions that transform SDL events into GamepadManager structures/data
//------------------------------------------------------------------------------

GM_Axis GamepadManager::getAxis (const SDL_Event* event)
{
    GM_Axis axis;

    axis.rawId = event->caxis.axis;
    axis.joystick = getJoystick (event);
    axis.identifier = getAxisName (axis.rawId);
    axis.value = (double) (event->caxis.value) / _MAX_VAL;

    return axis;
}

GM_Button GamepadManager::getButton (const SDL_Event* event)
{
    GM_Button button;

    button.rawId = event->cbutton.button;
    button.joystick = getJoystick (event);
    button.identifier = getButtonName (button.rawId);
    button.pressed = event->cbutton.state == SDL_PRESSED;

    return button;
}

GM_Joystick GamepadManager::getJoystick (const SDL_Event* event)
{
    GM_Joystick stick;

    stick.id = getDynamicId (event->cdevice.which);
    stick.numAxes = getNumAxes (event->cdevice.which);
    stick.numButtons = getNumButtons (event->cdevice.which);
    stick.displayName = getJoystickName (event->cdevice.which);

    return stick;
}

//------------------------------------------------------------------------------
// SDL magic
//------------------------------------------------------------------------------

int GamepadManager::getDynamicId (int id)
{
    id = m_tracker - (id + 1);
    if (id < 0) id = abs (id);
    if (id >= SDL_NumJoysticks()) id -= 1;

    return id;
}

void GamepadManager::readSdlEvents()
{
    SDL_Event event;
    while (SDL_PollEvent (&event)) {
        switch (event.type) {
        case SDL_CONTROLLERDEVICEADDED:
            ++m_tracker;
            onControllerAdded (&event);
            emit countChanged (joystickList());
            emit countChanged (SDL_NumJoysticks());
            break;
        case SDL_CONTROLLERDEVICEREMOVED:
            onControllerRemoved (&event);
            emit countChanged (joystickList());
            emit countChanged (SDL_NumJoysticks());
            break;
        case SDL_CONTROLLERAXISMOTION:
            onAxisEvent (&event);
            break;
        case SDL_CONTROLLERBUTTONDOWN:
            onButtonEvent (&event);
            break;
        case SDL_CONTROLLERBUTTONUP:
            onButtonEvent (&event);
            break;
        }
    }

    QTimer::singleShot (m_time, this, SLOT (readSdlEvents()));
}

//------------------------------------------------------------------------------
// Functions that react to SDL events
//------------------------------------------------------------------------------

void GamepadManager::onAxisEvent (const SDL_Event* event)
{
    emit axisEvent (getAxis (event));
}

void GamepadManager::onButtonEvent (const SDL_Event* event)
{
    emit buttonEvent (getButton (event));
}

void GamepadManager::onControllerAdded (const SDL_Event* event)
{
    if (!SDL_IsGameController (event->cdevice.which)) {
        SDL_Joystick* js = SDL_JoystickOpen (event->cdevice.which);

        if (js) {
            char guid[1024];
            SDL_JoystickGetGUIDString (SDL_JoystickGetGUID (js),
                                       guid,
                                       sizeof (guid));

            QString mapping = QString ("%1,%2,%3")
                              .arg (guid)
                              .arg (SDL_JoystickName (js))
                              .arg (m_genericMapping);

            SDL_GameControllerAddMapping (mapping.toStdString().c_str());
            SDL_JoystickClose (js);
        }
    }

    SDL_GameControllerOpen (event->cdevice.which);
}

void GamepadManager::onControllerRemoved (const SDL_Event* event)
{
    SDL_GameControllerClose (SDL_GameControllerOpen (event->cdevice.which));
}
<commit_msg>Fix undetected joysticks<commit_after>/*
 * Copyright (c) 2015 WinT 3794 <http://wint3794.org>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include <QDir>
#include <QFile>
#include <QTimer>
#include <QMessageBox>
#include <QApplication>

#include <math.h>

#include "Settings.h"
#include "GamepadManager.h"

/* Maximum axis value */
#define _MAX_VAL 32767

/* Location of community-maintained joystick mappings */
#define _CONTROLLER_DB ":/sdl/database.txt"

/* Used to create mappings from unsupported controllers */
#if defined _WIN32 || defined _WIN64
#define _GENERIC_MAPPINGS ":/sdl/generic/windows.txt"
#elif defined __APPLE__
#define _GENERIC_MAPPINGS ":/sdl/generic/mac-osx.txt"
#elif defined __gnu_linux__
#define _GENERIC_MAPPINGS ":/sdl/generic/linux.txt"
#endif

#define SDL_MAIN_HANDLED
#define _INIT_CODE SDL_INIT_JOYSTICK | SDL_INIT_GAMECONTROLLER

GamepadManager* GamepadManager::m_instance = nullptr;

//------------------------------------------------------------------------------
// Class initialization functions
//------------------------------------------------------------------------------

GamepadManager::GamepadManager()
{
    SDL_SetHint (SDL_HINT_JOYSTICK_ALLOW_BACKGROUND_EVENTS, "1");

    if (SDL_Init (_INIT_CODE) != 0) {
        QMessageBox::critical (nullptr, tr ("Fatal Error!"),
                               tr ("SDL Init Error: %1").arg (SDL_GetError()));

        exit (EXIT_FAILURE);
    }

    /* Enable event states to use them in the event loop */
    SDL_JoystickEventState (SDL_ENABLE);
    SDL_GameControllerEventState (SDL_ENABLE);

    /* Load community controller database */
    QFile db (_CONTROLLER_DB);
    if (db.open (QFile::ReadOnly)) {
        while (!db.atEnd())
            SDL_GameControllerAddMapping (
                QVariant (db.readLine()).toString().toStdString().c_str());

        db.close();
    }

    /* Load generic mapping string, used for unsupported controllers */
    QFile generic (_GENERIC_MAPPINGS);
    if (generic.open (QFile::ReadOnly)) {
        m_genericMapping = (QString)generic.readAll();
        generic.close();
    }
}

GamepadManager::~GamepadManager()
{
    for (int i = 0; i < SDL_NumJoysticks(); ++i)
        SDL_GameControllerClose (SDL_GameControllerOpen (i));

    SDL_Quit();
}

GamepadManager* GamepadManager::getInstance()
{
    if (m_instance == nullptr)
        m_instance = new GamepadManager();

    return m_instance;
}

//------------------------------------------------------------------------------
// Functions that give information about a selected joystick
//------------------------------------------------------------------------------

int GamepadManager::getNumAxes (int joystick)
{
    return SDL_JoystickNumAxes (SDL_JoystickOpen (joystick));
}

int GamepadManager::getNumButtons (int joystick)
{
    return SDL_JoystickNumButtons (SDL_JoystickOpen (joystick));
}

QString GamepadManager::getAxisName (int axis)
{
    return QString ("Axis %1").arg (axis);
}

QString GamepadManager::getButtonName (int button)
{
    return QString ("Button %1").arg (button);
}

QString GamepadManager::getJoystickName (int joystick)
{
    QString name = (QString) SDL_GameControllerNameForIndex (joystick);
    return name.isEmpty() ? SDL_JoystickNameForIndex (joystick) : name;
}

//------------------------------------------------------------------------------
// Functions that give information about all joysticks
//------------------------------------------------------------------------------

QStringList GamepadManager::joystickList()
{
    QStringList list;

    for (int i = 0; i < SDL_NumJoysticks(); ++i)
        list.append (getJoystickName (i));

    return list;
}

//------------------------------------------------------------------------------
// Functions that need to be called after initalization of UI
//------------------------------------------------------------------------------

void GamepadManager::init()
{
    m_time = 20;
    m_tracker = -1;
    QTimer::singleShot (500, this, SLOT (readSdlEvents()));
}

void GamepadManager::setUpdateInterval (int time)
{
    if (time >= 0)
        m_time = time;
}

//------------------------------------------------------------------------------
// Functions that control behaviour of a specific joystick
//------------------------------------------------------------------------------

void GamepadManager::rumble (int joystick, int time)
{
    SDL_InitSubSystem (SDL_INIT_HAPTIC);
    SDL_Haptic* haptic = SDL_HapticOpen (joystick);

    if (haptic != nullptr) {
        SDL_HapticRumbleInit (haptic);
        SDL_HapticRumblePlay (haptic, 1, time);
    }
}

//------------------------------------------------------------------------------
// Functions that transform SDL events into GamepadManager structures/data
//------------------------------------------------------------------------------

GM_Axis GamepadManager::getAxis (const SDL_Event* event)
{
    GM_Axis axis;

    axis.rawId = event->caxis.axis;
    axis.joystick = getJoystick (event);
    axis.identifier = getAxisName (axis.rawId);
    axis.value = (double) (event->caxis.value) / _MAX_VAL;

    return axis;
}

GM_Button GamepadManager::getButton (const SDL_Event* event)
{
    GM_Button button;

    button.rawId = event->cbutton.button;
    button.joystick = getJoystick (event);
    button.identifier = getButtonName (button.rawId);
    button.pressed = event->cbutton.state == SDL_PRESSED;

    return button;
}

GM_Joystick GamepadManager::getJoystick (const SDL_Event* event)
{
    GM_Joystick stick;

    stick.id = getDynamicId (event->cdevice.which);
    stick.numAxes = getNumAxes (event->cdevice.which);
    stick.numButtons = getNumButtons (event->cdevice.which);
    stick.displayName = getJoystickName (event->cdevice.which);

    return stick;
}

//------------------------------------------------------------------------------
// SDL magic
//------------------------------------------------------------------------------

int GamepadManager::getDynamicId (int id)
{
    id = m_tracker - (id + 1);
    if (id < 0) id = abs (id);
    if (id >= SDL_NumJoysticks()) id -= 1;

    return id;
}

void GamepadManager::readSdlEvents()
{
    SDL_Event event;
    while (SDL_PollEvent (&event)) {
        switch (event.type) {
        case SDL_JOYDEVICEADDED:
            ++m_tracker;
            onControllerAdded (&event);
            emit countChanged (joystickList());
            emit countChanged (SDL_NumJoysticks());
            break;
        case SDL_CONTROLLERDEVICEADDED:
            ++m_tracker;
            onControllerAdded (&event);
            emit countChanged (joystickList());
            emit countChanged (SDL_NumJoysticks());
            break;
        case SDL_CONTROLLERDEVICEREMOVED:
            onControllerRemoved (&event);
            emit countChanged (joystickList());
            emit countChanged (SDL_NumJoysticks());
            break;
        case SDL_CONTROLLERAXISMOTION:
            onAxisEvent (&event);
            break;
        case SDL_CONTROLLERBUTTONDOWN:
            onButtonEvent (&event);
            break;
        case SDL_CONTROLLERBUTTONUP:
            onButtonEvent (&event);
            break;
        }
    }

    QTimer::singleShot (m_time, this, SLOT (readSdlEvents()));
}

//------------------------------------------------------------------------------
// Functions that react to SDL events
//------------------------------------------------------------------------------

void GamepadManager::onAxisEvent (const SDL_Event* event)
{
    emit axisEvent (getAxis (event));
}

void GamepadManager::onButtonEvent (const SDL_Event* event)
{
    emit buttonEvent (getButton (event));
}

void GamepadManager::onControllerAdded (const SDL_Event* event)
{
    if (!SDL_IsGameController (event->cdevice.which)) {
        SDL_Joystick* js = SDL_JoystickOpen (event->cdevice.which);

        if (js) {
            char guid[1024];
            SDL_JoystickGetGUIDString (SDL_JoystickGetGUID (js),
                                       guid,
                                       sizeof (guid));

            QString mapping = QString ("%1,%2,%3")
                              .arg (guid)
                              .arg (SDL_JoystickName (js))
                              .arg (m_genericMapping);

            SDL_GameControllerAddMapping (mapping.toStdString().c_str());
            SDL_JoystickClose (js);
        }
    }

    SDL_GameControllerOpen (event->cdevice.which);
}

void GamepadManager::onControllerRemoved (const SDL_Event* event)
{
    SDL_GameControllerClose (SDL_GameControllerOpen (event->cdevice.which));
}
<|endoftext|>


<commit_before>/*********************************************************************************
 * Nelder-Mead Optimierungsalgorithmus.                                          *
 * Autor: Sonja Biedermann, a1402891 (Gruppe 7, Team 4)                          *
 *                                                                               *
 * Minimiert eine gegebene Funktion f.                                           *
 * Wendet dazu den Nelder-Mead Algorithmus an, der einen sich an die Funkions-   *
 * landschaft anpassenden Simplex zum Minimum wandern lässt.                     *
 *                                                                               *
 * Als Referenzen wurden folgende Quellen verwendet:                             *
 * http://mathfaculty.fullerton.edu/mathews/n2003/neldermead/NelderMeadProof.pdf *
 * https://en.wikipedia.org/wiki/Nelder%E2%80%93Mead_method                      *
 *********************************************************************************/

#pragma once
#include <cmath>
#include <tuple>
#include <exception>

#include "Funktion.h"
#include "point.hpp"

struct invalid_value: public std::exception {
    char const* err = "invalid value";
    invalid_value(char const* e) : err{ e } {}
    char const* what() const noexcept { return err; }
};

class nelder_mead_optimizer {
private:
    Funktion& f;
    point b; // best
    point g; // good
    point w; // worst
    double eps;
    size_t iter_c = 0;

    double alpha_; // Alpha: Reflexionsfaktor, default: 1
    double gamma_; // Gamma: Expansionsfaktor, default: 2
    double beta_;  // Rho: Kontrahierungsfaktor, default: .5
    double delta_; // Komprimierungsfaktor, default: .5

    bool is_done = false;

    point& min(point& p1, point& p2) {
        return f(p1.x, p1.y) < f(p2.x, p2.y) ? p1 : p2;
    }

    point& min(point& p1, point& p2, point& p3) {
        double z1 = f(p1.x, p1.y);
        double z2 = f(p2.x, p2.y);
        double z3 = f(p3.x, p3.y);
        return z1 < z2 ? (z1 < z3 ? p1 : p3) : (z2 < z3 ? p2 : p3);
    }

    void sort_points_by_fvalue() {
        std::swap(b, min(b, g, w));
        std::swap(g, min(g, w));
    }

    void do_step() {
        point m = (b + g) / 2; // mittelpunkt
        point r = m + alpha_ * (m - w); // reflektiere schlechtesten punkt

        if(f(r.x, r.y) < f(w.x, w.y)) {
            if(f(r.x, r.y) < f(b.x, b.y)) { // r besser as b: expandiere weiter
                point e = m + gamma_ * (m - w);
                w = min(e, r);
                return;
            }
            w = r;
            return;
        }

        point c;
        if(f(r.x, r.y) < f(w.x, w.y)) { // outside: näher an r
            if(f(c.x, c.y) < f(r.x, r.y)) {
                w = c;
                return;
            }
        } else { // inside: näher an w
            c = m + beta_ * (w - m);
            if(f(c.x, c.y) < f(w.x, w.y)) {
                w = c;
                return;
            }
        }

        g = b + delta_ * (g - b);
        w = b + delta_ * (w - b);
    }

public:
    nelder_mead_optimizer(Funktion& f, point const& p1, point const& p2, point const& p3, double eps = 0.00001,
            double alpha = 1, double gamma = 2, double beta = .5, double delta = .5)
        : f( f ), b{ p1 }, g{ p2 }, w{ p3 }, eps{ eps } {
            sort_points_by_fvalue();
            set_alpha(alpha);
            set_gamma(gamma);
            set_beta(beta);
            set_delta(delta);
        }

    double alpha() const { return alpha_; }
    double gamma() const { return gamma_; }
    double beta() const  { return beta_; }
    double delta() const { return delta_; }

    size_t iteration_count() const { return iter_c; }
    point const& best_point() const { return b; }

    bool done() const { return is_done; }

    std::tuple<point, point, point> current_simplex() const {
        return std::make_tuple(b, g, w);
    }

    void set_alpha(double alpha) {
        if(!(alpha > 0)) {
            throw invalid_value("alpha value violates constraint alpha > 0");
        }
        alpha_ = alpha;
    }

    void set_gamma(double gamma) {
        if(!(gamma > 1)) {
            throw invalid_value("gamma value violates constraint gamma > 1");
        } else if(!(gamma > alpha_)) {
            throw invalid_value("gamma value violates constraint gamma > alpha");
        }
        gamma_ = gamma;
    }

    void set_beta(double beta) {
        if(!(0 < beta && beta < 1)) {
            throw invalid_value("beta value violates constraint 0 < beta < 1");
        }
        beta_ = beta;
    }

    void set_delta(double delta) {
        if(!(0 < delta && delta < 1)) {
            throw invalid_value("delta value violates constraint 0 < delta < 1");
        }
        delta_ = delta;
    }

    void step() {
        if(is_done) { return; }
        do_step();
        ++iter_c;
        sort_points_by_fvalue();

        /* Konvergenzkriterium: Differenz zwischen bestem und schlechtetestem Punkt
         * erreicht oder unterschreitet Epsilon. */
        if(std::abs(f(b.x, b.y) - f(w.x, w.y)) <= eps) { is_done = true; }
    }

    void optimize() { while(!is_done) { step(); } }
};
/* vim: set ts=4 sw=4 tw=0 et :*/
<commit_msg>what the actual fuck<commit_after>/*********************************************************************************
 * Nelder-Mead Optimierungsalgorithmus.                                          *
 * Autor: Sonja Biedermann, a1402891 (Gruppe 7, Team 4)                          *
 *                                                                               *
 * Minimiert eine gegebene Funktion f.                                           *
 * Wendet dazu den Nelder-Mead Algorithmus an, der einen sich an die Funkions-   *
 * landschaft anpassenden Simplex zum Minimum wandern lässt.                     *
 *                                                                               *
 * Als Referenzen wurden folgende Quellen verwendet:                             *
 * http://mathfaculty.fullerton.edu/mathews/n2003/neldermead/NelderMeadProof.pdf *
 * https://en.wikipedia.org/wiki/Nelder%E2%80%93Mead_method                      *
 *********************************************************************************/

#pragma once
#include <cmath>
#include <tuple>
#include <exception>

#include "Funktion.h"
#include "point.hpp"

struct invalid_value: public std::exception {
    char const* err = "invalid value";
    invalid_value(char const* e) : err{ e } {}
    char const* what() const noexcept { return err; }
};

class nelder_mead_optimizer {
private:
    Funktion& f;
    point b; // best
    point g; // good
    point w; // worst
    double eps;
    size_t iter_c = 0;

    double alpha_; // Alpha: Reflexionsfaktor, default: 1
    double gamma_; // Gamma: Expansionsfaktor, default: 2
    double beta_;  // Rho: Kontrahierungsfaktor, default: .5
    double delta_; // Komprimierungsfaktor, default: .5

    bool is_done = false;

    point& min(point& p1, point& p2) {
        return f(p1.x, p1.y) < f(p2.x, p2.y) ? p1 : p2;
    }

    point& min(point& p1, point& p2, point& p3) {
        double z1 = f(p1.x, p1.y);
        double z2 = f(p2.x, p2.y);
        double z3 = f(p3.x, p3.y);
        return z1 < z2 ? (z1 < z3 ? p1 : p3) : (z2 < z3 ? p2 : p3);
    }

    void sort_points_by_fvalue() {
        std::swap(b, min(b, g, w));
        std::swap(g, min(g, w));
    }

    void do_step() {
        point m = (b + g) / 2; // mittelpunkt
        point r = m + alpha_ * (m - w); // reflektiere schlechtesten punkt

        if(f(r.x, r.y) < f(w.x, w.y)) {
            if(f(r.x, r.y) < f(b.x, b.y)) { // r besser as b: expandiere weiter
                point e = m + gamma_ * (m - w);
                w = min(e, r);
                return;
            }
            w = r;
            return;
        }

        point c;
        if(f(r.x, r.y) < f(w.x, w.y)) { // outside: näher an r
            c = m + beta_ * (r - m);
            if(f(c.x, c.y) < f(r.x, r.y)) {
                w = c;
                return;
            }
        } else { // inside: näher an w
            c = m + beta_ * (w - m);
            if(f(c.x, c.y) < f(w.x, w.y)) {
                w = c;
                return;
            }
        }

        g = b + delta_ * (g - b);
        w = b + delta_ * (w - b);
    }

public:
    nelder_mead_optimizer(Funktion& f, point const& p1, point const& p2, point const& p3, double eps = 0.00001,
            double alpha = 1, double gamma = 2, double beta = .5, double delta = .5)
        : f( f ), b{ p1 }, g{ p2 }, w{ p3 }, eps{ eps } {
            sort_points_by_fvalue();
            set_alpha(alpha);
            set_gamma(gamma);
            set_beta(beta);
            set_delta(delta);
        }

    double alpha() const { return alpha_; }
    double gamma() const { return gamma_; }
    double beta() const  { return beta_; }
    double delta() const { return delta_; }

    size_t iteration_count() const { return iter_c; }
    point const& best_point() const { return b; }

    bool done() const { return is_done; }

    std::tuple<point, point, point> current_simplex() const {
        return std::make_tuple(b, g, w);
    }

    void set_alpha(double alpha) {
        if(!(alpha > 0)) {
            throw invalid_value("alpha value violates constraint alpha > 0");
        }
        alpha_ = alpha;
    }

    void set_gamma(double gamma) {
        if(!(gamma > 1)) {
            throw invalid_value("gamma value violates constraint gamma > 1");
        } else if(!(gamma > alpha_)) {
            throw invalid_value("gamma value violates constraint gamma > alpha");
        }
        gamma_ = gamma;
    }

    void set_beta(double beta) {
        if(!(0 < beta && beta < 1)) {
            throw invalid_value("beta value violates constraint 0 < beta < 1");
        }
        beta_ = beta;
    }

    void set_delta(double delta) {
        if(!(0 < delta && delta < 1)) {
            throw invalid_value("delta value violates constraint 0 < delta < 1");
        }
        delta_ = delta;
    }

    void step() {
        if(is_done) { return; }
        do_step();
        ++iter_c;
        sort_points_by_fvalue();

        /* Konvergenzkriterium: Differenz zwischen bestem und schlechtetestem Punkt
         * erreicht oder unterschreitet Epsilon. */
        if(std::abs(f(b.x, b.y) - f(w.x, w.y)) <= eps) { is_done = true; }
    }

    void optimize() { while(!is_done) { step(); } }
};
/* vim: set ts=4 sw=4 tw=0 et :*/
<|endoftext|>