<commit_before>// Copyright 2010 Google
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Capacitated Vehicle Routing Problem with Time Windows (and optional orders).
// A description of the problem can be found here:
// http://en.wikipedia.org/wiki/Vehicle_routing_problem.
// The variant which is tackled by this model includes a capacity dimension,
// time windows and optional orders, with a penalty cost if orders are not
// performed. For the sake of simplicty, orders are randomly located and
// distances are computed using the Manhattan distance. Distances are assumed
// to be in meters and times in seconds.

#include <vector>

#include "base/callback.h"
#include "constraint_solver/routing.h"
#include "base/random.h"

using operations_research::Assignment;
using operations_research::IntVar;
using operations_research::RoutingModel;
using operations_research::Solver;
using operations_research::ACMRandom;
using operations_research::StringAppendF;
using operations_research::StringPrintf;
using operations_research::scoped_array;
using operations_research::scoped_ptr;

DEFINE_int32(vrp_orders, 100, "Nodes in the problem.");
DEFINE_int32(vrp_vehicles, 20, "Size of Traveling Salesman Problem instance.");
DEFINE_bool(vrp_use_deterministic_random_seed, false,
            "Use deterministic random seeds.");

const char* kTime = "Time";
const char* kCapacity = "Capacity";

// Random seed generator.
int32 GetSeed() {
  if (FLAGS_vrp_use_deterministic_random_seed) {
    return ACMRandom::DeterministicSeed();
  } else {
    return ACMRandom::HostnamePidTimeSeed();
  }
}

// Location container, contains positions of orders and can be used to obtain
// Manhattan distances/times between locations.
class LocationContainer {
 public:
  explicit LocationContainer(int64 speed)
      : randomizer_(GetSeed()), speed_(speed) {
    CHECK_LT(0, speed_);
  }
  void AddLocation(int64 x, int64 y) {
    locations_.push_back(Location(x, y));
  }
  void AddRandomLocation(int64 x_max, int64 y_max) {
    AddLocation(randomizer_.Uniform(x_max + 1), randomizer_.Uniform(y_max + 1));
  }
  int64 ManhattanDistance(int64 from, int64 to) const {
    return locations_[from].DistanceTo(locations_[to]);
  }
  int64 ManhattanTime(int64 from, int64 to) const {
    return ManhattanDistance(from, to) / speed_;
  }
 private:
  class Location {
   public:
    Location() : x_(0), y_(0) {}
    Location(int64 x, int64 y) : x_(x), y_(y) {}
    int64 DistanceTo(const Location& location) const {
      return Abs(x_ - location.x_) + Abs(y_ - location.y_);
    }
   private:
    static int64 Abs(int64 value) { return std::max(value, -value); }

    int64 x_;
    int64 y_;
  };

  ACMRandom randomizer_;
  const int64 speed_;
  vector<Location> locations_;
};

// Random demand.
class RandomDemand {
 public:
  RandomDemand(int size, int64 depot) : size_(size), depot_(depot) {
    CHECK_LT(0, size_);
  }
  void Initialize() {
    const int64 kDemandMax = 5;
    const int64 kDemandMin = 1;
    demand_.reset(new int64[size_]);
    ACMRandom randomizer(GetSeed());
    for (int order = 0; order < size_; ++order) {
      if (order == depot_) {
        demand_[order] = 0;
      } else {
        demand_[order] =
            kDemandMin + randomizer.Uniform(kDemandMax - kDemandMin + 1);
      }
    }
  }
  int64 Demand(int64 from, int64 to) const {
    return demand_[from];
  }
 private:
  scoped_array<int64> demand_;
  const int size_;
  const int64 depot_;
};

// Service time (proportional to demand) + transition time callback.
class ServiceTimePlusTransition {
 public:
  ServiceTimePlusTransition(int64 time_per_demand_unit,
                            Solver::IndexEvaluator2* demand,
                            Solver::IndexEvaluator2* transition_time)
      : time_per_demand_unit_(time_per_demand_unit),
        demand_(demand),
        transition_time_(transition_time) {}
  int64 Compute(int64 from, int64 to) const {
    return time_per_demand_unit_ * demand_->Run(from, to)
        + transition_time_->Run(from, to);
  }
 private:
  const int64 time_per_demand_unit_;
  scoped_ptr<Solver::IndexEvaluator2> demand_;
  scoped_ptr<Solver::IndexEvaluator2> transition_time_;
};

// Route plan displayer.
// TODO(user): Move the display code to the routing library.
void DisplayPlan(const RoutingModel& routing, const Assignment& plan) {
  // Display plan cost.
  string plan_output = StringPrintf("Cost %lld\n", plan.ObjectiveValue());

  // Display dropped orders.
  string dropped;
  for (int order = 1; order < routing.nodes(); ++order) {
    if (plan.Value(routing.NextVar(order)) == order) {
      if (dropped.empty()) {
        StringAppendF(&dropped, " %d", order);
      } else {
        StringAppendF(&dropped, ", %d", order);
      }
    }
  }
  if (!dropped.empty()) {
    plan_output += "Dropped orders:" + dropped + "\n";
  }

  // Display actual output for each vehicle.
  for (int route_number = 0;
       route_number < routing.vehicles();
       ++route_number) {
    int64 order = routing.Start(route_number);
    StringAppendF(&plan_output, "Route %d: ", route_number);
    if (routing.IsEnd(plan.Value(routing.NextVar(order)))) {
      plan_output += "Empty\n";
    } else {
      while (!routing.IsEnd(order)) {
        IntVar* load_var = routing.CumulVar(order, kCapacity);
        IntVar* time_var = routing.CumulVar(order, kTime);
        StringAppendF(&plan_output, "%lld Load(%lld) Time(%lld, %lld) -> ",
                      order,
                      plan.Value(load_var),
                      plan.Min(time_var),
                      plan.Max(time_var));
        order = plan.Value(routing.NextVar(order));
      }
      IntVar* load_var = routing.CumulVar(order, kCapacity);
      IntVar* time_var = routing.CumulVar(order, kTime);
      StringAppendF(&plan_output, "%lld Load(%lld) Time(%lld, %lld)\n",
                    order,
                    plan.Value(load_var),
                    plan.Min(time_var),
                    plan.Max(time_var));
    }
  }
  LG << plan_output;
}

int main(int argc, char **argv) {
  google::ParseCommandLineFlags(&argc, &argv, true);
  CHECK_LT(0, FLAGS_vrp_orders) << "Specify an instance size greater than 0.";
  CHECK_LT(0, FLAGS_vrp_vehicles) << "Specify a non-null vehicle fleet size.";
  // VRP of size FLAGS_vrp_size.
  // Nodes are indexed from 0 to FLAGS_vrp_orders, the starts and ends of
  // the routes are at node 0.
  const int64 kDepot = 0;
  RoutingModel routing(FLAGS_vrp_orders + 1, FLAGS_vrp_vehicles);
  routing.SetDepot(kDepot);
  // Setting first solution heuristic (cheapest addition).
  routing.SetCommandLineOption("routing_first_solution", "PathCheapestArc");
  // Disabling Large Neighborhood Search, comment out to activate it.
  routing.SetCommandLineOption("routing_no_lns", "true");

  // Setting up locations.
  const int64 kXMax = 100000;
  const int64 kYMax = 100000;
  const int64 kSpeed = 10;
  LocationContainer locations(kSpeed);
  for (int location = 0; location <= FLAGS_vrp_orders; ++location) {
    locations.AddRandomLocation(kXMax, kYMax);
  }

  // Setting the cost function.
  routing.SetCost(NewPermanentCallback(&locations,
                                       &LocationContainer::ManhattanDistance));

  // Adding capacity dimension constraints.
  const int64 kVehicleCapacity = 40;
  const int64 kNullCapacitySlack = 0;
  RandomDemand demand(routing.nodes(), kDepot);
  demand.Initialize();
  routing.AddDimension(NewPermanentCallback(&demand, &RandomDemand::Demand),
                       kNullCapacitySlack, kVehicleCapacity, kCapacity);

  // Adding time dimension constraints.
  const int64 kTimePerDemandUnit = 300;
  const int64 kHorizon = 24 * 3600;
  ServiceTimePlusTransition time(
      kTimePerDemandUnit,
      NewPermanentCallback(&demand, &RandomDemand::Demand),
      NewPermanentCallback(&locations, &LocationContainer::ManhattanTime));
  routing.AddDimension(
      NewPermanentCallback(&time,
                           &ServiceTimePlusTransition::Compute),
      kHorizon, kHorizon, kTime);
  // Adding time windows.
  ACMRandom randomizer(GetSeed());
  const int64 kTWDuration = 5 * 3600;
  for (int order = 1; order < routing.nodes(); ++order) {
    const int64 start = randomizer.Uniform(kHorizon - kTWDuration);
    routing.CumulVar(order, kTime)->SetRange(start, start + kTWDuration);
  }

  // Adding penalty costs to allow skipping orders.
  const int64 kPenalty = 100000;
  for (int order = 1; order < routing.nodes(); ++order) {
    vector<int64> orders(1, order);
    routing.AddDisjunction(orders, kPenalty);
  }

  // Solve, returns a solution if any (owned by RoutingModel).
  const Assignment* solution = routing.Solve();
  if (solution != NULL) {
    DisplayPlan(routing, *solution);
  } else {
    LG << "No solution found.";
  }
  return 0;
}
<commit_msg>cosmetic fix<commit_after>// Copyright 2010 Google
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Capacitated Vehicle Routing Problem with Time Windows (and optional orders).
// A description of the problem can be found here:
// http://en.wikipedia.org/wiki/Vehicle_routing_problem.
// The variant which is tackled by this model includes a capacity dimension,
// time windows and optional orders, with a penalty cost if orders are not
// performed. For the sake of simplicty, orders are randomly located and
// distances are computed using the Manhattan distance. Distances are assumed
// to be in meters and times in seconds.

#include <vector>

#include "base/callback.h"
#include "constraint_solver/routing.h"
#include "base/random.h"

using operations_research::Assignment;
using operations_research::IntVar;
using operations_research::RoutingModel;
using operations_research::Solver;
using operations_research::ACMRandom;
using operations_research::StringAppendF;
using operations_research::StringPrintf;
using operations_research::scoped_array;
using operations_research::scoped_ptr;

DEFINE_int32(vrp_orders, 100, "Nodes in the problem.");
DEFINE_int32(vrp_vehicles, 20, "Size of Traveling Salesman Problem instance.");
DEFINE_bool(vrp_use_deterministic_random_seed, false,
            "Use deterministic random seeds.");

const char* kTime = "Time";
const char* kCapacity = "Capacity";

// Random seed generator.
int32 GetSeed() {
  if (FLAGS_vrp_use_deterministic_random_seed) {
    return ACMRandom::DeterministicSeed();
  } else {
    return ACMRandom::HostnamePidTimeSeed();
  }
}

// Location container, contains positions of orders and can be used to obtain
// Manhattan distances/times between locations.
class LocationContainer {
 public:
  explicit LocationContainer(int64 speed)
      : randomizer_(GetSeed()), speed_(speed) {
    CHECK_LT(0, speed_);
  }
  void AddLocation(int64 x, int64 y) {
    locations_.push_back(Location(x, y));
  }
  void AddRandomLocation(int64 x_max, int64 y_max) {
    AddLocation(randomizer_.Uniform(x_max + 1), randomizer_.Uniform(y_max + 1));
  }
  int64 ManhattanDistance(int64 from, int64 to) const {
    return locations_[from].DistanceTo(locations_[to]);
  }
  int64 ManhattanTime(int64 from, int64 to) const {
    return ManhattanDistance(from, to) / speed_;
  }
 private:
  class Location {
   public:
    Location() : x_(0), y_(0) {}
    Location(int64 x, int64 y) : x_(x), y_(y) {}
    int64 DistanceTo(const Location& location) const {
      return Abs(x_ - location.x_) + Abs(y_ - location.y_);
    }
   private:
    static int64 Abs(int64 value) { return std::max(value, -value); }

    int64 x_;
    int64 y_;
  };

  ACMRandom randomizer_;
  const int64 speed_;
  vector<Location> locations_;
};

// Random demand.
class RandomDemand {
 public:
  RandomDemand(int size, int64 depot) : size_(size), depot_(depot) {
    CHECK_LT(0, size_);
  }
  void Initialize() {
    const int64 kDemandMax = 5;
    const int64 kDemandMin = 1;
    demand_.reset(new int64[size_]);
    ACMRandom randomizer(GetSeed());
    for (int order = 0; order < size_; ++order) {
      if (order == depot_) {
        demand_[order] = 0;
      } else {
        demand_[order] =
            kDemandMin + randomizer.Uniform(kDemandMax - kDemandMin + 1);
      }
    }
  }
  int64 Demand(int64 from, int64 to) const {
    return demand_[from];
  }
 private:
  scoped_array<int64> demand_;
  const int size_;
  const int64 depot_;
};

