<commit_before>/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2018, Mohamad Ayman.
 *  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.
 *   * The name of Mohamad Ayman may not be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Mohamad Ayman */

// SA
#include "simulation_widget.h"
#include "header_widget.h"
#include "../tools/xml_syntax_highlighter.h"
#include <moveit/setup_assistant/tools/xml_manipulation.h>

// Qt
#include <QColor>
#include <QLabel>
#include <QMessageBox>
#include <QPushButton>
#include <QTextEdit>
#include <QVBoxLayout>
#include <QProcess>

#include <moveit/robot_state/conversions.h>
#include <moveit_msgs/DisplayRobotState.h>

#include <regex>

namespace moveit_setup_assistant
{
// ******************************************************************************************
// Constructor
// ******************************************************************************************
SimulationWidget::SimulationWidget(QWidget* parent, const MoveItConfigDataPtr& config_data)
  : SetupScreenWidget(parent), config_data_(config_data)
{
  // Basic widget container
  QVBoxLayout* layout = new QVBoxLayout();
  layout->setAlignment(Qt::AlignTop);

  // Top Header Area ------------------------------------------------

  HeaderWidget* header = new HeaderWidget("Simulate With Gazebo",
                                          "The following tool will auto-generate the URDF changes needed "
                                          "for Gazebo compatibility with ROSControl and MoveIt. The "
                                          "needed changes are shown in green.",
                                          this);
  layout->addWidget(header);
  layout->addSpacerItem(new QSpacerItem(1, 8, QSizePolicy::Fixed, QSizePolicy::Fixed));

  // Top Buttons --------------------------------------------------
  QHBoxLayout* controls_layout = new QHBoxLayout();

  // Used to overwrite the original URDF
  btn_overwrite_ = new QPushButton("Over&write original URDF", this);
  btn_overwrite_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
  connect(btn_overwrite_, SIGNAL(clicked()), this, SLOT(overwriteURDF()));
  controls_layout->addWidget(btn_overwrite_);

  btn_open_ = new QPushButton("&Open original URDF", this);
  btn_open_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
  btn_open_->setToolTip("Open original URDF file in editor");
  connect(btn_open_, SIGNAL(clicked()), this, SLOT(openURDF()));
  controls_layout->addWidget(btn_open_);

  // Align buttons to the left
  controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Fixed));

  // Add layout
  layout->addLayout(controls_layout);

  // When there are no changes to be made
  no_changes_label_ = new QLabel(this);
  no_changes_label_->setText("URDF is ready for Gazebo. No changes required.");
  no_changes_label_->setFont(QFont(QFont().defaultFamily(), 18));
  no_changes_label_->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding);
  no_changes_label_->setAlignment(Qt::AlignTop);
  layout->addWidget(no_changes_label_);

  // URDF text
  simulation_text_ = new QTextEdit(this);
  simulation_text_->setLineWrapMode(QTextEdit::NoWrap);
  layout->addWidget(simulation_text_);
  // Configure highlighter
  auto highlighter = new XmlSyntaxHighlighter(simulation_text_->document());
  QTextCharFormat format;
  format.setForeground(Qt::darkGreen);
  highlighter->addTag("inertial", format);
  highlighter->addTag("transmission", format);
  highlighter->addTag("gazebo", format);

  // Copy URDF link, hidden initially
  copy_urdf_ = new QLabel(this);
  copy_urdf_->setText("Copy to Clipboard");
  connect(copy_urdf_, &QLabel::linkActivated, this, &SimulationWidget::copyURDF);
  layout->addWidget(copy_urdf_);

  // Finish Layout --------------------------------------------------
  this->setLayout(layout);
}

void SimulationWidget::focusGiven()
{
  if (!simulation_text_->document()->isEmpty())
    return;  // nothing to do

  simulation_text_->setVisible(true);
  std::string text = generateGazeboCompatibleURDF();
  config_data_->gazebo_urdf_string_ = text;

  simulation_text_->document()->setPlainText(QString::fromStdString(text));

  // Add generated Gazebo URDF to config file if not empty
  bool have_changes = !text.empty();
  config_data_->save_gazebo_urdf_ = have_changes;

  // GUI elements are visible only if there are URDF changes to display/edit
  simulation_text_->setVisible(have_changes);
  btn_overwrite_->setVisible(have_changes);
  btn_open_->setVisible(have_changes && !qgetenv("EDITOR").isEmpty());
  copy_urdf_->setVisible(have_changes);
  no_changes_label_->setVisible(!have_changes);

  // Disable overwrite button if URDF originates from xacro
  btn_overwrite_->setDisabled(config_data_->urdf_from_xacro_);
  QString tooltip;
  if (btn_overwrite_->isEnabled())
    tooltip = tr("Overwrite URDF in original location:\n").append(config_data_->urdf_path_.c_str());
  else
    tooltip = tr("Cannot overwrite original, xacro-based URDF");
  btn_overwrite_->setToolTip(tooltip);

  if (have_changes)
    config_data_->changes |= MoveItConfigData::SIMULATION;
  else
    config_data_->changes &= ~MoveItConfigData::SIMULATION;
}

bool SimulationWidget::focusLost()
{
  if (!config_data_->save_gazebo_urdf_)
    return true;  // saving is disabled anyway

  // validate XML
  TiXmlDocument doc;
  auto urdf = simulation_text_->document()->toPlainText().toStdString();
  doc.Parse(urdf.c_str(), nullptr, TIXML_ENCODING_UTF8);
  if (doc.Error())
  {
    QTextCursor cursor = simulation_text_->textCursor();
    cursor.movePosition(QTextCursor::Start);
    cursor.movePosition(QTextCursor::Down, QTextCursor::MoveAnchor, doc.ErrorRow());
    cursor.movePosition(QTextCursor::Right, QTextCursor::MoveAnchor, doc.ErrorCol());
    simulation_text_->setTextCursor(cursor);
    QMessageBox::warning(this, tr("Gazebo URDF"), tr("Error parsing XML:\n").append(doc.ErrorDesc()));
    simulation_text_->setFocus(Qt::OtherFocusReason);
    return false;  // reject switching
  }
  else
    config_data_->gazebo_urdf_string_ = std::move(urdf);
  return true;
}

// ******************************************************************************************
// Called when save URDF button is clicked
// ******************************************************************************************
void SimulationWidget::overwriteURDF()
{
  if (!focusLost())  // validate XML
    return;

  if (!config_data_->outputGazeboURDFFile(config_data_->urdf_path_))
    QMessageBox::warning(this, "Gazebo URDF", tr("Failed to save to ").append(config_data_->urdf_path_.c_str()));
  else  // Display success message
    QMessageBox::information(this, "Overwriting Successfull",
                             "Original robot description URDF was successfully overwritten.");

  // Remove Gazebo URDF file from list of to-be-written config files
  config_data_->save_gazebo_urdf_ = false;
  config_data_->changes &= ~MoveItConfigData::SIMULATION;
}

void SimulationWidget::openURDF()
{
  QProcess::startDetached(qgetenv("EDITOR"), { config_data_->urdf_path_.c_str() });
}

// ******************************************************************************************
// Called the copy to clipboard button is clicked
// ******************************************************************************************
void SimulationWidget::copyURDF()
{
  simulation_text_->selectAll();
  simulation_text_->copy();
}

// Generate a Gazebo-compatible robot URDF
std::string SimulationWidget::generateGazeboCompatibleURDF() const
{
  TiXmlDocument doc;
  doc.Parse(config_data_->urdf_string_.c_str(), nullptr, TIXML_ENCODING_UTF8);
  auto root = doc.RootElement();

  // Normalize original urdf_string_
  TiXmlPrinter orig_urdf;
  doc.Accept(&orig_urdf);

  // Map existing SimpleTransmission elements to their joint name
  std::map<std::string, TiXmlElement*> transmission_elements;
  for (TiXmlElement* element = root->FirstChildElement("transmission"); element != nullptr;
       element = element->NextSiblingElement(element->Value()))
  {
    auto type_tag = element->FirstChildElement("type");
    auto joint_tag = element->FirstChildElement("joint");
    if (!type_tag || !type_tag->GetText() || !joint_tag || !joint_tag->Attribute("name"))
      continue;  // ignore invalid tags
    if (std::string(type_tag->GetText()) == "transmission_interface/SimpleTransmission")
      transmission_elements[element->FirstChildElement("joint")->Attribute("name")] = element;
  }

  // Loop through Link and Joint elements and add Gazebo tags if not present
  for (TiXmlElement* element = root->FirstChildElement(); element != nullptr; element = element->NextSiblingElement())
  {
    const std::string tag_name(element->Value());
    if (tag_name == "link" && element->FirstChildElement("collision"))
    {
      TiXmlElement* inertial = uniqueInsert(*element, "inertial");
      uniqueInsert(*inertial, "mass", { { "value", "0.1" } });
      uniqueInsert(*inertial, "inertia",
                   { { "ixx", "0.03" },
                     { "iyy", "0.03" },
                     { "izz", "0.03" },
                     { "ixy", "0.0" },
                     { "ixz", "0.0" },
                     { "iyz", "0.0" } });
    }
    else if (tag_name == "joint")
    {
      const char* joint_type = element->Attribute("type");
      const char* joint_name = element->Attribute("name");
      if (!joint_type || !joint_name || strcmp(joint_type, "fixed") == 0)
        continue;  // skip invalid or fixed joints

      // find existing or create new transmission element for this joint
      TiXmlElement* transmission;
      auto it = transmission_elements.find(joint_name);
      if (it != transmission_elements.end())
        transmission = it->second;
      else
      {
        transmission = root->InsertEndChild(TiXmlElement("transmission"))->ToElement();
        transmission->SetAttribute("name", std::string("trans_") + joint_name);
      }

      uniqueInsert(*transmission, "type", {}, "transmission_interface/SimpleTransmission");

      std::string hw_interface = config_data_->getJointHardwareInterface(joint_name);
      auto* joint = uniqueInsert(*transmission, "joint", { { "name", joint_name } });
      uniqueInsert(*joint, "hardwareInterface", {}, hw_interface.c_str());

      auto actuator_name = joint_name + std::string("_motor");
      auto* actuator = uniqueInsert(*transmission, "actuator", { { "name", actuator_name.c_str() } });
      uniqueInsert(*actuator, "hardwareInterface", {}, hw_interface.c_str());
      uniqueInsert(*actuator, "mechanicalReduction", {}, "1");
    }
  }

  // Add gazebo_ros_control plugin which reads the transmission tags
  TiXmlElement* gazebo = uniqueInsert(*root, "gazebo");
  TiXmlElement* plugin = uniqueInsert(*gazebo, "plugin", { { "name", "gazebo_ros_control", true } });
  uniqueInsert(*plugin, "robotNamespace", {}, "/");

  // generate new URDF
  TiXmlPrinter new_urdf;
  doc.Accept(&new_urdf);
  // and return it when there are changes
  return orig_urdf.Str() == new_urdf.Str() ? std::string() : new_urdf.Str();
}

}  // namespace moveit_setup_assistant
<commit_msg>Add inertial/origin tag<commit_after>/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2018, Mohamad Ayman.
 *  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.
 *   * The name of Mohamad Ayman may not be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Mohamad Ayman */

// SA
#include "simulation_widget.h"
#include "header_widget.h"
#include "../tools/xml_syntax_highlighter.h"
#include <moveit/setup_assistant/tools/xml_manipulation.h>

