<commit_before>/*
    This file is part of Magnum.

    Original authors — credit is appreciated but not required:

        2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018, 2019, 2020 —
            Vladimír Vondruš <mosra@centrum.cz>
        2020 — Nghia Truong <nghiatruong.vn@gmail.com>

    This is free and unencumbered software released into the public domain.

    Anyone is free to copy, modify, publish, use, compile, sell, or distribute
    this software, either in source code form or as a compiled binary, for any
    purpose, commercial or non-commercial, and by any means.

    In jurisdictions that recognize copyright laws, the author or authors of
    this software dedicate any and all copyright interest in the software to
    the public domain. We make this dedication for the benefit of the public
    at large and to the detriment of our heirs and successors. We intend this
    dedication to be an overt act of relinquishment in perpetuity of all
    present and future rights to this software under copyright law.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
    THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
    IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
    CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#include "LooseOctree.h"

#include <Corrade/Containers/GrowableArray.h>

namespace Magnum { namespace Examples {

OctreeNode::OctreeNode(LooseOctree* const tree, OctreeNode* const parent,
    const Vector3& nodeCenter, const Float halfWidth, const size_t depth):
    _center{nodeCenter},
    _bounds{nodeCenter - Vector3{halfWidth}, nodeCenter + Vector3{halfWidth}},
    _boundsExtended(nodeCenter - Vector3{2.0f*halfWidth},
        nodeCenter + Vector3{2.0f*halfWidth}),
    _halfWidth{halfWidth}, _depth{depth}, _tree{tree}, _parent{parent},
    _children{nullptr}, _maxDepth{tree->_maxDepth}, _isLeaf{true} {}

const OctreeNode& OctreeNode::childNode(const std::size_t childIdx) const {
    CORRADE_INTERNAL_ASSERT(_children);
    return _children->_nodes[childIdx];
}

void OctreeNode::removePointFromSubTree() {
    arrayResize(_nodePoints, 0);

    if(!_isLeaf) for(std::size_t childIdx = 0; childIdx < 8; ++childIdx)
        _children->_nodes[childIdx].removePointFromSubTree();
}

void OctreeNode::split() {
    if(!_isLeaf || _depth == _maxDepth) return;

    if(_isLeaf) {
        /*    6-------7
             /|      /|
            2-+-----3 |
            | |     | |   y
            | 4-----+-5   | z
            |/      |/    |/
            0-------1     +--x

            0   =>   0, 0, 0
            1   =>   0, 0, 1
            2   =>   0, 1, 0
            3   =>   0, 1, 1
            4   =>   1, 0, 0
            5   =>   1, 0, 1
            6   =>   1, 1, 0
            7   =>   1, 1, 1   */
        _children = _tree->requestChildrenFromPool();

        const auto childHalfWidth = _halfWidth*0.5f;
        for(std::size_t childIdx = 0; childIdx < 8; ++childIdx) {
            Vector3 newCenter = _center;
            newCenter[0] += (childIdx & 1) ? childHalfWidth : -childHalfWidth;
            newCenter[1] += (childIdx & 2) ? childHalfWidth : -childHalfWidth;
            newCenter[2] += (childIdx & 4) ? childHalfWidth : -childHalfWidth;

            OctreeNode* const pChildNode = &_children->_nodes[childIdx];
            pChildNode->~OctreeNode(); /* call destructor to release resource */

            /* Placement new: re-use existing memory block, call constructor to
               re-initialize data */
            new(pChildNode) OctreeNode(_tree, this, newCenter, childHalfWidth, _depth + 1u);
        }

        /* Must explicitly mark as non-leaf node, and must do this after all
           children nodes are ready */
        _isLeaf = false;
    }
}

void OctreeNode::removeAllDescendants() {
    if(_isLeaf) return;

    for(std::size_t childIdx = 0; childIdx < 8; ++childIdx)
        _children->_nodes[childIdx].removeAllDescendants();

    _tree->returnChildrenToPool(_children);
    _isLeaf = true;
}

void OctreeNode::removeEmptyDescendants() {
    if(_isLeaf) return;

    bool allEmpty = true;
    bool allLeaves = true;
    for(std::size_t childIdx = 0; childIdx < 8; ++childIdx) {
        auto& pChildNode = _children->_nodes[childIdx];
        pChildNode.removeEmptyDescendants();
        allLeaves &= pChildNode.isLeaf();
        allEmpty &= (pChildNode.pointCount() == 0);
    }

    /* Remove all 8 children nodes iff they are all leaf nodes and all empty
       nodes */
    if(allEmpty && allLeaves) {
        _tree->returnChildrenToPool(_children);
        _isLeaf = true;
    }
}

void OctreeNode::keepPoint(OctreePoint& point) {
    point.nodePtr() = this;
    point.isValid() = true;
    arrayAppend(_nodePoints, &point);
}

void OctreeNode::insertPoint(OctreePoint& point) {
    if(_depth == _maxDepth) {
        keepPoint(point);
        return;
    }

    /* Split node if this is a leaf node */
    split();

    /* Compute the index of the child node that contains this point */
    const Vector3 ppos = point.position();
    std::size_t childIdx = 0;
    for(std::size_t dim = 0; dim < 3; ++dim)
        if(_center[dim] < ppos[dim]) childIdx |= (1ull << dim);

    _children->_nodes[childIdx].insertPoint(point);
}

LooseOctree::~LooseOctree() {
    /* Firstly clear data recursively */
    clear();

    /* Deallocate memory pool */
    CORRADE_ASSERT(_numAllocatedNodes == (_freeNodeBlocks.size() + _activeNodeBlocks.size())*8 + 1u,
        "Internal data corrupted, maybe all nodes were not returned from the tree", );

    for(OctreeNodeBlock* nodeBlock: _freeNodeBlocks)
        delete nodeBlock;
    for(OctreeNodeBlock* nodeBlock: _activeNodeBlocks)
        delete nodeBlock;
}

void LooseOctree::clear() {
    /* Return all tree nodes to memory pool except the root node */
    _rootNode.removeAllDescendants();

    clearPoints();

    /* Set state to imcomplete build */
    _completeBuild = false;
}

void LooseOctree::clearPoints() {
    /* Recursively clear point data */
    _rootNode.removePointFromSubTree();

    /* Clear the main point data array */
    arrayResize(_octreePoints, Containers::NoInit, 0);
}

void LooseOctree::setPoints(Containers::Array<Vector3>& points) {
    clearPoints();

    arrayResize(_octreePoints, Containers::NoInit, points.size());
    for(std::size_t i = 0; i != points.size(); ++i)
        _octreePoints[i] = OctreePoint{points, i};
}

std::size_t LooseOctree::maxNumPointInNodes() const {
    std::size_t count = 0;
    for(const OctreeNodeBlock* nodeBlock: _activeNodeBlocks)
        for(std::size_t childIdx = 0; childIdx < 8; ++childIdx)
            count = Math::max(count, nodeBlock->_nodes[childIdx].pointCount());

    return count;
}

void LooseOctree::build() {
    /* Compute max depth that the tree can reach */
    _maxDepth = 0;
    std::size_t numLevelNodes = 1;
    std::size_t maxNumTreeNodes = 1;
    Float nodeHalfWidth = _halfWidth;

    while(nodeHalfWidth > _minHalfWidth) {
        ++_maxDepth;
        numLevelNodes *= 8;
        maxNumTreeNodes += numLevelNodes;
        nodeHalfWidth *= 0.5f;
    }

    _rootNode._maxDepth = _maxDepth;
    rebuild();
    _completeBuild = true;

    Debug{} << "Octree info:";
    Debug{} << "  Center:" << _center;
    Debug{} << "  Half width:" << _halfWidth;
    Debug{} << "  Min half width:" << _minHalfWidth;
    Debug{} << "  Max depth:" << _maxDepth;
    Debug{} << "  Max tree nodes:" << maxNumTreeNodes;
}

void LooseOctree::update() {
    if(!_completeBuild) build();

    if(_alwaysRebuild) rebuild();
    else incrementalUpdate();
}

void LooseOctree::rebuild() {
    /* Recursively remove all tree nodes other than root node */
    _rootNode.removeAllDescendants();

    /* Clear root node point data */
    _rootNode.removePointFromSubTree();

    /* Populate all points to tree nodes in a top-down manner */
    populatePoints();
}

void LooseOctree::populatePoints() {
    for(OctreePoint& point: _octreePoints)
        _rootNode.insertPoint(point);
}

void LooseOctree::incrementalUpdate() {
    checkValidity();
    removeInvalidPointsFromNodes();
    reinsertInvalidPointsToNodes();

    /* Recursively remove all empty nodes, returning them to memory pool for
       recycling */
    _rootNode.removeEmptyDescendants();
}

void LooseOctree::checkValidity() {
    const OctreeNode* const rootNodePtr = &_rootNode;
    for(OctreePoint& point: _octreePoints) {
        OctreeNode* pNode = point.nodePtr();
        const Vector3 ppos = point.position();

        if(!pNode->looselyContains(ppos) && pNode != rootNodePtr) {
            /* Go up, find the node tightly containing it (or stop if reached
               the root node) */
            while(pNode != rootNodePtr) {
                pNode = pNode->_parent;
                if(pNode->contains(ppos) || pNode == rootNodePtr) {
                    point.isValid() = false;
                    point.nodePtr() = pNode;
                    break;
                }
            }

        } else point.isValid() = (pNode != rootNodePtr) ? true : false;
    }
}

void LooseOctree::removeInvalidPointsFromNodes() {
    for(OctreeNodeBlock* const pNodeBlock: _activeNodeBlocks) {
        for(std::size_t childIdx = 0; childIdx < 8; ++childIdx) {
            auto& pointList = pNodeBlock->_nodes[childIdx]._nodePoints;
            for(std::size_t i = 0, iend = pointList.size(); i < iend; ++i) {
                OctreePoint* const point = pointList[i];
                if(!point->isValid()) {
                    pointList[i] = pointList[iend - 1];
                    arrayResize(pointList, iend - 1);
                    --iend;
                }
            }
        }
    }
}

void LooseOctree::reinsertInvalidPointsToNodes() {
    for(OctreePoint& point: _octreePoints)
        if(!point.isValid()) _rootNode.insertPoint(point);
}

OctreeNodeBlock* LooseOctree::requestChildrenFromPool() {
    if(_freeNodeBlocks.size() == 0) {
        /* Allocate more node blocks and put to the pool */
        constexpr std::size_t numAllocations = 16;
        for(std::size_t i = 0; i < numAllocations; ++i)
            arrayAppend(_freeNodeBlocks, new OctreeNodeBlock);

        _numAllocatedNodes += numAllocations*8;
    }

    OctreeNodeBlock* const nodeBlock = _freeNodeBlocks.back();
    arrayResize(_freeNodeBlocks, _freeNodeBlocks.size() - 1);
    _activeNodeBlocks.insert(nodeBlock);
    return nodeBlock;
}

void LooseOctree::returnChildrenToPool(OctreeNodeBlock*& nodeBlock) {
    arrayAppend(_freeNodeBlocks, nodeBlock);
    _activeNodeBlocks.erase(nodeBlock);
    nodeBlock = nullptr;
}

}}
<commit_msg>octree: yes, this is how booleans work.<commit_after>/*
    This file is part of Magnum.

    Original authors — credit is appreciated but not required:

        2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018, 2019, 2020 —
            Vladimír Vondruš <mosra@centrum.cz>
        2020 — Nghia Truong <nghiatruong.vn@gmail.com>

    This is free and unencumbered software released into the public domain.

    Anyone is free to copy, modify, publish, use, compile, sell, or distribute
    this software, either in source code form or as a compiled binary, for any
    purpose, commercial or non-commercial, and by any means.

    In jurisdictions that recognize copyright laws, the author or authors of
    this software dedicate any and all copyright interest in the software to
    the public domain. We make this dedication for the benefit of the public
    at large and to the detriment of our heirs and successors. We intend this
    dedication to be an overt act of relinquishment in perpetuity of all
    present and future rights to this software under copyright law.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
    THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
    IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
    CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#include "LooseOctree.h"

#include <Corrade/Containers/GrowableArray.h>

namespace Magnum { namespace Examples {

OctreeNode::OctreeNode(LooseOctree* const tree, OctreeNode* const parent,
    const Vector3& nodeCenter, const Float halfWidth, const size_t depth):
    _center{nodeCenter},
    _bounds{nodeCenter - Vector3{halfWidth}, nodeCenter + Vector3{halfWidth}},
    _boundsExtended(nodeCenter - Vector3{2.0f*halfWidth},
        nodeCenter + Vector3{2.0f*halfWidth}),
    _halfWidth{halfWidth}, _depth{depth}, _tree{tree}, _parent{parent},
    _children{nullptr}, _maxDepth{tree->_maxDepth}, _isLeaf{true} {}

const OctreeNode& OctreeNode::childNode(const std::size_t childIdx) const {
    CORRADE_INTERNAL_ASSERT(_children);
    return _children->_nodes[childIdx];
}

void OctreeNode::removePointFromSubTree() {
    arrayResize(_nodePoints, 0);

    if(!_isLeaf) for(std::size_t childIdx = 0; childIdx < 8; ++childIdx)
        _children->_nodes[childIdx].removePointFromSubTree();
}

void OctreeNode::split() {
    if(!_isLeaf || _depth == _maxDepth) return;

    if(_isLeaf) {
        /*    6-------7
             /|      /|
            2-+-----3 |
            | |     | |   y
            | 4-----+-5   | z
            |/      |/    |/
            0-------1     +--x

            0   =>   0, 0, 0
            1   =>   0, 0, 1
            2   =>   0, 1, 0
            3   =>   0, 1, 1
            4   =>   1, 0, 0
            5   =>   1, 0, 1
            6   =>   1, 1, 0
            7   =>   1, 1, 1   */
        _children = _tree->requestChildrenFromPool();

        const auto childHalfWidth = _halfWidth*0.5f;
        for(std::size_t childIdx = 0; childIdx < 8; ++childIdx) {
            Vector3 newCenter = _center;
            newCenter[0] += (childIdx & 1) ? childHalfWidth : -childHalfWidth;
            newCenter[1] += (childIdx & 2) ? childHalfWidth : -childHalfWidth;
            newCenter[2] += (childIdx & 4) ? childHalfWidth : -childHalfWidth;

            OctreeNode* const pChildNode = &_children->_nodes[childIdx];
            pChildNode->~OctreeNode(); /* call destructor to release resource */

            /* Placement new: re-use existing memory block, call constructor to
               re-initialize data */
            new(pChildNode) OctreeNode(_tree, this, newCenter, childHalfWidth, _depth + 1u);
        }

        /* Must explicitly mark as non-leaf node, and must do this after all
           children nodes are ready */
        _isLeaf = false;
    }
}

