<commit_before>//===-------------------------- SILCombine --------------------------------===//
//
// This source file is part of the Swift.org open source project
//
// Copyright (c) 2014 - 2015 Apple Inc. and the Swift project authors
// Licensed under Apache License v2.0 with Runtime Library Exception
//
// See http://swift.org/LICENSE.txt for license information
// See http://swift.org/CONTRIBUTORS.txt for the list of Swift project authors
//
//===----------------------------------------------------------------------===//
//
// A port of LLVM's InstCombine pass to SIL. Its main purpose is for performing
// small combining operations/peepholes at the SIL level. It additionally DCEs
// before in order to reduce compile time.
//
//===----------------------------------------------------------------------===//

#define DEBUG_TYPE "sil-combine"
#include "llvm/ADT/SmallPtrSet.h"
#include "llvm/ADT/SmallVector.h"
#include "llvm/ADT/Statistic.h"
#include "llvm/Support/Debug.h"
#include "swift/SIL/SILBuilder.h"
#include "swift/SIL/SILVisitor.h"
#include "swift/SILPasses/Utils/Local.h"
#include "swift/Subsystems.h"

using namespace swift;

STATISTIC(NumCombined, "Number of instructions combined.");
STATISTIC(NumDeadInst, "Num dead insts eliminated.");

//===----------------------------------------------------------------------===//
//                             SILCombineWorklist
//===----------------------------------------------------------------------===//

namespace swift {

/// This is the worklist management logic for SILCombine.
class SILCombineWorklist {
  llvm::SmallVector<SILInstruction*, 256> Worklist;
  llvm::DenseMap<SILInstruction*, unsigned> WorklistMap;
  llvm::SmallVector<SILInstruction *, 8> TrackingList;

  void operator=(const SILCombineWorklist&RHS) = delete;
  SILCombineWorklist(const SILCombineWorklist&) = delete;
public:
  SILCombineWorklist() {}

  /// Returns true if the worklist is empty.
  bool isEmpty() const { return Worklist.empty(); }

  /// Add the specified instruction to the worklist if it isn't already in it.
  void add(SILInstruction *I) {
    if (WorklistMap.insert(std::make_pair(I, Worklist.size())).second) {
      DEBUG(llvm::dbgs() << "SC: ADD: " << *I << '\n');
      Worklist.push_back(I);
    }
  }

  /// Add the given ValueBase if it is an instruction to the worklist if it is a
  /// SILInstruction.
  void addValue(ValueBase *V) {
    if (SILInstruction *I = llvm::dyn_cast<SILInstruction>(V))
      add(I);
  }

  /// Add the specified batch of stuff in reverse order.  which should only be
  /// done when the worklist is empty and when the group has no duplicates.
  void addInitialGroup(SILInstruction *const *List, unsigned NumEntries) {
    assert(Worklist.empty() && "Worklist must be empty to add initial group");
    Worklist.reserve(NumEntries+16);
    WorklistMap.resize(NumEntries);
    DEBUG(llvm::dbgs() << "SC: ADDING: " << NumEntries
                       << " instrs to worklist\n");
    for (unsigned Idx = 0; NumEntries; --NumEntries) {
      SILInstruction *I = List[NumEntries-1];
      WorklistMap.insert(std::make_pair(I, Idx++));
      Worklist.push_back(I);
    }
  }

  // Remove I from the worklist if it exists.
  void remove(SILInstruction *I) {
    auto It = WorklistMap.find(I);
    if (It == WorklistMap.end()) return; // Not in worklist.

    // Don't bother moving everything down, just null out the slot. We will
    // check before we process any instruction if it is 0 allowing us to not
    // have to shift all of the elements of our worklist down everytime an
    // instruction is removed.
    Worklist[It->second] = 0;

    WorklistMap.erase(It);
  }

  /// Remove the top element from the worklist.
  SILInstruction *removeOne() {
    SILInstruction *I = Worklist.pop_back_val();
    WorklistMap.erase(I);
    return I;
  }

  /// When an instruction is simplified, add all users of the instruction to the
  /// work lists because they might get more simplified now.
  void addUsersToWorklist(SILInstruction &I) {
    for (auto UI : I.getUses())
      add(llvm::cast<SILInstruction>(UI->getUser()));
  }

  /// Check that the worklist is empty and nuke the backing store for the map if
  /// it is large.
  void zap() {
    assert(WorklistMap.empty() && "Worklist empty, but the map is not?");

    // Do an explicit clear, this shrinks the map if needed.
    WorklistMap.clear();
  }
};

} // end namespace swift

//===----------------------------------------------------------------------===//
//                                SILCombiner
//===----------------------------------------------------------------------===//

namespace swift {

/// This is a class which maintains the state of the combine and simplifies many
/// operations such as removing/adding instructions and syncing that with the
/// worklist.
class SILCombiner :
    public SILInstructionVisitor<SILCombiner, SILInstruction *> {
public:
  SILCombiner() : Worklist(), MadeChange(false), Iteration(0), Builder(0) { }

  bool runOnFunction(SILFunction &F) {
    bool MadeActualChange = false;
    Iteration = 0;

    // Create a SILBuilder for F and initialize the TrackingList.
    SILBuilder B(F);
    B.setTrackingList(&TrackingList);
    Builder = &B;

    // Perform iterations until we do not make any changes.
    while (doOneIteration(F, Iteration)) {
      Iteration++;
      MadeActualChange = true;
    }

    // Cleanup builder and return whether or not we made any changes.
    Builder = 0;
    return MadeActualChange;
  }

  void clear() {
    Iteration = 0;
    Worklist.zap();
    MadeChange = false;
  }

  // Insert the instruction New before instruction Old in the program.  Add the
  // new instruction to the worklist.
  SILInstruction *insertNewInstBefore(SILInstruction *New,
                                      SILInstruction &Old) {
    assert(New && New->getParent() == 0 &&
           "New instruction already inserted into a basic block!");
    SILBasicBlock *BB = Old.getParent();
    BB->getInstList().insert(&Old, New);  // Insert inst
    Worklist.add(New);
    return New;
  }

  // This method is to be used when an instruction is found to be dead,
  // replacable with another preexisting expression. Here we add all uses of I
  // to the worklist, replace all uses of I with the new value, then return I,
  // so that the inst combiner will know that I was modified.
  SILInstruction *replaceInstUsesWith(SILInstruction &I, ValueBase *V) {
    Worklist.addUsersToWorklist(I);   // Add all modified instrs to worklist.

    DEBUG(llvm::dbgs() << "SC: Replacing " << I << "\n"
          "    with " << *V << '\n');

    SILValue(&I, 0).replaceAllUsesWith(V);
    return &I;
  }

  // When dealing with an instruction that has side effects or produces a void
  // value, we can't rely on DCE to delete the instruction.  Instead, visit
  // methods should return the value returned by this function.
  SILInstruction *eraseInstFromFunction(SILInstruction &I) {
    DEBUG(llvm::dbgs() << "SC: ERASE " << I << '\n');

    assert(I.use_empty() && "Cannot erase instruction that is used!");
    // Make sure that we reprocess all operands now that we reduced their
    // use counts.
    if (I.getNumOperands() < 8) {
      for (auto &OpI : I.getAllOperands())
        if (SILInstruction *Op = llvm::dyn_cast<SILInstruction>(&*OpI.get()))
          Worklist.add(Op);
    }
    Worklist.remove(&I);
    I.eraseFromParent();
    MadeChange = true;
    return 0;  // Don't do anything with FI
  }

  void addInitialGroup(SILInstruction *const *List, unsigned NumEntries) {
    Worklist.addInitialGroup(List, NumEntries);
  }

  /// Base visitor that does not do anything.
  SILInstruction *visitValueBase(ValueBase *V) { return nullptr; }

private:
  /// Perform one SILCombine iteration.
  bool doOneIteration(SILFunction &F, unsigned Iteration);

  /// All of the instructions that need to be simplified.
  SILCombineWorklist Worklist;
  /// Did we make a chanbge?
  bool MadeChange;
  /// What is the current iteration of the SILCombine we are on.
  unsigned Iteration;
  /// Builder used to insert instructions.
  SILBuilder *Builder;
  /// A list that the builder puts newly created instructions into. Its contents
  /// are added to the worklist after every iteration and then is cleared.
  llvm::SmallVector<SILInstruction *, 64> TrackingList;
};

} // end namespace swift

//===----------------------------------------------------------------------===//
//                         SILCombine Implementation
//===----------------------------------------------------------------------===//

/// addReachableCodeToWorklist - Walk the function in depth-first order, adding
/// all reachable code to the worklist.
///
/// This has a couple of tricks to make the code faster and more powerful.  In
/// particular, we DCE instructions as we go, to avoid adding them to the
/// worklist (this significantly speeds up instcombine on code where many
/// instructions are dead or constant).
static void addReachableCodeToWorklist(SILBasicBlock *BB, SILCombiner &IC) {
  llvm::SmallVector<SILBasicBlock*, 256> Worklist;
  llvm::SmallVector<SILInstruction*, 128> InstrsForInstCombineWorklist;
  llvm::SmallPtrSet<SILBasicBlock*, 64> Visited;

  Worklist.push_back(BB);
  do {
    BB = Worklist.pop_back_val();

    // We have now visited this block!  If we've already been here, ignore it.
    if (!Visited.insert(BB)) continue;

    for (SILBasicBlock::iterator BBI = BB->begin(), E = BB->end(); BBI != E; ) {
      SILInstruction *Inst = BBI++;

      // DCE instruction if trivially dead.
      if (isInstructionTriviallyDead(Inst)) {
        ++NumDeadInst;
        DEBUG(llvm::dbgs() << "SC: DCE: " << *Inst << '\n');
        Inst->eraseFromParent();
        continue;
      }

      InstrsForInstCombineWorklist.push_back(Inst);
    }

    // Recursively visit successors.
    for (auto SI = BB->succ_begin(), SE = BB->succ_end(); SI != SE; ++SI)
      Worklist.push_back(*SI);
  } while (!Worklist.empty());

  // Once we've found all of the instructions to add to SILCombine's worklist,
  // add them in reverse order.  This way SILCombine will visit from the top of
  // the function down.  This jives well with the way that it adds all uses of
  // instructions to the worklist after doing a transformation, thus avoiding
  // some N^2 behavior in pathological cases.
  IC.addInitialGroup(&InstrsForInstCombineWorklist[0],
                     InstrsForInstCombineWorklist.size());
}