// Qt
#include <QColor>
#include <QLabel>
#include <QMessageBox>
#include <QPushButton>
#include <QTextEdit>
#include <QVBoxLayout>
#include <QProcess>

#include <moveit/robot_state/conversions.h>
#include <moveit_msgs/DisplayRobotState.h>

#include <regex>

namespace moveit_setup_assistant
{
// ******************************************************************************************
// Constructor
// ******************************************************************************************
SimulationWidget::SimulationWidget(QWidget* parent, const MoveItConfigDataPtr& config_data)
  : SetupScreenWidget(parent), config_data_(config_data)
{
  // Basic widget container
  QVBoxLayout* layout = new QVBoxLayout();
  layout->setAlignment(Qt::AlignTop);

  // Top Header Area ------------------------------------------------

  HeaderWidget* header = new HeaderWidget("Simulate With Gazebo",
                                          "The following tool will auto-generate the URDF changes needed "
                                          "for Gazebo compatibility with ROSControl and MoveIt. The "
                                          "needed changes are shown in green.",
                                          this);
  layout->addWidget(header);
  layout->addSpacerItem(new QSpacerItem(1, 8, QSizePolicy::Fixed, QSizePolicy::Fixed));

  // Top Buttons --------------------------------------------------
  QHBoxLayout* controls_layout = new QHBoxLayout();

  // Used to overwrite the original URDF
  btn_overwrite_ = new QPushButton("Over&write original URDF", this);
  btn_overwrite_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
  connect(btn_overwrite_, SIGNAL(clicked()), this, SLOT(overwriteURDF()));
  controls_layout->addWidget(btn_overwrite_);

  btn_open_ = new QPushButton("&Open original URDF", this);
  btn_open_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
  btn_open_->setToolTip("Open original URDF file in editor");
  connect(btn_open_, SIGNAL(clicked()), this, SLOT(openURDF()));
  controls_layout->addWidget(btn_open_);

  // Align buttons to the left
  controls_layout->addItem(new QSpacerItem(20, 20, QSizePolicy::Expanding, QSizePolicy::Fixed));

  // Add layout
  layout->addLayout(controls_layout);

  // When there are no changes to be made
  no_changes_label_ = new QLabel(this);
  no_changes_label_->setText("URDF is ready for Gazebo. No changes required.");
  no_changes_label_->setFont(QFont(QFont().defaultFamily(), 18));
  no_changes_label_->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding);
  no_changes_label_->setAlignment(Qt::AlignTop);
  layout->addWidget(no_changes_label_);

  // URDF text
  simulation_text_ = new QTextEdit(this);
  simulation_text_->setLineWrapMode(QTextEdit::NoWrap);
  layout->addWidget(simulation_text_);
  // Configure highlighter
  auto highlighter = new XmlSyntaxHighlighter(simulation_text_->document());
  QTextCharFormat format;
  format.setForeground(Qt::darkGreen);
  highlighter->addTag("inertial", format);
  highlighter->addTag("transmission", format);
  highlighter->addTag("gazebo", format);

  // Copy URDF link, hidden initially
  copy_urdf_ = new QLabel(this);
  copy_urdf_->setText("Copy to Clipboard");
  connect(copy_urdf_, &QLabel::linkActivated, this, &SimulationWidget::copyURDF);
  layout->addWidget(copy_urdf_);

  // Finish Layout --------------------------------------------------
  this->setLayout(layout);
}

void SimulationWidget::focusGiven()
{
  if (!simulation_text_->document()->isEmpty())
    return;  // nothing to do

  simulation_text_->setVisible(true);
  std::string text = generateGazeboCompatibleURDF();
  config_data_->gazebo_urdf_string_ = text;

  simulation_text_->document()->setPlainText(QString::fromStdString(text));

  // Add generated Gazebo URDF to config file if not empty
  bool have_changes = !text.empty();
  config_data_->save_gazebo_urdf_ = have_changes;

  // GUI elements are visible only if there are URDF changes to display/edit
  simulation_text_->setVisible(have_changes);
  btn_overwrite_->setVisible(have_changes);
  btn_open_->setVisible(have_changes && !qgetenv("EDITOR").isEmpty());
  copy_urdf_->setVisible(have_changes);
  no_changes_label_->setVisible(!have_changes);

  // Disable overwrite button if URDF originates from xacro
  btn_overwrite_->setDisabled(config_data_->urdf_from_xacro_);
  QString tooltip;
  if (btn_overwrite_->isEnabled())
    tooltip = tr("Overwrite URDF in original location:\n").append(config_data_->urdf_path_.c_str());
  else
    tooltip = tr("Cannot overwrite original, xacro-based URDF");
  btn_overwrite_->setToolTip(tooltip);

  if (have_changes)
    config_data_->changes |= MoveItConfigData::SIMULATION;
  else
    config_data_->changes &= ~MoveItConfigData::SIMULATION;
}

bool SimulationWidget::focusLost()
{
  if (!config_data_->save_gazebo_urdf_)
    return true;  // saving is disabled anyway

  // validate XML
  TiXmlDocument doc;
  auto urdf = simulation_text_->document()->toPlainText().toStdString();
  doc.Parse(urdf.c_str(), nullptr, TIXML_ENCODING_UTF8);
  if (doc.Error())
  {
    QTextCursor cursor = simulation_text_->textCursor();
    cursor.movePosition(QTextCursor::Start);
    cursor.movePosition(QTextCursor::Down, QTextCursor::MoveAnchor, doc.ErrorRow());
    cursor.movePosition(QTextCursor::Right, QTextCursor::MoveAnchor, doc.ErrorCol());
    simulation_text_->setTextCursor(cursor);
    QMessageBox::warning(this, tr("Gazebo URDF"), tr("Error parsing XML:\n").append(doc.ErrorDesc()));
    simulation_text_->setFocus(Qt::OtherFocusReason);
    return false;  // reject switching
  }
  else
    config_data_->gazebo_urdf_string_ = std::move(urdf);
  return true;
}

// ******************************************************************************************
// Called when save URDF button is clicked
// ******************************************************************************************
void SimulationWidget::overwriteURDF()
{
  if (!focusLost())  // validate XML
    return;

  if (!config_data_->outputGazeboURDFFile(config_data_->urdf_path_))
    QMessageBox::warning(this, "Gazebo URDF", tr("Failed to save to ").append(config_data_->urdf_path_.c_str()));
  else  // Display success message
    QMessageBox::information(this, "Overwriting Successfull",
                             "Original robot description URDF was successfully overwritten.");

  // Remove Gazebo URDF file from list of to-be-written config files
  config_data_->save_gazebo_urdf_ = false;
  config_data_->changes &= ~MoveItConfigData::SIMULATION;
}

void SimulationWidget::openURDF()
{
  QProcess::startDetached(qgetenv("EDITOR"), { config_data_->urdf_path_.c_str() });
}

// ******************************************************************************************
// Called the copy to clipboard button is clicked
// ******************************************************************************************
void SimulationWidget::copyURDF()
{
  simulation_text_->selectAll();
  simulation_text_->copy();
}

// Generate a Gazebo-compatible robot URDF
std::string SimulationWidget::generateGazeboCompatibleURDF() const
{
  TiXmlDocument doc;
  doc.Parse(config_data_->urdf_string_.c_str(), nullptr, TIXML_ENCODING_UTF8);
  auto root = doc.RootElement();

  // Normalize original urdf_string_
  TiXmlPrinter orig_urdf;
  doc.Accept(&orig_urdf);

  // Map existing SimpleTransmission elements to their joint name
  std::map<std::string, TiXmlElement*> transmission_elements;
  for (TiXmlElement* element = root->FirstChildElement("transmission"); element != nullptr;
       element = element->NextSiblingElement(element->Value()))
  {
    auto type_tag = element->FirstChildElement("type");
    auto joint_tag = element->FirstChildElement("joint");
    if (!type_tag || !type_tag->GetText() || !joint_tag || !joint_tag->Attribute("name"))
      continue;  // ignore invalid tags
    if (std::string(type_tag->GetText()) == "transmission_interface/SimpleTransmission")
      transmission_elements[element->FirstChildElement("joint")->Attribute("name")] = element;
  }

  // Loop through Link and Joint elements and add Gazebo tags if not present
  for (TiXmlElement* element = root->FirstChildElement(); element != nullptr; element = element->NextSiblingElement())
  {
    const std::string tag_name(element->Value());
    if (tag_name == "link" && element->FirstChildElement("collision"))
    {
      TiXmlElement* inertial = uniqueInsert(*element, "inertial");
      uniqueInsert(*inertial, "mass", { { "value", "0.1" } });
      uniqueInsert(*inertial, "origin", { { "xyz", "0 0 0" }, { "rpy", "0 0 0" } });
      uniqueInsert(*inertial, "inertia",
                   { { "ixx", "0.03" },
                     { "iyy", "0.03" },
                     { "izz", "0.03" },
                     { "ixy", "0.0" },
                     { "ixz", "0.0" },
                     { "iyz", "0.0" } });
    }
    else if (tag_name == "joint")
    {
      const char* joint_type = element->Attribute("type");
      const char* joint_name = element->Attribute("name");
      if (!joint_type || !joint_name || strcmp(joint_type, "fixed") == 0)
        continue;  // skip invalid or fixed joints

      // find existing or create new transmission element for this joint
      TiXmlElement* transmission;
      auto it = transmission_elements.find(joint_name);
      if (it != transmission_elements.end())
        transmission = it->second;
      else
      {
        transmission = root->InsertEndChild(TiXmlElement("transmission"))->ToElement();
        transmission->SetAttribute("name", std::string("trans_") + joint_name);
      }

      uniqueInsert(*transmission, "type", {}, "transmission_interface/SimpleTransmission");

      std::string hw_interface = config_data_->getJointHardwareInterface(joint_name);
      auto* joint = uniqueInsert(*transmission, "joint", { { "name", joint_name } });
      uniqueInsert(*joint, "hardwareInterface", {}, hw_interface.c_str());

      auto actuator_name = joint_name + std::string("_motor");
      auto* actuator = uniqueInsert(*transmission, "actuator", { { "name", actuator_name.c_str() } });
      uniqueInsert(*actuator, "hardwareInterface", {}, hw_interface.c_str());
      uniqueInsert(*actuator, "mechanicalReduction", {}, "1");
    }
  }

  // Add gazebo_ros_control plugin which reads the transmission tags
  TiXmlElement* gazebo = uniqueInsert(*root, "gazebo");
  TiXmlElement* plugin = uniqueInsert(*gazebo, "plugin", { { "name", "gazebo_ros_control", true } });
  uniqueInsert(*plugin, "robotNamespace", {}, "/");

  // generate new URDF
  TiXmlPrinter new_urdf;
  doc.Accept(&new_urdf);
  // and return it when there are changes
  return orig_urdf.Str() == new_urdf.Str() ? std::string() : new_urdf.Str();
}

}  // namespace moveit_setup_assistant
<|endoftext|>


<commit_before>// includes files
#include <gl\glew.h> ///< always include glew before glfw
#include "Window.h"  ///< include after glew, never before. that include glfw too
#include "EasyErrors.h"
#include "InputManager.h"
#include "GLProgram.h"
#include "Camera3D.h"
#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>
#include <glm\gtx\rotate_vector.hpp>
#include <stdio.h>
#include <stdlib.h>
#include <chrono>
#include "FpsCounter.h"
#include "DrawBatch.h"
#include "Block.h"
#include "Light.h"
#include "OBBRayPicking.h"