void OctreeNode::removeAllDescendants() {
    if(_isLeaf) return;

    for(std::size_t childIdx = 0; childIdx < 8; ++childIdx)
        _children->_nodes[childIdx].removeAllDescendants();

    _tree->returnChildrenToPool(_children);
    _isLeaf = true;
}

void OctreeNode::removeEmptyDescendants() {
    if(_isLeaf) return;

    bool allEmpty = true;
    bool allLeaves = true;
    for(std::size_t childIdx = 0; childIdx < 8; ++childIdx) {
        auto& pChildNode = _children->_nodes[childIdx];
        pChildNode.removeEmptyDescendants();
        allLeaves &= pChildNode.isLeaf();
        allEmpty &= (pChildNode.pointCount() == 0);
    }

    /* Remove all 8 children nodes iff they are all leaf nodes and all empty
       nodes */
    if(allEmpty && allLeaves) {
        _tree->returnChildrenToPool(_children);
        _isLeaf = true;
    }
}

void OctreeNode::keepPoint(OctreePoint& point) {
    point.nodePtr() = this;
    point.isValid() = true;
    arrayAppend(_nodePoints, &point);
}

void OctreeNode::insertPoint(OctreePoint& point) {
    if(_depth == _maxDepth) {
        keepPoint(point);
        return;
    }

    /* Split node if this is a leaf node */
    split();

    /* Compute the index of the child node that contains this point */
    const Vector3 ppos = point.position();
    std::size_t childIdx = 0;
    for(std::size_t dim = 0; dim < 3; ++dim)
        if(_center[dim] < ppos[dim]) childIdx |= (1ull << dim);

    _children->_nodes[childIdx].insertPoint(point);
}

LooseOctree::~LooseOctree() {
    /* Firstly clear data recursively */
    clear();

    /* Deallocate memory pool */
    CORRADE_ASSERT(_numAllocatedNodes == (_freeNodeBlocks.size() + _activeNodeBlocks.size())*8 + 1u,
        "Internal data corrupted, maybe all nodes were not returned from the tree", );

    for(OctreeNodeBlock* nodeBlock: _freeNodeBlocks)
        delete nodeBlock;
    for(OctreeNodeBlock* nodeBlock: _activeNodeBlocks)
        delete nodeBlock;
}

void LooseOctree::clear() {
    /* Return all tree nodes to memory pool except the root node */
    _rootNode.removeAllDescendants();

    clearPoints();

    /* Set state to imcomplete build */
    _completeBuild = false;
}

void LooseOctree::clearPoints() {
    /* Recursively clear point data */
    _rootNode.removePointFromSubTree();

    /* Clear the main point data array */
    arrayResize(_octreePoints, Containers::NoInit, 0);
}

void LooseOctree::setPoints(Containers::Array<Vector3>& points) {
    clearPoints();

    arrayResize(_octreePoints, Containers::NoInit, points.size());
    for(std::size_t i = 0; i != points.size(); ++i)
        _octreePoints[i] = OctreePoint{points, i};
}

std::size_t LooseOctree::maxNumPointInNodes() const {
    std::size_t count = 0;
    for(const OctreeNodeBlock* nodeBlock: _activeNodeBlocks)
        for(std::size_t childIdx = 0; childIdx < 8; ++childIdx)
            count = Math::max(count, nodeBlock->_nodes[childIdx].pointCount());

    return count;
}

void LooseOctree::build() {
    /* Compute max depth that the tree can reach */
    _maxDepth = 0;
    std::size_t numLevelNodes = 1;
    std::size_t maxNumTreeNodes = 1;
    Float nodeHalfWidth = _halfWidth;

    while(nodeHalfWidth > _minHalfWidth) {
        ++_maxDepth;
        numLevelNodes *= 8;
        maxNumTreeNodes += numLevelNodes;
        nodeHalfWidth *= 0.5f;
    }

    _rootNode._maxDepth = _maxDepth;
    rebuild();
    _completeBuild = true;

    Debug{} << "Octree info:";
    Debug{} << "  Center:" << _center;
    Debug{} << "  Half width:" << _halfWidth;
    Debug{} << "  Min half width:" << _minHalfWidth;
    Debug{} << "  Max depth:" << _maxDepth;
    Debug{} << "  Max tree nodes:" << maxNumTreeNodes;
}

void LooseOctree::update() {
    if(!_completeBuild) build();

    if(_alwaysRebuild) rebuild();
    else incrementalUpdate();
}

void LooseOctree::rebuild() {
    /* Recursively remove all tree nodes other than root node */
    _rootNode.removeAllDescendants();

    /* Clear root node point data */
    _rootNode.removePointFromSubTree();

    /* Populate all points to tree nodes in a top-down manner */
    populatePoints();
}

void LooseOctree::populatePoints() {
    for(OctreePoint& point: _octreePoints)
        _rootNode.insertPoint(point);
}

void LooseOctree::incrementalUpdate() {
    checkValidity();
    removeInvalidPointsFromNodes();
    reinsertInvalidPointsToNodes();

    /* Recursively remove all empty nodes, returning them to memory pool for
       recycling */
    _rootNode.removeEmptyDescendants();
}

void LooseOctree::checkValidity() {
    const OctreeNode* const rootNodePtr = &_rootNode;
    for(OctreePoint& point: _octreePoints) {
        OctreeNode* pNode = point.nodePtr();
        const Vector3 ppos = point.position();

        if(!pNode->looselyContains(ppos) && pNode != rootNodePtr) {
            /* Go up, find the node tightly containing it (or stop if reached
               the root node) */
            while(pNode != rootNodePtr) {
                pNode = pNode->_parent;
                if(pNode->contains(ppos) || pNode == rootNodePtr) {
                    point.isValid() = false;
                    point.nodePtr() = pNode;
                    break;
                }
            }

        } else point.isValid() = pNode != rootNodePtr;
    }
}

void LooseOctree::removeInvalidPointsFromNodes() {
    for(OctreeNodeBlock* const pNodeBlock: _activeNodeBlocks) {
        for(std::size_t childIdx = 0; childIdx < 8; ++childIdx) {
            auto& pointList = pNodeBlock->_nodes[childIdx]._nodePoints;
            for(std::size_t i = 0, iend = pointList.size(); i < iend; ++i) {
                OctreePoint* const point = pointList[i];
                if(!point->isValid()) {
                    pointList[i] = pointList[iend - 1];
                    arrayResize(pointList, iend - 1);
                    --iend;
                }
            }
        }
    }
}

void LooseOctree::reinsertInvalidPointsToNodes() {
    for(OctreePoint& point: _octreePoints)
        if(!point.isValid()) _rootNode.insertPoint(point);
}

OctreeNodeBlock* LooseOctree::requestChildrenFromPool() {
    if(_freeNodeBlocks.size() == 0) {
        /* Allocate more node blocks and put to the pool */
        constexpr std::size_t numAllocations = 16;
        for(std::size_t i = 0; i < numAllocations; ++i)
            arrayAppend(_freeNodeBlocks, new OctreeNodeBlock);

        _numAllocatedNodes += numAllocations*8;
    }

    OctreeNodeBlock* const nodeBlock = _freeNodeBlocks.back();
    arrayResize(_freeNodeBlocks, _freeNodeBlocks.size() - 1);
    _activeNodeBlocks.insert(nodeBlock);
    return nodeBlock;
}

void LooseOctree::returnChildrenToPool(OctreeNodeBlock*& nodeBlock) {
    arrayAppend(_freeNodeBlocks, nodeBlock);
    _activeNodeBlocks.erase(nodeBlock);
    nodeBlock = nullptr;
}

}}
<|endoftext|>


<commit_before>#include <iostream>

void print_range(int low, int high){
    for(int i = low; i <= high; ++i){
        std::cout << i << std::endl;
    }
}

int main(){
    int num1{0}, num2{0};
    std::cout << "Enter two numbers: " << std::endl;
    std::cin >> num1 >> num2;
    if(num1 > num2){
        print_range(num2, num1);
    } else if(num2 > num1) {
        print_range(num1,num2);
    } else {
        std::cout << "This numbers are equals." << std::endl;
    }
    return 0;
}<commit_msg>Fix, using while statement now<commit_after>#include <iostream>

void print_range(int low, int high){
    while(low <= high) {
        std::cout << low << std::endl;
        ++low;
    }
}

int main(){
    int num1{0}, num2{0};
    std::cout << "Enter two numbers: " << std::endl;
    std::cin >> num1 >> num2;
    if(num1 > num2){
        print_range(num2, num1);
    } else if(num2 > num1) {
        print_range(num1,num2);
    } else {
        std::cout << "This numbers are equals." << std::endl;
    }
    return 0;
}<|endoftext|>


<commit_before>#include <iostream>
using namespace std;

int main(void)
{
	int a, b;
	cin >> a >> b;
	cout << static_cast<double>(a) / b << endl;

	return 0;
}<commit_msg>Deal with the situation when the second integer is 0 using run_time exception covered in this chapter.<commit_after>#include <iostream>
#include <string>
using std::string;
using std::cin;
using std::cout;
using std::endl;

int main () {
    int a=0, b=0;
    cout<<"Please input two integers: ";
    while (cin>>a>>b) {
        try{
            if (b==0) {
                throw std::runtime_error("One of the two integers is 0.");}
            cout<<static_cast <double>(a)/b<<endl;
        }
        catch (std::runtime_error err)
            {cout<<err.what();
                cout<<"\nDo you want to try it again? Yes or No\n";
                string a={};
                cin>>a;
                if (a.empty()||a[0]=='N') {
                    break;
                }
            }
    }
    return 0;
}
<|endoftext|>


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

#include "RenderManager.h"
#include <QMutexLocker>
#include "CaptureContext.h"
#include "QRDUtils.h"

RenderManager::RenderManager()
{
  m_Running = false;
  m_Thread = NULL;
}

RenderManager::~RenderManager()
{
}

void RenderManager::OpenCapture(const QString &logfile, float *progress)
{
  if(m_Running)
    return;

  m_ProxyRenderer = -1;
  m_ReplayHost = "";
  m_Logfile = logfile;
  m_Progress = progress;

  *progress = 0.0f;

  m_Thread = new LambdaThread([this]() { run(); });
  m_Thread->start(QThread::HighestPriority);

  while(m_Thread->isRunning() && !m_Running)
  {
  }
}

void RenderManager::DeleteCapture(const QString &logfile, bool local)
{
  if(IsRunning())
  {
    AsyncInvoke([this, logfile, local](IReplayRenderer *) { DeleteCapture(logfile, local); });
    return;
  }

  if(local)
  {
    QFile::remove(logfile);
  }
  else
  {
    // TODO Remote
    // m_Remote.TakeOwnershipCapture(logfile);
  }
}

bool RenderManager::IsRunning()
{
  return m_Thread && m_Thread->isRunning() && m_Running;
}

void RenderManager::AsyncInvoke(RenderManager::InvokeMethod m)
{
  InvokeHandle *cmd = new InvokeHandle(m);
  cmd->selfdelete = true;

  PushInvoke(cmd);
}

void RenderManager::BlockInvoke(RenderManager::InvokeMethod m)
{
  InvokeHandle *cmd = new InvokeHandle(m);

  PushInvoke(cmd);

  for(;;)
  {
    if(cmd->processed.tryAcquire())
      break;
  }

  delete cmd;
}

