<commit_before>//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.

#include <vector>

#include "llvm/IR/DataLayout.h"
#include "llvm/IR/BasicBlock.h"
#include "llvm/IR/Constants.h"
#include "llvm/IR/Function.h"
#include "llvm/IR/GlobalVariable.h"
#include "llvm/IR/Instructions.h"
#include "llvm/IR/Module.h"
#include "llvm/Pass.h"
#include "llvm/IR/Type.h"
#include "llvm/IR/TypeBuilder.h"
#if (LLVM_VERSION_MINOR >= 5)
  #include "llvm/IR/InstIterator.h"
#else
  #include "llvm/Support/InstIterator.h"
#endif
#include "llvm/Support/raw_ostream.h"
#include "llvm/Transforms/Utils/BasicBlockUtils.h"

using namespace llvm;

class InitializeUninitialized : public FunctionPass {
  public:
    static char ID;

    InitializeUninitialized() : FunctionPass(ID) {}

    virtual bool runOnFunction(Function &F);
};

static RegisterPass<InitializeUninitialized> INIUNINI("initialize-uninitialized",
                                                      "initialize all uninitialized variables to non-deterministic value");
char InitializeUninitialized::ID;

bool InitializeUninitialized::runOnFunction(Function &F)
{
  // do not run the initializer on __VERIFIER and __INSTR functions
  const auto& fname = F.getName();
  if (fname.startswith("__VERIFIER_") || fname.startswith("__INSTR_"))
    return false;

  bool modified = false;
  Module *M = F.getParent();
  LLVMContext& Ctx = M->getContext();
  DataLayout *DL = new DataLayout(M->getDataLayout());
  Constant *name_init = ConstantDataArray::getString(Ctx, "nondet");
  GlobalVariable *name = new GlobalVariable(*M, name_init->getType(), true, GlobalValue::PrivateLinkage, name_init);
  Type *size_t_Ty;

  if (DL->getPointerSizeInBits() > 32)
    size_t_Ty = Type::getInt64Ty(Ctx);
  else
    size_t_Ty = Type::getInt32Ty(Ctx);

  //void klee_make_symbolic(void *addr, size_t nbytes, const char *name);
  Constant *C = M->getOrInsertFunction("klee_make_symbolic",
                                       Type::getVoidTy(Ctx),
                                       Type::getInt8PtrTy(Ctx), // addr
                                       size_t_Ty,   // nbytes
                                       Type::getInt8PtrTy(Ctx), // name
                                       NULL);

  for (inst_iterator I = inst_begin(F), E = inst_end(F); I != E;) {
    Instruction *ins = &*I;
    ++I;

    if (AllocaInst *AI = dyn_cast<AllocaInst>(ins)) {
      Type *Ty = AI->getAllocatedType();
      AllocaInst *newAlloca = NULL;
      CallInst *CI = NULL;
      CastInst *CastI = NULL;
      StoreInst *SI = NULL;
      LoadInst *LI = NULL;

      std::vector<Value *> args;

      // create new allocainst, declare it symbolic and store it
      // to the original alloca. This way slicer will slice this
      // initialization away if program initialize it manually later
      if (Ty->isSized()) {
        // if this is an array allocation, just call klee_make_symbolic on it,
        // since storing whole symbolic array into it would have soo huge overhead
        if (Ty->isArrayTy()) {
            CastI = CastInst::CreatePointerCast(AI, Type::getInt8PtrTy(Ctx));
            args.push_back(CastI);
            args.push_back(ConstantInt::get(size_t_Ty, DL->getTypeAllocSize(Ty)));
            args.push_back(ConstantExpr::getPointerCast(name, Type::getInt8PtrTy(Ctx)));

            CI = CallInst::Create(C, args);
            CastI->insertAfter(AI);
            CI->insertAfter(CastI);
        } else {
            // when this is not an array allocation, create new symbolic memory and
            // store it into the allocated memory using normal StoreInst.
            // That will allow slice away more unneeded allocations
            newAlloca = new AllocaInst(Ty, "alloca_uninitial");
            CastI = CastInst::CreatePointerCast(newAlloca, Type::getInt8PtrTy(Ctx));

            args.push_back(CastI);
            args.push_back(ConstantInt::get(size_t_Ty, DL->getTypeAllocSize(Ty)));
            args.push_back(ConstantExpr::getPointerCast(name, Type::getInt8PtrTy(Ctx)));
            CI = CallInst::Create(C, args);

            LI = new LoadInst(newAlloca);
            SI = new StoreInst(LI, AI);

            newAlloca->insertAfter(AI);
            CastI->insertAfter(newAlloca);
            CI->insertAfter(CastI);
            LI->insertAfter(CI);
            SI->insertAfter(LI);
        }

        modified = true;
      }
    }
  }

  delete DL;
  return modified;
}

<commit_msg>InitializeUninitialized: initialize less memory<commit_after>//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.

#include <vector>

#include "llvm/IR/DataLayout.h"
#include "llvm/IR/BasicBlock.h"
#include "llvm/IR/Constants.h"
#include "llvm/IR/Function.h"
#include "llvm/IR/GlobalVariable.h"
#include "llvm/IR/Instructions.h"
#include "llvm/IR/Module.h"
#include "llvm/Pass.h"
#include "llvm/IR/Type.h"
#include "llvm/IR/TypeBuilder.h"
#if (LLVM_VERSION_MINOR >= 5)
  #include "llvm/IR/InstIterator.h"
#else
  #include "llvm/Support/InstIterator.h"
#endif
#include "llvm/Support/raw_ostream.h"
#include "llvm/Transforms/Utils/BasicBlockUtils.h"

using namespace llvm;

class InitializeUninitialized : public FunctionPass {
  public:
    static char ID;

    InitializeUninitialized() : FunctionPass(ID) {}

    virtual bool runOnFunction(Function &F);
};

static RegisterPass<InitializeUninitialized> INIUNINI("initialize-uninitialized",
                                                      "initialize all uninitialized variables to non-deterministic value");
char InitializeUninitialized::ID;

// no hard analysis, just check wether the alloca is initialized
// in the same block. (we could do an O(n) analysis that would
// do DFS and if the alloca would be initialized on every path
// before reaching some backedge, then it must be initialized),
// for all allocas the running time would be O(n^2) and it could
// probably be decreased (without pointers)
static bool mayBeUnititialized(const llvm::AllocaInst *AI)
{
    using namespace llvm;

	Type *AITy = AI->getAllocatedType();
	if(!AITy->isSized())
		return true;

    const BasicBlock *block = AI->getParent();
    auto I = block->begin();
    auto E = block->end();
    // shift to AI
    while (I != E && (&*I) != AI)
        ++I;

    if (I == E)
        return true;

    // iterate over instructions after AI in this block
    for (++I /* shift after AI */; I != E; ++I) {
        if (const StoreInst *SI = dyn_cast<StoreInst>(&*I)) {
            // we store into AI and we store the same type
            // (that is, we overwrite the whole memory?)
            if (SI->getPointerOperand() == AI &&
                SI->getValueOperand()->getType() == AITy)
                return false;
        }
    }

    return true;
}

bool InitializeUninitialized::runOnFunction(Function &F)
{
  // do not run the initializer on __VERIFIER and __INSTR functions
  const auto& fname = F.getName();
  if (fname.startswith("__VERIFIER_") || fname.startswith("__INSTR_"))
    return false;

  bool modified = false;
  Module *M = F.getParent();
  LLVMContext& Ctx = M->getContext();
  DataLayout *DL = new DataLayout(M->getDataLayout());
  Constant *name_init = ConstantDataArray::getString(Ctx, "nondet");
  GlobalVariable *name = new GlobalVariable(*M, name_init->getType(), true,
                                            GlobalValue::PrivateLinkage,
                                            name_init);
  Type *size_t_Ty;

  if (DL->getPointerSizeInBits() > 32)
    size_t_Ty = Type::getInt64Ty(Ctx);
  else
    size_t_Ty = Type::getInt32Ty(Ctx);

  //void klee_make_symbolic(void *addr, size_t nbytes, const char *name);
  Constant *C = M->getOrInsertFunction("klee_make_symbolic",
                                       Type::getVoidTy(Ctx),
                                       Type::getInt8PtrTy(Ctx), // addr
                                       size_t_Ty,   // nbytes
                                       Type::getInt8PtrTy(Ctx), // name
                                       NULL);

  for (inst_iterator I = inst_begin(F), E = inst_end(F); I != E;) {
    Instruction *ins = &*I;
    ++I;

    if (AllocaInst *AI = dyn_cast<AllocaInst>(ins)) {
      if (!mayBeUnititialized(AI))
        continue;

      Type *Ty = AI->getAllocatedType();
      AllocaInst *newAlloca = nullptr;
      CallInst *CI = nullptr;
      CastInst *CastI = nullptr;
      StoreInst *SI = nullptr;
      LoadInst *LI = nullptr;

      std::vector<Value *> args;

      // create new allocainst, declare it symbolic and store it
      // to the original alloca. This way slicer will slice this
      // initialization away if program initialize it manually later
      if (Ty->isSized()) {
        // if this is an array allocation, just call klee_make_symbolic on it,
        // since storing whole symbolic array into it would have soo huge overhead
        if (Ty->isArrayTy()) {
            CastI = CastInst::CreatePointerCast(AI, Type::getInt8PtrTy(Ctx));
            args.push_back(CastI);
            args.push_back(ConstantInt::get(size_t_Ty, DL->getTypeAllocSize(Ty)));
            args.push_back(ConstantExpr::getPointerCast(name, Type::getInt8PtrTy(Ctx)));

            CI = CallInst::Create(C, args);
            CastI->insertAfter(AI);
            CI->insertAfter(CastI);
        } else {
            // when this is not an array allocation, create new symbolic memory and
            // store it into the allocated memory using normal StoreInst.
            // That will allow slice away more unneeded allocations
            newAlloca = new AllocaInst(Ty, "alloca_uninitial");
            CastI = CastInst::CreatePointerCast(newAlloca, Type::getInt8PtrTy(Ctx));

            args.push_back(CastI);
            args.push_back(ConstantInt::get(size_t_Ty, DL->getTypeAllocSize(Ty)));
            args.push_back(ConstantExpr::getPointerCast(name, Type::getInt8PtrTy(Ctx)));
            CI = CallInst::Create(C, args);

            LI = new LoadInst(newAlloca);
            SI = new StoreInst(LI, AI);

            newAlloca->insertAfter(AI);
            CastI->insertAfter(newAlloca);
            CI->insertAfter(CastI);
            LI->insertAfter(CI);
            SI->insertAfter(LI);
        }

        modified = true;
      }
    }
  }

  delete DL;
  return modified;
}