int main()
{
	auto t_start = std::chrono::high_resolution_clock::now();

	const int width = 1600;
	const int height = 1200;

	if (!glfwInit())
	{
		Debug_Log("Failed to initialize GLFW\n");
	}
	// create a window
	Window m_window;
	m_window.init(width, height, "Doxel");

	// Set up glew
	glewExperimental = GL_TRUE;
	if (glewInit() != GLEW_OK)
	{
		Debug_Log("Failed to initialize GLEW\n");
	}

	// Set up the Input Manager
	InputManager m_inputManager;
	m_inputManager.init(m_window.getGlfwWindow());

	glm::vec3 cPos = glm::vec3(0,2,-5);
	Camera3D m_camera;
	m_camera.init(cPos, 45.0f,m_window.getAspectRatio(),1.0, 10000);

	glEnable(GL_DEPTH_TEST);

	DrawBatch m_drawBatch;
	m_drawBatch.init(&m_camera);

	// compile shaders, add attribes and all that
	GLProgram m_glProgram;
	m_glProgram.loadShaders("Shaders/shader.vert", "Shaders/shader.frag");

	glm::mat4 model, projection, view;
	glm::vec3 mPos(0, 0, 0);

	projection = m_camera.getProjectionMatrix();

	FpsCounter m_fpsCounter;
	glm::vec3 lightPos(0, 100, 100);

	//ChunkStuff
	ChunkManager m_chunkManager;
	m_chunkManager.init();

	Light m_light;
	m_light.init(glm::vec3(0, 0, 100), Color3f(1.0,0,0), 1, &m_glProgram);

	m_fpsCounter.start();

	OBBRayPicking rayPicker;

	std::vector<BlockClicked> m_clickedBlocks;
	BlockClicked m_falseBlock;
	m_falseBlock.chunkX = NUM_CHUNKS + 1;

	// This is the game loop
	while (!m_window.shouldWindowClose())
	{

		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
		glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
		m_inputManager.update();
		m_camera.update();
		// update input
		// handle input from user
		if (m_inputManager.isKeyPressed(KEYS::ESC))
		{
			m_window.setWindowClose();
		}

		if (m_inputManager.isKeyPressed(KEYS::W) || m_inputManager.isKeyHeldDown(KEYS::W)) ///< FORWARD
		{
			m_camera.applyMovement(cMove::Forward);
		}
		if (m_inputManager.isKeyPressed(KEYS::S) || m_inputManager.isKeyHeldDown(KEYS::S)) ///< BACKWARD
		{
			m_camera.applyMovement(cMove::Backward);
		}
		if (m_inputManager.isKeyPressed(KEYS::A) || m_inputManager.isKeyHeldDown(KEYS::A)) ///< LEFT
		{
			m_camera.applyMovement(cMove::Left);
		}
		if (m_inputManager.isKeyPressed(KEYS::D) || m_inputManager.isKeyHeldDown(KEYS::D)) ///< RIGHT
		{
			m_camera.applyMovement(cMove::Right);
		}
		if (m_inputManager.isKeyPressed(KEYS::Z) || m_inputManager.isKeyHeldDown(KEYS::Z)) ///<UP
		{
			m_camera.applyMovement(cMove::Up);
		}
		if (m_inputManager.isKeyPressed(KEYS::X) || m_inputManager.isKeyHeldDown(KEYS::X))///< DOWN
		{
			m_camera.applyMovement(cMove::Down);
		}
		if (m_inputManager.isKeyPressed(KEYS::Q) || m_inputManager.isKeyHeldDown(KEYS::Q))///< rotate left
		{
			m_camera.applyMovement(cMove::Rotate_Left);
		}
		if (m_inputManager.isKeyPressed(KEYS::E) || m_inputManager.isKeyHeldDown(KEYS::E))///< rotate left
		{
			m_camera.applyMovement(cMove::Rotate_Right);
		}
		if (m_inputManager.isKeyPressed(KEYS::F) || m_inputManager.isKeyHeldDown(KEYS::F))///< rotate left
		{
			m_camera.applyMovement(cMove::Roll_Left);
		}
		if (m_inputManager.isKeyPressed(KEYS::G) || m_inputManager.isKeyHeldDown(KEYS::G))///< rotate left
		{
			m_camera.applyMovement(cMove::Roll_Right);
		}
		if (m_inputManager.isKeyPressed(KEYS::C) || m_inputManager.isKeyHeldDown(KEYS::C))///< rotate left
		{
			m_camera.applyMovement(cMove::Pitch_Up);
		}
		if (m_inputManager.isKeyPressed(KEYS::V) || m_inputManager.isKeyHeldDown(KEYS::V))///< rotate left
		{
			m_camera.applyMovement(cMove::Pitch_Down);
		}
		if (m_inputManager.isKeyPressed(KEYS::UP) || m_inputManager.isKeyHeldDown(KEYS::UP))
		{
			m_camera.increaseSpeed();
		}
		if (m_inputManager.isKeyPressed(KEYS::DOWN) || m_inputManager.isKeyHeldDown(KEYS::DOWN))
		{
			m_camera.decreaseSpeed();
		}
		if (m_inputManager.isKeyPressed(KEYS::SPACE))
		{
			m_camera.setPosition(glm::vec3(0));
			m_camera.setDirection(glm::vec3(1, 0, 0));
			m_camera.setUpDir(glm::vec3(0, 1, 0));
			m_camera.setSpeed(0.1f);
		}
		if (m_inputManager.isKeyPressed(KEYS::R) || m_inputManager.isKeyHeldDown(KEYS::R))
		{
			m_chunkManager.setGenMethod(GEN_METHOD::RANDOM);
			m_clickedBlocks.clear();
		}
		if (m_inputManager.isKeyPressed(KEYS::T))
		{
			m_chunkManager.setGenMethod(GEN_METHOD::SPHERE);
			m_clickedBlocks.clear();

		}
		if (m_inputManager.isKeyPressed(KEYS::Y))
		{
			m_chunkManager.setGenMethod(GEN_METHOD::ALL);
			m_clickedBlocks.clear();

		}

		auto t_now = std::chrono::high_resolution_clock::now();
		float time = std::chrono::duration_cast<std::chrono::duration<float>>(t_now - t_start).count();

		glm::mat4 view = m_camera.getViewMatrix();
		glm::mat4 mvp = projection * view * model;

		if (m_inputManager.isKeyPressed(KEYS::MOUSE_RIGHT)|| m_inputManager.isKeyHeldDown(KEYS::MOUSE_RIGHT))
		{
			if (!m_clickedBlocks.empty())
			{
				m_chunkManager.setBlock(m_clickedBlocks.back(), true);
				m_clickedBlocks.pop_back();
			}
		}
		// ray picking

			bool intersected = false;
			BlockClicked bestCube;
			bestCube.distance = 10000.0f;
			glm::vec3 rayOrigin;
			glm::vec3 rayDirection;
			glm::vec3 aabbMax(BLOCK_WIDTH, BLOCK_WIDTH, BLOCK_WIDTH);
			glm::vec3 aabbMin(0.0, 0.0, 0.0);
			float distance;
			rayPicker.screenPosToWorldRay(m_inputManager.getCurrentMouseCoords().x, m_window.getHeight() - m_inputManager.getCurrentMouseCoords().y, m_window.getWidth(), m_window.getHeight(), view, projection, rayOrigin, rayDirection);
			bool procced = true;
			// THE SUPER LOOP
			// TODO: improve ray algorithem
			for (unsigned int chunkX = 0; chunkX < NUM_CHUNKS; chunkX++)
			{
				if (!procced)
				{
					break;
				}
				for (unsigned int chunkZ = 0; chunkZ < NUM_CHUNKS; chunkZ++)
				{
					if (!procced)
					{
						break;
					}
					if (!m_chunkManager.getActiveChunk(chunkX, chunkZ))
					{
						continue;
					}
					for (unsigned int blockX = 0; blockX < CHUNK_SIZE; blockX++)
					{
						if (!procced)
						{
							break;
						}
						for (unsigned int blockY = 0; blockY < CHUNK_SIZE; blockY++)
						{
							if (!procced)
							{
								break;
							}
							for (unsigned int blockZ = 0; blockZ < CHUNK_SIZE; blockZ++)
							{
								if (!m_chunkManager.getActiveBlock(chunkX, chunkZ, blockX, blockY, blockZ))
								{
									continue;
								}
								if (!procced)
								{
									break;
								}

								if (rayPicker.testRayOBBIntersection(rayOrigin, rayDirection,
									glm::vec3(chunkX * (CHUNK_SIZE ) + (blockX*BLOCK_WIDTH), (blockY*BLOCK_WIDTH), chunkZ * (CHUNK_SIZE ) + blockZ*BLOCK_WIDTH) + aabbMin,
									glm::vec3(chunkX * (CHUNK_SIZE ) + (blockX*BLOCK_WIDTH), (blockY*BLOCK_WIDTH), chunkZ * (CHUNK_SIZE ) + blockZ*BLOCK_WIDTH) + aabbMax, model,
									glm::vec3(chunkX * (CHUNK_SIZE ) + (blockX*BLOCK_WIDTH), (blockY*BLOCK_WIDTH), chunkZ * (CHUNK_SIZE ) + blockZ*BLOCK_WIDTH), distance))
								{

									if (bestCube.distance > distance)
									{
										bestCube.setChunk(chunkX, chunkZ);
										bestCube.setBlock(blockX, blockY, blockZ);
										bestCube.distance = distance;
										intersected = true;

									}

								}
							}
						}
					}
				}
			}
			if (intersected)
			{
				if (m_inputManager.isKeyPressed(KEYS::MOUSE_LEFT))
				{
					m_chunkManager.setBlock(bestCube.chunkX, bestCube.chunkZ, bestCube.blockX, bestCube.blockY, bestCube.blockZ, false);
					Debug_Log("CHOSEN");
					Debug_Log("INTERSECTION DISTANCE: " << bestCube.distance);
					Debug_Log("CHUNK IS: " << bestCube.chunkX << "," << bestCube.chunkZ);
					Debug_Log("BLOCK IS: " << bestCube.blockX << "," << bestCube.blockY << "," << bestCube.blockZ);
				}
		//		m_chunkManager.setBlock(bestCube.chunkX, bestCube.chunkZ, bestCube.blockX, bestCube.blockY, bestCube.blockZ, false);

				m_clickedBlocks.push_back(bestCube);
			}

	//	}
	//	glm::vec3 lightPos = glm::vec3(cosf(time / 2)* CHUNK_SIZE * NUM_CHUNKS * BLOCK_WIDTH * 1.1, sinf(time / 2) * CHUNK_SIZE * NUM_CHUNKS * BLOCK_WIDTH*1.1, CHUNK_SIZE * NUM_CHUNKS * BLOCK_WIDTH / 2);
		glm::vec3 lightPos(BLOCK_WIDTH*CHUNK_SIZE*NUM_CHUNKS / 4, 20, BLOCK_WIDTH*CHUNK_SIZE*NUM_CHUNKS /4);
		m_light.setPosition(lightPos);
		m_glProgram.uploadUniformMatrix("mvp", 1, mvp, GL_FALSE);
		m_glProgram.uploadUniformMatrix("m", 1, model, GL_FALSE);
		m_glProgram.uploadUniformMatrix("v", 1, view, GL_FALSE);

		m_light.update();

		m_chunkManager.update(m_camera.getPosition());

		m_drawBatch.start();
		if (!m_clickedBlocks.empty() )
		{
		//	m_drawBatch.draw(glm::vec3(m_clickedBlocks.back().chunkX * CHUNK_SIZE + m_clickedBlocks.back().blockX, m_clickedBlocks.back().blockY, m_clickedBlocks.back().chunkZ * CHUNK_SIZE + m_clickedBlocks.back().blockZ), glm::vec3(BLOCK_WIDTH), Color8(255, 255, 255, 255));
			if (intersected)
			{
				Debug_Log("Draw true");
				m_chunkManager.setCurrentClicked(m_clickedBlocks.back());
			}
			else
			{
				Debug_Log("Draw False");
				m_chunkManager.setCurrentClicked(m_falseBlock);
			}
		}
		if (m_clickedBlocks.size() > 60)
		{
			m_clickedBlocks.clear();
		}
		m_chunkManager.draw(&m_drawBatch);
		m_drawBatch.draw(glm::vec3(0, -0.1, 0), glm::vec3((CHUNK_SIZE + 1) * (NUM_CHUNKS), EPSILON * 100, (CHUNK_SIZE + 1) * (NUM_CHUNKS)), Color8(250, 214, 165, 255), true);
		m_drawBatch.draw(m_light.getPosition(), glm::vec3(10, 10, 10), Color8(255, 252, 127, 255));
		m_drawBatch.end();
		m_drawBatch.renderBatch();

		// update the window
		m_window.update();

		m_fpsCounter.end();
	}
	m_clickedBlocks.clear();
	//m_chunkManager.dispose(); TODO:FIX
	m_glProgram.deleteProgram();

	m_drawBatch.dispose();
	//Close the window
	m_chunkManager.dispose();
	m_window.dispose();

	glfwTerminate();
	return 0;
}
<commit_msg>Removed some debugging code<commit_after>// includes files
#include <gl\glew.h> ///< always include glew before glfw
#include "Window.h"  ///< include after glew, never before. that include glfw too
#include "EasyErrors.h"
#include "InputManager.h"
#include "GLProgram.h"
#include "Camera3D.h"
#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>
#include <glm\gtx\rotate_vector.hpp>
#include <stdio.h>
#include <stdlib.h>
#include <chrono>
#include "FpsCounter.h"
#include "DrawBatch.h"
#include "Block.h"
#include "Light.h"
#include "OBBRayPicking.h"