void RenderManager::CloseThread()
{
  m_Running = false;

  m_RenderCondition.wakeAll();

  if(m_Thread == NULL)
    return;

  // wait for the thread to close and clean up
  while(m_Thread->isRunning())
  {
  }

  m_Thread->deleteLater();
  m_Thread = NULL;
}

uint32_t RenderManager::ExecuteAndInject(const QString &exe, const QString &workingDir,
                                         const QString &cmdLine,
                                         const QList<EnvironmentModification> &env,
                                         const QString &logfile, CaptureOptions opts)
{
  // if (m_Remote == null)
  {
    void *envList = RENDERDOC_MakeEnvironmentModificationList(env.size());

    for(int i = 0; i < env.size(); i++)
      RENDERDOC_SetEnvironmentModification(envList, i, env[i].variable.toUtf8().data(),
                                           env[i].value.toUtf8().data(), env[i].type,
                                           env[i].separator);

    uint32_t ret = RENDERDOC_ExecuteAndInject(exe.toUtf8().data(), workingDir.toUtf8().data(),
                                              cmdLine.toUtf8().data(), envList,
                                              logfile.toUtf8().data(), &opts, false);

    RENDERDOC_FreeEnvironmentModificationList(envList);

    return ret;
  }
  /*
  else
  {
  }
  */
}

void RenderManager::PushInvoke(RenderManager::InvokeHandle *cmd)
{
  if(m_Thread == NULL || !m_Thread->isRunning() || !m_Running)
  {
    cmd->processed.release();
    if(cmd->selfdelete)
      delete cmd;
    return;
  }

  m_RenderLock.lock();
  m_RenderQueue.push_back(cmd);
  m_RenderCondition.wakeAll();
  m_RenderLock.unlock();
}

void RenderManager::run()
{
  IReplayRenderer *renderer = NULL;

  m_CreateStatus = RENDERDOC_CreateReplayRenderer(m_Logfile.toUtf8(), m_Progress, &renderer);

  if(renderer == NULL)
    return;

  qInfo() << QString("QRenderDoc - renderer created for %1").arg(m_Logfile);

  m_Running = true;

  // main render command loop
  while(m_Running)
  {
    QQueue<InvokeHandle *> queue;

    // wait for the condition to be woken, grab current queue,
    // unlock again.
    {
      m_RenderLock.lock();
      m_RenderCondition.wait(&m_RenderLock, 10);
      m_RenderQueue.swap(queue);
      m_RenderLock.unlock();
    }

    // process all the commands
    for(InvokeHandle *cmd : queue)
    {
      if(cmd == NULL)
        continue;

      if(cmd->method != NULL)
        cmd->method(renderer);

      cmd->processed.release();

      // if it's a throwaway command, delete it
      if(cmd->selfdelete)
        delete cmd;
    }
  }

  // clean up anything left in the queue
  {
    QQueue<InvokeHandle *> queue;

    m_RenderLock.lock();
    m_RenderQueue.swap(queue);
    m_RenderLock.unlock();

    for(InvokeHandle *cmd : queue)
    {
      if(cmd == NULL)
        continue;

      cmd->processed.release();

      if(cmd->selfdelete)
        delete cmd;
    }
  }

  // close the core renderer
  renderer->Shutdown();
}
<commit_msg>Fix a potential race where we'd double-delete a command<commit_after>/******************************************************************************
 * The MIT License (MIT)
 *
 * Copyright (c) 2016 Baldur Karlsson
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 ******************************************************************************/

#include "RenderManager.h"
#include <QMutexLocker>
#include "CaptureContext.h"
#include "QRDUtils.h"

RenderManager::RenderManager()
{
  m_Running = false;
  m_Thread = NULL;
}

RenderManager::~RenderManager()
{
}

void RenderManager::OpenCapture(const QString &logfile, float *progress)
{
  if(m_Running)
    return;

  m_ProxyRenderer = -1;
  m_ReplayHost = "";
  m_Logfile = logfile;
  m_Progress = progress;

  *progress = 0.0f;

  m_Thread = new LambdaThread([this]() { run(); });
  m_Thread->start(QThread::HighestPriority);

  while(m_Thread->isRunning() && !m_Running)
  {
  }
}

void RenderManager::DeleteCapture(const QString &logfile, bool local)
{
  if(IsRunning())
  {
    AsyncInvoke([this, logfile, local](IReplayRenderer *) { DeleteCapture(logfile, local); });
    return;
  }

  if(local)
  {
    QFile::remove(logfile);
  }
  else
  {
    // TODO Remote
    // m_Remote.TakeOwnershipCapture(logfile);
  }
}

bool RenderManager::IsRunning()
{
  return m_Thread && m_Thread->isRunning() && m_Running;
}

void RenderManager::AsyncInvoke(RenderManager::InvokeMethod m)
{
  InvokeHandle *cmd = new InvokeHandle(m);
  cmd->selfdelete = true;

  PushInvoke(cmd);
}

void RenderManager::BlockInvoke(RenderManager::InvokeMethod m)
{
  InvokeHandle *cmd = new InvokeHandle(m);

  PushInvoke(cmd);

  for(;;)
  {
    if(cmd->processed.tryAcquire())
      break;
  }

  delete cmd;
}

void RenderManager::CloseThread()
{
  m_Running = false;

  m_RenderCondition.wakeAll();

  if(m_Thread == NULL)
    return;

  // wait for the thread to close and clean up
  while(m_Thread->isRunning())
  {
  }

  m_Thread->deleteLater();
  m_Thread = NULL;
}

uint32_t RenderManager::ExecuteAndInject(const QString &exe, const QString &workingDir,
                                         const QString &cmdLine,
                                         const QList<EnvironmentModification> &env,
                                         const QString &logfile, CaptureOptions opts)
{
  // if (m_Remote == null)
  {
    void *envList = RENDERDOC_MakeEnvironmentModificationList(env.size());

    for(int i = 0; i < env.size(); i++)
      RENDERDOC_SetEnvironmentModification(envList, i, env[i].variable.toUtf8().data(),
                                           env[i].value.toUtf8().data(), env[i].type,
                                           env[i].separator);

    uint32_t ret = RENDERDOC_ExecuteAndInject(exe.toUtf8().data(), workingDir.toUtf8().data(),
                                              cmdLine.toUtf8().data(), envList,
                                              logfile.toUtf8().data(), &opts, false);

    RENDERDOC_FreeEnvironmentModificationList(envList);

    return ret;
  }
  /*
  else
  {
  }
  */
}

void RenderManager::PushInvoke(RenderManager::InvokeHandle *cmd)
{
  if(m_Thread == NULL || !m_Thread->isRunning() || !m_Running)
  {
    if(cmd->selfdelete)
      delete cmd;
    else
      cmd->processed.release();
    return;
  }

  m_RenderLock.lock();
  m_RenderQueue.push_back(cmd);
  m_RenderCondition.wakeAll();
  m_RenderLock.unlock();
}

void RenderManager::run()
{
  IReplayRenderer *renderer = NULL;

  m_CreateStatus = RENDERDOC_CreateReplayRenderer(m_Logfile.toUtf8(), m_Progress, &renderer);

  if(renderer == NULL)
    return;

  qInfo() << QString("QRenderDoc - renderer created for %1").arg(m_Logfile);

  m_Running = true;

  // main render command loop
  while(m_Running)
  {
    QQueue<InvokeHandle *> queue;

    // wait for the condition to be woken, grab current queue,
    // unlock again.
    {
      m_RenderLock.lock();
      m_RenderCondition.wait(&m_RenderLock, 10);
      m_RenderQueue.swap(queue);
      m_RenderLock.unlock();
    }

    // process all the commands
    for(InvokeHandle *cmd : queue)
    {
      if(cmd == NULL)
        continue;

      if(cmd->method != NULL)
        cmd->method(renderer);

      // if it's a throwaway command, delete it
      if(cmd->selfdelete)
        delete cmd;
      else
        cmd->processed.release();
    }
  }

  // clean up anything left in the queue
  {
    QQueue<InvokeHandle *> queue;

    m_RenderLock.lock();
    m_RenderQueue.swap(queue);
    m_RenderLock.unlock();

    for(InvokeHandle *cmd : queue)
    {
      if(cmd == NULL)
        continue;

      if(cmd->selfdelete)
        delete cmd;
      else
        cmd->processed.release();
    }
  }

  // close the core renderer
  renderer->Shutdown();
}
<|endoftext|>


<commit_before>
#include "SysConfig.h"
#if(HAS_BNO055)

#include "CBNO055.h"
#include <BNO055.h>
#include "CTimer.h"
#include <Wire.h>

#include "NDataManager.h"

namespace
{
	CTimer bno055_sample_timer;
	CTimer report_timer;
	CTimer imuTimer;
	CTimer fusionTimer;

	bool initalized				= false;
	bool browserPingReceived	= false;

	bool inFusionMode			= false;

	// Use the first Wire interface by default
	CBNO055Driver bno;

	imu::Vector<3> euler;

	float roll = 0.0f;
	float pitch = 0.0f;
	float yaw = 0.0f;

	int m_wireInterfaceSelector = 0;
	TwoWire *m_pWire = nullptr;

	void InitializeSensor()
	{
		// Attempt to initialize. If this fails, we try every 30 seconds in its update loop
		if( !bno.Initialize( m_pWire ) )
		{
			Serial.println( "BNO_INIT_STATUS:FAILED;" );
			initalized = false;
		}
		else
		{
			Serial.println( "BNO_INIT_STATUS:SUCCESS;" );

			Serial.print( "BNO055.SW_Revision_ID:" );
			Serial.print( bno.m_softwareVersionMajor, HEX );
			Serial.print( "." );
			Serial.print( bno.m_softwareVersionMinor, HEX );
			Serial.println( ";" );

			Serial.print( "BNO055.bootloader:" );
			Serial.print( bno.m_bootloaderRev );
			Serial.println( ";" );

			initalized = true;
			inFusionMode = true;
		}
	}
}

// CBNO055::CBNO055( int wireInterfaceSelectorIn )
// {
// 	m_wireInterfaceSelector = wireInterfaceSelectorIn;
// }

void CBNO055::Initialize()
{
	Serial.println( "BNO055.Status:INIT;" );

	if( m_wireInterfaceSelector > WIRE_INTERFACES_COUNT )
	{
		// Invalid wire interface selected
		Serial.println( "ERROR:Invalid wire interface selected for BNO055;" );
		m_pWire = nullptr;
	}

	switch( m_wireInterfaceSelector )
	{
		case BNO_WIRE_INTERFACE_0:
			{
				m_pWire = WIRE_INTERFACE_0;
				break;
			}

		case BNO_WIRE_INTERFACE_1:
			{
				m_pWire = WIRE_INTERFACE_1;
				break;
			}

		case BNO_WIRE_INTERFACE_2:
			{
				m_pWire = WIRE_INTERFACE_2;
				break;
			}

		case BNO_WIRE_INTERFACE_3:
			{
				m_pWire = WIRE_INTERFACE_3;
				break;
			}

		case BNO_WIRE_INTERFACE_4:
			{
				m_pWire = WIRE_INTERFACE_4;
				break;
			}

		case BNO_WIRE_INTERFACE_5:
			{
				m_pWire = WIRE_INTERFACE_5;
				break;
			}

		default:
			m_pWire = nullptr;
			break;
	}

	//Reset timers
	bno055_sample_timer.Reset();
	report_timer.Reset();
	imuTimer.Reset();
	fusionTimer.Reset();

	Serial.println( "BNO055.Status:POSTINIT;" );
}