<|endoftext|>


<commit_before>//////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2019, Vinitha Ranganeni
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//     1. Redistributions of source code must retain the above copyright notice
//        this list of conditions and the following disclaimer.
//     2. Redistributions in binary form must reproduce the above copyright
//        notice, this list of conditions and the following disclaimer in the
//        documentation and/or other materials provided with the distribution.
//     3. Neither the name of the copyright holder nor the names of its
//        contributors may be used to endorse or promote products derived from
//        this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
////////////////////////////////////////////////////////////////////////////////

#ifndef LIBCOZMO_STATESPACE_STATESPACE_HPP_
#define LIBCOZMO_STATESPACE_STATESPACE_HPP_

#include "aikido/distance/SE2.hpp"

namespace libcozmo {
namespace statespace {

class StateSpace
{
 public:

 	/// Base class for all discrete states
 	class State;

 	/// Checks if given (discrete) state exists in the statespace; if not,
    /// creates and adds the state to the statespace
    ///
    /// \param state Input state
    /// \return state ID
 	virtual int get_or_create_state(const State* _state) = 0;

 	/// Checks if given (continuous) state exists in the statespace; if not,
    /// creates and adds the state to the statespace
    ///
    /// \param state Input state
    /// \return State ID
 	virtual int get_or_create_state(
 		const aikido::statespace::StateSpace::State* _state) = 0;

    /// Converts the given discrete state into a continuous state
    ///
    /// \param _state Input discrete state
    /// \param[out] _state Output continuous state
 	virtual void discrete_state_to_continuous(
        const State* _state,
        aikido::statespace::StateSpace::State* _continuous_state) const = 0;

    /// Converts the given continuous state into a discrete state
    ///
    /// \param _state Input discrete state
    /// \param[out] _state Output continuous state
 	virtual void continuous_state_to_discrete(
        const aikido::statespace::StateSpace::State* _state,
        State* _discrete_state) const = 0;

 	/// Gets the state ID for the given state if the state exists in the
    /// statespace
    ///
    /// \param _state Input discrete state
    /// \param[out] state_id State ID
    /// \return True if the state ID was found and false otherwise
 	virtual bool get_state_id(const State* _state, int* _state_id) const = 0;

 	/// Gets the state for the give ID if the state exists in the statespace
    ///
    /// \param state_id The ID of the state
    /// \return Pointer to the discrete state
 	virtual State* get_state(const int& _state_id) const = 0;

 	/// Checks if the given state is a valid state; validity of state varies
 	/// based on state type
    ///
    /// \param _state Input discrete state
    /// \return True if the state if valid and false otherwise
 	virtual bool is_valid_state(const State* _state) const = 0;

    /// Get the number of states in the statespace
    ///
    /// \return Number of states
    virtual int size() const = 0;

    /// Gets the distance between two SE2 states
    ///
    /// \param state_1, state_2 The discrete states to calculate the SE2
    /// distance between
    /// \return Distance between the states
    virtual double get_distance(
        const State* _state_1, const State* _state_2) const = 0;

    /// Gets the distance between two SE2 states
    ///
    /// \param state_1, state_2 The continuous states to calculate the SE2
    /// distance between
    /// \return Distance between the states
    virtual double get_distance(
        const aikido::statespace::StateSpace::State* _state_1,
        const aikido::statespace::StateSpace::State* _state_2) const = 0;

    /// Copies a discrete state
    ///
    /// \param _source Input state (state to copy)
    /// \param[out] _destination Output state
    virtual void copy_state(const State* _source, State* _destination) const = 0;

 private:
 	virtual State* create_state() = 0;
};

class StateSpace::State
{
 protected:
  	// This is a base class that should only only be used in derived classes.
  	State() = default;

  	~State() = default;
};

}  // namespace statespace
}  // namespace libcozmo

#endif<commit_msg>converted indentation to spaces<commit_after>//////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2019, Vinitha Ranganeni
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//     1. Redistributions of source code must retain the above copyright notice
//        this list of conditions and the following disclaimer.
//     2. Redistributions in binary form must reproduce the above copyright
//        notice, this list of conditions and the following disclaimer in the
//        documentation and/or other materials provided with the distribution.
//     3. Neither the name of the copyright holder nor the names of its
//        contributors may be used to endorse or promote products derived from
//        this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
////////////////////////////////////////////////////////////////////////////////

#ifndef LIBCOZMO_STATESPACE_STATESPACE_HPP_
#define LIBCOZMO_STATESPACE_STATESPACE_HPP_

#include "aikido/distance/SE2.hpp"

namespace libcozmo {
namespace statespace {

class StateSpace
{
 public:

    /// Base class for all discrete states
    class State;

    /// Checks if given (discrete) state exists in the statespace; if not,
    /// creates and adds the state to the statespace
    ///
    /// \param state Input state
    /// \return state ID
    virtual int get_or_create_state(const State* _state) = 0;

    /// Checks if given (continuous) state exists in the statespace; if not,
    /// creates and adds the state to the statespace
    ///
    /// \param state Input state
    /// \return State ID
    virtual int get_or_create_state(
        const aikido::statespace::StateSpace::State* _state) = 0;

    /// Converts the given discrete state into a continuous state
    ///
    /// \param _state Input discrete state
    /// \param[out] _state Output continuous state
    virtual void discrete_state_to_continuous(
        const State* _state,
        aikido::statespace::StateSpace::State* _continuous_state) const = 0;

    /// Converts the given continuous state into a discrete state
    ///
    /// \param _state Input discrete state
    /// \param[out] _state Output continuous state
    virtual void continuous_state_to_discrete(
        const aikido::statespace::StateSpace::State* _state,
        State* _discrete_state) const = 0;

    /// Gets the state ID for the given state if the state exists in the
    /// statespace
    ///
    /// \param _state Input discrete state
    /// \param[out] state_id State ID
    /// \return True if the state ID was found and false otherwise
    virtual bool get_state_id(const State* _state, int* _state_id) const = 0;

    /// Gets the state for the give ID if the state exists in the statespace
    ///
    /// \param state_id The ID of the state
    /// \return Pointer to the discrete state
    virtual State* get_state(const int& _state_id) const = 0;

    /// Checks if the given state is a valid state; validity of state varies
    /// based on state type
    ///
    /// \param _state Input discrete state
    /// \return True if the state if valid and false otherwise
    virtual bool is_valid_state(const State* _state) const = 0;

    /// Get the number of states in the statespace
    ///
    /// \return Number of states
    virtual int size() const = 0;

    /// Gets the distance between two SE2 states
    ///
    /// \param state_1, state_2 The discrete states to calculate the SE2
    /// distance between
    /// \return Distance between the states
    virtual double get_distance(
        const State* _state_1, const State* _state_2) const = 0;

    /// Gets the distance between two SE2 states
    ///
    /// \param state_1, state_2 The continuous states to calculate the SE2
    /// distance between
    /// \return Distance between the states
    virtual double get_distance(
        const aikido::statespace::StateSpace::State* _state_1,
        const aikido::statespace::StateSpace::State* _state_2) const = 0;

    /// Copies a discrete state
    ///
    /// \param _source Input state (state to copy)
    /// \param[out] _destination Output state
    virtual void copy_state(const State* _source, State* _destination) const = 0;

 private:
    virtual State* create_state() = 0;
};

class StateSpace::State
{
 protected:
    // This is a base class that should only only be used in derived classes.
    State() = default;

    ~State() = default;
};

}  // namespace statespace
}  // namespace libcozmo

#endif<|endoftext|>


<commit_before>//
// Created by Adrian Palmquist on 15-04-28.
//

#pragma once

#include <BulletCollision/NarrowPhaseCollision/btRaycastCallback.h>
#include <events/DebugdrawerEvent.hpp>
#include <events/GravityChangeEvent.hpp>
#include "entityx/entityx.h"
#include "btBulletDynamicsCommon.h"
#include "components/PhysicsComponent.hpp"
#include "physics/MyDebugDrawer.hpp"
#include "HelperFunctions.hpp"

#include "events/ProjectionViewEvent.hpp"
#include "events/PickUpObjectEvent.hpp"

namespace ex = entityx;

namespace sw {

    class PhysicsSystem : public ex::System<PhysicsSystem>, public ex::Receiver<PhysicsSystem> {
    public:

        PhysicsSystem() {
            // create the collision configuration
            m_pCollisionConfiguration = new btDefaultCollisionConfiguration();

            // Default collision dispatcher
            m_pDispatcher = new btCollisionDispatcher(m_pCollisionConfiguration);

            // create the broadphase
            m_pBroadphase = new btDbvtBroadphase();

            // create the constraint solver
            m_pSolver = new btSequentialImpulseConstraintSolver();

            // create the world
            m_pWorld = new btDiscreteDynamicsWorld(m_pDispatcher, m_pBroadphase, m_pSolver, m_pCollisionConfiguration);

            m_pWorld->setGravity(btVector3(0, -10, 0));

            debugDrawer_ = new MyDebugDrawer();
            m_pWorld->setDebugDrawer(debugDrawer_);

            m_pWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_MAX_DEBUG_DRAW_MODE);