bool SILCombiner::doOneIteration(SILFunction &F, unsigned Iteration) {
  MadeChange = false;

  DEBUG(llvm::dbgs() << "\n\nSILCOMBINE ITERATION #" << Iteration << " on "
                     << F.getName() << "\n");

  // Add reachable instructions to our worklist.
  addReachableCodeToWorklist(F.begin(), *this);

  // Process until we run out of items in our worklist.
  while (!Worklist.isEmpty()) {
    SILInstruction *I = Worklist.removeOne();

    // When we erase an instruction, we use the map in the worklist to check if
    // the instruction is in the worklist. If it is, we replace it with 0
    // instead of shifting all members of the worklist towards the front. This
    // check makes sure that if we run into any such residual null pointers, we
    // skip them.
    if (I == 0)
      continue;

    // Check to see if we can DCE the instruction.
    if (isInstructionTriviallyDead(I)) {
      DEBUG(llvm::dbgs() << "SC: DCE: " << *I << '\n');
      eraseInstFromFunction(*I);
      ++NumDeadInst;
      MadeChange = true;
      continue;
    }

    // Now that we have an instruction, try simplifying it.
    Builder->setInsertionPoint(I->getParent(), I);

#ifndef NDEBUG
    std::string OrigI;
#endif
    DEBUG(llvm::raw_string_ostream SS(OrigI); I->print(SS); OrigI = SS.str(););
    DEBUG(llvm::dbgs() << "SC: Visiting: " << OrigI << '\n');

    if (SILInstruction *Result = visit(I)) {
      ++NumCombined;
      // Should we replace the old instruction with a new one?
      if (Result != I) {
        DEBUG(llvm::dbgs() << "SC: Old = " << *I << '\n'
                     << "    New = " << *Result << '\n');

        // Everything uses the new instruction now.
        replaceInstUsesWith(*I, Result);

        // Push the new instruction and any users onto the worklist.
        Worklist.add(Result);
        Worklist.addUsersToWorklist(*Result);

        // Insert the new instruction into the basic block...
        SILBasicBlock *InstParent = I->getParent();
        SILBasicBlock::iterator InsertPos = I;

        InstParent->getInstList().insert(InsertPos, Result);

        eraseInstFromFunction(*I);
      } else {
        DEBUG(llvm::dbgs() << "SC: Mod = " << OrigI << '\n'
                     << "    New = " << *I << '\n');

        // If the instruction was modified, it's possible that it is now dead.
        // if so, remove it.
        if (isInstructionTriviallyDead(I)) {
          eraseInstFromFunction(*I);
        } else {
          Worklist.add(I);
          Worklist.addUsersToWorklist(*I);
        }
      }
      MadeChange = true;
    }

    // Our tracking list which has been accumulating instructions created by the
    // SILBuilder doing this iteration. Go through the tracking list and add
    // them to the worklist and then clear the TrackingList in preparation for
    // the next iteration.
    for (SILInstruction *I : TrackingList) {
      Worklist.add(I);
    }
    TrackingList.clear();
  }

  Worklist.zap();
  return MadeChange;
}

//===----------------------------------------------------------------------===//
//                              Top Level Driver
//===----------------------------------------------------------------------===//

bool swift::performSILCombine(SILModule *M) {
  bool MadeChange = false;
  SILCombiner Combiner;

  // Process each function in M.
  for (SILFunction &F : *M) {
    // If F is just a declaration without any basic blocks, skip it.
    if (!F.size())
      continue;

    // Clear the combiner just in case.
    Combiner.clear();
    // Record if we made any changes.
    MadeChange |= Combiner.runOnFunction(F);
  }

  // Return true if we made any changes.
  return MadeChange;
}
<commit_msg>[sil-combine] Fix comments.<commit_after>//===-------------------------- SILCombine --------------------------------===//
//
// This source file is part of the Swift.org open source project
//
// Copyright (c) 2014 - 2015 Apple Inc. and the Swift project authors
// Licensed under Apache License v2.0 with Runtime Library Exception
//
// See http://swift.org/LICENSE.txt for license information
// See http://swift.org/CONTRIBUTORS.txt for the list of Swift project authors
//
//===----------------------------------------------------------------------===//
//
// A port of LLVM's InstCombine pass to SIL. Its main purpose is for performing
// small combining operations/peepholes at the SIL level. It additionally
// performs dead code elimination when it initially adds instructions to the
// work queue in order to reduce compile time by not visiting trivially dead
// instructions.
//
//===----------------------------------------------------------------------===//

#define DEBUG_TYPE "sil-combine"
#include "llvm/ADT/SmallPtrSet.h"
#include "llvm/ADT/SmallVector.h"
#include "llvm/ADT/Statistic.h"
#include "llvm/Support/Debug.h"
#include "swift/SIL/SILBuilder.h"
#include "swift/SIL/SILVisitor.h"
#include "swift/SILPasses/Utils/Local.h"
#include "swift/Subsystems.h"

using namespace swift;

STATISTIC(NumCombined, "Number of instructions combined.");
STATISTIC(NumDeadInst, "Num dead insts eliminated.");

//===----------------------------------------------------------------------===//
//                             SILCombineWorklist
//===----------------------------------------------------------------------===//

namespace swift {

/// This is the worklist management logic for SILCombine.
class SILCombineWorklist {
  llvm::SmallVector<SILInstruction*, 256> Worklist;
  llvm::DenseMap<SILInstruction*, unsigned> WorklistMap;
  llvm::SmallVector<SILInstruction *, 8> TrackingList;

  void operator=(const SILCombineWorklist&RHS) = delete;
  SILCombineWorklist(const SILCombineWorklist&) = delete;
public:
  SILCombineWorklist() {}

  /// Returns true if the worklist is empty.
  bool isEmpty() const { return Worklist.empty(); }

  /// Add the specified instruction to the worklist if it isn't already in it.
  void add(SILInstruction *I) {
    if (WorklistMap.insert(std::make_pair(I, Worklist.size())).second) {
      DEBUG(llvm::dbgs() << "SC: ADD: " << *I << '\n');
      Worklist.push_back(I);
    }
  }

  /// If the given ValueBase is a SILInstruction add it to the worklist.
  void addValue(ValueBase *V) {
    if (SILInstruction *I = llvm::dyn_cast<SILInstruction>(V))
      add(I);
  }

  /// Add the given list of instructions in reverse order to the worklist. This
  /// routine assumes that the worklist is empty and the given list has no
  /// duplicates.
  void addInitialGroup(SILInstruction *const *List, unsigned NumEntries) {
    assert(Worklist.empty() && "Worklist must be empty to add initial group");
    Worklist.reserve(NumEntries+16);
    WorklistMap.resize(NumEntries);
    DEBUG(llvm::dbgs() << "SC: ADDING: " << NumEntries
                       << " instrs to worklist\n");
    for (unsigned Idx = 0; NumEntries; --NumEntries) {
      SILInstruction *I = List[NumEntries-1];
      WorklistMap.insert(std::make_pair(I, Idx++));
      Worklist.push_back(I);
    }
  }

  // If I is in the worklist, remove it.
  void remove(SILInstruction *I) {
    auto It = WorklistMap.find(I);
    if (It == WorklistMap.end()) return; // Not in worklist.

    // Don't bother moving everything down, just null out the slot. We will
    // check before we process any instruction if it is 0.
    Worklist[It->second] = 0;

    WorklistMap.erase(It);
  }

  /// Remove the top element from the worklist.
  SILInstruction *removeOne() {
    SILInstruction *I = Worklist.pop_back_val();
    WorklistMap.erase(I);
    return I;
  }

  /// When an instruction has been simplified, add all of its users to the
  /// worklist since additional simplifications of its users may have been
  /// exposed.
  void addUsersToWorklist(SILInstruction &I) {
    for (auto UI : I.getUses())
      add(llvm::cast<SILInstruction>(UI->getUser()));
  }

  /// Check that the worklist is empty and nuke the backing store for the map if
  /// it is large.
  void zap() {
    assert(WorklistMap.empty() && "Worklist empty, but the map is not?");

    // Do an explicit clear, this shrinks the map if needed.
    WorklistMap.clear();
  }
};

} // end namespace swift

//===----------------------------------------------------------------------===//
//                                SILCombiner
//===----------------------------------------------------------------------===//

namespace swift {

/// This is a class which maintains the state of the combiner and simplifies
/// many operations such as removing/adding instructions and syncing them with
/// the worklist.
class SILCombiner :
    public SILInstructionVisitor<SILCombiner, SILInstruction *> {
public:
  SILCombiner() : Worklist(), MadeChange(false), Iteration(0), Builder(0) { }

  bool runOnFunction(SILFunction &F) {
    bool MadeActualChange = false;
    Iteration = 0;

    // Create a SILBuilder for F and initialize the tracking list.
    SILBuilder B(F);
    B.setTrackingList(&TrackingList);
    Builder = &B;

    // Perform iterations until we do not make any changes.
    while (doOneIteration(F, Iteration)) {
      Iteration++;
      MadeActualChange = true;
    }

    // Cleanup the builder and return whether or not we made any changes.
    Builder = 0;
    return MadeActualChange;
  }

  void clear() {
    Iteration = 0;
    Worklist.zap();
    MadeChange = false;
  }

  // Insert the instruction New before instruction Old in Old's parent BB. Add
  // New to the worklist.
  SILInstruction *insertNewInstBefore(SILInstruction *New,
                                      SILInstruction &Old) {
    assert(New && New->getParent() == 0 &&
           "New instruction already inserted into a basic block!");
    SILBasicBlock *BB = Old.getParent();
    BB->getInstList().insert(&Old, New);  // Insert inst
    Worklist.add(New);
    return New;
  }

  // This method is to be used when an instruction is found to be dead,
  // replacable with another preexisting expression. Here we add all uses of I
  // to the worklist, replace all uses of I with the new value, then return I,
  // so that the combiner will know that I was modified.
  SILInstruction *replaceInstUsesWith(SILInstruction &I, ValueBase *V) {
    Worklist.addUsersToWorklist(I);   // Add all modified instrs to worklist.

    DEBUG(llvm::dbgs() << "SC: Replacing " << I << "\n"
          "    with " << *V << '\n');

    SILValue(&I, 0).replaceAllUsesWith(V);
    return &I;
  }

  // Some instructions can never be "trivially dead" due to side effects or
  // producing a void value. In those cases, since we can not rely on
  // SILCombines trivially dead instruction DCE in order to delete the
  // instruction, visit methods should use this method to delete the given
  // instruction and upon completion of their peephole return the value returned
  // by this method.
  SILInstruction *eraseInstFromFunction(SILInstruction &I) {
    DEBUG(llvm::dbgs() << "SC: ERASE " << I << '\n');

    assert(I.use_empty() && "Cannot erase instruction that is used!");
    // Make sure that we reprocess all operands now that we reduced their
    // use counts.
    if (I.getNumOperands() < 8) {
      for (auto &OpI : I.getAllOperands())
        if (SILInstruction *Op = llvm::dyn_cast<SILInstruction>(&*OpI.get()))
          Worklist.add(Op);
    }
    Worklist.remove(&I);
    I.eraseFromParent();
    MadeChange = true;
    return 0;  // Don't do anything with I
  }

  void addInitialGroup(SILInstruction *const *List, unsigned NumEntries) {
    Worklist.addInitialGroup(List, NumEntries);
  }

  /// Base visitor that does not do anything.
  SILInstruction *visitValueBase(ValueBase *V) { return nullptr; }

private:
  /// Perform one SILCombine iteration.
  bool doOneIteration(SILFunction &F, unsigned Iteration);

  /// Worklist containing all of the instructions primed for simplification.
  SILCombineWorklist Worklist;
  /// Variable to track if the SILCombiner made any changes.
  bool MadeChange;
  /// The current iteration of the SILCombine.
  unsigned Iteration;
  /// Builder used to insert instructions.
  SILBuilder *Builder;
  /// A list that the builder inserts newly created instructions into. Its
  /// contents are added to the worklist after every iteration and then the list
  /// is cleared.
  llvm::SmallVector<SILInstruction *, 64> TrackingList;
};

} // end namespace swift