void CBNO055::Update( CCommand &commandIn )
{
	// 100hz
	if( bno055_sample_timer.HasElapsed( 10 ) )
	{
		if( !initalized )
		{
			// Attempt every 10 secs
			if( report_timer.HasElapsed( 5000 ) )
			{
				Serial.println( "BNO055.Status:TRYING;" );
				// Attempt to initialize the chip again
				InitializeSensor();
			}

			return;
		}

		// bno.GetCalibration();
		// bno.GetSystemStatus();
		// bno.GetSystemError();
		// bno.GetVector( bosch::EVectorType::VECTOR_EULER, euler );

		// yaw = fmod( euler.x() + 90.0f, 360.0f );

		// if( yaw < 0.0f )
		// {
		// 	yaw += 360.0f;
		// }

		// pitch		= euler.y();
		// roll		= euler.z();

		// System status checks
		if( report_timer.HasElapsed( 1000 ) )
		{
			// System calibration
			if( bno.GetCalibration() )
			{
				Serial.print( "BNO055.CALIB_MAG:" );
				Serial.print( bno.m_magCal );
				Serial.println( ';' );
				Serial.print( "BNO055.CALIB_ACC:" );
				Serial.print( bno.m_accelCal );
				Serial.println( ';' );
				Serial.print( "BNO055.CALIB_GYR:" );
				Serial.print( bno.m_gyroCal );
				Serial.println( ';' );
				Serial.print( "BNO055.CALIB_SYS:" );
				Serial.print( bno.m_systemCal );
				Serial.println( ';' );

				// Get offsets
				bno.GetGyroOffsets();
				bno.GetAccelerometerOffsets();
				bno.GetMagnetometerOffsets();
			}
			else
			{
				Serial.print( "BNO055.CALIB_MAG:" );
				Serial.print( "N/A" );
				Serial.println( ';' );
				Serial.print( "BNO055.CALIB_ACC:" );
				Serial.print( "N/A" );
				Serial.println( ';' );
				Serial.print( "BNO055.CALIB_GYR:" );
				Serial.print( "N/A" );
				Serial.println( ';' );
				Serial.print( "BNO055.CALIB_SYS:" );
				Serial.print( "N/A" );
				Serial.println( ';' );
			}

			// Operating mode
			if( bno.GetOperatingMode() )
			{
				Serial.print( "BNO055.MODE:" );
				Serial.print( bno.m_operatingMode );
				Serial.println( ';' );
			}
			else
			{
				Serial.print( "BNO055.MODE:" );
				Serial.print( "N/A" );
				Serial.println( ';' );
			}

			// System status
			if( bno.GetSystemStatus() )
			{
				Serial.print( "BNO055_STATUS:" );
				Serial.print( bno.m_systemStatus, HEX );
				Serial.println( ";" );
			}
			else
			{
				Serial.print( "BNO055_STATUS:" );
				Serial.print( "N/A" );
				Serial.println( ";" );
			}

			// System Error
			if( bno.GetSystemError() )
			{
				Serial.print( "BNO055_ERROR_FLAG:" );
				Serial.print( bno.m_systemError );
				Serial.println( ";" );
			}
			else
			{
				Serial.print( "BNO055_ERROR_FLAG:" );
				Serial.print( "N/A" );
				Serial.println( ";" );
			}

		}

		// Get orientation data
        if( bno.GetVector( bosch::VECTOR_EULER, euler ) )
        {
            // Throw out exactly zero heading values that are all zeroes - necessary when switching modes
            if( euler.x() != 0.0f  )
            {
                // These may need adjusting

                NDataManager::m_navData.YAW		= euler.x();
                NDataManager::m_navData.HDGD	= euler.x();
            }

			NDataManager::m_navData.PITC	= euler.z();
			NDataManager::m_navData.ROLL	= -euler.y();
        }
	}
}

#endif<commit_msg>Changed initialization to use the devices m_isInitialized member so that it can control its state internally<commit_after>
#include "SysConfig.h"
#if(HAS_BNO055)

#include "CBNO055.h"
#include <BNO055.h>
#include "CTimer.h"
#include <Wire.h>

#include "NDataManager.h"

namespace
{
	CTimer bno055_sample_timer;
	CTimer report_timer;
	CTimer imuTimer;
	CTimer fusionTimer;

	bool initalized				= false;
	bool browserPingReceived	= false;

	bool inFusionMode			= false;

	// Use the first Wire interface by default
	CBNO055Driver bno;

	imu::Vector<3> euler;

	float roll = 0.0f;
	float pitch = 0.0f;
	float yaw = 0.0f;

	int m_wireInterfaceSelector = 0;
	TwoWire *m_pWire = nullptr;

	void InitializeSensor()
	{
		// Attempt to initialize. If this fails, we try every 30 seconds in its update loop
		if( !bno.Initialize( m_pWire ) )
		{
			Serial.println( "BNO_INIT_STATUS:FAILED;" );
		}
		else
		{
			Serial.println( "BNO_INIT_STATUS:SUCCESS;" );

			Serial.print( "BNO055.SW_Revision_ID:" );
			Serial.print( bno.m_softwareVersionMajor, HEX );
			Serial.print( "." );
			Serial.print( bno.m_softwareVersionMinor, HEX );
			Serial.println( ";" );

			Serial.print( "BNO055.bootloader:" );
			Serial.print( bno.m_bootloaderRev );
			Serial.println( ";" );

			inFusionMode = true;
		}
	}
}

// CBNO055::CBNO055( int wireInterfaceSelectorIn )
// {
// 	m_wireInterfaceSelector = wireInterfaceSelectorIn;
// }

void CBNO055::Initialize()
{
	Serial.println( "BNO055.Status:INIT;" );

	if( m_wireInterfaceSelector > WIRE_INTERFACES_COUNT )
	{
		// Invalid wire interface selected
		Serial.println( "ERROR:Invalid wire interface selected for BNO055;" );
		m_pWire = nullptr;
	}

	switch( m_wireInterfaceSelector )
	{
		case BNO_WIRE_INTERFACE_0:
			{
				m_pWire = WIRE_INTERFACE_0;
				break;
			}

		case BNO_WIRE_INTERFACE_1:
			{
				m_pWire = WIRE_INTERFACE_1;
				break;
			}

		case BNO_WIRE_INTERFACE_2:
			{
				m_pWire = WIRE_INTERFACE_2;
				break;
			}

		case BNO_WIRE_INTERFACE_3:
			{
				m_pWire = WIRE_INTERFACE_3;
				break;
			}

		case BNO_WIRE_INTERFACE_4:
			{
				m_pWire = WIRE_INTERFACE_4;
				break;
			}

		case BNO_WIRE_INTERFACE_5:
			{
				m_pWire = WIRE_INTERFACE_5;
				break;
			}

		default:
			m_pWire = nullptr;
			break;
	}

	//Reset timers
	bno055_sample_timer.Reset();
	report_timer.Reset();
	imuTimer.Reset();
	fusionTimer.Reset();

	Serial.println( "BNO055.Status:POSTINIT;" );
}

void CBNO055::Update( CCommand &commandIn )
{
	// 100hz
	if( bno055_sample_timer.HasElapsed( 10 ) )
	{
		if( !bno.m_isInitialized )
		{
			// Attempt every 10 secs
			if( report_timer.HasElapsed( 5000 ) )
			{
				Serial.println( "BNO055.Status:TRYING;" );
				// Attempt to initialize the chip again
				InitializeSensor();
			}

			return;
		}

		// bno.GetCalibration();
		// bno.GetSystemStatus();
		// bno.GetSystemError();
		// bno.GetVector( bosch::EVectorType::VECTOR_EULER, euler );

		// yaw = fmod( euler.x() + 90.0f, 360.0f );

		// if( yaw < 0.0f )
		// {
		// 	yaw += 360.0f;
		// }

		// pitch		= euler.y();
		// roll		= euler.z();

		// System status checks
		if( report_timer.HasElapsed( 1000 ) )
		{
			// System calibration
			if( bno.GetCalibration() )
			{
				Serial.print( "BNO055.CALIB_MAG:" );
				Serial.print( bno.m_magCal );
				Serial.println( ';' );
				Serial.print( "BNO055.CALIB_ACC:" );
				Serial.print( bno.m_accelCal );
				Serial.println( ';' );
				Serial.print( "BNO055.CALIB_GYR:" );
				Serial.print( bno.m_gyroCal );
				Serial.println( ';' );
				Serial.print( "BNO055.CALIB_SYS:" );
				Serial.print( bno.m_systemCal );
				Serial.println( ';' );

				// Get offsets
				bno.GetGyroOffsets();
				bno.GetAccelerometerOffsets();
				bno.GetMagnetometerOffsets();
			}
			else
			{
				Serial.print( "BNO055.CALIB_MAG:" );
				Serial.print( "N/A" );
				Serial.println( ';' );
				Serial.print( "BNO055.CALIB_ACC:" );
				Serial.print( "N/A" );
				Serial.println( ';' );
				Serial.print( "BNO055.CALIB_GYR:" );
				Serial.print( "N/A" );
				Serial.println( ';' );
				Serial.print( "BNO055.CALIB_SYS:" );
				Serial.print( "N/A" );
				Serial.println( ';' );
			}

			// Operating mode
			if( bno.GetOperatingMode() )
			{
				Serial.print( "BNO055.MODE:" );
				Serial.print( bno.m_operatingMode );
				Serial.println( ';' );
			}
			else
			{
				Serial.print( "BNO055.MODE:" );
				Serial.print( "N/A" );
				Serial.println( ';' );
			}

			// System status
			if( bno.GetSystemStatus() )
			{
				Serial.print( "BNO055_STATUS:" );
				Serial.print( bno.m_systemStatus, HEX );
				Serial.println( ";" );
			}
			else
			{
				Serial.print( "BNO055_STATUS:" );
				Serial.print( "N/A" );
				Serial.println( ";" );
			}

			// System Error
			if( bno.GetSystemError() )
			{
				Serial.print( "BNO055_ERROR_FLAG:" );
				Serial.print( bno.m_systemError );
				Serial.println( ";" );
			}
			else
			{
				Serial.print( "BNO055_ERROR_FLAG:" );
				Serial.print( "N/A" );
				Serial.println( ";" );
			}

		}

		// Get orientation data
        if( bno.GetVector( bosch::VECTOR_EULER, euler ) )
        {
            // Throw out exactly zero heading values that are all zeroes - necessary when switching modes
            if( euler.x() != 0.0f  )
            {
                // These may need adjusting

                NDataManager::m_navData.YAW		= euler.x();
                NDataManager::m_navData.HDGD	= euler.x();
            }

			NDataManager::m_navData.PITC	= euler.z();
			NDataManager::m_navData.ROLL	= -euler.y();
        }
	}
}

#endif<|endoftext|>


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

Copyright (c) 2000-2009 Torus Knot Software Ltd
Also see acknowledgements in Readme.html

This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.

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 Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA, or go to
http://www.gnu.org/copyleft/lesser.txt.

You may alternatively use this source under the terms of a specific version of
the OGRE Unrestricted License provided you have obtained such a license from
Torus Knot Software Ltd.
-----------------------------------------------------------------------------
*/
#include "OgreTerrainMaterialGeneratorA.h"
#include "OgreRoot.h"
#include "OgreSceneManager.h"
#include "OgreSceneManagerEnumerator.h"
#include "OgreTextureManager.h"
#include "OgreTexture.h"
#include "OgreTerrain.h"
#include "OgreManualObject.h"
#include "OgreCamera.h"
#include "OgreViewport.h"
#include "OgreRenderTexture.h"
#include "OgreHardwarePixelBuffer.h"
#include "OgreRenderSystem.h"