// Service time (proportional to demand) + transition time callback.
class ServiceTimePlusTransition {
 public:
  ServiceTimePlusTransition(int64 time_per_demand_unit,
                            Solver::IndexEvaluator2* demand,
                            Solver::IndexEvaluator2* transition_time)
      : time_per_demand_unit_(time_per_demand_unit),
        demand_(demand),
        transition_time_(transition_time) {}
  int64 Compute(int64 from, int64 to) const {
    return time_per_demand_unit_ * demand_->Run(from, to)
        + transition_time_->Run(from, to);
  }
 private:
  const int64 time_per_demand_unit_;
  scoped_ptr<Solver::IndexEvaluator2> demand_;
  scoped_ptr<Solver::IndexEvaluator2> transition_time_;
};

// Route plan displayer.
// TODO(user): Move the display code to the routing library.
void DisplayPlan(const RoutingModel& routing, const Assignment& plan) {
  // Display plan cost.
  string plan_output = StringPrintf("Cost %lld\n", plan.ObjectiveValue());

  // Display dropped orders.
  string dropped;
  for (int order = 1; order < routing.nodes(); ++order) {
    if (plan.Value(routing.NextVar(order)) == order) {
      if (dropped.empty()) {
        StringAppendF(&dropped, " %d", order);
      } else {
        StringAppendF(&dropped, ", %d", order);
      }
    }
  }
  if (!dropped.empty()) {
    plan_output += "Dropped orders:" + dropped + "\n";
  }

  // Display actual output for each vehicle.
  for (int route_number = 0;
       route_number < routing.vehicles();
       ++route_number) {
    int64 order = routing.Start(route_number);
    StringAppendF(&plan_output, "Route %d: ", route_number);
    if (routing.IsEnd(plan.Value(routing.NextVar(order)))) {
      plan_output += "Empty\n";
    } else {
      while (!routing.IsEnd(order)) {
        IntVar* load_var = routing.CumulVar(order, kCapacity);
        IntVar* time_var = routing.CumulVar(order, kTime);
        StringAppendF(&plan_output, "%lld Load(%lld) Time(%lld, %lld) -> ",
                      order,
                      plan.Value(load_var),
                      plan.Min(time_var),
                      plan.Max(time_var));
        order = plan.Value(routing.NextVar(order));
      }
      IntVar* load_var = routing.CumulVar(order, kCapacity);
      IntVar* time_var = routing.CumulVar(order, kTime);
      StringAppendF(&plan_output, "%lld Load(%lld) Time(%lld, %lld)\n",
                    order,
                    plan.Value(load_var),
                    plan.Min(time_var),
                    plan.Max(time_var));
    }
  }
  LG << plan_output;
}

int main(int argc, char **argv) {
  google::ParseCommandLineFlags(&argc, &argv, true);
  CHECK_LT(0, FLAGS_vrp_orders) << "Specify an instance size greater than 0.";
  CHECK_LT(0, FLAGS_vrp_vehicles) << "Specify a non-null vehicle fleet size.";
  // VRP of size FLAGS_vrp_size.
  // Nodes are indexed from 0 to FLAGS_vrp_orders, the starts and ends of
  // the routes are at node 0.
  const int64 kDepot = 0;
  RoutingModel routing(FLAGS_vrp_orders + 1, FLAGS_vrp_vehicles);
  routing.SetDepot(kDepot);
  // Setting first solution heuristic (cheapest addition).
  routing.SetCommandLineOption("routing_first_solution", "PathCheapestArc");
  // Disabling Large Neighborhood Search, comment out to activate it.
  routing.SetCommandLineOption("routing_no_lns", "true");

  // Setting up locations.
  const int64 kXMax = 100000;
  const int64 kYMax = 100000;
  const int64 kSpeed = 10;
  LocationContainer locations(kSpeed);
  for (int location = 0; location <= FLAGS_vrp_orders; ++location) {
    locations.AddRandomLocation(kXMax, kYMax);
  }

  // Setting the cost function.
  routing.SetCost(NewPermanentCallback(&locations,
                                       &LocationContainer::ManhattanDistance));

  // Adding capacity dimension constraints.
  const int64 kVehicleCapacity = 40;
  const int64 kNullCapacitySlack = 0;
  RandomDemand demand(routing.nodes(), kDepot);
  demand.Initialize();
  routing.AddDimension(NewPermanentCallback(&demand, &RandomDemand::Demand),
                       kNullCapacitySlack, kVehicleCapacity, kCapacity);

  // Adding time dimension constraints.
  const int64 kTimePerDemandUnit = 300;
  const int64 kHorizon = 24 * 3600;
  ServiceTimePlusTransition time(
      kTimePerDemandUnit,
      NewPermanentCallback(&demand, &RandomDemand::Demand),
      NewPermanentCallback(&locations, &LocationContainer::ManhattanTime));
  routing.AddDimension(
      NewPermanentCallback(&time,
                           &ServiceTimePlusTransition::Compute),
      kHorizon, kHorizon, kTime);
  // Adding time windows.
  ACMRandom randomizer(GetSeed());
  const int64 kTWDuration = 5 * 3600;
  for (int order = 1; order < routing.nodes(); ++order) {
    const int64 start = randomizer.Uniform(kHorizon - kTWDuration);
    routing.CumulVar(order, kTime)->SetRange(start, start + kTWDuration);
  }

  // Adding penalty costs to allow skipping orders.
  const int64 kPenalty = 100000;
  for (int order = 1; order < routing.nodes(); ++order) {
    vector<int64> orders(1, order);
    routing.AddDisjunction(orders, kPenalty);
  }

  // Solve, returns a solution if any (owned by RoutingModel).
  const Assignment* solution = routing.Solve();
  if (solution != NULL) {
    DisplayPlan(routing, *solution);
  } else {
    LG << "No solution found.";
  }
  return 0;
}
<|endoftext|>


<commit_before>//                       MFEM Example 25 - Parallel Version
//
// Compile with: make ex25p
//
// Sample runs:  mpirun -np 4 ex25p -m ../data/star.mesh
//               mpirun -np 4 ex25p -m ../data/fichera.mesh
//               mpirun -np 4 ex25p -m ../data/beam-hex.mesh
//
// Device sample runs:
//               mpirun -np 4 ex25p -d cuda
//               mpirun -np 4 ex25p -d occa-cuda
//               mpirun -np 4 ex25p -d raja-omp
//               mpirun -np 4 ex25p -d ceed-cpu
//               mpirun -np 4 ex25p -d ceed-cuda
//
// Description:  This example code demonstrates the use of MFEM to define a
//               simple finite element discretization of the Laplace problem
//               -Delta u = 1 with homogeneous Dirichlet boundary conditions
//               as in example 1. It highlights on the creation of a hierarchy
//               of discretization spaces with partial assembly and the
//               construction of an efficient p-multigrid preconditioner for the
//               iterative solver.

#include "mfem.hpp"
#include <fstream>
#include <iostream>

using namespace std;
using namespace mfem;

class MultigridDiffusionOperator : public MultigridOperator
{
private:
   ParMesh* pmesh_lor;
   H1_FECollection* fec_lor;
   ParFiniteElementSpace* fespace_lor;
   Array<BilinearForm*> bfs;
   Array<Array<int>*> essentialTrueDofs;
   ConstantCoefficient one;

public:
   /// Constructor for a multigrid diffusion operator for a given SpaceHierarchy. Uses Chebyshev accelerated smoothing.
   MultigridDiffusionOperator(ParSpaceHierarchy& spaceHierarchy,
                              Array<int>& ess_bdr, int chebyshevOrder = 2)
      : one(1.0)
   {
      ParMesh* pmesh = spaceHierarchy.GetFESpaceAtLevel(0).GetParMesh();
      pmesh_lor = new ParMesh(pmesh, 1, BasisType::GaussLobatto);
      fec_lor =
         new H1_FECollection(1, pmesh->Dimension(), BasisType::GaussLobatto);
      fespace_lor = new ParFiniteElementSpace(pmesh_lor, fec_lor);
      ParBilinearForm* a_lor = new ParBilinearForm(fespace_lor);
      AddIntegrators(a_lor);
      a_lor->UsePrecomputedSparsity();
      a_lor->Assemble();
      bfs.Append(a_lor);

      essentialTrueDofs.Append(new Array<int>());
      spaceHierarchy.GetFESpaceAtLevel(0).GetEssentialTrueDofs(
         ess_bdr, *essentialTrueDofs.Last());

      HypreParMatrix* hypreCoarseMat = new HypreParMatrix();
      a_lor->FormSystemMatrix(*essentialTrueDofs.Last(), *hypreCoarseMat);

      HypreBoomerAMG* amg = new HypreBoomerAMG(*hypreCoarseMat);
      amg->SetPrintLevel(-1);

      AddCoarsestLevel(hypreCoarseMat, amg, true, true);

      for (int level = 1; level < spaceHierarchy.GetNumLevels(); ++level)
      {
         ParBilinearForm* form = new ParBilinearForm(&spaceHierarchy.GetFESpaceAtLevel(
                                                        level));
         form->SetAssemblyLevel(AssemblyLevel::PARTIAL);
         AddIntegrators(form);
         form->Assemble();
         bfs.Append(form);

         essentialTrueDofs.Append(new Array<int>());
         spaceHierarchy.GetFESpaceAtLevel(level).GetEssentialTrueDofs(
            ess_bdr, *essentialTrueDofs.Last());

         OperatorPtr opr;
         opr.SetType(Operator::ANY_TYPE);
         form->FormSystemMatrix(*essentialTrueDofs.Last(), opr);
         opr.SetOperatorOwner(false);

         Vector diag(spaceHierarchy.GetFESpaceAtLevel(level).GetTrueVSize());
         form->AssembleDiagonal(diag);

         Solver* smoother = new OperatorChebyshevSmoother(
            opr.Ptr(), diag, *essentialTrueDofs.Last(), chebyshevOrder, pmesh->GetComm());

         Operator* P =
            new TrueTransferOperator(spaceHierarchy.GetFESpaceAtLevel(level - 1),
                                     spaceHierarchy.GetFESpaceAtLevel(level));

         AddLevel(opr.Ptr(), smoother, P, true, true, true);
      }
   }

   virtual ~MultigridDiffusionOperator()
   {
      for (int i = 0; i < bfs.Size(); ++i)
      {
         delete bfs[i];
      }

      for (int i = 0; i < essentialTrueDofs.Size(); ++i)
      {
         delete essentialTrueDofs[i];
      }

      delete fespace_lor;
      delete fec_lor;
      delete pmesh_lor;
   }

   void EliminateBCs(Vector& x, Vector& b, Vector& X, Vector& B)
   {
      OperatorPtr oper;
      bfs.Last()->FormLinearSystem(*essentialTrueDofs.Last(), x, b,
                                   oper, X, B);
   }

   void RecoverFEMSolution(const Vector& X, const Vector& b, Vector& x)
   {
      bfs.Last()->RecoverFEMSolution(X, b, x);
   }

private:
   void AddIntegrators(BilinearForm* form)
   {
      form->AddDomainIntegrator(new DiffusionIntegrator(one));
   }
};