int main()
{
	auto t_start = std::chrono::high_resolution_clock::now();

	const int width = 1600;
	const int height = 1200;

	if (!glfwInit())
	{
		Debug_Log("Failed to initialize GLFW\n");
	}
	// create a window
	Window m_window;
	m_window.init(width, height, "Doxel");

	// Set up glew
	glewExperimental = GL_TRUE;
	if (glewInit() != GLEW_OK)
	{
		Debug_Log("Failed to initialize GLEW\n");
	}

	// Set up the Input Manager
	InputManager m_inputManager;
	m_inputManager.init(m_window.getGlfwWindow());

	glm::vec3 cPos = glm::vec3(0,2,-5);
	Camera3D m_camera;
	m_camera.init(cPos, 45.0f,m_window.getAspectRatio(),1.0, 10000);

	glEnable(GL_DEPTH_TEST);

	DrawBatch m_drawBatch;
	m_drawBatch.init(&m_camera);

	// compile shaders, add attribes and all that
	GLProgram m_glProgram;
	m_glProgram.loadShaders("Shaders/shader.vert", "Shaders/shader.frag");

	glm::mat4 model, projection, view;
	glm::vec3 mPos(0, 0, 0);

	projection = m_camera.getProjectionMatrix();

	FpsCounter m_fpsCounter;
	glm::vec3 lightPos(0, 100, 100);

	//ChunkStuff
	ChunkManager m_chunkManager;
	m_chunkManager.init();

	Light m_light;
	m_light.init(glm::vec3(0, 0, 100), Color3f(1.0,0,0), 1, &m_glProgram);

	m_fpsCounter.start();

	OBBRayPicking rayPicker;

	std::vector<BlockClicked> m_clickedBlocks;
	BlockClicked m_falseBlock;
	m_falseBlock.chunkX = NUM_CHUNKS + 1;

	// This is the game loop
	while (!m_window.shouldWindowClose())
	{

		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
		glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
		m_inputManager.update();
		m_camera.update();
		// update input
		// handle input from user
		if (m_inputManager.isKeyPressed(KEYS::ESC))
		{
			m_window.setWindowClose();
		}

		if (m_inputManager.isKeyPressed(KEYS::W) || m_inputManager.isKeyHeldDown(KEYS::W)) ///< FORWARD
		{
			m_camera.applyMovement(cMove::Forward);
		}
		if (m_inputManager.isKeyPressed(KEYS::S) || m_inputManager.isKeyHeldDown(KEYS::S)) ///< BACKWARD
		{
			m_camera.applyMovement(cMove::Backward);
		}
		if (m_inputManager.isKeyPressed(KEYS::A) || m_inputManager.isKeyHeldDown(KEYS::A)) ///< LEFT
		{
			m_camera.applyMovement(cMove::Left);
		}
		if (m_inputManager.isKeyPressed(KEYS::D) || m_inputManager.isKeyHeldDown(KEYS::D)) ///< RIGHT
		{
			m_camera.applyMovement(cMove::Right);
		}
		if (m_inputManager.isKeyPressed(KEYS::Z) || m_inputManager.isKeyHeldDown(KEYS::Z)) ///<UP
		{
			m_camera.applyMovement(cMove::Up);
		}
		if (m_inputManager.isKeyPressed(KEYS::X) || m_inputManager.isKeyHeldDown(KEYS::X))///< DOWN
		{
			m_camera.applyMovement(cMove::Down);
		}
		if (m_inputManager.isKeyPressed(KEYS::Q) || m_inputManager.isKeyHeldDown(KEYS::Q))///< rotate left
		{
			m_camera.applyMovement(cMove::Rotate_Left);
		}
		if (m_inputManager.isKeyPressed(KEYS::E) || m_inputManager.isKeyHeldDown(KEYS::E))///< rotate left
		{
			m_camera.applyMovement(cMove::Rotate_Right);
		}
		if (m_inputManager.isKeyPressed(KEYS::F) || m_inputManager.isKeyHeldDown(KEYS::F))///< rotate left
		{
			m_camera.applyMovement(cMove::Roll_Left);
		}
		if (m_inputManager.isKeyPressed(KEYS::G) || m_inputManager.isKeyHeldDown(KEYS::G))///< rotate left
		{
			m_camera.applyMovement(cMove::Roll_Right);
		}
		if (m_inputManager.isKeyPressed(KEYS::C) || m_inputManager.isKeyHeldDown(KEYS::C))///< rotate left
		{
			m_camera.applyMovement(cMove::Pitch_Up);
		}
		if (m_inputManager.isKeyPressed(KEYS::V) || m_inputManager.isKeyHeldDown(KEYS::V))///< rotate left
		{
			m_camera.applyMovement(cMove::Pitch_Down);
		}
		if (m_inputManager.isKeyPressed(KEYS::UP) || m_inputManager.isKeyHeldDown(KEYS::UP))
		{
			m_camera.increaseSpeed();
		}
		if (m_inputManager.isKeyPressed(KEYS::DOWN) || m_inputManager.isKeyHeldDown(KEYS::DOWN))
		{
			m_camera.decreaseSpeed();
		}
		if (m_inputManager.isKeyPressed(KEYS::SPACE))
		{
			m_camera.setPosition(glm::vec3(0));
			m_camera.setDirection(glm::vec3(1, 0, 0));
			m_camera.setUpDir(glm::vec3(0, 1, 0));
			m_camera.setSpeed(0.1f);
		}
		if (m_inputManager.isKeyPressed(KEYS::R) || m_inputManager.isKeyHeldDown(KEYS::R))
		{
			m_chunkManager.setGenMethod(GEN_METHOD::RANDOM);
			m_clickedBlocks.clear();
		}
		if (m_inputManager.isKeyPressed(KEYS::T))
		{
			m_chunkManager.setGenMethod(GEN_METHOD::SPHERE);
			m_clickedBlocks.clear();

		}
		if (m_inputManager.isKeyPressed(KEYS::Y))
		{
			m_chunkManager.setGenMethod(GEN_METHOD::ALL);
			m_clickedBlocks.clear();

		}

		auto t_now = std::chrono::high_resolution_clock::now();
		float time = std::chrono::duration_cast<std::chrono::duration<float>>(t_now - t_start).count();

		glm::mat4 view = m_camera.getViewMatrix();
		glm::mat4 mvp = projection * view * model;

		if (m_inputManager.isKeyPressed(KEYS::MOUSE_RIGHT)|| m_inputManager.isKeyHeldDown(KEYS::MOUSE_RIGHT))
		{
			if (!m_clickedBlocks.empty())
			{
				m_chunkManager.setBlock(m_clickedBlocks.back(), true);
				m_clickedBlocks.pop_back();
			}
		}
		// ray picking

			bool intersected = false;
			BlockClicked bestCube;
			bestCube.distance = 10000.0f;
			glm::vec3 rayOrigin;
			glm::vec3 rayDirection;
			glm::vec3 aabbMax(BLOCK_WIDTH, BLOCK_WIDTH, BLOCK_WIDTH);
			glm::vec3 aabbMin(0.0, 0.0, 0.0);
			float distance;
			rayPicker.screenPosToWorldRay(m_inputManager.getCurrentMouseCoords().x, m_window.getHeight() - m_inputManager.getCurrentMouseCoords().y, m_window.getWidth(), m_window.getHeight(), view, projection, rayOrigin, rayDirection);
			bool procced = true;
			// THE SUPER LOOP
			// TODO: improve ray algorithem
			for (unsigned int chunkX = 0; chunkX < NUM_CHUNKS; chunkX++)
			{
				if (!procced)
				{
					break;
				}
				for (unsigned int chunkZ = 0; chunkZ < NUM_CHUNKS; chunkZ++)
				{
					if (!procced)
					{
						break;
					}
					if (!m_chunkManager.getActiveChunk(chunkX, chunkZ))
					{
						continue;
					}
					for (unsigned int blockX = 0; blockX < CHUNK_SIZE; blockX++)
					{
						if (!procced)
						{
							break;
						}
						for (unsigned int blockY = 0; blockY < CHUNK_SIZE; blockY++)
						{
							if (!procced)
							{
								break;
							}
							for (unsigned int blockZ = 0; blockZ < CHUNK_SIZE; blockZ++)
							{
								if (!m_chunkManager.getActiveBlock(chunkX, chunkZ, blockX, blockY, blockZ))
								{
									continue;
								}
								if (!procced)
								{
									break;
								}

								if (rayPicker.testRayOBBIntersection(rayOrigin, rayDirection,
									glm::vec3(chunkX * (CHUNK_SIZE ) + (blockX*BLOCK_WIDTH), (blockY*BLOCK_WIDTH), chunkZ * (CHUNK_SIZE ) + blockZ*BLOCK_WIDTH) + aabbMin,
									glm::vec3(chunkX * (CHUNK_SIZE ) + (blockX*BLOCK_WIDTH), (blockY*BLOCK_WIDTH), chunkZ * (CHUNK_SIZE ) + blockZ*BLOCK_WIDTH) + aabbMax, model,
									glm::vec3(chunkX * (CHUNK_SIZE ) + (blockX*BLOCK_WIDTH), (blockY*BLOCK_WIDTH), chunkZ * (CHUNK_SIZE ) + blockZ*BLOCK_WIDTH), distance))
								{

									if (bestCube.distance > distance)
									{
										bestCube.setChunk(chunkX, chunkZ);
										bestCube.setBlock(blockX, blockY, blockZ);
										bestCube.distance = distance;
										intersected = true;

									}

								}
							}
						}
					}
				}
			}
			if (intersected)
			{
				if (m_inputManager.isKeyPressed(KEYS::MOUSE_LEFT))
				{
					m_chunkManager.setBlock(bestCube.chunkX, bestCube.chunkZ, bestCube.blockX, bestCube.blockY, bestCube.blockZ, false);
					Debug_Log("CHOSEN");
					Debug_Log("INTERSECTION DISTANCE: " << bestCube.distance);
					Debug_Log("CHUNK IS: " << bestCube.chunkX << "," << bestCube.chunkZ);
					Debug_Log("BLOCK IS: " << bestCube.blockX << "," << bestCube.blockY << "," << bestCube.blockZ);
				}
		//		m_chunkManager.setBlock(bestCube.chunkX, bestCube.chunkZ, bestCube.blockX, bestCube.blockY, bestCube.blockZ, false);

				m_clickedBlocks.push_back(bestCube);
			}

	//	}
	//	glm::vec3 lightPos = glm::vec3(cosf(time / 2)* CHUNK_SIZE * NUM_CHUNKS * BLOCK_WIDTH * 1.1, sinf(time / 2) * CHUNK_SIZE * NUM_CHUNKS * BLOCK_WIDTH*1.1, CHUNK_SIZE * NUM_CHUNKS * BLOCK_WIDTH / 2);
		glm::vec3 lightPos(BLOCK_WIDTH*CHUNK_SIZE*NUM_CHUNKS / 4, 20, BLOCK_WIDTH*CHUNK_SIZE*NUM_CHUNKS /4);
		m_light.setPosition(lightPos);
		m_glProgram.uploadUniformMatrix("mvp", 1, mvp, GL_FALSE);
		m_glProgram.uploadUniformMatrix("m", 1, model, GL_FALSE);
		m_glProgram.uploadUniformMatrix("v", 1, view, GL_FALSE);

		m_light.update();

		m_chunkManager.update(m_camera.getPosition());

		m_drawBatch.start();
		if (!m_clickedBlocks.empty() )
		{
		//	m_drawBatch.draw(glm::vec3(m_clickedBlocks.back().chunkX * CHUNK_SIZE + m_clickedBlocks.back().blockX, m_clickedBlocks.back().blockY, m_clickedBlocks.back().chunkZ * CHUNK_SIZE + m_clickedBlocks.back().blockZ), glm::vec3(BLOCK_WIDTH), Color8(255, 255, 255, 255));
			if (intersected)
			{
				m_chunkManager.setCurrentClicked(m_clickedBlocks.back());
			}
			else
			{
				m_chunkManager.setCurrentClicked(m_falseBlock);
			}
		}
		if (m_clickedBlocks.size() > 60)
		{
			m_clickedBlocks.clear();
		}
		m_chunkManager.draw(&m_drawBatch);
		m_drawBatch.draw(glm::vec3(0, -0.1, 0), glm::vec3((CHUNK_SIZE + 1) * (NUM_CHUNKS), EPSILON * 100, (CHUNK_SIZE + 1) * (NUM_CHUNKS)), Color8(250, 214, 165, 255), true);
		m_drawBatch.draw(m_light.getPosition(), glm::vec3(10, 10, 10), Color8(255, 252, 127, 255));
		m_drawBatch.end();
		m_drawBatch.renderBatch();

		// update the window
		m_window.update();

		m_fpsCounter.end();
	}
	m_clickedBlocks.clear();
	//m_chunkManager.dispose(); TODO:FIX
	m_glProgram.deleteProgram();

	m_drawBatch.dispose();
	//Close the window
	m_chunkManager.dispose();
	m_window.dispose();

	glfwTerminate();
	return 0;
}
<|endoftext|>