//===----------------------------------------------------------------------===//
//                         SILCombine Implementation
//===----------------------------------------------------------------------===//

/// addReachableCodeToWorklist - Walk the function in depth-first order, adding
/// all reachable code to the worklist.
///
/// This has a couple of tricks to make the code faster and more powerful.  In
/// particular, we DCE instructions as we go, to avoid adding them to the
/// worklist (this significantly speeds up SILCombine on code where many
/// instructions are dead or constant).
static void addReachableCodeToWorklist(SILBasicBlock *BB, SILCombiner &SC) {
  llvm::SmallVector<SILBasicBlock*, 256> Worklist;
  llvm::SmallVector<SILInstruction*, 128> InstrsForSILCombineWorklist;
  llvm::SmallPtrSet<SILBasicBlock*, 64> Visited;

  Worklist.push_back(BB);
  do {
    BB = Worklist.pop_back_val();

    // We have now visited this block!  If we've already been here, ignore it.
    if (!Visited.insert(BB)) continue;

    for (SILBasicBlock::iterator BBI = BB->begin(), E = BB->end(); BBI != E; ) {
      SILInstruction *Inst = BBI++;

      // DCE instruction if trivially dead.
      if (isInstructionTriviallyDead(Inst)) {
        ++NumDeadInst;
        DEBUG(llvm::dbgs() << "SC: DCE: " << *Inst << '\n');
        Inst->eraseFromParent();
        continue;
      }

      InstrsForSILCombineWorklist.push_back(Inst);
    }

    // Recursively visit successors.
    for (auto SI = BB->succ_begin(), SE = BB->succ_end(); SI != SE; ++SI)
      Worklist.push_back(*SI);
  } while (!Worklist.empty());

  // Once we've found all of the instructions to add to the worklist, add them
  // in reverse order. This way SILCombine will visit from the top of the
  // function down. This jives well with the way that it adds all uses of
  // instructions to the worklist after doing a transformation, thus avoiding
  // some N^2 behavior in pathological cases.
  SC.addInitialGroup(&InstrsForSILCombineWorklist[0],
                     InstrsForSILCombineWorklist.size());
}

bool SILCombiner::doOneIteration(SILFunction &F, unsigned Iteration) {
  MadeChange = false;

  DEBUG(llvm::dbgs() << "\n\nSILCOMBINE ITERATION #" << Iteration << " on "
                     << F.getName() << "\n");

  // Add reachable instructions to our worklist.
  addReachableCodeToWorklist(F.begin(), *this);

  // Process until we run out of items in our worklist.
  while (!Worklist.isEmpty()) {
    SILInstruction *I = Worklist.removeOne();

    // When we erase an instruction, we use the map in the worklist to check if
    // the instruction is in the worklist. If it is, we replace it with 0
    // instead of shifting all members of the worklist towards the front. This
    // check makes sure that if we run into any such residual null pointers, we
    // skip them.
    if (I == 0)
      continue;

    // Check to see if we can DCE the instruction.
    if (isInstructionTriviallyDead(I)) {
      DEBUG(llvm::dbgs() << "SC: DCE: " << *I << '\n');
      eraseInstFromFunction(*I);
      ++NumDeadInst;
      MadeChange = true;
      continue;
    }

    // Now that we have an instruction, try simplifying it.
    Builder->setInsertionPoint(I->getParent(), I);

#ifndef NDEBUG
    std::string OrigI;
#endif
    DEBUG(llvm::raw_string_ostream SS(OrigI); I->print(SS); OrigI = SS.str(););
    DEBUG(llvm::dbgs() << "SC: Visiting: " << OrigI << '\n');

    if (SILInstruction *Result = visit(I)) {
      ++NumCombined;
      // Should we replace the old instruction with a new one?
      if (Result != I) {
        DEBUG(llvm::dbgs() << "SC: Old = " << *I << '\n'
                     << "    New = " << *Result << '\n');

        // Everything uses the new instruction now.
        replaceInstUsesWith(*I, Result);

        // Push the new instruction and any users onto the worklist.
        Worklist.add(Result);
        Worklist.addUsersToWorklist(*Result);

        // Insert the new instruction into the basic block...
        SILBasicBlock *InstParent = I->getParent();
        SILBasicBlock::iterator InsertPos = I;

        InstParent->getInstList().insert(InsertPos, Result);

        eraseInstFromFunction(*I);
      } else {
        DEBUG(llvm::dbgs() << "SC: Mod = " << OrigI << '\n'
                     << "    New = " << *I << '\n');

        // If the instruction was modified, it's possible that it is now dead.
        // if so, remove it.
        if (isInstructionTriviallyDead(I)) {
          eraseInstFromFunction(*I);
        } else {
          Worklist.add(I);
          Worklist.addUsersToWorklist(*I);
        }
      }
      MadeChange = true;
    }

    // Our tracking list has been accumulating instructions created by the
    // SILBuilder during this iteration. Go through the tracking list and add
    // its contents to the worklist and then clear said list in preparation for
    // the next iteration.
    for (SILInstruction *I : TrackingList) {
      Worklist.add(I);
    }
    TrackingList.clear();
  }

  Worklist.zap();
  return MadeChange;
}

//===----------------------------------------------------------------------===//
//                              Top Level Driver
//===----------------------------------------------------------------------===//

bool swift::performSILCombine(SILModule *M) {
  bool MadeChange = false;
  SILCombiner Combiner;

  // Process each function in M.
  for (SILFunction &F : *M) {
    // If F is just a declaration without any basic blocks, skip it.
    if (!F.size())
      continue;

    // Clear the combiner just in case.
    Combiner.clear();
    // Record if we made any changes.
    MadeChange |= Combiner.runOnFunction(F);
  }

  // Return true if we made any changes.
  return MadeChange;
}
<|endoftext|>


<commit_before>//===- lib/Trace/TracedFunction.cpp ---------------------------------------===//
//
//                                    SeeC
//
// This file is distributed under The MIT License (MIT). See LICENSE.TXT for
// details.
//
//===----------------------------------------------------------------------===//
///
/// \file
///
//===----------------------------------------------------------------------===//

#include "seec/Trace/TracedFunction.hpp"
#include "seec/Trace/TraceThreadListener.hpp"

#include "llvm/IR/Constants.h"
#include "llvm/IR/Instruction.h"

#define SEEC_DEBUG_PTROBJ 0

