<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_PROSAC_H_
#define PCL_SAMPLE_CONSENSUS_IMPL_PROSAC_H_

#include <boost/math/distributions/binomial.hpp>
#include "pcl/sample_consensus/prosac.h"

//////////////////////////////////////////////////////////////////////////
// Variable naming uses capital letters to make the comparison with the original paper easier
template<typename PointT> bool
pcl::ProgressiveSampleConsensus<PointT>::computeModel (int debug_verbosity_level)
{
  // Warn and exit if no threshold was set
  if (threshold_ == DBL_MAX)
  {
    PCL_ERROR ("[pcl::ProgressiveSampleConsensus::computeModel] No threshold set!\n");
    return (false);
  }

  // Initialize some PROSAC constants
  float T_N = 200000;
  unsigned int N = sac_model_->indices_->size ();
  unsigned int m = sac_model_->getSampleSize ();
  float T_n = T_N;
  for (unsigned int i = 0; i < m; ++i)
    T_n *= (float)(m - i) / (float)(N - i);
  float T_prime_n = 1;
  unsigned int I_N_best = 0;
  float n = m;

  // Define the n_Start coefficients from Section 2.2
  float n_star = N;
  unsigned int I_n_star = 0;
  float epsilon_n_star = (float)I_n_star / n_star;
  unsigned int k_n_star = T_N;

  // Compute the I_n_star_min of Equation 8
  std::vector<unsigned int> I_n_star_min (N);

  // Initialize the usual RANSAC parameters
  iterations_ = 0;

  std::vector<int> inliers;
  std::vector<int> selection;
  Eigen::VectorXf model_coefficients;

  // We will increase the pool so the indices_ vector can only contain m elements at first
  std::vector<int> index_pool;
  index_pool.reserve (N);
  for (unsigned int i = 0; i < n; ++i)
    index_pool.push_back (sac_model_->indices_->operator[](i));

  // Iterate
  while ((unsigned int)iterations_ < k_n_star)
  {
    // Choose the samples

    // Step 1
    // According to Equation 5 in the text text, not the algorithm
    if ((iterations_ == T_prime_n) && (n < n_star))
    {
      // Increase the pool
      ++n;
      if (n >= N)
        break;
      index_pool.push_back (sac_model_->indices_->at(n - 1));
      // Update other variables
      float T_n_minus_1 = T_n;
      T_n *= (float)(n + 1) / (float)(n + 1 - m);
      T_prime_n += ceil(T_n - T_n_minus_1);
    }

    // Step 2
    sac_model_->indices_->swap (index_pool);
    selection.clear ();
    sac_model_->getSamples (iterations_, selection);
    if (T_prime_n < iterations_)
    {
      selection.pop_back ();
      selection.push_back (sac_model_->indices_->at(n - 1));
    }

    // Make sure we use the right indices for testing
    sac_model_->indices_->swap (index_pool);

    if (selection.empty ())
    {
      PCL_ERROR ("[pcl::ProgressiveSampleConsensus::computeModel] No samples could be selected!\n");
      break;
    }

    // Search for inliers in the point cloud for the current model
    if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
    {
      ++iterations_;
      continue;
    }

    // Select the inliers that are within threshold_ from the model
    inliers.clear ();
    sac_model_->selectWithinDistance (model_coefficients, threshold_, inliers);

    unsigned int I_N = inliers.size ();

    // If we find more inliers than before
    if (I_N > I_N_best)
    {
      I_N_best = I_N;

      // Save the current model/inlier/coefficients selection as being the best so far
      inliers_ = inliers;
      model_ = selection;
      model_coefficients_ = model_coefficients;

      // We estimate I_n_star for different possible values of n_star by using the inliers
      std::sort (inliers.begin (), inliers.end ());

      // Try to find a better n_star
      // We minimize k_n_star and therefore maximize epsilon_n_star = I_n_star / n_star
      unsigned int possible_n_star_best = N, I_possible_n_star_best = I_N;
      float epsilon_possible_n_star_best = (float)I_possible_n_star_best / possible_n_star_best;

      // We only need to compute possible better epsilon_n_star for when _n is just about to be removed an inlier
      unsigned int I_possible_n_star = I_N;
      for (std::vector<int>::const_reverse_iterator last_inlier = inliers.rbegin (); last_inlier != inliers.rend (); ++last_inlier, --I_possible_n_star)
      {
        // The best possible_n_star for a given I_possible_n_star is the index of the last inlier
        unsigned int possible_n_star = (*last_inlier) + 1;
        if (possible_n_star <= m)
          break;

        // If we find a better epsilon_n_star
        float epsilon_possible_n_star = (float)I_possible_n_star / possible_n_star;
        // Make sure we have a better epsilon_possible_n_star
        if ((epsilon_possible_n_star > epsilon_n_star) && (epsilon_possible_n_star > epsilon_possible_n_star_best))
        {
          using namespace boost::math;
          // Typo in Equation 7, not (n-m choose i-m) but (n choose i-m)
          unsigned int
                       I_possible_n_star_min = m
                           + ceil (quantile (complement (binomial_distribution<float>(possible_n_star, 0.1), 0.05)));
          // If Equation 9 is not verified, exit
          if (I_possible_n_star < I_possible_n_star_min)
            break;

          possible_n_star_best = possible_n_star;
          I_possible_n_star_best = I_possible_n_star;
          epsilon_possible_n_star_best = epsilon_possible_n_star;
        }
      }

      // Check if we get a better epsilon
      if (epsilon_possible_n_star_best > epsilon_n_star)
      {
        // update the best value
        epsilon_n_star = epsilon_possible_n_star_best;

        // Compute the new k_n_star
        float bottom_log = 1 - std::pow (epsilon_n_star, static_cast<float>(m));
        if (bottom_log == 0)
          k_n_star = 1;
        else if (bottom_log == 1)
          k_n_star = T_N;
        else
          k_n_star = (int)ceil (log(0.05) / log (bottom_log));
        // It seems weird to have very few iterations, so do have a few (totally empirical)
        k_n_star = (std::max)(k_n_star, 2 * m);
      }
    }

    ++iterations_;
    if (debug_verbosity_level > 1)
      PCL_DEBUG ("[pcl::ProgressiveSampleConsensus::computeModel] Trial %d out of %d: %d inliers (best is: %d so far).\n", iterations_, k_n_star, I_N, I_N_best);
    if (iterations_ > max_iterations_)
    {
      if (debug_verbosity_level > 0)
        PCL_DEBUG ("[pcl::ProgressiveSampleConsensus::computeModel] RANSAC reached the maximum number of trials.\n");
      break;
    }
  }

  if (debug_verbosity_level > 0)
    PCL_DEBUG ("[pcl::ProgressiveSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", (unsigned long)model_.size (), I_N_best);

  if (model_.empty ())
  {
    inliers_.clear ();
    return (false);
  }

  // Get the set of inliers that correspond to the best model found so far
  //sac_model_->selectWithinDistance (model_coefficients_, threshold_, inliers_);
  return (true);
}

#define PCL_INSTANTIATE_ProgressiveSampleConsensus(T) template class PCL_EXPORTS pcl::ProgressiveSampleConsensus<T>;

#endif    // PCL_SAMPLE_CONSENSUS_IMPL_PROSAC_H_
<commit_msg>friendlier PROSAC for pathCC<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_PROSAC_H_
#define PCL_SAMPLE_CONSENSUS_IMPL_PROSAC_H_

#include <boost/math/distributions/binomial.hpp>
#include "pcl/sample_consensus/prosac.h"