<commit_before>/**
@file Client/Unit.hpp
@brief Unit.

@author Timothy Howard
@copyright 2013-2014 Timothy Howard under the MIT license;
see @ref index or the accompanying LICENSE file for full text.
*/

#ifndef ONSANG_CLIENT_UNIT_HPP_
#define ONSANG_CLIENT_UNIT_HPP_

#include <Onsang/config.hpp>
#include <Onsang/aux.hpp>
#include <Onsang/String.hpp>
#include <Onsang/asio.hpp>
#include <Onsang/ConfigNode.hpp>
#include <Onsang/Net/Defs.hpp>
#include <Onsang/Net/Session.hpp>

#include <Beard/ui/Context.hpp>
#include <Beard/ui/PropertyMap.hpp>

#include <Hord/System/Driver.hpp>

#include <duct/StateStore.hpp>

namespace Onsang {
namespace Client {

class Unit final {
public:
	using session_vector_type = aux::vector<Net::Session>;

private:
	enum class Flags : unsigned {
		no_auto_connect = 1u << 0,
	};

	duct::StateStore<Flags> m_flags;
	Hord::System::Driver m_driver;

	asio::io_service m_io_service;
	session_vector_type m_sessions;

	Beard::ui::Context m_ui_ctx;

	ConfigNode m_config;
	ConfigNode m_args;

	Unit(Unit const&) = delete;
	Unit& operator=(Unit const&) = delete;

public:
// constructors, destructor, and operators
	~Unit() = default;
	Unit(Unit&&) = default;
	Unit& operator=(Unit&&) = default;

	Unit()
		: m_flags()
		, m_driver()
		, m_io_service()
		, m_sessions()
		, m_ui_ctx(Beard::ui::PropertyMap{true})
		, m_config(
			{},
			{
				{"log", {ConfigNode::Flags::optional, {
					{"path", {
						{duct::VarType::string},
						ConfigNode::Flags::optional
					}}
				}}},
				{"term", {
					{"info", {
						{duct::VarType::string}
					}}
				}},
				{"sessions", {
					{"builder", {
						new ConfigNode({
							{"type", {
								{duct::VarType::string}
							}},
							{"addr", {
								{duct::VarType::string}
							}}
						}),
						ConfigNode::Flags::node_matcher_named
					}}
				}}
			}
		)
		, m_args({
			{"--config", {
				{duct::VarMask::value}
			}},
			{"--log", {
				{duct::VarMask::value},
				ConfigNode::Flags::optional
			}},
			{"--no-auto", {
				{duct::VarType::null},
				ConfigNode::Flags::optional
			}}
		})
	{}

// properties
	Hord::System::Driver&
	get_driver() noexcept {
		return m_driver;
	}

	session_vector_type const&
	get_sessions() const noexcept {
		return m_sessions;
	}

// operations
	// May throw Hord::Error
	void
	add_session(
		String name,
		String type,
		String addr
	);

	bool
	init(
		signed const argc,
		char* argv[]
	);

	void
	start();
};

} // namespace Client
} // namespace Onsang

#endif // ONSANG_CLIENT_UNIT_HPP_
<commit_msg>Client::Unit: updated m_driver initialization.¹<commit_after>/**
@file Client/Unit.hpp
@brief Unit.

@author Timothy Howard
@copyright 2013-2014 Timothy Howard under the MIT license;
see @ref index or the accompanying LICENSE file for full text.
*/

#ifndef ONSANG_CLIENT_UNIT_HPP_
#define ONSANG_CLIENT_UNIT_HPP_

#include <Onsang/config.hpp>
#include <Onsang/aux.hpp>
#include <Onsang/String.hpp>
#include <Onsang/asio.hpp>
#include <Onsang/ConfigNode.hpp>
#include <Onsang/Net/Defs.hpp>
#include <Onsang/Net/Session.hpp>

#include <Beard/ui/Context.hpp>
#include <Beard/ui/PropertyMap.hpp>

#include <Hord/System/Driver.hpp>

#include <duct/StateStore.hpp>

namespace Onsang {
namespace Client {

class Unit final {
public:
	using session_vector_type = aux::vector<Net::Session>;

private:
	enum class Flags : unsigned {
		no_auto_connect = 1u << 0,
	};

	duct::StateStore<Flags> m_flags;
	Hord::System::Driver m_driver;

	asio::io_service m_io_service;
	session_vector_type m_sessions;

	Beard::ui::Context m_ui_ctx;

	ConfigNode m_config;
	ConfigNode m_args;

	Unit(Unit const&) = delete;
	Unit& operator=(Unit const&) = delete;

public:
// constructors, destructor, and operators
	~Unit() = default;
	Unit(Unit&&) = default;
	Unit& operator=(Unit&&) = default;

	Unit()
		: m_flags()
		, m_driver(true)
		, m_io_service()
		, m_sessions()
		, m_ui_ctx(Beard::ui::PropertyMap{true})
		, m_config(
			{},
			{
				{"log", {ConfigNode::Flags::optional, {
					{"path", {
						{duct::VarType::string},
						ConfigNode::Flags::optional
					}}
				}}},
				{"term", {
					{"info", {
						{duct::VarType::string}
					}}
				}},
				{"sessions", {
					{"builder", {
						new ConfigNode({
							{"type", {
								{duct::VarType::string}
							}},
							{"addr", {
								{duct::VarType::string}
							}}
						}),
						ConfigNode::Flags::node_matcher_named
					}}
				}}
			}
		)
		, m_args({
			{"--config", {
				{duct::VarMask::value}
			}},
			{"--log", {
				{duct::VarMask::value},
				ConfigNode::Flags::optional
			}},
			{"--no-auto", {
				{duct::VarType::null},
				ConfigNode::Flags::optional
			}}
		})
	{}

// properties
	Hord::System::Driver&
	get_driver() noexcept {
		return m_driver;
	}

	session_vector_type const&
	get_sessions() const noexcept {
		return m_sessions;
	}

// operations
	// May throw Hord::Error
	void
	add_session(
		String name,
		String type,
		String addr
	);

	bool
	init(
		signed const argc,
		char* argv[]
	);

	void
	start();
};

} // namespace Client
} // namespace Onsang

#endif // ONSANG_CLIENT_UNIT_HPP_
<|endoftext|>


<commit_before>/****************************************************************************
 * Copyright (c) 2012-2017 by the DataTransferKit authors                   *
 * All rights reserved.                                                     *
 *                                                                          *
 * This file is part of the DataTransferKit library. DataTransferKit is     *
 * distributed under a BSD 3-clause license. For the licensing terms see    *
 * the LICENSE file in the top-level directory.                             *
 ****************************************************************************/
#ifndef DTK_DETAILS_TREE_TRAVERSAL_HPP
#define DTK_DETAILS_TREE_TRAVERSAL_HPP

#include <DTK_DBC.hpp>

#include <DTK_DetailsAlgorithms.hpp>
#include <DTK_DetailsNode.hpp>
#include <DTK_DetailsPredicate.hpp>
#include <DTK_DetailsPriorityQueue.hpp>
#include <DTK_DetailsStack.hpp>