namespace seec {

namespace trace {

//===----------------------------------------------------------------------===//
// Support getCurrentRuntimeValue.
//===----------------------------------------------------------------------===//

uintptr_t
TracedFunction::getRuntimeAddress(llvm::GlobalVariable const *GV) const
{
  return ThreadListener.getRuntimeAddress(GV);
}

uintptr_t TracedFunction::getRuntimeAddress(llvm::Function const *F) const
{
  return ThreadListener.getRuntimeAddress(F);
}

//===----------------------------------------------------------------------===//
// Accessors for active-only information.
//===----------------------------------------------------------------------===//

seec::Maybe<MemoryArea>
TracedFunction::getContainingMemoryArea(uintptr_t Address) const
{
  std::lock_guard<std::mutex> Lock(StackMutex);

  if (Address < StackLow || Address > StackHigh) {
    // Not occupied by our stack, but may belong to a byval argument.
    for (auto const &Arg : ByValArgs) {
      if (Arg.getArea().contains(Address)) {
        return Arg.getArea();
      }
    }
  }
  else {
    // May be occupied by our stack.
    for (auto const &Alloca : Allocas) {
      auto AllocaArea = Alloca.area();
      if (AllocaArea.contains(Address)) {
        return AllocaArea;
      }
    }
  }

  return seec::Maybe<MemoryArea>();
}

//===----------------------------------------------------------------------===//
// byval argument memory area tracking.
//===----------------------------------------------------------------------===//

void TracedFunction::addByValArg(llvm::Argument const * const Arg,
                                 MemoryArea const &Area)
{
  assert(!EventOffsetEnd && "Function has finished recording!");

  std::lock_guard<std::mutex> Lock(StackMutex);

  ByValArgs.emplace_back(Arg, std::move(Area));
}

seec::Maybe<seec::MemoryArea>
TracedFunction::getParamByValArea(llvm::Argument const *Arg) const
{
  assert(!EventOffsetEnd && "Function has finished recording!");

  std::lock_guard<std::mutex> Lock(StackMutex);

  for (auto const &PBV : ByValArgs)
    if (PBV.getArgument() == Arg)
      return PBV.getArea();

  return seec::Maybe<seec::MemoryArea>();
}

//===----------------------------------------------------------------------===//
// Pointer origin tracking.
//===----------------------------------------------------------------------===//

uintptr_t TracedFunction::getPointerObject(llvm::Argument const *A) const
{
  auto const It = ArgPointerObjects.find(A);
  return It != ArgPointerObjects.end() ? It->second : 0;
}

void TracedFunction::setPointerObject(llvm::Argument const *A,
                                      uintptr_t const Object)
{
  ArgPointerObjects[A] = Object;
#if SEEC_DEBUG_PTROBJ
  llvm::errs() << "set ptr " << Object << " for argument " << *A << "\n";
#endif
}

uintptr_t TracedFunction::getPointerObject(llvm::Instruction const *I) const
{
  auto const It = PointerObjects.find(I);
  return It != PointerObjects.end() ? It->second : 0;
}

void TracedFunction::setPointerObject(llvm::Instruction const *I,
                                      uintptr_t const Object)
{
  PointerObjects[I] = Object;
#if SEEC_DEBUG_PTROBJ
  llvm::errs() << "set ptr " << Object << " for instruction " << *I << "\n";
#endif
}

uintptr_t TracedFunction::getPointerObject(llvm::Value const *V) const
{
  if (auto const I = llvm::dyn_cast<llvm::Instruction>(V))
    return getPointerObject(I);
  else if (auto const A = llvm::dyn_cast<llvm::Argument>(V))
    return getPointerObject(A);
  else if (auto const C = llvm::dyn_cast<llvm::Constant>(V)) {
    if (auto const CE = llvm::dyn_cast<llvm::ConstantExpr>(V)) {
      if (CE->isCast()) {
        return getPointerObject(CE->getOperand(0));
      }
    }
  }
  return ThreadListener.getProcessListener().getPointerObject(V);
}

uintptr_t TracedFunction::transferPointerObject(llvm::Value const *From,
                                                llvm::Instruction const *To)
{
  auto const Object = getPointerObject(From);
  if (Object)
    setPointerObject(To, Object);
  return Object;
}

uintptr_t TracedFunction::transferArgPointerObjectToCall(unsigned const ArgNo)
{
  auto const Call = llvm::dyn_cast<llvm::CallInst>(ActiveInstruction);
  assert(Call && "No CallInst active!");

  return transferPointerObject(Call->getArgOperand(ArgNo), Call);
}

//===----------------------------------------------------------------------===//
// Mutators.
//===----------------------------------------------------------------------===//

void TracedFunction::finishRecording(offset_uint EventOffsetEnd,
                                     uint64_t ThreadTimeExited) {
  assert(!this->EventOffsetEnd && "Function has finished recording!");

  std::lock_guard<std::mutex> Lock(StackMutex);

  this->EventOffsetEnd = EventOffsetEnd;
  this->ThreadTimeExited = ThreadTimeExited;

  // clear active-only information
  ActiveInstruction = nullptr;
  Allocas.clear();
  ByValArgs.clear();
  StackLow = 0;
  StackHigh = 0;
  CurrentValues.clear();
  ArgPointerObjects.clear();
  PointerObjects.clear();
}

void TracedFunction::addAlloca(TracedAlloca Alloca) {
  assert(!EventOffsetEnd && "Function has finished recording!");

  std::lock_guard<std::mutex> Lock(StackMutex);

  auto const &Area = Alloca.area();

  if (Area.address() < StackLow || !StackLow)
    StackLow = Area.address();

  if (Area.lastAddress() > StackHigh || !StackHigh)
    StackHigh = Area.lastAddress();

  Allocas.push_back(std::move(Alloca));
}

void TracedFunction::stackSave(uintptr_t Key) {
  assert(!EventOffsetEnd && "Function has finished recording!");

  std::lock_guard<std::mutex> Lock(StackMutex);

  StackSaves[Key] = Allocas;
}

MemoryArea TracedFunction::stackRestore(uintptr_t Key) {
  assert(!EventOffsetEnd && "Function has finished recording!");

  std::lock_guard<std::mutex> Lock(StackMutex);

  auto const &RestoreAllocas = StackSaves[Key];

  // Calculate invalidated memory area. This is the area occupied by all
  // allocas that are currently active, but will be removed by the restore.
  uintptr_t ClearLow = 0;
  uintptr_t ClearHigh = 0;

  for (std::size_t i = 0; i < Allocas.size(); ++i) {
    if (i >= RestoreAllocas.size() || Allocas[i] != RestoreAllocas[i]) {
      auto FirstArea = Allocas[i].area();
      auto FinalArea = Allocas.back().area();

      ClearLow = std::min(FirstArea.address(), FinalArea.address());
      ClearHigh = std::max(FirstArea.lastAddress(), FinalArea.lastAddress());

      break;
    }
  }

  // Restore saved allocas.
  Allocas = RestoreAllocas;

  return MemoryArea(ClearLow, (ClearHigh - ClearLow) + 1);
}

} // namespace trace (in seec)

} // namespace seec
<commit_msg>Update pointer object for byval argument pointers (because it will differ from the caller's pointer, but is valid).<commit_after>//===- lib/Trace/TracedFunction.cpp ---------------------------------------===//
//
//                                    SeeC
//
// This file is distributed under The MIT License (MIT). See LICENSE.TXT for
// details.
//
//===----------------------------------------------------------------------===//
///
/// \file
///
//===----------------------------------------------------------------------===//

#include "seec/Trace/TracedFunction.hpp"
#include "seec/Trace/TraceThreadListener.hpp"

#include "llvm/IR/Constants.h"
#include "llvm/IR/Instruction.h"

#define SEEC_DEBUG_PTROBJ 0

namespace seec {

namespace trace {

//===----------------------------------------------------------------------===//
// Support getCurrentRuntimeValue.
//===----------------------------------------------------------------------===//

uintptr_t
TracedFunction::getRuntimeAddress(llvm::GlobalVariable const *GV) const
{
  return ThreadListener.getRuntimeAddress(GV);
}

uintptr_t TracedFunction::getRuntimeAddress(llvm::Function const *F) const
{
  return ThreadListener.getRuntimeAddress(F);
}

//===----------------------------------------------------------------------===//
// Accessors for active-only information.
//===----------------------------------------------------------------------===//

seec::Maybe<MemoryArea>
TracedFunction::getContainingMemoryArea(uintptr_t Address) const
{
  std::lock_guard<std::mutex> Lock(StackMutex);

  if (Address < StackLow || Address > StackHigh) {
    // Not occupied by our stack, but may belong to a byval argument.
    for (auto const &Arg : ByValArgs) {
      if (Arg.getArea().contains(Address)) {
        return Arg.getArea();
      }
    }
  }
  else {
    // May be occupied by our stack.
    for (auto const &Alloca : Allocas) {
      auto AllocaArea = Alloca.area();
      if (AllocaArea.contains(Address)) {
        return AllocaArea;
      }
    }
  }

  return seec::Maybe<MemoryArea>();
}

//===----------------------------------------------------------------------===//
// byval argument memory area tracking.
//===----------------------------------------------------------------------===//

void TracedFunction::addByValArg(llvm::Argument const * const Arg,
                                 MemoryArea const &Area)
{
  assert(!EventOffsetEnd && "Function has finished recording!");

  ArgPointerObjects[Arg] = Area.start();

  std::lock_guard<std::mutex> Lock(StackMutex);

  ByValArgs.emplace_back(Arg, std::move(Area));
}

seec::Maybe<seec::MemoryArea>
TracedFunction::getParamByValArea(llvm::Argument const *Arg) const
{
  assert(!EventOffsetEnd && "Function has finished recording!");

  std::lock_guard<std::mutex> Lock(StackMutex);

  for (auto const &PBV : ByValArgs)
    if (PBV.getArgument() == Arg)
      return PBV.getArea();

  return seec::Maybe<seec::MemoryArea>();
}

//===----------------------------------------------------------------------===//
// Pointer origin tracking.
//===----------------------------------------------------------------------===//

uintptr_t TracedFunction::getPointerObject(llvm::Argument const *A) const
{
  auto const It = ArgPointerObjects.find(A);
  return It != ArgPointerObjects.end() ? It->second : 0;
}

void TracedFunction::setPointerObject(llvm::Argument const *A,
                                      uintptr_t const Object)
{
  ArgPointerObjects[A] = Object;
#if SEEC_DEBUG_PTROBJ
  llvm::errs() << "set ptr " << Object << " for argument " << *A << "\n";
#endif
}

uintptr_t TracedFunction::getPointerObject(llvm::Instruction const *I) const
{
  auto const It = PointerObjects.find(I);
  return It != PointerObjects.end() ? It->second : 0;
}

void TracedFunction::setPointerObject(llvm::Instruction const *I,
                                      uintptr_t const Object)
{
  PointerObjects[I] = Object;
#if SEEC_DEBUG_PTROBJ
  llvm::errs() << "set ptr " << Object << " for instruction " << *I << "\n";
#endif
}

uintptr_t TracedFunction::getPointerObject(llvm::Value const *V) const
{
  if (auto const I = llvm::dyn_cast<llvm::Instruction>(V))
    return getPointerObject(I);
  else if (auto const A = llvm::dyn_cast<llvm::Argument>(V))
    return getPointerObject(A);
  else if (auto const C = llvm::dyn_cast<llvm::Constant>(V)) {
    if (auto const CE = llvm::dyn_cast<llvm::ConstantExpr>(V)) {
      if (CE->isCast()) {
        return getPointerObject(CE->getOperand(0));
      }
    }
  }
  return ThreadListener.getProcessListener().getPointerObject(V);
}

uintptr_t TracedFunction::transferPointerObject(llvm::Value const *From,
                                                llvm::Instruction const *To)
{
  auto const Object = getPointerObject(From);
  if (Object)
    setPointerObject(To, Object);
  return Object;
}

uintptr_t TracedFunction::transferArgPointerObjectToCall(unsigned const ArgNo)
{
  auto const Call = llvm::dyn_cast<llvm::CallInst>(ActiveInstruction);
  assert(Call && "No CallInst active!");

  return transferPointerObject(Call->getArgOperand(ArgNo), Call);
}

//===----------------------------------------------------------------------===//
// Mutators.
//===----------------------------------------------------------------------===//

void TracedFunction::finishRecording(offset_uint EventOffsetEnd,
                                     uint64_t ThreadTimeExited) {
  assert(!this->EventOffsetEnd && "Function has finished recording!");

  std::lock_guard<std::mutex> Lock(StackMutex);

  this->EventOffsetEnd = EventOffsetEnd;
  this->ThreadTimeExited = ThreadTimeExited;

  // clear active-only information
  ActiveInstruction = nullptr;
  Allocas.clear();
  ByValArgs.clear();
  StackLow = 0;
  StackHigh = 0;
  CurrentValues.clear();
  ArgPointerObjects.clear();
  PointerObjects.clear();
}

void TracedFunction::addAlloca(TracedAlloca Alloca) {
  assert(!EventOffsetEnd && "Function has finished recording!");

  std::lock_guard<std::mutex> Lock(StackMutex);

  auto const &Area = Alloca.area();

  if (Area.address() < StackLow || !StackLow)
    StackLow = Area.address();

  if (Area.lastAddress() > StackHigh || !StackHigh)
    StackHigh = Area.lastAddress();

  Allocas.push_back(std::move(Alloca));
}

void TracedFunction::stackSave(uintptr_t Key) {
  assert(!EventOffsetEnd && "Function has finished recording!");

  std::lock_guard<std::mutex> Lock(StackMutex);

  StackSaves[Key] = Allocas;
}

MemoryArea TracedFunction::stackRestore(uintptr_t Key) {
  assert(!EventOffsetEnd && "Function has finished recording!");

  std::lock_guard<std::mutex> Lock(StackMutex);

  auto const &RestoreAllocas = StackSaves[Key];

  // Calculate invalidated memory area. This is the area occupied by all
  // allocas that are currently active, but will be removed by the restore.
  uintptr_t ClearLow = 0;
  uintptr_t ClearHigh = 0;

  for (std::size_t i = 0; i < Allocas.size(); ++i) {
    if (i >= RestoreAllocas.size() || Allocas[i] != RestoreAllocas[i]) {
      auto FirstArea = Allocas[i].area();
      auto FinalArea = Allocas.back().area();

      ClearLow = std::min(FirstArea.address(), FinalArea.address());
      ClearHigh = std::max(FirstArea.lastAddress(), FinalArea.lastAddress());

      break;
    }
  }

  // Restore saved allocas.
  Allocas = RestoreAllocas;

  return MemoryArea(ClearLow, (ClearHigh - ClearLow) + 1);
}

} // namespace trace (in seec)

} // namespace seec
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2017, Ubiquity Robotics
 * 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.
 *
 * 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.
 *
 * The views and conclusions contained in the software and documentation are
 * those of the authors and should not be interpreted as representing official
 * policies, either expressed or implied, of the FreeBSD Project.
 *
 */

#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>

#include <list>
#include <string>

using namespace std;

// Radians to degrees

static double rad2deg(double rad)
{
    return rad * 180.0 / M_PI;
}

// retreive the 3 DOF we are interested in from a Transform

static void getPose(const tf2::Transform& T, double& x, double& y, double& yaw)
{
    tf2::Vector3 trans = T.getOrigin();
    x = trans.x();
    y = trans.y();

    double roll, pitch;
    T.getBasis().getRPY(roll, pitch, yaw);
}

class MoveBasic {
  private:
    ros::Subscriber goalSub;
    ros::Publisher cmdPub;

    double angularVelocity;
    double angularTolerance;

    double linearVelocity;
    double linearTolerance;

    tf2_ros::Buffer tfBuffer;
    tf2_ros::TransformListener listener;

    tf2::Transform goalOdom;
    bool haveGoal;

    void goalCallback(const geometry_msgs::PoseStamped::ConstPtr &msg);
    void sendCmd(double angular, double linear);

    bool getTransform(const std::string& from, const std::string& to,
                      tf2::Transform& tf);

  public:
    MoveBasic(ros::NodeHandle &nh);

    void run();

    bool handleRotation();
    bool handleLinear();
};

// Constructor

MoveBasic::MoveBasic(ros::NodeHandle &nh): tfBuffer(ros::Duration(30.0)),
                                           listener(tfBuffer), haveGoal(false)
{
    nh.param<double>("angular_velocity", angularVelocity, 0.3);
    nh.param<double>("angular_tolerance", angularTolerance, 0.01);

    nh.param<double>("linear_velocity", linearVelocity, 0.4);
    nh.param<double>("linear_tolerance", linearTolerance, 0.01);

    cmdPub = ros::Publisher(nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1));

    goalSub = nh.subscribe("/move_base_simple/goal", 1,
                            &MoveBasic::goalCallback, this);

    ROS_INFO("Move Basic ready");
}