//////////////////////////////////////////////////////////////////////////
// Variable naming uses capital letters to make the comparison with the original paper easier
template<typename PointT> bool
pcl::ProgressiveSampleConsensus<PointT>::computeModel (int debug_verbosity_level)
{
  // Warn and exit if no threshold was set
  if (threshold_ == DBL_MAX)
  {
    PCL_ERROR ("[pcl::ProgressiveSampleConsensus::computeModel] No threshold set!\n");
    return (false);
  }

  // Initialize some PROSAC constants
  float T_N = 200000;
  unsigned int N = sac_model_->indices_->size ();
  unsigned int m = sac_model_->getSampleSize ();
  float T_n = T_N;
  for (unsigned int i = 0; i < m; ++i)
    T_n *= (float)(m - i) / (float)(N - i);
  float T_prime_n = 1;
  unsigned int I_N_best = 0;
  float n = m;

  // Define the n_Start coefficients from Section 2.2
  float n_star = N;
  unsigned int I_n_star = 0;
  float epsilon_n_star = (float)I_n_star / n_star;
  unsigned int k_n_star = T_N;

  // Compute the I_n_star_min of Equation 8
  std::vector<unsigned int> I_n_star_min (N);

  // Initialize the usual RANSAC parameters
  iterations_ = 0;

  std::vector<int> inliers;
  std::vector<int> selection;
  Eigen::VectorXf model_coefficients;

  // We will increase the pool so the indices_ vector can only contain m elements at first
  std::vector<int> index_pool;
  index_pool.reserve (N);
  for (unsigned int i = 0; i < n; ++i)
    index_pool.push_back (sac_model_->indices_->operator[](i));

  // Iterate
  while ((unsigned int)iterations_ < k_n_star)
  {
    // Choose the samples

    // Step 1
    // According to Equation 5 in the text text, not the algorithm
    if ((iterations_ == T_prime_n) && (n < n_star))
    {
      // Increase the pool
      ++n;
      if (n >= N)
        break;
      index_pool.push_back (sac_model_->indices_->at(n - 1));
      // Update other variables
      float T_n_minus_1 = T_n;
      T_n *= (float)(n + 1) / (float)(n + 1 - m);
      T_prime_n += ceil(T_n - T_n_minus_1);
    }

    // Step 2
    sac_model_->indices_->swap (index_pool);
    selection.clear ();
    sac_model_->getSamples (iterations_, selection);
    if (T_prime_n < iterations_)
    {
      selection.pop_back ();
      selection.push_back (sac_model_->indices_->at(n - 1));
    }

    // Make sure we use the right indices for testing
    sac_model_->indices_->swap (index_pool);

    if (selection.empty ())
    {
      PCL_ERROR ("[pcl::ProgressiveSampleConsensus::computeModel] No samples could be selected!\n");
      break;
    }

    // Search for inliers in the point cloud for the current model
    if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
    {
      ++iterations_;
      continue;
    }

    // Select the inliers that are within threshold_ from the model
    inliers.clear ();
    sac_model_->selectWithinDistance (model_coefficients, threshold_, inliers);

    unsigned int I_N = inliers.size ();

    // If we find more inliers than before
    if (I_N > I_N_best)
    {
      I_N_best = I_N;

      // Save the current model/inlier/coefficients selection as being the best so far
      inliers_ = inliers;
      model_ = selection;
      model_coefficients_ = model_coefficients;

      // We estimate I_n_star for different possible values of n_star by using the inliers
      std::sort (inliers.begin (), inliers.end ());

      // Try to find a better n_star
      // We minimize k_n_star and therefore maximize epsilon_n_star = I_n_star / n_star
      unsigned int possible_n_star_best = N, I_possible_n_star_best = I_N;
      float epsilon_possible_n_star_best = (float)I_possible_n_star_best / possible_n_star_best;

      // We only need to compute possible better epsilon_n_star for when _n is just about to be removed an inlier
      unsigned int I_possible_n_star = I_N;
      for (std::vector<int>::const_reverse_iterator last_inlier = inliers.rbegin (),
                                                    inliers_end = inliers.rend ();
           last_inlier != inliers_end;
           ++last_inlier, --I_possible_n_star)
      {
        // The best possible_n_star for a given I_possible_n_star is the index of the last inlier
        unsigned int possible_n_star = (*last_inlier) + 1;
        if (possible_n_star <= m)
          break;

        // If we find a better epsilon_n_star
        float epsilon_possible_n_star = (float)I_possible_n_star / possible_n_star;
        // Make sure we have a better epsilon_possible_n_star
        if ((epsilon_possible_n_star > epsilon_n_star) && (epsilon_possible_n_star > epsilon_possible_n_star_best))
        {
          using namespace boost::math;
          // Typo in Equation 7, not (n-m choose i-m) but (n choose i-m)
          unsigned int
                       I_possible_n_star_min = m
                           + ceil (quantile (complement (binomial_distribution<float>(possible_n_star, 0.1), 0.05)));
          // If Equation 9 is not verified, exit
          if (I_possible_n_star < I_possible_n_star_min)
            break;

          possible_n_star_best = possible_n_star;
          I_possible_n_star_best = I_possible_n_star;
          epsilon_possible_n_star_best = epsilon_possible_n_star;
        }
      }

      // Check if we get a better epsilon
      if (epsilon_possible_n_star_best > epsilon_n_star)
      {
        // update the best value
        epsilon_n_star = epsilon_possible_n_star_best;

        // Compute the new k_n_star
        float bottom_log = 1 - std::pow (epsilon_n_star, static_cast<float>(m));
        if (bottom_log == 0)
          k_n_star = 1;
        else if (bottom_log == 1)
          k_n_star = T_N;
        else
          k_n_star = (int)ceil (log(0.05) / log (bottom_log));
        // It seems weird to have very few iterations, so do have a few (totally empirical)
        k_n_star = (std::max)(k_n_star, 2 * m);
      }
    }

    ++iterations_;
    if (debug_verbosity_level > 1)
      PCL_DEBUG ("[pcl::ProgressiveSampleConsensus::computeModel] Trial %d out of %d: %d inliers (best is: %d so far).\n", iterations_, k_n_star, I_N, I_N_best);
    if (iterations_ > max_iterations_)
    {
      if (debug_verbosity_level > 0)
        PCL_DEBUG ("[pcl::ProgressiveSampleConsensus::computeModel] RANSAC reached the maximum number of trials.\n");
      break;
    }
  }

  if (debug_verbosity_level > 0)
    PCL_DEBUG ("[pcl::ProgressiveSampleConsensus::computeModel] Model: %lu size, %d inliers.\n", (unsigned long)model_.size (), I_N_best);

  if (model_.empty ())
  {
    inliers_.clear ();
    return (false);
  }

  // Get the set of inliers that correspond to the best model found so far
  //sac_model_->selectWithinDistance (model_coefficients_, threshold_, inliers_);
  return (true);
}

#define PCL_INSTANTIATE_ProgressiveSampleConsensus(T) template class PCL_EXPORTS pcl::ProgressiveSampleConsensus<T>;

#endif    // PCL_SAMPLE_CONSENSUS_IMPL_PROSAC_H_
<|endoftext|>


<commit_before>#include "test.h"

#include "include/config.h"
#include "geo/point2.h"
#include "geo/vector2.h"

using namespace std;
using namespace valhalla::geo;

namespace {

void TryDotProduct(const Vector2& a, const Vector2& b, float expected) {
  float result = a.Dot(b);
  if (expected != result)
    throw runtime_error("DotProduct test failed");
}

void TestDotProduct() {
  TryDotProduct(Vector2(3.0f, 0.0f), Vector2(5.0f, 5.0f), 15.0f);
  TryDotProduct(Vector2(3.0f, 4.0f), Vector2(-8.0f, 6.0f), 0.0f);
}

void TryCrossProduct(const Vector2& a, const Vector2& b, float expected) {
  float result = a.Cross(b);
  if (expected != result)
    throw runtime_error("CrossProduct test failed");
}

void TestCrossProduct() {
  TryCrossProduct(Vector2(3.0f, 0.0f), Vector2(5.0f, 5.0f), 15.0f);
  TryCrossProduct(Vector2(3.0f, 4.0f), Vector2(-8.0f, 6.0f), 50.0f);
}

void TryPerpendicular(const Vector2& a, const Vector2& expected) {
  Vector2 result = a.GetPerpendicular();
  if (!(expected == result))
    throw runtime_error("Perpendicular test failed");
}

void TestPerpendicular() {
  TryPerpendicular(Vector2(3.0f, 4.0f), Vector2(-4.0f, 3.0f));
}

void TryNorm(const Vector2& a, float expected) {
  float result = a.Norm();
  if (expected != result)
    throw runtime_error("Norm test failed");
}

void TestNorm() {
  TryNorm(Vector2(3.0f, 4.0f), 5.0f);
  TryNorm(Vector2(6.0f, 8.0f), 10.0f);
}

void TryNormSquared(const Vector2& a, float expected) {
  float result = a.NormSquared();
  if (expected != result)
    throw runtime_error("NormSquared test failed");
}

void TestNormSquared() {
  TryNormSquared(Vector2(3.0f, 4.0f), 25.0f);
  TryNormSquared(Vector2(6.0f, 8.0f), 100.0f);
}

void TryNormalize(Vector2& a, const Vector2& expected) {
  a.Normalize();
  if (!(expected == a))
    throw runtime_error("Normalize test failed");
}

void TestNormalize() {
  Vector2 v(3.0f, 4.0f);
  TryNormalize(v, Vector2(3.0f / 5.0f, 4.0f / 5.0f));
  Vector2 w(6.0f, 8.0f);
  TryNormalize(w, Vector2(6.0f / 10.0f, 8.0f / 10.0f));
}

void TryComponent(const Vector2& a, const Vector2& b, float expected) {
  float result = a.Component(b);
  if (expected != result)
    throw runtime_error("Component test failed");
}

void TestComponent() {
  TryComponent(Vector2(3.0f, 4.0f), Vector2(6.0f, 8.0f), 0.5f);
  TryComponent(Vector2(6.0f, 8.0f), Vector2(3.0f, 4.0f), 2.0f);
}

void TryProjection(const Vector2& a, const Vector2& b,
                   const Vector2& expected) {
  Vector2 result = a.Projection(b);
  if (!(expected == result))
    throw runtime_error("Projection test failed");
}

void TestProjection() {
  TryProjection(Vector2(3.0f, 4.0f), Vector2(6.0f, 8.0f), Vector2(3.0f, 4.0f));
  TryProjection(Vector2(6.0f, 8.0f), Vector2(3.0f, 4.0f), Vector2(6.0f, 8.0f));
  TryProjection(Vector2(2.0f, 1.0f), Vector2(-3.0f, 4.0f),
                Vector2(6.0f / 25.0f, -8.0f / 25.0f));
}

void TryAngleBetween(const Vector2& a, const Vector2& b, float expected) {
  float result = (a.AngleBetween(b) * kDegPerRad);
  if (expected != result)
    throw runtime_error("AngleBetween test failed");
}

void TestAngleBetween() {
  TryAngleBetween(Vector2(3.0f, 0.0f), Vector2(5.0f, 5.0f), 45.0f);
  TryAngleBetween(Vector2(3.0f, 4.0f), Vector2(-8.0f, 6.0f), 90.0f);
}

void TryReflect(const Vector2& a, const Vector2& b, const Vector2& expected) {
  Vector2 result = a.Reflect(b);
  if (!(expected == result))
    throw runtime_error("Reflect test failed");
}

void TestReflect() {
  Vector2 n1 { 0.0f, 2.0f };
  n1.Normalize();
  TryReflect(Vector2(4.0f, -2.0f), n1, Vector2(4.0f, 2.0f));
  Vector2 n2 { -3.0f, 0.0f };
  n2.Normalize();
  TryReflect(Vector2(3.0f, -4.0f), n2, Vector2(-3.0f, -4.0f));
}

}