            will_update_current_bodies_ = false;
            dim_from_ = Dim::DIMENSION_ONE;
            last_dim_change_num = -1;
        }

        void configure(ex::EventManager &events) override {
            events.subscribe<DebugdrawerEvent>(*this);
            events.subscribe<DimensionChangeInProgressEvent>(*this);
            events.subscribe<PickUpObjectEvent>(*this);
            events.subscribe<ProjectionViewEvent>(*this);
            events.subscribe<GravityChangeEvent>(*this);

        }

        void populateWorld(ex::EntityManager &es) {
            auto physics = ex::ComponentHandle<PhysicsComponent>();
            auto dimension = ex::ComponentHandle<DimensionComponent>();

            for (ex::Entity e : es.entities_with_components(physics, dimension)) {
                Dim d = dimension->dimension_;

                std::cout << "populateworld" << std::endl;
                /*
                std::cout << "Group: " << physics->group_ << std::endl;
                std::cout << "Mask: " << physics->mask_ << std::endl;
                std::cout << "Previous flags: " << physics->body_->getCollisionFlags() << std::endl;
                 */
                std::cout << "  dimension: " << d << "\n";
                physics->body_->activate(true);

                m_pWorld->addRigidBody(physics->body_, d, d);
            }

            auto player = ex::ComponentHandle<PlayerComponent>();
            for (ex::Entity e : es.entities_with_components(player, physics)) {

                btTransform temp;
                physics->body_->getMotionState()->getWorldTransform(temp);
                btVector3 playerMin = temp.getOrigin();
                btVector3 btTo(playerMin.x(), -500.0f, playerMin.z());

                btCollisionWorld::ClosestRayResultCallback res(playerMin, btTo);

                m_pWorld->rayTest(playerMin, btTo, res);

                if (res.hasHit()) {

                    btTo = res.m_hitPointWorld;
                    btScalar p = playerMin.distance(btTo);

                    player->distance_to_bottom_ = physics->height_;

                    std::cout << "Distance to bottom: " << p << std::endl;

                }

            }
        }

        ~PhysicsSystem() {
            delete m_pWorld;
            delete debugDrawer_;
            delete m_pBroadphase;
            delete m_pDispatcher;
            delete m_pCollisionConfiguration;
            delete m_pSolver;
        }

        void update(entityx::EntityManager &entityManager, entityx::EventManager &eventManager, entityx::TimeDelta dt) {
            if (will_update_current_bodies_) {
                /*std::cout << "Will update bitches\n";
                auto dimension = ex::ComponentHandle<DimensionComponent>();
                auto physics = ex::ComponentHandle<PhysicsComponent>();
                for (ex::Entity e : entityManager.entities_with_components(dimension, physics)) {
                    Dim d = dimension->dimension_;
                    if (d == Dim::DIMENSION_BOTH)
                        continue;
                    if (d == dim_from_) {
                        //m_pWorld->removeRigidBody(physics->body_);
                        physics->body_->setActivationState(ISLAND_SLEEPING);
                        //physics->body_->setActivationState(DISABLE_SIMULATION);
                        std::cout << "Disabled a bitch\n";
                    }
                    else {
                        //m_pWorld->addRigidBody(physics->body_);
                        //physics->body_->setActivationState(DISABLE_DEACTIVATION);
                        physics->body_->activate(true);
                        std::cout << "Enabled a bitch\n";
                    }
                }*/
                auto physics = ex::ComponentHandle<PhysicsComponent>();
                auto dimension = ex::ComponentHandle<DimensionComponent>();
                auto player = ex::ComponentHandle<PlayerComponent>();

                for (ex::Entity p : entityManager.entities_with_components(player, physics, dimension)) {
                    Dim d = dimension->dimension_;
                    m_pWorld->removeRigidBody(physics->body_);
                    m_pWorld->addRigidBody(physics->body_, d, d);
                    std::cout << "Current player dimension is: " << d << "\n";
                }

                will_update_current_bodies_ = false;
            }

            m_pWorld->stepSimulation(dt);

            if (m_debugMode) {
                m_pWorld->debugDrawWorld();
                debugDrawer_->drawLines();

            }

            auto player = ex::ComponentHandle<PlayerComponent>();
            auto physics = ex::ComponentHandle<PhysicsComponent>();

            for (ex::Entity e : entityManager.entities_with_components(player, physics)) {

                if (!proj_view_events_.empty()) {
                    view_ = proj_view_events_.front().view_matrix_;
                    camera_projection_ = proj_view_events_.front().projection_matrix_;

                    proj_view_events_.clear();
                }

                if (!pickup_events_.empty()) {
                    processPickupEvents(physics);
                }

                btTransform temp;
                btVector3 tempVec;
                btVector3 playerMin;

                physics->body_->getMotionState()->getWorldTransform(temp);

                playerMin = temp.getOrigin();

                playerMin.setY(playerMin.getY() - (physics->height_));

                //std::cout << "playerMin: " << playerMin.getY() << std::endl;

                btVector3 btTo(playerMin.x(), -500.0f, playerMin.z());

                btCollisionWorld::ClosestRayResultCallback res(playerMin, btTo);

                //btCollisionWorld::ClosestConvexResultCallback

                m_pWorld->rayTest(playerMin, btTo, res);

                res.m_flags |= btTriangleRaycastCallback::kF_FilterBackfaces;

                res.m_flags |= btTriangleRaycastCallback::kF_KeepUnflippedNormal;

                if (res.hasHit()) {

                    btTo = res.m_hitPointWorld;
                    btVector3 normal = res.m_hitNormalWorld;

                    btScalar p = playerMin.distance(btTo);

                    //sw::printMatVec("Collision, p: ", p);

                    //sw::printMatVec("Playermin: ", playerMin);

                    //std::cout << "Distance to collision point: " << p << std::endl;

                    //std::cout << "p: " << p << " ,distance_to_bottom_: " << player->distance_to_bottom_ << std::endl;

                    if (p > 0.05 && std::abs(physics->body_->getLinearVelocity().getY()) > 0.0) {
                        player->state_ = STATE_AIRBOURNE;
                    } else {
                        player->state_ = STATE_STANDING;
                    }
                }
            }

            //std::cout << "Player speed: " << physics->body_->getLinearVelocity().getY() << std::endl;
        }

        // Process pickup events generated this frame
        void processPickupEvents(ex::ComponentHandle<PhysicsComponent> physics) {

            pickup_events_.clear();

            glm::mat4 inverted_view = glm::inverse(view_);

            glm::vec4 lookAtInViewSpace(0, 0, -1,
                                        1); // Forward vector (use 0, 0, -1 if you have a right-handed coordinate system)
            glm::vec4 lookAtInWorldSpaceTemp = lookAtInViewSpace * inverted_view; // Transform into world space.

            glm::vec3 lookAtInWorldSpace = glm::vec3(lookAtInWorldSpaceTemp);

            lookAtInWorldSpace = lookAtInWorldSpace * 1000.0f;

            // The player's position
            btTransform temp;
            physics->body_->getMotionState()->getWorldTransform(temp);
            btVector3 bt_out_origin = temp.getOrigin();

            // The ray's direction
            btVector3 bt_out_direction = sw::vec3_to_btVector3(lookAtInWorldSpace);

            printMatVec("View: ", view_);
            printMatVec("Origin: ", bt_out_origin);
            printMatVec("Direction", bt_out_direction);

            btCollisionWorld::ClosestRayResultCallback RayCallback(bt_out_origin, bt_out_direction);
            RayCallback.m_flags |= btTriangleRaycastCallback::kF_FilterBackfaces;
            RayCallback.m_flags |= btTriangleRaycastCallback::kF_KeepUnflippedNormal;

            m_pWorld->rayTest(bt_out_origin, bt_out_direction, RayCallback);

            std::string message;

            // If the ray hit something, set the object's linear velocity and print its position
            if (RayCallback.hasHit()) {
                if(!is_lifting) {
                    std::ostringstream oss;
                    oss << "hit";
                    btRigidBody *temp_body = (btRigidBody *) RayCallback.m_collisionObject->getUserPointer();
                    //temp_body->setLinearVelocity(btVector3(0.0f, 2.0f, 0.0f));
                    btTransform transA, transB;
                    bool useLinearReferenceFrameA = true;
                    transA.setIdentity();
                    transB.setIdentity();
                    bt_out_direction.setY(0.5f);
                    transB.setOrigin(bt_out_direction / 1000.f);
                    cons = new btGeneric6DofConstraint(*temp_body, *physics->body_, transA,
                                                                                transB, useLinearReferenceFrameA);
                    m_pWorld->addConstraint(cons);
                    temp_body->setAngularFactor(0.f);
                    temp_body->setActivationState(DISABLE_DEACTIVATION);
                    is_lifting = true;
                }
                else{
                    cons->getRigidBodyA().setAngularFactor(1.f);
                    cons->getRigidBodyA().setActivationState(ACTIVE_TAG);
                    cons->getRigidBodyA().removeConstraintRef(cons);
                    cons->getRigidBodyB().removeConstraintRef(cons);
                    m_pWorld->removeConstraint(cons);
                    is_lifting = false;
                }
               // printMatVec(temp_body->getWorldTransform().getOrigin());
            } else {
                message = "background";
            }

            std::cout << message << std::endl;

        }

        void receive(const DimensionChangeInProgressEvent &dimChange) {
            if (last_dim_change_num == dimChange.NUM_)
                return;

            if (dimChange.completion_factor_ >= 0.5f) {
                dim_from_ = dimChange.dim_from_;
                last_dim_change_num = dimChange.NUM_;

                // The dimension change has occurred
                will_update_current_bodies_ = true;
            }
        }

        void receive(const DebugdrawerEvent &debugdraw) {
            m_debugMode = debugdraw.debugMode_;
        }

        void receive(const ProjectionViewEvent &proj_view_event) {
            proj_view_events_.push_back(proj_view_event);
        }