// Lookup the specified transform, returns true on success

bool MoveBasic::getTransform(const std::string& from, const std::string& to,
                             tf2::Transform& tf)
{
    try {
        geometry_msgs::TransformStamped tfs =
            tfBuffer.lookupTransform(to, from, ros::Time(0));
        tf2::fromMsg(tfs.transform, tf);
        return true;
    }
    catch (tf2::TransformException &ex) {
         ROS_WARN("%s", ex.what());
         return false;
    }
}

// Called when a simple goal message is received

void MoveBasic::goalCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    tf2::Transform goal;
    tf2::fromMsg(msg->pose, goal);
    string frameId = msg->header.frame_id;

    double x, y, yaw;
    getPose(goal, x, y, yaw);
    ROS_INFO("Received goal %f %f %f", x, y, rad2deg(yaw));

    tf2::Transform tfMapOdom;
    if (!getTransform(frameId, "odom", tfMapOdom)) {
        ROS_WARN("Cannot determine robot pose");
        return;
    }
    goalOdom = tfMapOdom * goal;

    //goalOdom = goal;
    getPose(goalOdom, x, y, yaw);
    ROS_INFO("Goal in odom  %f %f %f", x, y, rad2deg(yaw));
    haveGoal = true;
}

// Send a motion command

void MoveBasic::sendCmd(double angular, double linear)
{
   geometry_msgs::Twist msg;
   msg.angular.z = angular;
   msg.linear.x = linear;

   cmdPub.publish(msg);
}

// Main loop

void MoveBasic::run()
{
    ros::Rate r(20);
    while (ros::ok()) {
        ros::spinOnce();
        r.sleep();

        if (haveGoal) {
            haveGoal = false;
            handleRotation();
            handleLinear();
        }
    }
}

// Do angular part of goal

bool MoveBasic::handleRotation()
{
    bool done = false;
    ros::Rate r(50);

    while (!done && ros::ok()) {
        ros::spinOnce();
        r.sleep();

        tf2::Transform poseOdom;
        if (!getTransform("base_link", "odom", poseOdom)) {
             ROS_WARN("Cannot determine robot pose for rotation");
             continue;
        }

        tf2::Vector3 linear = goalOdom.getOrigin() - poseOdom.getOrigin();
        double requestedYaw = atan2(linear.y(), linear.x());

        double x, y, currentYaw;
        getPose(poseOdom, x, y, currentYaw);

        double demand = requestedYaw - currentYaw;
        if (demand < -M_PI) demand += 2 * M_PI;
        if (demand > M_PI) demand -= 2 * M_PI;

        double velocity = 0;
        if (demand < 0) {
            velocity = -angularVelocity;
        }
        else {
            velocity = angularVelocity;
        }

        if (haveGoal) { // new goal received
            ROS_INFO("Stopping rotation due to new goal");
            done = true;
            velocity = 0;
        }
//        ROS_INFO("Demand %f %f %f", rad2deg(demand), demand, velocity);
        if (fabs(demand) < angularTolerance) {
            velocity = 0;
            done = true;
            ROS_INFO("Done rotation, error %f radians %f degrees", demand, rad2deg(demand));
        }
        sendCmd(velocity, 0);
    }
    return done;
}

// Do linear part of goal

bool MoveBasic::handleLinear()
{
    bool done = false;
    ros::Rate r(50);

    tf2::Transform poseOdomInitial;
    if (!getTransform("base_link", "odom", poseOdomInitial)) {
         ROS_WARN("Cannot determine robot pose for linrar");
         return false;
    }

    tf2::Vector3 linear = goalOdom.getOrigin() - poseOdomInitial.getOrigin();
    double requestedDist = linear.length();;
    ROS_INFO("Requested distance %f", requestedDist);

    while (!done && ros::ok()) {
        ros::spinOnce();
        r.sleep();

        tf2::Transform poseOdom;
        if (!getTransform("base_link", "odom", poseOdom)) {
             ROS_WARN("Cannot determine robot pose for linear");
             continue;
        }

        tf2::Vector3 travelled = poseOdomInitial.getOrigin() - poseOdom.getOrigin();
        double distTravelled = travelled.length();;

        double demand = requestedDist - distTravelled;
        double velocity = linearVelocity;

        if (haveGoal) { // new goal received
            ROS_INFO("Stopping rotation due to new goal");
            done = true;
            velocity = 0;
        }

//        ROS_INFO("Demand %f %f", requestedDist-distTravelled, velocity);

        if (distTravelled > requestedDist - linearTolerance) {
            velocity = 0;
            done = true;
            ROS_INFO("Done linear, error %f meters", demand);
        }
        sendCmd(0, velocity);
    }
    return done;
}

int main(int argc, char ** argv) {
    ros::init(argc, argv, "move_basic");
    ros::NodeHandle nh("~");
    MoveBasic node(nh);
    node.run();

    return 0;
}
<commit_msg>Address Rohan's comments: final rotation, std::abs(), publish path<commit_after>/*
 * Copyright (c) 2017, Ubiquity Robotics
 * 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.
 *
 * 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.
 *
 * The views and conclusions contained in the software and documentation are
 * those of the authors and should not be interpreted as representing official
 * policies, either expressed or implied, of the FreeBSD Project.
 *
 */

#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Path.h>

#include <list>
#include <string>

class MoveBasic {
  private:
    ros::Subscriber goalSub;
    ros::Publisher cmdPub;
    ros::Publisher pathPub;

    double angularVelocity;
    double angularTolerance;

    double linearVelocity;
    double linearTolerance;

    tf2_ros::Buffer tfBuffer;
    tf2_ros::TransformListener listener;

    tf2::Transform goalOdom;
    bool haveGoal;

    void goalCallback(const geometry_msgs::PoseStamped::ConstPtr &msg);
    void sendCmd(double angular, double linear);

    bool getTransform(const std::string& from, const std::string& to,
                      tf2::Transform& tf);

    bool handleRotation();
    bool handleLinear();

  public:
    MoveBasic(ros::NodeHandle &nh);

    void run();

    bool moveLinear(double requestedDistance);
    bool rotateTo(double requestedYaw);
    bool rotateRel(double yaw);
};

// Radians to degrees

static double rad2deg(double rad)
{
    return rad * 180.0 / M_PI;
}

// Adjust angle to be between -PI and PI

static void normalizeAngle(double& angle)
{
    if (angle < -M_PI) {
         angle += 2 * M_PI;
    }
    if (angle > M_PI) {
        angle -= 2 * M_PI;
    }
}

// retreive the 3 DOF we are interested in from a Transform

static void getPose(const tf2::Transform& tf, double& x, double& y, double& yaw)
{
    tf2::Vector3 trans = tf.getOrigin();
    x = trans.x();
    y = trans.y();

    double roll, pitch;
    tf.getBasis().getRPY(roll, pitch, yaw);
}

// Constructor

MoveBasic::MoveBasic(ros::NodeHandle &nh): tfBuffer(ros::Duration(30.0)),
                                           listener(tfBuffer), haveGoal(false)
{
    nh.param<double>("angular_velocity", angularVelocity, 0.3);
    nh.param<double>("angular_tolerance", angularTolerance, 0.01);

    nh.param<double>("linear_velocity", linearVelocity, 0.4);
    nh.param<double>("linear_tolerance", linearTolerance, 0.01);

    cmdPub = ros::Publisher(nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1));

    pathPub = ros::Publisher(nh.advertise<nav_msgs::Path>("/plan", 1));

    goalSub = nh.subscribe("/move_base_simple/goal", 1,
                            &MoveBasic::goalCallback, this);

    ROS_INFO("Move Basic ready");
}

// Lookup the specified transform, returns true on success

bool MoveBasic::getTransform(const std::string& from, const std::string& to,
                             tf2::Transform& tf)
{
    try {
        geometry_msgs::TransformStamped tfs =
            tfBuffer.lookupTransform(to, from, ros::Time(0));
        tf2::fromMsg(tfs.transform, tf);
        return true;
    }
    catch (tf2::TransformException &ex) {
         ROS_WARN("%s", ex.what());
         return false;
    }
}

// Called when a simple goal message is received

void MoveBasic::goalCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    tf2::Transform goal;
    tf2::fromMsg(msg->pose, goal);
    std::string frameId = msg->header.frame_id;

    double x, y, yaw;
    getPose(goal, x, y, yaw);
    ROS_INFO("Received goal %f %f %f", x, y, rad2deg(yaw));

    tf2::Transform tfMapOdom;
    if (!getTransform(frameId, "odom", tfMapOdom)) {
        ROS_WARN("Cannot determine robot pose");
        return;
    }
    goalOdom = tfMapOdom * goal;

    getPose(goalOdom, x, y, yaw);
    ROS_INFO("Goal in odom  %f %f %f", x, y, rad2deg(yaw));
    haveGoal = true;

    nav_msgs::Path path;
    geometry_msgs::PoseStamped p0, p1;
    path.header.frame_id = "odom";
    p0.pose.position.x = x;
    p0.pose.position.y = y;
    path.poses.push_back(p0);

    tf2::Transform poseOdom;
    if (!getTransform("base_link", "odom", poseOdom)) {
         ROS_WARN("Cannot determine robot pose for rotation");
         return;
    }
    getPose(poseOdom, x, y, yaw);
    p1.pose.position.x = x;
    p1.pose.position.y = y;
    path.poses.push_back(p1);

    pathPub.publish(path);
}

// Send a motion command

void MoveBasic::sendCmd(double angular, double linear)
{
   geometry_msgs::Twist msg;
   msg.angular.z = angular;
   msg.linear.x = linear;

   cmdPub.publish(msg);
}

// Main loop

void MoveBasic::run()
{
    ros::Rate r(20);
    while (ros::ok()) {
        ros::spinOnce();
        r.sleep();

        if (haveGoal) {
            haveGoal = false;
            if (!handleRotation()) {
                continue;
            }
            if (!handleLinear()) {
                continue;
            }
            double x, y, yaw;
            getPose(goalOdom, x, y, yaw);
            rotateTo(yaw);
        }
    }
}

// Do angular part of goal

bool MoveBasic::handleRotation()
{
    tf2::Transform poseOdom;
    if (!getTransform("base_link", "odom", poseOdom)) {
         ROS_WARN("Cannot determine robot pose for rotation");
         return false;
    }

    tf2::Vector3 linear = goalOdom.getOrigin() - poseOdom.getOrigin();
    double requestedYaw = atan2(linear.y(), linear.x());

    return rotateTo(requestedYaw);
}