int main() {
  test::suite suite("vector2");

  // Dot Product
  suite.test(TEST_CASE(TestDotProduct));

  // Cross Product
  suite.test(TEST_CASE(TestCrossProduct));

  // Perpendicular
  suite.test(TEST_CASE(TestPerpendicular));

  // Norm
  suite.test(TEST_CASE(TestNorm));

  // NormSquared
  suite.test(TEST_CASE(TestNormSquared));

  // Normalize
  suite.test(TEST_CASE(TestNormalize));

  // Component
  suite.test(TEST_CASE(TestComponent));

  // Projection
  suite.test(TEST_CASE(TestProjection));

  // Angle Between
  suite.test(TEST_CASE(TestAngleBetween));

  // Reflect
  suite.test(TEST_CASE(TestReflect));

  return suite.tear_down();
}
<commit_msg>Added ctor and op assignment unit tests as follows:<commit_after>#include "test.h"

#include "include/config.h"
#include "geo/point2.h"
#include "geo/vector2.h"

using namespace std;
using namespace valhalla::geo;

namespace {

void TestCtorDefault() {
  Vector2 target;
  Vector2 expected { 0.0f, 0.0f };
  if (!(expected == target))
    throw runtime_error("CtorDefault test failed");
}

void TryCtorPoint2(const Point2& pt, const Vector2& expected) {
  Vector2 result(pt);
  if (!(expected == result))
    throw runtime_error("CtorPoint2 test failed");
}

void TestCtorPoint2() {
  TryCtorPoint2(Point2(3.0f, 0.0f), Vector2(3.0f, 0.0f));
  TryCtorPoint2(Point2(-8.0f, 6.0f), Vector2(-8.0f, 6.0f));
}

void TryCtorFloatFloat(const float x, const float y, const Vector2& expected) {
  Vector2 result(x, y);
  if (!(expected == result))
    throw runtime_error("CtorFloatFloat test failed");
}

void TestCtorFloatFloat() {
  TryCtorFloatFloat(3.0f, 0.0f, Vector2(3.0f, 0.0f));
  TryCtorFloatFloat(-8.0f, 6.0f, Vector2(-8.0f, 6.0f));
}

void TryCtorPoint2Point2(const Point2& from, const Point2& to,
                         const Vector2& expected) {
  Vector2 result(from, to);
  if (!(expected == result))
    throw runtime_error("CtorPoint2Point2 test failed");
}

void TestCtorPoint2Point2() {
  TryCtorPoint2Point2(Point2(4.0f, 0.0f), Point2(3.0f, 3.0f),
                      Vector2(-1.0f, 3.0f));
  TryCtorPoint2Point2(Point2(4.0f, 2.0f), Point2(4.0f, -2.0f),
                      Vector2(0.0f, -4.0f));
}

void TryCtorVector2(const Vector2& v, const Vector2& expected) {
  Vector2 result(v);
  if (!(expected == result))
    throw runtime_error("CtorVector2 test failed");
}

void TestCtorVector2() {
  TryCtorVector2(Vector2(3.0f, 0.0f), Vector2(3.0f, 0.0f));
  TryCtorVector2(Vector2(-8.0f, 6.0f), Vector2(-8.0f, 6.0f));
}

void TryOpAssignment(const Vector2& v, const Vector2& expected) {
  Vector2 result;
  result = v;
  if (!(expected == result))
    throw runtime_error("OpAssignment test failed");
}

void TestOpAssignment() {
  TryOpAssignment(Vector2(3.0f, 0.0f), Vector2(3.0f, 0.0f));
  TryOpAssignment(Vector2(-8.0f, 6.0f), Vector2(-8.0f, 6.0f));
}

void TryDotProduct(const Vector2& a, const Vector2& b, float expected) {
  float result = a.Dot(b);
  if (expected != result)
    throw runtime_error("DotProduct test failed");
}

void TestDotProduct() {
  TryDotProduct(Vector2(3.0f, 0.0f), Vector2(5.0f, 5.0f), 15.0f);
  TryDotProduct(Vector2(3.0f, 4.0f), Vector2(-8.0f, 6.0f), 0.0f);
}

void TryCrossProduct(const Vector2& a, const Vector2& b, float expected) {
  float result = a.Cross(b);
  if (expected != result)
    throw runtime_error("CrossProduct test failed");
}

void TestCrossProduct() {
  TryCrossProduct(Vector2(3.0f, 0.0f), Vector2(5.0f, 5.0f), 15.0f);
  TryCrossProduct(Vector2(3.0f, 4.0f), Vector2(-8.0f, 6.0f), 50.0f);
}

void TryPerpendicular(const Vector2& a, const Vector2& expected) {
  Vector2 result = a.GetPerpendicular();
  if (!(expected == result))
    throw runtime_error("Perpendicular test failed");
}

void TestPerpendicular() {
  TryPerpendicular(Vector2(3.0f, 4.0f), Vector2(-4.0f, 3.0f));
}

void TryNorm(const Vector2& a, float expected) {
  float result = a.Norm();
  if (expected != result)
    throw runtime_error("Norm test failed");
}

void TestNorm() {
  TryNorm(Vector2(3.0f, 4.0f), 5.0f);
  TryNorm(Vector2(6.0f, 8.0f), 10.0f);
}

void TryNormSquared(const Vector2& a, float expected) {
  float result = a.NormSquared();
  if (expected != result)
    throw runtime_error("NormSquared test failed");
}

void TestNormSquared() {
  TryNormSquared(Vector2(3.0f, 4.0f), 25.0f);
  TryNormSquared(Vector2(6.0f, 8.0f), 100.0f);
}

void TryNormalize(Vector2& a, const Vector2& expected) {
  a.Normalize();
  if (!(expected == a))
    throw runtime_error("Normalize test failed");
}

void TestNormalize() {
  Vector2 v(3.0f, 4.0f);
  TryNormalize(v, Vector2(3.0f / 5.0f, 4.0f / 5.0f));
  Vector2 w(6.0f, 8.0f);
  TryNormalize(w, Vector2(6.0f / 10.0f, 8.0f / 10.0f));
}

void TryComponent(const Vector2& a, const Vector2& b, float expected) {
  float result = a.Component(b);
  if (expected != result)
    throw runtime_error("Component test failed");
}

void TestComponent() {
  TryComponent(Vector2(3.0f, 4.0f), Vector2(6.0f, 8.0f), 0.5f);
  TryComponent(Vector2(6.0f, 8.0f), Vector2(3.0f, 4.0f), 2.0f);
}

void TryProjection(const Vector2& a, const Vector2& b,
                   const Vector2& expected) {
  Vector2 result = a.Projection(b);
  if (!(expected == result))
    throw runtime_error("Projection test failed");
}

void TestProjection() {
  TryProjection(Vector2(3.0f, 4.0f), Vector2(6.0f, 8.0f), Vector2(3.0f, 4.0f));
  TryProjection(Vector2(6.0f, 8.0f), Vector2(3.0f, 4.0f), Vector2(6.0f, 8.0f));
  TryProjection(Vector2(2.0f, 1.0f), Vector2(-3.0f, 4.0f),
                Vector2(6.0f / 25.0f, -8.0f / 25.0f));
}

void TryAngleBetween(const Vector2& a, const Vector2& b, float expected) {
  float result = (a.AngleBetween(b) * kDegPerRad);
  if (expected != result)
    throw runtime_error("AngleBetween test failed");
}

void TestAngleBetween() {
  TryAngleBetween(Vector2(3.0f, 0.0f), Vector2(5.0f, 5.0f), 45.0f);
  TryAngleBetween(Vector2(3.0f, 4.0f), Vector2(-8.0f, 6.0f), 90.0f);
}

void TryReflect(const Vector2& a, const Vector2& b, const Vector2& expected) {
  Vector2 result = a.Reflect(b);
  if (!(expected == result))
    throw runtime_error("Reflect test failed");
}

void TestReflect() {
  Vector2 n1 { 0.0f, 2.0f };
  n1.Normalize();
  TryReflect(Vector2(4.0f, -2.0f), n1, Vector2(4.0f, 2.0f));
  Vector2 n2 { -3.0f, 0.0f };
  n2.Normalize();
  TryReflect(Vector2(3.0f, -4.0f), n2, Vector2(-3.0f, -4.0f));
}

}

int main() {
  test::suite suite("vector2");

  // Ctor default
  suite.test(TEST_CASE(TestCtorDefault));

  // Ctor Point2
  suite.test(TEST_CASE(TestCtorPoint2));

  // Ctor float, float
  suite.test(TEST_CASE(TestCtorFloatFloat));

  // Ctor Point2, Point2
  suite.test(TEST_CASE(TestCtorPoint2Point2));

  // Ctor Vector2
  suite.test(TEST_CASE(TestCtorVector2));

  // Op Assignment
  suite.test(TEST_CASE(TestOpAssignment));

  // Dot Product
  suite.test(TEST_CASE(TestDotProduct));

  // Cross Product
  suite.test(TEST_CASE(TestCrossProduct));

  // Perpendicular
  suite.test(TEST_CASE(TestPerpendicular));

  // Norm
  suite.test(TEST_CASE(TestNorm));

  // NormSquared
  suite.test(TEST_CASE(TestNormSquared));

  // Normalize
  suite.test(TEST_CASE(TestNormalize));

  // Component
  suite.test(TEST_CASE(TestComponent));

  // Projection
  suite.test(TEST_CASE(TestProjection));

  // Angle Between
  suite.test(TEST_CASE(TestAngleBetween));

  // Reflect
  suite.test(TEST_CASE(TestReflect));

  return suite.tear_down();
}
<|endoftext|>


<commit_before>/*
-----------------------------------------------------------------------------
This source file is part of OGRE
(Object-oriented Graphics Rendering Engine)
For the latest info, see http://www.ogre3d.org

Copyright (c) 2000-2014 Torus Knot Software Ltd
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 "OgreShaderPrecompiledHeaders.h"
#ifdef RTSHADER_SYSTEM_BUILD_EXT_SHADERS

#define SGX_LIB_NORMALMAP                           "SGXLib_NormalMap"
#define SGX_FUNC_FETCHNORMAL                        "SGX_FetchNormal"

namespace Ogre {
namespace RTShader {

/************************************************************************/
/*                                                                      */
/************************************************************************/
const String SRS_NORMALMAP                      = "NormalMap";