#include <DTK_LinearBVH.hpp>

namespace DataTransferKit
{
namespace Details
{
template <typename DeviceType>
struct TreeTraversal
{
  public:
    using ExecutionSpace = typename DeviceType::execution_space;

    template <typename Predicate, typename Insert>
    KOKKOS_INLINE_FUNCTION static int query( BVH<DeviceType> const bvh,
                                             Predicate const &pred,
                                             Insert const &insert )
    {
        using Tag = typename Predicate::Tag;
        return query_dispatch( bvh, pred, insert, Tag{} );
    }

    /**
     * Return true if the node is a leaf.
     */
    KOKKOS_INLINE_FUNCTION
    static bool isLeaf( BVH<DeviceType> bvh, Node const *node )
    {
        // COMMENT: could also check that pointer is in the range [leaf_nodes,
        // leaf_nodes+n]
        (void)bvh;
        return ( node->children.first == nullptr ) &&
               ( node->children.second == nullptr );
    }

    /**
     * Return the index of the leaf node.
     */
    KOKKOS_INLINE_FUNCTION
    static int getIndex( BVH<DeviceType> bvh, Node const *leaf )
    {
        return bvh._indices[leaf - bvh._leaf_nodes.data()];
    }

    /**
     * Return the root node of the BVH.
     */
    KOKKOS_INLINE_FUNCTION
    static Node const *getRoot( BVH<DeviceType> bvh )
    {
        return bvh._internal_nodes.data();
    }
};

// There are two (related) families of search: one using a spatial predicate and
// one using nearest neighbours query (see boost::geometry::queries
// documentation).
template <typename DeviceType, typename Predicate, typename Insert>
KOKKOS_FUNCTION int spatial_query( BVH<DeviceType> const bvh,
                                   Predicate const &predicate,
                                   Insert const &insert )
{
    Stack<Node const *> stack;

    Node const *node = TreeTraversal<DeviceType>::getRoot( bvh );
    stack.push( node );
    int count = 0;

    while ( !stack.empty() )
    {
        node = stack.top();
        stack.pop();

        if ( TreeTraversal<DeviceType>::isLeaf( bvh, node ) )
        {
            insert( TreeTraversal<DeviceType>::getIndex( bvh, node ) );
            count++;
        }
        else
        {
            for ( Node const *child :
                  {node->children.first, node->children.second} )
            {
                if ( predicate( child ) )
                {
                    stack.push( child );
                }
            }
        }
    }
    return count;
}

// query k nearest neighbours
template <typename DeviceType, typename Insert>
KOKKOS_FUNCTION int nearest_query( BVH<DeviceType> const bvh,
                                   Point const &query_point, int k,
                                   Insert const &insert )
{
    using PairNodePtrDistance = Kokkos::pair<Node const *, double>;

    struct CompareDistance
    {
        KOKKOS_INLINE_FUNCTION bool operator()( PairNodePtrDistance const &lhs,
                                                PairNodePtrDistance const &rhs )
        {
            // reverse order (larger distance means lower priority)
            return lhs.second > rhs.second;
        }
    };

    PriorityQueue<PairNodePtrDistance, CompareDistance> queue;
    // priority does not matter for the root since the node will be
    // processed directly and removed from the priority queue we don't even
    // bother computing the distance to it
    Node const *node = TreeTraversal<DeviceType>::getRoot( bvh );
    double node_distance = 0.0;
    queue.push( node, node_distance );
    int count = 0;

    while ( !queue.empty() && count < k )
    {
        // get the node that is on top of the priority list (i.e. is the
        // closest to the query point)
        node = queue.top().first; // std::tie(node, std::ignore) = ...
        queue.pop();
        if ( TreeTraversal<DeviceType>::isLeaf( bvh, node ) )
        {
            insert( TreeTraversal<DeviceType>::getIndex( bvh, node ) );
            count++;
        }
        else
        {
            // insert children of the node in the priority list
            for ( Node const *child :
                  {node->children.first, node->children.second} )
            {
                double child_distance =
                    distance( query_point, child->bounding_box );
                queue.push( child, child_distance );
            }
        }
    }
    return count;
}

template <typename DeviceType, typename Predicate, typename Insert>
KOKKOS_INLINE_FUNCTION int
query_dispatch( BVH<DeviceType> const bvh, Predicate const &pred,
                Insert const &insert, SpatialPredicateTag )
{
    return spatial_query( bvh, pred, insert );
}

template <typename DeviceType, typename Predicate, typename Insert>
KOKKOS_INLINE_FUNCTION int
query_dispatch( BVH<DeviceType> const bvh, Predicate const &pred,
                Insert const &insert, NearestPredicateTag )
{
    return nearest_query( bvh, pred._query_point, pred._k, insert );
}

} // end namespace Details
} // end namespace DataTransferKit

#endif
<commit_msg>Enable spatial and nearest queries for tree with less than two leaves<commit_after>/****************************************************************************
 * Copyright (c) 2012-2017 by the DataTransferKit authors                   *
 * All rights reserved.                                                     *
 *                                                                          *
 * This file is part of the DataTransferKit library. DataTransferKit is     *
 * distributed under a BSD 3-clause license. For the licensing terms see    *
 * the LICENSE file in the top-level directory.                             *
 ****************************************************************************/
#ifndef DTK_DETAILS_TREE_TRAVERSAL_HPP
#define DTK_DETAILS_TREE_TRAVERSAL_HPP

#include <DTK_DBC.hpp>

#include <DTK_DetailsAlgorithms.hpp>
#include <DTK_DetailsNode.hpp>
#include <DTK_DetailsPredicate.hpp>
#include <DTK_DetailsPriorityQueue.hpp>
#include <DTK_DetailsStack.hpp>

#include <DTK_LinearBVH.hpp>

namespace DataTransferKit
{
namespace Details
{
template <typename DeviceType>
struct TreeTraversal
{
  public:
    using ExecutionSpace = typename DeviceType::execution_space;

    template <typename Predicate, typename Insert>
    KOKKOS_INLINE_FUNCTION static int query( BVH<DeviceType> const bvh,
                                             Predicate const &pred,
                                             Insert const &insert )
    {
        using Tag = typename Predicate::Tag;
        return query_dispatch( bvh, pred, insert, Tag{} );
    }

    /**
     * Return true if the node is a leaf.
     */
    KOKKOS_INLINE_FUNCTION
    static bool isLeaf( BVH<DeviceType> bvh, Node const *node )
    {
        // COMMENT: could also check that pointer is in the range [leaf_nodes,
        // leaf_nodes+n]
        (void)bvh;
        return ( node->children.first == nullptr ) &&
               ( node->children.second == nullptr );
    }

    /**
     * Return the index of the leaf node.
     */
    KOKKOS_INLINE_FUNCTION
    static int getIndex( BVH<DeviceType> bvh, Node const *leaf )
    {
        return bvh._indices[leaf - bvh._leaf_nodes.data()];
    }

    /**
     * Return the root node of the BVH.
     */
    KOKKOS_INLINE_FUNCTION
    static Node const *getRoot( BVH<DeviceType> bvh )
    {
        if ( bvh._internal_nodes.extent( 0 ) > 0 )
            return bvh._internal_nodes.data();
        else
            return bvh._leaf_nodes.data();
    }
};

// There are two (related) families of search: one using a spatial predicate and
// one using nearest neighbours query (see boost::geometry::queries
// documentation).
template <typename DeviceType, typename Predicate, typename Insert>
KOKKOS_FUNCTION int spatial_query( BVH<DeviceType> const bvh,
                                   Predicate const &predicate,
                                   Insert const &insert )
{
    if ( bvh.empty() )
        return 0;

    Stack<Node const *> stack;

    Node const *node = TreeTraversal<DeviceType>::getRoot( bvh );
    // need to actually check that the node verifies the predicate because
    // getRoot directly returns the leaf node in the case there is only one in
    // the tree.
    if ( predicate( node ) )
        stack.push( node );
    int count = 0;

    while ( !stack.empty() )
    {
        node = stack.top();
        stack.pop();

        if ( TreeTraversal<DeviceType>::isLeaf( bvh, node ) )
        {
            insert( TreeTraversal<DeviceType>::getIndex( bvh, node ) );
            count++;
        }
        else
        {
            for ( Node const *child :
                  {node->children.first, node->children.second} )
            {
                if ( predicate( child ) )
                {
                    stack.push( child );
                }
            }
        }
    }
    return count;
}

// query k nearest neighbours
template <typename DeviceType, typename Insert>
KOKKOS_FUNCTION int nearest_query( BVH<DeviceType> const bvh,
                                   Point const &query_point, int k,
                                   Insert const &insert )
{
    if ( bvh.empty() )
        return 0;

    using PairNodePtrDistance = Kokkos::pair<Node const *, double>;

    struct CompareDistance
    {
        KOKKOS_INLINE_FUNCTION bool operator()( PairNodePtrDistance const &lhs,
                                                PairNodePtrDistance const &rhs )
        {
            // reverse order (larger distance means lower priority)
            return lhs.second > rhs.second;
        }
    };

    PriorityQueue<PairNodePtrDistance, CompareDistance> queue;
    // priority does not matter for the root since the node will be
    // processed directly and removed from the priority queue we don't even
    // bother computing the distance to it
    Node const *node = TreeTraversal<DeviceType>::getRoot( bvh );
    double node_distance = 0.0;
    queue.push( node, node_distance );
    int count = 0;

    while ( !queue.empty() && count < k )
    {
        // get the node that is on top of the priority list (i.e. is the
        // closest to the query point)
        node = queue.top().first; // std::tie(node, std::ignore) = ...
        queue.pop();
        if ( TreeTraversal<DeviceType>::isLeaf( bvh, node ) )
        {
            insert( TreeTraversal<DeviceType>::getIndex( bvh, node ) );
            count++;
        }
        else
        {
            // insert children of the node in the priority list
            for ( Node const *child :
                  {node->children.first, node->children.second} )
            {
                double child_distance =
                    distance( query_point, child->bounding_box );
                queue.push( child, child_distance );
            }
        }
    }
    return count;
}

template <typename DeviceType, typename Predicate, typename Insert>
KOKKOS_INLINE_FUNCTION int
query_dispatch( BVH<DeviceType> const bvh, Predicate const &pred,
                Insert const &insert, SpatialPredicateTag )
{
    return spatial_query( bvh, pred, insert );
}

template <typename DeviceType, typename Predicate, typename Insert>
KOKKOS_INLINE_FUNCTION int
query_dispatch( BVH<DeviceType> const bvh, Predicate const &pred,
                Insert const &insert, NearestPredicateTag )
{
    return nearest_query( bvh, pred._query_point, pred._k, insert );
}

} // end namespace Details
} // end namespace DataTransferKit

#endif
<|endoftext|>


<commit_before>#include "Players/Genetic_AI.h"

#include <iostream>
#include <fstream>
#include <cmath>
#include <algorithm>
#include <stdexcept>
#include <cassert>

#include "Players/Player.h"
#include "Moves/Move.h"
#include "Game/Board.h"
#include "Game/Clock.h"
#include "Game/Game_Result.h"
#include "Players/Thinking.h"
#include "Players/Game_Tree_Node_Result.h"

#include "Utility.h"

int Genetic_AI::next_id = 0;