namespace Ogre
{
	//---------------------------------------------------------------------
	TerrainMaterialGenerator::TerrainMaterialGenerator()
		: mActiveProfile(0)
		, mChangeCounter(0)
		, mDebugLevel(0)
		, mCompositeMapSM(0)
		, mCompositeMapCam(0)
		, mCompositeMapRTT(0)
		, mCompositeMapPlane(0)
		, mCompositeMapLight(0)
	{

	}
	//---------------------------------------------------------------------
	TerrainMaterialGenerator::~TerrainMaterialGenerator()
	{
		for (ProfileList::iterator i = mProfiles.begin(); i != mProfiles.end(); ++i)
			OGRE_DELETE *i;

		if (mCompositeMapRTT && TextureManager::getSingletonPtr())
		{
			TextureManager::getSingleton().remove(mCompositeMapRTT->getHandle());
			mCompositeMapRTT = 0;
		}
		if (mCompositeMapSM && Root::getSingletonPtr())
		{
			// will also delete cam and objects etc
			Root::getSingleton().destroySceneManager(mCompositeMapSM);
			mCompositeMapSM = 0;
			mCompositeMapCam = 0;
			mCompositeMapPlane = 0;
			mCompositeMapLight = 0;
		}
	}
	//---------------------------------------------------------------------
	void TerrainMaterialGenerator::_renderCompositeMap(size_t size,
		const Rect& rect, const MaterialPtr& mat, const TexturePtr& destCompositeMap)
	{
		if (!mCompositeMapSM)
		{
			// dedicated SceneManager
			mCompositeMapSM = Root::getSingleton().createSceneManager(DefaultSceneManagerFactory::FACTORY_TYPE_NAME);
			float camDist = 100;
			float halfCamDist = camDist * 0.5f;
			mCompositeMapCam = mCompositeMapSM->createCamera("cam");
			mCompositeMapCam->setPosition(0, 0, camDist);
			mCompositeMapCam->lookAt(Vector3::ZERO);
			mCompositeMapCam->setProjectionType(PT_ORTHOGRAPHIC);
			mCompositeMapCam->setNearClipDistance(10);
			mCompositeMapCam->setFarClipDistance(500);
			mCompositeMapCam->setOrthoWindow(camDist, camDist);

			// Just in case material relies on light auto params
			mCompositeMapLight = mCompositeMapSM->createLight();
			mCompositeMapLight->setType(Light::LT_DIRECTIONAL);

			RenderSystem* rSys = Root::getSingleton().getRenderSystem();
			Real hOffset = rSys->getHorizontalTexelOffset() / (Real)size;
			Real vOffset = rSys->getVerticalTexelOffset() / (Real)size;

			// set up scene
			mCompositeMapPlane = mCompositeMapSM->createManualObject();
			mCompositeMapPlane->begin(mat->getName());
			mCompositeMapPlane->position(-halfCamDist, halfCamDist, 0);
			mCompositeMapPlane->textureCoord(0 - hOffset, 0 - vOffset);
			mCompositeMapPlane->position(-halfCamDist, -halfCamDist, 0);
			mCompositeMapPlane->textureCoord(0 - hOffset, 1 - vOffset);
			mCompositeMapPlane->position(halfCamDist, -halfCamDist, 0);
			mCompositeMapPlane->textureCoord(1 - hOffset, 1 - vOffset);
			mCompositeMapPlane->position(halfCamDist, halfCamDist, 0);
			mCompositeMapPlane->textureCoord(1 - hOffset, 0 - vOffset);
			mCompositeMapPlane->quad(0, 1, 2, 3);
			mCompositeMapPlane->end();
			mCompositeMapSM->getRootSceneNode()->attachObject(mCompositeMapPlane);

		}

		// update
		mCompositeMapPlane->setMaterialName(0, mat->getName());
		mCompositeMapLight->setDirection(TerrainGlobalOptions::getLightMapDirection());
		mCompositeMapLight->setDiffuseColour(TerrainGlobalOptions::getCompositeMapDiffuse());
		mCompositeMapSM->setAmbientLight(TerrainGlobalOptions::getCompositeMapAmbient());

		// check for size change (allow smaller to be reused)
		if (mCompositeMapRTT && size != mCompositeMapRTT->getWidth())
		{
			TextureManager::getSingleton().remove(mCompositeMapRTT->getHandle());
			mCompositeMapRTT = 0;
		}

		if (!mCompositeMapRTT)
		{
			mCompositeMapRTT = TextureManager::getSingleton().createManual(
				mCompositeMapSM->getName() + "/compRTT",
				ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, TEX_TYPE_2D, size, size, 0, PF_BYTE_RGBA,
				TU_RENDERTARGET).get();
			RenderTarget* rtt = mCompositeMapRTT->getBuffer()->getRenderTarget();
			// don't render all the time, only on demand
			rtt->setAutoUpdated(false);
			Viewport* vp = rtt->addViewport(mCompositeMapCam);
			// don't render overlays
			vp->setOverlaysEnabled(false);

		}

		// calculate the area we need to update
		Real vpleft = (Real)rect.left / (Real)size;
		Real vptop = (Real)rect.top / (Real)size;
		Real vpright = (Real)rect.right / (Real)size;
		Real vpbottom = (Real)rect.bottom / (Real)size;
		Real vpwidth = (Real)rect.width()/ (Real)size;
		Real vpheight = (Real)rect.height() / (Real)size;

		RenderTarget* rtt = mCompositeMapRTT->getBuffer()->getRenderTarget();
		Viewport* vp = rtt->getViewport(0);
		mCompositeMapCam->setWindow(vpleft, vptop, vpright, vpbottom);

		rtt->update();

		// We have an RTT, we want to copy the results into a regular texture
		// That's because in non-update scenarios we don't want to keep an RTT
		// around. We use a single RTT to serve all terrain pages which is more
		// efficient.
		Image::Box box(rect.left, rect.top, rect.right, rect.bottom);
		destCompositeMap->getBuffer()->blit(mCompositeMapRTT->getBuffer(), box, box);

	}
	//---------------------------------------------------------------------
	//---------------------------------------------------------------------
	void TerrainMaterialGenerator::Profile::updateCompositeMap(const Terrain* terrain, const Rect& rect)
	{
		// convert point-space rect into image space
		size_t compSize = terrain->getCompositeMap()->getWidth();
		Rect imgRect;
		Vector3 inVec, outVec;
		inVec.x = rect.left;
		inVec.y = rect.bottom - 1; // this is 'top' in image space, also make inclusive
		terrain->convertPosition(Terrain::POINT_SPACE, inVec, Terrain::TERRAIN_SPACE, outVec);
		imgRect.left = outVec.x * compSize;
		imgRect.top = (1.0 - outVec.y) * compSize;
		inVec.x = rect.right - 1;
		inVec.y = rect.top; // this is 'bottom' in image space
		terrain->convertPosition(Terrain::POINT_SPACE, inVec, Terrain::TERRAIN_SPACE, outVec);
		imgRect.right = outVec.x * (Real)compSize;
		imgRect.bottom = (1.0 - outVec.y) * compSize;

		mParent->_renderCompositeMap(
			compSize, imgRect,
			terrain->getCompositeMapMaterial(),
			terrain->getCompositeMap());

	}

}

<commit_msg>Fix some rectangle calculation edge cases in blend map / composite map conversion<commit_after>/*
-----------------------------------------------------------------------------
This source file is part of OGRE
(Object-oriented Graphics Rendering Engine)
For the latest info, see http://www.ogre3d.org/

Copyright (c) 2000-2009 Torus Knot Software Ltd
Also see acknowledgements in Readme.html

This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.

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 Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA, or go to
http://www.gnu.org/copyleft/lesser.txt.

You may alternatively use this source under the terms of a specific version of
the OGRE Unrestricted License provided you have obtained such a license from
Torus Knot Software Ltd.
-----------------------------------------------------------------------------
*/
#include "OgreTerrainMaterialGeneratorA.h"
#include "OgreRoot.h"
#include "OgreSceneManager.h"
#include "OgreSceneManagerEnumerator.h"
#include "OgreTextureManager.h"
#include "OgreTexture.h"
#include "OgreTerrain.h"
#include "OgreManualObject.h"
#include "OgreCamera.h"
#include "OgreViewport.h"
#include "OgreRenderTexture.h"
#include "OgreHardwarePixelBuffer.h"
#include "OgreRenderSystem.h"

namespace Ogre
{
	//---------------------------------------------------------------------
	TerrainMaterialGenerator::TerrainMaterialGenerator()
		: mActiveProfile(0)
		, mChangeCounter(0)
		, mDebugLevel(0)
		, mCompositeMapSM(0)
		, mCompositeMapCam(0)
		, mCompositeMapRTT(0)
		, mCompositeMapPlane(0)
		, mCompositeMapLight(0)
	{

	}
	//---------------------------------------------------------------------
	TerrainMaterialGenerator::~TerrainMaterialGenerator()
	{
		for (ProfileList::iterator i = mProfiles.begin(); i != mProfiles.end(); ++i)
			OGRE_DELETE *i;

		if (mCompositeMapRTT && TextureManager::getSingletonPtr())
		{
			TextureManager::getSingleton().remove(mCompositeMapRTT->getHandle());
			mCompositeMapRTT = 0;
		}
		if (mCompositeMapSM && Root::getSingletonPtr())
		{
			// will also delete cam and objects etc
			Root::getSingleton().destroySceneManager(mCompositeMapSM);
			mCompositeMapSM = 0;
			mCompositeMapCam = 0;
			mCompositeMapPlane = 0;
			mCompositeMapLight = 0;
		}
	}
	//---------------------------------------------------------------------
	void TerrainMaterialGenerator::_renderCompositeMap(size_t size,
		const Rect& rect, const MaterialPtr& mat, const TexturePtr& destCompositeMap)
	{
		if (!mCompositeMapSM)
		{
			// dedicated SceneManager
			mCompositeMapSM = Root::getSingleton().createSceneManager(DefaultSceneManagerFactory::FACTORY_TYPE_NAME);
			float camDist = 100;
			float halfCamDist = camDist * 0.5f;
			mCompositeMapCam = mCompositeMapSM->createCamera("cam");
			mCompositeMapCam->setPosition(0, 0, camDist);
			mCompositeMapCam->lookAt(Vector3::ZERO);
			mCompositeMapCam->setProjectionType(PT_ORTHOGRAPHIC);
			mCompositeMapCam->setNearClipDistance(10);
			mCompositeMapCam->setFarClipDistance(500);
			mCompositeMapCam->setOrthoWindow(camDist, camDist);

			// Just in case material relies on light auto params
			mCompositeMapLight = mCompositeMapSM->createLight();
			mCompositeMapLight->setType(Light::LT_DIRECTIONAL);

			RenderSystem* rSys = Root::getSingleton().getRenderSystem();
			Real hOffset = rSys->getHorizontalTexelOffset() / (Real)size;
			Real vOffset = rSys->getVerticalTexelOffset() / (Real)size;

			// set up scene
			mCompositeMapPlane = mCompositeMapSM->createManualObject();
			mCompositeMapPlane->begin(mat->getName());
			mCompositeMapPlane->position(-halfCamDist, halfCamDist, 0);
			mCompositeMapPlane->textureCoord(0 - hOffset, 0 - vOffset);
			mCompositeMapPlane->position(-halfCamDist, -halfCamDist, 0);
			mCompositeMapPlane->textureCoord(0 - hOffset, 1 - vOffset);
			mCompositeMapPlane->position(halfCamDist, -halfCamDist, 0);
			mCompositeMapPlane->textureCoord(1 - hOffset, 1 - vOffset);
			mCompositeMapPlane->position(halfCamDist, halfCamDist, 0);
			mCompositeMapPlane->textureCoord(1 - hOffset, 0 - vOffset);
			mCompositeMapPlane->quad(0, 1, 2, 3);
			mCompositeMapPlane->end();
			mCompositeMapSM->getRootSceneNode()->attachObject(mCompositeMapPlane);

		}

		// update
		mCompositeMapPlane->setMaterialName(0, mat->getName());
		mCompositeMapLight->setDirection(TerrainGlobalOptions::getLightMapDirection());
		mCompositeMapLight->setDiffuseColour(TerrainGlobalOptions::getCompositeMapDiffuse());
		mCompositeMapSM->setAmbientLight(TerrainGlobalOptions::getCompositeMapAmbient());

		// check for size change (allow smaller to be reused)
		if (mCompositeMapRTT && size != mCompositeMapRTT->getWidth())
		{
			TextureManager::getSingleton().remove(mCompositeMapRTT->getHandle());
			mCompositeMapRTT = 0;
		}

		if (!mCompositeMapRTT)
		{
			mCompositeMapRTT = TextureManager::getSingleton().createManual(
				mCompositeMapSM->getName() + "/compRTT",
				ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, TEX_TYPE_2D, size, size, 0, PF_BYTE_RGBA,
				TU_RENDERTARGET).get();
			RenderTarget* rtt = mCompositeMapRTT->getBuffer()->getRenderTarget();
			// don't render all the time, only on demand
			rtt->setAutoUpdated(false);
			Viewport* vp = rtt->addViewport(mCompositeMapCam);
			// don't render overlays
			vp->setOverlaysEnabled(false);

		}

		// calculate the area we need to update
		Real vpleft = (Real)rect.left / (Real)size;
		Real vptop = (Real)rect.top / (Real)size;
		Real vpright = (Real)rect.right / (Real)size;
		Real vpbottom = (Real)rect.bottom / (Real)size;
		Real vpwidth = (Real)rect.width()/ (Real)size;
		Real vpheight = (Real)rect.height() / (Real)size;

		RenderTarget* rtt = mCompositeMapRTT->getBuffer()->getRenderTarget();
		Viewport* vp = rtt->getViewport(0);
		mCompositeMapCam->setWindow(vpleft, vptop, vpright, vpbottom);

		rtt->update();

		// We have an RTT, we want to copy the results into a regular texture
		// That's because in non-update scenarios we don't want to keep an RTT
		// around. We use a single RTT to serve all terrain pages which is more
		// efficient.
		Image::Box box(rect.left, rect.top, rect.right, rect.bottom);
		destCompositeMap->getBuffer()->blit(mCompositeMapRTT->getBuffer(), box, box);

	}
	//---------------------------------------------------------------------
	//---------------------------------------------------------------------
	void TerrainMaterialGenerator::Profile::updateCompositeMap(const Terrain* terrain, const Rect& rect)
	{
		// convert point-space rect into image space
		size_t compSize = terrain->getCompositeMap()->getWidth();
		Rect imgRect;
		Vector3 inVec, outVec;
		inVec.x = rect.left;
		inVec.y = rect.bottom - 1; // this is 'top' in image space, also make inclusive
		terrain->convertPosition(Terrain::POINT_SPACE, inVec, Terrain::TERRAIN_SPACE, outVec);
		imgRect.left = outVec.x * compSize;
		imgRect.top = (1.0 - outVec.y) * compSize;
		inVec.x = rect.right - 1;
		inVec.y = rect.top; // this is 'bottom' in image space
		terrain->convertPosition(Terrain::POINT_SPACE, inVec, Terrain::TERRAIN_SPACE, outVec);
		imgRect.right = outVec.x * (Real)compSize + 1;
		imgRect.bottom = (1.0 - outVec.y) * compSize + 1;

		imgRect.left = std::max(0L, imgRect.left);
		imgRect.top = std::max(0L, imgRect.top);
		imgRect.right = std::min((long)compSize, imgRect.right);
		imgRect.bottom = std::min((long)compSize, imgRect.bottom);

		mParent->_renderCompositeMap(
			compSize, imgRect,
			terrain->getCompositeMapMaterial(),
			terrain->getCompositeMap());

	}

}

<|endoftext|>


<commit_before>#include "GraphLegend.h"

namespace CppChart
{
	GraphLegendUnit::GraphLegendUnit(float x, float y, const sf::Color& c, const std::string& str)
	{
		LogFnStart();

		SetRectangleShape(x, y, c);
		SetString(str);

		LogFnEnd();
	}