//-----------------------------------------------------------------------
NormalMapLighting::NormalMapLighting()
{
    mNormalMapSamplerIndex          = 0;
    mVSTexCoordSetIndex             = 0;
    mNormalMapSpace                 = NMS_TANGENT;
    mNormalMapSampler = TextureManager::getSingleton().createSampler();
    mNormalMapSampler->setMipmapBias(-1.0);
}

//-----------------------------------------------------------------------
const String& NormalMapLighting::getType() const
{
    return SRS_NORMALMAP;
}
//-----------------------------------------------------------------------
bool NormalMapLighting::createCpuSubPrograms(ProgramSet* programSet)
{
    Program* vsProgram = programSet->getCpuProgram(GPT_VERTEX_PROGRAM);
    Function* vsMain = vsProgram->getEntryPointFunction();
    Program* psProgram = programSet->getCpuProgram(GPT_FRAGMENT_PROGRAM);
    Function* psMain = psProgram->getEntryPointFunction();

    vsProgram->addDependency(FFP_LIB_TRANSFORM);

    psProgram->addDependency(FFP_LIB_TRANSFORM);
    psProgram->addDependency(FFP_LIB_TEXTURING);
    psProgram->addDependency(SGX_LIB_NORMALMAP);

    // Resolve texture coordinates.
    auto vsInTexcoord = vsMain->resolveInputParameter(
        Parameter::Content(Parameter::SPC_TEXTURE_COORDINATE0 + mVSTexCoordSetIndex), GCT_FLOAT2);
    auto vsOutTexcoord = vsMain->resolveOutputParameter(
        Parameter::Content(Parameter::SPC_TEXTURE_COORDINATE0 + mVSTexCoordSetIndex), GCT_FLOAT2);
    auto psInTexcoord = psMain->resolveInputParameter(vsOutTexcoord);

    // Resolve normal.
    auto vsInNormal = vsMain->resolveInputParameter(Parameter::SPC_NORMAL_OBJECT_SPACE);
    auto vsOutNormal = vsMain->resolveOutputParameter(Parameter::SPC_NORMAL_VIEW_SPACE);
    auto viewNormal = psMain->resolveInputParameter(vsOutNormal);
    auto newViewNormal = psMain->resolveLocalParameter(Parameter::SPC_NORMAL_VIEW_SPACE);

    // insert before lighting stage
    auto vstage = vsMain->getStage(FFP_PS_COLOUR_BEGIN + 1);
    auto fstage = psMain->getStage(FFP_PS_COLOUR_BEGIN + 1);

    // Output texture coordinates.
    vstage.assign(vsInTexcoord, vsOutTexcoord);

    // Add the normal fetch function invocation
    auto normalMapSampler = psProgram->resolveParameter(GCT_SAMPLER2D, "gNormalMapSampler", mNormalMapSamplerIndex);
    fstage.callFunction(SGX_FUNC_FETCHNORMAL, normalMapSampler, psInTexcoord, newViewNormal);

    if (mNormalMapSpace & NMS_TANGENT)
    {
        auto vsInTangent = vsMain->resolveInputParameter(Parameter::SPC_TANGENT_OBJECT_SPACE);
        auto vsOutTangent = vsMain->resolveOutputParameter(Parameter::SPC_TANGENT_OBJECT_SPACE);
        auto psInTangent = psMain->resolveInputParameter(vsOutTangent);

        // transform normal & tangent
        auto normalMatrix = vsProgram->resolveParameter(GpuProgramParameters::ACT_NORMAL_MATRIX);
        vstage.callFunction(FFP_FUNC_TRANSFORM, normalMatrix, vsInNormal, vsOutNormal);
        vstage.callFunction(FFP_FUNC_TRANSFORM, normalMatrix, vsInTangent, vsOutTangent);

        // transform normal
        fstage.callFunction("SGX_TransformNormal", {In(viewNormal), In(psInTangent), InOut(newViewNormal)});
    }
    else if (mNormalMapSpace & NMS_OBJECT)
    {
        // transform normal in FS
        auto normalMatrix = psProgram->resolveParameter(GpuProgramParameters::ACT_NORMAL_MATRIX);
        fstage.callFunction(FFP_FUNC_TRANSFORM, normalMatrix, newViewNormal, newViewNormal);
    }

    if (mNormalMapSpace == NMS_PARALLAX)
    {
        // assuming: lighting stage computed this
        auto vsOutViewPos = vsMain->resolveOutputParameter(Parameter::SPC_POSITION_VIEW_SPACE);
        auto viewPos = psMain->resolveInputParameter(vsOutViewPos);

        // TODO: user specificed scale and bias
        fstage.callFunction("SGX_Generate_Parallax_Texcoord", {In(normalMapSampler), In(psInTexcoord), In(viewPos),
                                                              In(Vector2(0.04, -0.02)), Out(psInTexcoord)});

        // overwrite texcoord0 unconditionally, only one texcoord set is supported with parallax mapping
        // we are before FFP_PS_TEXTURING, so the new value will be used
        auto texcoord0 = psMain->resolveInputParameter(Parameter::SPC_TEXTURE_COORDINATE0, GCT_FLOAT2);
        fstage.assign(psInTexcoord, texcoord0);
    }

    return true;
}

//-----------------------------------------------------------------------
void NormalMapLighting::copyFrom(const SubRenderState& rhs)
{
    const NormalMapLighting& rhsLighting = static_cast<const NormalMapLighting&>(rhs);

    mNormalMapSpace = rhsLighting.mNormalMapSpace;
    mNormalMapTextureName = rhsLighting.mNormalMapTextureName;
    mNormalMapSampler = rhsLighting.mNormalMapSampler;
}

//-----------------------------------------------------------------------
bool NormalMapLighting::preAddToRenderState(const RenderState* renderState, Pass* srcPass, Pass* dstPass)
{
    TextureUnitState* normalMapTexture = dstPass->createTextureUnitState();

    normalMapTexture->setTextureName(mNormalMapTextureName);
    normalMapTexture->setSampler(mNormalMapSampler);
    mNormalMapSamplerIndex = dstPass->getNumTextureUnitStates() - 1;

    return true;
}

bool NormalMapLighting::setParameter(const String& name, const String& value)
{
	if(name == "normalmap_space")
	{
        // Normal map defines normals in tangent space.
        if (value == "tangent_space")
        {
            setNormalMapSpace(NMS_TANGENT);
            return true;
        }
        // Normal map defines normals in object space.
        if (value == "object_space")
        {
            setNormalMapSpace(NMS_OBJECT);
            return true;
        }
        if (value == "parallax")
        {
            setNormalMapSpace(NMS_PARALLAX);
            return true;
        }
		return false;
	}

	if(name == "texture")
	{
		mNormalMapTextureName = value;
		return true;
	}

	if(name == "texcoord_index")
	{
		setTexCoordIndex(StringConverter::parseInt(value));
		return true;
	}

    if(name == "sampler")
    {
        auto sampler = TextureManager::getSingleton().getSampler(value);
        if(!sampler)
            return false;
        mNormalMapSampler = sampler;
        return true;
    }

	return false;
}

//-----------------------------------------------------------------------
const String& NormalMapLightingFactory::getType() const
{
    return SRS_NORMALMAP;
}

//-----------------------------------------------------------------------
SubRenderState* NormalMapLightingFactory::createInstance(ScriptCompiler* compiler,
                                                        PropertyAbstractNode* prop, Pass* pass, SGScriptTranslator* translator)
{
    if (prop->name == "lighting_stage")
    {
        if(prop->values.size() >= 2)
        {
            String strValue;
            AbstractNodeList::const_iterator it = prop->values.begin();

            // Read light model type.
            if(false == SGScriptTranslator::getString(*it, &strValue))
            {
                compiler->addError(ScriptCompiler::CE_INVALIDPARAMETERS, prop->file, prop->line);
                return NULL;
            }

            // Case light model type is normal map
            if (strValue == "normal_map")
            {
                ++it;
                if (false == SGScriptTranslator::getString(*it, &strValue))
                {
                    compiler->addError(ScriptCompiler::CE_STRINGEXPECTED, prop->file, prop->line);
                    return NULL;
                }

                SubRenderState* subRenderState = createOrRetrieveInstance(translator);
                NormalMapLighting* normalMapSubRenderState = static_cast<NormalMapLighting*>(subRenderState);

                normalMapSubRenderState->setParameter("texture", strValue);

                // Read normal map space type.
                if (prop->values.size() >= 3)
                {
                    ++it;
                    if (false == SGScriptTranslator::getString(*it, &strValue))
                    {
                        compiler->addError(ScriptCompiler::CE_STRINGEXPECTED, prop->file, prop->line);
                        return NULL;
                    }

                    if(!normalMapSubRenderState->setParameter("normalmap_space", strValue))
                    {
                        compiler->addError(ScriptCompiler::CE_INVALIDPARAMETERS, prop->file, prop->line);
                        return NULL;
                    }
                }

                // Read texture coordinate index.
                if (prop->values.size() >= 4)
                {
                    unsigned int textureCoordinateIndex = 0;

                    ++it;
                    if (SGScriptTranslator::getUInt(*it, &textureCoordinateIndex))
                    {
                        normalMapSubRenderState->setTexCoordIndex(textureCoordinateIndex);
                    }
                }

                // Read texture filtering format.
                if (prop->values.size() >= 5)
                {
                    ++it;
                    if (false == SGScriptTranslator::getString(*it, &strValue))
                    {
                        compiler->addError(ScriptCompiler::CE_STRINGEXPECTED, prop->file, prop->line);
                        return NULL;
                    }

                    // sampler reference
                    if(!normalMapSubRenderState->setParameter("sampler", strValue))
                    {
                        compiler->addError(ScriptCompiler::CE_INVALIDPARAMETERS, prop->file, prop->line);
                        return NULL;
                    }
                }

                return subRenderState;
            }
        }
    }

    return NULL;
}