// Rotate relative to current orientation

bool MoveBasic::rotateRel(double yaw)
{
    tf2::Transform poseOdom;
    if (!getTransform("base_link", "odom", poseOdom)) {
         ROS_WARN("Cannot determine robot pose for rotation");
         return false;
    }

    double x, y, currentYaw;
    getPose(poseOdom, x, y, currentYaw);
    double requestedYaw = currentYaw + yaw;
    normalizeAngle(requestedYaw);

    return rotateTo(requestedYaw);
}

// Rotate to specified orientation (in radians)

bool MoveBasic::rotateTo(double requestedYaw)
{
    bool done = false;
    ros::Rate r(50);

    while (!done && ros::ok()) {
        ros::spinOnce();
        r.sleep();

        double x, y, currentYaw;
        tf2::Transform poseOdom;
        if (!getTransform("base_link", "odom", poseOdom)) {
             ROS_WARN("Cannot determine robot pose for rotation");
             return false;
        }
        getPose(poseOdom, x, y, currentYaw);

        double demand = requestedYaw - currentYaw;
        normalizeAngle(demand);

        double velocity = 0;
        if (demand < 0) {
            velocity = -angularVelocity;
        }
        else {
            velocity = angularVelocity;
        }

        if (haveGoal) { // new goal received
            ROS_INFO("Stopping rotation due to new goal");
            done = true;
            velocity = 0;
        }
//        ROS_INFO("Demand %f %f %f", rad2deg(demand), demand, velocity);
        if (std::abs(demand) < angularTolerance) {
            velocity = 0;
            done = true;
            ROS_INFO("Done rotation, error %f radians %f degrees", demand, rad2deg(demand));
        }
        sendCmd(velocity, 0);
    }
    return done;
}

// Do linear part of goal

bool MoveBasic::handleLinear()
{
    bool done = false;

    tf2::Transform poseOdomInitial;
    if (!getTransform("base_link", "odom", poseOdomInitial)) {
         ROS_WARN("Cannot determine robot pose for linrar");
         return false;
    }

    tf2::Vector3 linear = goalOdom.getOrigin() - poseOdomInitial.getOrigin();
    double requestedDistance = linear.length();;
    ROS_INFO("Requested distance %f", requestedDistance);

    return moveLinear(requestedDistance);
}

// Move foreward specified distance

bool MoveBasic::moveLinear(double requestedDistance)
{
    bool done = false;
    ros::Rate r(50);

    tf2::Transform poseOdomInitial;
    if (!getTransform("base_link", "odom", poseOdomInitial)) {
         ROS_WARN("Cannot determine robot pose for linrar");
         return false;
    }

    while (!done && ros::ok()) {
        ros::spinOnce();
        r.sleep();

        tf2::Transform poseOdom;
        if (!getTransform("base_link", "odom", poseOdom)) {
             ROS_WARN("Cannot determine robot pose for linear");
             continue;
        }

        tf2::Vector3 travelled = poseOdomInitial.getOrigin() - poseOdom.getOrigin();
        double distTravelled = travelled.length();;

        double demand = requestedDistance - distTravelled;
        double velocity = linearVelocity;

        if (haveGoal) { // new goal received
            ROS_INFO("Stopping rotation due to new goal");
            done = true;
            velocity = 0;
        }

//        ROS_INFO("Demand %f %f", requestedDist-distTravelled, velocity);

        if (distTravelled > requestedDistance - linearTolerance) {
            velocity = 0;
            done = true;
            ROS_INFO("Done linear, error %f meters", demand);
        }
        sendCmd(0, velocity);
    }
    return done;
}

int main(int argc, char ** argv) {
    ros::init(argc, argv, "move_basic");
    ros::NodeHandle nh("~");
    MoveBasic node(nh);
    node.run();

    return 0;
}
<|endoftext|>


<commit_before>/*********************************/
/* Motion flow computation class */
/*********************************/
#include "nebulaFlow.h"

void nebulaFlow::setup(){
    // setup flow parameter GUI
  guiGrp.setName("Motion flow parameters");
  guiGrp.add(enabled.set("enable",true));
  guiGrp.add(forceCPU.set("Force CPU", false));
  guiGrp.add(fbPyrScale.set("fbPyrScale", .5, 0, .99));
  guiGrp.add(fbLevels.set("fbLevels", 4, 1, 8));
  guiGrp.add(fbIterations.set("fbIterations", 2, 1, 8));
  guiGrp.add(fbPolyN.set("fbPolyN", 7, 5, 10));
  guiGrp.add(fbPolySigma.set("fbPolySigma", 1.5, 1.1, 2));
  guiGrp.add(fbUseGaussian.set("fbUseGaussian", false));
  guiGrp.add(fbWinSize.set("winSize", 32, 4, 64));

  // OpenCL setup
  cv::initModule_video();
  cv::setUseOptimized(true);
  cv::setNumThreads(8);

  cv::ocl::DevicesInfo devicesInfo;
  cv::ocl::getOpenCLDevices(devicesInfo);
  ofLogNotice("nebulaFlow") << "Found " << devicesInfo.size() << " OpenCL device(s).";
  for ( size_t i = 0; i < devicesInfo.size(); i++){
    ofLogNotice("nebulaFlow") << devicesInfo[i]->deviceVendor << " " << devicesInfo[i];
  }

  if ( devicesInfo.size() == 0 ){
    ofLogNotice("nebulaFlow") << "can't find OpenCL device, switch to CPU mode";
    gpuMode = false;
  } else {
    gpuMode = true;
  }
}

void nebulaFlow::update(ofPixels &img){
  if(!enabled) return;

  if ( gpuMode ){
    oclFbFlow.pyrScale=fbPyrScale;
    oclFbFlow.numLevels=fbLevels;
    oclFbFlow.winSize=fbWinSize;
    oclFbFlow.numIters=fbIterations;
    oclFbFlow.polyN=fbPolyN;
    oclFbFlow.polySigma=fbPolySigma;

    cv::Mat input = ofxCv::toCv(img), gray;
    if(input.channels() == 1){
      gray = input;
    } else {
        cv::cvtColor(input, gray, CV_RGB2GRAY);
    }
    d_input = gray;
    if ( m_flow.size()  != d_input.size() ) m_flow.create(d_input.size(), CV_32FC2);
    if ( d_flow.size()  != d_input.size() ) d_flow.create(d_input.size(), CV_32FC2);
    if ( d_prev.size()  != d_input.size() ) d_prev.create(d_input.size(), CV_8UC1);
    if ( d_flowx.size() != d_input.size() ) d_flowx.create(d_input.size(), CV_32FC1);
    if ( d_flowy.size() != d_input.size() ) d_flowy.create(d_input.size(), CV_32FC1);
    oclFbFlow(d_prev, d_input, d_flowx, d_flowy);
    const vector<cv::ocl::oclMat> matVec = { d_flowx, d_flowy };
    cv::ocl::merge(matVec, d_flow);
    d_flow.download(m_flow);
    d_input.copyTo(d_prev); // TODO use copyTo to apply mask
  } else {
    flow.setPyramidScale(fbPyrScale);
    flow.setNumLevels(fbLevels);
    flow.setWindowSize(fbWinSize);
    flow.setNumIterations(fbIterations);
    flow.setPolyN(fbPolyN);
    flow.setPolySigma(fbPolySigma);
    flow.setUseGaussian(fbUseGaussian);

    flow.calcOpticalFlow(img);
  }
}

void nebulaFlow::draw(int x, int y, int w, int h){
  if(!enabled) return;
  flow.draw(x,y,w,h);
  if ( gpuMode )
    ofDrawBitmapStringHighlight("GPU Mode", ofPoint(x+10,y+10), ofColor(255,0,0), ofColor(255,255,255));
}
<commit_msg>draw GPU flow<commit_after>/*********************************/
/* Motion flow computation class */
/*********************************/
#include "nebulaFlow.h"

void nebulaFlow::setup(){
    // setup flow parameter GUI
  guiGrp.setName("Motion flow parameters");
  guiGrp.add(enabled.set("enable",true));
  guiGrp.add(forceCPU.set("Force CPU", false));
  guiGrp.add(fbPyrScale.set("fbPyrScale", .5, 0, .99));
  guiGrp.add(fbLevels.set("fbLevels", 4, 1, 8));
  guiGrp.add(fbIterations.set("fbIterations", 2, 1, 8));
  guiGrp.add(fbPolyN.set("fbPolyN", 7, 5, 10));
  guiGrp.add(fbPolySigma.set("fbPolySigma", 1.5, 1.1, 2));
  guiGrp.add(fbUseGaussian.set("fbUseGaussian", false));
  guiGrp.add(fbWinSize.set("winSize", 32, 4, 64));

  // OpenCL setup
  cv::initModule_video();
  cv::setUseOptimized(true);
  cv::setNumThreads(8);

  cv::ocl::DevicesInfo devicesInfo;
  cv::ocl::getOpenCLDevices(devicesInfo);
  ofLogNotice("nebulaFlow") << "Found " << devicesInfo.size() << " OpenCL device(s).";
  for ( size_t i = 0; i < devicesInfo.size(); i++){
    ofLogNotice("nebulaFlow") << devicesInfo[i]->deviceVendor << " " << devicesInfo[i];
  }

  if ( devicesInfo.size() == 0 ){
    ofLogNotice("nebulaFlow") << "can't find OpenCL device, switch to CPU mode";
    gpuMode = false;
  } else {
    gpuMode = true;
  }
}

void nebulaFlow::update(ofPixels &img){
  if(!enabled) return;

  if ( gpuMode ){
    oclFbFlow.pyrScale=fbPyrScale;
    oclFbFlow.numLevels=fbLevels;
    oclFbFlow.winSize=fbWinSize;
    oclFbFlow.numIters=fbIterations;
    oclFbFlow.polyN=fbPolyN;
    oclFbFlow.polySigma=fbPolySigma;

    cv::Mat input = ofxCv::toCv(img), gray;
    if(input.channels() == 1){
      gray = input;
    } else {
        cv::cvtColor(input, gray, CV_RGB2GRAY);
    }
    d_input = gray;
    if ( m_flow.size()  != d_input.size() ) m_flow.create(d_input.size(), CV_32FC2);
    if ( d_flow.size()  != d_input.size() ) d_flow.create(d_input.size(), CV_32FC2);
    if ( d_prev.size()  != d_input.size() ) d_prev.create(d_input.size(), CV_8UC1);
    if ( d_flowx.size() != d_input.size() ) d_flowx.create(d_input.size(), CV_32FC1);
    if ( d_flowy.size() != d_input.size() ) d_flowy.create(d_input.size(), CV_32FC1);
    oclFbFlow(d_prev, d_input, d_flowx, d_flowy);
    const vector<cv::ocl::oclMat> matVec = { d_flowx, d_flowy };
    cv::ocl::merge(matVec, d_flow);
    d_flow.download(m_flow);
    d_input.copyTo(d_prev); // TODO use copyTo to apply mask
  } else {
    flow.setPyramidScale(fbPyrScale);
    flow.setNumLevels(fbLevels);
    flow.setWindowSize(fbWinSize);
    flow.setNumIterations(fbIterations);
    flow.setPolyN(fbPolyN);
    flow.setPolySigma(fbPolySigma);
    flow.setUseGaussian(fbUseGaussian);

    flow.calcOpticalFlow(img);
  }
}