	GraphLegendUnit::GraphLegendUnit(float r, const sf::Color& c, const std::string& str)
	{
		LogFnStart();

		SetCircleShape(r, c);
		SetString(str);

		LogFnEnd();
	}

	void GraphLegendUnit::SetRectangleShape(float x, float y, const sf::Color& c)
	{
		LogFnStart();

		m_shape = new sf::RectangleShape(sf::Vector2f(x, y));
		m_shape->setFillColor(c);
		m_shapeType = Shape::RECTANGULAR;

		LogFnEnd();
	}

	void GraphLegendUnit::SetCircleShape(float r, const sf::Color& c)
	{
		LogFnStart();

		m_shape = new sf::CircleShape(r);
		m_shape->setFillColor(c);
		m_shapeType = Shape::CIRCULAR;

		LogFnEnd();
	}
}<commit_msg>Implement vertical part of GraphLegend::Render()<commit_after>#include "GraphLegend.h"

namespace CppChart
{
	GraphLegendUnit::GraphLegendUnit(float x, float y, const sf::Color& c, const std::string& str)
	{
		LogFnStart();

		SetRectangleShape(x, y, c);
		SetString(str);

		LogFnEnd();
	}

	GraphLegendUnit::GraphLegendUnit(float r, const sf::Color& c, const std::string& str)
	{
		LogFnStart();

		SetCircleShape(r, c);
		SetString(str);

		LogFnEnd();
	}

	void GraphLegendUnit::SetRectangleShape(float x, float y, const sf::Color& c)
	{
		LogFnStart();

		m_shape = new sf::RectangleShape(sf::Vector2f(x, y));
		m_shape->setFillColor(c);
		m_shapeType = Shape::RECTANGULAR;

		LogFnEnd();
	}

	void GraphLegendUnit::SetCircleShape(float r, const sf::Color& c)
	{
		LogFnStart();

		m_shape = new sf::CircleShape(r);
		m_shape->setFillColor(c);
		m_shapeType = Shape::CIRCULAR;

		LogFnEnd();
	}

	void GraphLegend::Render()
	{
		LogFnStart();

		float lx = 0.0f, ly = 0.0f;

		m_texture.create(m_width, m_height);
		m_texture.clear(m_bgColor);
		CreateLegendObjects();

		switch (m_orientation)
		{
		case Orientation::VERTICAL:
			for (auto& e : m_legend)
			{
				lx = 0.0f;
				e.m_text.setCharacterSize(m_fontSize);
				e.m_text.setFont(m_font);

				if (m_beforeText)
				{
					e.m_shape->setPosition(sf::Vector2f(lx, ly));
					lx += m_shape.width + 5.0f;
					SetTextAtCenter(e.m_text, lx, ly, m_shape.width, m_shape.height);
				}
				else
				{
					SetTextAtCenter(e.m_text, lx, ly, m_shape.width, m_shape.height);
					lx += m_fontSize * e.m_text.getString().getSize() + 5.0f;
					e.m_shape->setPosition(sf::Vector2f(lx, ly));
				}

				ly += m_shape.height + 5.0f;
				m_texture.draw(e.m_text);
				m_texture.draw(*e.m_shape);
			}
			break;
		case Orientation::HORIZONTAL:
			break;
		default:
			throw std::exception("This is never allowed to happen.");
		}

		LogFnEnd();
	}
}<|endoftext|>


<commit_before>/*
* Software License Agreement (MIT License)
*
* Copyright (c) 2014 S.R.Manikandasriram
* All rights reserved.
*
* This software is distributed under MIT License.
*/

#include <ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
#include <iostream>

#define _USE_MATH_DEFINES
#include <math.h>
#include "ev3dev.h"
#include <thread>
#include <chrono>

using namespace std;
using namespace ev3dev;

string ns = "/robot3/";
ros::NodeHandle  nh;

float x = 0.0, y = 0.0, t = 0.0;
float x_est = 0.0, y_est = 0.0, t_est = 0.0;
bool new_estimate = false;
float R, L, speed;	// Update R and L from Parameter server
const float deg2rad = M_PI/180.0;

motor left_motor, right_motor;
sensor s;

float vx, wt, vl, vr;
bool running = true;

int encoder[2]={0,0}, prev_encoder[2] = {0,0}, dl, dr;

const char map_name[4] = "map";

void cmd_vel_cb(const geometry_msgs::Twist& cmd){
	float vx, wt, vl, vr;
	vx = cmd.linear.x;
	wt = cmd.angular.z;
	vl = (vx+L/2*wt)/speed;
	vr = (vx-L/2*wt)/speed;
	if(vl!=0)
	{
		left_motor.set_pulses_per_second_setpoint(vl);
		left_motor.run(true);
	}
	else
	{
		left_motor.set_pulses_per_second_setpoint(0);
		left_motor.run(false);
	}
	if(vr!=0)
	{
		right_motor.set_pulses_per_second_setpoint(vr);
		right_motor.run(true);
	}
	else
	{
		right_motor.set_pulses_per_second_setpoint(0);
		right_motor.run(false);
	}
	cout << "received " << vx << "," << wt << "=>" << vl <<"," << vr << endl;
}

void pose_cb(const geometry_msgs::PoseStamped& msg){
	x_est = msg.pose.position.x;
	y_est = msg.pose.position.y;
	geometry_msgs::Quaternion q = msg.pose.orientation;
	t_est = 2.0*atan2(q.z,q.w);
	new_estimate = true;
	cout << " Estimate: " << x_est << "," << y_est << "," << t_est <<endl;
}

ros::Subscriber<geometry_msgs::Twist> cmd_sub("/robot3/cmd_vel", cmd_vel_cb );
ros::Subscriber<geometry_msgs::PoseStamped> pose_sub("/robot3/pose_estimate" , pose_cb );

void odometry()
{
	int encoder[2]={0,0}, prev_encoder[2] = {0,0}, dl, dr;
	while(running){
		if(new_estimate){
			x = x_est;
			y = y_est;
			t = t_est;
			new_estimate = false;
		}
		encoder[0] = left_motor.position();
		encoder[1] = right_motor.position();
		dl = encoder[0]-prev_encoder[0];
		dr = encoder[1]-prev_encoder[1];
		x += cos(t)*speed*(dl+dr)/2.0;
		y += sin(t)*speed*(dl+dr)/2.0;
		t += speed*(dl-dr)/L;
		prev_encoder[0] = encoder[0];
		prev_encoder[1] = encoder[1];
		vl = left_motor.pulses_per_second();
		vr = right_motor.pulses_per_second();
		vx = (vl+vr)/2*speed;
		wt = (vl-vr)/L*speed;
		this_thread::sleep_for(chrono::milliseconds(100));
	}
}

int main(int argc, char* argv[])
{
	if(argc<5)
	{
		cerr << "Usage: " << argv[0] << " <socket> <left_motor_port> <right_motor_port> <sensor_port> <hz>" << endl;
		return 1;
	}
	int milliseconds = 100;
	if(argc==6)
    	milliseconds = 1000/atoi(argv[5]);
    // cout<<"milliseconds"<<milliseconds;
    string left_motor_port (argv[2]);
    string right_motor_port (argv[3]);
    string sensor_port (argv[4]);
    // if(left_motor_port<1||left_motor_port>4||right_motor_port<1||right_motor_port>4||left_motor_port==right_motor_port)
    // {
		// cerr << "Invalid motor port numbers. Must be 1, 2, 3 or 4 and distinct." << endl;
		// return 1;
	// }

	// TODO: Check if both are of same type

	left_motor = motor(left_motor_port);
	right_motor = motor(right_motor_port);
	s = sensor(sensor_port);
    if(s.type()!="ev3-uart-30")
    {
		cerr << "Invalid sensor type. Must be EV3 ultrasonic. Given sensor is of type " << s.type() << endl;
		return 1;
	}

	// TODO: Check if both were initialised

	left_motor.reset();
	left_motor.set_position(0);
	left_motor.set_run_mode("forever");
	left_motor.set_stop_mode("brake");
	left_motor.set_regulation_mode("on");

	right_motor.reset();
	right_motor.set_position(0);
	right_motor.set_run_mode("forever");
	right_motor.set_stop_mode("brake");
	right_motor.set_regulation_mode("on");

	// Set mode
	string mode="US-DIST-CM";
	s.set_mode(mode);

	cout<<" Initialiased motors."<<endl;

    nav_msgs::Odometry odom_msg;
	ros::Publisher odom_pub("/robot3/odom", &odom_msg);
	nh.advertise(odom_pub);
	tf::TransformBroadcaster odom_broadcaster;
	tf::TransformBroadcaster scan_broadcaster;
	nh.subscribe(cmd_sub);
	nh.subscribe(pose_sub);
	cout<<" Initialiased Publisher and TransformBroadcaster."<<endl;

 	sensor_msgs::LaserScan us_msg;
 	ros::Publisher us_pub("/robot3/scan", &us_msg);
 	nh.advertise(us_pub);
 	int number = 100;
 	float ranges[100]={0.0}, intensities[100] = {255};
 	const char laser_frame_name[20] = "/robot3/laser_frame";
 	us_msg.header.frame_id = laser_frame_name;
	us_msg.angle_min = -M_PI/40;
    us_msg.angle_max = M_PI/40;
    us_msg.angle_increment = M_PI/(20*number);
    us_msg.time_increment = 0.0000;
    us_msg.scan_time = 0.0000;
    us_msg.range_min = 0.01;
    us_msg.range_max = 2.55;
    us_msg.ranges_length = number;
    us_msg.intensities_length = number;

	ros::Time current_time, last_time;
	current_time = nh.now();
	last_time = nh.now();

	//ros::Rate r(1.0);

    cout<<" Initialiased Time variables."<<endl;

    nh.initNode(argv[1]);
	odom_broadcaster.init(nh);
	scan_broadcaster.init(nh);
    while(!nh.connected()) {nh.spinOnce();}

	cout<<" Initialiased node."<<endl;

	if (! nh.getParam((ns+"R").c_str(), &R)){
	     R = 0.03;
	     speed = R*deg2rad;
	}

	if (! nh.getParam((ns+"L").c_str(), &L)){
	     L = 0.12;
	}

	cout<<" Retrieved params R,L : "<<R<<","<<L<<endl;
	thread odom (odometry);
	while(1){

		nh.spinOnce();               // check for incoming messages
		current_time = nh.now();

		// encoder[0] = left_motor.position();
		// encoder[1] = right_motor.position();
		// dl = encoder[0]-prev_encoder[0];
		// dr = encoder[1]-prev_encoder[1];
		// x += cos(t)*speed*(dl+dr)/2.0;
		// y += sin(t)*speed*(dl+dr)/2.0;
		// t += speed*(dl-dr)/L;
		// prev_encoder[0] = encoder[0];
		// prev_encoder[1] = encoder[1];

		// cout<<" Calculated: "<<x<<","<<y<<","<<t<<endl;

		//since all odometry is 6DOF we'll need a quaternion created from yaw
		geometry_msgs::Quaternion odom_quat = tf::createQuaternionFromYaw(t);

		// cout<<" Calculated quat: "<<endl;

		//first, we'll publish the transform over tf
		geometry_msgs::TransformStamped odom_trans;
		odom_trans.header.stamp = current_time;
		odom_trans.header.frame_id = map_name;
		const char base_link_name[18] = "/robot3/base_link";
		odom_trans.child_frame_id = base_link_name;

		// cout<<" constructed header"<<endl;

		odom_trans.transform.translation.x = x;
		odom_trans.transform.translation.y = y;
		odom_trans.transform.translation.z = 0.0;
		odom_trans.transform.rotation = odom_quat;

		// cout<<" Constructed full message"<<endl;

		//send the transform
		odom_broadcaster.sendTransform(odom_trans);

		geometry_msgs::Quaternion scan_quat = tf::createQuaternionFromYaw(0);

		geometry_msgs::TransformStamped scan_trans;
		scan_trans.header.stamp = current_time;
		scan_trans.header.frame_id = base_link_name;
		scan_trans.child_frame_id = laser_frame_name;

		// cout<<" constructed header"<<endl;

		scan_trans.transform.translation.x = L/2.0;
		scan_trans.transform.translation.y = 0;
		scan_trans.transform.translation.z = 0;
		scan_trans.transform.rotation = scan_quat;

		// cout<<" Constructed full message"<<endl;

		//send the transform
		scan_broadcaster.sendTransform(scan_trans);

		// cout<<" Sent transform"<<endl;

		//next, we'll publish the odometry message over ROS
		odom_msg.header.stamp = current_time;
		odom_msg.header.frame_id = map_name;

		//set the position
		odom_msg.pose.pose.position.x = x;
		odom_msg.pose.pose.position.y = y;
		odom_msg.pose.pose.position.z = 0.0;
		odom_msg.pose.pose.orientation = odom_quat;

		//set the velocity
		odom_msg.child_frame_id = base_link_name;
		odom_msg.twist.twist.linear.x = vx;
		odom_msg.twist.twist.linear.y = 0;
		odom_msg.twist.twist.angular.z = wt;

		// cout<<" sizeof "<<sizeof(odom_msg)<<endl;
		//publish the message
		odom_pub.publish(&odom_msg);

		// cout<<" Published odometry"<<endl;
		us_msg.header.stamp = current_time;
		float distance = s.value()/1000.0;
	    for (int i = 0; i < number; ++i)
	    {
	    	ranges[i] = distance;
	    	// if(abs(i-number/2)<4)
	    		// ranges[i] = distance;
	    }
	    us_msg.ranges = ranges;
	    us_msg.intensities = intensities;
	    us_pub.publish(&us_msg);
		cout<<" sizeof "<<sizeof(us_msg)<<endl;
		last_time = current_time;
		this_thread::sleep_for(chrono::milliseconds(milliseconds));
	}
	odom.join();
    return 0;

}<commit_msg>Delete diffdrive_ultrasonic.cpp<commit_after><|endoftext|>