        void receive(const PickUpObjectEvent &pickUpObjectEvent) {
            pickup_events_.push_back(pickUpObjectEvent);
        }

        void receive(const GravityChangeEvent &gravityChangeEvent){
            if (gravityChangeEvent.gravityChange_){
                for (int n=0;n<m_pWorld->getCollisionObjectArray().size();n++) {
                    m_pWorld->getCollisionObjectArray().at(n)->setActivationState(DISABLE_DEACTIVATION);
                }
                    m_pWorld->setGravity(btVector3(0.f,0.1f,0.f));
            }
            else{
                for (int n=0;n<m_pWorld->getCollisionObjectArray().size();n++) {
                    m_pWorld->getCollisionObjectArray().at(n)->setActivationState(ACTIVE_TAG);
                }
                m_pWorld->setGravity(btVector3(0.f,-10.,0.f));
            }
        }

    private:
        btBroadphaseInterface *m_pBroadphase;

        btCollisionDispatcher *m_pDispatcher;
        btSequentialImpulseConstraintSolver *m_pSolver;
        btDefaultCollisionConfiguration *m_pCollisionConfiguration;
        btDynamicsWorld *m_pWorld;

        MyDebugDrawer *debugDrawer_;

        std::vector<PickUpObjectEvent> pickup_events_;
        std::vector<ProjectionViewEvent> proj_view_events_;

        glm::mat4 camera_projection_;
        glm::mat4 view_;

        bool m_debugMode = false;

        bool is_lifting = false;
        btGeneric6DofConstraint *cons;

        /* DIMENSION CHANGE */
        Dim dim_from_;
        bool will_update_current_bodies_;
        int last_dim_change_num;

    };

}<commit_msg>fix: can't pick up static objects or yourself<commit_after>//
// Created by Adrian Palmquist on 15-04-28.
//

#pragma once

#include <BulletCollision/NarrowPhaseCollision/btRaycastCallback.h>
#include <events/DebugdrawerEvent.hpp>
#include <events/GravityChangeEvent.hpp>
#include "entityx/entityx.h"
#include "btBulletDynamicsCommon.h"
#include "components/PhysicsComponent.hpp"
#include "physics/MyDebugDrawer.hpp"
#include "HelperFunctions.hpp"

#include "events/ProjectionViewEvent.hpp"
#include "events/PickUpObjectEvent.hpp"

namespace ex = entityx;

namespace sw {

    class PhysicsSystem : public ex::System<PhysicsSystem>, public ex::Receiver<PhysicsSystem> {
    public:

        PhysicsSystem() {
            // create the collision configuration
            m_pCollisionConfiguration = new btDefaultCollisionConfiguration();

            // Default collision dispatcher
            m_pDispatcher = new btCollisionDispatcher(m_pCollisionConfiguration);

            // create the broadphase
            m_pBroadphase = new btDbvtBroadphase();

            // create the constraint solver
            m_pSolver = new btSequentialImpulseConstraintSolver();

            // create the world
            m_pWorld = new btDiscreteDynamicsWorld(m_pDispatcher, m_pBroadphase, m_pSolver, m_pCollisionConfiguration);

            m_pWorld->setGravity(btVector3(0, -10, 0));

            debugDrawer_ = new MyDebugDrawer();
            m_pWorld->setDebugDrawer(debugDrawer_);

            m_pWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_MAX_DEBUG_DRAW_MODE);

            will_update_current_bodies_ = false;
            dim_from_ = Dim::DIMENSION_ONE;
            last_dim_change_num = -1;
        }

        void configure(ex::EventManager &events) override {
            events.subscribe<DebugdrawerEvent>(*this);
            events.subscribe<DimensionChangeInProgressEvent>(*this);
            events.subscribe<PickUpObjectEvent>(*this);
            events.subscribe<ProjectionViewEvent>(*this);
            events.subscribe<GravityChangeEvent>(*this);

        }

        void populateWorld(ex::EntityManager &es) {
            auto physics = ex::ComponentHandle<PhysicsComponent>();
            auto dimension = ex::ComponentHandle<DimensionComponent>();

            for (ex::Entity e : es.entities_with_components(physics, dimension)) {
                Dim d = dimension->dimension_;

                std::cout << "populateworld" << std::endl;
                /*
                std::cout << "Group: " << physics->group_ << std::endl;
                std::cout << "Mask: " << physics->mask_ << std::endl;
                std::cout << "Previous flags: " << physics->body_->getCollisionFlags() << std::endl;
                 */
                std::cout << "  dimension: " << d << "\n";
                physics->body_->activate(true);

                m_pWorld->addRigidBody(physics->body_, d, d);
            }

            auto player = ex::ComponentHandle<PlayerComponent>();
            for (ex::Entity e : es.entities_with_components(player, physics)) {

                btTransform temp;
                physics->body_->getMotionState()->getWorldTransform(temp);
                btVector3 playerMin = temp.getOrigin();
                btVector3 btTo(playerMin.x(), -500.0f, playerMin.z());

                btCollisionWorld::ClosestRayResultCallback res(playerMin, btTo);

                m_pWorld->rayTest(playerMin, btTo, res);

                if (res.hasHit()) {

                    btTo = res.m_hitPointWorld;
                    btScalar p = playerMin.distance(btTo);

                    player->distance_to_bottom_ = physics->height_;

                    std::cout << "Distance to bottom: " << p << std::endl;

                }

            }
        }

        ~PhysicsSystem() {
            delete m_pWorld;
            delete debugDrawer_;
            delete m_pBroadphase;
            delete m_pDispatcher;
            delete m_pCollisionConfiguration;
            delete m_pSolver;
        }

        void update(entityx::EntityManager &entityManager, entityx::EventManager &eventManager, entityx::TimeDelta dt) {
            if (will_update_current_bodies_) {
                /*std::cout << "Will update bitches\n";
                auto dimension = ex::ComponentHandle<DimensionComponent>();
                auto physics = ex::ComponentHandle<PhysicsComponent>();
                for (ex::Entity e : entityManager.entities_with_components(dimension, physics)) {
                    Dim d = dimension->dimension_;
                    if (d == Dim::DIMENSION_BOTH)
                        continue;
                    if (d == dim_from_) {
                        //m_pWorld->removeRigidBody(physics->body_);
                        physics->body_->setActivationState(ISLAND_SLEEPING);
                        //physics->body_->setActivationState(DISABLE_SIMULATION);
                        std::cout << "Disabled a bitch\n";
                    }
                    else {
                        //m_pWorld->addRigidBody(physics->body_);
                        //physics->body_->setActivationState(DISABLE_DEACTIVATION);
                        physics->body_->activate(true);
                        std::cout << "Enabled a bitch\n";
                    }
                }*/
                auto physics = ex::ComponentHandle<PhysicsComponent>();
                auto dimension = ex::ComponentHandle<DimensionComponent>();
                auto player = ex::ComponentHandle<PlayerComponent>();

                for (ex::Entity p : entityManager.entities_with_components(player, physics, dimension)) {
                    Dim d = dimension->dimension_;
                    m_pWorld->removeRigidBody(physics->body_);
                    m_pWorld->addRigidBody(physics->body_, d, d);
                    std::cout << "Current player dimension is: " << d << "\n";
                }

                will_update_current_bodies_ = false;
            }

            m_pWorld->stepSimulation(dt);

            if (m_debugMode) {
                m_pWorld->debugDrawWorld();
                debugDrawer_->drawLines();

            }

            auto player = ex::ComponentHandle<PlayerComponent>();
            auto physics = ex::ComponentHandle<PhysicsComponent>();

            for (ex::Entity e : entityManager.entities_with_components(player, physics)) {

                if (!proj_view_events_.empty()) {
                    view_ = proj_view_events_.front().view_matrix_;
                    camera_projection_ = proj_view_events_.front().projection_matrix_;

                    proj_view_events_.clear();
                }

                if (!pickup_events_.empty()) {
                    processPickupEvents(physics);
                }

                btTransform temp;
                btVector3 tempVec;
                btVector3 playerMin;

                physics->body_->getMotionState()->getWorldTransform(temp);

                playerMin = temp.getOrigin();

                playerMin.setY(playerMin.getY() - (physics->height_));

                //std::cout << "playerMin: " << playerMin.getY() << std::endl;

                btVector3 btTo(playerMin.x(), -500.0f, playerMin.z());

                btCollisionWorld::ClosestRayResultCallback res(playerMin, btTo);

                //btCollisionWorld::ClosestConvexResultCallback

                m_pWorld->rayTest(playerMin, btTo, res);

                res.m_flags |= btTriangleRaycastCallback::kF_FilterBackfaces;

                res.m_flags |= btTriangleRaycastCallback::kF_KeepUnflippedNormal;

                if (res.hasHit()) {

                    btTo = res.m_hitPointWorld;
                    btVector3 normal = res.m_hitNormalWorld;

                    btScalar p = playerMin.distance(btTo);

                    //sw::printMatVec("Collision, p: ", p);

                    //sw::printMatVec("Playermin: ", playerMin);

                    //std::cout << "Distance to collision point: " << p << std::endl;

                    //std::cout << "p: " << p << " ,distance_to_bottom_: " << player->distance_to_bottom_ << std::endl;

                    if (p > 0.05 && std::abs(physics->body_->getLinearVelocity().getY()) > 0.0) {
                        player->state_ = STATE_AIRBOURNE;
                    } else {
                        player->state_ = STATE_STANDING;
                    }
                }
            }

            //std::cout << "Player speed: " << physics->body_->getLinearVelocity().getY() << std::endl;
        }

