<commit_before>
#include "sqlhelper.h"
#include <iostream>
#include <string>

namespace hsql {

  void printOperatorExpression(Expr* expr, uintmax_t numIndent);

  std::string indent(uintmax_t numIndent) {
    return std::string(numIndent, '\t');
  }
  void inprint(int64_t val, uintmax_t numIndent) {
    std::cout << indent(numIndent).c_str() << val << "  " << std::endl;
  }
  void inprint(float val, uintmax_t numIndent) {
    std::cout << indent(numIndent).c_str() << val << std::endl;
  }
  void inprint(const char* val, uintmax_t numIndent) {
    std::cout << indent(numIndent).c_str() << val << std::endl;
  }
  void inprint(const char* val, const char* val2, uintmax_t numIndent) {
    std::cout << indent(numIndent).c_str() << val << "->" << val2 << std::endl;
  }
  void inprintC(char val, uintmax_t numIndent) {
    std::cout << indent(numIndent).c_str() << val << std::endl;
  }
  void inprintU(uint64_t val, uintmax_t numIndent) {
    std::cout << indent(numIndent).c_str() << val << std::endl;
  }

  void printTableRefInfo(TableRef* table, uintmax_t numIndent) {
    switch (table->type) {
    case kTableName:
      inprint(table->name, numIndent);
      break;
    case kTableSelect:
      printSelectStatementInfo(table->select, numIndent);
      break;
    case kTableJoin:
      inprint("Join Table", numIndent);
      inprint("Left", numIndent + 1);
      printTableRefInfo(table->join->left, numIndent + 2);
      inprint("Right", numIndent + 1);
      printTableRefInfo(table->join->right, numIndent + 2);
      inprint("Join Condition", numIndent + 1);
      printExpression(table->join->condition, numIndent + 2);
      break;
    case kTableCrossProduct:
      for (TableRef* tbl : *table->list) printTableRefInfo(tbl, numIndent);
      break;
    }
    if (table->alias != nullptr) {
      inprint("Alias", numIndent + 1);
      inprint(table->alias, numIndent + 2);
    }
  }

  void printOperatorExpression(Expr* expr, uintmax_t numIndent) {
    if (expr == nullptr) {
      inprint("null", numIndent);
      return;
    }

    switch (expr->opType) {
    case kOpAnd:
      inprint("AND", numIndent);
      break;
    case kOpOr:
      inprint("OR", numIndent);
      break;
    case kOpNot:
      inprint("NOT", numIndent);
      break;
    default:
      inprintU(expr->opType, numIndent);
      break;
    }
    printExpression(expr->expr, numIndent + 1);
    if (expr->expr2 != nullptr) printExpression(expr->expr2, numIndent + 1);
  }

  void printExpression(Expr* expr, uintmax_t numIndent) {
    switch (expr->type) {
    case kExprStar:
      inprint("*", numIndent);
      break;
    case kExprColumnRef:
      inprint(expr->name, numIndent);
      break;
    // case kExprTableColumnRef: inprint(expr->table, expr->name, numIndent); break;
    case kExprLiteralFloat:
      inprint(expr->fval, numIndent);
      break;
    case kExprLiteralInt:
      inprint(expr->ival, numIndent);
      break;
    case kExprLiteralString:
      inprint(expr->name, numIndent);
      break;
    case kExprFunctionRef:
      inprint(expr->name, numIndent);
      inprint(expr->expr->name, numIndent + 1);
      break;
    case kExprOperator:
      printOperatorExpression(expr, numIndent);
      break;
    default:
      std::cerr << "Unrecognized expression type " << expr->type << std::endl;
      return;
    }
    if (expr->alias != nullptr) {
      inprint("Alias", numIndent + 1);
      inprint(expr->alias, numIndent + 2);
    }
  }

  void printSelectStatementInfo(const SelectStatement* stmt, uintmax_t numIndent) {
    inprint("SelectStatement", numIndent);
    inprint("Fields:", numIndent + 1);
    for (Expr* expr : *stmt->selectList) printExpression(expr, numIndent + 2);

    inprint("Sources:", numIndent + 1);
    printTableRefInfo(stmt->fromTable, numIndent + 2);

    if (stmt->whereClause != nullptr) {
      inprint("Search Conditions:", numIndent + 1);
      printExpression(stmt->whereClause, numIndent + 2);
    }

    if (stmt->unionSelect != nullptr) {
      inprint("Union:", numIndent + 1);
      printSelectStatementInfo(stmt->unionSelect, numIndent + 2);
    }

    if (stmt->order != nullptr) {
      inprint("OrderBy:", numIndent + 1);
      printExpression(stmt->order->at(0)->expr, numIndent + 2);
      if (stmt->order->at(0)->type == kOrderAsc) inprint("ascending", numIndent + 2);
      else inprint("descending", numIndent + 2);
    }

    if (stmt->limit != nullptr) {
      inprint("Limit:", numIndent + 1);
      inprint(stmt->limit->limit, numIndent + 2);
    }
  }

  void printImportStatementInfo(const ImportStatement* stmt, uintmax_t numIndent) {
    inprint("ImportStatment", numIndent);
    inprint(stmt->filePath, numIndent + 1);
    inprint(stmt->tableName, numIndent + 1);
  }

  void printCreateStatementInfo(const CreateStatement* stmt, uintmax_t numIndent) {
    inprint("CreateStatment", numIndent);
    inprint(stmt->tableName, numIndent + 1);
    inprint(stmt->filePath, numIndent + 1);
  }

  void printInsertStatementInfo(const InsertStatement* stmt, uintmax_t numIndent) {
    inprint("InsertStatment", numIndent);
    inprint(stmt->tableName, numIndent + 1);
    if (stmt->columns != nullptr) {
      inprint("Columns", numIndent + 1);
      for (char* col_name : *stmt->columns) {
        inprint(col_name, numIndent + 2);
      }
    }
    switch (stmt->type) {
    case kInsertValues:
      inprint("Values", numIndent + 1);
      for (Expr* expr : *stmt->values) {
        printExpression(expr, numIndent + 2);
      }
      break;
    case kInsertSelect:
      printSelectStatementInfo(stmt->select, numIndent + 1);
      break;
    }
  }