void nebulaFlow::draw(int x, int y, int w, int h){
  if(!enabled) return;
  if ( gpuMode ){
    ofDrawBitmapStringHighlight("GPU Mode", ofPoint(x+10,y+10), ofColor(255,0,0), ofColor(255,255,255));
      ofRectangle rect(x,y,w,h);
      ofVec2f offset(rect.x,rect.y);
      ofVec2f scale(rect.width/m_flow.cols, rect.height/m_flow.rows);
      int stepSize = 4; //TODO: make class-level parameteric
      for(int y = 0; y < m_flow.rows; y += stepSize) {
          for(int x = 0; x < m_flow.cols; x += stepSize) {
              ofVec2f cur = ofVec2f(x, y) * scale + offset;
              const cv::Vec2f& vec = m_flow.at<cv::Vec2f>(y, x);
              ofDrawLine(cur, ofVec2f(x + vec[0], y + vec[1]) * scale + offset);
          }
      }
  } else {
    flow.draw(x,y,w,h);
  }
}
<|endoftext|>


<commit_before>#include <vector>
#include <cfloat>
#include <png++/png.hpp>
#include <mpi.h>

#include "camera.hpp"
#include "color.hpp"
#include "ray.hpp"
#include "ray_tracer.hpp"
#include "shape.hpp"
#include "sphere.hpp"

using V2 = Eigen::Vector2f;

#define EPSILON 0.00001
#define NOTHING_TO_SEND 32767

// Helper
static void gen_rand_samples(int bins_per_side, std::vector<V2>& points);

Ray_Tracer::Ray_Tracer(const Camera& cam, unsigned int x_res, unsigned int y_res,
              Color background_col, unsigned int sample_bins, unsigned int depth_limit) {
    this->cam = cam;
    this->x_res = x_res;
    this->y_res = y_res;
    image_buf.resize(x_res);
    for (std::vector<Color>& y_buf: image_buf) {
        y_buf.resize(y_res, Color(0,0,0));
    }

    this->background_col = background_col;
    this->sample_bins = sample_bins;
    this->depth_limit = depth_limit;
}

Ray_Tracer& Ray_Tracer::operator=(const Ray_Tracer& rt) {

    this->cam = rt.cam;
    this->x_res = rt.x_res;
    this->y_res = rt.y_res;
    image_buf.resize(x_res);
    for (std::vector<Color>& y_buf: image_buf)
        y_buf.resize(y_res);

    this->background_col = rt.background_col;
    this->sample_bins = rt.sample_bins;
    this->depth_limit = rt.depth_limit;

    return *this;
}

void Ray_Tracer::add_shape(Shape* shape) {
    shapes.push_back(shape);
    if (shape->is_light)
        lights.push_back(shape);
}

void Ray_Tracer::add_light(Sphere* sphere) {
    shapes.push_back(sphere);
    if (sphere->is_light)
        lights.push_back(sphere);
}

void Ray_Tracer::free_shapes() {
    for (Shape* shape : shapes)
        delete shape;
}

bool Ray_Tracer::trace(const Ray& r, float t_min, float t_max,
                        Ray_Hit& rh, bool shadow) const {
    bool hit_once = false;
    Ray r2 = r;
    return kd->hit_local(r2, this, t_min, t_max, rh, shadow);
    return hit_once;
}

void Ray_Tracer::worker() {

    MPI_Barrier(MPI_COMM_WORLD);
    std::stack<Ray_Trace> rts;
    std::vector<Ray_Hit_Remote> rhrs;
    while (true) {
        MPI_Status status;
        int flag;
        MPI_Iprobe(MPI_ANY_SOURCE, MPI_ANY_TAG, MPI_COMM_WORLD, &flag, &status);
        if (flag) {
            Ray_Trace rt;
            MPI_Recv(&rt, sizeof(Ray_Trace), MPI_BYTE, MPI_ANY_SOURCE,
                         MPI_ANY_TAG, MPI_COMM_WORLD, &status);

            if (rt.t_min == -1 && rt.t_max == -1)
                break;

            rts.push(rt);

        // Removed because interleaving comms and work is slow
        //    continue;
        } /*else {
            if (!rts.empty()) {
                Ray_Trace rt = rts.top();
                rts.pop();
                Ray_Hit rh;
                Ray_Hit_Remote rhr;
                if (!(kd->hit_local(rt.r, this, rt.t_min, rt.t_max, rh, false))) {
                    rhr.t = FLT_MAX;
                } else {
                    rhr.t = rh.t;
                    rhr.col = rh.col;
                    rhr.ray_id = rt.ray_id;
                    rhrs.push_back(rhr);
                }
            }
        }
        */
    }

    std::cout << "Just processing now" << std::endl;

    // Process remaining rays
    while (!rts.empty()) {
        Ray_Trace rt = rts.top();
        rts.pop();
        Ray_Hit rh;
        Ray_Hit_Remote rhr;
        if (!(kd->hit_local(rt.r, this, rt.t_min, rt.t_max, rh, false))) {
            rhr.t = FLT_MAX;
        } else {
            rhr.t = rh.t;
            rhr.col = rh.col;
            rhr.ray_id = rt.ray_id;
            rhrs.push_back(rhr);
        }
    }

    std::cout << "Done processing, gonna send now" << std::endl;

    if (!rhrs.empty()) {
        // Send back ray info
        MPI_Ssend(rhrs.data(), sizeof(rhrs[0]) * rhrs.size(),
                    MPI_BYTE, 0, 0, MPI_COMM_WORLD);
    } else {
        // Send a message staying we're done, but have nothing to sedn
        unsigned int done = 0xFFFFFFFF;
        MPI_Ssend(&done, 1, MPI_INT, 0, NOTHING_TO_SEND, MPI_COMM_WORLD);
    }

/*
    MPI_Barrier(MPI_COMM_WORLD);
    while (true) {
        Ray_Trace rt;
        MPI_Status status;
        MPI_Recv(&rt, sizeof(Ray_Trace), MPI_BYTE, MPI_ANY_SOURCE,
                     MPI_ANY_TAG, MPI_COMM_WORLD, &status);

        if (rt.t_min == -1 && rt.t_max == -1)
            break;

        Ray_Hit rh;
        Ray_Hit_Remote rhr;
        if (!(kd->hit_local(rt.r, this, rt.t_min, rt.t_max, rh, false))) {
            rhr.t = FLT_MAX;
        } else {
            rhr.t = rh.t;
        }

        rhr.col = rh.col;

        MPI_Send(&rhr, sizeof(Ray_Hit_Remote), MPI_BYTE, status.MPI_SOURCE,
                    status.MPI_TAG, MPI_COMM_WORLD);
    }
*/
}

void Ray_Tracer::trace_all(int rank, int np) {

    std::cout << "hi" << std::endl;
    if (rank != 0) {
        worker();
        return;
    }

    std::vector<V2> points;
    MPI_Barrier(MPI_COMM_WORLD);
    for (unsigned int i = 0; i < x_res; ++i) {
        for (unsigned int j = 0; j < x_res; ++j) {
            points.clear();
            gen_rand_samples(sample_bins, points);

            Color total;
            for (unsigned int k = 0; k < points.size(); ++k) {
                V2& p = points[k];
                Ray r = cam.make_ray((i + p.x() + 0.5)/x_res, (j + p.y() + 0.5)/y_res);
                local_rays.push(r);
                local_dest.push(Int_pair(i, j));
                Ray_Hit rh;
                unsigned int ray_id = i << 19 | j << 6 | k;
                kd->hit(r, this, EPSILON, FLT_MAX, rh, false, ray_id);
                //trace(r, EPSILON, FLT_MAX, rh, false);
                    //total += rh.col;
            }
//            image_buf[i][j] = total / points.size();
        }
    }

    // Tell all workers to start processing
    Ray_Trace rt;
    Ray r;
    rt.r = r; // dummy
    rt.t_min = -1;
    rt.t_max = -1;
    rt.ray_id = 0; // dummy
    std::cout << "Done sending Rays" << std::endl;
    for (int i = 1; i < np; ++i) {
        MPI_Ssend(&rt, sizeof(Ray_Trace), MPI_BYTE, i, 0, MPI_COMM_WORLD);
    }

    std::map<unsigned int, Ray_Hit_Remote> rhrs;

    int procs_to_recv = np - 1;
    while (procs_to_recv > 0) {
        MPI_Status status;
        MPI_Probe(MPI_ANY_SOURCE, MPI_ANY_TAG, MPI_COMM_WORLD, &status);
        if (status.MPI_TAG == NOTHING_TO_SEND) {
            int dummy;
            MPI_Recv(&dummy, 1, MPI_INT, MPI_ANY_SOURCE,
                        MPI_ANY_TAG, MPI_COMM_WORLD, &status);
            procs_to_recv--;
            continue;
        } else {
            int byte_count;
            MPI_Get_count(&status, MPI_BYTE, &byte_count);
            int elements = byte_count / sizeof(Ray_Hit_Remote);
            Ray_Hit_Remote* rhr = new Ray_Hit_Remote[elements];
            MPI_Recv(rhr, byte_count, MPI_BYTE, status.MPI_SOURCE,
                        status.MPI_TAG, MPI_COMM_WORLD, &status);

            for (int i = 0; i < elements; ++i) {
                std::pair<unsigned int, Ray_Hit_Remote> p;
                p.first = rhr[i].ray_id;
                p.second = rhr[i];
                auto res = rhrs.insert(p);
                if (!res.second) { // already exists
                    if (res.first->second.t > rhr[i].t)
                        res.first->second = rhr[i];
                }
            }
            procs_to_recv--;
        }
    }

    int samples = points.size();
    for (auto& r : rhrs) {
        int x = r.first >> 19;
        int y = (r.first >> 6) & 0x00001FFF;
        image_buf[x][y] += r.second.col/ samples;
    }

    /*
    Ray r;
    Ray_Hit rh;
    Int_pair dest;
    int samples = sample_bins*sample_bins;
    omp_set_num_threads(1);
    #pragma omp parallel
    {
    #pragma omp single private(r, dest, rh)
    {
    std::cout << "hi" << std::endl;
    while (!(local_rays.empty())) {
            r = local_rays.top();
            local_rays.pop();
            dest = local_dest.top();
            local_dest.pop();
        #pragma omp task firstprivate(r, dest, rh)
        {
        if (trace(r, EPSILON, FLT_MAX, rh, false)) {
            image_buf[dest.first][dest.second] += rh.col / samples;
        }
        }
    }
    }
    }
    */

    return;

}

void Ray_Tracer::write_buffer(std::string filename) {
    png::image< png::rgb_pixel > ray_image(x_res, y_res);
    for (unsigned int i = 0; i < x_res; ++i) {
        for (unsigned int j = 0; j < y_res; ++j) {
            ray_image[i][j] = image_buf[i][j].to_png_pixel();
        }
    }
    ray_image.write(filename);
}