        // Process pickup events generated this frame
        void processPickupEvents(ex::ComponentHandle<PhysicsComponent> physics) {

            pickup_events_.clear();

            glm::mat4 inverted_view = glm::inverse(view_);

            glm::vec4 lookAtInViewSpace(0, 0, -1,
                                        1); // Forward vector (use 0, 0, -1 if you have a right-handed coordinate system)
            glm::vec4 lookAtInWorldSpaceTemp = lookAtInViewSpace * inverted_view; // Transform into world space.

            glm::vec3 lookAtInWorldSpace = glm::vec3(lookAtInWorldSpaceTemp);

            lookAtInWorldSpace = lookAtInWorldSpace * 1000.0f;

            // The player's position
            btTransform temp;
            physics->body_->getMotionState()->getWorldTransform(temp);
            btVector3 bt_out_origin = temp.getOrigin();

            // The ray's direction
            btVector3 bt_out_direction = sw::vec3_to_btVector3(lookAtInWorldSpace);

            printMatVec("View: ", view_);
            printMatVec("Origin: ", bt_out_origin);
            printMatVec("Direction", bt_out_direction);

            btCollisionWorld::ClosestRayResultCallback RayCallback(bt_out_origin, bt_out_direction);
            RayCallback.m_flags |= btTriangleRaycastCallback::kF_FilterBackfaces;
            RayCallback.m_flags |= btTriangleRaycastCallback::kF_KeepUnflippedNormal;

            m_pWorld->rayTest(bt_out_origin, bt_out_direction, RayCallback);

            std::string message;

            // If the ray hit something, set the object's linear velocity and print its position
            if (RayCallback.hasHit()) {
                btRigidBody *temp_body = (btRigidBody *) RayCallback.m_collisionObject->getUserPointer();
                int colFlag = temp_body->getCollisionFlags();
                int angFac = temp_body->getAngularFactor().getX();
                if(!is_lifting && colFlag != 1 && angFac != 0.f) {
                    std::ostringstream oss;
                    oss << "hit";
                    btTransform transA, transB;
                    bool useLinearReferenceFrameA = true;
                    transA.setIdentity();
                    transB.setIdentity();
                    bt_out_direction.setY(0.5f);
                    transB.setOrigin(bt_out_direction / 1000.f);
                    cons = new btGeneric6DofConstraint(*temp_body, *physics->body_, transA,
                                                                                transB, useLinearReferenceFrameA);
                    m_pWorld->addConstraint(cons);
                    temp_body->setAngularFactor(0.f);
                    temp_body->setActivationState(DISABLE_DEACTIVATION);
                    is_lifting = true;
                    return;
                }
               // printMatVec(temp_body->getWorldTransform().getOrigin());
            } else {
                message = "background";
            }
            if (is_lifting) {
                cons->getRigidBodyA().setAngularFactor(1.f);
                cons->getRigidBodyA().setActivationState(ACTIVE_TAG);
                cons->getRigidBodyA().removeConstraintRef(cons);
                cons->getRigidBodyB().removeConstraintRef(cons);
                m_pWorld->removeConstraint(cons);
                delete cons;
                is_lifting = false;
            }

            std::cout << message << std::endl;

        }

        void receive(const DimensionChangeInProgressEvent &dimChange) {
            if (last_dim_change_num == dimChange.NUM_)
                return;

            if (dimChange.completion_factor_ >= 0.5f) {
                dim_from_ = dimChange.dim_from_;
                last_dim_change_num = dimChange.NUM_;

                // The dimension change has occurred
                will_update_current_bodies_ = true;
            }
        }

        void receive(const DebugdrawerEvent &debugdraw) {
            m_debugMode = debugdraw.debugMode_;
        }

        void receive(const ProjectionViewEvent &proj_view_event) {
            proj_view_events_.push_back(proj_view_event);
        }

        void receive(const PickUpObjectEvent &pickUpObjectEvent) {
            pickup_events_.push_back(pickUpObjectEvent);
        }

        void receive(const GravityChangeEvent &gravityChangeEvent){
            if (gravityChangeEvent.gravityChange_){
                for (int n=0;n<m_pWorld->getCollisionObjectArray().size();n++) {
                    m_pWorld->getCollisionObjectArray().at(n)->setActivationState(DISABLE_DEACTIVATION);
                }
                    m_pWorld->setGravity(btVector3(0.f,0.1f,0.f));
            }
            else{
                for (int n=0;n<m_pWorld->getCollisionObjectArray().size();n++) {
                    m_pWorld->getCollisionObjectArray().at(n)->setActivationState(ACTIVE_TAG);
                }
                m_pWorld->setGravity(btVector3(0.f,-10.,0.f));
            }
        }

    private:
        btBroadphaseInterface *m_pBroadphase;

        btCollisionDispatcher *m_pDispatcher;
        btSequentialImpulseConstraintSolver *m_pSolver;
        btDefaultCollisionConfiguration *m_pCollisionConfiguration;
        btDynamicsWorld *m_pWorld;

        MyDebugDrawer *debugDrawer_;

        std::vector<PickUpObjectEvent> pickup_events_;
        std::vector<ProjectionViewEvent> proj_view_events_;

        glm::mat4 camera_projection_;
        glm::mat4 view_;

        bool m_debugMode = false;

        bool is_lifting = false;
        btGeneric6DofConstraint *cons;

        /* DIMENSION CHANGE */
        Dim dim_from_;
        bool will_update_current_bodies_;
        int last_dim_change_num;

    };

}<|endoftext|>


<commit_before>#ifndef TWISTEDCPP_PROTOCOL_CORE
#define TWISTEDCPP_PROTOCOL_CORE

#include "detail/sockets.hpp"

#include <boost/asio/ip/tcp.hpp>
#include <boost/asio/steady_timer.hpp>
#include <boost/optional.hpp>
#include <boost/asio/spawn.hpp>
#include <boost/asio/write.hpp>
#include <boost/ref.hpp>

#ifndef NDEBUG
#include <iostream>
#endif

namespace twisted {

template <typename ChildProtocol, typename BufferType>
class protocol_core : public std::enable_shared_from_this<ChildProtocol> {
public:
    typedef detail::socket_base socket_type;
    typedef boost::asio::steady_timer timer_type;
    typedef boost::asio::io_service::strand strand_type;
    typedef BufferType buffer_type;
    typedef typename buffer_type::iterator buffer_iterator;
    typedef typename buffer_type::const_iterator const_buffer_iterator;

    protocol_core() = default;
    protocol_core(protocol_core&&) = default;
    protocol_core& operator=(protocol_core&&) = default;

    void set_socket(std::unique_ptr<socket_type> socket) {
        _socket = std::move(socket);
        _strand = boost::in_place(boost::ref(_socket->get_io_service()));
    }

    void run_protocol() {
        auto self = this_protocol().shared_from_this();
        boost::asio::spawn(*_strand,
                           [this, self](boost::asio::yield_context yield) {
            _yield = boost::in_place(yield);

            try {
                _socket->do_handshake(*_yield);

                while (_socket->is_open()) {
                    auto bytes_read =
                        _socket->async_read_some(asio_buffer(), yield);
                    this_protocol().on_message(
                        buffer_begin(), std::next(buffer_begin(), bytes_read));
                }
            }
            catch (boost::system::system_error& connection_error) {
                handle_network_error(connection_error);
            }
            catch (...) {
                handle_user_error(std::current_exception());
            }
        });
    }

    template <typename Iter>
    void send_message(Iter begin, Iter end) {
        _socket->async_write(
            boost::asio::buffer(&*begin, std::distance(begin, end)), *_yield);
    }

    void send_buffers(const std::array<boost::asio::const_buffer, 2>& buffers) {
        _socket->async_write(buffers, *_yield);
    }

    /*
     * @brief default on_disconnect implementation; does nothing
     */
    void on_disconnect() {}

    /*
     * @brief default on_error implementation; swallows everything
     */
    void on_error(std::exception_ptr eptr) {
        try {
            std::rethrow_exception(eptr);
        }
        catch (const std::exception& excep) {
            print_exception_what(excep);
        }
    }

    void lose_connection() { _socket->close(); }

    /*
     * @brief CRTP wrapper for derived class access
     */
    ChildProtocol& this_protocol() {
        return *static_cast<ChildProtocol*>(this);
    }

    /*
     * @brief CRTP wrapper for derived class access
     */
    const ChildProtocol& this_protocol() const {
        return *static_cast<ChildProtocol*>(this);
    }

private:
#ifdef NDEBUG
    void handle_network_error(boost::system::system_error&) {
#else
    void handle_network_error(boost::system::system_error& connection_error) {
        print_connection_error(connection_error);
#endif
        this_protocol().on_disconnect();
    }

    void handle_user_error(const std::exception_ptr& excep) {
        this_protocol().on_error(excep);
    }

    void print_connection_error(
        const boost::system::system_error& connection_error) const {
        std::cerr << "Client disconnected with code " << connection_error.what()
                  << std::endl;
    }

    void print_exception_what(const std::exception& excep) {
        std::cerr << "Killing connection, exception in client handler: "
                  << excep.what() << std::endl;
    }

    buffer_iterator buffer_begin() {
        return this_protocol().read_buffer().begin();
    }

    buffer_iterator buffer_begin() const {
        return this_protocol().read_buffer().begin();
    }

    buffer_iterator buffer_end() { return this_protocol().read_buffer().end(); }

    buffer_iterator buffer_end() const {
        return this_protocol().read_buffer().end();
    }

    boost::asio::mutable_buffers_1 asio_buffer() {
        return boost::asio::buffer(&*buffer_begin(),
                                   std::distance(buffer_begin(), buffer_end()));
    }

private:
    boost::optional<boost::asio::yield_context> _yield;
    std::unique_ptr<socket_type> _socket;
    boost::optional<strand_type> _strand;
};

} // namespace twisted

#endif
<commit_msg>added wait_for function to protocol_core<commit_after>#ifndef TWISTEDCPP_PROTOCOL_CORE
#define TWISTEDCPP_PROTOCOL_CORE

#include "detail/sockets.hpp"

#include <boost/asio/ip/tcp.hpp>
#include <boost/asio/steady_timer.hpp>
#include <boost/optional.hpp>
#include <boost/asio/spawn.hpp>
#include <boost/asio/write.hpp>
#include <boost/asio/high_resolution_timer.hpp>
#include <boost/ref.hpp>

#ifndef NDEBUG
#include <iostream>
#endif

namespace twisted {

template <typename ChildProtocol, typename BufferType>
class protocol_core : public std::enable_shared_from_this<ChildProtocol> {
public:
    typedef detail::socket_base socket_type;
    typedef boost::asio::steady_timer timer_type;
    typedef boost::asio::io_service::strand strand_type;
    typedef BufferType buffer_type;
    typedef typename buffer_type::iterator buffer_iterator;
    typedef typename buffer_type::const_iterator const_buffer_iterator;