Genetic_AI::Genetic_AI() :
    genome(),
    id(next_id++)
{
    calibrate_thinking_speed();
}

// Sexual reproduction
Genetic_AI::Genetic_AI(const Genetic_AI& A, const Genetic_AI& B) :
    genome(A.genome, B.genome),
    id(next_id++)
{
    calibrate_thinking_speed();
}

Genetic_AI::Genetic_AI(const std::string& file_name)
{
    std::ifstream ifs(file_name);
    if( ! ifs)
    {
        throw std::runtime_error("Could not read: " + file_name);
    }

    read_from(ifs);

    calibrate_thinking_speed();
}

Genetic_AI::Genetic_AI(std::istream& is)
{
    read_from(is);

    calibrate_thinking_speed();
}

Genetic_AI::Genetic_AI(const std::string& file_name, int id_in) : id(id_in)
{
    if(id >= next_id)
    {
        next_id = id + 1;
    }

    std::ifstream ifs(file_name);
    if( ! ifs)
    {
        throw std::runtime_error("Could not read: " + file_name);
    }

    std::string line;
    while(std::getline(ifs, line))
    {
        if( ! String::starts_with(line, "ID:"))
        {
            continue;
        }

        auto param_value = String::split(line, ":", 1);
        if(id_in == std::stoi(param_value[1]))
        {
            genome.read_from(ifs);

            calibrate_thinking_speed();

            return;
        }
    }

    throw std::runtime_error("Could not locate ID " + std::to_string(id_in) + " inside file " + file_name);
}

void Genetic_AI::read_from(std::istream& is)
{
    std::string line;
    id = -1;
    while(std::getline(is, line))
    {
        if(line.empty())
        {
            continue;
        }

        if(String::starts_with(line, "ID:"))
        {
            id = std::stoi(String::split(line)[1]);
            break;
        }
        else
        {
            throw std::runtime_error("Invalid Genetic_AI line: " + line);
        }
    }

    if( ! is)
    {
        throw std::runtime_error("Incomplete Genetic_AI spec in file for ID " + std::to_string(id));
    }

    if(id >= next_id)
    {
        next_id = id + 1;
    }

    genome.read_from(is);
}

const Move& Genetic_AI::choose_move(const Board& board, const Clock& clock) const
{
    // Erase data from previous board when starting new game
    if(board.get_game_record().size() <= 1)
    {
        principal_variation.clear();
        commentary.clear();
    }

    nodes_searched = 0;
    clock_start_time = clock.time_left(clock.running_for());
    maximum_depth = 0;

    const auto& legal_moves = board.legal_moves();
    if(legal_moves.size() == 1)
    {
        if(principal_variation.size() > 2 && principal_variation[1] == board.get_game_record().back())
        {
            // search_game_tree() assumes the principal variation starts
            // with the previous move of this player. If a move was forced,
            // then the principal variation needs to be updated to start with
            // the next move of this side after checking that the immediately
            // preceding move was the expected one.
            principal_variation.erase(principal_variation.begin(),
                                      principal_variation.begin() + 2);
        }
        else
        {
            principal_variation.clear();
        }

        commentary.push_back(principal_variation);

        return *legal_moves.front(); // If there's only one legal move, take it.
    }

    auto time_to_use = genome.time_to_examine(board, clock);

    // alpha = highest score found that opponent will allow
    Game_Tree_Node_Result alpha_start = {Math::lose_score,
                                         board.whose_turn(),
                                         0,
                                         {}};

    // beta = score that will cause opponent to choose a different prior move
    Game_Tree_Node_Result beta_start = {Math::win_score,
                                        board.whose_turn(),
                                        0,
                                        {}};

    auto result = search_game_tree(board,
                                   time_to_use,
                                   clock,
                                   0,
                                   alpha_start,
                                   beta_start);

    if(board.get_thinking_mode() == CECP)
    {
        output_thinking_cecp(result, clock, board.whose_turn());
        std::cout << std::flush;
    }

    if(result.depth > 0)
    {
        principal_variation = result.variation;
    }
    else
    {
        principal_variation.clear();
    }
    commentary.push_back(principal_variation);

    positions_per_second = nodes_searched/(clock_start_time - clock.time_left(clock.running_for()));

    return *result.variation.front();
}

Game_Tree_Node_Result Genetic_AI::search_game_tree(const Board& board,
                                                   const double time_to_examine,
                                                   const Clock& clock,
                                                   const size_t depth,
                                                   Game_Tree_Node_Result alpha,
                                                   Game_Tree_Node_Result beta) const
{
    auto time_start = clock.time_left(clock.running_for());
    maximum_depth = std::max(maximum_depth, depth);
    auto all_legal_moves = board.legal_moves();

    // The first item in the principal variation is the last move that
    // this player made. Since then, the opponent has also made a move,
    // so the first item in the principal variation corresponds to the
    // game record item at game_record.size() - 2. The depth of the game
    // tree search increments both the game_record index and the principal
    // variation index in step.
    bool still_on_principal_variation = false;
    if(principal_variation.size() > depth + 2)
    {
        still_on_principal_variation = std::equal(board.get_game_record().end() - 2 - depth,
                                                  board.get_game_record().end(),
                                                  principal_variation.begin());

        if(still_on_principal_variation)
        {
            auto next_principal_variation_move = principal_variation[depth + 2];
            auto move_iter = std::find(all_legal_moves.begin(),
                                       all_legal_moves.end(),
                                       next_principal_variation_move);

            assert(move_iter != all_legal_moves.end());

            // Put principal variation move at start of list to allow
            // the most pruning later.
            std::iter_swap(all_legal_moves.begin(), move_iter);
        }
    }

    auto perspective = board.whose_turn();
    auto moves_left = all_legal_moves.size();

    Game_Tree_Node_Result best_result = {Math::lose_score,
                                         perspective,
                                         depth,
                                         {all_legal_moves.front()}};

    for(const auto& move : all_legal_moves)
    {
        ++nodes_searched;

        auto next_board = board;

        auto move_result = next_board.submit_move(*move);
        if(move_result.get_winner() != NONE)
        {
            // Mate in one (try to pick the shortest path to checkmate)
            return {genome.evaluate(next_board, move_result, perspective),
                    perspective,
                    depth,
                    {next_board.get_game_record().end() - (depth + 1),
                     next_board.get_game_record().end()}};
        }

        if(alpha.depth <= depth + 2 && alpha.corrected_score(perspective) == Math::win_score)
        {
            // This move will take a longer path to victory
            // than one already found. Use "depth + 2" since,
            // if this move isn't winning (and it isn't, since
            // we're here), then the earliest move that can
            // win is the next one, which is two away (after
            // opponent's move).
            continue;
        }

        double time_left = time_to_examine - (time_start - clock.time_left(clock.running_for()));
        double time_allotted_for_this_move = time_left/moves_left;

        bool recurse;
        if(move_result.game_has_ended())
        {
            recurse = false;
        }
        else if(next_board.legal_moves().size() == 1)
        {
            recurse = true;
        }
        else if(still_on_principal_variation)
        {
            recurse = true;
        }
        else
        {
            recurse = genome.enough_time_to_recurse(time_allotted_for_this_move, next_board, positions_per_second);
        }

        Game_Tree_Node_Result result;
        if(recurse)
        {
            result = search_game_tree(next_board,
                                      time_allotted_for_this_move,
                                      clock,
                                      depth + 1,
                                      beta,
                                      alpha);
        }
        else
        {
            // Record immediate result without looking ahead further
            result = {genome.evaluate(next_board, move_result, perspective),
                      perspective,
                      depth,
                      {next_board.get_game_record().end() - (depth + 1),
                       next_board.get_game_record().end()}};
        }

        if(better_than(result, best_result, perspective))
        {
            best_result = result;

            if(better_than(best_result, alpha, perspective))
            {
                alpha = best_result;
                if(better_than(alpha, beta, perspective) || alpha == beta)
                {
                    break;
                }
                else if(depth % 2 == 0 && board.get_thinking_mode() == CECP)
                {
                    output_thinking_cecp(alpha, clock, perspective);
                }
            }
        }

        --moves_left;
        still_on_principal_variation = false; // only the first move is part of the principal variation

        if(clock.time_left(clock.running_for()) < 0)
        {
            break;
        }
    }

    return best_result;
}

void Genetic_AI::mutate()
{
    genome.mutate();
}

void Genetic_AI::print_genome(const std::string& file_name) const
{
    if(file_name.empty())
    {
        print_genome(std::cout);
    }
    else
    {
        auto dest = std::ofstream(file_name, std::ofstream::app);
        print_genome(dest);
    }
}

void Genetic_AI::print_genome(std::ostream& os) const
{
    os << "ID: " << get_id() << std::endl;
    genome.print(os);
    os << "END" << std::endl << std::endl;
}

std::string Genetic_AI::name() const
{
    return "Genetic AI " + std::to_string(get_id());
}

std::string Genetic_AI::author() const
{
    return "Mark Harrison";
}

int Genetic_AI::get_id() const
{
    return id;
}

bool Genetic_AI::operator<(const Genetic_AI& other) const
{
    return get_id() < other.get_id();
}

bool Genetic_AI::operator==(const Genetic_AI& other) const
{
    return get_id() == other.get_id();
}

std::string Genetic_AI::get_commentary_for_move(size_t move_number) const
{
    std::string result;
    if(move_number < commentary.size() && ! commentary.at(move_number).empty())
    {
        result = commentary.at(move_number).front()->coordinate_move();
        for(size_t i = 1; i < commentary.at(move_number).size(); ++i)
        {
            result += " " + commentary.at(move_number).at(i)->coordinate_move();
        }
    }

    return result;
}

void Genetic_AI::output_thinking_cecp(const Game_Tree_Node_Result& thought,
                                      const Clock& clock,
                                      Color perspective) const
{
    auto score = thought.corrected_score(perspective);

    // Indicate "mate in N moves" where N == thought.depth
    if(score == Math::win_score)
    {
        score = 10000.0 - thought.depth;
    }
    else if(score == Math::lose_score)
    {
        score = -(10000.0 - thought.depth);
    }

    auto time_so_far = clock_start_time - clock.time_left(clock.running_for());
    std::cout << thought.depth // ply
              << " "
              << int(score) // score in what should be centipawns
              << " "
              << int(time_so_far*100) // search time in centiseconds
              << " "
              << nodes_searched
              << " "
              << maximum_depth
              << " "
              << int(nodes_searched/time_so_far)
              << '\t';

    // Principal variation
    for(const auto& move : thought.variation)
    {
        std::cout << move->coordinate_move() << ' ';
    }

    std::cout << '\n';
}

void Genetic_AI::calibrate_thinking_speed()
{
    positions_per_second = 100; // very conservative initial guess
    auto calibration_time = 1.0; // seconds
    Board board;
    Clock clock(calibration_time, 1, 0.0);
    clock.start();
    choose_move(board, clock);
    // choose_move() keeps track of the time it takes and the number of positions
    // it sees, so this practice move will update the positions_per_second to a
    // more reasonable value.
}

<commit_msg>Don't record principal variation unless it looks beyond opponent's move<commit_after>#include "Players/Genetic_AI.h"

#include <iostream>
#include <fstream>
#include <cmath>
#include <algorithm>
#include <stdexcept>
#include <cassert>