int main(int argc, char *argv[])
{
   // 1. Initialize MPI.
   int num_procs, myid;
   MPI_Init(&argc, &argv);
   MPI_Comm_size(MPI_COMM_WORLD, &num_procs);
   MPI_Comm_rank(MPI_COMM_WORLD, &myid);

   // 2. Parse command-line options.
   const char *mesh_file = "../data/star.mesh";
   int geometricrefinements = 0;
   int orderrefinements = 2;
   const char *device_config = "cpu";
   bool visualization = true;

   OptionsParser args(argc, argv);
   args.AddOption(&mesh_file, "-m", "--mesh",
                  "Mesh file to use.");
   args.AddOption(&geometricrefinements, "-gr", "--geometricrefinements",
                  "Number of geometric refinements done prior to order refinements.");
   args.AddOption(&orderrefinements, "-or", "--orderrefinements",
                  "Number of order refinements. Finest level in the hierarchy has order 2^{or}.");
   args.AddOption(&device_config, "-d", "--device",
                  "Device configuration string, see Device::Configure().");
   args.AddOption(&visualization, "-vis", "--visualization", "-no-vis",
                  "--no-visualization",
                  "Enable or disable GLVis visualization.");
   args.Parse();
   if (!args.Good())
   {
      if (myid == 0)
      {
         args.PrintUsage(cout);
      }
      MPI_Finalize();
      return 1;
   }
   if (myid == 0)
   {
      args.PrintOptions(cout);
   }

   // 3. Enable hardware devices such as GPUs, and programming models such as
   //    CUDA, OCCA, RAJA and OpenMP based on command line options.
   Device device(device_config);
   if (myid == 0) { device.Print(); }

   // 4. Read the (serial) mesh from the given mesh file on all processors.  We
   //    can handle triangular, quadrilateral, tetrahedral, hexahedral, surface
   //    and volume meshes with the same code.
   Mesh *mesh = new Mesh(mesh_file, 1, 1);
   int dim = mesh->Dimension();

   // 5. Refine the serial mesh on all processors to increase the resolution. In
   //    this example we do 'ref_levels' of uniform refinement. We choose
   //    'ref_levels' to be the largest number that gives a final mesh with no
   //    more than 10,000 elements.
   {
      int ref_levels =
         (int)floor(log(1000./mesh->GetNE())/log(2.)/dim);
      for (int l = 0; l < ref_levels; l++)
      {
         mesh->UniformRefinement();
      }
   }

   // 6. Define a parallel mesh by a partitioning of the serial mesh. Refine
   //    this mesh further in parallel to increase the resolution. Once the
   //    parallel mesh is defined, the serial mesh can be deleted.
   ParMesh *pmesh = new ParMesh(MPI_COMM_WORLD, *mesh);
   delete mesh;
   {
      int par_ref_levels = 2;
      for (int l = 0; l < par_ref_levels; l++)
      {
         pmesh->UniformRefinement();
      }
   }

   // 7. Define a parallel finite element space hierarchy on the parallel mesh.
   //    Here we use continuous Lagrange finite elements. We start with order 1
   //    on the coarse level and increase the order by of factor of 2 for each
   //    additional level.
   FiniteElementCollection *fec = new H1_FECollection(1, dim);
   ParFiniteElementSpace *fespace = new ParFiniteElementSpace(pmesh, fec);

   Array<FiniteElementCollection*> collections;
   collections.Append(fec);
   ParSpaceHierarchy* spaceHierarchy = new ParSpaceHierarchy(pmesh, fespace, true,
                                                             true);
   for (int level = 0; level < geometricrefinements; ++level)
   {
      spaceHierarchy->AddUniformlyRefinedLevel();
   }
   for (int level = 0; level < orderrefinements; ++level)
   {
      collections.Append(new H1_FECollection(std::pow(2, level+1), dim));
      spaceHierarchy->AddOrderRefinedLevel(collections.Last());
   }

   HYPRE_Int size = spaceHierarchy->GetFinestFESpace().GlobalTrueVSize();
   if (myid == 0)
   {
      cout << "Number of finite element unknowns: " << size << endl;
   }

   // 8. Set up the parallel linear form b(.) which corresponds to the
   //    right-hand side of the FEM linear system, which in this case is
   //    (1,phi_i) where phi_i are the basis functions in fespace.
   ParLinearForm *b = new ParLinearForm(&spaceHierarchy->GetFinestFESpace());
   ConstantCoefficient one(1.0);
   b->AddDomainIntegrator(new DomainLFIntegrator(one));
   b->Assemble();

   // 9. Define the solution vector x as a parallel finite element grid function
   //    corresponding to fespace. Initialize x with initial guess of zero,
   //    which satisfies the boundary conditions.
   ParGridFunction x(&spaceHierarchy->GetFinestFESpace());
   x = 0.0;

   // 10. Create the multigrid operator using the previously created parallel
   //     SpaceHierarchy and additional boundary information. This operator
   //     is then used to create the MultigridSolver as a preconditioner in the
   //     iterative solver.
   Array<int> ess_bdr(pmesh->bdr_attributes.Max());
   if (pmesh->bdr_attributes.Size())
   {
      ess_bdr = 1;
   }
   MultigridDiffusionOperator* mgOperator = new MultigridDiffusionOperator(
      *spaceHierarchy, ess_bdr);
   MultigridSolver* prec = new MultigridSolver(mgOperator,
                                               MultigridSolver::CycleType::VCYCLE,
                                               1, 1);

   Vector X, B;
   mgOperator->EliminateBCs(x, *b, X, B);

   // 11. Solve the linear system A X = B.
   CGSolver cg(MPI_COMM_WORLD);
   cg.SetRelTol(1e-12);
   cg.SetMaxIter(2000);
   cg.SetPrintLevel(1);
   cg.SetOperator(*mgOperator);
   cg.SetPreconditioner(*prec);
   cg.Mult(B, X);

   // 12. Recover the parallel grid function corresponding to X. This is the
   //     local finite element solution on each processor.
   mgOperator->RecoverFEMSolution(X, *b, x);

   // 13. Save the refined mesh and the solution in parallel. This output can
   //     be viewed later using GLVis: "glvis -np <np> -m mesh -g sol".
   {
      ostringstream mesh_name, sol_name;
      mesh_name << "mesh." << setfill('0') << setw(6) << myid;
      sol_name << "sol." << setfill('0') << setw(6) << myid;

      ofstream mesh_ofs(mesh_name.str().c_str());
      mesh_ofs.precision(8);
      pmesh->Print(mesh_ofs);

      ofstream sol_ofs(sol_name.str().c_str());
      sol_ofs.precision(8);
      x.Save(sol_ofs);
   }

   // 14. Send the solution by socket to a GLVis server.
   if (visualization)
   {
      char vishost[] = "localhost";
      int  visport   = 19916;
      socketstream sol_sock(vishost, visport);
      sol_sock << "parallel " << num_procs << " " << myid << "\n";
      sol_sock.precision(8);
      sol_sock << "solution\n" << *pmesh << x << flush;
   }

   // 15. Free the used memory.
   delete prec;
   delete mgOperator;
   delete b;
   delete spaceHierarchy;
   for (int level = 0; level < collections.Size(); ++level)
   {
      delete collections[level];
   }

   MPI_Finalize();

   return 0;
}
<commit_msg>Update examples/ex25p.cpp<commit_after>//                       MFEM Example 25 - Parallel Version
//
// Compile with: make ex25p
//
// Sample runs:  mpirun -np 4 ex25p -m ../data/star.mesh
//               mpirun -np 4 ex25p -m ../data/fichera.mesh
//               mpirun -np 4 ex25p -m ../data/beam-hex.mesh
//
// Device sample runs:
//               mpirun -np 4 ex25p -d cuda
//               mpirun -np 4 ex25p -d occa-cuda
//               mpirun -np 4 ex25p -d raja-omp
//               mpirun -np 4 ex25p -d ceed-cpu
//               mpirun -np 4 ex25p -d ceed-cuda
//
// Description:  This example code demonstrates the use of MFEM to define a
//               simple finite element discretization of the Laplace problem
//               -Delta u = 1 with homogeneous Dirichlet boundary conditions
//               as in example 1. It highlights on the creation of a hierarchy
//               of discretization spaces with partial assembly and the
//               construction of an efficient p-multigrid preconditioner for the
//               iterative solver.

#include "mfem.hpp"
#include <fstream>
#include <iostream>

using namespace std;
using namespace mfem;

class MultigridDiffusionOperator : public MultigridOperator
{
private:
   ParMesh* pmesh_lor;
   H1_FECollection* fec_lor;
   ParFiniteElementSpace* fespace_lor;
   Array<BilinearForm*> bfs;
   Array<Array<int>*> essentialTrueDofs;
   ConstantCoefficient one;

public:
   /// Constructor for a multigrid diffusion operator for a given SpaceHierarchy. Uses Chebyshev accelerated smoothing.
   MultigridDiffusionOperator(ParSpaceHierarchy& spaceHierarchy,
                              Array<int>& ess_bdr, int chebyshevOrder = 2)
      : one(1.0)
   {
      ParMesh* pmesh = spaceHierarchy.GetFESpaceAtLevel(0).GetParMesh();
      pmesh_lor = new ParMesh(pmesh, 1, BasisType::GaussLobatto);
      fec_lor =
         new H1_FECollection(1, pmesh->Dimension(), BasisType::GaussLobatto);
      fespace_lor = new ParFiniteElementSpace(pmesh_lor, fec_lor);
      ParBilinearForm* a_lor = new ParBilinearForm(fespace_lor);
      AddIntegrators(a_lor);
      a_lor->UsePrecomputedSparsity();
      a_lor->Assemble();
      bfs.Append(a_lor);

      essentialTrueDofs.Append(new Array<int>());
      spaceHierarchy.GetFESpaceAtLevel(0).GetEssentialTrueDofs(
         ess_bdr, *essentialTrueDofs.Last());

      HypreParMatrix* hypreCoarseMat = new HypreParMatrix();
      a_lor->FormSystemMatrix(*essentialTrueDofs.Last(), *hypreCoarseMat);

      HypreBoomerAMG* amg = new HypreBoomerAMG(*hypreCoarseMat);
      amg->SetPrintLevel(-1);

      AddCoarsestLevel(hypreCoarseMat, amg, true, true);

      for (int level = 1; level < spaceHierarchy.GetNumLevels(); ++level)
      {
         ParBilinearForm* form = new ParBilinearForm(&spaceHierarchy.GetFESpaceAtLevel(
                                                        level));
         form->SetAssemblyLevel(AssemblyLevel::PARTIAL);
         AddIntegrators(form);
         form->Assemble();
         bfs.Append(form);

         essentialTrueDofs.Append(new Array<int>());
         spaceHierarchy.GetFESpaceAtLevel(level).GetEssentialTrueDofs(
            ess_bdr, *essentialTrueDofs.Last());

         OperatorPtr opr;
         opr.SetType(Operator::ANY_TYPE);
         form->FormSystemMatrix(*essentialTrueDofs.Last(), opr);
         opr.SetOperatorOwner(false);

         Vector diag(spaceHierarchy.GetFESpaceAtLevel(level).GetTrueVSize());
         form->AssembleDiagonal(diag);

         Solver* smoother = new OperatorChebyshevSmoother(
            opr.Ptr(), diag, *essentialTrueDofs.Last(), chebyshevOrder, pmesh->GetComm());

         Operator* P =
            new TrueTransferOperator(spaceHierarchy.GetFESpaceAtLevel(level - 1),
                                     spaceHierarchy.GetFESpaceAtLevel(level));

         AddLevel(opr.Ptr(), smoother, P, true, true, true);
      }
   }

   virtual ~MultigridDiffusionOperator()
   {
      for (int i = 0; i < bfs.Size(); ++i)
      {
         delete bfs[i];
      }

      for (int i = 0; i < essentialTrueDofs.Size(); ++i)
      {
         delete essentialTrueDofs[i];
      }

      delete fespace_lor;
      delete fec_lor;
      delete pmesh_lor;
   }

   void EliminateBCs(Vector& x, Vector& b, Vector& X, Vector& B)
   {
      OperatorPtr oper;
      bfs.Last()->FormLinearSystem(*essentialTrueDofs.Last(), x, b,
                                   oper, X, B);
   }

   void RecoverFEMSolution(const Vector& X, const Vector& b, Vector& x)
   {
      bfs.Last()->RecoverFEMSolution(X, b, x);
   }

private:
   void AddIntegrators(BilinearForm* form)
   {
      form->AddDomainIntegrator(new DiffusionIntegrator(one));
   }
};