    protocol_core() = default;
    protocol_core(protocol_core&&) = default;
    protocol_core& operator=(protocol_core&&) = default;

    void set_socket(std::unique_ptr<socket_type> socket) {
        _socket = std::move(socket);
        _strand = boost::in_place(boost::ref(_socket->get_io_service()));
    }

    void run_protocol() {
        auto self = this_protocol().shared_from_this();
        boost::asio::spawn(*_strand,
                           [this, self](boost::asio::yield_context yield) {
            _yield = boost::in_place(yield);

            try {
                _socket->do_handshake(*_yield);

                while (_socket->is_open()) {
                    auto bytes_read =
                        _socket->async_read_some(asio_buffer(), yield);
                    this_protocol().on_message(
                        buffer_begin(), std::next(buffer_begin(), bytes_read));
                }
            }
            catch (boost::system::system_error& connection_error) {
                handle_network_error(connection_error);
            }
            catch (...) {
                handle_user_error(std::current_exception());
            }
        });
    }

    template <typename Duration>
    void wait_for(const Duration& duration) const {
        boost::asio::high_resolution_timer timer(_socket->get_io_service(),
                                                 duration);
        timer.async_wait(*_yield);
    }

    template <typename Iter>
    void send_message(Iter begin, Iter end) {
        _socket->async_write(
            boost::asio::buffer(&*begin, std::distance(begin, end)), *_yield);
    }

    void send_buffers(const std::array<boost::asio::const_buffer, 2>& buffers) {
        _socket->async_write(buffers, *_yield);
    }

    /*
     * @brief default on_disconnect implementation; does nothing
     */
    void on_disconnect() {}

    /*
     * @brief default on_error implementation; swallows everything
     */
    void on_error(std::exception_ptr eptr) {
        try {
            std::rethrow_exception(eptr);
        }
        catch (const std::exception& excep) {
            print_exception_what(excep);
        }
    }

    void lose_connection() { _socket->close(); }

    /*
     * @brief CRTP wrapper for derived class access
     */
    ChildProtocol& this_protocol() {
        return *static_cast<ChildProtocol*>(this);
    }