static void gen_rand_samples(int bins_per_side, std::vector<V2>& points) {
    static std::random_device rd;
    static std::mt19937 gen(rd());
    static std::uniform_real_distribution<> dis(0, 1);

    float bin_length = 1.0f / bins_per_side;

    float bin_x = 0.0;
    float bin_y = 0.0;

    for (int i = 0; i < bins_per_side; ++i) {
        for (int j = 0; j < bins_per_side; ++j) {
            float rand_num_unit = dis(gen);
            bin_x = i * bin_length + bin_length*rand_num_unit - 0.5;
            rand_num_unit = dis(gen);
            bin_y = j * bin_length + bin_length*rand_num_unit - 0.5;
            points.push_back(V2(bin_x, bin_y));
        }
    }
}
<commit_msg>fixed bug where tracing y range is set wrong, this bug doesnt show when tracing a square image<commit_after>#include <vector>
#include <cfloat>
#include <png++/png.hpp>
#include <mpi.h>

#include "camera.hpp"
#include "color.hpp"
#include "ray.hpp"
#include "ray_tracer.hpp"
#include "shape.hpp"
#include "sphere.hpp"

using V2 = Eigen::Vector2f;

#define EPSILON 0.00001
#define NOTHING_TO_SEND 32767

// Helper
static void gen_rand_samples(int bins_per_side, std::vector<V2>& points);

Ray_Tracer::Ray_Tracer(const Camera& cam, unsigned int x_res, unsigned int y_res,
              Color background_col, unsigned int sample_bins, unsigned int depth_limit) {
    this->cam = cam;
    this->x_res = x_res;
    this->y_res = y_res;
    image_buf.resize(x_res);
    for (std::vector<Color>& y_buf: image_buf) {
        y_buf.resize(y_res, Color(0,0,0));
    }

    this->background_col = background_col;
    this->sample_bins = sample_bins;
    this->depth_limit = depth_limit;
}

Ray_Tracer& Ray_Tracer::operator=(const Ray_Tracer& rt) {

    this->cam = rt.cam;
    this->x_res = rt.x_res;
    this->y_res = rt.y_res;
    image_buf.resize(x_res);
    for (std::vector<Color>& y_buf: image_buf)
        y_buf.resize(y_res);

    this->background_col = rt.background_col;
    this->sample_bins = rt.sample_bins;
    this->depth_limit = rt.depth_limit;

    return *this;
}

void Ray_Tracer::add_shape(Shape* shape) {
    shapes.push_back(shape);
    if (shape->is_light)
        lights.push_back(shape);
}

void Ray_Tracer::add_light(Sphere* sphere) {
    shapes.push_back(sphere);
    if (sphere->is_light)
        lights.push_back(sphere);
}

void Ray_Tracer::free_shapes() {
    for (Shape* shape : shapes)
        delete shape;
}

bool Ray_Tracer::trace(const Ray& r, float t_min, float t_max,
                        Ray_Hit& rh, bool shadow) const {
    bool hit_once = false;
    Ray r2 = r;
    return kd->hit_local(r2, this, t_min, t_max, rh, shadow);
    return hit_once;
}

void Ray_Tracer::worker() {

    MPI_Barrier(MPI_COMM_WORLD);
    std::stack<Ray_Trace> rts;
    std::vector<Ray_Hit_Remote> rhrs;
    while (true) {
        MPI_Status status;
        int flag;
        MPI_Iprobe(MPI_ANY_SOURCE, MPI_ANY_TAG, MPI_COMM_WORLD, &flag, &status);
        if (flag) {
            Ray_Trace rt;
            MPI_Recv(&rt, sizeof(Ray_Trace), MPI_BYTE, MPI_ANY_SOURCE,
                         MPI_ANY_TAG, MPI_COMM_WORLD, &status);

            if (rt.t_min == -1 && rt.t_max == -1)
                break;

            rts.push(rt);

        // Removed because interleaving comms and work is slow
        //    continue;
        } /*else {
            if (!rts.empty()) {
                Ray_Trace rt = rts.top();
                rts.pop();
                Ray_Hit rh;
                Ray_Hit_Remote rhr;
                if (!(kd->hit_local(rt.r, this, rt.t_min, rt.t_max, rh, false))) {
                    rhr.t = FLT_MAX;
                } else {
                    rhr.t = rh.t;
                    rhr.col = rh.col;
                    rhr.ray_id = rt.ray_id;
                    rhrs.push_back(rhr);
                }
            }
        }
        */
    }

    std::cout << "Just processing now" << std::endl;

    // Process remaining rays
    while (!rts.empty()) {
        Ray_Trace rt = rts.top();
        rts.pop();
        Ray_Hit rh;
        Ray_Hit_Remote rhr;
        if (!(kd->hit_local(rt.r, this, rt.t_min, rt.t_max, rh, false))) {
            rhr.t = FLT_MAX;
        } else {
            rhr.t = rh.t;
            rhr.col = rh.col;
            rhr.ray_id = rt.ray_id;
            rhrs.push_back(rhr);
        }
    }

    std::cout << "Done processing, gonna send now" << std::endl;

    if (!rhrs.empty()) {
        // Send back ray info
        MPI_Ssend(rhrs.data(), sizeof(rhrs[0]) * rhrs.size(),
                    MPI_BYTE, 0, 0, MPI_COMM_WORLD);
    } else {
        // Send a message staying we're done, but have nothing to sedn
        unsigned int done = 0xFFFFFFFF;
        MPI_Ssend(&done, 1, MPI_INT, 0, NOTHING_TO_SEND, MPI_COMM_WORLD);
    }

/*
    MPI_Barrier(MPI_COMM_WORLD);
    while (true) {
        Ray_Trace rt;
        MPI_Status status;
        MPI_Recv(&rt, sizeof(Ray_Trace), MPI_BYTE, MPI_ANY_SOURCE,
                     MPI_ANY_TAG, MPI_COMM_WORLD, &status);

        if (rt.t_min == -1 && rt.t_max == -1)
            break;

        Ray_Hit rh;
        Ray_Hit_Remote rhr;
        if (!(kd->hit_local(rt.r, this, rt.t_min, rt.t_max, rh, false))) {
            rhr.t = FLT_MAX;
        } else {
            rhr.t = rh.t;
        }

        rhr.col = rh.col;

        MPI_Send(&rhr, sizeof(Ray_Hit_Remote), MPI_BYTE, status.MPI_SOURCE,
                    status.MPI_TAG, MPI_COMM_WORLD);
    }
*/
}

void Ray_Tracer::trace_all(int rank, int np) {

    std::cout << "hi" << std::endl;
    if (rank != 0) {
        worker();
        return;
    }

    std::vector<V2> points;
    MPI_Barrier(MPI_COMM_WORLD);
    for (unsigned int i = 0; i < x_res; ++i) {
        for (unsigned int j = 0; j < y_res; ++j) {
            points.clear();
            gen_rand_samples(sample_bins, points);

            Color total;
            for (unsigned int k = 0; k < points.size(); ++k) {
                V2& p = points[k];
                Ray r = cam.make_ray((i + p.x() + 0.5)/x_res, (j + p.y() + 0.5)/y_res);
                local_rays.push(r);
                local_dest.push(Int_pair(i, j));
                Ray_Hit rh;
                unsigned int ray_id = i << 19 | j << 6 | k;
                kd->hit(r, this, EPSILON, FLT_MAX, rh, false, ray_id);
                //trace(r, EPSILON, FLT_MAX, rh, false);
                    //total += rh.col;
            }
//            image_buf[i][j] = total / points.size();
        }
    }

    // Tell all workers to start processing
    Ray_Trace rt;
    Ray r;
    rt.r = r; // dummy
    rt.t_min = -1;
    rt.t_max = -1;
    rt.ray_id = 0; // dummy
    std::cout << "Done sending Rays" << std::endl;
    for (int i = 1; i < np; ++i) {
        MPI_Ssend(&rt, sizeof(Ray_Trace), MPI_BYTE, i, 0, MPI_COMM_WORLD);
    }

    std::map<unsigned int, Ray_Hit_Remote> rhrs;

    int procs_to_recv = np - 1;
    while (procs_to_recv > 0) {
        MPI_Status status;
        MPI_Probe(MPI_ANY_SOURCE, MPI_ANY_TAG, MPI_COMM_WORLD, &status);
        if (status.MPI_TAG == NOTHING_TO_SEND) {
            int dummy;
            MPI_Recv(&dummy, 1, MPI_INT, MPI_ANY_SOURCE,
                        MPI_ANY_TAG, MPI_COMM_WORLD, &status);
            procs_to_recv--;
            continue;
        } else {
            int byte_count;
            MPI_Get_count(&status, MPI_BYTE, &byte_count);
            int elements = byte_count / sizeof(Ray_Hit_Remote);
            Ray_Hit_Remote* rhr = new Ray_Hit_Remote[elements];
            MPI_Recv(rhr, byte_count, MPI_BYTE, status.MPI_SOURCE,
                        status.MPI_TAG, MPI_COMM_WORLD, &status);

            for (int i = 0; i < elements; ++i) {
                std::pair<unsigned int, Ray_Hit_Remote> p;
                p.first = rhr[i].ray_id;
                p.second = rhr[i];
                auto res = rhrs.insert(p);
                if (!res.second) { // already exists
                    if (res.first->second.t > rhr[i].t)
                        res.first->second = rhr[i];
                }
            }
            procs_to_recv--;
        }
    }

    int samples = points.size();
    for (auto& r : rhrs) {
        int x = r.first >> 19;
        int y = (r.first >> 6) & 0x00001FFF;
        image_buf[x][y] += r.second.col/ samples;
    }

    /*
    Ray r;
    Ray_Hit rh;
    Int_pair dest;
    int samples = sample_bins*sample_bins;
    omp_set_num_threads(1);
    #pragma omp parallel
    {
    #pragma omp single private(r, dest, rh)
    {
    std::cout << "hi" << std::endl;
    while (!(local_rays.empty())) {
            r = local_rays.top();
            local_rays.pop();
            dest = local_dest.top();
            local_dest.pop();
        #pragma omp task firstprivate(r, dest, rh)
        {
        if (trace(r, EPSILON, FLT_MAX, rh, false)) {
            image_buf[dest.first][dest.second] += rh.col / samples;
        }
        }
    }
    }
    }
    */

    return;

}

void Ray_Tracer::write_buffer(std::string filename) {
    png::image< png::rgb_pixel > ray_image(x_res, y_res);
    for (unsigned int i = 0; i < x_res; ++i) {
        for (unsigned int j = 0; j < y_res; ++j) {
            ray_image[i][j] = image_buf[i][j].to_png_pixel();
        }
    }
    ray_image.write(filename);
}

static void gen_rand_samples(int bins_per_side, std::vector<V2>& points) {
    static std::random_device rd;
    static std::mt19937 gen(rd());
    static std::uniform_real_distribution<> dis(0, 1);

    float bin_length = 1.0f / bins_per_side;

    float bin_x = 0.0;
    float bin_y = 0.0;

    for (int i = 0; i < bins_per_side; ++i) {
        for (int j = 0; j < bins_per_side; ++j) {
            float rand_num_unit = dis(gen);
            bin_x = i * bin_length + bin_length*rand_num_unit - 0.5;
            rand_num_unit = dis(gen);
            bin_y = j * bin_length + bin_length*rand_num_unit - 0.5;
            points.push_back(V2(bin_x, bin_y));
        }
    }
}
<|endoftext|>