int main(int argc, char *argv[])
{
   // 1. Initialize MPI.
   int num_procs, myid;
   MPI_Init(&argc, &argv);
   MPI_Comm_size(MPI_COMM_WORLD, &num_procs);
   MPI_Comm_rank(MPI_COMM_WORLD, &myid);

   // 2. Parse command-line options.
   const char *mesh_file = "../data/star.mesh";
   int geometricrefinements = 0;
   int orderrefinements = 2;
   const char *device_config = "cpu";
   bool visualization = true;

   OptionsParser args(argc, argv);
   args.AddOption(&mesh_file, "-m", "--mesh",
                  "Mesh file to use.");
   args.AddOption(&geometricrefinements, "-gr", "--geometricrefinements",
                  "Number of geometric refinements done prior to order refinements.");
   args.AddOption(&orderrefinements, "-or", "--orderrefinements",
                  "Number of order refinements. Finest level in the hierarchy has order 2^{or}.");
   args.AddOption(&device_config, "-d", "--device",
                  "Device configuration string, see Device::Configure().");
   args.AddOption(&visualization, "-vis", "--visualization", "-no-vis",
                  "--no-visualization",
                  "Enable or disable GLVis visualization.");
   args.Parse();
   if (!args.Good())
   {
      if (myid == 0)
      {
         args.PrintUsage(cout);
      }
      MPI_Finalize();
      return 1;
   }
   if (myid == 0)
   {
      args.PrintOptions(cout);
   }

   // 3. Enable hardware devices such as GPUs, and programming models such as
   //    CUDA, OCCA, RAJA and OpenMP based on command line options.
   Device device(device_config);
   if (myid == 0) { device.Print(); }

   // 4. Read the (serial) mesh from the given mesh file on all processors.  We
   //    can handle triangular, quadrilateral, tetrahedral, hexahedral, surface
   //    and volume meshes with the same code.
   Mesh *mesh = new Mesh(mesh_file, 1, 1);
   int dim = mesh->Dimension();

   // 5. Refine the serial mesh on all processors to increase the resolution. In
   //    this example we do 'ref_levels' of uniform refinement. We choose
   //    'ref_levels' to be the largest number that gives a final mesh with no
   //    more than 1,000 elements.
   {
      int ref_levels =
         (int)floor(log(1000./mesh->GetNE())/log(2.)/dim);
      for (int l = 0; l < ref_levels; l++)
      {
         mesh->UniformRefinement();
      }
   }

   // 6. Define a parallel mesh by a partitioning of the serial mesh. Refine
   //    this mesh further in parallel to increase the resolution. Once the
   //    parallel mesh is defined, the serial mesh can be deleted.
   ParMesh *pmesh = new ParMesh(MPI_COMM_WORLD, *mesh);
   delete mesh;
   {
      int par_ref_levels = 2;
      for (int l = 0; l < par_ref_levels; l++)
      {
         pmesh->UniformRefinement();
      }
   }

   // 7. Define a parallel finite element space hierarchy on the parallel mesh.
   //    Here we use continuous Lagrange finite elements. We start with order 1
   //    on the coarse level and increase the order by of factor of 2 for each
   //    additional level.
   FiniteElementCollection *fec = new H1_FECollection(1, dim);
   ParFiniteElementSpace *fespace = new ParFiniteElementSpace(pmesh, fec);

   Array<FiniteElementCollection*> collections;
   collections.Append(fec);
   ParSpaceHierarchy* spaceHierarchy = new ParSpaceHierarchy(pmesh, fespace, true,
                                                             true);
   for (int level = 0; level < geometricrefinements; ++level)
   {
      spaceHierarchy->AddUniformlyRefinedLevel();
   }
   for (int level = 0; level < orderrefinements; ++level)
   {
      collections.Append(new H1_FECollection(std::pow(2, level+1), dim));
      spaceHierarchy->AddOrderRefinedLevel(collections.Last());
   }

   HYPRE_Int size = spaceHierarchy->GetFinestFESpace().GlobalTrueVSize();
   if (myid == 0)
   {
      cout << "Number of finite element unknowns: " << size << endl;
   }

   // 8. Set up the parallel linear form b(.) which corresponds to the
   //    right-hand side of the FEM linear system, which in this case is
   //    (1,phi_i) where phi_i are the basis functions in fespace.
   ParLinearForm *b = new ParLinearForm(&spaceHierarchy->GetFinestFESpace());
   ConstantCoefficient one(1.0);
   b->AddDomainIntegrator(new DomainLFIntegrator(one));
   b->Assemble();

   // 9. Define the solution vector x as a parallel finite element grid function
   //    corresponding to fespace. Initialize x with initial guess of zero,
   //    which satisfies the boundary conditions.
   ParGridFunction x(&spaceHierarchy->GetFinestFESpace());
   x = 0.0;

   // 10. Create the multigrid operator using the previously created parallel
   //     SpaceHierarchy and additional boundary information. This operator
   //     is then used to create the MultigridSolver as a preconditioner in the
   //     iterative solver.
   Array<int> ess_bdr(pmesh->bdr_attributes.Max());
   if (pmesh->bdr_attributes.Size())
   {
      ess_bdr = 1;
   }
   MultigridDiffusionOperator* mgOperator = new MultigridDiffusionOperator(
      *spaceHierarchy, ess_bdr);
   MultigridSolver* prec = new MultigridSolver(mgOperator,
                                               MultigridSolver::CycleType::VCYCLE,
                                               1, 1);

   Vector X, B;
   mgOperator->EliminateBCs(x, *b, X, B);

   // 11. Solve the linear system A X = B.
   CGSolver cg(MPI_COMM_WORLD);
   cg.SetRelTol(1e-12);
   cg.SetMaxIter(2000);
   cg.SetPrintLevel(1);
   cg.SetOperator(*mgOperator);
   cg.SetPreconditioner(*prec);
   cg.Mult(B, X);

   // 12. Recover the parallel grid function corresponding to X. This is the
   //     local finite element solution on each processor.
   mgOperator->RecoverFEMSolution(X, *b, x);

   // 13. Save the refined mesh and the solution in parallel. This output can
   //     be viewed later using GLVis: "glvis -np <np> -m mesh -g sol".
   {
      ostringstream mesh_name, sol_name;
      mesh_name << "mesh." << setfill('0') << setw(6) << myid;
      sol_name << "sol." << setfill('0') << setw(6) << myid;

      ofstream mesh_ofs(mesh_name.str().c_str());
      mesh_ofs.precision(8);
      pmesh->Print(mesh_ofs);

      ofstream sol_ofs(sol_name.str().c_str());
      sol_ofs.precision(8);
      x.Save(sol_ofs);
   }

   // 14. Send the solution by socket to a GLVis server.
   if (visualization)
   {
      char vishost[] = "localhost";
      int  visport   = 19916;
      socketstream sol_sock(vishost, visport);
      sol_sock << "parallel " << num_procs << " " << myid << "\n";
      sol_sock.precision(8);
      sol_sock << "solution\n" << *pmesh << x << flush;
   }

   // 15. Free the used memory.
   delete prec;
   delete mgOperator;
   delete b;
   delete spaceHierarchy;
   for (int level = 0; level < collections.Size(); ++level)
   {
      delete collections[level];
   }

   MPI_Finalize();

   return 0;
}
<|endoftext|>


<commit_before>#include <stdio.h>

#include <QPainter>
#include <QLineEdit>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QLabel>
#include <QTimer>
#include <QCheckBox>

#include <geometry_msgs/Twist.h>

#include "drive_widget.h"
#include "gui_teleop.h"

namespace sesto_rviz_plugins
{

  GuiTeleop::GuiTeleop( QWidget* parent )
    : rviz::Panel( parent )
    , linear_velocity_( 0 )
    , angular_velocity_( 0 )
  {

    // max_linear_vel = DEFAULT_MAX_LINEAR_VEL;
    // max_angular_vel = DEFAULT_MAX_ANGULAR_VEL;

    QHBoxLayout* topic_layout = new QHBoxLayout;

    topic_layout->addWidget( new QLabel( "Output Topic:" ));
    output_topic_editor_ = new QLineEdit;
    topic_layout->addWidget( output_topic_editor_ );

    QHBoxLayout* max_vel_layout = new QHBoxLayout;

    max_vel_layout->addWidget(new QLabel("Max Lin"));
    max_linear_vel_editor_ = new QLineEdit;
    max_vel_layout->addWidget(max_linear_vel_editor_);

    max_vel_layout->addWidget(new QLabel("Max Ang"));
    max_angular_vel_editor_ = new QLineEdit;
    max_vel_layout->addWidget(max_angular_vel_editor_);

    QHBoxLayout* pub_checkbox_layout = new QHBoxLayout;

    pub_check_box_ = new QCheckBox("Publish", this);
    pub_checkbox_layout->addWidget(pub_check_box_);

    drive_widget_ = new DriveWidget;

    QVBoxLayout* layout = new QVBoxLayout;
    layout->addLayout( topic_layout );
    layout->addLayout( max_vel_layout);
    layout->addLayout( pub_checkbox_layout);
    layout->addWidget( drive_widget_ );
    setLayout( layout );

    QTimer* output_timer = new QTimer( this );

    connect( drive_widget_, SIGNAL( outputVelocity( float, float )), this, SLOT( setVel( float, float )));
    connect( output_topic_editor_, SIGNAL( editingFinished() ), this, SLOT( updateTopic() ));
    connect( output_timer, SIGNAL( timeout() ), this, SLOT( sendVel() ));
    connect( max_linear_vel_editor_, SIGNAL(editingFinished()), this, SLOT( setMaxLinearVel() ));
    connect( max_angular_vel_editor_, SIGNAL(editingFinished()), this, SLOT( setMaxAngularVel() ));

    output_timer->start( 100 );

    drive_widget_->setEnabled( false );
  }

  void GuiTeleop::setVel( float lin, float ang )
  {
    linear_velocity_ = lin;
    angular_velocity_ = ang;
  }

  void GuiTeleop::updateTopic()
  {
    setTopic( output_topic_editor_->text() );
  }

  void GuiTeleop::setMaxLinearVel()
  {
    max_linear_vel = max_linear_vel_editor_->text().toFloat();
  }

  void GuiTeleop::setMaxAngularVel()
  {
    max_angular_vel = max_angular_vel_editor_->text().toFloat();
  }

  void GuiTeleop::setTopic( const QString& new_topic )
  {
    if( new_topic != output_topic_ )
    {
      output_topic_ = new_topic;

      if( output_topic_ == "" )
      {
        velocity_publisher_.shutdown();
      }
      else
      {
        velocity_publisher_ = nh_.advertise<geometry_msgs::Twist>( output_topic_.toStdString(), 1 );
      }

      Q_EMIT configChanged();
    }

    drive_widget_->setEnabled( output_topic_ != "" );
  }

  void GuiTeleop::sendVel()
  {
    if( ros::ok() && velocity_publisher_ && pub_check_box_->isChecked())
    {
      suppressSpeed();

      geometry_msgs::Twist msg;
      msg.linear.x = linear_velocity_;
      msg.linear.y = 0;
      msg.linear.z = 0;
      msg.angular.x = 0;
      msg.angular.y = 0;
      msg.angular.z = angular_velocity_;
      velocity_publisher_.publish( msg );
    }
  }

  void GuiTeleop::suppressSpeed()
  {
    if (linear_velocity_ > max_linear_vel) {
      linear_velocity_ = max_linear_vel;
    }

    if (angular_velocity_ > max_angular_vel) {
      angular_velocity_ = max_angular_vel;
    }

  }

// TODO: save other states
  void GuiTeleop::save( rviz::Config config ) const
  {
    rviz::Panel::save( config );
    config.mapSetValue( "Topic", output_topic_ );
    config.mapSetValue("MaxLinVel", max_linear_vel);
    config.mapSetValue("MaxAngVel", max_angular_vel);
  }

  void GuiTeleop::load( const rviz::Config& config )
  {
    rviz::Panel::load( config );
    QString topic;
    if( config.mapGetString( "Topic", &topic ))
    {
      output_topic_editor_->setText( topic );
      updateTopic();
    }

    QString initMaxLinVel, initMaxAngVel;

    if (config.mapGetString("MaxLinVel", &initMaxLinVel)) {
      max_linear_vel = initMaxLinVel.toFloat();
    } else {
      max_linear_vel = DEFAULT_MAX_LINEAR_VEL;
    }

    max_linear_vel_editor_->setText(QString::fromStdString(std::to_string(max_linear_vel)));

    if (config.mapGetString("MaxAngVel", &initMaxAngVel)) {
      max_angular_vel = initMaxAngVel.toFloat();
    } else {
      max_angular_vel = DEFAULT_MAX_ANGULAR_VEL;
    }

    max_angular_vel_editor_->setText(QString::fromStdString(std::to_string(max_angular_vel)));

  }

}

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(sesto_rviz_plugins::GuiTeleop,rviz::Panel )
<commit_msg>Modified string conversion<commit_after>#include <stdio.h>
#include <sstream>

#include <QPainter>
#include <QLineEdit>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QLabel>
#include <QTimer>
#include <QCheckBox>

#include <geometry_msgs/Twist.h>

#include "drive_widget.h"
#include "gui_teleop.h"

namespace sesto_rviz_plugins
{

  GuiTeleop::GuiTeleop( QWidget* parent )
    : rviz::Panel( parent )
    , linear_velocity_( 0 )
    , angular_velocity_( 0 )
  {

    // max_linear_vel = DEFAULT_MAX_LINEAR_VEL;
    // max_angular_vel = DEFAULT_MAX_ANGULAR_VEL;

    QHBoxLayout* topic_layout = new QHBoxLayout;

    topic_layout->addWidget( new QLabel( "Output Topic:" ));
    output_topic_editor_ = new QLineEdit;
    topic_layout->addWidget( output_topic_editor_ );

    QHBoxLayout* max_vel_layout = new QHBoxLayout;

    max_vel_layout->addWidget(new QLabel("Max Lin"));
    max_linear_vel_editor_ = new QLineEdit;
    max_vel_layout->addWidget(max_linear_vel_editor_);

    max_vel_layout->addWidget(new QLabel("Max Ang"));
    max_angular_vel_editor_ = new QLineEdit;
    max_vel_layout->addWidget(max_angular_vel_editor_);

    QHBoxLayout* pub_checkbox_layout = new QHBoxLayout;

    pub_check_box_ = new QCheckBox("Publish", this);
    pub_checkbox_layout->addWidget(pub_check_box_);

    drive_widget_ = new DriveWidget;

    QVBoxLayout* layout = new QVBoxLayout;
    layout->addLayout( topic_layout );
    layout->addLayout( max_vel_layout);
    layout->addLayout( pub_checkbox_layout);
    layout->addWidget( drive_widget_ );
    setLayout( layout );

    QTimer* output_timer = new QTimer( this );

    connect( drive_widget_, SIGNAL( outputVelocity( float, float )), this, SLOT( setVel( float, float )));
    connect( output_topic_editor_, SIGNAL( editingFinished() ), this, SLOT( updateTopic() ));
    connect( output_timer, SIGNAL( timeout() ), this, SLOT( sendVel() ));
    connect( max_linear_vel_editor_, SIGNAL(editingFinished()), this, SLOT( setMaxLinearVel() ));
    connect( max_angular_vel_editor_, SIGNAL(editingFinished()), this, SLOT( setMaxAngularVel() ));

    output_timer->start( 100 );

    drive_widget_->setEnabled( false );
  }