//-----------------------------------------------------------------------
void NormalMapLightingFactory::writeInstance(MaterialSerializer* ser,
                                             SubRenderState* subRenderState,
                                             Pass* srcPass, Pass* dstPass)
{
    NormalMapLighting* normalMapSubRenderState = static_cast<NormalMapLighting*>(subRenderState);

    ser->writeAttribute(4, "lighting_stage");
    ser->writeValue("normal_map");
    ser->writeValue(normalMapSubRenderState->getNormalMapTextureName());

    if (normalMapSubRenderState->getNormalMapSpace() == NormalMapLighting::NMS_TANGENT)
    {
        ser->writeValue("tangent_space");
    }
    else if (normalMapSubRenderState->getNormalMapSpace() == NormalMapLighting::NMS_OBJECT)
    {
        ser->writeValue("object_space");
    }

    ser->writeValue(StringConverter::toString(normalMapSubRenderState->getTexCoordIndex()));
}

//-----------------------------------------------------------------------
SubRenderState* NormalMapLightingFactory::createInstanceImpl()
{
    return OGRE_NEW NormalMapLighting;
}

}
}

#endif
<commit_msg>RTSS: NormalMap - we must normalise tangents & normals in VS<commit_after>/*
-----------------------------------------------------------------------------
This source file is part of OGRE
(Object-oriented Graphics Rendering Engine)
For the latest info, see http://www.ogre3d.org

Copyright (c) 2000-2014 Torus Knot Software Ltd
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 "OgreShaderPrecompiledHeaders.h"
#ifdef RTSHADER_SYSTEM_BUILD_EXT_SHADERS

#define SGX_LIB_NORMALMAP                           "SGXLib_NormalMap"
#define SGX_FUNC_FETCHNORMAL                        "SGX_FetchNormal"

namespace Ogre {
namespace RTShader {

/************************************************************************/
/*                                                                      */
/************************************************************************/
const String SRS_NORMALMAP                      = "NormalMap";

//-----------------------------------------------------------------------
NormalMapLighting::NormalMapLighting()
{
    mNormalMapSamplerIndex          = 0;
    mVSTexCoordSetIndex             = 0;
    mNormalMapSpace                 = NMS_TANGENT;
    mNormalMapSampler = TextureManager::getSingleton().createSampler();
    mNormalMapSampler->setMipmapBias(-1.0);
}

//-----------------------------------------------------------------------
const String& NormalMapLighting::getType() const
{
    return SRS_NORMALMAP;
}
//-----------------------------------------------------------------------
bool NormalMapLighting::createCpuSubPrograms(ProgramSet* programSet)
{
    Program* vsProgram = programSet->getCpuProgram(GPT_VERTEX_PROGRAM);
    Function* vsMain = vsProgram->getEntryPointFunction();
    Program* psProgram = programSet->getCpuProgram(GPT_FRAGMENT_PROGRAM);
    Function* psMain = psProgram->getEntryPointFunction();

    vsProgram->addDependency(FFP_LIB_TRANSFORM);

    psProgram->addDependency(FFP_LIB_TRANSFORM);
    psProgram->addDependency(FFP_LIB_TEXTURING);
    psProgram->addDependency(SGX_LIB_NORMALMAP);

    // Resolve texture coordinates.
    auto vsInTexcoord = vsMain->resolveInputParameter(
        Parameter::Content(Parameter::SPC_TEXTURE_COORDINATE0 + mVSTexCoordSetIndex), GCT_FLOAT2);
    auto vsOutTexcoord = vsMain->resolveOutputParameter(
        Parameter::Content(Parameter::SPC_TEXTURE_COORDINATE0 + mVSTexCoordSetIndex), GCT_FLOAT2);
    auto psInTexcoord = psMain->resolveInputParameter(vsOutTexcoord);

    // Resolve normal.
    auto vsInNormal = vsMain->resolveInputParameter(Parameter::SPC_NORMAL_OBJECT_SPACE);
    auto vsOutNormal = vsMain->resolveOutputParameter(Parameter::SPC_NORMAL_VIEW_SPACE);
    auto viewNormal = psMain->resolveInputParameter(vsOutNormal);
    auto newViewNormal = psMain->resolveLocalParameter(Parameter::SPC_NORMAL_VIEW_SPACE);

    // insert before lighting stage
    auto vstage = vsMain->getStage(FFP_PS_COLOUR_BEGIN + 1);
    auto fstage = psMain->getStage(FFP_PS_COLOUR_BEGIN + 1);

    // Output texture coordinates.
    vstage.assign(vsInTexcoord, vsOutTexcoord);

    // Add the normal fetch function invocation
    auto normalMapSampler = psProgram->resolveParameter(GCT_SAMPLER2D, "gNormalMapSampler", mNormalMapSamplerIndex);
    fstage.callFunction(SGX_FUNC_FETCHNORMAL, normalMapSampler, psInTexcoord, newViewNormal);

    if (mNormalMapSpace & NMS_TANGENT)
    {
        auto vsInTangent = vsMain->resolveInputParameter(Parameter::SPC_TANGENT_OBJECT_SPACE);
        auto vsOutTangent = vsMain->resolveOutputParameter(Parameter::SPC_TANGENT_OBJECT_SPACE);
        auto psInTangent = psMain->resolveInputParameter(vsOutTangent);

        // transform normal & tangent
        auto normalMatrix = vsProgram->resolveParameter(GpuProgramParameters::ACT_NORMAL_MATRIX);
        vstage.callFunction(FFP_FUNC_TRANSFORM, normalMatrix, vsInNormal, vsOutNormal);
        vstage.callBuiltin("normalize", vsOutNormal, vsOutNormal);
        vstage.callFunction(FFP_FUNC_TRANSFORM, normalMatrix, vsInTangent, vsOutTangent);
        vstage.callBuiltin("normalize", vsOutTangent, vsOutTangent);

        // transform normal
        fstage.callFunction("SGX_TransformNormal", {In(viewNormal), In(psInTangent), InOut(newViewNormal)});
    }
    else if (mNormalMapSpace & NMS_OBJECT)
    {
        // transform normal in FS
        auto normalMatrix = psProgram->resolveParameter(GpuProgramParameters::ACT_NORMAL_MATRIX);
        fstage.callFunction(FFP_FUNC_TRANSFORM, normalMatrix, newViewNormal, newViewNormal);
    }

    if (mNormalMapSpace == NMS_PARALLAX)
    {
        // assuming: lighting stage computed this
        auto vsOutViewPos = vsMain->resolveOutputParameter(Parameter::SPC_POSITION_VIEW_SPACE);
        auto viewPos = psMain->resolveInputParameter(vsOutViewPos);

        // TODO: user specificed scale and bias
        fstage.callFunction("SGX_Generate_Parallax_Texcoord", {In(normalMapSampler), In(psInTexcoord), In(viewPos),
                                                              In(Vector2(0.04, -0.02)), Out(psInTexcoord)});

        // overwrite texcoord0 unconditionally, only one texcoord set is supported with parallax mapping
        // we are before FFP_PS_TEXTURING, so the new value will be used
        auto texcoord0 = psMain->resolveInputParameter(Parameter::SPC_TEXTURE_COORDINATE0, GCT_FLOAT2);
        fstage.assign(psInTexcoord, texcoord0);
    }

    return true;
}

//-----------------------------------------------------------------------
void NormalMapLighting::copyFrom(const SubRenderState& rhs)
{
    const NormalMapLighting& rhsLighting = static_cast<const NormalMapLighting&>(rhs);

    mNormalMapSpace = rhsLighting.mNormalMapSpace;
    mNormalMapTextureName = rhsLighting.mNormalMapTextureName;
    mNormalMapSampler = rhsLighting.mNormalMapSampler;
}

//-----------------------------------------------------------------------
bool NormalMapLighting::preAddToRenderState(const RenderState* renderState, Pass* srcPass, Pass* dstPass)
{
    TextureUnitState* normalMapTexture = dstPass->createTextureUnitState();

    normalMapTexture->setTextureName(mNormalMapTextureName);
    normalMapTexture->setSampler(mNormalMapSampler);
    mNormalMapSamplerIndex = dstPass->getNumTextureUnitStates() - 1;

    return true;
}

bool NormalMapLighting::setParameter(const String& name, const String& value)
{
	if(name == "normalmap_space")
	{
        // Normal map defines normals in tangent space.
        if (value == "tangent_space")
        {
            setNormalMapSpace(NMS_TANGENT);
            return true;
        }
        // Normal map defines normals in object space.
        if (value == "object_space")
        {
            setNormalMapSpace(NMS_OBJECT);
            return true;
        }
        if (value == "parallax")
        {
            setNormalMapSpace(NMS_PARALLAX);
            return true;
        }
		return false;
	}

	if(name == "texture")
	{
		mNormalMapTextureName = value;
		return true;
	}

	if(name == "texcoord_index")
	{
		setTexCoordIndex(StringConverter::parseInt(value));
		return true;
	}

    if(name == "sampler")
    {
        auto sampler = TextureManager::getSingleton().getSampler(value);
        if(!sampler)
            return false;
        mNormalMapSampler = sampler;
        return true;
    }

	return false;
}

//-----------------------------------------------------------------------
const String& NormalMapLightingFactory::getType() const
{
    return SRS_NORMALMAP;
}