<commit_before>#define CATCH_CONFIG_MAIN  // This tells Catch to provide a main() - only do this in one cpp file

#include "videosourcefactory.h"
#include "catch.hpp"
<commit_msg>Issue #121: migrated VideoSourceFactory tests from Python py.test suite over to C++ Catch suite (still need to find a way of parametrizing the colour space and device)<commit_after>#define CATCH_CONFIG_MAIN  // This tells Catch to provide a main() - only do this in one cpp file

#include "videosourcefactory.h"
#include "catch.hpp"

// TODO Parametrize device and colour
gg::Device device = gg::DVI2PCIeDuo_DVI;
gg::ColourSpace colour = gg::I420;

TEST_CASE( "get_instance returns singleton", "[VideoSourceFactory]" )
{
    gg::VideoSourceFactory & factory = gg::VideoSourceFactory::get_instance();
    gg::VideoSourceFactory & factory_ = gg::VideoSourceFactory::get_instance();
    REQUIRE( &factory == &factory_ );
}

TEST_CASE( "get_device returns connection", "[VideoSourceFactory]" )
{
    gg::VideoSourceFactory & factory =  gg::VideoSourceFactory::get_instance();
    IVideoSource * source = nullptr;
    source = factory.get_device(device, colour);
    REQUIRE_FALSE( source == nullptr );
    REQUIRE( source->get_colour() == colour );

    gg::VideoFrame frame(colour, false);
    REQUIRE( frame.cols() == 0 );
    REQUIRE( frame.rows() == 0 );
    REQUIRE( source->get_frame(frame) );
    REQUIRE( frame.cols() > 0 );
    REQUIRE( frame.rows() > 0 );
}

TEST_CASE( "get_device returns singleton", "[VideoSourceFactory]" )
{
    gg::VideoSourceFactory & factory =  gg::VideoSourceFactory::get_instance();
    IVideoSource * source = factory.get_device(device, colour);
    IVideoSource * source_ = factory.get_device(device, colour);
    REQUIRE_FALSE( source == nullptr );
    REQUIRE( source == source_ );
}

TEST_CASE( "get_device does not create duplicate", "[VideoSourceFactory]" )
{
    gg::VideoSourceFactory & factory = gg::VideoSourceFactory::get_instance();
    gg::ColourSpace other_colour;
    switch(colour)
    {
    case gg::I420:
        other_colour = gg::BGRA;
        break;
    case gg::BGRA:
        other_colour = gg::I420;
        break;
    default:
        FAIL("Colour space unknown");
        break;
    }
    IVideoSource * source = factory.get_device(device, colour);
    IVideoSource * source_duplicate = nullptr;
    REQUIRE_THROWS_AS(
        source_duplicate = factory.get_device(device, other_colour),
        gg::DeviceAlreadyConnected
    );
    REQUIRE( source_duplicate == nullptr );
}

TEST_CASE( "Factory garbage-collects devices", "[VideoSourceFactory]" )
{
    IVideoSource * source_dvi = nullptr;
    IVideoSource * source_sdi = nullptr;
    { // artificial scope: factory is deleted at the end
        gg::VideoSourceFactory & factory = gg::VideoSourceFactory::get_instance();
        source_dvi = factory.get_device(gg::DVI2PCIeDuo_DVI, colour);
        REQUIRE_FALSE( source_dvi == nullptr );
        source_sdi = factory.get_device(gg::DVI2PCIeDuo_SDI, colour);
        REQUIRE_FALSE( source_sdi == nullptr );
    }
    REQUIRE( source_dvi == nullptr );
    REQUIRE( source_sdi == nullptr );
}
<|endoftext|>


<commit_before>/* Bucky Frost
 * Paul Gentemann
 * CS 381
 * File Name : splinepatch.cpp
 * Last Modified : Thu 07 Nov 2013 12:02:38 PM AKST
 * Description : A pair of spline plains, patched together, that will
 * ripple upon user input.
 */

// OpenGL/GLUT includes - DO THESE FIRST
#include <cstdlib>       // Do this before GL/GLUT includes
using std::exit;
#ifndef __APPLE__
# include <GL/glew.h>
# include <GL/glut.h>    // Includes OpenGL headers as well
#else
# include <GLEW/glew.h>
# include <GLUT/glut.h>  // Apple puts glut.h in a different place
#endif
#ifdef _MSC_VER          // Tell MS-Visual Studio about GLEW lib
# pragma comment(lib, "glew32.lib")
#endif
#include "lib381/bitmapprinter.h"
#include "lib381/glslprog.h"    // For GLSL code-handling functions
#include <string>
using std::string;
#include <iostream>
using std::cout; using std::cerr; using std::endl;
#include <vector>
using std::vector;

#include "mouse.h"
Mouse mousy = Mouse();

// Function prototype
void documentation();

// Global variables
const int ESCKEY = 27;          // ASCII value of Escape
const int startwinsize = 600;   // Start window width & height (pixels)
int winw = 1, winh = 1;         // Window width, height (pixels)
bool help = false;

// Shaders
string vshader1fname;          // Filename for vertex shader source
string fshader1fname;          // Filename for fragment shader source
GLhandleARB prog1;             // GLSL Program Object

// Camera
GLdouble viewmatrix[16],
         viewang = 5.;          // Degrees

void drawBezierPatch(int subdivs)
{
    GLdouble cpts[48] = {
         1.5, -1.5, 0.,
//         mousy.getX()*1.5, -1.5, 0.,
         1.5, -0.5,-3.,
//         1.5, -0.5*mousy.getY(),-3.,
         1.5,  0.5,-2.,
         1.5,  1.5, 1.,
         0.5, -1.5, 0.,
         0.5, -0.5, 0.,
         0.5,  0.5, 0.,
         0.5,  1.5, 0.,
        -0.5, -1.5, 0.,
        -0.5, -0.5, 1.,
        -0.5,  0.5,-3.,
        -0.5,  1.5, 1.,
        -1.5, -1.5, 1.,
        -1.5, -0.5,-2.,
        -1.5,  0.5, 1.,
        -1.5,  1.5, 0.
    };

    glMap2d(GL_MAP2_VERTEX_3,
            0., 1.,
            3,
            4,
            0., 1.,
            3*4,
            4,
            cpts);
    glEnable(GL_MAP2_VERTEX_3);

    glMapGrid2d(subdivs, 0., 1., subdivs, 0., 1.);
    glEnable(GL_AUTO_NORMAL);

    glFrontFace(GL_CW);  // Normals are evidently backwards here :-(
    glEvalMesh2(GL_FILL, 0, subdivs, 0, subdivs);
    glFrontFace(GL_CCW);
}

// myDisplay
// The GLUT display function
void myDisplay()
{
    glClearColor(0.5f, 0.5f, 0.5f, 1.0f);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    GLhandleARB theprog;  // CURRENTLY-used program object or 0 if none
    // Activating the shaders
    glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
    theprog = prog1;

    glEnable(GL_DEPTH_TEST);    // Set up 3D

    // Setting up the camera view
    glLoadIdentity();
    glMultMatrixd(viewmatrix);

    // Position light source 0 & draw ball there
    glPushMatrix();
        glTranslated(0.0, 0.0, 1.0);
//        glRotated(lightrotang, 1.,0.,0.);
        glTranslated(-1., 1., 1.);
        GLfloat origin4[] = { 0.f, 0.f, 0.f, 1.f };
        glLightfv(GL_LIGHT0, GL_POSITION, origin4);
        GLfloat spotdir3[] = { 1.f, -1.f, -1.f };
        glLightfv(GL_LIGHT0, GL_SPOT_DIRECTION, spotdir3);
        glUseProgramObjectARB(0);
        glColor3d(1., 1., 1.);
        glutSolidSphere(0.1, 20, 15);
    glPopMatrix();

    //Send info to shader
    if (theprog)
    {
        GLint loc;  // Location for shader vars

        loc = glGetUniformLocationARB(theprog, "myb1");
        if (loc != -1)
        {
            glUniform1i(loc, false);
        }

        loc = glGetUniformLocationARB(theprog, "myf1");
        if (loc != -1)
        {
            glUniform1f(loc, GLfloat(.0));
        }
    }

    // Draw objects
    glColor3b(0.7, 0.0, 0.7);
    glTranslated(0,0,-4);
    drawBezierPatch(30);

    documentation();
    glutSwapBuffers();
}

// myIdle
// The GLUT idle function
void myIdle()
{
    glutPostRedisplay();
    // Print OpenGL errors, if there are any (for debugging)
    static int error_count = 0;
    if (GLenum err = glGetError())
    {
        ++error_count;
        cerr << "OpenGL ERROR " << error_count << ": "
             << gluErrorString(err) << endl;
    }
}

// set()
// Sets given matrix to given translation/rotation.
void set(GLdouble *arr, GLdouble tx, GLdouble ty, GLdouble tz,
         GLdouble rang=0., GLdouble rx=0., GLdouble ry=0., GLdouble rz=0.)
{
    glLoadIdentity();
    glTranslated(tx, ty, tz);
    glRotated(rang, rx, ry, rz);
    glMultMatrixd(arr);
    glGetDoublev(GL_MODELVIEW_MATRIX, arr);
}

// reset()
// Resets the given matrix to the identity
void reset(GLdouble *arr)
{
    glLoadIdentity();
    glGetDoublev(GL_MODELVIEW_MATRIX, arr);
}

// myKeyboard
// The GLUT keyboard function
void myKeyboard(unsigned char key, int x, int y)
{
    switch (key)
    {
    case ESCKEY:  // Esc: quit
        exit(0);
        break;
    case '+':
        set(viewmatrix, 1., 0., 0.);
        break;
    case '-':
        set(viewmatrix,-1., 0., 0.);
        break;
    case 'R':
    case 'r':
        reset(viewmatrix);
        break;
    case 'h':
    case 'H':
        help = !help;
    }
    glutPostRedisplay();
}

// mySpecial
// The GLUT special function
void mySpecial(int key, int x, int y)
{
    switch (key)
    {
        case GLUT_KEY_LEFT:
            set(viewmatrix, 0.,0.,0., -viewang, 0.,1.,0.);
            break;
        case GLUT_KEY_RIGHT:
            set(viewmatrix, 0.,0.,0., viewang, 0.,1.,0.);
            break;
        case GLUT_KEY_UP:
            set(viewmatrix, 0.,0.,0., -viewang,0.,.0,1.);
            break;
        case GLUT_KEY_DOWN:
            set(viewmatrix, 0.,0.,0., viewang,0.,.0,1.);
            break;
    }
    glutPostRedisplay();
}

// myReshape
// The GLUT reshape function
void myReshape(int w, int h)
{
    // Set viewport & save window dimensions in globals
    glViewport(0, 0, w, h);
    winw = w;
    winh = h;
    // Set up projection
    // Perspective projection with near quite near & far pretty far
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(60., double(w)/h, 0.01, 100.);
    glMatrixMode(GL_MODELVIEW);  // Always go back to model/view mode
}

// init
// Initialize GL states & global data
void init()
{
    reset(viewmatrix);    // Reset camera position
    glMatrixMode(GL_MODELVIEW);
}

// myPassiveMotion
// The GLUT passiveMotion function
void myPassiveMotion(int x, int y)
{
    // Save mouse position in class (haha)
    mousy.saveMouse(x, y);
}

// The main
int main(int argc, char ** argv)
{
    // Initilization of OpenGL/GLUT
    glutInit(&argc, argv);
    // Set shader source filenames. Done here, as opposed to in
    getShaderFilenames(vshader1fname, fshader1fname, argc, argv);
    //  function init, so that we can use command-line arguments.
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);

    // Creating the view window
    glutInitWindowSize(startwinsize, startwinsize);
    glutInitWindowPosition(50, 50);
    glutCreateWindow("CS 381 - Shaders, Normals, Lighting, and Splines Oh My!");

    // Init GLEW & check status - exit on failure
    if (glewInit() != GLEW_OK)
    {
        cerr << "glewInit failed" << endl;
        exit(1);
    }

    init();
    glutDisplayFunc(myDisplay);
    glutIdleFunc(myIdle);
    glutReshapeFunc(myReshape);