  void GuiTeleop::setVel( float lin, float ang )
  {
    linear_velocity_ = lin;
    angular_velocity_ = ang;
  }

  void GuiTeleop::updateTopic()
  {
    setTopic( output_topic_editor_->text() );
  }

  void GuiTeleop::setMaxLinearVel()
  {
    max_linear_vel = max_linear_vel_editor_->text().toFloat();
  }

  void GuiTeleop::setMaxAngularVel()
  {
    max_angular_vel = max_angular_vel_editor_->text().toFloat();
  }

  void GuiTeleop::setTopic( const QString& new_topic )
  {
    if( new_topic != output_topic_ )
    {
      output_topic_ = new_topic;

      if( output_topic_ == "" )
      {
        velocity_publisher_.shutdown();
      }
      else
      {
        velocity_publisher_ = nh_.advertise<geometry_msgs::Twist>( output_topic_.toStdString(), 1 );
      }

      Q_EMIT configChanged();
    }

    drive_widget_->setEnabled( output_topic_ != "" );
  }

  void GuiTeleop::sendVel()
  {
    if( ros::ok() && velocity_publisher_ && pub_check_box_->isChecked())
    {
      suppressSpeed();

      geometry_msgs::Twist msg;
      msg.linear.x = linear_velocity_;
      msg.linear.y = 0;
      msg.linear.z = 0;
      msg.angular.x = 0;
      msg.angular.y = 0;
      msg.angular.z = angular_velocity_;
      velocity_publisher_.publish( msg );
    }
  }

  void GuiTeleop::suppressSpeed()
  {
    if (linear_velocity_ > max_linear_vel) {
      linear_velocity_ = max_linear_vel;
    }

    if (angular_velocity_ > max_angular_vel) {
      angular_velocity_ = max_angular_vel;
    }

  }

// TODO: save other states
  void GuiTeleop::save( rviz::Config config ) const
  {
    rviz::Panel::save( config );
    config.mapSetValue( "Topic", output_topic_ );
    config.mapSetValue("MaxLinVel", max_linear_vel);
    config.mapSetValue("MaxAngVel", max_angular_vel);
  }

  void GuiTeleop::load( const rviz::Config& config )
  {
    rviz::Panel::load( config );
    QString topic;
    if( config.mapGetString( "Topic", &topic ))
    {
      output_topic_editor_->setText( topic );
      updateTopic();
    }

    QString initMaxLinVel, initMaxAngVel;

    if (config.mapGetString("MaxLinVel", &initMaxLinVel)) {
      max_linear_vel = initMaxLinVel.toFloat();
    } else {
      max_linear_vel = DEFAULT_MAX_LINEAR_VEL;
    }

    std::ostringstream temp;
    temp << max_linear_vel;

    max_linear_vel_editor_->setText(QString::fromStdString(temp.str().c_str()));

    if (config.mapGetString("MaxAngVel", &initMaxAngVel)) {
      max_angular_vel = initMaxAngVel.toFloat();
    } else {
      max_angular_vel = DEFAULT_MAX_ANGULAR_VEL;
    }

    temp << max_linear_vel;

    max_angular_vel_editor_->setText(QString::fromStdString(temp.str().c_str()));

  }

}

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(sesto_rviz_plugins::GuiTeleop,rviz::Panel )
<|endoftext|>


<commit_before>//
// Copyright (c) 2016, Andre Gaschler
// 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.
//
// 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 HOLDER 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.
//

#include <iostream>
#include <rl/math/Array.h>
#include <rl/math/Spline.h>