//-----------------------------------------------------------------------
SubRenderState* NormalMapLightingFactory::createInstance(ScriptCompiler* compiler,
                                                        PropertyAbstractNode* prop, Pass* pass, SGScriptTranslator* translator)
{
    if (prop->name == "lighting_stage")
    {
        if(prop->values.size() >= 2)
        {
            String strValue;
            AbstractNodeList::const_iterator it = prop->values.begin();

            // Read light model type.
            if(false == SGScriptTranslator::getString(*it, &strValue))
            {
                compiler->addError(ScriptCompiler::CE_INVALIDPARAMETERS, prop->file, prop->line);
                return NULL;
            }

            // Case light model type is normal map
            if (strValue == "normal_map")
            {
                ++it;
                if (false == SGScriptTranslator::getString(*it, &strValue))
                {
                    compiler->addError(ScriptCompiler::CE_STRINGEXPECTED, prop->file, prop->line);
                    return NULL;
                }

                SubRenderState* subRenderState = createOrRetrieveInstance(translator);
                NormalMapLighting* normalMapSubRenderState = static_cast<NormalMapLighting*>(subRenderState);

                normalMapSubRenderState->setParameter("texture", strValue);

                // Read normal map space type.
                if (prop->values.size() >= 3)
                {
                    ++it;
                    if (false == SGScriptTranslator::getString(*it, &strValue))
                    {
                        compiler->addError(ScriptCompiler::CE_STRINGEXPECTED, prop->file, prop->line);
                        return NULL;
                    }

                    if(!normalMapSubRenderState->setParameter("normalmap_space", strValue))
                    {
                        compiler->addError(ScriptCompiler::CE_INVALIDPARAMETERS, prop->file, prop->line);
                        return NULL;
                    }
                }

                // Read texture coordinate index.
                if (prop->values.size() >= 4)
                {
                    unsigned int textureCoordinateIndex = 0;

                    ++it;
                    if (SGScriptTranslator::getUInt(*it, &textureCoordinateIndex))
                    {
                        normalMapSubRenderState->setTexCoordIndex(textureCoordinateIndex);
                    }
                }

                // Read texture filtering format.
                if (prop->values.size() >= 5)
                {
                    ++it;
                    if (false == SGScriptTranslator::getString(*it, &strValue))
                    {
                        compiler->addError(ScriptCompiler::CE_STRINGEXPECTED, prop->file, prop->line);
                        return NULL;
                    }

                    // sampler reference
                    if(!normalMapSubRenderState->setParameter("sampler", strValue))
                    {
                        compiler->addError(ScriptCompiler::CE_INVALIDPARAMETERS, prop->file, prop->line);
                        return NULL;
                    }
                }

                return subRenderState;
            }
        }
    }

    return NULL;
}

//-----------------------------------------------------------------------
void NormalMapLightingFactory::writeInstance(MaterialSerializer* ser,
                                             SubRenderState* subRenderState,
                                             Pass* srcPass, Pass* dstPass)
{
    NormalMapLighting* normalMapSubRenderState = static_cast<NormalMapLighting*>(subRenderState);

    ser->writeAttribute(4, "lighting_stage");
    ser->writeValue("normal_map");
    ser->writeValue(normalMapSubRenderState->getNormalMapTextureName());

    if (normalMapSubRenderState->getNormalMapSpace() == NormalMapLighting::NMS_TANGENT)
    {
        ser->writeValue("tangent_space");
    }
    else if (normalMapSubRenderState->getNormalMapSpace() == NormalMapLighting::NMS_OBJECT)
    {
        ser->writeValue("object_space");
    }

    ser->writeValue(StringConverter::toString(normalMapSubRenderState->getTexCoordIndex()));
}

//-----------------------------------------------------------------------
SubRenderState* NormalMapLightingFactory::createInstanceImpl()
{
    return OGRE_NEW NormalMapLighting;
}

}
}

#endif
<|endoftext|>


<commit_before>/*------------ant.cpp---------------------------------------------------------//
*
*             ants -- We have them
*
* Purpose: Figure out ant pathing to food. I want food.
*
*-----------------------------------------------------------------------------*/

#include <iostream>
#include <vector>

/*----------------------------------------------------------------------------//
* STRUCTS
*-----------------------------------------------------------------------------*/

const int n = 5;

struct coord{
    int x, y;
};

struct ant{
    coord pos;
    std::vector<coord> phepath, antpath;
    int step;
};

struct grid{
    bool wall[n][n];
    coord prize;
};

/*----------------------------------------------------------------------------//
* MAIN
*-----------------------------------------------------------------------------*/

int main(){
}

/*----------------------------------------------------------------------------//
* SUBROUTINES
*-----------------------------------------------------------------------------*/
<commit_msg>updating generation 1 pathing.<commit_after>/*------------ant.cpp---------------------------------------------------------//
*
*             ants -- We have them
*
* Purpose: Figure out ant pathing to food. I want food.
*
*-----------------------------------------------------------------------------*/

#include <iostream>
#include <vector>
#include <random>

/*----------------------------------------------------------------------------//
* STRUCTS
*-----------------------------------------------------------------------------*/

// grid size
const int n = 5;

// structs for ant movement
struct coord{
    int x, y;
};

struct ant{
    coord pos;
    std::vector<coord> phepath, antpath;
    int step;
};

struct grid{
    bool wall[n][n];
    coord prize;
};

// Functions for ant movement
// Chooses step
ant step1(ant curr, grid landscape, coord spawn_point);

ant step(ant curr, grid landscape, coord spawn_point);

// Generates ants
std::vector <ant> gen_ants(coord spawn_point);

// Moves simulation every timestep
std::vector <ant> move(std::vector <ant> ants, grid landscape);

/*----------------------------------------------------------------------------//
* MAIN
*-----------------------------------------------------------------------------*/

int main(){
}

/*----------------------------------------------------------------------------//
* SUBROUTINES
*-----------------------------------------------------------------------------*/

// Chooses step
ant step1(ant curr, grid landscape, coord spawn_point){

    coord next_step[3];
    coord up, down, left, right, next;
    int pcount = 0;

    up.x = curr.pos.x;          up.y = curr.pos.y + 1;
    down.x = curr.pos.x;        down.y = curr.pos.y - 1;
    left.x = curr.pos.x - 1;    left.y = curr.pos.y;
    right.x = curr.pos.x + 1;   right.y = curr.pos.y;

    // determine possible movement
    for (int i = 0; i < 4; i++){
        switch(i){
        // up case
        case 0:
            if (up.x == curr.antpath.back().x &&
                up.y == curr.antpath.back().y){
            }
            else{
                next_step[pcount] = up;
                pcount++;
            }
            break;

        // down case
        case 1:
            if (down.x == curr.antpath.back().x &&
                down.y == curr.antpath.back().y){
            }
            else{
                next_step[pcount] = down;
                pcount++;
            }
            break;

        // left case
        case 2:
            if (left.x == curr.antpath.back().x &&
                left.y == curr.antpath.back().y){
            }
            else{
                next_step[pcount] = left;
                pcount++;
            }
            break;

        // right case
        case 3:
            if (right.x == curr.antpath.back().x &&
                right.y == curr.antpath.back().y){
            }
            else{
                next_step[pcount] = right;
                pcount++;
            }
            break;

        }

    }

    static std::random_device rd;
    int seed = rd();
    static std::mt19937 gen(seed);

    std::uniform_int_distribution<int> ant_distribution(0,3);

    next = next_step[ant_distribution(gen)];

    curr.antpath.push_back(curr.pos);
    curr.pos = next;

    return curr;

}

<|endoftext|>


<commit_before>/**
* @file CleanRoleDecision.h
*
* @author Schahin Tofangchi
*/

#include "CleanRoleDecision.h"
#include <PlatformInterface/Platform.h>
#include <Tools/DataConversion.h>

#include <math.h>
#include <list>

using namespace std;

CleanRoleDecision::CleanRoleDecision()
{
  getDebugParameterList().add(&parameters);

  DEBUG_REQUEST_REGISTER("RoleDecision:min_ball_distance", "draws the circle of the min. ball distance to be recognized as different balls between first/second striker.", false);
}

CleanRoleDecision::~CleanRoleDecision()
{
  getDebugParameterList().remove(&parameters);
}

void CleanRoleDecision::execute() {
    computeStrikers();

    // reset second striker decision, if we doesn't want to use the second striker
    if(!parameters.useSecondStriker) {
        getRoleDecisionModel().secondStriker = std::numeric_limits<unsigned int>::max();
    }
}//end execute

void CleanRoleDecision::computeStrikers()
{
    // skip "first frame"! (until i add myself to teamcomm)
    if(getTeamMessage().data.find(getPlayerInfo().playerNumber) == getTeamMessage().data.end()) { return; }

    // container storing robots, which want to be striker, sorted by their playernumber and their time to ball
    std::map<unsigned int, unsigned int> possible_striker;

    // iterate over all robots(messages)
    for (auto const& i : getTeamMessage().data) {
        unsigned int robotNumber = i.first;
        const TeamMessageData& msg = i.second;

        // if striker lost the ball, he gets a time bonus before he lost the ball completely ...
        double loose_ball_bonus = msg.playerNumber == getRoleDecisionModel().firstStriker?parameters.strikerBonusTime:0.0;

        // check if the robot is able to play and sees the ball
        bool isRobotInactive = !getTeamMessagePlayersState().isPlaying(robotNumber)
                || msg.ballAge < 0 //Ball was never seen
                || (msg.ballAge + getFrameInfo().getTimeSince(msg.frameInfo.getTime()) > parameters.maxBallLostTime + loose_ball_bonus); //Ball isn't fresh

        // ignore inactive robots
        if(robotNumber != getPlayerInfo().playerNumber && isRobotInactive) { continue; }

        // for all active robots, which sees the ball AND previously announced to want to be striker OR is striker ...
        if (msg.custom.wantsToBeStriker || msg.custom.wasStriker) {
            // ... remember them as possible striker with their time to ball
            possible_striker[robotNumber] = msg.custom.timeToBall;
        }
    }//end for

    // get my own message
    auto ownMessage = getTeamMessage().data.at(getPlayerInfo().playerNumber);
    // i want to be striker, if i'm not the goalie and i'm "active" (not fallen/penalized, see the ball)!!!
    getRoleDecisionModel().wantsToBeStriker = ownMessage.playerNumber != 1
                                              && getTeamMessagePlayersState().isPlaying(getPlayerInfo().playerNumber)
                                              && !(ownMessage.ballAge < 0
                                                   || (ownMessage.ballAge + getFrameInfo().getTimeSince(ownMessage.frameInfo.getTime()) > parameters.maxBallLostTime + (getPlayerInfo().playerNumber==getRoleDecisionModel().firstStriker?parameters.strikerBonusTime:0.0)));

    // if i'm striker, i get a time bonus!
    // NOTE: ownTimeToBall can be negative if the robot is close to ball (!)
    double ownTimeToBall = static_cast<double>(ownMessage.custom.timeToBall) - (ownMessage.custom.wasStriker ? 300.0 : 0.0);

    // clear for new striker decision
    getRoleDecisionModel().resetStriker();
    (this->*parameters.strikerSelectionFunction)(possible_striker, ownTimeToBall);

    PLOT(std::string("CleanRoleDecision:FirstStrikerDecision"), getRoleDecisionModel().firstStriker);
    PLOT(std::string("CleanRoleDecision:SecondStrikerDecision"), getRoleDecisionModel().secondStriker);
    PLOT(std::string("CleanRoleDecision:PossibleStriker"), static_cast<double>(possible_striker.size()));
}