//    glutMouseFunc(myMouse);
    glutKeyboardFunc(myKeyboard);
    glutSpecialFunc(mySpecial);
    glutPassiveMotionFunc(myPassiveMotion);
    // Start GLUT event handling loop
    glutMainLoop();

    return 0;
}

void documentation()
{
    // Draw documentation
    glDisable(GL_DEPTH_TEST);
    glLoadIdentity();
    glMatrixMode(GL_PROJECTION);  // Set up simple ortho projection
    glPushMatrix();
        glLoadIdentity();
        gluOrtho2D(0., double(winw), 0., double(winh));
        glColor3d(0., 0., 0.);        // Black text
        BitmapPrinter p(20., winh-20., 20.);
        if(help)
        {
            p.print("Arrows         Rotate Scene");
            p.print("+/-            Zoom in/out");
            p.print("r              Reset Camera");
        }
        else
        {
            p.print("h              help");
        }
        p.print("Esc            Quit");
    glPopMatrix();                // Restore prev projection
    glMatrixMode(GL_MODELVIEW);
}
<commit_msg>Added a sine function modifier to the z-coord of drawSpline function. Also, keyboard interface for the rotation of the draw objects, and the appropriate documentation.<commit_after>/* Bucky Frost
 * Paul Gentemann
 * CS 381
 * File Name : splinepatch.cpp
 * Last Modified : Thu 07 Nov 2013 12:02:38 PM AKST
 * Description : A pair of spline plains, patched together, that will
 * ripple upon user input.
 */

// OpenGL/GLUT includes - DO THESE FIRST
#include <cstdlib>       // Do this before GL/GLUT includes
using std::exit;
#ifndef __APPLE__
# include <GL/glew.h>
# include <GL/glut.h>    // Includes OpenGL headers as well
#else
# include <GLEW/glew.h>
# include <GLUT/glut.h>  // Apple puts glut.h in a different place
#endif
#ifdef _MSC_VER          // Tell MS-Visual Studio about GLEW lib
# pragma comment(lib, "glew32.lib")
#endif
#include "lib381/bitmapprinter.h"
#include "lib381/glslprog.h"    // For GLSL code-handling functions
#include <string>
using std::string;
#include <iostream>
using std::cout; using std::cerr; using std::endl;
#include <vector>
using std::vector;
#include <sstream>
using std::ostringstream;
#include <iomanip>
using std::setprecision;
using std::fixed;
#include <cmath>
using std::sin;
#include "mouse.h"
Mouse mousy = Mouse();

// Function prototypes
void documentation();
void drawBezierPatch(int subdivs, double mod);
void myDisplay();
void myIdle();
void reset(GLdouble *arr);
void myKeyboard(unsigned char key, int x, int y);
void myPassiveMotion(int x, int y);
void init();
void myReshape(int w, int h);
void set(GLdouble *arr, GLdouble tx, GLdouble ty, GLdouble tz,
         GLdouble rang, GLdouble rx, GLdouble ry, GLdouble rz);

// Global variables
const int ESCKEY = 27;         // ASCII value of Escape
const int startwinsize = 600;  // Start window width & height (pixels)
int winw = 1, winh = 1;        // Window width, height (pixels)
bool help = false;

int numsubdivs;                // Number of subdivisions for object
const int minsubdivs = 1;      //  Minumum of above
const int maxsubdivs = 50;     //  Maximum of above

bool wave;                     // Starts up the wave to propagate throught the splines

// Shaders
string vshader1fname;          // Filename for vertex shader source
string fshader1fname;          // Filename for fragment shader source
GLhandleARB prog1;             // GLSL Program Object

// Camera
GLdouble viewmatrix[16],
         viewang = 5.;         // Degrees

double modd;                   // Value for used in the waves in the Bezier patches

// drawBezierPatch
// Draws a number of control points for a bezier patch
// The z coordinates of all the points are translated
// by the sine of the mod parameter.
void drawBezierPatch(int subdivs, double mod)
{
    GLdouble cpts[48] = {
         1.5, -1.5, sin(mod) * 0.,
         1.5, -0.5, sin(mod) * -3.,
         1.5,  0.5, sin(mod) * -2.,
         1.5,  1.5, sin(mod) * 1.,
         0.5, -1.5, sin(mod) * 0.,
         0.5, -0.5, sin(mod) * 0.,
         0.5,  0.5, sin(mod) * 0.,
         0.5,  1.5, sin(mod) * 0.,
        -0.5, -1.5, sin(mod) * 0.,
        -0.5, -0.5, sin(mod) * 1.,
        -0.5,  0.5, sin(mod) * -3.,
        -0.5,  1.5, sin(mod) * 1.,
        -1.5, -1.5, sin(mod) * 1.,
        -1.5, -0.5, sin(mod) * -2.,
        -1.5,  0.5, sin(mod) * 1.,
        -1.5,  1.5, sin(mod) * 0.
    };

    glMap2d(GL_MAP2_VERTEX_3,
            0., 1.,
            3,
            4,
            0., 1.,
            3*4,
            4,
            cpts);
    glEnable(GL_MAP2_VERTEX_3);

    glMapGrid2d(subdivs, 0., 1., subdivs, 0., 1.);
    glEnable(GL_AUTO_NORMAL);

    glFrontFace(GL_CW);  // Normals are evidently backwards here :-(
    glEvalMesh2(GL_FILL, 0, subdivs, 0, subdivs);
    glFrontFace(GL_CCW);
}

// myDisplay
// The GLUT display function
void myDisplay()
{
    glClearColor(0.5f, 0.5f, 0.5f, 1.0f);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    GLhandleARB theprog;  // CURRENTLY-used program object or 0 if none
    // Activating the shaders
    glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
    theprog = prog1;

    glEnable(GL_DEPTH_TEST);    // Set up 3D

    // Setting up the camera view
    glLoadIdentity();
    glMultMatrixd(viewmatrix);

    // Position light source 0 & draw ball there
    glPushMatrix();
        glTranslated(0.0, 0.0, 1.0);
//        glRotated(lightrotang, 1.,0.,0.);
        glTranslated(-1., 1., 1.);
        GLfloat origin4[] = { 0.f, 0.f, 0.f, 1.f };
        glLightfv(GL_LIGHT0, GL_POSITION, origin4);
        GLfloat spotdir3[] = { 1.f, -1.f, -1.f };
        glLightfv(GL_LIGHT0, GL_SPOT_DIRECTION, spotdir3);
        glUseProgramObjectARB(0);
        glColor3d(1., 1., 1.);
        glutSolidSphere(0.1, 20, 15);
    glPopMatrix();

    //Send info to shader
    if (theprog)
    {
        GLint loc;  // Location for shader vars

        loc = glGetUniformLocationARB(theprog, "myb1");
        if (loc != -1)
        {
            glUniform1i(loc, false);
        }

        loc = glGetUniformLocationARB(theprog, "myf1");
        if (loc != -1)
        {
            glUniform1f(loc, GLfloat(.0));
        }
    }

    // Draw objects
    glColor3b(0.7, 0.0, 0.7);
    glTranslated(0,0,-4);
    if(wave)
        drawBezierPatch(numsubdivs, modd);
    else
        drawBezierPatch(numsubdivs, 0);

    documentation();
    glutSwapBuffers();
}

// myIdle
// The GLUT idle function
void myIdle()
{
    modd += 0.1;
    glutPostRedisplay();
    // Print OpenGL errors, if there are any (for debugging)
    static int error_count = 0;
    if (GLenum err = glGetError())
    {
        ++error_count;
        cerr << "OpenGL ERROR " << error_count << ": "
             << gluErrorString(err) << endl;
    }
}

// set()
// Sets given matrix to given translation/rotation.
void set(GLdouble *arr, GLdouble tx, GLdouble ty, GLdouble tz,
         GLdouble rang=0., GLdouble rx=0., GLdouble ry=0., GLdouble rz=0.)
{
    glLoadIdentity();
    glTranslated(tx, ty, tz);
    glRotated(rang, rx, ry, rz);
    glMultMatrixd(arr);
    glGetDoublev(GL_MODELVIEW_MATRIX, arr);
}

// reset()
// Resets the given matrix to the identity
void reset(GLdouble *arr)
{
    glLoadIdentity();
    glGetDoublev(GL_MODELVIEW_MATRIX, arr);
}

// myKeyboard
// The GLUT keyboard function
void myKeyboard(unsigned char key, int x, int y)
{
    switch (key)
    {
    case ESCKEY:  // Esc: quit
        exit(0);
        break;
    case '+':
        set(viewmatrix, 0., 0., 1.);
        break;
    case '-':
        set(viewmatrix, 0., 0., -1.);
        break;
// Commenting out the below section until the rotation function
// is rectectified
/*    case '<':
        set(viewmatrix, 0., -1., 0., 10,1,0,0);
        break;
    case '>':
        set(viewmatrix, 0., 1., 0., -10,1,0,0);
        break;
    case ',':
        set(viewmatrix, 0., -1., 0., 10,0,1,0);
        break;
    case '.':
        set(viewmatrix, 0., 1., 0., -10,0,1,0);
        break;
*/
    case 'R':
    case 'r':
        reset(viewmatrix);
        break;
    case ' ':
        wave = !wave;
        break;
    case '(':
        if(numsubdivs > minsubdivs)
            --numsubdivs;
        break;
    case ')':
        if(numsubdivs < maxsubdivs)
            ++numsubdivs;
        break;
    case 'h':
    case 'H':
        help = !help;
    }
    glutPostRedisplay();
}

// mySpecial
// The GLUT special function
void mySpecial(int key, int x, int y)
{
    switch (key)
    {
        case GLUT_KEY_LEFT:
            set(viewmatrix, 0.,0.,0., -viewang, 0.,1.,0.);
            break;
        case GLUT_KEY_RIGHT:
            set(viewmatrix, 0.,0.,0., viewang, 0.,1.,0.);
            break;
        case GLUT_KEY_UP:
            set(viewmatrix, 0.,0.,0., -viewang,0.,.0,1.);
            break;
        case GLUT_KEY_DOWN:
            set(viewmatrix, 0.,0.,0., viewang,0.,.0,1.);
            break;
    }
    glutPostRedisplay();
}

// myReshape
// The GLUT reshape function
void myReshape(int w, int h)
{
    // Set viewport & save window dimensions in globals
    glViewport(0, 0, w, h);
    winw = w;
    winh = h;
    // Set up projection
    // Perspective projection with near quite near & far pretty far
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(60., double(w)/h, 0.01, 100.);
    glMatrixMode(GL_MODELVIEW);  // Always go back to model/view mode
}

// init
// Initialize GL states & global data
void init()
{
    reset(viewmatrix);    // Reset camera position
    glMatrixMode(GL_MODELVIEW);
    modd = 0.0;
    wave = false;
    numsubdivs = 1;
}

// myPassiveMotion
// The GLUT passiveMotion function
void myPassiveMotion(int x, int y)
{
    // Save mouse position in class (haha)
    mousy.saveMouse(x, y);
}

// The main
int main(int argc, char ** argv)
{
    // Initilization of OpenGL/GLUT
    glutInit(&argc, argv);
    // Set shader source filenames. Done here, as opposed to in
    getShaderFilenames(vshader1fname, fshader1fname, argc, argv);
    //  function init, so that we can use command-line arguments.
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);

    // Creating the view window
    glutInitWindowSize(startwinsize, startwinsize);
    glutInitWindowPosition(50, 50);
    glutCreateWindow("CS 381 - Shaders, Normals, Lighting, and Splines Oh My!");

    // Init GLEW & check status - exit on failure
    if (glewInit() != GLEW_OK)
    {
        cerr << "glewInit failed" << endl;
        exit(1);
    }

    init();
    glutDisplayFunc(myDisplay);
    glutIdleFunc(myIdle);
    glutReshapeFunc(myReshape);
//    glutMouseFunc(myMouse);
    glutKeyboardFunc(myKeyboard);
    glutSpecialFunc(mySpecial);
    glutPassiveMotionFunc(myPassiveMotion);

    // Start GLUT event handling loop
    glutMainLoop();

    return 0;
}

void documentation()
{
    // Draw documentation
    glDisable(GL_DEPTH_TEST);
    glLoadIdentity();
    glMatrixMode(GL_PROJECTION);  // Set up simple ortho projection
    glPushMatrix();
        glLoadIdentity();
        gluOrtho2D(0., double(winw), 0., double(winh));
        glColor3d(0., 0., 0.);        // Black text
        BitmapPrinter p(20., winh-20., 20.);
        if(help)
        {
            ostringstream os2;
            os2 << fixed << setprecision(2) << numsubdivs;
            p.print("Arrows         Rotate Scene");
            p.print("( )            Change Subdivisions (" + os2.str() + ")");
            p.print("+/-            Zoom in/out");
            p.print("r              Reset Camera");
            p.print(string("space          Start wave (" )+ (wave ? "true" : "false") + ")");
        }
        else
        {
            p.print("h              help");
        }
        p.print("Esc            Quit");
    glPopMatrix();                // Restore prev projection
    glMatrixMode(GL_MODELVIEW);
}
<|endoftext|>