void printPoly(const rl::math::Polynomial<rl::math::ArrayX>& p)
{
	for (int n = p.degree(); n >= 0; --n)
	{
		if (p.coefficient(n).matrix().squaredNorm() != 0)
		{
			std::cout << p.coefficient(n) << "*x^" << n << " ";
		}
	}

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

bool testPolynomialTranslation(const rl::math::Polynomial<rl::math::ArrayX>& p)
{
	rl::math::Real eps = 1e-6;
	rl::math::Real translation = 0.2 + (p.degree() / 3);
	rl::math::Polynomial<rl::math::ArrayX> q = p.translatedX(translation);
	printPoly(p);
	printPoly(q);
	bool ok = true;

	for (rl::math::Real x = 0; x < 1; x += 0.1)
	{
		if ((p(x) - q(x - translation)).matrix().squaredNorm() > eps)
		{
			ok = false;
		}
	}

	if (!ok)
	{
		std::cerr << "testPolynomialTranslation: at degree " << p.degree() << " a difference was detected." << std::endl;
	}

	return ok;
}

int
main(int argc, char** argv)
{
	bool ok = true;

	for (std::size_t degree = 5; degree < 15; ++degree)
	{
		rl::math::Polynomial<rl::math::ArrayX> p(degree);

		for (std::size_t n = 0; n < p.degree() + 1; ++n)
		{
			p.coefficient(n) = rl::math::ArrayX::Zero(1);
		}

		p.coefficient(degree) = rl::math::ArrayX::Constant(1, 1.7);
		p.coefficient(degree / 2) = rl::math::ArrayX::Constant(1, -2.5);
		p.coefficient(degree / 3) = rl::math::ArrayX::Constant(1, 1.5);
		ok &= testPolynomialTranslation(p);

		p.coefficient(degree - 1) = rl::math::ArrayX::Constant(1, -0.5);
		ok &= testPolynomialTranslation(p);
	}

	if (ok)
	{
		std::cout << "rlSplineTranslationTest: Verify Polynomial::translatedX done." << std::endl;
		return EXIT_SUCCESS;
	}
	else
	{
		std::cerr << "rlSplineTranslationTest: Polynomial::translatedX is wrong." << std::endl;
		return EXIT_FAILURE;
	}
}
<commit_msg>Fix rlSplineTranslationTest<commit_after>//
// Copyright (c) 2016, Andre Gaschler
// 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.
//
// 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 HOLDER 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.
//

#include <iostream>
#include <rl/math/Array.h>
#include <rl/math/Spline.h>

void printPoly(const rl::math::Polynomial<rl::math::ArrayX>& p)
{
	for (int n = p.degree(); n >= 0; --n)
	{
		if (p.coefficient(n).matrix().squaredNorm() != 0)
		{
			std::cout << p.coefficient(n) << "*x^" << n << " ";
		}
	}

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

bool testPolynomialTranslation(const rl::math::Polynomial<rl::math::ArrayX>& p)
{
	rl::math::Real eps = 1e-6;
	rl::math::Real translation = 0.2 + (p.degree() / 3);
	rl::math::Polynomial<rl::math::ArrayX> q = p.translatedX(translation);
	printPoly(p);
	printPoly(q);
	bool ok = true;

	for (rl::math::Real x = 0; x < 1; x += 0.1)
	{
		if ((p(x) - q(x - translation)).matrix().squaredNorm() > eps)
		{
			ok = false;
		}
	}

	if (!ok)
	{
		std::cerr << "testPolynomialTranslation: at degree " << p.degree() << " a difference was detected." << std::endl;
	}

	return ok;
}

int
main(int argc, char** argv)
{
	bool ok = true;

	for (std::size_t degree = 5; degree < 15; ++degree)
	{
		rl::math::Polynomial<rl::math::ArrayX> p(degree);
		p.upper() = 1;

		for (std::size_t n = 0; n < p.degree() + 1; ++n)
		{
			p.coefficient(n) = rl::math::ArrayX::Zero(1);
		}

		p.coefficient(degree) = rl::math::ArrayX::Constant(1, 1.7);
		p.coefficient(degree / 2) = rl::math::ArrayX::Constant(1, -2.5);
		p.coefficient(degree / 3) = rl::math::ArrayX::Constant(1, 1.5);
		ok &= testPolynomialTranslation(p);

		p.coefficient(degree - 1) = rl::math::ArrayX::Constant(1, -0.5);
		ok &= testPolynomialTranslation(p);
	}

	if (ok)
	{
		std::cout << "rlSplineTranslationTest: Verify Polynomial::translatedX done." << std::endl;
		return EXIT_SUCCESS;
	}
	else
	{
		std::cerr << "rlSplineTranslationTest: Polynomial::translatedX is wrong." << std::endl;
		return EXIT_FAILURE;
	}
}
<|endoftext|>


<commit_before>#include <reactive/bridge.hpp>
#include <reactive/ptr_observable.hpp>
#include <reactive/buffer.hpp>
#include <reactive/generate.hpp>
#include <reactive/transform.hpp>
#include <SDL2/SDL.h>
#include <boost/system/system_error.hpp>
#include <boost/scope_exit.hpp>
#include <boost/variant.hpp>

namespace rx
{
	template <class In, class State, class Step>
	struct variable
			: public observable<State>
			, private observer<typename In::element_type>
	{
		typedef State element_type;

		variable(In in, element_type initial_state, Step step)
			: in(std::move(in))
			, state(std::move(initial_state))
			, step(std::move(step))
		{
		}

		virtual void async_get_one(observer<element_type> &receiver) SILICIUM_OVERRIDE
		{
			check_fetch();
			receiver.got_element(state);
		}

		virtual void cancel() SILICIUM_OVERRIDE
		{
		}

	private:

		In in;
		element_type state;
		Step step;
		bool fetching = false;

		virtual void got_element(typename In::element_type value) SILICIUM_OVERRIDE
		{
			assert(fetching);
			fetching = false;
			check_fetch();
			state = step(std::move(state), std::move(value));
		}

		virtual void ended() SILICIUM_OVERRIDE
		{
			assert(fetching);
			fetching = false;
			check_fetch();
		}

		void check_fetch()
		{
			if (!fetching)
			{
				fetching = true;
				in.async_get_one(*this);
			}
		}
	};

	template <class In, class State, class Step>
	auto make_var(In &&in, State &&initial_state, Step &&step)
	{
		return variable<
				typename std::decay<In>::type,
				typename std::decay<State>::type,
				typename std::decay<Step>::type>(std::forward<In>(in), std::forward<State>(initial_state), std::forward<Step>(step));
	}

	template <class Element>
	auto ref(rx::observable<Element> &identity)
	{
		return rx::ptr_observable<Element, rx::observable<Element> *>(&identity);
	}
}

namespace
{
	void throw_error()
	{
		throw std::runtime_error(std::string("SDL error :") + SDL_GetError());
	}

	void check_sdl(int rc)
	{
		if (rc < 0)
		{
			throw_error();
		}
	}

	struct window_destructor
	{
		void operator()(SDL_Window *w) const
		{
			SDL_DestroyWindow(w);
		}
	};

	struct renderer_destructor
	{
		void operator()(SDL_Renderer *r) const
		{
			SDL_DestroyRenderer(r);
		}
	};

	struct draw_filled_rect
	{
		SDL_Color color;
		SDL_Rect where;
	};

	typedef boost::variant<
		draw_filled_rect
	> draw_operation;

	struct frame
	{
		std::vector<draw_operation> operations;
	};

	SDL_Color make_color(uint8_t red, uint8_t green, uint8_t blue, uint8_t alpha)
	{
		SDL_Color result;
		result.r = red;
		result.g = green;
		result.b = blue;
		result.a = alpha;
		return result;
	}

	SDL_Rect make_rect(int x, int y, int w, int h)
	{
		SDL_Rect result;
		result.x = x;
		result.y = y;
		result.w = w;
		result.h = h;
		return result;
	}

	struct game_state
	{
		int x = 100;
	};

	frame draw_game_state(game_state const &state)
	{
		return frame
		{
			{
				draw_filled_rect{make_color(0xff, 0x00, 0x00, 0xff), make_rect(state.x, 100, 200, 125)}
			}
		};
	}

	game_state step_game_state(game_state previous, SDL_Event event_)
	{
		switch (event_.type)
		{
		case SDL_KEYUP:
			{
				switch (event_.key.keysym.sym)
				{
				case SDLK_LEFT:
					previous.x -= 10;
					break;

				case SDLK_RIGHT:
					previous.x += 10;
					break;
				}
				break;
			}
		}
		return previous;
	}

	auto make_frames(rx::observable<SDL_Event> &input)
	{
		game_state initial_state;
		return rx::transform(rx::make_var(rx::ref(input), initial_state, step_game_state), draw_game_state);
	}

	void set_render_draw_color(SDL_Renderer &renderer, SDL_Color color)
	{
		SDL_SetRenderDrawColor(&renderer, color.r, color.g, color.b, color.a);
	}

	struct draw_operation_renderer : boost::static_visitor<>
	{
		explicit draw_operation_renderer(SDL_Renderer &sdl)
			: sdl(&sdl)
		{
		}

		void operator()(draw_filled_rect const &operation) const
		{
			set_render_draw_color(*sdl, operation.color);
			SDL_RenderDrawRect(sdl, &operation.where);
		}

	private:

		SDL_Renderer *sdl;
	};

	struct frame_renderer : rx::observer<frame>
	{
		explicit frame_renderer(SDL_Renderer &sdl)
			: sdl(&sdl)
		{
		}

		virtual void got_element(frame value) SILICIUM_OVERRIDE
		{
			set_render_draw_color(*sdl, make_color(0, 0, 0, 0xff));
			SDL_RenderClear(sdl);
			for (auto const &operation : value.operations)
			{
				boost::apply_visitor(draw_operation_renderer{*sdl}, operation);
			}
			SDL_RenderPresent(sdl);
		}

		virtual void ended() SILICIUM_OVERRIDE
		{
		}

	private:

		SDL_Renderer *sdl;
	};
}

int main()
{
	check_sdl(SDL_Init(SDL_INIT_VIDEO | SDL_INIT_EVENTS));
	BOOST_SCOPE_EXIT(void)
	{
		SDL_Quit();
	}
	BOOST_SCOPE_EXIT_END;

	std::unique_ptr<SDL_Window, window_destructor> window(SDL_CreateWindow("Silicium react.cpp", SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED, 800, 600, 0));
	if (!window)
	{
		throw_error();
		return 1;
	}

	std::unique_ptr<SDL_Renderer, renderer_destructor> renderer(SDL_CreateRenderer(window.get(), -1, 0));

	rx::bridge<SDL_Event> input;
	auto frames = rx::wrap<frame>(make_frames(input));

	frame_renderer frame_renderer_(*renderer);
	for (;;)
	{
		SDL_Event event;
		while (SDL_PollEvent(&event))
		{
			switch (event.type)
			{
			case SDL_QUIT:
				return 0;
			}
			if (input.is_waiting())
			{
				input.got_element(event);
			}
		}
		frames.async_get_one(frame_renderer_);
	}
}
<commit_msg>more rects can be added with the plus key<commit_after>#include <reactive/bridge.hpp>
#include <reactive/ptr_observable.hpp>
#include <reactive/buffer.hpp>
#include <reactive/generate.hpp>
#include <reactive/transform.hpp>
#include <SDL2/SDL.h>
#include <boost/system/system_error.hpp>
#include <boost/scope_exit.hpp>
#include <boost/variant.hpp>

namespace rx
{
	template <class In, class State, class Step>
	struct variable
			: public observable<State>
			, private observer<typename In::element_type>
	{
		typedef State element_type;

		variable(In in, element_type initial_state, Step step)
			: in(std::move(in))
			, state(std::move(initial_state))
			, step(std::move(step))
		{
		}

		virtual void async_get_one(observer<element_type> &receiver) SILICIUM_OVERRIDE
		{
			check_fetch();
			receiver.got_element(state);
		}

		virtual void cancel() SILICIUM_OVERRIDE
		{
		}

	private:

		In in;
		element_type state;
		Step step;
		bool fetching = false;

		virtual void got_element(typename In::element_type value) SILICIUM_OVERRIDE
		{
			assert(fetching);
			fetching = false;
			check_fetch();
			state = step(std::move(state), std::move(value));
		}

		virtual void ended() SILICIUM_OVERRIDE
		{
			assert(fetching);
			fetching = false;
			check_fetch();
		}

		void check_fetch()
		{
			if (!fetching)
			{
				fetching = true;
				in.async_get_one(*this);
			}
		}
	};

	template <class In, class State, class Step>
	auto make_var(In &&in, State &&initial_state, Step &&step)
	{
		return variable<
				typename std::decay<In>::type,
				typename std::decay<State>::type,
				typename std::decay<Step>::type>(std::forward<In>(in), std::forward<State>(initial_state), std::forward<Step>(step));
	}

	template <class Element>
	auto ref(rx::observable<Element> &identity)
	{
		return rx::ptr_observable<Element, rx::observable<Element> *>(&identity);
	}
}

namespace
{
	void throw_error()
	{
		throw std::runtime_error(std::string("SDL error :") + SDL_GetError());
	}

	void check_sdl(int rc)
	{
		if (rc < 0)
		{
			throw_error();
		}
	}

	struct window_destructor
	{
		void operator()(SDL_Window *w) const
		{
			SDL_DestroyWindow(w);
		}
	};

	struct renderer_destructor
	{
		void operator()(SDL_Renderer *r) const
		{
			SDL_DestroyRenderer(r);
		}
	};

	struct draw_filled_rect
	{
		SDL_Color color;
		SDL_Rect where;
	};

	typedef boost::variant<
		draw_filled_rect
	> draw_operation;

	struct frame
	{
		std::vector<draw_operation> operations;
	};

	SDL_Color make_color(uint8_t red, uint8_t green, uint8_t blue, uint8_t alpha)
	{
		SDL_Color result;
		result.r = red;
		result.g = green;
		result.b = blue;
		result.a = alpha;
		return result;
	}

	SDL_Rect make_rect(int x, int y, int w, int h)
	{
		SDL_Rect result;
		result.x = x;
		result.y = y;
		result.w = w;
		result.h = h;
		return result;
	}

	struct game_state
	{
		int x = 100;
		int y = 100;
		int count = 1;
	};

	int const max_rect_count = 5;

	frame draw_game_state(game_state const &state)
	{
		std::vector<draw_operation> operations;
		for (int i = 0; i < state.count; ++i)
		{
			int const max_width = 80;
			int const single_offset = (max_width / 2) / max_rect_count;
			int const total_offset = single_offset * i;
			int const width = max_width - total_offset * 2;
			operations.emplace_back(draw_filled_rect{make_color(0xff, 0x00, 0x00, 0xff), make_rect(state.x + total_offset, state.y + total_offset, width, width)});
		}
		return frame
		{
			std::move(operations)
		};
	}

	game_state step_game_state(game_state previous, SDL_Event event_)
	{
		switch (event_.type)
		{
		case SDL_KEYUP:
			{
				switch (event_.key.keysym.sym)
				{
				case SDLK_LEFT:
					previous.x -= 10;
					break;

				case SDLK_RIGHT:
					previous.x += 10;
					break;

				case SDLK_UP:
					previous.y -= 10;
					break;

				case SDLK_DOWN:
					previous.y += 10;
					break;

				case SDLK_PLUS:
					previous.count = std::min(max_rect_count, previous.count + 1);
					break;

				case SDLK_MINUS:
					previous.count = std::max(1, previous.count - 1);
					break;
				}
				break;
			}
		}
		return previous;
	}

	auto make_frames(rx::observable<SDL_Event> &input)
	{
		game_state initial_state;
		return rx::transform(rx::make_var(rx::ref(input), initial_state, step_game_state), draw_game_state);
	}

	void set_render_draw_color(SDL_Renderer &renderer, SDL_Color color)
	{
		SDL_SetRenderDrawColor(&renderer, color.r, color.g, color.b, color.a);
	}

	struct draw_operation_renderer : boost::static_visitor<>
	{
		explicit draw_operation_renderer(SDL_Renderer &sdl)
			: sdl(&sdl)
		{
		}

		void operator()(draw_filled_rect const &operation) const
		{
			set_render_draw_color(*sdl, operation.color);
			SDL_RenderDrawRect(sdl, &operation.where);
		}

	private:

		SDL_Renderer *sdl;
	};

	struct frame_renderer : rx::observer<frame>
	{
		explicit frame_renderer(SDL_Renderer &sdl)
			: sdl(&sdl)
		{
		}

		virtual void got_element(frame value) SILICIUM_OVERRIDE
		{
			set_render_draw_color(*sdl, make_color(0, 0, 0, 0xff));
			SDL_RenderClear(sdl);
			for (auto const &operation : value.operations)
			{
				boost::apply_visitor(draw_operation_renderer{*sdl}, operation);
			}
			SDL_RenderPresent(sdl);
		}

		virtual void ended() SILICIUM_OVERRIDE
		{
		}

	private:

		SDL_Renderer *sdl;
	};
}

int main()
{
	check_sdl(SDL_Init(SDL_INIT_VIDEO | SDL_INIT_EVENTS));
	BOOST_SCOPE_EXIT(void)
	{
		SDL_Quit();
	}
	BOOST_SCOPE_EXIT_END;

	std::unique_ptr<SDL_Window, window_destructor> window(SDL_CreateWindow("Silicium react.cpp", SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED, 800, 600, 0));
	if (!window)
	{
		throw_error();
		return 1;
	}

	std::unique_ptr<SDL_Renderer, renderer_destructor> renderer(SDL_CreateRenderer(window.get(), -1, 0));

	rx::bridge<SDL_Event> input;
	auto frames = rx::wrap<frame>(make_frames(input));

	frame_renderer frame_renderer_(*renderer);
	for (;;)
	{
		SDL_Event event;
		while (SDL_PollEvent(&event))
		{
			switch (event.type)
			{
			case SDL_QUIT:
				return 0;
			}
			if (input.is_waiting())
			{
				input.got_element(event);
			}
		}
		frames.async_get_one(frame_renderer_);
	}
}
<|endoftext|>


<commit_before>/**\file
 * \brief "Hello World" HTTP Server
 *
 * \copyright
 * Copyright (c) 2015, ef.gy Project Members
 * \copyright
 * 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:
 * \copyright
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 * \copyright
 * 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.
 *
 * \see Project Documentation: http://ef.gy/documentation/libefgy
 * \see Project Source Code: https://github.com/ef-gy/libefgy
 */

#include <ef.gy/http.h>
#include <iostream>

using namespace efgy;
using namespace asio;
using namespace std;

using asio::ip::tcp;

class processHelloWorld {
public:
  bool operator()(net::http::session<tcp, processHelloWorld> &a) {
    a.reply(200, "Hello World!");

    return true;
  }
};

int main(int argc, char *argv[]) {
  try {
    if (argc != 3) {
      std::cerr << "Usage: http-hello <host> <port>\n";
      return 1;
    }

    asio::io_service io_service;
    tcp::resolver resolver(io_service);
    tcp::resolver::query query(argv[1], argv[2]);
    tcp::resolver::iterator endpoint_iterator = resolver.resolve(query);
    tcp::resolver::iterator end;

    if (endpoint_iterator != end) {
      tcp::endpoint endpoint = *endpoint_iterator;
      net::http::server<tcp, processHelloWorld> s(io_service, endpoint);
      io_service.run();
    }
  } catch (std::exception & e) {
    std::cerr << "Exception: " << e.what() << "\n";
  } catch (std::system_error & e) {
    std::cerr << "System Error: " << e.what() << "\n";
  }

  return 0;
}
<commit_msg>hmm, threading we probably don't need...<commit_after>/**\file
 * \brief "Hello World" HTTP Server
 *
 * \copyright
 * Copyright (c) 2015, ef.gy Project Members
 * \copyright
 * 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:
 * \copyright
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 * \copyright
 * 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.
 *
 * \see Project Documentation: http://ef.gy/documentation/libefgy
 * \see Project Source Code: https://github.com/ef-gy/libefgy
 */

#define ASIO_DISABLE_THREADS
#include <ef.gy/http.h>
#include <iostream>

using namespace efgy;
using namespace asio;
using namespace std;

using asio::ip::tcp;

class processHelloWorld {
public:
  bool operator()(net::http::session<tcp, processHelloWorld> &a) {
    a.reply(200, "Hello World!");

    return true;
  }
};

int main(int argc, char *argv[]) {
  try {
    if (argc != 3) {
      std::cerr << "Usage: http-hello <host> <port>\n";
      return 1;
    }

    asio::io_service io_service;
    tcp::resolver resolver(io_service);
    tcp::resolver::query query(argv[1], argv[2]);
    tcp::resolver::iterator endpoint_iterator = resolver.resolve(query);
    tcp::resolver::iterator end;

    if (endpoint_iterator != end) {
      tcp::endpoint endpoint = *endpoint_iterator;
      net::http::server<tcp, processHelloWorld> s(io_service, endpoint);
      io_service.run();
    }
  } catch (std::exception & e) {
    std::cerr << "Exception: " << e.what() << "\n";
  } catch (std::system_error & e) {
    std::cerr << "System Error: " << e.what() << "\n";
  }

  return 0;
}
<|endoftext|>


<commit_before>#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>

int n; /* size of the board */
int *board; /* 0 MEANS EMPTY */

/*
    Coordinates of the board

      1 2 3 4
    1 +------- y (cell)
    2 |
    3 |
    4 |
      x (line)

    x starts with 1; 0 is not used
    y starts with 1; 0 MEANS EMPTY

    board[0] is not used.
    board[1] = 3; means queen is at line 1, cell 3.
    board[1] = 0; means queen is not exist at line 1.
*/

bool is_setable_se(const int x, const int y)
{
    /* base case */
    if(x < 1 || x > n || y < 1 || y > n) return true;
    if(board[x - 1] == y) return false; /* queen is exist */

    /* recursive case */
    return is_setable_se(x + 1, y + 1);
}

bool is_setable_s(const int x, const int y)
{
    /* base case */
    if(x < 1 || x > n || y < 1 || y > n) return true;
    if(board[x - 1] == y) return false; /* queen is exist */

    /* recursive case */
    return is_setable_s(x + 1, y);
}

bool is_setable_sw(const int x, const int y)
{
    /* base case */
    if(x < 1 || x > n || y < 1 || y > n) return true;
    if(board[x - 1] == y) return false; /* queen is exist */

    /* recursive case */
    return is_setable_sw(x + 1, y - 1);
}

bool is_setable(const int x, const int y)
{
    if(is_setable_se(x, y) && is_setable_s(x, y) && is_setable_sw(x, y)) return true;
    return false;
}

int b_set(const int x, const int y)
{
    /* set y to x */
    board[x - 1] = y;
    return 0;
}

int b_unset(const int x)
{
    /* unset x */
    board[x - 1] = 0; /* 0 MEANS EMPTY */
    return 0;
}

int count_n_queens(const int x, const int y)
{
    /* base case */
    if(y < 1) return 0; /* y reached left side */

    /* recursive case */
    if(is_setable(x, y))
    {
        if(x - 1 >= 1)              return count_n_queens(x, y - 1) + b_set(x, y) + count_n_queens(x - 1, ::n) + b_unset(x);
        if(is_setable(x - 1, ::n))  return count_n_queens(x, y - 1) + 1; /* x reached top side; Queen can be here! */
    }
    return count_n_queens(x, y - 1);
}

int main(void)
{
    for(n = 1; n <= 15; n++)
    {
        board = (int *)malloc(sizeof(int) * n);
        memset(board, 0, sizeof(int) * n); /* 0 MEANS EMPTY */

        printf("N(%d) Result=%d\n", n, count_n_queens(n, n));
        free(board);
    }

    return 0;
}
<commit_msg>Add comments<commit_after>#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>

int n; /* Size of the board */
int *board;

/*
    Coordinates of the board

      1 2 3 4
    1 +------- y (cell)
    2 |
    3 |
    4 |
      x (line)

    x starts with 1; 0 is not used
    y starts with 1; 0 MEANS EMPTY

    board[0] is not used.
    board[1] = 3; means queen is at line 1, cell 3.
    board[1] = 0; means queen is not exist at line 1.
*/

bool is_setable_se(const int x, const int y)
{
    /* base case */
    if(x < 1 || x > n || y < 1 || y > n) return true;
    if(board[x - 1] == y) return false; /* Queen cannot be here */

    /* recursive case */
    return is_setable_se(x + 1, y + 1);
}

bool is_setable_s(const int x, const int y)
{
    if(x < 1 || x > n || y < 1 || y > n) return true;
    if(board[x - 1] == y) return false;
    return is_setable_s(x + 1, y);
}

bool is_setable_sw(const int x, const int y)
{
    if(x < 1 || x > n || y < 1 || y > n) return true;
    if(board[x - 1] == y) return false;
    return is_setable_sw(x + 1, y - 1);
}

bool is_setable(const int x, const int y)
{
    if(is_setable_se(x, y) && is_setable_s(x, y) && is_setable_sw(x, y)) return true;
    return false;
}

int b_set(const int x, const int y)
{
    /* set y to x */
    board[x - 1] = y;
    return 0; /* Do not effect the count calculation */
}

int b_unset(const int x)
{
    /* unset x */
    board[x - 1] = 0; /* 0 MEANS EMPTY */
    return 0;
}

int count_n_queens(const int x, const int y)
{
    /* base case */
    if(y < 1) return 0; /* y reached left side */

    /* recursive case */
    if(is_setable(x, y))
    {
        if(x - 1 < 1) /* x reached top side */
            return 1 + count_n_queens(x, y - 1); /* Queen can be here! */

        return b_set(x, y) + count_n_queens(x - 1, ::n) + b_unset(x) + count_n_queens(x, y - 1);
    }
    return count_n_queens(x, y - 1); /* Queen cannot be here */
}

int main(void)
{
    for(n = 1; n <= 15; n++)
    {
        board = (int *)malloc(sizeof(int) * n);
        memset(board, 0, sizeof(int) * n);

        printf("N(%d) Result=%d\n", n, count_n_queens(n, n));
        free(board);
    }

    return 0;
}
<|endoftext|>


<commit_before>#include <proton/default_container.hpp>
#include <proton/messaging_handler.hpp>

#include <string>

class ExampleHandler: public proton::messaging_handler {
    std::string url_;

    // The container has started
    void on_container_start(proton::container&) override {
    }

    // A message can be sent
    void on_sendable(proton::sender&) override {
    }

    // A message is received
    void on_message(proton::delivery&, proton::message&) override {
    }

  public:
    ExampleHandler(const std::string& url) : url_(url) {}
};

int main() {
    ExampleHandler h("localhost");
    proton::default_container(h).run();
}

<commit_msg>Make change parallel to guide<commit_after>#include <proton/default_container.hpp>
#include <proton/messaging_handler.hpp>

#include <string>

class ExampleHandler: public proton::messaging_handler {
    std::string url_;

    // The container has started
    void on_container_start(proton::container& c) override {
    }

    // A message can be sent
    void on_sendable(proton::sender& s) override {
    }

    // A message is received
    void on_message(proton::delivery& d, proton::message& m) override {
    }

  public:
    ExampleHandler(const std::string& url) : url_(url) {}
};

int main() {
    ExampleHandler h("localhost");
    proton::default_container(h).run();
}

<|endoftext|>


<commit_before>// This file is part of meshoptimizer library; see meshoptimizer.h for version/license details
#include "meshoptimizer.h"

#include <cassert>
#include <cstring>

// This work is based on:
// Conor Stokes. Vertex Cache Optimised Index Buffer Compression. 2014
namespace meshopt
{

typedef unsigned int VertexFifo[16];
typedef unsigned int EdgeFifo[16][2];

static const unsigned int kTriangleIndexOrder[3][3] =
    {
        // clang-format array formatting is broken :/
        {0, 1, 2},
        {1, 2, 0},
        {2, 0, 1},
};

static int rotateTriangle(unsigned int a, unsigned int b, unsigned int c, unsigned int next)
{
	(void)a;

	return (b == next) ? 1 : (c == next) ? 2 : 0;
}

static int getEdgeFifo(EdgeFifo fifo, unsigned int a, unsigned int b, unsigned int c, size_t offset)
{
	for (int i = 0; i < 16; ++i)
	{
		unsigned int index = (offset - 1 - i) & 15;
		unsigned int e0 = fifo[index][0];
		unsigned int e1 = fifo[index][1];

		if (e0 == a && e1 == b)
			return (i << 2) | 0;
		if (e0 == b && e1 == c)
			return (i << 2) | 1;
		if (e0 == c && e1 == a)
			return (i << 2) | 2;
	}

	return -1;
}

static void pushEdgeFifo(EdgeFifo fifo, unsigned int a, unsigned int b, size_t& offset)
{
	fifo[offset][0] = a;
	fifo[offset][1] = b;
	offset = (offset + 1) & 15;
}

static int getVertexFifo(VertexFifo fifo, unsigned int v, size_t offset)
{
	for (int i = 0; i < 16; ++i)
	{
		unsigned int index = (offset - 1 - i) & 15;

		if (fifo[index] == v)
			return i;
	}

	return -1;
}

static void pushVertexFifo(VertexFifo fifo, unsigned int v, size_t& offset)
{
	fifo[offset] = v;
	offset = (offset + 1) & 15;
}

static void encodeVarInt(unsigned char*& data, unsigned int v)
{
	do
	{
		*data++ = (v & 127) | (v > 127 ? 128 : 0);
		v >>= 7;
	} while (v);
}

static unsigned int decodeVarInt(const unsigned char*& data)
{
	unsigned int result = 0;
	unsigned int shift = 0;
	unsigned char group;

	do
	{
		group = *data++;
		result |= (group & 127) << shift;
		shift += 7;
	} while (group & 128);

	return result;
}
}

size_t meshopt_encodeIndexBuffer(unsigned char* buffer, size_t buffer_size, const unsigned int* indices, size_t index_count)
{
	using namespace meshopt;

	assert(index_count % 3 == 0);

	EdgeFifo edgefifo;
	memset(edgefifo, -1, sizeof(edgefifo));

	VertexFifo vertexfifo;
	memset(vertexfifo, -1, sizeof(vertexfifo));

	size_t edgefifooffset = 0;
	size_t vertexfifooffset = 0;

	unsigned int next = 0;

	unsigned char* data = buffer;

	for (size_t i = 0; i < index_count; i += 3)
	{
		int fer = getEdgeFifo(edgefifo, indices[i + 0], indices[i + 1], indices[i + 2], edgefifooffset);

		if (fer >= 0 && (fer >> 2) < 15)
		{
			const unsigned int* order = kTriangleIndexOrder[fer & 3];

			unsigned int a = indices[i + order[0]], b = indices[i + order[1]], c = indices[i + order[2]];

			int fe = fer >> 2;
			int fc = getVertexFifo(vertexfifo, c, vertexfifooffset);

			int fec = (fc >= 0 && fc < 14) ? (fc + 1) : (c == next) ? (next++, 0) : 15;

			*data++ = static_cast<unsigned char>(fe | (fec << 4));

			if (fec == 15)
				encodeVarInt(data, next - c);

			if (fec == 0 || fec == 15)
				pushVertexFifo(vertexfifo, c, vertexfifooffset);

			pushEdgeFifo(edgefifo, c, b, edgefifooffset);
			pushEdgeFifo(edgefifo, a, c, edgefifooffset);
		}
		else
		{
			const unsigned int* order = kTriangleIndexOrder[rotateTriangle(indices[i + 0], indices[i + 1], indices[i + 2], next)];

			unsigned int a = indices[i + order[0]], b = indices[i + order[1]], c = indices[i + order[2]];

			int fa = getVertexFifo(vertexfifo, a, vertexfifooffset);
			int fb = getVertexFifo(vertexfifo, b, vertexfifooffset);
			int fc = getVertexFifo(vertexfifo, c, vertexfifooffset);

			int fea = (fa >= 0 && fa < 14) ? (fa + 1) : (a == next) ? (next++, 0) : 15;
			int feb = (fb >= 0 && fb < 14) ? (fb + 1) : (b == next) ? (next++, 0) : 15;
			int fec = (fc >= 0 && fc < 14) ? (fc + 1) : (c == next) ? (next++, 0) : 15;

			*data++ = static_cast<unsigned char>((fea << 4) | 15);
			*data++ = static_cast<unsigned char>((feb << 4) | fec);

			if (fea == 15)
				encodeVarInt(data, next - a);

			if (feb == 15)
				encodeVarInt(data, next - b);

			if (fec == 15)
				encodeVarInt(data, next - c);

			if (fea == 0 || fea == 15)
				pushVertexFifo(vertexfifo, a, vertexfifooffset);

			if (feb == 0 || feb == 15)
				pushVertexFifo(vertexfifo, b, vertexfifooffset);

			if (fec == 0 || fec == 15)
				pushVertexFifo(vertexfifo, c, vertexfifooffset);

			pushEdgeFifo(edgefifo, b, a, edgefifooffset);
			pushEdgeFifo(edgefifo, c, b, edgefifooffset);
			pushEdgeFifo(edgefifo, a, c, edgefifooffset);
		}
	}

	assert(data <= buffer + buffer_size);
	(void)buffer_size;

	return data - buffer;
}

size_t meshopt_encodeIndexBufferBound(size_t index_count, size_t vertex_count)
{
	assert(index_count % 3 == 0);

	// compute number of bits required for each index
	unsigned int vertex_bits = 1;

	while (vertex_bits < 32 && vertex_count > size_t(1) << vertex_bits)
		vertex_bits++;

	// worst-case encoding is 2 header bytes + 3 varint-7 encoded indices
	unsigned int vertex_groups = (vertex_bits + 6) / 7;

	return (index_count / 3) * (2 + 3 * vertex_groups);
}

void meshopt_decodeIndexBuffer(unsigned int* destination, size_t index_count, const unsigned char* buffer, size_t buffer_size)
{
	using namespace meshopt;

	assert(index_count % 3 == 0);

	EdgeFifo edgefifo;
	memset(edgefifo, -1, sizeof(edgefifo));

	VertexFifo vertexfifo;
	memset(vertexfifo, -1, sizeof(vertexfifo));

	size_t edgefifooffset = 0;
	size_t vertexfifooffset = 0;

	unsigned int next = 0;

	const unsigned char* data = buffer;

	for (size_t i = 0; i < index_count; i += 3)
	{
		unsigned char code = *data++;

		int fe = code & 15;

		if (fe < 15)
		{
			unsigned int a = edgefifo[(edgefifooffset - 1 - fe) & 15][0];
			unsigned int b = edgefifo[(edgefifooffset - 1 - fe) & 15][1];

			int fec = code >> 4;

			unsigned int c = (fec == 0) ? next++ : (fec < 15) ? vertexfifo[(vertexfifooffset - fec) & 15] : next - decodeVarInt(data);

			destination[i + 0] = a;
			destination[i + 1] = b;
			destination[i + 2] = c;

			if (fec == 0 || fec == 15)
				pushVertexFifo(vertexfifo, c, vertexfifooffset);

			pushEdgeFifo(edgefifo, c, b, edgefifooffset);
			pushEdgeFifo(edgefifo, a, c, edgefifooffset);
		}
		else
		{
			int fea = code >> 4;

			unsigned char codeaux = *data++;

			int feb = codeaux >> 4;
			int fec = codeaux & 15;

			unsigned int a = (fea == 0) ? next++ : (fea < 15) ? vertexfifo[(vertexfifooffset - fea) & 15] : 0;
			unsigned int b = (feb == 0) ? next++ : (feb < 15) ? vertexfifo[(vertexfifooffset - feb) & 15] : 0;
			unsigned int c = (fec == 0) ? next++ : (fec < 15) ? vertexfifo[(vertexfifooffset - fec) & 15] : 0;

			if (fea == 15)
				a = next - decodeVarInt(data);

			if (feb == 15)
				b = next - decodeVarInt(data);

			if (fec == 15)
				c = next - decodeVarInt(data);

			destination[i + 0] = a;
			destination[i + 1] = b;
			destination[i + 2] = c;

			if (fea == 0 || fea == 15)
				pushVertexFifo(vertexfifo, a, vertexfifooffset);

			if (feb == 0 || feb == 15)
				pushVertexFifo(vertexfifo, b, vertexfifooffset);

			if (fec == 0 || fec == 15)
				pushVertexFifo(vertexfifo, c, vertexfifooffset);

			pushEdgeFifo(edgefifo, b, a, edgefifooffset);
			pushEdgeFifo(edgefifo, c, b, edgefifooffset);
			pushEdgeFifo(edgefifo, a, c, edgefifooffset);
		}
	}

	assert(data == buffer + buffer_size);
	(void)buffer_size;
}
<commit_msg>indexcodec: Switch nibbles in the first triangle byte<commit_after>// This file is part of meshoptimizer library; see meshoptimizer.h for version/license details
#include "meshoptimizer.h"

#include <cassert>
#include <cstring>

// This work is based on:
// Conor Stokes. Vertex Cache Optimised Index Buffer Compression. 2014
namespace meshopt
{

typedef unsigned int VertexFifo[16];
typedef unsigned int EdgeFifo[16][2];

static const unsigned int kTriangleIndexOrder[3][3] =
    {
        // clang-format array formatting is broken :/
        {0, 1, 2},
        {1, 2, 0},
        {2, 0, 1},
};

static int rotateTriangle(unsigned int a, unsigned int b, unsigned int c, unsigned int next)
{
	(void)a;

	return (b == next) ? 1 : (c == next) ? 2 : 0;
}

static int getEdgeFifo(EdgeFifo fifo, unsigned int a, unsigned int b, unsigned int c, size_t offset)
{
	for (int i = 0; i < 16; ++i)
	{
		unsigned int index = (offset - 1 - i) & 15;
		unsigned int e0 = fifo[index][0];
		unsigned int e1 = fifo[index][1];

		if (e0 == a && e1 == b)
			return (i << 2) | 0;
		if (e0 == b && e1 == c)
			return (i << 2) | 1;
		if (e0 == c && e1 == a)
			return (i << 2) | 2;
	}

	return -1;
}

static void pushEdgeFifo(EdgeFifo fifo, unsigned int a, unsigned int b, size_t& offset)
{
	fifo[offset][0] = a;
	fifo[offset][1] = b;
	offset = (offset + 1) & 15;
}

static int getVertexFifo(VertexFifo fifo, unsigned int v, size_t offset)
{
	for (int i = 0; i < 16; ++i)
	{
		unsigned int index = (offset - 1 - i) & 15;

		if (fifo[index] == v)
			return i;
	}

	return -1;
}

static void pushVertexFifo(VertexFifo fifo, unsigned int v, size_t& offset)
{
	fifo[offset] = v;
	offset = (offset + 1) & 15;
}

static void encodeVarInt(unsigned char*& data, unsigned int v)
{
	do
	{
		*data++ = (v & 127) | (v > 127 ? 128 : 0);
		v >>= 7;
	} while (v);
}

static unsigned int decodeVarInt(const unsigned char*& data)
{
	unsigned int result = 0;
	unsigned int shift = 0;
	unsigned char group;

	do
	{
		group = *data++;
		result |= (group & 127) << shift;
		shift += 7;
	} while (group & 128);

	return result;
}
}

size_t meshopt_encodeIndexBuffer(unsigned char* buffer, size_t buffer_size, const unsigned int* indices, size_t index_count)
{
	using namespace meshopt;

	assert(index_count % 3 == 0);

	EdgeFifo edgefifo;
	memset(edgefifo, -1, sizeof(edgefifo));

	VertexFifo vertexfifo;
	memset(vertexfifo, -1, sizeof(vertexfifo));

	size_t edgefifooffset = 0;
	size_t vertexfifooffset = 0;

	unsigned int next = 0;

	unsigned char* data = buffer;

	for (size_t i = 0; i < index_count; i += 3)
	{
		int fer = getEdgeFifo(edgefifo, indices[i + 0], indices[i + 1], indices[i + 2], edgefifooffset);

		if (fer >= 0 && (fer >> 2) < 15)
		{
			const unsigned int* order = kTriangleIndexOrder[fer & 3];

			unsigned int a = indices[i + order[0]], b = indices[i + order[1]], c = indices[i + order[2]];

			int fe = fer >> 2;
			int fc = getVertexFifo(vertexfifo, c, vertexfifooffset);

			int fec = (fc >= 0 && fc < 14) ? (fc + 1) : (c == next) ? (next++, 0) : 15;

			*data++ = static_cast<unsigned char>((fe << 4) | fec);

			if (fec == 15)
				encodeVarInt(data, next - c);

			if (fec == 0 || fec == 15)
				pushVertexFifo(vertexfifo, c, vertexfifooffset);

			pushEdgeFifo(edgefifo, c, b, edgefifooffset);
			pushEdgeFifo(edgefifo, a, c, edgefifooffset);
		}
		else
		{
			const unsigned int* order = kTriangleIndexOrder[rotateTriangle(indices[i + 0], indices[i + 1], indices[i + 2], next)];

			unsigned int a = indices[i + order[0]], b = indices[i + order[1]], c = indices[i + order[2]];

			int fa = getVertexFifo(vertexfifo, a, vertexfifooffset);
			int fb = getVertexFifo(vertexfifo, b, vertexfifooffset);
			int fc = getVertexFifo(vertexfifo, c, vertexfifooffset);

			int fea = (fa >= 0 && fa < 14) ? (fa + 1) : (a == next) ? (next++, 0) : 15;
			int feb = (fb >= 0 && fb < 14) ? (fb + 1) : (b == next) ? (next++, 0) : 15;
			int fec = (fc >= 0 && fc < 14) ? (fc + 1) : (c == next) ? (next++, 0) : 15;

			*data++ = static_cast<unsigned char>((15 << 4) | fea);
			*data++ = static_cast<unsigned char>((feb << 4) | fec);

			if (fea == 15)
				encodeVarInt(data, next - a);

			if (feb == 15)
				encodeVarInt(data, next - b);

			if (fec == 15)
				encodeVarInt(data, next - c);

			if (fea == 0 || fea == 15)
				pushVertexFifo(vertexfifo, a, vertexfifooffset);

			if (feb == 0 || feb == 15)
				pushVertexFifo(vertexfifo, b, vertexfifooffset);

			if (fec == 0 || fec == 15)
				pushVertexFifo(vertexfifo, c, vertexfifooffset);

			pushEdgeFifo(edgefifo, b, a, edgefifooffset);
			pushEdgeFifo(edgefifo, c, b, edgefifooffset);
			pushEdgeFifo(edgefifo, a, c, edgefifooffset);
		}
	}

	assert(data <= buffer + buffer_size);
	(void)buffer_size;

	return data - buffer;
}

size_t meshopt_encodeIndexBufferBound(size_t index_count, size_t vertex_count)
{
	assert(index_count % 3 == 0);

	// compute number of bits required for each index
	unsigned int vertex_bits = 1;

	while (vertex_bits < 32 && vertex_count > size_t(1) << vertex_bits)
		vertex_bits++;

	// worst-case encoding is 2 header bytes + 3 varint-7 encoded indices
	unsigned int vertex_groups = (vertex_bits + 6) / 7;

	return (index_count / 3) * (2 + 3 * vertex_groups);
}

void meshopt_decodeIndexBuffer(unsigned int* destination, size_t index_count, const unsigned char* buffer, size_t buffer_size)
{
	using namespace meshopt;

	assert(index_count % 3 == 0);

	EdgeFifo edgefifo;
	memset(edgefifo, -1, sizeof(edgefifo));

	VertexFifo vertexfifo;
	memset(vertexfifo, -1, sizeof(vertexfifo));

	size_t edgefifooffset = 0;
	size_t vertexfifooffset = 0;

	unsigned int next = 0;

	const unsigned char* data = buffer;

	for (size_t i = 0; i < index_count; i += 3)
	{
		unsigned char code = *data++;

		int fe = code >> 4;

		if (fe < 15)
		{
			unsigned int a = edgefifo[(edgefifooffset - 1 - fe) & 15][0];
			unsigned int b = edgefifo[(edgefifooffset - 1 - fe) & 15][1];

			int fec = code & 15;

			unsigned int c = (fec == 0) ? next++ : (fec < 15) ? vertexfifo[(vertexfifooffset - fec) & 15] : next - decodeVarInt(data);

			destination[i + 0] = a;
			destination[i + 1] = b;
			destination[i + 2] = c;

			if (fec == 0 || fec == 15)
				pushVertexFifo(vertexfifo, c, vertexfifooffset);

			pushEdgeFifo(edgefifo, c, b, edgefifooffset);
			pushEdgeFifo(edgefifo, a, c, edgefifooffset);
		}
		else
		{
			int fea = code & 15;

			unsigned char codeaux = *data++;

			int feb = codeaux >> 4;
			int fec = codeaux & 15;

			unsigned int a = (fea == 0) ? next++ : (fea < 15) ? vertexfifo[(vertexfifooffset - fea) & 15] : 0;
			unsigned int b = (feb == 0) ? next++ : (feb < 15) ? vertexfifo[(vertexfifooffset - feb) & 15] : 0;
			unsigned int c = (fec == 0) ? next++ : (fec < 15) ? vertexfifo[(vertexfifooffset - fec) & 15] : 0;

			if (fea == 15)
				a = next - decodeVarInt(data);

			if (feb == 15)
				b = next - decodeVarInt(data);

			if (fec == 15)
				c = next - decodeVarInt(data);

			destination[i + 0] = a;
			destination[i + 1] = b;
			destination[i + 2] = c;

			if (fea == 0 || fea == 15)
				pushVertexFifo(vertexfifo, a, vertexfifooffset);

			if (feb == 0 || feb == 15)
				pushVertexFifo(vertexfifo, b, vertexfifooffset);

			if (fec == 0 || fec == 15)
				pushVertexFifo(vertexfifo, c, vertexfifooffset);

			pushEdgeFifo(edgefifo, b, a, edgefifooffset);
			pushEdgeFifo(edgefifo, c, b, edgefifooffset);
			pushEdgeFifo(edgefifo, a, c, edgefifooffset);
		}
	}

	assert(data == buffer + buffer_size);
	(void)buffer_size;
}
<|endoftext|>