void CleanRoleDecision::strikerSelectionByNumber(std::map<unsigned int, unsigned int>& possible_striker, double& ownTimeToBall)
{
    for (auto it = possible_striker.cbegin(); it != possible_striker.cend(); ++it) {
        //If two robots want to be striker, the one with a smaller number is favoured => is the first in the map!!
        // NOTE: we're doesn't using 'timeToBall' as decision criteria by intention!
        //       if goalie is striker, he get's the first striker
        if(getRoleDecisionModel().firstStriker == std::numeric_limits<unsigned int>::max()) {
            getRoleDecisionModel().firstStriker = it->first;
        } else if (getRoleDecisionModel().secondStriker == std::numeric_limits<unsigned int>::max()) {
            getRoleDecisionModel().secondStriker = it->first;
        }
        // if there's a robot closer to the ball than myself, i don't want to be striker!
        if(it->second < ownTimeToBall) {
            getRoleDecisionModel().wantsToBeStriker = false;
        }
    }
}

void CleanRoleDecision::strikerSelectionByTime(std::map<unsigned int, unsigned int>& possible_striker, double &ownTimeToBall)
{
    unsigned int stFastest = std::numeric_limits<unsigned int>::max(); // time to ball of the fastest player
    unsigned int ndFastest = std::numeric_limits<unsigned int>::max(); // time to ball of the second fastest player

    for (auto it = possible_striker.cbegin(); it != possible_striker.cend(); ++it) {
        // current player faster?
        if(it->second < stFastest) {
            // is there already a "fastest" striker ... but the current player is faster
            if(getRoleDecisionModel().firstStriker != std::numeric_limits<unsigned int>::max()) {
                // make the previous player the "second fastest" player
                getRoleDecisionModel().secondStriker = getRoleDecisionModel().firstStriker;
                ndFastest = stFastest;
            }
            // set the fastest player
            getRoleDecisionModel().firstStriker = it->first;
            stFastest = it->second;
        } else if (it->second < ndFastest) {
            // set the second fastest player
            getRoleDecisionModel().secondStriker = it->first;
            ndFastest = it->second;
        }
        // if there's a 2nd robot closer to the ball than myself, i don't want to be (second)striker!
        if(ndFastest < ownTimeToBall) {
            getRoleDecisionModel().wantsToBeStriker = false;
        }
    }
}

void CleanRoleDecision::strikerSelectionByTimeExceptGoalie(std::map<unsigned int, unsigned int>& possible_striker, double &ownTimeToBall)
{
    unsigned int stFastest = std::numeric_limits<unsigned int>::max(); // time to ball of the fastest player
    unsigned int ndFastest = std::numeric_limits<unsigned int>::max(); // time to ball of the second fastest player

    // NOTE: if the goalie is one of the "possible striker", it gets set as first strike! (sorted map!)
    //       and doesn't change, even if someone is faster (condition!)
    // ATTENTION: we're iterating from the smallest player number to the highest!
    for (auto it = possible_striker.cbegin(); it != possible_striker.cend(); ++it) {
        // is current player clearly faster?
        if(it->second < stFastest && (it->second + parameters.strikerSelectionDiffThreshold) < stFastest && getRoleDecisionModel().firstStriker != 1) {
            // is there already a "fastest" striker ... but the current player is faster
            if(getRoleDecisionModel().firstStriker != std::numeric_limits<unsigned int>::max()) {
                // make the previous player the "second fastest" player
                getRoleDecisionModel().secondStriker = getRoleDecisionModel().firstStriker;
                ndFastest = stFastest;
            }
            // set the fastest player
            getRoleDecisionModel().firstStriker = it->first;
            stFastest = it->second;
        } else if (it->second < ndFastest && (it->second + parameters.strikerSelectionDiffThreshold) < ndFastest) {
            // set the second (clearly) fastest player
            getRoleDecisionModel().secondStriker = it->first;
            ndFastest = it->second;
        }
        // if there's a 2nd robot closer to the ball than myself, i don't want to be (second)striker!
        if(ndFastest < ownTimeToBall) {
            getRoleDecisionModel().wantsToBeStriker = false;
        }
    }
}

void CleanRoleDecision::strikerSelectionByTimeExceptGoalieWithBallCompare(std::map<unsigned int, unsigned int>& possible_striker, double &ownTimeToBall)
{
    unsigned int stFastest = std::numeric_limits<unsigned int>::max(); // time to ball of the fastest player
    unsigned int ndFastest = std::numeric_limits<unsigned int>::max(); // time to ball of the second fastest player

    // NOTE: if the goalie is one of the "possible striker", it gets set as first strike! (sorted map!)
    //       and doesn't change, even if someone is faster (condition!)
    // ATTENTION: we're iterating from the smallest player number to the highest!
    for (auto it = possible_striker.cbegin(); it != possible_striker.cend(); ++it) {
        // is current player clearly faster?
        if(it->second < stFastest && (it->second + parameters.strikerSelectionDiffThreshold) < stFastest && getRoleDecisionModel().firstStriker != 1) {
            // is there already a "fastest" striker ... but the current player is faster
            if(getRoleDecisionModel().firstStriker != std::numeric_limits<unsigned int>::max() && isSecondStrikerDifferentFromFirst(getRoleDecisionModel().firstStriker, it->first)) {
                // make the previous player the "second fastest" player, if they see different balls
                getRoleDecisionModel().secondStriker = getRoleDecisionModel().firstStriker;
                ndFastest = stFastest;
            }
            // set the fastest player
            getRoleDecisionModel().firstStriker = it->first;
            stFastest = it->second;
        } else if (it->second < ndFastest && (it->second + parameters.strikerSelectionDiffThreshold) < ndFastest && isSecondStrikerDifferentFromFirst(getRoleDecisionModel().firstStriker, it->first)) {
            // make the previous player the "second fastest" player, if they see different balls
            getRoleDecisionModel().secondStriker = it->first;
            ndFastest = it->second;
        }
        // if there's a 2nd robot closer to the ball than myself, i don't want to be (second)striker!
        if(ndFastest < ownTimeToBall) {
            getRoleDecisionModel().wantsToBeStriker = false;
        }
    }
}

bool CleanRoleDecision::isSecondStrikerDifferentFromFirst(unsigned int firstNumber, unsigned int secondNumber) {
    // retrieve the message of the players
    const auto& first = getTeamMessage().data.at(firstNumber);
    const auto& second = getTeamMessage().data.at(secondNumber);
    // get the global ball position
    Vector2d firstBall = first.pose * first.ballPosition;
    Vector2d secondBall = second.pose * second.ballPosition;
    // check if the ball distance is greater than the given parameter distance radius
    double r = parameters.radius(first.ballPosition, second.ballPosition);

    DEBUG_REQUEST("RoleDecision:min_ball_distance",
      FIELD_DRAWING_CONTEXT;
      PEN("000000", 30);
      CIRCLE(firstBall.x, firstBall.y, r);
      TEXT_DRAWING(firstBall.x, firstBall.y, secondNumber);
    );

    return ((firstBall - secondBall).abs2() > r*r);
}
<commit_msg>better description/info of the drawing<commit_after>/**
* @file CleanRoleDecision.h
*
* @author Schahin Tofangchi
*/

#include "CleanRoleDecision.h"
#include <PlatformInterface/Platform.h>
#include <Tools/DataConversion.h>

#include <math.h>
#include <list>

using namespace std;

CleanRoleDecision::CleanRoleDecision()
{
  getDebugParameterList().add(&parameters);

  DEBUG_REQUEST_REGISTER(
    "RoleDecision:min_ball_distance",
    "Draws the distance in which the balls of the first and second striker are recognized as identical (only with strikerSelection = 3).",
    false
  );
}

CleanRoleDecision::~CleanRoleDecision()
{
  getDebugParameterList().remove(&parameters);
}

void CleanRoleDecision::execute() {
    computeStrikers();

    // reset second striker decision, if we doesn't want to use the second striker
    if(!parameters.useSecondStriker) {
        getRoleDecisionModel().secondStriker = std::numeric_limits<unsigned int>::max();
    }
}//end execute