#include "Players/Player.h"
#include "Moves/Move.h"
#include "Game/Board.h"
#include "Game/Clock.h"
#include "Game/Game_Result.h"
#include "Players/Thinking.h"
#include "Players/Game_Tree_Node_Result.h"

#include "Utility.h"

int Genetic_AI::next_id = 0;

Genetic_AI::Genetic_AI() :
    genome(),
    id(next_id++)
{
    calibrate_thinking_speed();
}

// Sexual reproduction
Genetic_AI::Genetic_AI(const Genetic_AI& A, const Genetic_AI& B) :
    genome(A.genome, B.genome),
    id(next_id++)
{
    calibrate_thinking_speed();
}

Genetic_AI::Genetic_AI(const std::string& file_name)
{
    std::ifstream ifs(file_name);
    if( ! ifs)
    {
        throw std::runtime_error("Could not read: " + file_name);
    }

    read_from(ifs);

    calibrate_thinking_speed();
}

Genetic_AI::Genetic_AI(std::istream& is)
{
    read_from(is);

    calibrate_thinking_speed();
}

Genetic_AI::Genetic_AI(const std::string& file_name, int id_in) : id(id_in)
{
    if(id >= next_id)
    {
        next_id = id + 1;
    }

    std::ifstream ifs(file_name);
    if( ! ifs)
    {
        throw std::runtime_error("Could not read: " + file_name);
    }

    std::string line;
    while(std::getline(ifs, line))
    {
        if( ! String::starts_with(line, "ID:"))
        {
            continue;
        }

        auto param_value = String::split(line, ":", 1);
        if(id_in == std::stoi(param_value[1]))
        {
            genome.read_from(ifs);

            calibrate_thinking_speed();

            return;
        }
    }

    throw std::runtime_error("Could not locate ID " + std::to_string(id_in) + " inside file " + file_name);
}

void Genetic_AI::read_from(std::istream& is)
{
    std::string line;
    id = -1;
    while(std::getline(is, line))
    {
        if(line.empty())
        {
            continue;
        }

        if(String::starts_with(line, "ID:"))
        {
            id = std::stoi(String::split(line)[1]);
            break;
        }
        else
        {
            throw std::runtime_error("Invalid Genetic_AI line: " + line);
        }
    }

    if( ! is)
    {
        throw std::runtime_error("Incomplete Genetic_AI spec in file for ID " + std::to_string(id));
    }

    if(id >= next_id)
    {
        next_id = id + 1;
    }

    genome.read_from(is);
}

const Move& Genetic_AI::choose_move(const Board& board, const Clock& clock) const
{
    // Erase data from previous board when starting new game
    if(board.get_game_record().size() <= 1)
    {
        principal_variation.clear();
        commentary.clear();
    }

    nodes_searched = 0;
    clock_start_time = clock.time_left(clock.running_for());
    maximum_depth = 0;

    const auto& legal_moves = board.legal_moves();
    if(legal_moves.size() == 1)
    {
        if(principal_variation.size() > 2 && principal_variation[1] == board.get_game_record().back())
        {
            // search_game_tree() assumes the principal variation starts
            // with the previous move of this player. If a move was forced,
            // then the principal variation needs to be updated to start with
            // the next move of this side after checking that the immediately
            // preceding move was the expected one.
            principal_variation.erase(principal_variation.begin(),
                                      principal_variation.begin() + 2);
        }
        else
        {
            principal_variation.clear();
        }

        commentary.push_back(principal_variation);

        return *legal_moves.front(); // If there's only one legal move, take it.
    }

    auto time_to_use = genome.time_to_examine(board, clock);

    // alpha = highest score found that opponent will allow
    Game_Tree_Node_Result alpha_start = {Math::lose_score,
                                         board.whose_turn(),
                                         0,
                                         {}};

    // beta = score that will cause opponent to choose a different prior move
    Game_Tree_Node_Result beta_start = {Math::win_score,
                                        board.whose_turn(),
                                        0,
                                        {}};

    auto result = search_game_tree(board,
                                   time_to_use,
                                   clock,
                                   0,
                                   alpha_start,
                                   beta_start);

    if(board.get_thinking_mode() == CECP)
    {
        output_thinking_cecp(result, clock, board.whose_turn());
        std::cout << std::flush;
    }

    if(result.depth > 0)
    {
        commentary.push_back(result.variation);
    }
    else
    {
        commentary.push_back({});
    }

    if(result.depth > 1)
    {
        principal_variation = result.variation;
    }
    else
    {
        principal_variation.clear();
    }

    positions_per_second = nodes_searched/(clock_start_time - clock.time_left(clock.running_for()));

    return *result.variation.front();
}

Game_Tree_Node_Result Genetic_AI::search_game_tree(const Board& board,
                                                   const double time_to_examine,
                                                   const Clock& clock,
                                                   const size_t depth,
                                                   Game_Tree_Node_Result alpha,
                                                   Game_Tree_Node_Result beta) const
{
    auto time_start = clock.time_left(clock.running_for());
    maximum_depth = std::max(maximum_depth, depth);
    auto all_legal_moves = board.legal_moves();

    // The first item in the principal variation is the last move that
    // this player made. Since then, the opponent has also made a move,
    // so the first item in the principal variation corresponds to the
    // game record item at game_record.size() - 2. The depth of the game
    // tree search increments both the game_record index and the principal
    // variation index in step.
    bool still_on_principal_variation = false;
    if(principal_variation.size() > depth + 2)
    {
        still_on_principal_variation = std::equal(board.get_game_record().end() - 2 - depth,
                                                  board.get_game_record().end(),
                                                  principal_variation.begin());

        if(still_on_principal_variation)
        {
            auto next_principal_variation_move = principal_variation[depth + 2];
            auto move_iter = std::find(all_legal_moves.begin(),
                                       all_legal_moves.end(),
                                       next_principal_variation_move);

            assert(move_iter != all_legal_moves.end());

            // Put principal variation move at start of list to allow
            // the most pruning later.
            std::iter_swap(all_legal_moves.begin(), move_iter);
        }
    }

    auto perspective = board.whose_turn();
    auto moves_left = all_legal_moves.size();

    Game_Tree_Node_Result best_result = {Math::lose_score,
                                         perspective,
                                         depth,
                                         {all_legal_moves.front()}};

    for(const auto& move : all_legal_moves)
    {
        ++nodes_searched;

        auto next_board = board;

        auto move_result = next_board.submit_move(*move);
        if(move_result.get_winner() != NONE)
        {
            // Mate in one (try to pick the shortest path to checkmate)
            return {genome.evaluate(next_board, move_result, perspective),
                    perspective,
                    depth,
                    {next_board.get_game_record().end() - (depth + 1),
                     next_board.get_game_record().end()}};
        }

        if(alpha.depth <= depth + 2 && alpha.corrected_score(perspective) == Math::win_score)
        {
            // This move will take a longer path to victory
            // than one already found. Use "depth + 2" since,
            // if this move isn't winning (and it isn't, since
            // we're here), then the earliest move that can
            // win is the next one, which is two away (after
            // opponent's move).
            continue;
        }

        double time_left = time_to_examine - (time_start - clock.time_left(clock.running_for()));
        double time_allotted_for_this_move = time_left/moves_left;

        bool recurse;
        if(move_result.game_has_ended())
        {
            recurse = false;
        }
        else if(next_board.legal_moves().size() == 1)
        {
            recurse = true;
        }
        else if(still_on_principal_variation)
        {
            recurse = true;
        }
        else
        {
            recurse = genome.enough_time_to_recurse(time_allotted_for_this_move, next_board, positions_per_second);
        }

        Game_Tree_Node_Result result;
        if(recurse)
        {
            result = search_game_tree(next_board,
                                      time_allotted_for_this_move,
                                      clock,
                                      depth + 1,
                                      beta,
                                      alpha);
        }
        else
        {
            // Record immediate result without looking ahead further
            result = {genome.evaluate(next_board, move_result, perspective),
                      perspective,
                      depth,
                      {next_board.get_game_record().end() - (depth + 1),
                       next_board.get_game_record().end()}};
        }

        if(better_than(result, best_result, perspective))
        {
            best_result = result;

            if(better_than(best_result, alpha, perspective))
            {
                alpha = best_result;
                if(better_than(alpha, beta, perspective) || alpha == beta)
                {
                    break;
                }
                else if(depth % 2 == 0 && board.get_thinking_mode() == CECP)
                {
                    output_thinking_cecp(alpha, clock, perspective);
                }
            }
        }

        --moves_left;
        still_on_principal_variation = false; // only the first move is part of the principal variation

        if(clock.time_left(clock.running_for()) < 0)
        {
            break;
        }
    }

    return best_result;
}

void Genetic_AI::mutate()
{
    genome.mutate();
}

void Genetic_AI::print_genome(const std::string& file_name) const
{
    if(file_name.empty())
    {
        print_genome(std::cout);
    }
    else
    {
        auto dest = std::ofstream(file_name, std::ofstream::app);
        print_genome(dest);
    }
}

void Genetic_AI::print_genome(std::ostream& os) const
{
    os << "ID: " << get_id() << std::endl;
    genome.print(os);
    os << "END" << std::endl << std::endl;
}

std::string Genetic_AI::name() const
{
    return "Genetic AI " + std::to_string(get_id());
}

std::string Genetic_AI::author() const
{
    return "Mark Harrison";
}

int Genetic_AI::get_id() const
{
    return id;
}

bool Genetic_AI::operator<(const Genetic_AI& other) const
{
    return get_id() < other.get_id();
}

bool Genetic_AI::operator==(const Genetic_AI& other) const
{
    return get_id() == other.get_id();
}

std::string Genetic_AI::get_commentary_for_move(size_t move_number) const
{
    std::string result;
    if(move_number < commentary.size() && ! commentary.at(move_number).empty())
    {
        result = commentary.at(move_number).front()->coordinate_move();
        for(size_t i = 1; i < commentary.at(move_number).size(); ++i)
        {
            result += " " + commentary.at(move_number).at(i)->coordinate_move();
        }
    }

    return result;
}

void Genetic_AI::output_thinking_cecp(const Game_Tree_Node_Result& thought,
                                      const Clock& clock,
                                      Color perspective) const
{
    auto score = thought.corrected_score(perspective);

    // Indicate "mate in N moves" where N == thought.depth
    if(score == Math::win_score)
    {
        score = 10000.0 - thought.depth;
    }
    else if(score == Math::lose_score)
    {
        score = -(10000.0 - thought.depth);
    }

    auto time_so_far = clock_start_time - clock.time_left(clock.running_for());
    std::cout << thought.depth // ply
              << " "
              << int(score) // score in what should be centipawns
              << " "
              << int(time_so_far*100) // search time in centiseconds
              << " "
              << nodes_searched
              << " "
              << maximum_depth
              << " "
              << int(nodes_searched/time_so_far)
              << '\t';

    // Principal variation
    for(const auto& move : thought.variation)
    {
        std::cout << move->coordinate_move() << ' ';
    }

    std::cout << '\n';
}

void Genetic_AI::calibrate_thinking_speed()
{
    positions_per_second = 100; // very conservative initial guess
    auto calibration_time = 1.0; // seconds
    Board board;
    Clock clock(calibration_time, 1, 0.0);
    clock.start();
    choose_move(board, clock);
    // choose_move() keeps track of the time it takes and the number of positions
    // it sees, so this practice move will update the positions_per_second to a
    // more reasonable value.
}

<|endoftext|>