    /*
     * @brief CRTP wrapper for derived class access
     */
    const ChildProtocol& this_protocol() const {
        return *static_cast<ChildProtocol*>(this);
    }

private:
#ifdef NDEBUG
    void handle_network_error(boost::system::system_error&) {
#else
    void handle_network_error(boost::system::system_error& connection_error) {
        print_connection_error(connection_error);
#endif
        this_protocol().on_disconnect();
    }

    void handle_user_error(const std::exception_ptr& excep) {
        this_protocol().on_error(excep);
    }

    void print_connection_error(
        const boost::system::system_error& connection_error) const {
        std::cerr << "Client disconnected with code " << connection_error.what()
                  << std::endl;
    }

    void print_exception_what(const std::exception& excep) {
        std::cerr << "Killing connection, exception in client handler: "
                  << excep.what() << std::endl;
    }

    buffer_iterator buffer_begin() {
        return this_protocol().read_buffer().begin();
    }

    buffer_iterator buffer_begin() const {
        return this_protocol().read_buffer().begin();
    }

    buffer_iterator buffer_end() { return this_protocol().read_buffer().end(); }

    buffer_iterator buffer_end() const {
        return this_protocol().read_buffer().end();
    }

    boost::asio::mutable_buffers_1 asio_buffer() {
        return boost::asio::buffer(&*buffer_begin(),
                                   std::distance(buffer_begin(), buffer_end()));
    }

private:
    boost::optional<boost::asio::yield_context> _yield;
    std::unique_ptr<socket_type> _socket;
    boost::optional<strand_type> _strand;
};

} // namespace twisted

#endif
<|endoftext|>


<commit_before>// Copyright (c) 2010-2020, Lawrence Livermore National Security, LLC. Produced
// at the Lawrence Livermore National Laboratory. All Rights reserved. See files
// LICENSE and NOTICE for details. LLNL-CODE-806117.
//
// This file is part of the MFEM library. For more information and source code
// availability visit https://mfem.org.
//
// MFEM is free software; you can redistribute it and/or modify it under the
// terms of the BSD-3 license. We welcome feedback and contributions, see file
// CONTRIBUTING.md for details.

#include "estimators.hpp"

namespace mfem
{

void ZienkiewiczZhuEstimator::ComputeEstimates()
{
   flux_space->Update(false);
   // In parallel, 'flux' can be a GridFunction, as long as 'flux_space' is a
   // ParFiniteElementSpace and 'solution' is a ParGridFunction.
   GridFunction flux(flux_space);

   if (!anisotropic) { aniso_flags.SetSize(0); }
   total_error = ZZErrorEstimator(*integ, *solution, flux, error_estimates,
                                  anisotropic ? &aniso_flags : NULL,
                                  flux_averaging,
                                  with_coeff);

   current_sequence = solution->FESpace()->GetMesh()->GetSequence();
}

#ifdef MFEM_USE_MPI

void L2ZienkiewiczZhuEstimator::ComputeEstimates()
{
   flux_space->Update(false);
   smooth_flux_space->Update(false);

   // TODO: move these parameters in the class, and add Set* methods.
   const double solver_tol = 1e-12;
   const int solver_max_it = 200;
   total_error = L2ZZErrorEstimator(*integ, *solution, *smooth_flux_space,
                                    *flux_space, error_estimates,
                                    local_norm_p, solver_tol, solver_max_it);

   current_sequence = solution->FESpace()->GetMesh()->GetSequence();
}

void KellyErrorEstimator::ComputeEstimates()
{
   // Remarks:
   // For some context you may have to consult the documentation of
   // the FaceInfo class [1]. Also, the FaceElementTransformations
   // documentation [2] may be helpful to grasp what is going on. Note
   // that the FaceElementTransformations also works in the non-
   // conforming case to transfer the gauss points from the slave to
   // the master element.
   // [1]
   // https://github.com/mfem/mfem/blob/02d0bfe9c18ce049c3c93a6a4208080fcfc96991/mesh/mesh.hpp#L94
   // [2]
   // https://github.com/mfem/mfem/blob/02d0bfe9c18ce049c3c93a6a4208080fcfc96991/fem/eltrans.hpp#L435
   flux_space->Update(false);

   auto xfes      = solution->ParFESpace();
   auto pmesh     = xfes->GetParMesh();

   this->error_estimates.SetSize(xfes->GetNE());
   this->error_estimates = 0.0;

   // 1. Compute fluxes in discontinuous space
   ParGridFunction flux(flux_space);
   flux = 0.0;

   Array<int> xdofs, fdofs;
   Vector el_x, el_f;
   for (int e = 0; e < xfes->GetNE(); e++)
   {
      auto attr = xfes->GetAttribute(e);
      if (attributes.Size() && attributes.Find(attr) == -1)
      {
         continue;
      }

      xfes->GetElementVDofs(e, xdofs);
      solution->GetSubVector(xdofs, el_x);

      ElementTransformation* Transf = xfes->GetElementTransformation(e);
      flux_integrator->ComputeElementFlux(*xfes->GetFE(e), *Transf, el_x,
                                          *flux_space->GetFE(e), el_f, true);

      flux_space->GetElementVDofs(e, fdofs);
      flux.AddElementVector(fdofs, el_f);
   }

   // 2. Add error contribution from local interior faces
   auto int_rules = IntegrationRules();
   for (int f = 0; f < pmesh->GetNumFaces(); f++)
   {
      auto FT = pmesh->GetFaceElementTransformations(f);

      const auto int_rule =
         int_rules.Get(FT->FaceGeom, 2 * xfes->GetFaceOrder(f));
      const auto nip = int_rule.GetNPoints();

      if (pmesh->FaceIsInterior(f))
      {
         int Inf1, Inf2, NCFace;
         pmesh->GetFaceInfos(f, &Inf1, &Inf2, &NCFace);

         // Convention
         // * Conforming face: Face side with smaller element id handles
         // the integration
         // * Non-conforming face: The slave handles the integration.
         // See FaceInfo documentation for details.
         bool isNCSlave    = FT->Elem2No >= 0 && NCFace >= 0;
         bool isConforming = FT->Elem2No >= 0 && NCFace == -1;
         if ((FT->Elem1No < FT->Elem2No && isConforming) || isNCSlave)
         {
            if (attributes.Size() &&
                (attributes.Find(FT->Elem1->Attribute) == -1
                 || attributes.Find(FT->Elem2->Attribute) == -1))
            {
               continue;
            }

            IntegrationRule eir;
            Vector jumps(nip);

            // Integral over local half face on the side of e₁
            // i.e. the numerical integration of ∫ flux ⋅ n dS₁
            for (int i = 0; i < nip; i++)
            {
               // Evaluate flux at IP
               auto fip = int_rule.IntPoint(i);
               IntegrationPoint ip;
               FT->Loc1.Transform(fip, ip);

               Vector val(flux_space->GetVDim());
               flux.GetVectorValue(FT->Elem1No, ip, val);

               // And build scalar product with normal
               Vector normal(pmesh->SpaceDimension());
               FT->Face->SetIntPoint(&fip);
               if (pmesh->Dimension() == pmesh->SpaceDimension())
               {
                  CalcOrtho(FT->Face->Jacobian(), normal);
               }
               else
               {
                  Vector ref_normal(pmesh->Dimension());
                  FT->Loc1.Transf.SetIntPoint(&fip);
                  CalcOrtho(FT->Loc1.Transf.Jacobian(), ref_normal);
                  auto &e1 = FT->GetElement1Transformation();
                  e1.AdjugateJacobian().MultTranspose(ref_normal, normal);
                  normal /= e1.Weight();
               }
               jumps(i) = val * normal * fip.weight * FT->Face->Weight();
            }

            // Subtract integral over half face of e₂
            // i.e. the numerical integration of ∫ flux ⋅ n dS₂
            for (int i = 0; i < nip; i++)
            {
               // Evaluate flux vector at IP
               auto fip = int_rule.IntPoint(i);
               IntegrationPoint ip;
               FT->Loc2.Transform(fip, ip);

               Vector val(flux_space->GetVDim());
               flux.GetVectorValue(FT->Elem2No, ip, val);

               // And build scalar product with normal
               Vector normal(pmesh->SpaceDimension());
               FT->Face->SetIntPoint(&fip);
               if (pmesh->Dimension() == pmesh->SpaceDimension())
               {
                  CalcOrtho(FT->Face->Jacobian(), normal);
               }
               else
               {
                  Vector ref_normal(pmesh->Dimension());
                  FT->Loc1.Transf.SetIntPoint(&fip);
                  CalcOrtho(FT->Loc1.Transf.Jacobian(), ref_normal);
                  auto &e1 = FT->GetElement1Transformation();
                  e1.AdjugateJacobian().MultTranspose(ref_normal, normal);
                  normal /= e1.Weight();
               }
               normal.Neg();

               jumps(i) -= val * normal * fip.weight * FT->Face->Weight();
            }

            // Finalize "local" L₂ contribution
            for (int i = 0; i < nip; i++)
            {
               jumps(i) *= jumps(i);
            }
            auto h_k_face = compute_face_coefficient(pmesh, f, false);
            double jump_integral = h_k_face*jumps.Sum();

            // A local face is shared between two local elements, so we
            // can get away with integrating the jump only once and add
            // it to both elements. To minimize communication, the jump
            // of shared faces is computed locally by each process.
            error_estimates(FT->Elem1No) += jump_integral;
            error_estimates(FT->Elem2No) += jump_integral;
         }
      }
   }

   // 3. Add error contribution from shared interior faces
   // Synchronize face data.
   flux.ExchangeFaceNbrData();

   for (int sf = 0; sf < pmesh->GetNSharedFaces(); sf++)
   {
      auto FT = pmesh->GetSharedFaceTransformations(sf, true);
      if (attributes.Size() &&
          (attributes.Find(FT->Elem1->Attribute) == -1
           || attributes.Find(FT->Elem2->Attribute) == -1))
      {
         continue;
      }

      const auto int_rule =
         int_rules.Get(FT->FaceGeom, 2 * xfes->GetFaceOrder(0));
      const auto nip = int_rule.GetNPoints();

      IntegrationRule eir;
      Vector jumps(nip);

      // Integral over local half face on the side of e₁
      // i.e. the numerical integration of ∫ flux ⋅ n dS₁
      for (int i = 0; i < nip; i++)
      {
         // Evaluate flux vector at integration point
         auto fip = int_rule.IntPoint(i);
         IntegrationPoint ip;
         FT->Loc1.Transform(fip, ip);

         Vector val(flux_space->GetVDim());
         flux.GetVectorValue(FT->Elem1No, ip, val);

         Vector normal(pmesh->SpaceDimension());
         FT->Face->SetIntPoint(&fip);
         if (pmesh->Dimension() == pmesh->SpaceDimension())
         {
            CalcOrtho(FT->Face->Jacobian(), normal);
         }
         else
         {
            Vector ref_normal(pmesh->Dimension());
            FT->Loc1.Transf.SetIntPoint(&fip);
            CalcOrtho(FT->Loc1.Transf.Jacobian(), ref_normal);
            auto &e1 = FT->GetElement1Transformation();
            e1.AdjugateJacobian().MultTranspose(ref_normal, normal);
            normal /= e1.Weight();
         }

         jumps(i) = val * normal * fip.weight * FT->Face->Weight();
      }

      // Subtract integral over non-local half face of e₂
      // i.e. the numerical integration of ∫ flux ⋅ n dS₂
      for (int i = 0; i < nip; i++)
      {
         // Evaluate flux vector at integration point
         auto fip = int_rule.IntPoint(i);
         IntegrationPoint ip;
         FT->Loc2.Transform(fip, ip);

         Vector val(flux_space->GetVDim());
         flux.GetVectorValue(FT->Elem2No, ip, val);

         // Evaluate gauss point
         Vector normal(pmesh->SpaceDimension());
         FT->Face->SetIntPoint(&fip);
         if (pmesh->Dimension() == pmesh->SpaceDimension())
         {
            CalcOrtho(FT->Face->Jacobian(), normal);
         }
         else
         {
            Vector ref_normal(pmesh->Dimension());
            CalcOrtho(FT->Loc1.Transf.Jacobian(), ref_normal);
            auto &e1 = FT->GetElement1Transformation();
            e1.AdjugateJacobian().MultTranspose(ref_normal, normal);
            normal /= e1.Weight();
         }
         normal.Neg();

         jumps(i) -= val * normal * fip.weight * FT->Face->Weight();
      }

      // Finalize "local" L₂ contribution
      for (int i = 0; i < nip; i++)
      {
         jumps(i) *= jumps(i);
      }
      auto h_k_face = compute_face_coefficient(pmesh, sf, true);
      double jump_integral = h_k_face*jumps.Sum();

      error_estimates(FT->Elem1No) += jump_integral;
      // We skip "error_estimates(FT->Elem2No) += jump_integral"
      // because the error is stored on the remote process and
      // recomputed there.
   }

   // Finalize element errors
   for (int e = 0; e < xfes->GetNE(); e++)
   {
      auto factor = compute_element_coefficient(pmesh, e);
      // The sqrt belongs to the norm and hₑ to the indicator.
      error_estimates(e) = sqrt(factor * error_estimates(e));
   }

   current_sequence = solution->FESpace()->GetMesh()->GetSequence();

   // Finish by computing the global error.
   double process_local_error = error_estimates.Sum();
   MPI_Allreduce(&process_local_error, &total_error, 1, MPI_DOUBLE,
                 MPI_SUM, xfes->GetComm());
}

#endif // MFEM_USE_MPI

void LpErrorEstimator::ComputeEstimates()
{
   MFEM_VERIFY(coef != NULL || vcoef != NULL,
               "LpErrorEstimator has no coefficient!  Call SetCoef first.");

   error_estimates.SetSize(sol->FESpace()->GetMesh()->GetNE());
   if (coef)
   {
      sol->ComputeElementLpErrors(local_norm_p, *coef, error_estimates);
   }
   else
   {
      sol->ComputeElementLpErrors(local_norm_p, *vcoef, error_estimates);
   }
   current_sequence = sol->FESpace()->GetMesh()->GetSequence();
}

} // namespace mfem
<commit_msg>Add error message.<commit_after>// Copyright (c) 2010-2020, Lawrence Livermore National Security, LLC. Produced
// at the Lawrence Livermore National Laboratory. All Rights reserved. See files
// LICENSE and NOTICE for details. LLNL-CODE-806117.
//
// This file is part of the MFEM library. For more information and source code
// availability visit https://mfem.org.
//
// MFEM is free software; you can redistribute it and/or modify it under the
// terms of the BSD-3 license. We welcome feedback and contributions, see file
// CONTRIBUTING.md for details.

#include "estimators.hpp"

namespace mfem
{

void ZienkiewiczZhuEstimator::ComputeEstimates()
{
   flux_space->Update(false);
   // In parallel, 'flux' can be a GridFunction, as long as 'flux_space' is a
   // ParFiniteElementSpace and 'solution' is a ParGridFunction.
   GridFunction flux(flux_space);

   if (!anisotropic) { aniso_flags.SetSize(0); }
   total_error = ZZErrorEstimator(*integ, *solution, flux, error_estimates,
                                  anisotropic ? &aniso_flags : NULL,
                                  flux_averaging,
                                  with_coeff);

   current_sequence = solution->FESpace()->GetMesh()->GetSequence();
}

#ifdef MFEM_USE_MPI

void L2ZienkiewiczZhuEstimator::ComputeEstimates()
{
   flux_space->Update(false);
   smooth_flux_space->Update(false);

   // TODO: move these parameters in the class, and add Set* methods.
   const double solver_tol = 1e-12;
   const int solver_max_it = 200;
   total_error = L2ZZErrorEstimator(*integ, *solution, *smooth_flux_space,
                                    *flux_space, error_estimates,
                                    local_norm_p, solver_tol, solver_max_it);

   current_sequence = solution->FESpace()->GetMesh()->GetSequence();
}

void KellyErrorEstimator::ComputeEstimates()
{
   // Remarks:
   // For some context you may have to consult the documentation of
   // the FaceInfo class [1]. Also, the FaceElementTransformations
   // documentation [2] may be helpful to grasp what is going on. Note
   // that the FaceElementTransformations also works in the non-
   // conforming case to transfer the gauss points from the slave to
   // the master element.
   // [1]
   // https://github.com/mfem/mfem/blob/02d0bfe9c18ce049c3c93a6a4208080fcfc96991/mesh/mesh.hpp#L94
   // [2]
   // https://github.com/mfem/mfem/blob/02d0bfe9c18ce049c3c93a6a4208080fcfc96991/fem/eltrans.hpp#L435
   flux_space->Update(false);

   auto xfes      = solution->ParFESpace();
   MFEM_ASSERT(xfes->GetVDim() == 1, "Estimation for vector-valued problems not implemented yet.");
   auto pmesh     = xfes->GetParMesh();

   this->error_estimates.SetSize(xfes->GetNE());
   this->error_estimates = 0.0;

   // 1. Compute fluxes in discontinuous space
   ParGridFunction flux(flux_space);
   flux = 0.0;

   Array<int> xdofs, fdofs;
   Vector el_x, el_f;
   for (int e = 0; e < xfes->GetNE(); e++)
   {
      auto attr = xfes->GetAttribute(e);
      if (attributes.Size() && attributes.Find(attr) == -1)
      {
         continue;
      }

      xfes->GetElementVDofs(e, xdofs);
      solution->GetSubVector(xdofs, el_x);

      ElementTransformation* Transf = xfes->GetElementTransformation(e);
      flux_integrator->ComputeElementFlux(*xfes->GetFE(e), *Transf, el_x,
                                          *flux_space->GetFE(e), el_f, true);

      flux_space->GetElementVDofs(e, fdofs);
      flux.AddElementVector(fdofs, el_f);
   }

   // 2. Add error contribution from local interior faces
   auto int_rules = IntegrationRules();
   for (int f = 0; f < pmesh->GetNumFaces(); f++)
   {
      auto FT = pmesh->GetFaceElementTransformations(f);

      const auto int_rule =
         int_rules.Get(FT->FaceGeom, 2 * xfes->GetFaceOrder(f));
      const auto nip = int_rule.GetNPoints();

      if (pmesh->FaceIsInterior(f))
      {
         int Inf1, Inf2, NCFace;
         pmesh->GetFaceInfos(f, &Inf1, &Inf2, &NCFace);

         // Convention
         // * Conforming face: Face side with smaller element id handles
         // the integration
         // * Non-conforming face: The slave handles the integration.
         // See FaceInfo documentation for details.
         bool isNCSlave    = FT->Elem2No >= 0 && NCFace >= 0;
         bool isConforming = FT->Elem2No >= 0 && NCFace == -1;
         if ((FT->Elem1No < FT->Elem2No && isConforming) || isNCSlave)
         {
            if (attributes.Size() &&
                (attributes.Find(FT->Elem1->Attribute) == -1
                 || attributes.Find(FT->Elem2->Attribute) == -1))
            {
               continue;
            }

            IntegrationRule eir;
            Vector jumps(nip);

            // Integral over local half face on the side of e₁
            // i.e. the numerical integration of ∫ flux ⋅ n dS₁
            for (int i = 0; i < nip; i++)
            {
               // Evaluate flux at IP
               auto fip = int_rule.IntPoint(i);
               IntegrationPoint ip;
               FT->Loc1.Transform(fip, ip);

               Vector val(flux_space->GetVDim());
               flux.GetVectorValue(FT->Elem1No, ip, val);

               // And build scalar product with normal
               Vector normal(pmesh->SpaceDimension());
               FT->Face->SetIntPoint(&fip);
               if (pmesh->Dimension() == pmesh->SpaceDimension())
               {
                  CalcOrtho(FT->Face->Jacobian(), normal);
               }
               else
               {
                  Vector ref_normal(pmesh->Dimension());
                  FT->Loc1.Transf.SetIntPoint(&fip);
                  CalcOrtho(FT->Loc1.Transf.Jacobian(), ref_normal);
                  auto &e1 = FT->GetElement1Transformation();
                  e1.AdjugateJacobian().MultTranspose(ref_normal, normal);
                  normal /= e1.Weight();
               }
               jumps(i) = val * normal * fip.weight * FT->Face->Weight();
            }

            // Subtract integral over half face of e₂
            // i.e. the numerical integration of ∫ flux ⋅ n dS₂
            for (int i = 0; i < nip; i++)
            {
               // Evaluate flux vector at IP
               auto fip = int_rule.IntPoint(i);
               IntegrationPoint ip;
               FT->Loc2.Transform(fip, ip);

               Vector val(flux_space->GetVDim());
               flux.GetVectorValue(FT->Elem2No, ip, val);

               // And build scalar product with normal
               Vector normal(pmesh->SpaceDimension());
               FT->Face->SetIntPoint(&fip);
               if (pmesh->Dimension() == pmesh->SpaceDimension())
               {
                  CalcOrtho(FT->Face->Jacobian(), normal);
               }
               else
               {
                  Vector ref_normal(pmesh->Dimension());
                  FT->Loc1.Transf.SetIntPoint(&fip);
                  CalcOrtho(FT->Loc1.Transf.Jacobian(), ref_normal);
                  auto &e1 = FT->GetElement1Transformation();
                  e1.AdjugateJacobian().MultTranspose(ref_normal, normal);
                  normal /= e1.Weight();
               }
               normal.Neg();

               jumps(i) -= val * normal * fip.weight * FT->Face->Weight();
            }

            // Finalize "local" L₂ contribution
            for (int i = 0; i < nip; i++)
            {
               jumps(i) *= jumps(i);
            }
            auto h_k_face = compute_face_coefficient(pmesh, f, false);
            double jump_integral = h_k_face*jumps.Sum();

            // A local face is shared between two local elements, so we
            // can get away with integrating the jump only once and add
            // it to both elements. To minimize communication, the jump
            // of shared faces is computed locally by each process.
            error_estimates(FT->Elem1No) += jump_integral;
            error_estimates(FT->Elem2No) += jump_integral;
         }
      }
   }

   // 3. Add error contribution from shared interior faces
   // Synchronize face data.
   flux.ExchangeFaceNbrData();

   for (int sf = 0; sf < pmesh->GetNSharedFaces(); sf++)
   {
      auto FT = pmesh->GetSharedFaceTransformations(sf, true);
      if (attributes.Size() &&
          (attributes.Find(FT->Elem1->Attribute) == -1
           || attributes.Find(FT->Elem2->Attribute) == -1))
      {
         continue;
      }

      const auto int_rule =
         int_rules.Get(FT->FaceGeom, 2 * xfes->GetFaceOrder(0));
      const auto nip = int_rule.GetNPoints();

      IntegrationRule eir;
      Vector jumps(nip);

      // Integral over local half face on the side of e₁
      // i.e. the numerical integration of ∫ flux ⋅ n dS₁
      for (int i = 0; i < nip; i++)
      {
         // Evaluate flux vector at integration point
         auto fip = int_rule.IntPoint(i);
         IntegrationPoint ip;
         FT->Loc1.Transform(fip, ip);

         Vector val(flux_space->GetVDim());
         flux.GetVectorValue(FT->Elem1No, ip, val);

         Vector normal(pmesh->SpaceDimension());
         FT->Face->SetIntPoint(&fip);
         if (pmesh->Dimension() == pmesh->SpaceDimension())
         {
            CalcOrtho(FT->Face->Jacobian(), normal);
         }
         else
         {
            Vector ref_normal(pmesh->Dimension());
            FT->Loc1.Transf.SetIntPoint(&fip);
            CalcOrtho(FT->Loc1.Transf.Jacobian(), ref_normal);
            auto &e1 = FT->GetElement1Transformation();
            e1.AdjugateJacobian().MultTranspose(ref_normal, normal);
            normal /= e1.Weight();
         }

         jumps(i) = val * normal * fip.weight * FT->Face->Weight();
      }

      // Subtract integral over non-local half face of e₂
      // i.e. the numerical integration of ∫ flux ⋅ n dS₂
      for (int i = 0; i < nip; i++)
      {
         // Evaluate flux vector at integration point
         auto fip = int_rule.IntPoint(i);
         IntegrationPoint ip;
         FT->Loc2.Transform(fip, ip);

         Vector val(flux_space->GetVDim());
         flux.GetVectorValue(FT->Elem2No, ip, val);

         // Evaluate gauss point
         Vector normal(pmesh->SpaceDimension());
         FT->Face->SetIntPoint(&fip);
         if (pmesh->Dimension() == pmesh->SpaceDimension())
         {
            CalcOrtho(FT->Face->Jacobian(), normal);
         }
         else
         {
            Vector ref_normal(pmesh->Dimension());
            CalcOrtho(FT->Loc1.Transf.Jacobian(), ref_normal);
            auto &e1 = FT->GetElement1Transformation();
            e1.AdjugateJacobian().MultTranspose(ref_normal, normal);
            normal /= e1.Weight();
         }
         normal.Neg();

         jumps(i) -= val * normal * fip.weight * FT->Face->Weight();
      }

      // Finalize "local" L₂ contribution
      for (int i = 0; i < nip; i++)
      {
         jumps(i) *= jumps(i);
      }
      auto h_k_face = compute_face_coefficient(pmesh, sf, true);
      double jump_integral = h_k_face*jumps.Sum();

      error_estimates(FT->Elem1No) += jump_integral;
      // We skip "error_estimates(FT->Elem2No) += jump_integral"
      // because the error is stored on the remote process and
      // recomputed there.
   }

   // Finalize element errors
   for (int e = 0; e < xfes->GetNE(); e++)
   {
      auto factor = compute_element_coefficient(pmesh, e);
      // The sqrt belongs to the norm and hₑ to the indicator.
      error_estimates(e) = sqrt(factor * error_estimates(e));
   }

   current_sequence = solution->FESpace()->GetMesh()->GetSequence();

   // Finish by computing the global error.
   double process_local_error = error_estimates.Sum();
   MPI_Allreduce(&process_local_error, &total_error, 1, MPI_DOUBLE,
                 MPI_SUM, xfes->GetComm());
}

#endif // MFEM_USE_MPI

void LpErrorEstimator::ComputeEstimates()
{
   MFEM_VERIFY(coef != NULL || vcoef != NULL,
               "LpErrorEstimator has no coefficient!  Call SetCoef first.");

   error_estimates.SetSize(sol->FESpace()->GetMesh()->GetNE());
   if (coef)
   {
      sol->ComputeElementLpErrors(local_norm_p, *coef, error_estimates);
   }
   else
   {
      sol->ComputeElementLpErrors(local_norm_p, *vcoef, error_estimates);
   }
   current_sequence = sol->FESpace()->GetMesh()->GetSequence();
}

} // namespace mfem
<|endoftext|>