void CleanRoleDecision::computeStrikers()
{
    // skip "first frame"! (until i add myself to teamcomm)
    if(getTeamMessage().data.find(getPlayerInfo().playerNumber) == getTeamMessage().data.end()) { return; }

    // container storing robots, which want to be striker, sorted by their playernumber and their time to ball
    std::map<unsigned int, unsigned int> possible_striker;

    // iterate over all robots(messages)
    for (auto const& i : getTeamMessage().data) {
        unsigned int robotNumber = i.first;
        const TeamMessageData& msg = i.second;

        // if striker lost the ball, he gets a time bonus before he lost the ball completely ...
        double loose_ball_bonus = msg.playerNumber == getRoleDecisionModel().firstStriker?parameters.strikerBonusTime:0.0;

        // check if the robot is able to play and sees the ball
        bool isRobotInactive = !getTeamMessagePlayersState().isPlaying(robotNumber)
                || msg.ballAge < 0 //Ball was never seen
                || (msg.ballAge + getFrameInfo().getTimeSince(msg.frameInfo.getTime()) > parameters.maxBallLostTime + loose_ball_bonus); //Ball isn't fresh

        // ignore inactive robots
        if(robotNumber != getPlayerInfo().playerNumber && isRobotInactive) { continue; }

        // for all active robots, which sees the ball AND previously announced to want to be striker OR is striker ...
        if (msg.custom.wantsToBeStriker || msg.custom.wasStriker) {
            // ... remember them as possible striker with their time to ball
            possible_striker[robotNumber] = msg.custom.timeToBall;
        }
    }//end for

    // get my own message
    auto ownMessage = getTeamMessage().data.at(getPlayerInfo().playerNumber);
    // i want to be striker, if i'm not the goalie and i'm "active" (not fallen/penalized, see the ball)!!!
    getRoleDecisionModel().wantsToBeStriker = ownMessage.playerNumber != 1
                                              && getTeamMessagePlayersState().isPlaying(getPlayerInfo().playerNumber)
                                              && !(ownMessage.ballAge < 0
                                                   || (ownMessage.ballAge + getFrameInfo().getTimeSince(ownMessage.frameInfo.getTime()) > parameters.maxBallLostTime + (getPlayerInfo().playerNumber==getRoleDecisionModel().firstStriker?parameters.strikerBonusTime:0.0)));

    // if i'm striker, i get a time bonus!
    // NOTE: ownTimeToBall can be negative if the robot is close to ball (!)
    double ownTimeToBall = static_cast<double>(ownMessage.custom.timeToBall) - (ownMessage.custom.wasStriker ? 300.0 : 0.0);

    // clear for new striker decision
    getRoleDecisionModel().resetStriker();
    (this->*parameters.strikerSelectionFunction)(possible_striker, ownTimeToBall);

    PLOT(std::string("CleanRoleDecision:FirstStrikerDecision"), getRoleDecisionModel().firstStriker);
    PLOT(std::string("CleanRoleDecision:SecondStrikerDecision"), getRoleDecisionModel().secondStriker);
    PLOT(std::string("CleanRoleDecision:PossibleStriker"), static_cast<double>(possible_striker.size()));
}

void CleanRoleDecision::strikerSelectionByNumber(std::map<unsigned int, unsigned int>& possible_striker, double& ownTimeToBall)
{
    for (auto it = possible_striker.cbegin(); it != possible_striker.cend(); ++it) {
        //If two robots want to be striker, the one with a smaller number is favoured => is the first in the map!!
        // NOTE: we're doesn't using 'timeToBall' as decision criteria by intention!
        //       if goalie is striker, he get's the first striker
        if(getRoleDecisionModel().firstStriker == std::numeric_limits<unsigned int>::max()) {
            getRoleDecisionModel().firstStriker = it->first;
        } else if (getRoleDecisionModel().secondStriker == std::numeric_limits<unsigned int>::max()) {
            getRoleDecisionModel().secondStriker = it->first;
        }
        // if there's a robot closer to the ball than myself, i don't want to be striker!
        if(it->second < ownTimeToBall) {
            getRoleDecisionModel().wantsToBeStriker = false;
        }
    }
}

void CleanRoleDecision::strikerSelectionByTime(std::map<unsigned int, unsigned int>& possible_striker, double &ownTimeToBall)
{
    unsigned int stFastest = std::numeric_limits<unsigned int>::max(); // time to ball of the fastest player
    unsigned int ndFastest = std::numeric_limits<unsigned int>::max(); // time to ball of the second fastest player

    for (auto it = possible_striker.cbegin(); it != possible_striker.cend(); ++it) {
        // current player faster?
        if(it->second < stFastest) {
            // is there already a "fastest" striker ... but the current player is faster
            if(getRoleDecisionModel().firstStriker != std::numeric_limits<unsigned int>::max()) {
                // make the previous player the "second fastest" player
                getRoleDecisionModel().secondStriker = getRoleDecisionModel().firstStriker;
                ndFastest = stFastest;
            }
            // set the fastest player
            getRoleDecisionModel().firstStriker = it->first;
            stFastest = it->second;
        } else if (it->second < ndFastest) {
            // set the second fastest player
            getRoleDecisionModel().secondStriker = it->first;
            ndFastest = it->second;
        }
        // if there's a 2nd robot closer to the ball than myself, i don't want to be (second)striker!
        if(ndFastest < ownTimeToBall) {
            getRoleDecisionModel().wantsToBeStriker = false;
        }
    }
}

void CleanRoleDecision::strikerSelectionByTimeExceptGoalie(std::map<unsigned int, unsigned int>& possible_striker, double &ownTimeToBall)
{
    unsigned int stFastest = std::numeric_limits<unsigned int>::max(); // time to ball of the fastest player
    unsigned int ndFastest = std::numeric_limits<unsigned int>::max(); // time to ball of the second fastest player

    // NOTE: if the goalie is one of the "possible striker", it gets set as first strike! (sorted map!)
    //       and doesn't change, even if someone is faster (condition!)
    // ATTENTION: we're iterating from the smallest player number to the highest!
    for (auto it = possible_striker.cbegin(); it != possible_striker.cend(); ++it) {
        // is current player clearly faster?
        if(it->second < stFastest && (it->second + parameters.strikerSelectionDiffThreshold) < stFastest && getRoleDecisionModel().firstStriker != 1) {
            // is there already a "fastest" striker ... but the current player is faster
            if(getRoleDecisionModel().firstStriker != std::numeric_limits<unsigned int>::max()) {
                // make the previous player the "second fastest" player
                getRoleDecisionModel().secondStriker = getRoleDecisionModel().firstStriker;
                ndFastest = stFastest;
            }
            // set the fastest player
            getRoleDecisionModel().firstStriker = it->first;
            stFastest = it->second;
        } else if (it->second < ndFastest && (it->second + parameters.strikerSelectionDiffThreshold) < ndFastest) {
            // set the second (clearly) fastest player
            getRoleDecisionModel().secondStriker = it->first;
            ndFastest = it->second;
        }
        // if there's a 2nd robot closer to the ball than myself, i don't want to be (second)striker!
        if(ndFastest < ownTimeToBall) {
            getRoleDecisionModel().wantsToBeStriker = false;
        }
    }
}

void CleanRoleDecision::strikerSelectionByTimeExceptGoalieWithBallCompare(std::map<unsigned int, unsigned int>& possible_striker, double &ownTimeToBall)
{
    unsigned int stFastest = std::numeric_limits<unsigned int>::max(); // time to ball of the fastest player
    unsigned int ndFastest = std::numeric_limits<unsigned int>::max(); // time to ball of the second fastest player

    // NOTE: if the goalie is one of the "possible striker", it gets set as first strike! (sorted map!)
    //       and doesn't change, even if someone is faster (condition!)
    // ATTENTION: we're iterating from the smallest player number to the highest!
    for (auto it = possible_striker.cbegin(); it != possible_striker.cend(); ++it) {
        // is current player clearly faster?
        if(it->second < stFastest && (it->second + parameters.strikerSelectionDiffThreshold) < stFastest && getRoleDecisionModel().firstStriker != 1) {
            // is there already a "fastest" striker ... but the current player is faster
            if(getRoleDecisionModel().firstStriker != std::numeric_limits<unsigned int>::max() && isSecondStrikerDifferentFromFirst(it->first, getRoleDecisionModel().firstStriker)) {
                // make the previous player the "second fastest" player, if they see different balls
                getRoleDecisionModel().secondStriker = getRoleDecisionModel().firstStriker;
                ndFastest = stFastest;
            }
            // set the fastest player
            getRoleDecisionModel().firstStriker = it->first;
            stFastest = it->second;
        } else if (it->second < ndFastest
                   && (it->second + parameters.strikerSelectionDiffThreshold) < ndFastest
                   && isSecondStrikerDifferentFromFirst(getRoleDecisionModel().firstStriker, it->first))
        {
            // make the previous player the "second fastest" player, if they see different balls
            getRoleDecisionModel().secondStriker = it->first;
            ndFastest = it->second;
        }
        // if there's a 2nd robot closer to the ball than myself, i don't want to be (second)striker!
        if(ndFastest < ownTimeToBall) {
            getRoleDecisionModel().wantsToBeStriker = false;
        }
    }
}

bool CleanRoleDecision::isSecondStrikerDifferentFromFirst(unsigned int firstNumber, unsigned int secondNumber) {
    // retrieve the message of the players
    const auto& first = getTeamMessage().data.at(firstNumber);
    const auto& second = getTeamMessage().data.at(secondNumber);
    // get the global ball position
    Vector2d firstBall = first.pose * first.ballPosition;
    Vector2d secondBall = second.pose * second.ballPosition;
    // check if the ball distance is greater than the given parameter distance radius
    double r = parameters.radius(first.ballPosition, second.ballPosition);

    DEBUG_REQUEST("RoleDecision:min_ball_distance",
      FIELD_DRAWING_CONTEXT;
      PEN("000000", 30);
      CIRCLE(firstBall.x, firstBall.y, r);
      TEXT_DRAWING(firstBall.x, firstBall.y+60, secondNumber);
      TEXT_DRAWING(firstBall.x, firstBall.y-60, r);
    );

    return ((firstBall - secondBall).abs2() > r*r);
}
<|endoftext|>


<commit_before>//#####################################################################
// Module math
//#####################################################################
#include <other/core/python/wrap.h>

void wrap_math() {
  OTHER_WRAP(uint128)
  OTHER_WRAP(numeric_limits)
}
<commit_msg>utility/module: Wrap integer_log for Python<commit_after>//#####################################################################
// Module math
//#####################################################################
#include <other/core/math/integer_log.h>
#include <other/core/python/wrap.h>
using namespace other;

void wrap_math() {
  OTHER_WRAP(uint128)
  OTHER_WRAP(numeric_limits)
  OTHER_FUNCTION_2(integer_log,static_cast<int(*)(uint64_t)>(integer_log));
}
<|endoftext|>