  void printStatementInfo(const SQLStatement* stmt) {
    switch (stmt->type()) {
    case kStmtSelect:
      printSelectStatementInfo((const SelectStatement*) stmt, 0);
      break;
    case kStmtInsert:
      printInsertStatementInfo((const InsertStatement*) stmt, 0);
      break;
    case kStmtCreate:
      printCreateStatementInfo((const CreateStatement*) stmt, 0);
      break;
    case kStmtImport:
      printImportStatementInfo((const ImportStatement*) stmt, 0);
      break;
    default:
      break;
    }
  }

} // namespace hsql
<commit_msg>Fix bug in print kExprFunctionRef expression. (#53)<commit_after>
#include "sqlhelper.h"
#include <iostream>
#include <string>

namespace hsql {

  void printOperatorExpression(Expr* expr, uintmax_t numIndent);

  std::string indent(uintmax_t numIndent) {
    return std::string(numIndent, '\t');
  }
  void inprint(int64_t val, uintmax_t numIndent) {
    std::cout << indent(numIndent).c_str() << val << "  " << std::endl;
  }
  void inprint(float val, uintmax_t numIndent) {
    std::cout << indent(numIndent).c_str() << val << std::endl;
  }
  void inprint(const char* val, uintmax_t numIndent) {
    std::cout << indent(numIndent).c_str() << val << std::endl;
  }
  void inprint(const char* val, const char* val2, uintmax_t numIndent) {
    std::cout << indent(numIndent).c_str() << val << "->" << val2 << std::endl;
  }
  void inprintC(char val, uintmax_t numIndent) {
    std::cout << indent(numIndent).c_str() << val << std::endl;
  }
  void inprintU(uint64_t val, uintmax_t numIndent) {
    std::cout << indent(numIndent).c_str() << val << std::endl;
  }

  void printTableRefInfo(TableRef* table, uintmax_t numIndent) {
    switch (table->type) {
    case kTableName:
      inprint(table->name, numIndent);
      break;
    case kTableSelect:
      printSelectStatementInfo(table->select, numIndent);
      break;
    case kTableJoin:
      inprint("Join Table", numIndent);
      inprint("Left", numIndent + 1);
      printTableRefInfo(table->join->left, numIndent + 2);
      inprint("Right", numIndent + 1);
      printTableRefInfo(table->join->right, numIndent + 2);
      inprint("Join Condition", numIndent + 1);
      printExpression(table->join->condition, numIndent + 2);
      break;
    case kTableCrossProduct:
      for (TableRef* tbl : *table->list) printTableRefInfo(tbl, numIndent);
      break;
    }
    if (table->alias != nullptr) {
      inprint("Alias", numIndent + 1);
      inprint(table->alias, numIndent + 2);
    }
  }

  void printOperatorExpression(Expr* expr, uintmax_t numIndent) {
    if (expr == nullptr) {
      inprint("null", numIndent);
      return;
    }

    switch (expr->opType) {
    case kOpAnd:
      inprint("AND", numIndent);
      break;
    case kOpOr:
      inprint("OR", numIndent);
      break;
    case kOpNot:
      inprint("NOT", numIndent);
      break;
    default:
      inprintU(expr->opType, numIndent);
      break;
    }
    printExpression(expr->expr, numIndent + 1);
    if (expr->expr2 != nullptr) printExpression(expr->expr2, numIndent + 1);
  }

  void printExpression(Expr* expr, uintmax_t numIndent) {
    switch (expr->type) {
    case kExprStar:
      inprint("*", numIndent);
      break;
    case kExprColumnRef:
      inprint(expr->name, numIndent);
      break;
    // case kExprTableColumnRef: inprint(expr->table, expr->name, numIndent); break;
    case kExprLiteralFloat:
      inprint(expr->fval, numIndent);
      break;
    case kExprLiteralInt:
      inprint(expr->ival, numIndent);
      break;
    case kExprLiteralString:
      inprint(expr->name, numIndent);
      break;
    case kExprFunctionRef:
      inprint(expr->name, numIndent);
      for (Expr* e : *expr->exprList) inprint(e->name, numIndent + 1);
      break;
    case kExprOperator:
      printOperatorExpression(expr, numIndent);
      break;
    default:
      std::cerr << "Unrecognized expression type " << expr->type << std::endl;
      return;
    }
    if (expr->alias != nullptr) {
      inprint("Alias", numIndent + 1);
      inprint(expr->alias, numIndent + 2);
    }
  }

  void printSelectStatementInfo(const SelectStatement* stmt, uintmax_t numIndent) {
    inprint("SelectStatement", numIndent);
    inprint("Fields:", numIndent + 1);
    for (Expr* expr : *stmt->selectList) printExpression(expr, numIndent + 2);

    inprint("Sources:", numIndent + 1);
    printTableRefInfo(stmt->fromTable, numIndent + 2);

    if (stmt->whereClause != nullptr) {
      inprint("Search Conditions:", numIndent + 1);
      printExpression(stmt->whereClause, numIndent + 2);
    }

    if (stmt->groupBy != nullptr) {
      inprint("GroupBy:", numIndent + 1);
      for (Expr* expr : *stmt->groupBy->columns) printExpression(expr, numIndent + 2);
      if (stmt->groupBy->having != nullptr) {
        inprint("Having:", numIndent + 1);
        printExpression(stmt->groupBy->having, numIndent + 2);
      }
    }

    if (stmt->unionSelect != nullptr) {
      inprint("Union:", numIndent + 1);
      printSelectStatementInfo(stmt->unionSelect, numIndent + 2);
    }

    if (stmt->order != nullptr) {
      inprint("OrderBy:", numIndent + 1);
      printExpression(stmt->order->at(0)->expr, numIndent + 2);
      if (stmt->order->at(0)->type == kOrderAsc) inprint("ascending", numIndent + 2);
      else inprint("descending", numIndent + 2);
    }

    if (stmt->limit != nullptr) {
      inprint("Limit:", numIndent + 1);
      inprint(stmt->limit->limit, numIndent + 2);
    }
  }

  void printImportStatementInfo(const ImportStatement* stmt, uintmax_t numIndent) {
    inprint("ImportStatment", numIndent);
    inprint(stmt->filePath, numIndent + 1);
    inprint(stmt->tableName, numIndent + 1);
  }

  void printCreateStatementInfo(const CreateStatement* stmt, uintmax_t numIndent) {
    inprint("CreateStatment", numIndent);
    inprint(stmt->tableName, numIndent + 1);
    inprint(stmt->filePath, numIndent + 1);
  }

  void printInsertStatementInfo(const InsertStatement* stmt, uintmax_t numIndent) {
    inprint("InsertStatment", numIndent);
    inprint(stmt->tableName, numIndent + 1);
    if (stmt->columns != nullptr) {
      inprint("Columns", numIndent + 1);
      for (char* col_name : *stmt->columns) {
        inprint(col_name, numIndent + 2);
      }
    }
    switch (stmt->type) {
    case kInsertValues:
      inprint("Values", numIndent + 1);
      for (Expr* expr : *stmt->values) {
        printExpression(expr, numIndent + 2);
      }
      break;
    case kInsertSelect:
      printSelectStatementInfo(stmt->select, numIndent + 1);
      break;
    }
  }

  void printStatementInfo(const SQLStatement* stmt) {
    switch (stmt->type()) {
    case kStmtSelect:
      printSelectStatementInfo((const SelectStatement*) stmt, 0);
      break;
    case kStmtInsert:
      printInsertStatementInfo((const InsertStatement*) stmt, 0);
      break;
    case kStmtCreate:
      printCreateStatementInfo((const CreateStatement*) stmt, 0);
      break;
    case kStmtImport:
      printImportStatementInfo((const ImportStatement*) stmt, 0);
      break;
    default:
      break;
    }
  }

} // namespace hsql
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2011-2013 libbitcoin developers (see AUTHORS)
 *
 * This file is part of libbitcoin.
 *
 * libbitcoin is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License with
 * additional permissions to the one published by the Free Software
 * Foundation, either version 3 of the License, or (at your option)
 * any later version. For more information see LICENSE.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */
#include <bitcoin/utility/mmfile.hpp>

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#ifndef _MSC_VER
#include <unistd.h>
#include <sys/mman.h>
#endif
#include <sys/types.h>
#include <sys/stat.h>
#include <bitcoin/utility/assert.hpp>

namespace libbitcoin {

mmfile::mmfile(const std::string& filename)
{
#ifndef _MSC_VER
    // Not yet MSVC portable (maybe windows).
    BITCOIN_ASSERT_MSG(sizeof (void*) == 8, "Not a 64bit system!");
    file_handle_ = open(filename.c_str(),
        O_RDWR, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
    if (file_handle_ == -1)
        return;
    struct stat sbuf;
    if (fstat(file_handle_, &sbuf) == -1)
        return;
    size_ = sbuf.st_size;
    // You can static_cast void* pointers.
    data_ = static_cast<uint8_t*>(mmap(
        0, size_, PROT_READ | PROT_WRITE, MAP_SHARED, file_handle_, 0));
    if (data_ == MAP_FAILED)
        data_ = nullptr;
#endif
}
mmfile::~mmfile()
{
    munmap(data_, size_);
    close(file_handle_);
}

uint8_t* mmfile::data()
{
    return data_;
}
const uint8_t* mmfile::data() const
{
    return data_;
}
size_t mmfile::size() const
{
    return size_;
}

bool mmfile::resize(size_t new_size)
{
    // Resize underlying file.
    if (ftruncate(file_handle_, new_size) == -1)
        return false;
    // Readjust memory map.
    data_ = static_cast<uint8_t*>(mremap(
        data_, size_, new_size, MREMAP_MAYMOVE));
    if (data_ == MAP_FAILED)
        return false;
    size_ = new_size;
    return true;
}

} // namespace libbitcoin
<commit_msg>Conditionally disable non-portable code.<commit_after>/*
 * Copyright (c) 2011-2013 libbitcoin developers (see AUTHORS)
 *
 * This file is part of libbitcoin.
 *
 * libbitcoin is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License with
 * additional permissions to the one published by the Free Software
 * Foundation, either version 3 of the License, or (at your option)
 * any later version. For more information see LICENSE.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */
#include <bitcoin/utility/mmfile.hpp>

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#ifndef _MSC_VER
#include <unistd.h>
#include <sys/mman.h>
#endif
#include <sys/types.h>
#include <sys/stat.h>
#include <bitcoin/utility/assert.hpp>

namespace libbitcoin {

mmfile::mmfile(const std::string& filename)
{
#ifndef _MSC_VER
    // Not yet MSVC portable (maybe windows).
    BITCOIN_ASSERT_MSG(sizeof (void*) == 8, "Not a 64bit system!");
    file_handle_ = open(filename.c_str(),
        O_RDWR, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
    if (file_handle_ == -1)
        return;
    struct stat sbuf;
    if (fstat(file_handle_, &sbuf) == -1)
        return;
    size_ = sbuf.st_size;
    // You can static_cast void* pointers.
    data_ = static_cast<uint8_t*>(mmap(
        0, size_, PROT_READ | PROT_WRITE, MAP_SHARED, file_handle_, 0));
    if (data_ == MAP_FAILED)
        data_ = nullptr;
#endif
}
mmfile::~mmfile()
{
#ifndef _MSC_VER
    // Not yet MSVC portable (maybe windows).
    munmap(data_, size_);
    close(file_handle_);
#endif
}

uint8_t* mmfile::data()
{
    return data_;
}
const uint8_t* mmfile::data() const
{
    return data_;
}
size_t mmfile::size() const
{
    return size_;
}

bool mmfile::resize(size_t new_size)
{
#ifndef _MSC_VER
    // Not yet MSVC portable (maybe windows).
    // Resize underlying file.
    if (ftruncate(file_handle_, new_size) == -1)
        return false;
    // Readjust memory map.
    data_ = static_cast<uint8_t*>(mremap(
        data_, size_, new_size, MREMAP_MAYMOVE));
    if (data_ == MAP_FAILED)
        return false;
    size_ = new_size;
    return true;
#else
    return false;
#endif
}

} // namespace libbitcoin
<|endoftext|>


<commit_before>#include "view/SceneView.h"

#include "DotSceneLoader.h"

#include <OgreRoot.h>
#include <OgreViewport.h>
#include <OgreConfigFile.h>
#include <OgreEntity.h>
#include <OgreWindowEventUtilities.h>

SceneView::SceneView()
    : camera(NULL),
    sceneManager(NULL),
    renderWindow(NULL) {
}

SceneView::~SceneView(void) {
    Ogre::Root* root = Ogre::Root::getSingletonPtr();
    delete root;
}

bool SceneView::initialize(std::string resourceConfigPath, std::string pluginConfigPath) {
    Ogre::Root* root = new Ogre::Root(pluginConfigPath);

    this->loadResourceConfig(resourceConfigPath);
    // (The actual precaching is done below, once there is a render context)

    if(!root->restoreConfig() && !root->showConfigDialog()) {
        return false;
    }

    renderWindow = root->initialise(
        true, // auto-create the render window now
        "Rally Sport Racing Game");

    sceneManager = root->createSceneManager("OctreeSceneManager"); // Todo: Research a good scene manager

    // This should be done after creating a scene manager, so that there is a render context (GL/D3D)
    Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups();

    camera = this->addCamera("MainCamera");
    Ogre::Viewport* viewport = this->addViewport(camera);
    camera->setAspectRatio(Ogre::Real(viewport->getActualWidth()) / Ogre::Real(viewport->getActualHeight()));

    // TODO: Fix this to follow car...
    camera->setPosition(Ogre::Vector3(100, 100, -50));
    camera->lookAt(Ogre::Vector3(0, 0, 0));

    // TODO: Implement separate scene loading (how do we do with lights?)
    Ogre::Entity* ogreHead = sceneManager->createEntity("Head", "ogrehead.mesh");
    Ogre::SceneNode* headNode = sceneManager->getRootSceneNode()->createChildSceneNode();
    //headNode->attachObject(ogreHead);
   sceneManager->setAmbientLight(Ogre::ColourValue(1, 1, 1));
	Ogre::Light* light = sceneManager->createLight("MainLight");
	light->setPosition(20, 80, 50);

	DotSceneLoader loader;
	loader.parseDotScene("chalmers1d.scene", "General", sceneManager, headNode);

	//headNode->scale(0.5,0.5,0.5);
}

Ogre::Viewport* SceneView::addViewport(Ogre::Camera* followedCamera) {
    Ogre::Viewport* viewport = renderWindow->addViewport(camera);
    viewport->setBackgroundColour(Ogre::ColourValue(0, 0, 0));

    return viewport;
}

Ogre::Camera* SceneView::addCamera(Ogre::String cameraName) {
    // Setup camera to match viewport
    Ogre::Camera* camera = sceneManager->createCamera(cameraName);
    camera->setNearClipDistance(5);

    return camera;
}

void SceneView::loadResourceConfig(Ogre::String resourceConfigPath) {
    Ogre::ResourceGroupManager& resourceGroupManager = Ogre::ResourceGroupManager::getSingleton();

    Ogre::ConfigFile resourceConfig;
    resourceConfig.load(resourceConfigPath);

    for(Ogre::ConfigFile::SectionIterator sectionIterator = resourceConfig.getSectionIterator();
            sectionIterator.hasMoreElements();
            sectionIterator.moveNext()) {
        Ogre::ConfigFile::SettingsMultiMap* settings = sectionIterator.peekNextValue();

        // Now load all the resources for this resource group
        for(Ogre::ConfigFile::SettingsMultiMap::iterator resource = settings->begin();
                resource != settings->end();
                ++resource) {
            resourceGroupManager.addResourceLocation(
                resource->second, // filename of directory
                resource->first, // resource type
                sectionIterator.peekNextKey()); // resource group
        }
    }
}

bool SceneView::renderFrame() {
    Ogre::WindowEventUtilities::messagePump();

    if(renderWindow->isClosed())  {
        return false;
    } else {
        Ogre::Root& root = Ogre::Root::getSingleton();
        if(!root.renderOneFrame()) {
            return false;
        }
    }
    return true;
}
<commit_msg>Implemented bulletfile loading...<commit_after>#include "view/SceneView.h"

#include "DotSceneLoader.h"

#include <OgreRoot.h>
#include <OgreViewport.h>
#include <OgreConfigFile.h>
#include <OgreEntity.h>
#include <OgreWindowEventUtilities.h>

#include <btBulletDynamicsCommon.h>
#include <../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h>

SceneView::SceneView()
    : camera(NULL),
    sceneManager(NULL),
    renderWindow(NULL) {
}

SceneView::~SceneView(void) {
    Ogre::Root* root = Ogre::Root::getSingletonPtr();
    delete root;
}

bool SceneView::initialize(std::string resourceConfigPath, std::string pluginConfigPath) {
    Ogre::Root* root = new Ogre::Root(pluginConfigPath);

    this->loadResourceConfig(resourceConfigPath);
    // (The actual precaching is done below, once there is a render context)

    if(!root->restoreConfig() && !root->showConfigDialog()) {
        return false;
    }

    renderWindow = root->initialise(
        true, // auto-create the render window now
        "Rally Sport Racing Game");

    sceneManager = root->createSceneManager("OctreeSceneManager"); // Todo: Research a good scene manager

    // This should be done after creating a scene manager, so that there is a render context (GL/D3D)
    Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups();

    camera = this->addCamera("MainCamera");
    Ogre::Viewport* viewport = this->addViewport(camera);
    camera->setAspectRatio(Ogre::Real(viewport->getActualWidth()) / Ogre::Real(viewport->getActualHeight()));

    // TODO: Fix this to follow car...
    camera->setPosition(Ogre::Vector3(0, 100, -250));
    camera->lookAt(Ogre::Vector3(0, 0, 0));

    // TODO: Implement separate scene loading (how do we do with lights?)
    //Ogre::Entity* ogreHead = sceneManager->createEntity("Head", "ogrehead.mesh");
    Ogre::SceneNode* sceneNode = sceneManager->getRootSceneNode()->createChildSceneNode();
    //headNode->attachObject(ogreHead);
	sceneManager->setAmbientLight(Ogre::ColourValue(1, 1, 1));
	Ogre::Light* light = sceneManager->createLight("MainLight");
	light->setPosition(20, 80, 50);

	// Load the scene.
	DotSceneLoader loader;
	loader.parseDotScene("chalmers1d.scene", "General", sceneManager, sceneNode);

	//*******BULLET*******//
	btBroadphaseInterface* broadphase = new btDbvtBroadphase();

    // Collison config and dispatcher
    btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
    btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);

    // The physics solver
    btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver;

    // The world.
    btDiscreteDynamicsWorld* dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher, broadphase, solver, collisionConfiguration);

	// Load .bullet
	btBulletWorldImporter* fileLoader = new btBulletWorldImporter(dynamicsWorld);

	//optionally enable the verbose mode to provide debugging information during file loading (a lot of data is generated, so this option is very slow)
	//fileLoader->setVerboseMode(true);

	fileLoader->loadFile("world.bullet");

	// Output the amount of loaded rigid bodies
	int body_count = fileLoader->getNumRigidBodies();
	Ogre::LogManager::getSingleton().logMessage("Number of rigid bodies: " + std::to_string(body_count));

}

Ogre::Viewport* SceneView::addViewport(Ogre::Camera* followedCamera) {
    Ogre::Viewport* viewport = renderWindow->addViewport(camera);
    viewport->setBackgroundColour(Ogre::ColourValue(0, 0, 0));

    return viewport;
}

Ogre::Camera* SceneView::addCamera(Ogre::String cameraName) {
    // Setup camera to match viewport
    Ogre::Camera* camera = sceneManager->createCamera(cameraName);
    camera->setNearClipDistance(5);

    return camera;
}

void SceneView::loadResourceConfig(Ogre::String resourceConfigPath) {
    Ogre::ResourceGroupManager& resourceGroupManager = Ogre::ResourceGroupManager::getSingleton();

    Ogre::ConfigFile resourceConfig;
    resourceConfig.load(resourceConfigPath);

    for(Ogre::ConfigFile::SectionIterator sectionIterator = resourceConfig.getSectionIterator();
            sectionIterator.hasMoreElements();
            sectionIterator.moveNext()) {
        Ogre::ConfigFile::SettingsMultiMap* settings = sectionIterator.peekNextValue();

        // Now load all the resources for this resource group
        for(Ogre::ConfigFile::SettingsMultiMap::iterator resource = settings->begin();
                resource != settings->end();
                ++resource) {
            resourceGroupManager.addResourceLocation(
                resource->second, // filename of directory
                resource->first, // resource type
                sectionIterator.peekNextKey()); // resource group
        }
    }
}

bool SceneView::renderFrame() {
    Ogre::WindowEventUtilities::messagePump();

    if(renderWindow->isClosed())  {
        return false;
    } else {
        Ogre::Root& root = Ogre::Root::getSingleton();
        if(!root.renderOneFrame()) {
            return false;
        }
    }
    return true;
}
<|endoftext|>


<commit_before>#include "BSD.h"
#include <unistd.h>
#include <string.h>
#include <stdio.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <fcntl.h>
#include <assert.h>
#include <errno.h>

namespace XNet
{

#define MAX_PACKET_SIZE 1500

BSDSocket::BSDSocket(int sock)
: socket(sock)
{
}

BSDSocket::~BSDSocket()
{
	close(socket);
}

void BSDSocket::Send(uint32_t host, uint16_t port, const void* data, size_t length)
{
	struct sockaddr_in address;
	memset(&address, 0, sizeof(address));
	address.sin_len = sizeof(address);
	address.sin_family = AF_INET;
	address.sin_port = port;
	address.sin_addr.s_addr = host;
	int rc = sendto(socket, data, length, 0, (const struct sockaddr*)&address, sizeof(address));
	if (rc == -1 && errno == EAGAIN) // would block, drop the packet
	{
		return;
	}
	else
	{
		assert((size_t)rc == length);
	}
}

void* BSDSocket::Receive(uint32_t& host, uint16_t& port, size_t& length)
{
	char packet[MAX_PACKET_SIZE];
	struct sockaddr_in address;
	socklen_t address_length = sizeof(address);
	ssize_t rv = recvfrom(socket, packet, MAX_PACKET_SIZE, 0, (struct sockaddr*)&address, &address_length);
	if (rv < 0)
	{
		if (errno != EAGAIN) // EAGAIN is quite expected
		{
			perror("failed to receive packet");
		}
		length = 0;
		return NULL;
	}
	else if (rv == 0)
	{
		length = 0;
		return NULL;
	}
	else
	{
		void* data = malloc(length);
		memcpy(data, packet, length);
		length = rv;
		host = address.sin_addr.s_addr;
		assert(host);
		port = address.sin_port;
		assert(port);
		return data;
	}
}

BSDSocketProvider::~BSDSocketProvider()
{
}

Socket* BSDSocketProvider::NewSocket(uint16_t port)
{
	int rc;
	int sock = socket(PF_INET, SOCK_DGRAM, 0);
	struct sockaddr_in address;
	memset(&address, 0, sizeof(address));
	address.sin_port = port;
	address.sin_family = AF_INET;
	rc = bind(sock, (const struct sockaddr*)&address, sizeof(address));
	assert(rc != 0);
	int flags;
	flags = fcntl(sock, F_GETFL);
	flags |= O_NONBLOCK;
	rc = fcntl(sock, F_SETFL, flags);
	assert(rc != -1);
	return new BSDSocket(sock);
}

uint32_t BSDSocketProvider::ResolveHost(const std::string& hostname)
{
	struct hostent* hostent = gethostbyname2(hostname.c_str(), AF_INET);
	if (hostent && hostent->h_addr)
	{
		uint32_t value = *(uint32_t*)hostent->h_addr;
		//fprintf(stderr, "%s = 0x%08X\n", hostname.c_str(), value);
		return value;
	}
	else
	{
		return 0;
	}
}

std::string BSDSocketProvider::ReverseLookup(uint32_t host)
{
	char buf[16];
	union
	{
		char octets[4];
		uint32_t val;
	} bitcast;
	bitcast.val = host;
	sprintf(buf, "%d.%d.%d.%d", bitcast.octets[0], bitcast.octets[1], bitcast.octets[2], bitcast.octets[3]);
	return std::string(buf);
}

}
<commit_msg>Made BSD properly handle EINTR the same as EAGAIN.<commit_after>#include "BSD.h"
#include <unistd.h>
#include <string.h>
#include <stdio.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <fcntl.h>
#include <assert.h>
#include <errno.h>

namespace XNet
{

#define MAX_PACKET_SIZE 1500

BSDSocket::BSDSocket(int sock)
: socket(sock)
{
}

BSDSocket::~BSDSocket()
{
	close(socket);
}

void BSDSocket::Send(uint32_t host, uint16_t port, const void* data, size_t length)
{
	struct sockaddr_in address;
	memset(&address, 0, sizeof(address));
	address.sin_len = sizeof(address);
	address.sin_family = AF_INET;
	address.sin_port = port;
	address.sin_addr.s_addr = host;
	int rc = sendto(socket, data, length, 0, (const struct sockaddr*)&address, sizeof(address));
	if (rc == -1 && (errno == EAGAIN || errno == EINTR)) // would block, drop the packet
	{
		return;
	}
	else
	{
		assert((size_t)rc == length);
	}
}

void* BSDSocket::Receive(uint32_t& host, uint16_t& port, size_t& length)
{
	char packet[MAX_PACKET_SIZE];
	struct sockaddr_in address;
	socklen_t address_length = sizeof(address);
	ssize_t rv = recvfrom(socket, packet, MAX_PACKET_SIZE, 0, (struct sockaddr*)&address, &address_length);
	if (rv < 0)
	{
		if (errno != EAGAIN && errno != EINTR) // EAGAIN and EINTR can happen
		{
			perror("failed to receive packet");
		}
		length = 0;
		return NULL;
	}
	else if (rv == 0)
	{
		length = 0;
		return NULL;
	}
	else
	{
		void* data = malloc(length);
		memcpy(data, packet, length);
		length = rv;
		host = address.sin_addr.s_addr;
		assert(host);
		port = address.sin_port;
		assert(port);
		return data;
	}
}

BSDSocketProvider::~BSDSocketProvider()
{
}

Socket* BSDSocketProvider::NewSocket(uint16_t port)
{
	int rc;
	int sock = socket(PF_INET, SOCK_DGRAM, 0);
	struct sockaddr_in address;
	memset(&address, 0, sizeof(address));
	address.sin_port = port;
	address.sin_family = AF_INET;
	rc = bind(sock, (const struct sockaddr*)&address, sizeof(address));
	assert(rc != 0);
	int flags;
	flags = fcntl(sock, F_GETFL);
	flags |= O_NONBLOCK;
	rc = fcntl(sock, F_SETFL, flags);
	assert(rc != -1);
	return new BSDSocket(sock);
}

uint32_t BSDSocketProvider::ResolveHost(const std::string& hostname)
{
	struct hostent* hostent = gethostbyname2(hostname.c_str(), AF_INET);
	if (hostent && hostent->h_addr)
	{
		uint32_t value = *(uint32_t*)hostent->h_addr;
		//fprintf(stderr, "%s = 0x%08X\n", hostname.c_str(), value);
		return value;
	}
	else
	{
		return 0;
	}
}

std::string BSDSocketProvider::ReverseLookup(uint32_t host)
{
	char buf[16];
	union
	{
		char octets[4];
		uint32_t val;
	} bitcast;
	bitcast.val = host;
	sprintf(buf, "%d.%d.%d.%d", bitcast.octets[0], bitcast.octets[1], bitcast.octets[2], bitcast.octets[3]);
	return std::string(buf);
}

}
<|endoftext|>


<commit_before>#include "dreal/solver/sat_solver.h"

#include <ostream>
#include <utility>

#include "dreal/util/assert.h"
#include "dreal/util/exception.h"
#include "dreal/util/logging.h"
#include "dreal/util/stat.h"
#include "dreal/util/timer.h"

namespace dreal {

using std::cout;
using std::set;
using std::vector;

SatSolver::SatSolver(const Config& config) : sat_{picosat_init()} {
  // Enable partial checks via picosat_deref_partial. See the call-site in
  // SatSolver::CheckSat().
  picosat_save_original_clauses(sat_);
  if (config.random_seed() != 0) {
    picosat_set_seed(sat_, config.random_seed());
    DREAL_LOG_DEBUG("SatSolver::Set Random Seed {}", config.random_seed());
  }
  picosat_set_global_default_phase(
      sat_, static_cast<int>(config.sat_default_phase()));
  DREAL_LOG_DEBUG("SatSolver::Set Default Phase {}",
                  config.sat_default_phase());
}

SatSolver::SatSolver(const Config& config, const vector<Formula>& clauses)
    : SatSolver{config} {
  AddClauses(clauses);
}

SatSolver::~SatSolver() { picosat_reset(sat_); }

void SatSolver::AddFormula(const Formula& f) {
  DREAL_LOG_DEBUG("SatSolver::AddFormula({})", f);
  vector<Formula> clauses{cnfizer_.Convert(f)};
  // Collect Tseitin variables.
  for (const auto& p : cnfizer_.map()) {
    tseitin_variables_.insert(p.first.get_id());
  }
  for (Formula& clause : clauses) {
    clause = predicate_abstractor_.Convert(clause);
  }
  AddClauses(clauses);
}

void SatSolver::AddFormulas(const vector<Formula>& formulas) {
  for (const Formula& f : formulas) {
    AddFormula(f);
  }
}

void SatSolver::AddLearnedClause(const set<Formula>& formulas) {
  for (const Formula& f : formulas) {
    AddLiteral(!predicate_abstractor_.Convert(f));
  }
  picosat_add(sat_, 0);
}

void SatSolver::AddClauses(const vector<Formula>& formulas) {
  for (const Formula& f : formulas) {
    AddClause(f);
  }
}

void SatSolver::AddClause(const Formula& f) {
  DREAL_LOG_DEBUG("SatSolver::AddClause({})", f);
  // Set up Variable ⇔ Literal (in SAT) map.
  for (const Variable& var : f.GetFreeVariables()) {
    MakeSatVar(var);
  }
  // Add clauses to SAT solver.
  DoAddClause(f);
}

namespace {
class SatSolverStat : public Stat {
 public:
  explicit SatSolverStat(const bool enabled) : Stat{enabled} {};
  SatSolverStat(const SatSolverStat&) = default;
  SatSolverStat(SatSolverStat&&) = default;
  SatSolverStat& operator=(const SatSolverStat&) = delete;
  SatSolverStat& operator=(SatSolverStat&&) = delete;
  ~SatSolverStat() override {
    if (enabled()) {
      using fmt::print;
      print(cout, "{:<45} @ {:<20} = {:>15}\n", "Total # of CheckSat",
            "SAT level", num_check_sat_);
      print(cout, "{:<45} @ {:<20} = {:>15f} sec\n",
            "Total time spent in SAT checks", "SAT level",
            timer_check_sat_.seconds());
    }
  }

  int num_check_sat_{0};
  Timer timer_check_sat_;
};
}  // namespace

optional<SatSolver::Model> SatSolver::CheckSat() {
  static SatSolverStat stat{DREAL_LOG_INFO_ENABLED};
  DREAL_LOG_DEBUG("SatSolver::CheckSat(#vars = {}, #clauses = {})",
                  picosat_variables(sat_),
                  picosat_added_original_clauses(sat_));
  stat.num_check_sat_++;
  // Call SAT solver.
  TimerGuard check_sat_timer_guard(&stat.timer_check_sat_,
                                   DREAL_LOG_INFO_ENABLED);
  const int ret{picosat_sat(sat_, -1)};
  check_sat_timer_guard.pause();

  Model model;
  if (ret == PICOSAT_SATISFIABLE) {
    // SAT Case.
    const auto& var_to_formula_map = predicate_abstractor_.var_to_formula_map();
    for (int i = 1; i <= picosat_variables(sat_); ++i) {
      const int model_i{has_picosat_pop_used_ ? picosat_deref(sat_, i)
                                              : picosat_deref_partial(sat_, i)};
      if (model_i == 0) {
        continue;
      }
      const auto it_var = to_sym_var_.find(i);
      if (it_var == to_sym_var_.end()) {
        // There is no symbolic::Variable corresponding to this
        // picosat variable (int). This could be because of
        // picosat_push/pop.
        continue;
      }
      const Variable& var{it_var->second};
      const auto it = var_to_formula_map.find(var);
      if (it != var_to_formula_map.end()) {
        DREAL_LOG_TRACE("SatSolver::CheckSat: Add theory literal {}{} to Model",
                        model_i ? "" : "¬", var);
        auto& theory_model = model.second;
        theory_model.emplace_back(var, model_i == 1);
      } else if (tseitin_variables_.count(var.get_id()) == 0) {
        DREAL_LOG_TRACE(
            "SatSolver::CheckSat: Add Boolean literal {}{} to Model ",
            model_i ? "" : "¬", var);
        auto& boolean_model = model.first;
        boolean_model.emplace_back(var, model_i == 1);
      } else {
        DREAL_LOG_TRACE(
            "SatSolver::CheckSat: Skip {}{} which is a temporary variable.",
            model_i ? "" : "¬", var);
      }
    }
    DREAL_LOG_DEBUG("SatSolver::CheckSat() Found a model.");
    return model;
  } else if (ret == PICOSAT_UNSATISFIABLE) {
    DREAL_LOG_DEBUG("SatSolver::CheckSat() No solution.");
    // UNSAT Case.
    return {};
  } else {
    DREAL_ASSERT(ret == PICOSAT_UNKNOWN);
    DREAL_LOG_CRITICAL("PICOSAT returns PICOSAT_UNKNOWN.");
    throw DREAL_RUNTIME_ERROR("PICOSAT returns PICOSAT_UNKNOWN.");
  }
}

void SatSolver::Pop() {
  DREAL_LOG_DEBUG("SatSolver::Pop()");
  tseitin_variables_.pop();
  to_sym_var_.pop();
  to_sat_var_.pop();
  picosat_pop(sat_);
  has_picosat_pop_used_ = true;
}

void SatSolver::Push() {
  DREAL_LOG_DEBUG("SatSolver::Push()");
  picosat_push(sat_);
  to_sat_var_.push();
  to_sym_var_.push();
  tseitin_variables_.push();
}

void SatSolver::AddLiteral(const Formula& f) {
  DREAL_ASSERT(is_variable(f) ||
               (is_negation(f) && is_variable(get_operand(f))));
  if (is_variable(f)) {
    // f = b
    const Variable& var{get_variable(f)};
    DREAL_ASSERT(var.get_type() == Variable::Type::BOOLEAN);
    // Add l = b
    picosat_add(sat_, to_sat_var_[var.get_id()]);
  } else {
    // f = ¬b
    DREAL_ASSERT(is_negation(f) && is_variable(get_operand(f)));
    const Variable& var{get_variable(get_operand(f))};
    DREAL_ASSERT(var.get_type() == Variable::Type::BOOLEAN);
    // Add l = ¬b
    picosat_add(sat_, -to_sat_var_[var.get_id()]);
  }
}

void SatSolver::DoAddClause(const Formula& f) {
  if (is_disjunction(f)) {
    // f = l₁ ∨ ... ∨ lₙ
    for (const Formula& l : get_operands(f)) {
      AddLiteral(l);
    }
  } else {
    // f = b or f = ¬b.
    AddLiteral(f);
  }
  picosat_add(sat_, 0);
}

void SatSolver::MakeSatVar(const Variable& var) {
  auto it = to_sat_var_.find(var.get_id());
  if (it != to_sat_var_.end()) {
    // Found.
    return;
  }
  // It's not in the maps, let's make one and add it.
  const int sat_var{picosat_inc_max_var(sat_)};
  to_sat_var_.insert(var.get_id(), sat_var);
  to_sym_var_.insert(sat_var, var);
  DREAL_LOG_DEBUG("SatSolver::MakeSatVar({} ↦ {})", var, sat_var);
}
}  // namespace dreal
<commit_msg>refactor(solver/sat_solver.cc): Fix logging; add comments<commit_after>#include "dreal/solver/sat_solver.h"

#include <ostream>
#include <utility>

#include "dreal/util/assert.h"
#include "dreal/util/exception.h"
#include "dreal/util/logging.h"
#include "dreal/util/stat.h"
#include "dreal/util/timer.h"

namespace dreal {

using std::cout;
using std::set;
using std::vector;

SatSolver::SatSolver(const Config& config) : sat_{picosat_init()} {
  // Enable partial checks via picosat_deref_partial. See the call-site in
  // SatSolver::CheckSat().
  picosat_save_original_clauses(sat_);
  if (config.random_seed() != 0) {
    picosat_set_seed(sat_, config.random_seed());
    DREAL_LOG_DEBUG("SatSolver::Set Random Seed {}", config.random_seed());
  }
  picosat_set_global_default_phase(
      sat_, static_cast<int>(config.sat_default_phase()));
  DREAL_LOG_DEBUG("SatSolver::Set Default Phase {}",
                  config.sat_default_phase());
}

SatSolver::SatSolver(const Config& config, const vector<Formula>& clauses)
    : SatSolver{config} {
  AddClauses(clauses);
}

SatSolver::~SatSolver() { picosat_reset(sat_); }

void SatSolver::AddFormula(const Formula& f) {
  DREAL_LOG_DEBUG("SatSolver::AddFormula({})", f);
  vector<Formula> clauses{cnfizer_.Convert(f)};
  // Collect Tseitin variables.
  for (const auto& p : cnfizer_.map()) {
    tseitin_variables_.insert(p.first.get_id());
  }
  for (Formula& clause : clauses) {
    clause = predicate_abstractor_.Convert(clause);
  }
  AddClauses(clauses);
}

void SatSolver::AddFormulas(const vector<Formula>& formulas) {
  for (const Formula& f : formulas) {
    AddFormula(f);
  }
}

void SatSolver::AddLearnedClause(const set<Formula>& formulas) {
  for (const Formula& f : formulas) {
    AddLiteral(!predicate_abstractor_.Convert(f));
  }
  picosat_add(sat_, 0);
}

void SatSolver::AddClauses(const vector<Formula>& formulas) {
  for (const Formula& f : formulas) {
    AddClause(f);
  }
}

void SatSolver::AddClause(const Formula& f) {
  DREAL_LOG_DEBUG("SatSolver::AddClause({})", f);
  // Set up Variable ⇔ Literal (in SAT) map.
  for (const Variable& var : f.GetFreeVariables()) {
    MakeSatVar(var);
  }
  // Add clauses to SAT solver.
  DoAddClause(f);
}

namespace {
class SatSolverStat : public Stat {
 public:
  explicit SatSolverStat(const bool enabled) : Stat{enabled} {};
  SatSolverStat(const SatSolverStat&) = default;
  SatSolverStat(SatSolverStat&&) = default;
  SatSolverStat& operator=(const SatSolverStat&) = delete;
  SatSolverStat& operator=(SatSolverStat&&) = delete;
  ~SatSolverStat() override {
    if (enabled()) {
      using fmt::print;
      print(cout, "{:<45} @ {:<20} = {:>15}\n", "Total # of CheckSat",
            "SAT level", num_check_sat_);
      print(cout, "{:<45} @ {:<20} = {:>15f} sec\n",
            "Total time spent in SAT checks", "SAT level",
            timer_check_sat_.seconds());
    }
  }

  int num_check_sat_{0};
  Timer timer_check_sat_;
};
}  // namespace

optional<SatSolver::Model> SatSolver::CheckSat() {
  static SatSolverStat stat{DREAL_LOG_INFO_ENABLED};
  DREAL_LOG_DEBUG("SatSolver::CheckSat(#vars = {}, #clauses = {})",
                  picosat_variables(sat_),
                  picosat_added_original_clauses(sat_));
  stat.num_check_sat_++;
  // Call SAT solver.
  TimerGuard check_sat_timer_guard(&stat.timer_check_sat_,
                                   DREAL_LOG_INFO_ENABLED);
  const int ret{picosat_sat(sat_, -1 /* decision_limit == no limit */)};
  check_sat_timer_guard.pause();

  Model model;
  if (ret == PICOSAT_SATISFIABLE) {
    // SAT Case.
    const auto& var_to_formula_map = predicate_abstractor_.var_to_formula_map();
    for (int i = 1; i <= picosat_variables(sat_); ++i) {
      const int model_i{has_picosat_pop_used_ ? picosat_deref(sat_, i)
                                              : picosat_deref_partial(sat_, i)};
      if (model_i == 0) {
        continue;
      }
      const auto it_var = to_sym_var_.find(i);
      if (it_var == to_sym_var_.end()) {
        // There is no symbolic::Variable corresponding to this
        // picosat variable (int). This could be because of
        // picosat_push/pop.
        continue;
      }
      const Variable& var{it_var->second};
      const auto it = var_to_formula_map.find(var);
      if (it != var_to_formula_map.end()) {
        DREAL_LOG_TRACE("SatSolver::CheckSat: Add theory literal {}{} to Model",
                        model_i == 1 ? "" : "¬", var);
        auto& theory_model = model.second;
        theory_model.emplace_back(var, model_i == 1);
      } else if (tseitin_variables_.count(var.get_id()) == 0) {
        DREAL_LOG_TRACE(
            "SatSolver::CheckSat: Add Boolean literal {}{} to Model ",
            model_i == 1 ? "" : "¬", var);
        auto& boolean_model = model.first;
        boolean_model.emplace_back(var, model_i == 1);
      } else {
        DREAL_LOG_TRACE(
            "SatSolver::CheckSat: Skip {}{} which is a temporary variable.",
            model_i == 1 ? "" : "¬", var);
      }
    }
    DREAL_LOG_DEBUG("SatSolver::CheckSat() Found a model.");
    return model;
  } else if (ret == PICOSAT_UNSATISFIABLE) {
    DREAL_LOG_DEBUG("SatSolver::CheckSat() No solution.");
    // UNSAT Case.
    return {};
  } else {
    DREAL_ASSERT(ret == PICOSAT_UNKNOWN);
    DREAL_LOG_CRITICAL("PICOSAT returns PICOSAT_UNKNOWN.");
    throw DREAL_RUNTIME_ERROR("PICOSAT returns PICOSAT_UNKNOWN.");
  }
}

void SatSolver::Pop() {
  DREAL_LOG_DEBUG("SatSolver::Pop()");
  tseitin_variables_.pop();
  to_sym_var_.pop();
  to_sat_var_.pop();
  picosat_pop(sat_);
  has_picosat_pop_used_ = true;
}

void SatSolver::Push() {
  DREAL_LOG_DEBUG("SatSolver::Push()");
  picosat_push(sat_);
  to_sat_var_.push();
  to_sym_var_.push();
  tseitin_variables_.push();
}

void SatSolver::AddLiteral(const Formula& f) {
  DREAL_ASSERT(is_variable(f) ||
               (is_negation(f) && is_variable(get_operand(f))));
  if (is_variable(f)) {
    // f = b
    const Variable& var{get_variable(f)};
    DREAL_ASSERT(var.get_type() == Variable::Type::BOOLEAN);
    // Add l = b
    picosat_add(sat_, to_sat_var_[var.get_id()]);
  } else {
    // f = ¬b
    DREAL_ASSERT(is_negation(f) && is_variable(get_operand(f)));
    const Variable& var{get_variable(get_operand(f))};
    DREAL_ASSERT(var.get_type() == Variable::Type::BOOLEAN);
    // Add l = ¬b
    picosat_add(sat_, -to_sat_var_[var.get_id()]);
  }
}

void SatSolver::DoAddClause(const Formula& f) {
  if (is_disjunction(f)) {
    // f = l₁ ∨ ... ∨ lₙ
    for (const Formula& l : get_operands(f)) {
      AddLiteral(l);
    }
  } else {
    // f = b or f = ¬b.
    AddLiteral(f);
  }
  picosat_add(sat_, 0);
}

void SatSolver::MakeSatVar(const Variable& var) {
  auto it = to_sat_var_.find(var.get_id());
  if (it != to_sat_var_.end()) {
    // Found.
    return;
  }
  // It's not in the maps, let's make one and add it.
  const int sat_var{picosat_inc_max_var(sat_)};
  to_sat_var_.insert(var.get_id(), sat_var);
  to_sym_var_.insert(sat_var, var);
  DREAL_LOG_DEBUG("SatSolver::MakeSatVar({} ↦ {})", var, sat_var);
}
}  // namespace dreal
<|endoftext|>


<commit_before>/*=========================================================================

  Program:   ORFEO Toolbox
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) Centre National d'Etudes Spatiales. All rights reserved.
  See OTBCopyright.txt for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

#include "otbPipelineMemoryPrintCalculator.h"

#include "otbMath.h"
#include "otbImage.h"
#include "otbVectorImage.h"
#include "itkFixedArray.h"
#include "otbImageList.h"

namespace otb
{
const double PipelineMemoryPrintCalculator::ByteToMegabyte = 1./vcl_pow(2.0, 20);

PipelineMemoryPrintCalculator
::PipelineMemoryPrintCalculator()
  : m_MemoryPrint(0),
    m_AvailableMemory(256000000),
    m_OptimalNumberOfStreamDivisions(1),
    m_DataToWrite(NULL),
    m_BiasCorrectionFactor(1.),
    m_VisitedProcessObjects()
{}

PipelineMemoryPrintCalculator
::~PipelineMemoryPrintCalculator()
{}

void
PipelineMemoryPrintCalculator
::PrintSelf(std::ostream& os, itk::Indent indent) const
{
  // Call superclass implementation
  Superclass::PrintSelf(os, indent);

  // Display parameters
  os<<indent<<"Data to write:                      "<<m_DataToWrite<<std::endl;
  os<<indent<<"Memory print of whole pipeline:     "<<m_MemoryPrint * ByteToMegabyte <<" Mb"<<std::endl;
  os<<indent<<"Available memory:                   "<<m_AvailableMemory * ByteToMegabyte <<" Mb"<<std::endl;
  os<<indent<<"Optimal number of stream divisions: "<<m_OptimalNumberOfStreamDivisions<<std::endl;
  os<<indent<<"Bias correction factor applied:     "<<m_BiasCorrectionFactor<<std::endl;
}

void
PipelineMemoryPrintCalculator
::Compute()
{
  // Clear the visisted process objects set
  m_VisitedProcessObjects.clear();

  // Dry run of pipeline synchronisation
  m_DataToWrite->UpdateOutputInformation();
  m_DataToWrite->SetRequestedRegionToLargestPossibleRegion();
  m_DataToWrite->PropagateRequestedRegion();

  // Get the source process object
  ProcessObjectType * source = m_DataToWrite->GetSource();

  // Check if source exists
  if(source)
    {
    // Call the recursive memory print evaluation
    m_MemoryPrint = EvaluateMemoryPrint(source);
    }
  else
    {
    // Get memory print for this data only
    m_MemoryPrint = EvaluateDataObjectPrint(m_DataToWrite);
    }

  // Apply bias correction factor
  m_MemoryPrint *= m_BiasCorrectionFactor;

  // Compute the optimal number of stream division
  m_OptimalNumberOfStreamDivisions = vcl_ceil(static_cast<double>(m_MemoryPrint)
                                              /m_AvailableMemory);
}

PipelineMemoryPrintCalculator::MemoryPrintType
PipelineMemoryPrintCalculator
::EvaluateMemoryPrint(ProcessObjectType * process)
{
  // This variable will store the final print
  MemoryPrintType print = 0;

  // Check if this process object has already been visited
  if(m_VisitedProcessObjects.count(process))
    {
    return print;
    }
  // Else register it as a visited process object
  else
    {
    m_VisitedProcessObjects.insert(process);
    }

  // Retrieve the array of inputs
  ProcessObjectType::DataObjectPointerArray inputs = process->GetInputs();
  // First, recurse on each input source
  for(unsigned int i = 0; i < process->GetNumberOfInputs(); ++i)
    {
    // Retrieve the data object
    DataObjectType * input = inputs[i];

    // Retrieve possible source
    ProcessObjectType * source = input->GetSource();

    // If data object has a source
    if(source)
      {
      print += this->EvaluateMemoryPrint(source);
      }
    else
      {
      MemoryPrintType localPrint = this->EvaluateDataObjectPrint(input);
      print += localPrint;
      }
    }
  // Retrieve the output array
  ProcessObjectType::DataObjectPointerArray outputs = process->GetOutputs();

  // Now, evaluate the current object print
  for(unsigned int i = 0; i < process->GetNumberOfOutputs(); ++i)
    {
    MemoryPrintType localPrint = this->EvaluateDataObjectPrint(outputs[0]);
    print += localPrint;
    }

  // Finally, return the total print
  return print;
}

PipelineMemoryPrintCalculator::MemoryPrintType
PipelineMemoryPrintCalculator
::EvaluateDataObjectPrint(DataObjectType * data) const
{

#define OTB_IMAGE_SIZE_BLOCK(type)                                      \
  if(dynamic_cast<itk::Image<type, 2> *>(data) != NULL)                  \
    {                                                                   \
    itk::Image<type, 2> * image = dynamic_cast<itk::Image<type, 2> *>(data); \
    return image->GetRequestedRegion().GetNumberOfPixels()              \
      * image->GetNumberOfComponentsPerPixel() * sizeof(type); \
    }                                                                   \
  if(dynamic_cast<itk::VectorImage<type, 2> * >(data) != NULL)           \
    {                                                                   \
    itk::VectorImage<type, 2> * image = dynamic_cast<itk::VectorImage<type, 2> *>(data); \
    return image->GetRequestedRegion().GetNumberOfPixels()              \
      * image->GetNumberOfComponentsPerPixel() * sizeof(type); \
    }                                                                   \
  if(dynamic_cast<ImageList<Image<type, 2> > *>(data) != NULL)   \
    {                                                                   \
    ImageList<Image<type, 2> > * imageList = dynamic_cast<otb::ImageList<otb::Image<type, 2> > *>(data); \
    MemoryPrintType print(0);                                         \
    for(ImageList<Image<type, 2> >::ConstIterator it = imageList->Begin(); \
       it != imageList->End(); ++it)                                    \
       {                                                             \
       print += it.Get()->GetRequestedRegion().GetNumberOfPixels()   \
       * it.Get()->GetNumberOfComponentsPerPixel() * sizeof(type); \
       }                                                           \
    return print;                                                  \
    }                                                              \
  if(dynamic_cast<ImageList<VectorImage<type, 2> > *>(data) != NULL)   \
    {                                                                   \
    ImageList<VectorImage<type, 2> > * imageList = dynamic_cast<otb::ImageList<otb::VectorImage<type, 2> > *>(data); \
    MemoryPrintType print(0);                                         \
    for(ImageList<VectorImage<type, 2> >::ConstIterator it = imageList->Begin(); \
       it != imageList->End(); ++it)                                    \
       {                                                             \
       print += it.Get()->GetRequestedRegion().GetNumberOfPixels()   \
       * it.Get()->GetNumberOfComponentsPerPixel() * sizeof(type); \
       }                                                           \
    return print;                                                  \
    }                                                              \

  // Call the macro for each pixel type
  OTB_IMAGE_SIZE_BLOCK(unsigned char)
  OTB_IMAGE_SIZE_BLOCK(char)
  OTB_IMAGE_SIZE_BLOCK(unsigned short)
  OTB_IMAGE_SIZE_BLOCK(short)
  OTB_IMAGE_SIZE_BLOCK(unsigned int)
  OTB_IMAGE_SIZE_BLOCK(int)
  OTB_IMAGE_SIZE_BLOCK(unsigned long)
  OTB_IMAGE_SIZE_BLOCK(long)
  OTB_IMAGE_SIZE_BLOCK(float)
  OTB_IMAGE_SIZE_BLOCK(double)
  OTB_IMAGE_SIZE_BLOCK(std::complex<float>)
  OTB_IMAGE_SIZE_BLOCK(std::complex<double>)
  typedef itk::FixedArray<float, 2> FloatFixedArray2Type;
  typedef itk::FixedArray<float, 2> DoubleFixedArray2Type;
  OTB_IMAGE_SIZE_BLOCK(FloatFixedArray2Type)
  OTB_IMAGE_SIZE_BLOCK(DoubleFixedArray2Type)

  // If we are still here, none of the macro call succeed
  return 0;
}

} // End namespace otb
<commit_msg>ENH: add a MsgDevMacro to show what objects are visited<commit_after>/*=========================================================================

  Program:   ORFEO Toolbox
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) Centre National d'Etudes Spatiales. All rights reserved.
  See OTBCopyright.txt for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

#include "otbPipelineMemoryPrintCalculator.h"

#include "otbMath.h"
#include "otbImage.h"
#include "otbVectorImage.h"
#include "itkFixedArray.h"
#include "otbImageList.h"

namespace otb
{
const double PipelineMemoryPrintCalculator::ByteToMegabyte = 1./vcl_pow(2.0, 20);

PipelineMemoryPrintCalculator
::PipelineMemoryPrintCalculator()
  : m_MemoryPrint(0),
    m_AvailableMemory(256000000),
    m_OptimalNumberOfStreamDivisions(1),
    m_DataToWrite(NULL),
    m_BiasCorrectionFactor(1.),
    m_VisitedProcessObjects()
{}

PipelineMemoryPrintCalculator
::~PipelineMemoryPrintCalculator()
{}

void
PipelineMemoryPrintCalculator
::PrintSelf(std::ostream& os, itk::Indent indent) const
{
  // Call superclass implementation
  Superclass::PrintSelf(os, indent);

  // Display parameters
  os<<indent<<"Data to write:                      "<<m_DataToWrite<<std::endl;
  os<<indent<<"Memory print of whole pipeline:     "<<m_MemoryPrint * ByteToMegabyte <<" Mb"<<std::endl;
  os<<indent<<"Available memory:                   "<<m_AvailableMemory * ByteToMegabyte <<" Mb"<<std::endl;
  os<<indent<<"Optimal number of stream divisions: "<<m_OptimalNumberOfStreamDivisions<<std::endl;
  os<<indent<<"Bias correction factor applied:     "<<m_BiasCorrectionFactor<<std::endl;
}

void
PipelineMemoryPrintCalculator
::Compute()
{
  // Clear the visited process objects set
  m_VisitedProcessObjects.clear();

  // Dry run of pipeline synchronisation
  m_DataToWrite->UpdateOutputInformation();
  m_DataToWrite->SetRequestedRegionToLargestPossibleRegion();
  m_DataToWrite->PropagateRequestedRegion();

  // Get the source process object
  ProcessObjectType * source = m_DataToWrite->GetSource();

  // Check if source exists
  if(source)
    {
    // Call the recursive memory print evaluation
    m_MemoryPrint = EvaluateMemoryPrint(source);
    }
  else
    {
    // Get memory print for this data only
    m_MemoryPrint = EvaluateDataObjectPrint(m_DataToWrite);
    }

  // Apply bias correction factor
  m_MemoryPrint *= m_BiasCorrectionFactor;

  // Compute the optimal number of stream division
  m_OptimalNumberOfStreamDivisions = vcl_ceil(static_cast<double>(m_MemoryPrint)
                                              /m_AvailableMemory);
}

PipelineMemoryPrintCalculator::MemoryPrintType
PipelineMemoryPrintCalculator
::EvaluateMemoryPrint(ProcessObjectType * process)
{
  otbMsgDevMacro(<< "EvaluateMemoryPrint for " << process->GetNameOfClass() << " (" << process << ")")
  // This variable will store the final print
  MemoryPrintType print = 0;

  // Check if this process object has already been visited
  if(m_VisitedProcessObjects.count(process))
    {
    return print;
    }
  // Else register it as a visited process object
  else
    {
    m_VisitedProcessObjects.insert(process);
    }

  // Retrieve the array of inputs
  ProcessObjectType::DataObjectPointerArray inputs = process->GetInputs();
  // First, recurse on each input source
  for(unsigned int i = 0; i < process->GetNumberOfInputs(); ++i)
    {
    // Retrieve the data object
    DataObjectType * input = inputs[i];

    // Retrieve possible source
    ProcessObjectType * source = input->GetSource();

    // If data object has a source
    if(source)
      {
      print += this->EvaluateMemoryPrint(source);
      }
    else
      {
      MemoryPrintType localPrint = this->EvaluateDataObjectPrint(input);
      print += localPrint;
      }
    }
  // Retrieve the output array
  ProcessObjectType::DataObjectPointerArray outputs = process->GetOutputs();

  // Now, evaluate the current object print
  for(unsigned int i = 0; i < process->GetNumberOfOutputs(); ++i)
    {
    MemoryPrintType localPrint = this->EvaluateDataObjectPrint(outputs[0]);
    print += localPrint;
    }

  // Finally, return the total print
  return print;
}

PipelineMemoryPrintCalculator::MemoryPrintType
PipelineMemoryPrintCalculator
::EvaluateDataObjectPrint(DataObjectType * data) const
{
  otbMsgDevMacro(<< "EvaluateMemoryPrint for " << data->GetNameOfClass() << " (" << data << ")")

#define OTB_IMAGE_SIZE_BLOCK(type)                                      \
  if(dynamic_cast<itk::Image<type, 2> *>(data) != NULL)                  \
    {                                                                   \
    itk::Image<type, 2> * image = dynamic_cast<itk::Image<type, 2> *>(data); \
    return image->GetRequestedRegion().GetNumberOfPixels()              \
      * image->GetNumberOfComponentsPerPixel() * sizeof(type); \
    }                                                                   \
  if(dynamic_cast<itk::VectorImage<type, 2> * >(data) != NULL)           \
    {                                                                   \
    itk::VectorImage<type, 2> * image = dynamic_cast<itk::VectorImage<type, 2> *>(data); \
    return image->GetRequestedRegion().GetNumberOfPixels()              \
      * image->GetNumberOfComponentsPerPixel() * sizeof(type); \
    }                                                                   \
  if(dynamic_cast<ImageList<Image<type, 2> > *>(data) != NULL)   \
    {                                                                   \
    ImageList<Image<type, 2> > * imageList = dynamic_cast<otb::ImageList<otb::Image<type, 2> > *>(data); \
    MemoryPrintType print(0);                                         \
    for(ImageList<Image<type, 2> >::ConstIterator it = imageList->Begin(); \
       it != imageList->End(); ++it)                                    \
       {                                                             \
       print += it.Get()->GetRequestedRegion().GetNumberOfPixels()   \
       * it.Get()->GetNumberOfComponentsPerPixel() * sizeof(type); \
       }                                                           \
    return print;                                                  \
    }                                                              \
  if(dynamic_cast<ImageList<VectorImage<type, 2> > *>(data) != NULL)   \
    {                                                                   \
    ImageList<VectorImage<type, 2> > * imageList = dynamic_cast<otb::ImageList<otb::VectorImage<type, 2> > *>(data); \
    MemoryPrintType print(0);                                         \
    for(ImageList<VectorImage<type, 2> >::ConstIterator it = imageList->Begin(); \
       it != imageList->End(); ++it)                                    \
       {                                                             \
       print += it.Get()->GetRequestedRegion().GetNumberOfPixels()   \
       * it.Get()->GetNumberOfComponentsPerPixel() * sizeof(type); \
       }                                                           \
    return print;                                                  \
    }                                                              \

  // Call the macro for each pixel type
  OTB_IMAGE_SIZE_BLOCK(unsigned char)
  OTB_IMAGE_SIZE_BLOCK(char)
  OTB_IMAGE_SIZE_BLOCK(unsigned short)
  OTB_IMAGE_SIZE_BLOCK(short)
  OTB_IMAGE_SIZE_BLOCK(unsigned int)
  OTB_IMAGE_SIZE_BLOCK(int)
  OTB_IMAGE_SIZE_BLOCK(unsigned long)
  OTB_IMAGE_SIZE_BLOCK(long)
  OTB_IMAGE_SIZE_BLOCK(float)
  OTB_IMAGE_SIZE_BLOCK(double)
  OTB_IMAGE_SIZE_BLOCK(std::complex<float>)
  OTB_IMAGE_SIZE_BLOCK(std::complex<double>)
  typedef itk::FixedArray<float, 2> FloatFixedArray2Type;
  typedef itk::FixedArray<float, 2> DoubleFixedArray2Type;
  OTB_IMAGE_SIZE_BLOCK(FloatFixedArray2Type)
  OTB_IMAGE_SIZE_BLOCK(DoubleFixedArray2Type)

  // If we are still here, none of the macro call succeed
  return 0;
}

} // End namespace otb
<|endoftext|>


<commit_before>//
// Log - simple message logging to a file.
//
// Copyright (c) 2002-2003 Virtual Terrain Project
// Free for all uses, see license.txt for details.
//

#include "cpl_error.h"	// From GDAL/include
#include "vtString.h"
#include "vtLog.h"
#include <stdarg.h>

#ifndef __DARWIN_OSX__
#include <wchar.h>		// for fputws()
#endif

#ifdef _MSC_VER
#include <windows.h>	// for OutputDebugString, unfortunately
#endif

vtLog g_Log;

void cpl_error_handler(CPLErr eErrClass, int err_no, const char *msg)
{
	if (eErrClass == CE_Debug)
		g_Log._Log("CPL Debug: ");
	else if (eErrClass == CE_Warning)
		g_Log.Printf("CPL Warning %d: ", err_no);
	else
		g_Log.Printf("CPL Error %d: ", err_no);
	g_Log._Log(msg);
	g_Log._Log("\n");
}

vtLog::vtLog()
{
	m_log = NULL;
}

vtLog::~vtLog()
{
	if (m_log)
	{
		CPLPopErrorHandler();
		fclose(m_log);
		m_log = NULL;
	}
}

void vtLog::_StartLog(const char *fname)
{
	m_log = fopen(fname, "wb");
	CPLPushErrorHandler(cpl_error_handler);
}

void vtLog::_Log(const char *msg)
{
	if (m_log)
	{
		fputs(msg, m_log);
		fflush(m_log);
	}
#ifdef _MSC_VER
	OutputDebugStringA(msg);
#endif
}

void vtLog::_Log(const wchar_t *msg)
{
#ifndef __DARWIN_OSX__
	if (m_log)
	{
		// it is not so useful to write wide characters to the file, which
		// otherwise contains 8-bit text, so convert back first
//		fputws(msg, m_log);
		wstring2 str = msg;
		fputs(str.eb_str(), m_log);
		fflush(m_log);
	}
#endif	// __DARWIN_OSX__

#ifdef _MSC_VER
	OutputDebugStringW(msg);
#endif
}

void vtLog::Printf(const char *pFormat, ...)
{
	va_list va;
	va_start(va, pFormat);

	char ach[1024];
	vsprintf(ach, pFormat, va);

	_Log(ach);
}

void vtLog::Printf(const wchar_t *pFormat, ...)
{
#ifndef __DARWIN_OSX__
	va_list va;
	va_start(va, pFormat);

#if defined (__DARWIN_OSX__) || defined (macintosh)
	// Mac seems to not have all the ANSI functions like vswprintf, so
	//  convert the format string to 8-bit and use vsprintf instead.
	char ach[1024];
	wstring2 strFormat = pFormat;
	vsprintf(ach, strFormat.eb_str(), va);
#else
	// Use wide characters
	wchar_t ach[1024];
#ifdef _MSC_VER
	vswprintf(ach, pFormat, va);
#else
	// apparently on non-MSVC platforms this takes 4 arguments (safer)
	vswprintf(ach, 1024, pFormat, va);
#endif
#endif

	_Log(ach);
#endif	// __DARWIN_OSX__
}
<commit_msg>send log to stdout, for those console-mode developers<commit_after>//
// Log - simple message logging to a file.
//
// Copyright (c) 2002-2003 Virtual Terrain Project
// Free for all uses, see license.txt for details.
//

#include "cpl_error.h"	// From GDAL/include
#include "vtString.h"
#include "vtLog.h"
#include <stdarg.h>

#ifndef __DARWIN_OSX__
#include <wchar.h>		// for fputws()
#endif

#ifdef _MSC_VER
#include <windows.h>	// for OutputDebugString, unfortunately
#endif

vtLog g_Log;

void cpl_error_handler(CPLErr eErrClass, int err_no, const char *msg)
{
	if (eErrClass == CE_Debug)
		g_Log._Log("CPL Debug: ");
	else if (eErrClass == CE_Warning)
		g_Log.Printf("CPL Warning %d: ", err_no);
	else
		g_Log.Printf("CPL Error %d: ", err_no);
	g_Log._Log(msg);
	g_Log._Log("\n");
}

vtLog::vtLog()
{
	m_log = NULL;
}

vtLog::~vtLog()
{
	if (m_log)
	{
		CPLPopErrorHandler();
		fclose(m_log);
		m_log = NULL;
	}
}

void vtLog::_StartLog(const char *fname)
{
	m_log = fopen(fname, "wb");
	CPLPushErrorHandler(cpl_error_handler);
}

void vtLog::_Log(const char *msg)
{
	if (m_log)
	{
		fputs(msg, m_log);
		fflush(m_log);
	}
#ifdef _MSC_VER
	OutputDebugStringA(msg);
#endif
	// also send to the console, for those console-mode developers!
	fputs(msg, stdout);
}

void vtLog::_Log(const wchar_t *msg)
{
#ifndef __DARWIN_OSX__
	if (m_log)
	{
		// it is not so useful to write wide characters to the file, which
		// otherwise contains 8-bit text, so convert back first
//		fputws(msg, m_log);
		wstring2 str = msg;
		fputs(str.eb_str(), m_log);
		fflush(m_log);
	}
#endif	// __DARWIN_OSX__

#ifdef _MSC_VER
	OutputDebugStringW(msg);
#endif
}

void vtLog::Printf(const char *pFormat, ...)
{
	va_list va;
	va_start(va, pFormat);

	char ach[2048];
	vsprintf(ach, pFormat, va);

	_Log(ach);
}

void vtLog::Printf(const wchar_t *pFormat, ...)
{
#ifndef __DARWIN_OSX__
	va_list va;
	va_start(va, pFormat);

#if defined (__DARWIN_OSX__) || defined (macintosh)
	// Mac seems to not have all the ANSI functions like vswprintf, so
	//  convert the format string to 8-bit and use vsprintf instead.
	char ach[1024];
	wstring2 strFormat = pFormat;
	vsprintf(ach, strFormat.eb_str(), va);
#else
	// Use wide characters
	wchar_t ach[1024];
#ifdef _MSC_VER
	vswprintf(ach, pFormat, va);
#else
	// apparently on non-MSVC platforms this takes 4 arguments (safer)
	vswprintf(ach, 1024, pFormat, va);
#endif
#endif

	_Log(ach);
#endif	// __DARWIN_OSX__
}
<|endoftext|>


<commit_before>/* mbed Microcontroller Library
 * Copyright (c) 2006-2019 ARM Limited
 * SPDX-License-Identifier: Apache-2.0
 *
 * 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.
 */

//#if MBED_CONF_NSAPI_PRESENT

//#if EP_ATLAS_ENABLE_CELL

#include "gpio_api.h"
#include "platform/mbed_thread.h"
#include "PinNames.h"
//#include "mbed.h"

#include "UARTSerial.h"
#include "ONBOARD_TELIT_ME310.h"
#include "ThisThread.h"
#include "CellularLog.h"

using namespace mbed;

ONBOARD_TELIT_ME310::ONBOARD_TELIT_ME310(FileHandle *fh) : TELIT_ME310(fh, P0_31, true)
{
}

nsapi_error_t ONBOARD_TELIT_ME310::hard_power_on()
{
    onboard_modem_init();
    return NSAPI_ERROR_OK;
}

nsapi_error_t ONBOARD_TELIT_ME310::hard_power_off()
{
    onboard_modem_deinit();
    return NSAPI_ERROR_OK;
}

nsapi_error_t ONBOARD_TELIT_ME310::soft_power_on()
{
    onboard_modem_power_up();
    // From Telit_xE310 Global form factor App note: It is mandatory to avoid sending data to the serial ports during the first 200ms of the module start-up.
    rtos::ThisThread::sleep_for(200);
    return NSAPI_ERROR_OK;
}

nsapi_error_t ONBOARD_TELIT_ME310::soft_power_off()
{
    onboard_modem_power_down();
    return NSAPI_ERROR_OK;
}

nsapi_error_t ONBOARD_TELIT_ME310::init()
{
    nsapi_error_t err = AT_CellularDevice::init();
    if (err != NSAPI_ERROR_OK) {
        return err;
    }
    _at->lock();
#if DEVICE_SERIAL_FC
    _at->at_cmd_discard("&K3;&C1;&D0", "");
#else
    _at->at_cmd_discard("&K0;&C1;&D0", "");
#endif

    // AT#QSS=1
    // Enable the Query SIM Status unsolicited indication in the ME. The format of the
    // unsolicited indication is the following:
    // #QSS: <status>
    // The ME informs at
    // every SIM status change through the basic unsolicited indication where <status> range is 0...1
    // <status> values:
    // - 0: SIM not inserted
    // - 1: SIM inserted
    _at->at_cmd_discard("#QSS", "=1");

    // AT#PSNT=1
    // Set command enables unsolicited result code for packet service network type (PSNT)
    // having the following format:
    // #PSNT:<nt>
    // <nt> values:
    // - 0: GPRS network
    // - 4: LTE network
    // - 5: unknown or not registered
    _at->at_cmd_discard("#PSNT", "=1");

    // AT+CMER=2
    // Set command enables sending of unsolicited result codes from TA to TE in the case of
    // indicator state changes.
    // Current setting: buffer +CIEV Unsolicited Result Codes in the TA when TA-TE link is
    // reserved (e.g. on-line data mode) and flush them to the TE after
    // reservation; otherwise forward them directly to the TE
    _at->at_cmd_discard("+CMER", "=2");

    // AT+CMEE=2
    // Set command disables the use of result code +CME ERROR: <err> as an indication of an
    // error relating to the +Cxxx command issued. When enabled, device related errors cause the +CME
    // ERROR: <err> final result code instead of the default ERROR final result code. ERROR is returned
    // normally when the error message is related to syntax, invalid parameters or DTE functionality.
    // Current setting: enable and use verbose <err> values
    _at->at_cmd_discard("+CMEE", "=2");

    // AT#PORTCFG=0
    // Set command allows to connect Service Access Points to the external physical ports giving a great
    // flexibility. Examples of Service Access Points: AT Parser Instance #1, #2, #3, etc..
    _at->at_cmd_discard("#PORTCFG", "=", "%d", EP_ATLAS_PORT_CONFIGURATION_VARIANT);

    // AT&W&P
    // - AT&W: Execution command stores on profile <n> the complete configuration of the device. If
    //         parameter is omitted, the command has the same behavior of AT&W0.
    // - AT&P: Execution command defines which full profile will be loaded at startup. If parameter
    //         is omitted, the command has the same behavior as AT&P0
    _at->at_cmd_discard("&W&P", "");

    return _at->unlock_return_error();
}

void ONBOARD_TELIT_ME310::press_power_button(int time_ms)
{
    gpio_t gpio_CELL_ON_OFF;
    //gpio_t gpio_PWR_MON;
    // volatile int read_pwr_mon_gpio = 0;

    // gpio_init_in(&gpio_PWR_MON, PIN_NAME_CELL_PWRMON);
    // read_pwr_mon_gpio = gpio_read(&gpio_PWR_MON);

//    if(!(gpio_read(&gpio_PWR_MON)))
//   {
//         gpio_init_out_ex(&gpio_CELL_ON_OFF, P0_31, 1);
//         gpio_write(&gpio_CELL_ON_OFF, 1);
//         thread_sleep_for(time_ms);
//         gpio_write(&gpio_CELL_ON_OFF, 0);
//     }

gpio_init_out_ex(&gpio_CELL_ON_OFF, P0_31, 1);
gpio_write(&gpio_CELL_ON_OFF, 1);

//     DigitalIn pwr_mon(PIN_NAME_CELL_PWRMON);
//     DigitalInOut cell_on_off(P0_31,PIN_INPUT,PullNone,1);

//    if(!pwr_mon.read())
//    {
//     cell_on_off.output();
//     cell_on_off = 0;
//     ThisThread::sleep_for(time_ms);
//     cell_on_off = 1;
//     }

}

void ONBOARD_TELIT_ME310::onboard_modem_init()
{
    // gpio_t gpio;

    // gpio_init_out_ex(&gpio, PIN_NAME_CELL_POWER_ENABLE, 0);
    // gpio_write(&gpio, 1);
}

void ONBOARD_TELIT_ME310::onboard_modem_deinit()
{
    // gpio_t gpio;

    // gpio_init_out_ex(&gpio, PIN_NAME_CELL_POWER_ENABLE, 1);
    // gpio_write(&gpio, 0);
}

void ONBOARD_TELIT_ME310::onboard_modem_power_up()
{
    /* keep the power line low for 5 seconds */
    press_power_button(6000);
    /* give modem a little time to respond */
}

void ONBOARD_TELIT_ME310::onboard_modem_power_down()
{
    gpio_t gpio;

    gpio_init_out_ex(&gpio, P0_31, 0);
    /* keep the power line low for more than 3 seconds.
     * If 3G_ON_OFF pin is kept low for more than a second, a controlled disconnect and shutdown takes
     * place, Due to the network disconnect, shut-off can take up to 30 seconds. However, we wait for 10
     * seconds only   */
    thread_sleep_for(10 * 1000);
}

CellularDevice *CellularDevice::get_target_default_instance()
{
    static UARTSerial serial(P1_2, P1_1, 115200);
#if DEVICE_SERIAL_FC
    if (P0_11 != NC && P1_8 != NC) {
        tr_debug("Modem flow control: RTS %d CTS %d", P0_11, P1_8);
        serial.set_flow_control(SerialBase::RTSCTS, P0_11, P1_8);
    }
#endif
    static ONBOARD_TELIT_ME310 device(&serial);
    return &device;
}

//#endif // EP_ATLAS_ENABLE_CELL

//#endif // MBED_CONF_NSAPI_PRESENT
<commit_msg>updates for mbed os 6<commit_after>/* mbed Microcontroller Library
 * Copyright (c) 2006-2019 ARM Limited
 * SPDX-License-Identifier: Apache-2.0
 *
 * 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.
 */

//#if MBED_CONF_NSAPI_PRESENT

//#if EP_ATLAS_ENABLE_CELL

#include "gpio_api.h"
#include "platform/mbed_thread.h"
#include "PinNames.h"
//#include "mbed.h"

#include "BufferedSerial.h"
#include "ONBOARD_TELIT_ME310.h"
#include "ThisThread.h"
#include "CellularLog.h"

using namespace mbed;

ONBOARD_TELIT_ME310::ONBOARD_TELIT_ME310(FileHandle *fh) : TELIT_ME310(fh, P0_31, true)
{
}

nsapi_error_t ONBOARD_TELIT_ME310::hard_power_on()
{
    onboard_modem_init();
    return NSAPI_ERROR_OK;
}

nsapi_error_t ONBOARD_TELIT_ME310::hard_power_off()
{
    onboard_modem_deinit();
    return NSAPI_ERROR_OK;
}

nsapi_error_t ONBOARD_TELIT_ME310::soft_power_on()
{
    onboard_modem_power_up();
    // From Telit_xE310 Global form factor App note: It is mandatory to avoid sending data to the serial ports during the first 200ms of the module start-up.
    rtos::ThisThread::sleep_for(200);
    return NSAPI_ERROR_OK;
}

nsapi_error_t ONBOARD_TELIT_ME310::soft_power_off()
{
    onboard_modem_power_down();
    return NSAPI_ERROR_OK;
}

nsapi_error_t ONBOARD_TELIT_ME310::init()
{
    nsapi_error_t err = AT_CellularDevice::init();
    if (err != NSAPI_ERROR_OK) {
        return err;
    }
    _at.lock();
#if DEVICE_SERIAL_FC
    _at.at_cmd_discard("&K3;&C1;&D0", "");
#else
    _at.at_cmd_discard("&K0;&C1;&D0", "");
#endif

    // AT#QSS=1
    // Enable the Query SIM Status unsolicited indication in the ME. The format of the
    // unsolicited indication is the following:
    // #QSS: <status>
    // The ME informs at
    // every SIM status change through the basic unsolicited indication where <status> range is 0...1
    // <status> values:
    // - 0: SIM not inserted
    // - 1: SIM inserted
    _at.at_cmd_discard("#QSS", "=1");

    // AT#PSNT=1
    // Set command enables unsolicited result code for packet service network type (PSNT)
    // having the following format:
    // #PSNT:<nt>
    // <nt> values:
    // - 0: GPRS network
    // - 4: LTE network
    // - 5: unknown or not registered
    _at.at_cmd_discard("#PSNT", "=1");

    // AT+CMER=2
    // Set command enables sending of unsolicited result codes from TA to TE in the case of
    // indicator state changes.
    // Current setting: buffer +CIEV Unsolicited Result Codes in the TA when TA-TE link is
    // reserved (e.g. on-line data mode) and flush them to the TE after
    // reservation; otherwise forward them directly to the TE
    _at.at_cmd_discard("+CMER", "=2");

    // AT+CMEE=2
    // Set command disables the use of result code +CME ERROR: <err> as an indication of an
    // error relating to the +Cxxx command issued. When enabled, device related errors cause the +CME
    // ERROR: <err> final result code instead of the default ERROR final result code. ERROR is returned
    // normally when the error message is related to syntax, invalid parameters or DTE functionality.
    // Current setting: enable and use verbose <err> values
    _at.at_cmd_discard("+CMEE", "=2");

    // AT#PORTCFG=0
    // Set command allows to connect Service Access Points to the external physical ports giving a great
    // flexibility. Examples of Service Access Points: AT Parser Instance #1, #2, #3, etc..
    _at.at_cmd_discard("#PORTCFG", "=", "%d", EP_ATLAS_PORT_CONFIGURATION_VARIANT);

    // AT&W&P
    // - AT&W: Execution command stores on profile <n> the complete configuration of the device. If
    //         parameter is omitted, the command has the same behavior of AT&W0.
    // - AT&P: Execution command defines which full profile will be loaded at startup. If parameter
    //         is omitted, the command has the same behavior as AT&P0
    _at.at_cmd_discard("&W&P", "");

    return _at.unlock_return_error();
}

void ONBOARD_TELIT_ME310::press_power_button(int time_ms)
{
    gpio_t gpio_CELL_ON_OFF;
    //gpio_t gpio_PWR_MON;
    // volatile int read_pwr_mon_gpio = 0;

    // gpio_init_in(&gpio_PWR_MON, PIN_NAME_CELL_PWRMON);
    // read_pwr_mon_gpio = gpio_read(&gpio_PWR_MON);

//    if(!(gpio_read(&gpio_PWR_MON)))
//   {
//         gpio_init_out_ex(&gpio_CELL_ON_OFF, P0_31, 1);
//         gpio_write(&gpio_CELL_ON_OFF, 1);
//         thread_sleep_for(time_ms);
//         gpio_write(&gpio_CELL_ON_OFF, 0);
//     }

gpio_init_out_ex(&gpio_CELL_ON_OFF, P0_31, 1);
gpio_write(&gpio_CELL_ON_OFF, 1);

//     DigitalIn pwr_mon(PIN_NAME_CELL_PWRMON);
//     DigitalInOut cell_on_off(P0_31,PIN_INPUT,PullNone,1);

//    if(!pwr_mon.read())
//    {
//     cell_on_off.output();
//     cell_on_off = 0;
//     ThisThread::sleep_for(time_ms);
//     cell_on_off = 1;
//     }

}

void ONBOARD_TELIT_ME310::onboard_modem_init()
{
    // gpio_t gpio;

    // gpio_init_out_ex(&gpio, PIN_NAME_CELL_POWER_ENABLE, 0);
    // gpio_write(&gpio, 1);
}

void ONBOARD_TELIT_ME310::onboard_modem_deinit()
{
    // gpio_t gpio;

    // gpio_init_out_ex(&gpio, PIN_NAME_CELL_POWER_ENABLE, 1);
    // gpio_write(&gpio, 0);
}

void ONBOARD_TELIT_ME310::onboard_modem_power_up()
{
    /* keep the power line low for 5 seconds */
    press_power_button(6000);
    /* give modem a little time to respond */
}

void ONBOARD_TELIT_ME310::onboard_modem_power_down()
{
    gpio_t gpio;

    gpio_init_out_ex(&gpio, P0_31, 0);
    /* keep the power line low for more than 3 seconds.
     * If 3G_ON_OFF pin is kept low for more than a second, a controlled disconnect and shutdown takes
     * place, Due to the network disconnect, shut-off can take up to 30 seconds. However, we wait for 10
     * seconds only   */
    thread_sleep_for(10 * 1000);
}

CellularDevice *CellularDevice::get_target_default_instance()
{
    static BufferedSerial serial(P1_2, P1_1, 115200);
#if DEVICE_SERIAL_FC
    if (P0_11 != NC && P1_8 != NC) {
        tr_debug("Modem flow control: RTS %d CTS %d", P0_11, P1_8);
        serial.set_flow_control(SerialBase::RTSCTS, P0_11, P1_8);
    }
#endif
    static ONBOARD_TELIT_ME310 device(&serial);
    return &device;
}

//#endif // EP_ATLAS_ENABLE_CELL

//#endif // MBED_CONF_NSAPI_PRESENT
<|endoftext|>


<commit_before>#ifndef GLM_FORCE_RADIANS
#define GLM_FORCE_RADIANS
#define DEGREES_TO_RADIANS(x) (x * PI / 180.0f)
#endif

#include "las_loader.hpp"
#include "code/ylikuutio/triangulation/quad_triangulation.hpp"
#include "code/ylikuutio/common/globals.hpp"
#include "code/ylikuutio/common/global_variables.hpp"

// Include GLM
#ifndef __GLM_GLM_HPP_INCLUDED
#define __GLM_GLM_HPP_INCLUDED
#include <glm/glm.hpp> // glm
#endif

// Include standard headers
#include <cmath>    // NAN, std::isnan, std::pow
#include <cstdio>   // std::FILE, std::fclose, std::fopen, std::fread, std::getchar, std::printf etc.
#include <iostream> // std::cout, std::cin, std::cerr
#include <stdint.h> // uint32_t etc.
#include <string>   // std::string
#include <vector>   // std::vector

namespace loaders
{
    bool load_LASF_data(
            std::string laser_scaling_file_name,
            std::vector<glm::vec3>& laser_points)
    {
        std::cout << "Loading LASF file " << laser_scaling_file_name << " ...\n";

        // Data read from the header of the LASF file
        uint8_t header[227];

        // Open the file
        const char* char_laser_scaling_file_name = laser_scaling_file_name.c_str();
        std::FILE* file = std::fopen(char_laser_scaling_file_name, "rb");
        if (!file)
        {
            std::cerr << laser_scaling_file_name << " could not be opened.\n";
            return false;
        }

        // Read the header, i.e. the 227 first bytes

        // If less than 227 bytes are read, it's a problem.
        if (std::fread(header, 1, 227, file) != 227)
        {
            std::cerr << "Too few bytes read when reading the file header. " << laser_scaling_file_name << " is not a correct LASF file.\n";
            std::fclose(file);
            return false;
        }

        // A LASF files always begins with "LASF"
        if ((header[0] != 'L') || (header[1] != 'A') || (header[2] != 'S') || (header[3] != 'F'))
        {
            std::cerr << "No LASF signature. " << laser_scaling_file_name << " is not a correct LASF file.\n";
            std::fclose(file);
            return false;
        }

        const uint32_t max_number_of_returns = 4;

        uint32_t sizes_of_point_record_formats[4];
        sizes_of_point_record_formats[0] = 20; // 4 + 4 + 4 + 2 + 1* + 1 + 1 + 1 + 2. 1*) 3 bits + 3 bits + 1 bit + 1 bit.
        sizes_of_point_record_formats[1] = 28; // 4 + 4 + 4 + 2 + 1* + 1 + 1 + 1 + 2 + 8. 1*) 3 bits + 3 bits + 1 bit + 1 bit.
        sizes_of_point_record_formats[2] = 26; // 4 + 4 + 4 + 2 + 1* + 1 + 1 + 1 + 2 + 2 + 2 + 2. 1*) 3 bits + 3 bits + 1 bit + 1 bit.
        sizes_of_point_record_formats[3] = 34; // 4 + 4 + 4 + 2 + 1* + 1 + 1 + 1 + 2 + 8 + 2 + 2 + 2. 1*) 3 bits + 3 bits + 1 bit + 1 bit.

        uint16_t* pointer_to_16_bit_header_data = (uint16_t*) (header + 94);
        const uint16_t header_size_in_bytes = *pointer_to_16_bit_header_data++;

        if (header_size_in_bytes != 227)
        {
            std::cerr << "Invalid header size in file " << laser_scaling_file_name << " (" << header_size_in_bytes << " bytes). Header size must be 227 bytes.\n";
            std::fclose(file);
            return false;
        }

        uint32_t* pointer_to_32_bit_header_data = (uint32_t*) (header + 96);

        const uint32_t offset_to_point_data = *pointer_to_32_bit_header_data++;
        const uint32_t number_of_variable_length_records = *pointer_to_32_bit_header_data++;

        uint8_t* pointer_to_8_bit_header_data = (uint8_t*) pointer_to_32_bit_header_data;

        const uint8_t point_data_formatID = *pointer_to_8_bit_header_data++;

        if (point_data_formatID > 3)
        {
            std::cerr << "invalid point data format ID: " << static_cast<int16_t>(point_data_formatID) << " in file " << laser_scaling_file_name << "\n";
            return false;
        }

        // OK, the point data format ID is valid (0, 1, 2 or 3).

        pointer_to_16_bit_header_data = (uint16_t*) pointer_to_8_bit_header_data;

        const uint8_t point_data_record_length = *pointer_to_16_bit_header_data++;

        pointer_to_32_bit_header_data = (uint32_t*) pointer_to_16_bit_header_data;

        const uint32_t number_of_point_records = *pointer_to_32_bit_header_data++;
        uint32_t number_of_points_by_record[max_number_of_returns + 1];

        for (uint32_t n_returns = 0; n_returns <= max_number_of_returns; n_returns++)
        {
            number_of_points_by_record[n_returns] = *pointer_to_32_bit_header_data++;
        }

        double* pointer_to_double_header_data = (double*) pointer_to_32_bit_header_data;

        const double x_scale_factor = *pointer_to_double_header_data++;
        const double y_scale_factor = *pointer_to_double_header_data++;
        const double z_scale_factor = *pointer_to_double_header_data++;
        const double x_offset = *pointer_to_double_header_data++;
        const double y_offset = *pointer_to_double_header_data++;
        const double z_offset = *pointer_to_double_header_data++;
        const double max_x = *pointer_to_double_header_data++;
        const double min_x = *pointer_to_double_header_data++;
        const double max_y = *pointer_to_double_header_data++;
        const double min_y = *pointer_to_double_header_data++;
        const double max_z = *pointer_to_double_header_data++;
        const double min_z = *pointer_to_double_header_data++;

        // Create a buffer.
        uint32_t size_of_point_data_record = sizes_of_point_record_formats[point_data_formatID];
        uint32_t point_data_size = number_of_point_records * size_of_point_data_record;
        uint8_t* point_data = new uint8_t[point_data_size];

        // Move file pointer to the beginning of point data.
        if (fseek(file, offset_to_point_data, SEEK_SET) != 0)
        {
            delete point_data;
            std::cerr << "moving file pointer of file " << laser_scaling_file_name << " failed!\n";
        }

        // Read the actual image data from the file into the buffer.
        std::fread(point_data, 1, point_data_size, file);

        // Everything is in memory now, the file can be closed
        std::fclose(file);

        // Loop through point data, convert coordinates to `float` and store them in `laser_points`.
        int32_t* pointer_to_signed_32_bit_header_data = (int32_t*) point_data;

        for (uint32_t laser_point_i = 0; laser_point_i < number_of_point_records; laser_point_i++)
        {
            int32_t x_from_record = *pointer_to_signed_32_bit_header_data++;
            int32_t y_from_record = *pointer_to_signed_32_bit_header_data++;
            int32_t z_from_record = *pointer_to_signed_32_bit_header_data++;

            float x = static_cast<float>(x_scale_factor * x_from_record + x_offset);
            float y = static_cast<float>(y_scale_factor * y_from_record + y_offset);
            float z = static_cast<float>(z_scale_factor * z_from_record + z_offset);
            laser_points.push_back(glm::vec3(x, y, z));
        }

        delete point_data;

        return true;
    }
}
<commit_msg>Bugfix: `fclose` file when point data format ID is greater than 3.<commit_after>#ifndef GLM_FORCE_RADIANS
#define GLM_FORCE_RADIANS
#define DEGREES_TO_RADIANS(x) (x * PI / 180.0f)
#endif

#include "las_loader.hpp"
#include "code/ylikuutio/triangulation/quad_triangulation.hpp"
#include "code/ylikuutio/common/globals.hpp"
#include "code/ylikuutio/common/global_variables.hpp"

// Include GLM
#ifndef __GLM_GLM_HPP_INCLUDED
#define __GLM_GLM_HPP_INCLUDED
#include <glm/glm.hpp> // glm
#endif

// Include standard headers
#include <cmath>    // NAN, std::isnan, std::pow
#include <cstdio>   // std::FILE, std::fclose, std::fopen, std::fread, std::getchar, std::printf etc.
#include <iostream> // std::cout, std::cin, std::cerr
#include <stdint.h> // uint32_t etc.
#include <string>   // std::string
#include <vector>   // std::vector

namespace loaders
{
    bool load_LASF_data(
            std::string laser_scaling_file_name,
            std::vector<glm::vec3>& laser_points)
    {
        std::cout << "Loading LASF file " << laser_scaling_file_name << " ...\n";

        // Data read from the header of the LASF file
        uint8_t header[227];

        // Open the file
        const char* char_laser_scaling_file_name = laser_scaling_file_name.c_str();
        std::FILE* file = std::fopen(char_laser_scaling_file_name, "rb");
        if (!file)
        {
            std::cerr << laser_scaling_file_name << " could not be opened.\n";
            return false;
        }

        // Read the header, i.e. the 227 first bytes

        // If less than 227 bytes are read, it's a problem.
        if (std::fread(header, 1, 227, file) != 227)
        {
            std::cerr << "Too few bytes read when reading the file header. " << laser_scaling_file_name << " is not a correct LASF file.\n";
            std::fclose(file);
            return false;
        }

        // A LASF files always begins with "LASF"
        if ((header[0] != 'L') || (header[1] != 'A') || (header[2] != 'S') || (header[3] != 'F'))
        {
            std::cerr << "No LASF signature. " << laser_scaling_file_name << " is not a correct LASF file.\n";
            std::fclose(file);
            return false;
        }

        const uint32_t max_number_of_returns = 4;

        uint32_t sizes_of_point_record_formats[4];
        sizes_of_point_record_formats[0] = 20; // 4 + 4 + 4 + 2 + 1* + 1 + 1 + 1 + 2. 1*) 3 bits + 3 bits + 1 bit + 1 bit.
        sizes_of_point_record_formats[1] = 28; // 4 + 4 + 4 + 2 + 1* + 1 + 1 + 1 + 2 + 8. 1*) 3 bits + 3 bits + 1 bit + 1 bit.
        sizes_of_point_record_formats[2] = 26; // 4 + 4 + 4 + 2 + 1* + 1 + 1 + 1 + 2 + 2 + 2 + 2. 1*) 3 bits + 3 bits + 1 bit + 1 bit.
        sizes_of_point_record_formats[3] = 34; // 4 + 4 + 4 + 2 + 1* + 1 + 1 + 1 + 2 + 8 + 2 + 2 + 2. 1*) 3 bits + 3 bits + 1 bit + 1 bit.

        uint16_t* pointer_to_16_bit_header_data = (uint16_t*) (header + 94);
        const uint16_t header_size_in_bytes = *pointer_to_16_bit_header_data++;

        if (header_size_in_bytes != 227)
        {
            std::cerr << "Invalid header size in file " << laser_scaling_file_name << " (" << header_size_in_bytes << " bytes). Header size must be 227 bytes.\n";
            std::fclose(file);
            return false;
        }

        uint32_t* pointer_to_32_bit_header_data = (uint32_t*) (header + 96);

        const uint32_t offset_to_point_data = *pointer_to_32_bit_header_data++;
        const uint32_t number_of_variable_length_records = *pointer_to_32_bit_header_data++;

        uint8_t* pointer_to_8_bit_header_data = (uint8_t*) pointer_to_32_bit_header_data;

        const uint8_t point_data_formatID = *pointer_to_8_bit_header_data++;

        if (point_data_formatID > 3)
        {
            std::cerr << "invalid point data format ID: " << static_cast<int16_t>(point_data_formatID) << " in file " << laser_scaling_file_name << "\n";
            std::fclose(file);
            return false;
        }

        // OK, the point data format ID is valid (0, 1, 2 or 3).

        pointer_to_16_bit_header_data = (uint16_t*) pointer_to_8_bit_header_data;

        const uint8_t point_data_record_length = *pointer_to_16_bit_header_data++;

        pointer_to_32_bit_header_data = (uint32_t*) pointer_to_16_bit_header_data;

        const uint32_t number_of_point_records = *pointer_to_32_bit_header_data++;
        uint32_t number_of_points_by_record[max_number_of_returns + 1];

        for (uint32_t n_returns = 0; n_returns <= max_number_of_returns; n_returns++)
        {
            number_of_points_by_record[n_returns] = *pointer_to_32_bit_header_data++;
        }

        double* pointer_to_double_header_data = (double*) pointer_to_32_bit_header_data;

        const double x_scale_factor = *pointer_to_double_header_data++;
        const double y_scale_factor = *pointer_to_double_header_data++;
        const double z_scale_factor = *pointer_to_double_header_data++;
        const double x_offset = *pointer_to_double_header_data++;
        const double y_offset = *pointer_to_double_header_data++;
        const double z_offset = *pointer_to_double_header_data++;
        const double max_x = *pointer_to_double_header_data++;
        const double min_x = *pointer_to_double_header_data++;
        const double max_y = *pointer_to_double_header_data++;
        const double min_y = *pointer_to_double_header_data++;
        const double max_z = *pointer_to_double_header_data++;
        const double min_z = *pointer_to_double_header_data++;

        // Create a buffer.
        uint32_t size_of_point_data_record = sizes_of_point_record_formats[point_data_formatID];
        uint32_t point_data_size = number_of_point_records * size_of_point_data_record;
        uint8_t* point_data = new uint8_t[point_data_size];

        // Move file pointer to the beginning of point data.
        if (fseek(file, offset_to_point_data, SEEK_SET) != 0)
        {
            delete point_data;
            std::cerr << "moving file pointer of file " << laser_scaling_file_name << " failed!\n";
        }

        // Read the actual image data from the file into the buffer.
        std::fread(point_data, 1, point_data_size, file);

        // Everything is in memory now, the file can be closed
        std::fclose(file);

        // Loop through point data, convert coordinates to `float` and store them in `laser_points`.
        int32_t* pointer_to_signed_32_bit_header_data = (int32_t*) point_data;

        for (uint32_t laser_point_i = 0; laser_point_i < number_of_point_records; laser_point_i++)
        {
            int32_t x_from_record = *pointer_to_signed_32_bit_header_data++;
            int32_t y_from_record = *pointer_to_signed_32_bit_header_data++;
            int32_t z_from_record = *pointer_to_signed_32_bit_header_data++;

            float x = static_cast<float>(x_scale_factor * x_from_record + x_offset);
            float y = static_cast<float>(y_scale_factor * y_from_record + y_offset);
            float z = static_cast<float>(z_scale_factor * z_from_record + z_offset);
            laser_points.push_back(glm::vec3(x, y, z));
        }

        delete point_data;

        return true;
    }
}
<|endoftext|>