<commit_before>//===- LowerSwitch.cpp - Eliminate Switch instructions --------------------===//
//
// The LowerSwitch transformation rewrites switch statements with a sequence of
// branches, which allows targets to get away with not implementing the switch
// statement until it is convenient.
//
//===----------------------------------------------------------------------===//

#include "llvm/Transforms/Scalar.h"
#include "llvm/Function.h"
#include "llvm/iTerminators.h"
#include "llvm/iOperators.h"
#include "llvm/iPHINode.h"
#include "llvm/Pass.h"
#include "Support/Statistic.h"

namespace {
  Statistic<> NumLowered("lowerswitch", "Number of SwitchInst's replaced");

  /// LowerSwitch Pass - Replace all SwitchInst instructions with chained branch
  /// instructions.  Note that this cannot be a BasicBlock pass because it
  /// modifies the CFG!
  struct LowerSwitch : public FunctionPass {
    bool runOnFunction(Function &F);
    void processSwitchInst(SwitchInst *SI);
  };

  RegisterOpt<LowerSwitch>
  X("lowerswitch", "Lower SwitchInst's to branches");
}

// createLowerSwitchPass - Interface to this file...
FunctionPass *createLowerSwitchPass() {
  return new LowerSwitch();
}

bool LowerSwitch::runOnFunction(Function &F) {
  bool Changed = false;

  for (Function::iterator I = F.begin(), E = F.end(); I != E; ) {
    BasicBlock *Cur = I++; // Advance over block so we don't traverse new blocks

    if (SwitchInst *SI = dyn_cast<SwitchInst>(Cur->getTerminator())) {
      Changed = true;
      processSwitchInst(SI);
    }
  }

  return Changed;
}

// processSwitchInst - Replace the specified switch instruction with a sequence
// of chained basic blocks.  Right now we just insert an incredibly stupid
// linear sequence of branches.  It would be better to do a balanced binary
// search eventually.  FIXME
//
void LowerSwitch::processSwitchInst(SwitchInst *SI) {
  BasicBlock *CurBlock = SI->getParent();
  BasicBlock *OrigBlock = CurBlock;
  Function *F = CurBlock->getParent();
  Value *Val = SI->getOperand(0);  // The value we are switching on...

  // Unlink the switch instruction from it's block.
  CurBlock->getInstList().remove(SI);

  // If there is only the default destination, don't bother with the code below.
  if (SI->getNumOperands() == 2) {
    CurBlock->getInstList().push_back(new BranchInst(SI->getDefaultDest()));
    delete SI;
    return;
  }

  // Expand comparisons for all of the non-default cases...
  for (unsigned i = 2, e = SI->getNumOperands(); i != e; i += 2) {
    // Insert a new basic block after the current one...
    BasicBlock *NextBlock;
    if (i != e-2) {
      NextBlock = new BasicBlock("switchblock");
      F->getBasicBlockList().insert(CurBlock->getNext(), NextBlock);
    } else {   // Last case, if it's not the value, go to default block.
      NextBlock = cast<BasicBlock>(SI->getDefaultDest());
    }

    // Make the seteq instruction...
    Instruction *Comp = new SetCondInst(Instruction::SetEQ, Val,
                                        SI->getOperand(i), "switchcase");
    CurBlock->getInstList().push_back(Comp);

    // Make the conditional branch...
    BasicBlock *Succ = cast<BasicBlock>(SI->getOperand(i+1));
    Instruction *Br = new BranchInst(Succ, NextBlock, Comp);
    CurBlock->getInstList().push_back(Br);

    // If there were any PHI nodes in this successor, rewrite one entry from
    // OrigBlock to come from CurBlock.
    for (BasicBlock::iterator I = Succ->begin();
         PHINode *PN = dyn_cast<PHINode>(I); ++I) {
      int BlockIdx = PN->getBasicBlockIndex(OrigBlock);
      assert(BlockIdx != -1 && "Switch didn't go to this successor??");
      PN->setIncomingBlock((unsigned)BlockIdx, CurBlock);
    }

    if (i == e-2) {  // Is this looking at the default destination?
      // If there is an entry in any PHI nodes for the default edge, make sure
      // to update them as well.
      for (BasicBlock::iterator I = NextBlock->begin();
           PHINode *PN = dyn_cast<PHINode>(I); ++I) {
        int BlockIdx = PN->getBasicBlockIndex(OrigBlock);
        assert(BlockIdx != -1 && "Switch didn't go to this successor??");
        PN->setIncomingBlock((unsigned)BlockIdx, CurBlock);
      }
    }

    CurBlock = NextBlock;  // Move on to the next condition
  }

  // We are now done with the switch instruction, delete it.
  delete SI;
}
<commit_msg>Bill contributed this major rewrite of the -lowerswitch pass to make it generate logarithmic conditional branch sequences instead of linear sequences.  Thanks Bill!<commit_after>//===- LowerSwitch.cpp - Eliminate Switch instructions --------------------===//
//
// The LowerSwitch transformation rewrites switch statements with a sequence of
// branches, which allows targets to get away with not implementing the switch
// statement until it is convenient.
//
//===----------------------------------------------------------------------===//

#include "llvm/Transforms/Scalar.h"
#include "llvm/Constants.h"
#include "llvm/Function.h"
#include "llvm/iTerminators.h"
#include "llvm/iOperators.h"
#include "llvm/iPHINode.h"
#include "llvm/Pass.h"
#include "Support/Debug.h"
#include "Support/Statistic.h"

namespace {
  Statistic<> NumLowered("lowerswitch", "Number of SwitchInst's replaced");

  /// LowerSwitch Pass - Replace all SwitchInst instructions with chained branch
  /// instructions.  Note that this cannot be a BasicBlock pass because it
  /// modifies the CFG!
  class LowerSwitch : public FunctionPass {
  public:
    bool runOnFunction(Function &F);
    typedef std::pair<Constant*, BasicBlock*> Case;
    typedef std::vector<Case>::iterator       CaseItr;
  private:
    void processSwitchInst(SwitchInst *SI);

    BasicBlock* switchConvert(CaseItr Begin, CaseItr End, Value* Val,
                              BasicBlock* OrigBlock, BasicBlock* Default);
    BasicBlock* newLeafBlock(Case& Leaf, Value* Val,
                             BasicBlock* OrigBlock, BasicBlock* Default);
  };

  /// The comparison function for sorting the switch case values in the vector.
  struct CaseCmp {
    bool operator () (const LowerSwitch::Case& C1,
                      const LowerSwitch::Case& C2) {
      if (const ConstantUInt* U1 = dyn_cast<const ConstantUInt>(C1.first))
        return U1->getValue() < cast<const ConstantUInt>(C2.first)->getValue();

      const ConstantSInt* S1 = dyn_cast<const ConstantSInt>(C1.first);
      return S1->getValue() < cast<const ConstantSInt>(C2.first)->getValue();
    }
  };

  RegisterOpt<LowerSwitch>
  X("lowerswitch", "Lower SwitchInst's to branches");
}

// createLowerSwitchPass - Interface to this file...
FunctionPass *createLowerSwitchPass() {
  return new LowerSwitch();
}

bool LowerSwitch::runOnFunction(Function &F) {
  bool Changed = false;

  for (Function::iterator I = F.begin(), E = F.end(); I != E; ) {
    BasicBlock *Cur = I++; // Advance over block so we don't traverse new blocks

    if (SwitchInst *SI = dyn_cast<SwitchInst>(Cur->getTerminator())) {
      Changed = true;
      processSwitchInst(SI);
    }
  }

  return Changed;
}

// operator<< - Used for debugging purposes.
//
std::ostream& operator << (std::ostream& O, std::vector<LowerSwitch::Case>& C)
{
  O << "[";

  for (std::vector<LowerSwitch::Case>::iterator B = C.begin(), E = C.end();
      B != E; ) {
    O << *B->first;
    if (++B != E) O << ", ";
  }

  return O << "]";
}

// switchConvert - Convert the switch statement into a binary lookup of
// the case values. The function recursively builds this tree.
//
BasicBlock* LowerSwitch::switchConvert(CaseItr Begin, CaseItr End,
                                       Value* Val, BasicBlock* OrigBlock,
                                       BasicBlock* Default)
{
  unsigned Size = End - Begin;

  if (Size == 1)
    return newLeafBlock(*Begin, Val, OrigBlock, Default);

  unsigned Mid = Size / 2;
  std::vector<Case> LHS(Begin, Begin + Mid);
  DEBUG(std::cerr << "LHS: " << LHS << "\n");
  std::vector<Case> RHS(Begin + Mid, End);
  DEBUG(std::cerr << "RHS: " << RHS << "\n");

  Case& Pivot = *(Begin + Mid);
  DEBUG(std::cerr << "Pivot ==> "
                  << cast<ConstantUInt>(Pivot.first)->getValue() << "\n");

  BasicBlock* LBranch = switchConvert(LHS.begin(), LHS.end(), Val,
                                      OrigBlock, Default);
  BasicBlock* RBranch = switchConvert(RHS.begin(), RHS.end(), Val,
                                      OrigBlock, Default);

  // Create a new node that checks if the value is < pivot. Go to the
  // left branch if it is and right branch if not.
  Function* F = OrigBlock->getParent();
  BasicBlock* NewNode = new BasicBlock("NodeBlock");
  F->getBasicBlockList().insert(OrigBlock->getNext(), NewNode);

  SetCondInst* Comp = new SetCondInst(Instruction::SetLT, Val, Pivot.first,
                                      "Pivot");
  NewNode->getInstList().push_back(Comp);
  BranchInst* Br = new BranchInst(LBranch, RBranch, Comp);
  NewNode->getInstList().push_back(Br);
  return NewNode;
}

// newLeafBlock - Create a new leaf block for the binary lookup tree. It
// checks if the switch's value == the case's value. If not, then it
// jumps to the default branch. At this point in the tree, the value
// can't be another valid case value, so the jump to the "default" branch
// is warranted.
//
BasicBlock* LowerSwitch::newLeafBlock(Case& Leaf, Value* Val,
                                      BasicBlock* OrigBlock,
                                      BasicBlock* Default)
{
  Function* F = OrigBlock->getParent();
  BasicBlock* NewLeaf = new BasicBlock("LeafBlock");
  F->getBasicBlockList().insert(OrigBlock->getNext(), NewLeaf);

  // Make the seteq instruction...
  SetCondInst* Comp = new SetCondInst(Instruction::SetEQ, Val,
                                      Leaf.first, "SwitchLeaf");
  NewLeaf->getInstList().push_back(Comp);

  // Make the conditional branch...
  BasicBlock* Succ = Leaf.second;
  Instruction* Br = new BranchInst(Succ, Default, Comp);
  NewLeaf->getInstList().push_back(Br);

  // If there were any PHI nodes in this successor, rewrite one entry
  // from OrigBlock to come from NewLeaf.
  for (BasicBlock::iterator I = Succ->begin();
       PHINode* PN = dyn_cast<PHINode>(I); ++I) {
    int BlockIdx = PN->getBasicBlockIndex(OrigBlock);
    assert(BlockIdx != -1 && "Switch didn't go to this successor??");
    PN->setIncomingBlock((unsigned)BlockIdx, NewLeaf);
  }

  return NewLeaf;
}

// processSwitchInst - Replace the specified switch instruction with a sequence
// of chained if-then insts in a balanced binary search.
//
void LowerSwitch::processSwitchInst(SwitchInst *SI) {
  BasicBlock *CurBlock = SI->getParent();
  BasicBlock *OrigBlock = CurBlock;
  Function *F = CurBlock->getParent();
  Value *Val = SI->getOperand(0);  // The value we are switching on...
  BasicBlock* Default = SI->getDefaultDest();

  // Unlink the switch instruction from it's block.
  CurBlock->getInstList().remove(SI);

  // If there is only the default destination, don't bother with the code below.
  if (SI->getNumOperands() == 2) {
    CurBlock->getInstList().push_back(new BranchInst(SI->getDefaultDest()));
    delete SI;
    return;
  }

  // Create a new, empty default block so that the new hierarchy of
  // if-then statements go to this and the PHI nodes are happy.
  BasicBlock* NewDefault = new BasicBlock("NewDefault");
  F->getBasicBlockList().insert(Default, NewDefault);

  NewDefault->getInstList().push_back(new BranchInst(Default));

  // If there is an entry in any PHI nodes for the default edge, make sure
  // to update them as well.
  for (BasicBlock::iterator I = Default->begin();
       PHINode *PN = dyn_cast<PHINode>(I); ++I) {
    int BlockIdx = PN->getBasicBlockIndex(OrigBlock);
    assert(BlockIdx != -1 && "Switch didn't go to this successor??");
    PN->setIncomingBlock((unsigned)BlockIdx, NewDefault);
  }

  std::vector<Case> Cases;

  // Expand comparisons for all of the non-default cases...
  for (unsigned i = 1; i < SI->getNumSuccessors(); ++i)
    Cases.push_back(Case(SI->getSuccessorValue(i), SI->getSuccessor(i)));

  std::sort(Cases.begin(), Cases.end(), CaseCmp());
  DEBUG(std::cerr << "Cases: " << Cases << "\n");
  BasicBlock* SwitchBlock = switchConvert(Cases.begin(), Cases.end(), Val,
                                          OrigBlock, NewDefault);

  // Branch to our shiny new if-then stuff...
  OrigBlock->getInstList().push_back(new BranchInst(SwitchBlock));

  // We are now done with the switch instruction, delete it.
  delete SI;
}
<|endoftext|>


<commit_before>//===- ValueMapper.cpp - Interface shared by lib/Transforms/Utils ---------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
//
// This file defines the MapValue function, which is shared by various parts of
// the lib/Transforms/Utils library.
//
//===----------------------------------------------------------------------===//

#include "llvm/Transforms/Utils/ValueMapper.h"
#include "llvm/IR/Constants.h"
#include "llvm/IR/Function.h"
#include "llvm/IR/InlineAsm.h"
#include "llvm/IR/Instructions.h"
#include "llvm/IR/Metadata.h"
using namespace llvm;

// Out of line method to get vtable etc for class.
void ValueMapTypeRemapper::anchor() {}
void ValueMaterializer::anchor() {}

Value *llvm::MapValue(const Value *V, ValueToValueMapTy &VM, RemapFlags Flags,
                      ValueMapTypeRemapper *TypeMapper,
                      ValueMaterializer *Materializer) {
  ValueToValueMapTy::iterator I = VM.find(V);

  // If the value already exists in the map, use it.
  if (I != VM.end() && I->second) return I->second;

  // If we have a materializer and it can materialize a value, use that.
  if (Materializer) {
    if (Value *NewV = Materializer->materializeValueFor(const_cast<Value*>(V)))
      return VM[V] = NewV;
  }

  // Global values do not need to be seeded into the VM if they
  // are using the identity mapping.
  if (isa<GlobalValue>(V))
    return VM[V] = const_cast<Value*>(V);

  if (const InlineAsm *IA = dyn_cast<InlineAsm>(V)) {
    // Inline asm may need *type* remapping.
    FunctionType *NewTy = IA->getFunctionType();
    if (TypeMapper) {
      NewTy = cast<FunctionType>(TypeMapper->remapType(NewTy));

      if (NewTy != IA->getFunctionType())
        V = InlineAsm::get(NewTy, IA->getAsmString(), IA->getConstraintString(),
                           IA->hasSideEffects(), IA->isAlignStack());
    }

    return VM[V] = const_cast<Value*>(V);
  }

  if (const auto *MDV = dyn_cast<MetadataAsValue>(V)) {
    const Metadata *MD = MDV->getMetadata();
    // If this is a module-level metadata and we know that nothing at the module
    // level is changing, then use an identity mapping.
    if (!isa<LocalAsMetadata>(MD) && (Flags & RF_NoModuleLevelChanges))
      return VM[V] = const_cast<Value *>(V);

    auto *MappedMD = MapMetadata(MD, VM, Flags, TypeMapper, Materializer);
    if (MD == MappedMD || (!MappedMD && (Flags & RF_IgnoreMissingEntries)))
      return VM[V] = const_cast<Value *>(V);

    // FIXME: This assert crashes during bootstrap, but I think it should be
    // correct.  For now, just match behaviour from before the metadata/value
    // split.
    //
    //    assert(MappedMD && "Referenced metadata value not in value map");
    return VM[V] = MetadataAsValue::get(V->getContext(), MappedMD);
  }

  // Okay, this either must be a constant (which may or may not be mappable) or
  // is something that is not in the mapping table.
  Constant *C = const_cast<Constant*>(dyn_cast<Constant>(V));
  if (!C)
    return nullptr;

  if (BlockAddress *BA = dyn_cast<BlockAddress>(C)) {
    Function *F =
      cast<Function>(MapValue(BA->getFunction(), VM, Flags, TypeMapper, Materializer));
    BasicBlock *BB = cast_or_null<BasicBlock>(MapValue(BA->getBasicBlock(), VM,
                                                       Flags, TypeMapper, Materializer));
    return VM[V] = BlockAddress::get(F, BB ? BB : BA->getBasicBlock());
  }

  // Otherwise, we have some other constant to remap.  Start by checking to see
  // if all operands have an identity remapping.
  unsigned OpNo = 0, NumOperands = C->getNumOperands();
  Value *Mapped = nullptr;
  for (; OpNo != NumOperands; ++OpNo) {
    Value *Op = C->getOperand(OpNo);
    Mapped = MapValue(Op, VM, Flags, TypeMapper, Materializer);
    if (Mapped != C) break;
  }

  // See if the type mapper wants to remap the type as well.
  Type *NewTy = C->getType();
  if (TypeMapper)
    NewTy = TypeMapper->remapType(NewTy);

  // If the result type and all operands match up, then just insert an identity
  // mapping.
  if (OpNo == NumOperands && NewTy == C->getType())
    return VM[V] = C;

  // Okay, we need to create a new constant.  We've already processed some or
  // all of the operands, set them all up now.
  SmallVector<Constant*, 8> Ops;
  Ops.reserve(NumOperands);
  for (unsigned j = 0; j != OpNo; ++j)
    Ops.push_back(cast<Constant>(C->getOperand(j)));

  // If one of the operands mismatch, push it and the other mapped operands.
  if (OpNo != NumOperands) {
    Ops.push_back(cast<Constant>(Mapped));

    // Map the rest of the operands that aren't processed yet.
    for (++OpNo; OpNo != NumOperands; ++OpNo)
      Ops.push_back(MapValue(cast<Constant>(C->getOperand(OpNo)), VM,
                             Flags, TypeMapper, Materializer));
  }

  if (ConstantExpr *CE = dyn_cast<ConstantExpr>(C))
    return VM[V] = CE->getWithOperands(Ops, NewTy);
  if (isa<ConstantArray>(C))
    return VM[V] = ConstantArray::get(cast<ArrayType>(NewTy), Ops);
  if (isa<ConstantStruct>(C))
    return VM[V] = ConstantStruct::get(cast<StructType>(NewTy), Ops);
  if (isa<ConstantVector>(C))
    return VM[V] = ConstantVector::get(Ops);
  // If this is a no-operand constant, it must be because the type was remapped.
  if (isa<UndefValue>(C))
    return VM[V] = UndefValue::get(NewTy);
  if (isa<ConstantAggregateZero>(C))
    return VM[V] = ConstantAggregateZero::get(NewTy);
  assert(isa<ConstantPointerNull>(C));
  return VM[V] = ConstantPointerNull::get(cast<PointerType>(NewTy));
}

static Metadata *mapToMetadata(ValueToValueMapTy &VM, const Metadata *Key,
                     Metadata *Val) {
  VM.MD()[Key].reset(Val);
  return Val;
}

static Metadata *mapToSelf(ValueToValueMapTy &VM, const Metadata *MD) {
  return mapToMetadata(VM, MD, const_cast<Metadata *>(MD));
}

static Metadata *MapMetadataImpl(const Metadata *MD, ValueToValueMapTy &VM,
                                 RemapFlags Flags,
                                 ValueMapTypeRemapper *TypeMapper,
                                 ValueMaterializer *Materializer);

static Metadata *mapMetadataOp(Metadata *Op, ValueToValueMapTy &VM,
                                 RemapFlags Flags,
                                 ValueMapTypeRemapper *TypeMapper,
                                 ValueMaterializer *Materializer) {
  if (!Op)
    return nullptr;
  if (Metadata *MappedOp =
          MapMetadataImpl(Op, VM, Flags, TypeMapper, Materializer))
    return MappedOp;
  // Use identity map if MappedOp is null and we can ignore missing entries.
  if (Flags & RF_IgnoreMissingEntries)
    return Op;

  // FIXME: This assert crashes during bootstrap, but I think it should be
  // correct.  For now, just match behaviour from before the metadata/value
  // split.
  //
  //    llvm_unreachable("Referenced metadata not in value map!");
  return nullptr;
}

/// \brief Map a distinct MDNode.
///
/// Distinct nodes are not uniqued, so they must always recreated.
static Metadata *mapDistinctNode(const UniquableMDNode *Node,
                                 ValueToValueMapTy &VM, RemapFlags Flags,
                                 ValueMapTypeRemapper *TypeMapper,
                                 ValueMaterializer *Materializer) {
  assert(Node->isDistinct() && "Expected distinct node");

  // Create the node first so it's available for cyclical references.
  SmallVector<Metadata *, 4> EmptyOps(Node->getNumOperands());
  MDTuple *NewMD = MDTuple::getDistinct(Node->getContext(), EmptyOps);
  mapToMetadata(VM, Node, NewMD);

  // Fix the operands.
  for (unsigned I = 0, E = Node->getNumOperands(); I != E; ++I)
    NewMD->replaceOperandWith(I, mapMetadataOp(Node->getOperand(I), VM, Flags,
                                               TypeMapper, Materializer));

  return NewMD;
}

/// \brief Check whether a uniqued node needs to be remapped.
///
/// Check whether a uniqued node needs to be remapped (due to any operands
/// changing).
static bool shouldRemapUniquedNode(const UniquableMDNode *Node,
                                   ValueToValueMapTy &VM, RemapFlags Flags,
                                   ValueMapTypeRemapper *TypeMapper,
                                   ValueMaterializer *Materializer) {
  // Check all operands to see if any need to be remapped.
  for (unsigned I = 0, E = Node->getNumOperands(); I != E; ++I) {
    Metadata *Op = Node->getOperand(I);
    if (Op != mapMetadataOp(Op, VM, Flags, TypeMapper, Materializer))
      return true;
  }
  return false;
}

/// \brief Map a uniqued MDNode.
///
/// Uniqued nodes may not need to be recreated (they may map to themselves).
static Metadata *mapUniquedNode(const UniquableMDNode *Node,
                                 ValueToValueMapTy &VM, RemapFlags Flags,
                                 ValueMapTypeRemapper *TypeMapper,
                                 ValueMaterializer *Materializer) {
  assert(!Node->isDistinct() && "Expected uniqued node");

  // Create a dummy node in case we have a metadata cycle.
  MDNodeFwdDecl *Dummy = MDNode::getTemporary(Node->getContext(), None);
  mapToMetadata(VM, Node, Dummy);

  // Check all operands to see if any need to be remapped.
  if (!shouldRemapUniquedNode(Node, VM, Flags, TypeMapper, Materializer)) {
    // Use an identity mapping.
    mapToSelf(VM, Node);
    MDNode::deleteTemporary(Dummy);
    return const_cast<Metadata *>(static_cast<const Metadata *>(Node));
  }

  // At least one operand needs remapping.
  SmallVector<Metadata *, 4> Elts;
  Elts.reserve(Node->getNumOperands());
  for (unsigned I = 0, E = Node->getNumOperands(); I != E; ++I)
    Elts.push_back(mapMetadataOp(Node->getOperand(I), VM, Flags, TypeMapper,
                                 Materializer));

  MDNode *NewMD = MDTuple::get(Node->getContext(), Elts);
  Dummy->replaceAllUsesWith(NewMD);
  MDNode::deleteTemporary(Dummy);
  return mapToMetadata(VM, Node, NewMD);
}

static Metadata *MapMetadataImpl(const Metadata *MD, ValueToValueMapTy &VM,
                                 RemapFlags Flags,
                                 ValueMapTypeRemapper *TypeMapper,
                                 ValueMaterializer *Materializer) {
  // If the value already exists in the map, use it.
  if (Metadata *NewMD = VM.MD().lookup(MD).get())
    return NewMD;

  if (isa<MDString>(MD))
    return mapToSelf(VM, MD);

  if (isa<ConstantAsMetadata>(MD))
    if ((Flags & RF_NoModuleLevelChanges))
      return mapToSelf(VM, MD);

  if (const auto *VMD = dyn_cast<ValueAsMetadata>(MD)) {
    Value *MappedV =
        MapValue(VMD->getValue(), VM, Flags, TypeMapper, Materializer);
    if (VMD->getValue() == MappedV ||
        (!MappedV && (Flags & RF_IgnoreMissingEntries)))
      return mapToSelf(VM, MD);

    // FIXME: This assert crashes during bootstrap, but I think it should be
    // correct.  For now, just match behaviour from before the metadata/value
    // split.
    //
    //    assert(MappedV && "Referenced metadata not in value map!");
    if (MappedV)
      return mapToMetadata(VM, MD, ValueAsMetadata::get(MappedV));
    return nullptr;
  }

  const UniquableMDNode *Node = cast<UniquableMDNode>(MD);
  assert(Node->isResolved() && "Unexpected unresolved node");

  // If this is a module-level metadata and we know that nothing at the
  // module level is changing, then use an identity mapping.
  if (Flags & RF_NoModuleLevelChanges)
    return mapToSelf(VM, MD);

  if (Node->isDistinct())
    return mapDistinctNode(Node, VM, Flags, TypeMapper, Materializer);

  return mapUniquedNode(Node, VM, Flags, TypeMapper, Materializer);
}

Metadata *llvm::MapMetadata(const Metadata *MD, ValueToValueMapTy &VM,
                            RemapFlags Flags, ValueMapTypeRemapper *TypeMapper,
                            ValueMaterializer *Materializer) {
  Metadata *NewMD = MapMetadataImpl(MD, VM, Flags, TypeMapper, Materializer);
  if (NewMD && NewMD != MD)
    if (auto *N = dyn_cast<UniquableMDNode>(NewMD))
      N->resolveCycles();
  return NewMD;
}

MDNode *llvm::MapMetadata(const MDNode *MD, ValueToValueMapTy &VM,
                          RemapFlags Flags, ValueMapTypeRemapper *TypeMapper,
                          ValueMaterializer *Materializer) {
  return cast<MDNode>(MapMetadata(static_cast<const Metadata *>(MD), VM, Flags,
                                  TypeMapper, Materializer));
}

/// RemapInstruction - Convert the instruction operands from referencing the
/// current values into those specified by VMap.
///
void llvm::RemapInstruction(Instruction *I, ValueToValueMapTy &VMap,
                            RemapFlags Flags, ValueMapTypeRemapper *TypeMapper,
                            ValueMaterializer *Materializer){
  // Remap operands.
  for (User::op_iterator op = I->op_begin(), E = I->op_end(); op != E; ++op) {
    Value *V = MapValue(*op, VMap, Flags, TypeMapper, Materializer);
    // If we aren't ignoring missing entries, assert that something happened.
    if (V)
      *op = V;
    else
      assert((Flags & RF_IgnoreMissingEntries) &&
             "Referenced value not in value map!");
  }

  // Remap phi nodes' incoming blocks.
  if (PHINode *PN = dyn_cast<PHINode>(I)) {
    for (unsigned i = 0, e = PN->getNumIncomingValues(); i != e; ++i) {
      Value *V = MapValue(PN->getIncomingBlock(i), VMap, Flags);
      // If we aren't ignoring missing entries, assert that something happened.
      if (V)
        PN->setIncomingBlock(i, cast<BasicBlock>(V));
      else
        assert((Flags & RF_IgnoreMissingEntries) &&
               "Referenced block not in value map!");
    }
  }

  // Remap attached metadata.
  SmallVector<std::pair<unsigned, MDNode *>, 4> MDs;
  I->getAllMetadata(MDs);
  for (SmallVectorImpl<std::pair<unsigned, MDNode *>>::iterator
           MI = MDs.begin(),
           ME = MDs.end();
       MI != ME; ++MI) {
    MDNode *Old = MI->second;
    MDNode *New = MapMetadata(Old, VMap, Flags, TypeMapper, Materializer);
    if (New != Old)
      I->setMetadata(MI->first, New);
  }

  // If the instruction's type is being remapped, do so now.
  if (TypeMapper)
    I->mutateType(TypeMapper->remapType(I->getType()));
}
<commit_msg>Utils: Extract cloneMDTuple(), NFC<commit_after>//===- ValueMapper.cpp - Interface shared by lib/Transforms/Utils ---------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
//
// This file defines the MapValue function, which is shared by various parts of
// the lib/Transforms/Utils library.
//
//===----------------------------------------------------------------------===//

#include "llvm/Transforms/Utils/ValueMapper.h"
#include "llvm/IR/Constants.h"
#include "llvm/IR/Function.h"
#include "llvm/IR/InlineAsm.h"
#include "llvm/IR/Instructions.h"
#include "llvm/IR/Metadata.h"
using namespace llvm;

// Out of line method to get vtable etc for class.
void ValueMapTypeRemapper::anchor() {}
void ValueMaterializer::anchor() {}

Value *llvm::MapValue(const Value *V, ValueToValueMapTy &VM, RemapFlags Flags,
                      ValueMapTypeRemapper *TypeMapper,
                      ValueMaterializer *Materializer) {
  ValueToValueMapTy::iterator I = VM.find(V);

  // If the value already exists in the map, use it.
  if (I != VM.end() && I->second) return I->second;

  // If we have a materializer and it can materialize a value, use that.
  if (Materializer) {
    if (Value *NewV = Materializer->materializeValueFor(const_cast<Value*>(V)))
      return VM[V] = NewV;
  }

  // Global values do not need to be seeded into the VM if they
  // are using the identity mapping.
  if (isa<GlobalValue>(V))
    return VM[V] = const_cast<Value*>(V);

  if (const InlineAsm *IA = dyn_cast<InlineAsm>(V)) {
    // Inline asm may need *type* remapping.
    FunctionType *NewTy = IA->getFunctionType();
    if (TypeMapper) {
      NewTy = cast<FunctionType>(TypeMapper->remapType(NewTy));

      if (NewTy != IA->getFunctionType())
        V = InlineAsm::get(NewTy, IA->getAsmString(), IA->getConstraintString(),
                           IA->hasSideEffects(), IA->isAlignStack());
    }

    return VM[V] = const_cast<Value*>(V);
  }

  if (const auto *MDV = dyn_cast<MetadataAsValue>(V)) {
    const Metadata *MD = MDV->getMetadata();
    // If this is a module-level metadata and we know that nothing at the module
    // level is changing, then use an identity mapping.
    if (!isa<LocalAsMetadata>(MD) && (Flags & RF_NoModuleLevelChanges))
      return VM[V] = const_cast<Value *>(V);

    auto *MappedMD = MapMetadata(MD, VM, Flags, TypeMapper, Materializer);
    if (MD == MappedMD || (!MappedMD && (Flags & RF_IgnoreMissingEntries)))
      return VM[V] = const_cast<Value *>(V);

    // FIXME: This assert crashes during bootstrap, but I think it should be
    // correct.  For now, just match behaviour from before the metadata/value
    // split.
    //
    //    assert(MappedMD && "Referenced metadata value not in value map");
    return VM[V] = MetadataAsValue::get(V->getContext(), MappedMD);
  }

  // Okay, this either must be a constant (which may or may not be mappable) or
  // is something that is not in the mapping table.
  Constant *C = const_cast<Constant*>(dyn_cast<Constant>(V));
  if (!C)
    return nullptr;

  if (BlockAddress *BA = dyn_cast<BlockAddress>(C)) {
    Function *F =
      cast<Function>(MapValue(BA->getFunction(), VM, Flags, TypeMapper, Materializer));
    BasicBlock *BB = cast_or_null<BasicBlock>(MapValue(BA->getBasicBlock(), VM,
                                                       Flags, TypeMapper, Materializer));
    return VM[V] = BlockAddress::get(F, BB ? BB : BA->getBasicBlock());
  }

  // Otherwise, we have some other constant to remap.  Start by checking to see
  // if all operands have an identity remapping.
  unsigned OpNo = 0, NumOperands = C->getNumOperands();
  Value *Mapped = nullptr;
  for (; OpNo != NumOperands; ++OpNo) {
    Value *Op = C->getOperand(OpNo);
    Mapped = MapValue(Op, VM, Flags, TypeMapper, Materializer);
    if (Mapped != C) break;
  }

  // See if the type mapper wants to remap the type as well.
  Type *NewTy = C->getType();
  if (TypeMapper)
    NewTy = TypeMapper->remapType(NewTy);

  // If the result type and all operands match up, then just insert an identity
  // mapping.
  if (OpNo == NumOperands && NewTy == C->getType())
    return VM[V] = C;

  // Okay, we need to create a new constant.  We've already processed some or
  // all of the operands, set them all up now.
  SmallVector<Constant*, 8> Ops;
  Ops.reserve(NumOperands);
  for (unsigned j = 0; j != OpNo; ++j)
    Ops.push_back(cast<Constant>(C->getOperand(j)));

  // If one of the operands mismatch, push it and the other mapped operands.
  if (OpNo != NumOperands) {
    Ops.push_back(cast<Constant>(Mapped));

    // Map the rest of the operands that aren't processed yet.
    for (++OpNo; OpNo != NumOperands; ++OpNo)
      Ops.push_back(MapValue(cast<Constant>(C->getOperand(OpNo)), VM,
                             Flags, TypeMapper, Materializer));
  }

  if (ConstantExpr *CE = dyn_cast<ConstantExpr>(C))
    return VM[V] = CE->getWithOperands(Ops, NewTy);
  if (isa<ConstantArray>(C))
    return VM[V] = ConstantArray::get(cast<ArrayType>(NewTy), Ops);
  if (isa<ConstantStruct>(C))
    return VM[V] = ConstantStruct::get(cast<StructType>(NewTy), Ops);
  if (isa<ConstantVector>(C))
    return VM[V] = ConstantVector::get(Ops);
  // If this is a no-operand constant, it must be because the type was remapped.
  if (isa<UndefValue>(C))
    return VM[V] = UndefValue::get(NewTy);
  if (isa<ConstantAggregateZero>(C))
    return VM[V] = ConstantAggregateZero::get(NewTy);
  assert(isa<ConstantPointerNull>(C));
  return VM[V] = ConstantPointerNull::get(cast<PointerType>(NewTy));
}

static Metadata *mapToMetadata(ValueToValueMapTy &VM, const Metadata *Key,
                     Metadata *Val) {
  VM.MD()[Key].reset(Val);
  return Val;
}

static Metadata *mapToSelf(ValueToValueMapTy &VM, const Metadata *MD) {
  return mapToMetadata(VM, MD, const_cast<Metadata *>(MD));
}

static Metadata *MapMetadataImpl(const Metadata *MD, ValueToValueMapTy &VM,
                                 RemapFlags Flags,
                                 ValueMapTypeRemapper *TypeMapper,
                                 ValueMaterializer *Materializer);

static Metadata *mapMetadataOp(Metadata *Op, ValueToValueMapTy &VM,
                                 RemapFlags Flags,
                                 ValueMapTypeRemapper *TypeMapper,
                                 ValueMaterializer *Materializer) {
  if (!Op)
    return nullptr;
  if (Metadata *MappedOp =
          MapMetadataImpl(Op, VM, Flags, TypeMapper, Materializer))
    return MappedOp;
  // Use identity map if MappedOp is null and we can ignore missing entries.
  if (Flags & RF_IgnoreMissingEntries)
    return Op;

  // FIXME: This assert crashes during bootstrap, but I think it should be
  // correct.  For now, just match behaviour from before the metadata/value
  // split.
  //
  //    llvm_unreachable("Referenced metadata not in value map!");
  return nullptr;
}

/// \brief Map a distinct MDNode.
///
/// Distinct nodes are not uniqued, so they must always recreated.
static Metadata *mapDistinctNode(const UniquableMDNode *Node,
                                 ValueToValueMapTy &VM, RemapFlags Flags,
                                 ValueMapTypeRemapper *TypeMapper,
                                 ValueMaterializer *Materializer) {
  assert(Node->isDistinct() && "Expected distinct node");

  // Create the node first so it's available for cyclical references.
  SmallVector<Metadata *, 4> EmptyOps(Node->getNumOperands());
  MDTuple *NewMD = MDTuple::getDistinct(Node->getContext(), EmptyOps);
  mapToMetadata(VM, Node, NewMD);

  // Fix the operands.
  for (unsigned I = 0, E = Node->getNumOperands(); I != E; ++I)
    NewMD->replaceOperandWith(I, mapMetadataOp(Node->getOperand(I), VM, Flags,
                                               TypeMapper, Materializer));

  return NewMD;
}

/// \brief Check whether a uniqued node needs to be remapped.
///
/// Check whether a uniqued node needs to be remapped (due to any operands
/// changing).
static bool shouldRemapUniquedNode(const UniquableMDNode *Node,
                                   ValueToValueMapTy &VM, RemapFlags Flags,
                                   ValueMapTypeRemapper *TypeMapper,
                                   ValueMaterializer *Materializer) {
  // Check all operands to see if any need to be remapped.
  for (unsigned I = 0, E = Node->getNumOperands(); I != E; ++I) {
    Metadata *Op = Node->getOperand(I);
    if (Op != mapMetadataOp(Op, VM, Flags, TypeMapper, Materializer))
      return true;
  }
  return false;
}

static Metadata *cloneMDTuple(const UniquableMDNode *Node,
                              ValueToValueMapTy &VM, RemapFlags Flags,
                              ValueMapTypeRemapper *TypeMapper,
                              ValueMaterializer *Materializer) {
  SmallVector<Metadata *, 4> Elts;
  Elts.reserve(Node->getNumOperands());
  for (unsigned I = 0, E = Node->getNumOperands(); I != E; ++I)
    Elts.push_back(mapMetadataOp(Node->getOperand(I), VM, Flags, TypeMapper,
                                 Materializer));

  return MDTuple::get(Node->getContext(), Elts);
}

/// \brief Map a uniqued MDNode.
///
/// Uniqued nodes may not need to be recreated (they may map to themselves).
static Metadata *mapUniquedNode(const UniquableMDNode *Node,
                                 ValueToValueMapTy &VM, RemapFlags Flags,
                                 ValueMapTypeRemapper *TypeMapper,
                                 ValueMaterializer *Materializer) {
  assert(!Node->isDistinct() && "Expected uniqued node");

  // Create a dummy node in case we have a metadata cycle.
  MDNodeFwdDecl *Dummy = MDNode::getTemporary(Node->getContext(), None);
  mapToMetadata(VM, Node, Dummy);

  // Check all operands to see if any need to be remapped.
  if (!shouldRemapUniquedNode(Node, VM, Flags, TypeMapper, Materializer)) {
    // Use an identity mapping.
    mapToSelf(VM, Node);
    MDNode::deleteTemporary(Dummy);
    return const_cast<Metadata *>(static_cast<const Metadata *>(Node));
  }

  // At least one operand needs remapping.
  Metadata *NewMD = cloneMDTuple(Node, VM, Flags, TypeMapper, Materializer);
  Dummy->replaceAllUsesWith(NewMD);
  MDNode::deleteTemporary(Dummy);
  return mapToMetadata(VM, Node, NewMD);
}

static Metadata *MapMetadataImpl(const Metadata *MD, ValueToValueMapTy &VM,
                                 RemapFlags Flags,
                                 ValueMapTypeRemapper *TypeMapper,
                                 ValueMaterializer *Materializer) {
  // If the value already exists in the map, use it.
  if (Metadata *NewMD = VM.MD().lookup(MD).get())
    return NewMD;

  if (isa<MDString>(MD))
    return mapToSelf(VM, MD);

  if (isa<ConstantAsMetadata>(MD))
    if ((Flags & RF_NoModuleLevelChanges))
      return mapToSelf(VM, MD);

  if (const auto *VMD = dyn_cast<ValueAsMetadata>(MD)) {
    Value *MappedV =
        MapValue(VMD->getValue(), VM, Flags, TypeMapper, Materializer);
    if (VMD->getValue() == MappedV ||
        (!MappedV && (Flags & RF_IgnoreMissingEntries)))
      return mapToSelf(VM, MD);

    // FIXME: This assert crashes during bootstrap, but I think it should be
    // correct.  For now, just match behaviour from before the metadata/value
    // split.
    //
    //    assert(MappedV && "Referenced metadata not in value map!");
    if (MappedV)
      return mapToMetadata(VM, MD, ValueAsMetadata::get(MappedV));
    return nullptr;
  }

  const UniquableMDNode *Node = cast<UniquableMDNode>(MD);
  assert(Node->isResolved() && "Unexpected unresolved node");

  // If this is a module-level metadata and we know that nothing at the
  // module level is changing, then use an identity mapping.
  if (Flags & RF_NoModuleLevelChanges)
    return mapToSelf(VM, MD);

  if (Node->isDistinct())
    return mapDistinctNode(Node, VM, Flags, TypeMapper, Materializer);

  return mapUniquedNode(Node, VM, Flags, TypeMapper, Materializer);
}

Metadata *llvm::MapMetadata(const Metadata *MD, ValueToValueMapTy &VM,
                            RemapFlags Flags, ValueMapTypeRemapper *TypeMapper,
                            ValueMaterializer *Materializer) {
  Metadata *NewMD = MapMetadataImpl(MD, VM, Flags, TypeMapper, Materializer);
  if (NewMD && NewMD != MD)
    if (auto *N = dyn_cast<UniquableMDNode>(NewMD))
      N->resolveCycles();
  return NewMD;
}

MDNode *llvm::MapMetadata(const MDNode *MD, ValueToValueMapTy &VM,
                          RemapFlags Flags, ValueMapTypeRemapper *TypeMapper,
                          ValueMaterializer *Materializer) {
  return cast<MDNode>(MapMetadata(static_cast<const Metadata *>(MD), VM, Flags,
                                  TypeMapper, Materializer));
}

/// RemapInstruction - Convert the instruction operands from referencing the
/// current values into those specified by VMap.
///
void llvm::RemapInstruction(Instruction *I, ValueToValueMapTy &VMap,
                            RemapFlags Flags, ValueMapTypeRemapper *TypeMapper,
                            ValueMaterializer *Materializer){
  // Remap operands.
  for (User::op_iterator op = I->op_begin(), E = I->op_end(); op != E; ++op) {
    Value *V = MapValue(*op, VMap, Flags, TypeMapper, Materializer);
    // If we aren't ignoring missing entries, assert that something happened.
    if (V)
      *op = V;
    else
      assert((Flags & RF_IgnoreMissingEntries) &&
             "Referenced value not in value map!");
  }

  // Remap phi nodes' incoming blocks.
  if (PHINode *PN = dyn_cast<PHINode>(I)) {
    for (unsigned i = 0, e = PN->getNumIncomingValues(); i != e; ++i) {
      Value *V = MapValue(PN->getIncomingBlock(i), VMap, Flags);
      // If we aren't ignoring missing entries, assert that something happened.
      if (V)
        PN->setIncomingBlock(i, cast<BasicBlock>(V));
      else
        assert((Flags & RF_IgnoreMissingEntries) &&
               "Referenced block not in value map!");
    }
  }

  // Remap attached metadata.
  SmallVector<std::pair<unsigned, MDNode *>, 4> MDs;
  I->getAllMetadata(MDs);
  for (SmallVectorImpl<std::pair<unsigned, MDNode *>>::iterator
           MI = MDs.begin(),
           ME = MDs.end();
       MI != ME; ++MI) {
    MDNode *Old = MI->second;
    MDNode *New = MapMetadata(Old, VMap, Flags, TypeMapper, Materializer);
    if (New != Old)
      I->setMetadata(MI->first, New);
  }

  // If the instruction's type is being remapped, do so now.
  if (TypeMapper)
    I->mutateType(TypeMapper->remapType(I->getType()));
}
<|endoftext|>


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

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

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

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

  =========================================================================*/
// Wrappers
#include "otbWrapperApplicationFactory.h"
#include "otbWrapperChoiceParameter.h"
#include "otbWrapperElevationParametersHandler.h"
#include "otbWrapperMapProjectionParametersHandler.h"
#include "otbSensorModelAdapter.h"
#include "itkPoint.h"
#include "itkEuclideanDistanceMetric.h"
#include "otbGenericRSTransform.h"
#include "otbOGRDataSourceWrapper.h"
#include "ogrsf_frmts.h"

namespace otb
{
namespace Wrapper
{
class RefineSensorModel : public Application
{
public:
  /** Standard class typedefs. */
  typedef RefineSensorModel             Self;
  typedef Application                   Superclass;
  typedef itk::SmartPointer<Self>       Pointer;
  typedef itk::SmartPointer<const Self> ConstPointer;

  typedef itk::Point<double,3>                 PointType;
  typedef itk::Statistics::EuclideanDistanceMetric<PointType> DistanceType;

  typedef std::pair<PointType,PointType>       TiePointType;
  typedef std::vector<TiePointType>            TiePointsType;

  typedef otb::GenericRSTransform<double,3,3>  RSTransformType;

  /** Standard macro */
  itkNewMacro(Self);
  itkTypeMacro(RefineSensorModel, otb::Application);

private:
  void DoInit()
  {
    SetName("RefineSensorModel");
    SetDescription("Perform least-square fit of a sensor model to a set of tie points");

    SetDocName("Refine Sensor Model");
    SetDocLongDescription("This application reads a geom file containing a sensor model and a text file containing a list of ground control point, and performs a least-square fit of the sensor model adjustable parameters to these tie points. It produces an updated geom file as output, as well as an optional ground control points based statistics file and a vector file containing residues. The output geom file can then be used to ortho-rectify the data more accurately. Plaease note that for a proper use of the application, elevation must be correctly set (including DEM and geoid file). The map parameters allows one to choose a map projection in which the accuracy will be estimated in meters.");

    AddDocTag(Tags::Geometry);

    SetDocLimitations("None");
    SetDocSeeAlso("OrthoRectification,HomologousPointsExtraction");
    SetDocAuthors("OTB-Team");

    AddParameter(ParameterType_InputFilename,"ingeom","Input geom file");
    SetParameterDescription("ingeom","Geom file containing the sensor model to refine");

    AddParameter(ParameterType_OutputFilename,"outgeom","Output geom file");
    SetParameterDescription("outgeom","Geom file containing the refined sensor model");

    AddParameter(ParameterType_InputFilename,"inpoints","Input file containing tie points");
    SetParameterDescription("inpoints","Input file containing tie points. Points are stored in following format: row col lon lat. Line beginning with # are ignored.");

    AddParameter(ParameterType_OutputFilename,"outstat","Output file containing output precision statistics");
    SetParameterDescription("outstat","Output file containing the following info: ref_lon ref_lat elevation predicted_lon predicted_lat x_error_ref(meters) y_error_ref(meters) global_error_ref(meters) x_error(meters) y_error(meters) overall_error(meters)");
    MandatoryOff("outstat");
    DisableParameter("outstat");

    AddParameter(ParameterType_OutputFilename,"outvector","Output vector file with residues");
    SetParameterDescription("outvector","File containing segments representing residues");
    MandatoryOff("outvector");
    DisableParameter("outvector");

    // Build the Output Map Projection
    MapProjectionParametersHandler::AddMapProjectionParameters(this, "map");

    // Elevation
    ElevationParametersHandler::AddElevationParameters(this, "elev");

    // Doc example parameter settings
    SetDocExampleParameterValue("ingeom", "input.geom");
    SetDocExampleParameterValue("outgeom","output.geom");
    SetDocExampleParameterValue("inpoints","points.txt");
    SetDocExampleParameterValue("map","epsg");
    SetDocExampleParameterValue("map.epsg.code","32631");
  }

  void DoUpdateParameters()
  {
    // Nothing to do here : all parameters are independent
  }

  void DoExecute()
  {
    OGRMultiLineString mls;

    otb::SensorModelAdapter::Pointer sm     = otb::SensorModelAdapter::New();
    otb::SensorModelAdapter::Pointer sm_ref = otb::SensorModelAdapter::New();

    // Read the geom file
    sm->ReadGeomFile(GetParameterString("ingeom"));
    sm_ref->ReadGeomFile(GetParameterString("ingeom"));

    // Setup the DEM Handler
    otb::Wrapper::ElevationParametersHandler::SetupDEMHandlerFromElevationParameters(this,"elev");

    // Parse the input file for ground control points
    std::ifstream ifs;
    ifs.open(GetParameterString("inpoints").c_str());

  TiePointsType tiepoints;

  while(!ifs.eof())
    {
    std::string line;
    std::getline(ifs,line);

    double x,y,z,lat,lon;

    // Avoid commented lines or too short ones
    if (!line.empty() && line[0] != '#')
      {
      // retrieve the x component
      std::string::size_type pos = 0;
      std::string::size_type nextpos = line.find_first_of("\t", pos);
      x = atof(line.substr(pos, nextpos).c_str());
      pos = nextpos + 1;
      nextpos = line.find_first_of("\t", pos);
      y = atof(line.substr(pos, nextpos).c_str());
      pos = nextpos + 1;
      nextpos = line.find_first_of("\t", pos);
      lon = atof(line.substr(pos, nextpos).c_str());
      pos = nextpos + 1;
      nextpos = line.find_first_of("\t", pos);
      lat = atof(line.substr(pos, nextpos).c_str());

      z = otb::DEMHandler::Instance()->GetHeightAboveEllipsoid(lon,lat);

      otbAppLogINFO("Adding tie point x="<<x<<", y="<<y<<", z="<<z<<", lon="<<lon<<", lat="<<lat);

      sm->AddTiePoint(x,y,z,lon,lat);

      PointType p1,p2;
      p1[0]=x;
      p1[1]=y;
      p1[2]=z;
      p2[0]=lon;
      p2[1]=lat;
      p2[2]=z;

      tiepoints.push_back(std::make_pair(p1,p2));

      }
    }
  ifs.close();

  otbAppLogINFO("Optimization in progress ...");
  sm->Optimize();
  otbAppLogINFO("Done.\n");

  sm->WriteGeomFile(GetParameterString("outgeom"));

  double rmse = 0;
  double rmsex = 0;
  double rmsey = 0;

  double meanx = 0;
  double meany = 0;

  double rmse_ref = 0;
  double rmsex_ref = 0;
  double rmsey_ref = 0;

  double meanx_ref = 0;
  double meany_ref = 0;

  DistanceType::Pointer distance = DistanceType::New();

  RSTransformType::Pointer rsTransform = RSTransformType::New();
  rsTransform->SetOutputProjectionRef(MapProjectionParametersHandler::GetProjectionRefFromChoice(this, "map"));
  rsTransform->InstanciateTransform();

  std::ofstream ofs;
  ofs<<std::fixed;
  ofs.precision(12);
  if(IsParameterEnabled("outstat"))
    {
    ofs.open(GetParameterString("outstat").c_str());
    ofs<<"#ref_lon ref_lat elevation predicted_lon predicted_lat x_error_ref(meters) y_error_ref(meters) global_error_ref(meters) x_error(meters) y_error(meters) global_error(meters)"<<std::endl;
    }

  for(TiePointsType::const_iterator it = tiepoints.begin();
      it!=tiepoints.end(); ++it)
    {
    PointType tmpPoint,tmpPoint_ref,ref;
    sm->ForwardTransformPoint(it->first[0],it->first[1],it->first[2],tmpPoint[0],tmpPoint[1],tmpPoint[2]);
    sm_ref->ForwardTransformPoint(it->first[0],it->first[1],it->first[2],tmpPoint_ref[0],tmpPoint_ref[1],tmpPoint_ref[2]);

    tmpPoint = rsTransform->TransformPoint(tmpPoint);
    tmpPoint_ref = rsTransform->TransformPoint(tmpPoint_ref);

    ref[0] = it->second[0];
    ref[1] = it->second[1];
    ref[2] = it->second[2];

    ref = rsTransform->TransformPoint(ref);

    OGRLineString ls;
    ls.addPoint(tmpPoint[0],tmpPoint[1]);
    ls.addPoint(ref[0],ref[1]);
    mls.addGeometry(&ls);

    double gerror = distance->Evaluate(ref,tmpPoint);
    double xerror = ref[0]-tmpPoint[0];
    double yerror = ref[1]-tmpPoint[1];

    double gerror_ref = distance->Evaluate(ref,tmpPoint_ref);
    double xerror_ref = ref[0]-tmpPoint_ref[0];
    double yerror_ref = ref[1]-tmpPoint_ref[1];

    if(IsParameterEnabled("outstat"))
      ofs<<ref[0]<<"\t"<<ref[1]<<"\t"<<it->first[2]<<"\t"<<tmpPoint[0]<<"\t"<<tmpPoint[1]<<"\t"<<tmpPoint[2]<<"\t"<<xerror_ref<<"\t"<<yerror_ref<<"\t"<<gerror_ref<<"\t"<<xerror<<"\t"<<yerror<<"\t"<<gerror<<std::endl;

    rmse += gerror*gerror;
    rmsex+= xerror*xerror;
    rmsey+= yerror*yerror;

    meanx += xerror;
    meany += yerror;

    rmse_ref += gerror_ref*gerror_ref;
    rmsex_ref+=xerror_ref*xerror_ref;
    rmsey_ref+=yerror_ref*yerror_ref;

    meanx_ref += xerror_ref;
    meany_ref += yerror_ref;

    }

  rmse/=tiepoints.size();

  rmsex/=tiepoints.size();

  rmsey/=tiepoints.size();

  meanx/=tiepoints.size();
  meany/=tiepoints.size();

  rmse_ref/=tiepoints.size();

  rmsex_ref/=tiepoints.size();

  rmsey_ref/=tiepoints.size();

  meanx_ref/=tiepoints.size();
  meany_ref/=tiepoints.size();

  double stdevx = vcl_sqrt(rmsex - meanx * meanx);
  double stdevy = vcl_sqrt(rmsey - meany * meany);

  double stdevx_ref = vcl_sqrt(rmsex_ref - meanx_ref * meanx_ref);
  double stdevy_ref = vcl_sqrt(rmsey_ref - meany_ref * meany_ref);

  rmse=vcl_sqrt(rmse);
  rmsex=vcl_sqrt(rmsex);
  rmsey=vcl_sqrt(rmsey);

  rmse_ref=vcl_sqrt(rmse_ref);
  rmsex_ref=vcl_sqrt(rmsex_ref);
  rmsey_ref=vcl_sqrt(rmsey_ref);

  otbAppLogINFO("Estimation of input geom file accuracy: ");
  otbAppLogINFO("Overall Root Mean Square Error: "<<rmse_ref<<" meters");
  otbAppLogINFO("X Mean Error: "<<meanx_ref<<" meters");
  otbAppLogINFO("X standard deviation: "<<stdevx_ref<<" meters");
  otbAppLogINFO("X Root Mean Square Error: "<<rmsex_ref<<" meters");
  otbAppLogINFO("Y Mean Error: "<<meany_ref<<" meters");
  otbAppLogINFO("Y standard deviation: "<<stdevy_ref<<" meters");
  otbAppLogINFO("Y Root Mean Square Error: "<<rmsey_ref<<" meters\n");

  otbAppLogINFO("Estimation of final accuracy: ");

  otbAppLogINFO("Overall Root Mean Square Error: "<<rmse<<" meters");
  otbAppLogINFO("X Mean Error: "<<meanx<<" meters");
  otbAppLogINFO("X standard deviation: "<<stdevx<<" meters");
  otbAppLogINFO("X Root Mean Square Error: "<<rmsex<<" meters");
  otbAppLogINFO("Y Mean Error: "<<meany<<" meters");
  otbAppLogINFO("Y standard deviation: "<<stdevy<<" meters");
  otbAppLogINFO("Y Root Mean Square Error: "<<rmsey<<" meters");

  if(IsParameterEnabled("outstat"))
    ofs.close();

if(IsParameterEnabled("outvector"))
  {
  // Create the datasource (for matches export)
  otb::ogr::Layer layer(NULL, false);
  otb::ogr::DataSource::Pointer ogrDS;

  ogrDS = otb::ogr::DataSource::New(GetParameterString("outvector"), otb::ogr::DataSource::Modes::Overwrite);
  std::string projref = MapProjectionParametersHandler::GetProjectionRefFromChoice(this, "map");
  OGRSpatialReference oSRS(projref.c_str());

  // and create the layer
  layer = ogrDS->CreateLayer("matches", &oSRS, wkbMultiLineString);
  OGRFeatureDefn & defn = layer.GetLayerDefn();
  ogr::Feature feature(defn);

  feature.SetGeometry(&mls);
  layer.CreateFeature(feature);
  }
  }
};
}
}

OTB_APPLICATION_EXPORT(otb::Wrapper::RefineSensorModel)
<commit_msg>BUG: Mantis-1158: missing field in output header of RefineSensorModel<commit_after>/*=========================================================================

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

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

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

  =========================================================================*/
// Wrappers
#include "otbWrapperApplicationFactory.h"
#include "otbWrapperChoiceParameter.h"
#include "otbWrapperElevationParametersHandler.h"
#include "otbWrapperMapProjectionParametersHandler.h"
#include "otbSensorModelAdapter.h"
#include "itkPoint.h"
#include "itkEuclideanDistanceMetric.h"
#include "otbGenericRSTransform.h"
#include "otbOGRDataSourceWrapper.h"
#include "ogrsf_frmts.h"

namespace otb
{
namespace Wrapper
{
class RefineSensorModel : public Application
{
public:
  /** Standard class typedefs. */
  typedef RefineSensorModel             Self;
  typedef Application                   Superclass;
  typedef itk::SmartPointer<Self>       Pointer;
  typedef itk::SmartPointer<const Self> ConstPointer;

  typedef itk::Point<double,3>                 PointType;
  typedef itk::Statistics::EuclideanDistanceMetric<PointType> DistanceType;

  typedef std::pair<PointType,PointType>       TiePointType;
  typedef std::vector<TiePointType>            TiePointsType;

  typedef otb::GenericRSTransform<double,3,3>  RSTransformType;

  /** Standard macro */
  itkNewMacro(Self);
  itkTypeMacro(RefineSensorModel, otb::Application);

private:
  void DoInit()
  {
    SetName("RefineSensorModel");
    SetDescription("Perform least-square fit of a sensor model to a set of tie points");

    SetDocName("Refine Sensor Model");
    SetDocLongDescription("This application reads a geom file containing a sensor model and a text file containing a list of ground control point, and performs a least-square fit of the sensor model adjustable parameters to these tie points. It produces an updated geom file as output, as well as an optional ground control points based statistics file and a vector file containing residues. The output geom file can then be used to ortho-rectify the data more accurately. Plaease note that for a proper use of the application, elevation must be correctly set (including DEM and geoid file). The map parameters allows one to choose a map projection in which the accuracy will be estimated in meters.");

    AddDocTag(Tags::Geometry);

    SetDocLimitations("None");
    SetDocSeeAlso("OrthoRectification,HomologousPointsExtraction");
    SetDocAuthors("OTB-Team");

    AddParameter(ParameterType_InputFilename,"ingeom","Input geom file");
    SetParameterDescription("ingeom","Geom file containing the sensor model to refine");

    AddParameter(ParameterType_OutputFilename,"outgeom","Output geom file");
    SetParameterDescription("outgeom","Geom file containing the refined sensor model");

    AddParameter(ParameterType_InputFilename,"inpoints","Input file containing tie points");
    SetParameterDescription("inpoints","Input file containing tie points. Points are stored in following format: row col lon lat. Line beginning with # are ignored.");

    AddParameter(ParameterType_OutputFilename,"outstat","Output file containing output precision statistics");
    SetParameterDescription("outstat","Output file containing the following info: ref_lon ref_lat elevation predicted_lon predicted_lat x_error_ref(meters) y_error_ref(meters) global_error_ref(meters) x_error(meters) y_error(meters) overall_error(meters)");
    MandatoryOff("outstat");
    DisableParameter("outstat");

    AddParameter(ParameterType_OutputFilename,"outvector","Output vector file with residues");
    SetParameterDescription("outvector","File containing segments representing residues");
    MandatoryOff("outvector");
    DisableParameter("outvector");

    // Build the Output Map Projection
    MapProjectionParametersHandler::AddMapProjectionParameters(this, "map");

    // Elevation
    ElevationParametersHandler::AddElevationParameters(this, "elev");

    // Doc example parameter settings
    SetDocExampleParameterValue("ingeom", "input.geom");
    SetDocExampleParameterValue("outgeom","output.geom");
    SetDocExampleParameterValue("inpoints","points.txt");
    SetDocExampleParameterValue("map","epsg");
    SetDocExampleParameterValue("map.epsg.code","32631");
  }

  void DoUpdateParameters()
  {
    // Nothing to do here : all parameters are independent
  }

  void DoExecute()
  {
    OGRMultiLineString mls;

    otb::SensorModelAdapter::Pointer sm     = otb::SensorModelAdapter::New();
    otb::SensorModelAdapter::Pointer sm_ref = otb::SensorModelAdapter::New();

    // Read the geom file
    sm->ReadGeomFile(GetParameterString("ingeom"));
    sm_ref->ReadGeomFile(GetParameterString("ingeom"));

    // Setup the DEM Handler
    otb::Wrapper::ElevationParametersHandler::SetupDEMHandlerFromElevationParameters(this,"elev");

    // Parse the input file for ground control points
    std::ifstream ifs;
    ifs.open(GetParameterString("inpoints").c_str());

  TiePointsType tiepoints;

  while(!ifs.eof())
    {
    std::string line;
    std::getline(ifs,line);

    double x,y,z,lat,lon;

    // Avoid commented lines or too short ones
    if (!line.empty() && line[0] != '#')
      {
      // retrieve the x component
      std::string::size_type pos = 0;
      std::string::size_type nextpos = line.find_first_of("\t", pos);
      x = atof(line.substr(pos, nextpos).c_str());
      pos = nextpos + 1;
      nextpos = line.find_first_of("\t", pos);
      y = atof(line.substr(pos, nextpos).c_str());
      pos = nextpos + 1;
      nextpos = line.find_first_of("\t", pos);
      lon = atof(line.substr(pos, nextpos).c_str());
      pos = nextpos + 1;
      nextpos = line.find_first_of("\t", pos);
      lat = atof(line.substr(pos, nextpos).c_str());

      z = otb::DEMHandler::Instance()->GetHeightAboveEllipsoid(lon,lat);

      otbAppLogINFO("Adding tie point x="<<x<<", y="<<y<<", z="<<z<<", lon="<<lon<<", lat="<<lat);

      sm->AddTiePoint(x,y,z,lon,lat);

      PointType p1,p2;
      p1[0]=x;
      p1[1]=y;
      p1[2]=z;
      p2[0]=lon;
      p2[1]=lat;
      p2[2]=z;

      tiepoints.push_back(std::make_pair(p1,p2));

      }
    }
  ifs.close();

  otbAppLogINFO("Optimization in progress ...");
  sm->Optimize();
  otbAppLogINFO("Done.\n");

  sm->WriteGeomFile(GetParameterString("outgeom"));

  double rmse = 0;
  double rmsex = 0;
  double rmsey = 0;

  double meanx = 0;
  double meany = 0;

  double rmse_ref = 0;
  double rmsex_ref = 0;
  double rmsey_ref = 0;

  double meanx_ref = 0;
  double meany_ref = 0;

  DistanceType::Pointer distance = DistanceType::New();

  RSTransformType::Pointer rsTransform = RSTransformType::New();
  rsTransform->SetOutputProjectionRef(MapProjectionParametersHandler::GetProjectionRefFromChoice(this, "map"));
  rsTransform->InstanciateTransform();

  std::ofstream ofs;
  ofs<<std::fixed;
  ofs.precision(12);
  if(IsParameterEnabled("outstat"))
    {
    ofs.open(GetParameterString("outstat").c_str());
    ofs<<"#ref_lon ref_lat elevation predicted_lon predicted_lat predicted_elev x_error_ref(meters) y_error_ref(meters) global_error_ref(meters) x_error(meters) y_error(meters) global_error(meters)"<<std::endl;
    }

  for(TiePointsType::const_iterator it = tiepoints.begin();
      it!=tiepoints.end(); ++it)
    {
    PointType tmpPoint,tmpPoint_ref,ref;
    sm->ForwardTransformPoint(it->first[0],it->first[1],it->first[2],tmpPoint[0],tmpPoint[1],tmpPoint[2]);
    sm_ref->ForwardTransformPoint(it->first[0],it->first[1],it->first[2],tmpPoint_ref[0],tmpPoint_ref[1],tmpPoint_ref[2]);

    tmpPoint = rsTransform->TransformPoint(tmpPoint);
    tmpPoint_ref = rsTransform->TransformPoint(tmpPoint_ref);

    ref[0] = it->second[0];
    ref[1] = it->second[1];
    ref[2] = it->second[2];

    ref = rsTransform->TransformPoint(ref);

    OGRLineString ls;
    ls.addPoint(tmpPoint[0],tmpPoint[1]);
    ls.addPoint(ref[0],ref[1]);
    mls.addGeometry(&ls);

    double gerror = distance->Evaluate(ref,tmpPoint);
    double xerror = ref[0]-tmpPoint[0];
    double yerror = ref[1]-tmpPoint[1];

    double gerror_ref = distance->Evaluate(ref,tmpPoint_ref);
    double xerror_ref = ref[0]-tmpPoint_ref[0];
    double yerror_ref = ref[1]-tmpPoint_ref[1];

    if(IsParameterEnabled("outstat"))
      ofs<<ref[0]<<"\t"<<ref[1]<<"\t"<<it->first[2]<<"\t"<<tmpPoint[0]<<"\t"<<tmpPoint[1]<<"\t"<<tmpPoint[2]<<"\t"<<xerror_ref<<"\t"<<yerror_ref<<"\t"<<gerror_ref<<"\t"<<xerror<<"\t"<<yerror<<"\t"<<gerror<<std::endl;

    rmse += gerror*gerror;
    rmsex+= xerror*xerror;
    rmsey+= yerror*yerror;

    meanx += xerror;
    meany += yerror;

    rmse_ref += gerror_ref*gerror_ref;
    rmsex_ref+=xerror_ref*xerror_ref;
    rmsey_ref+=yerror_ref*yerror_ref;

    meanx_ref += xerror_ref;
    meany_ref += yerror_ref;

    }

  rmse/=tiepoints.size();

  rmsex/=tiepoints.size();

  rmsey/=tiepoints.size();

  meanx/=tiepoints.size();
  meany/=tiepoints.size();

  rmse_ref/=tiepoints.size();

  rmsex_ref/=tiepoints.size();

  rmsey_ref/=tiepoints.size();

  meanx_ref/=tiepoints.size();
  meany_ref/=tiepoints.size();

  double stdevx = vcl_sqrt(rmsex - meanx * meanx);
  double stdevy = vcl_sqrt(rmsey - meany * meany);

  double stdevx_ref = vcl_sqrt(rmsex_ref - meanx_ref * meanx_ref);
  double stdevy_ref = vcl_sqrt(rmsey_ref - meany_ref * meany_ref);

  rmse=vcl_sqrt(rmse);
  rmsex=vcl_sqrt(rmsex);
  rmsey=vcl_sqrt(rmsey);

  rmse_ref=vcl_sqrt(rmse_ref);
  rmsex_ref=vcl_sqrt(rmsex_ref);
  rmsey_ref=vcl_sqrt(rmsey_ref);

  otbAppLogINFO("Estimation of input geom file accuracy: ");
  otbAppLogINFO("Overall Root Mean Square Error: "<<rmse_ref<<" meters");
  otbAppLogINFO("X Mean Error: "<<meanx_ref<<" meters");
  otbAppLogINFO("X standard deviation: "<<stdevx_ref<<" meters");
  otbAppLogINFO("X Root Mean Square Error: "<<rmsex_ref<<" meters");
  otbAppLogINFO("Y Mean Error: "<<meany_ref<<" meters");
  otbAppLogINFO("Y standard deviation: "<<stdevy_ref<<" meters");
  otbAppLogINFO("Y Root Mean Square Error: "<<rmsey_ref<<" meters\n");

  otbAppLogINFO("Estimation of final accuracy: ");

  otbAppLogINFO("Overall Root Mean Square Error: "<<rmse<<" meters");
  otbAppLogINFO("X Mean Error: "<<meanx<<" meters");
  otbAppLogINFO("X standard deviation: "<<stdevx<<" meters");
  otbAppLogINFO("X Root Mean Square Error: "<<rmsex<<" meters");
  otbAppLogINFO("Y Mean Error: "<<meany<<" meters");
  otbAppLogINFO("Y standard deviation: "<<stdevy<<" meters");
  otbAppLogINFO("Y Root Mean Square Error: "<<rmsey<<" meters");

  if(IsParameterEnabled("outstat"))
    ofs.close();

if(IsParameterEnabled("outvector"))
  {
  // Create the datasource (for matches export)
  otb::ogr::Layer layer(NULL, false);
  otb::ogr::DataSource::Pointer ogrDS;

  ogrDS = otb::ogr::DataSource::New(GetParameterString("outvector"), otb::ogr::DataSource::Modes::Overwrite);
  std::string projref = MapProjectionParametersHandler::GetProjectionRefFromChoice(this, "map");
  OGRSpatialReference oSRS(projref.c_str());

  // and create the layer
  layer = ogrDS->CreateLayer("matches", &oSRS, wkbMultiLineString);
  OGRFeatureDefn & defn = layer.GetLayerDefn();
  ogr::Feature feature(defn);

  feature.SetGeometry(&mls);
  layer.CreateFeature(feature);
  }
  }
};
}
}

OTB_APPLICATION_EXPORT(otb::Wrapper::RefineSensorModel)
<|endoftext|>


<commit_before>/*
**  Copyright (C) 2013 Aldebaran Robotics
**  See COPYING for the license
*/
#include <qipython/pysignal.hpp>
#include <boost/python.hpp>
#include <boost/python/raw_function.hpp>
#include <qitype/signal.hpp>
#include <qitype/anyobject.hpp>
#include <qi/periodictask.hpp>
#include <qipython/gil.hpp>
#include <qipython/error.hpp>
#include <qipython/pyfuture.hpp>
#include <qipython/pyobject.hpp>
#include <qipython/pythreadsafeobject.hpp>

qiLogCategory("py.async");

namespace qi { namespace py {

    static void pyPeriodicCb(const PyThreadSafeObject& callable) {
      GILScopedLock _gil;
      PY_CATCH_ERROR(callable.object()());
    }

    class PyPeriodicTask : public qi::PeriodicTask {
    public:
      void setCallback(boost::python::object callable) {
        if (!PyCallable_Check(callable.ptr()))
          throw std::runtime_error("Not a callable");
        qi::PeriodicTask::setCallback(boost::bind<void>(pyPeriodicCb, PyThreadSafeObject(callable)));
      }

      void stop() {
        qi::py::GILScopedUnlock _unlock;
        //unlock because stop wait for the callback to finish.
        qi::PeriodicTask::stop();
      }
    };

    static boost::python::object pyAsync(PyThreadSafeObject safeargs) {
      GILScopedLock _gil;

      boost::python::list args(safeargs.object());
      boost::python::object callable = args[0];

      args.pop(0);

      try {
        return callable(*boost::python::tuple(args));
      } catch (const boost::python::error_already_set &) {
        throw std::runtime_error(PyFormatError());
      }
    }

    static boost::python::object pyasyncParamShrinker(boost::python::tuple args, boost::python::dict kwargs) {
      //arg0 always exists
      //check args[0] is a callable
      boost::python::object callable = args[0];
      if (!PyCallable_Check(callable.ptr()))
        throw std::runtime_error("Not a callable");

      qi::uint64_t delay = boost::python::extract<qi::uint64_t>(kwargs.get("delay", 0));

      //Does not use PyThreadSafeObject, because, setValue will be done under the lock
      //and the the future will be a PyFuture, that will be destroyed and use in the python world
      //under the lock too.
      boost::function<boost::python::object()> f = boost::bind(&pyAsync, PyThreadSafeObject(args));

      qi::Future<boost::python::object> fut = qi::getEventLoop()->async(f, delay);

      return boost::python::object(qi::py::toPyFuture(fut));
    }

    void export_pyasync() {
      boost::python::object async = boost::python::raw_function(&pyasyncParamShrinker, 1);

      async.attr("__doc__") = "async(callback [, delay=usec] [, arg1, ..., argn]) -> future\n"
                              ":param callback: the callback that will be called\n"
                              ":param delay: an optional delay in microseconds\n"
                              ":return: a future with the return value of the function\n"
                              "\n";

      boost::python::def("async", async);

      boost::python::class_<PyPeriodicTask, boost::shared_ptr<PyPeriodicTask>, boost::noncopyable >("PeriodicTask")
        .def(boost::python::init<>())
        .def("setCallback", &PyPeriodicTask::setCallback,
             "setCallback(callable)\n"
             ":param callable: a python callable, could be a method or a function\n"
             "\n"
             "set the callback used by the periodictask, this function can only be called once")
        .def("setUsPeriod", &PyPeriodicTask::setUsPeriod,
             "setUsPeriod(usPeriod)\n"
             ":param usPeriod: the period in microseconds\n"
             "\n"
             "Set the call interval in microseconds.\n"
             "This call will wait until next callback invocation to apply the change.\n"
             "To apply the change immediately, use: \n"
             "\n"
             ".. code-block:: python\n"
             "\n"
             "   task.stop()\n"
             "   task.setUsPeriod()\n"
             "   task.start()\n"
             "\n")
        .def("start", &PyPeriodicTask::start,
             "start(immediate)\n"
             ":param immediate: immediate if true, first schedule of the task will happen with no delay.\n"
             "\n"
             "start the periodic task at specified period. No effect if already running\n"
             "\n"
             ".. warning::\n"
             "   concurrent calls to start() and stop() will result in undefined behavior."
             "\n")
        .def("stop", &PyPeriodicTask::stop,
             "stop()\n"
             "Stop the periodic task. When this function returns, the callback will not be called anymore.\n"
             "Can be called from within the callback function\n"
             "\n"
             ".. warning::\n"
             "   concurrent calls to start() and stop() will result in undefined behavior."
             "\n")
        .def("asyncStop", &PyPeriodicTask::asyncStop,
             "asyncStop()\n"
             "Can be called from within the callback function\n"
             "Request for periodic task to stop asynchronously")
        .def("compensateCallbackTime", &PyPeriodicTask::compensateCallbackTime,
             "compensateCallbackTime(compensate)\n"
             ":param compensate: If true, call interval will take into account call duration to maintain the period.")
        .def("setName", &PyPeriodicTask::setName,
             "setName(name)\n"
             "Set name for debugging and tracking purpose")
        .def("isRunning", &PyPeriodicTask::isRunning,
             "isRunning() -> bool\n"
             ":return: true is task is running\n")
        .def("isStopping", &PyPeriodicTask::isStopping,
             "isStopping() -> bool\n"
             ":return: whether state is stopping or stopped.\n"
             "\n"
             "Can be called from within the callback to know if stop() or asyncStop()  was called.")
          ;
    }

  }
}
<commit_msg>python: periodictask: unlock start (which can call stop)<commit_after>/*
**  Copyright (C) 2013 Aldebaran Robotics
**  See COPYING for the license
*/
#include <qipython/pysignal.hpp>
#include <boost/python.hpp>
#include <boost/python/raw_function.hpp>
#include <qitype/signal.hpp>
#include <qitype/anyobject.hpp>
#include <qi/periodictask.hpp>
#include <qipython/gil.hpp>
#include <qipython/error.hpp>
#include <qipython/pyfuture.hpp>
#include <qipython/pyobject.hpp>
#include <qipython/pythreadsafeobject.hpp>

qiLogCategory("py.async");

namespace qi { namespace py {

    static void pyPeriodicCb(const PyThreadSafeObject& callable) {
      GILScopedLock _gil;
      PY_CATCH_ERROR(callable.object()());
    }

    class PyPeriodicTask : public qi::PeriodicTask {
    public:
      void setCallback(boost::python::object callable) {
        if (!PyCallable_Check(callable.ptr()))
          throw std::runtime_error("Not a callable");
        qi::PeriodicTask::setCallback(boost::bind<void>(pyPeriodicCb, PyThreadSafeObject(callable)));
      }

      void stop() {
        qi::py::GILScopedUnlock _unlock;
        //unlock because stop wait for the callback to finish.
        qi::PeriodicTask::stop();
      }

      void start(bool immediate) {
        qi::py::GILScopedUnlock _unlock;
        //unlock because stop wait for the callback to finish.
        qi::PeriodicTask::start(immediate);
      }

    };

    static boost::python::object pyAsync(PyThreadSafeObject safeargs) {
      GILScopedLock _gil;

      boost::python::list args(safeargs.object());
      boost::python::object callable = args[0];

      args.pop(0);

      try {
        return callable(*boost::python::tuple(args));
      } catch (const boost::python::error_already_set &) {
        throw std::runtime_error(PyFormatError());
      }
    }

    static boost::python::object pyasyncParamShrinker(boost::python::tuple args, boost::python::dict kwargs) {
      //arg0 always exists
      //check args[0] is a callable
      boost::python::object callable = args[0];
      if (!PyCallable_Check(callable.ptr()))
        throw std::runtime_error("Not a callable");

      qi::uint64_t delay = boost::python::extract<qi::uint64_t>(kwargs.get("delay", 0));

      //Does not use PyThreadSafeObject, because, setValue will be done under the lock
      //and the the future will be a PyFuture, that will be destroyed and use in the python world
      //under the lock too.
      boost::function<boost::python::object()> f = boost::bind(&pyAsync, PyThreadSafeObject(args));

      qi::Future<boost::python::object> fut = qi::getEventLoop()->async(f, delay);

      return boost::python::object(qi::py::toPyFuture(fut));
    }

    void export_pyasync() {
      boost::python::object async = boost::python::raw_function(&pyasyncParamShrinker, 1);

      async.attr("__doc__") = "async(callback [, delay=usec] [, arg1, ..., argn]) -> future\n"
                              ":param callback: the callback that will be called\n"
                              ":param delay: an optional delay in microseconds\n"
                              ":return: a future with the return value of the function\n"
                              "\n";

      boost::python::def("async", async);

      boost::python::class_<PyPeriodicTask, boost::shared_ptr<PyPeriodicTask>, boost::noncopyable >("PeriodicTask")
        .def(boost::python::init<>())
        .def("setCallback", &PyPeriodicTask::setCallback,
             "setCallback(callable)\n"
             ":param callable: a python callable, could be a method or a function\n"
             "\n"
             "set the callback used by the periodictask, this function can only be called once")
        .def("setUsPeriod", &PyPeriodicTask::setUsPeriod,
             "setUsPeriod(usPeriod)\n"
             ":param usPeriod: the period in microseconds\n"
             "\n"
             "Set the call interval in microseconds.\n"
             "This call will wait until next callback invocation to apply the change.\n"
             "To apply the change immediately, use: \n"
             "\n"
             ".. code-block:: python\n"
             "\n"
             "   task.stop()\n"
             "   task.setUsPeriod()\n"
             "   task.start()\n"
             "\n")
        .def("start", &PyPeriodicTask::start,
             "start(immediate)\n"
             ":param immediate: immediate if true, first schedule of the task will happen with no delay.\n"
             "\n"
             "start the periodic task at specified period. No effect if already running\n"
             "\n"
             ".. warning::\n"
             "   concurrent calls to start() and stop() will result in undefined behavior."
             "\n")
        .def("stop", &PyPeriodicTask::stop,
             "stop()\n"
             "Stop the periodic task. When this function returns, the callback will not be called anymore.\n"
             "Can be called from within the callback function\n"
             "\n"
             ".. warning::\n"
             "   concurrent calls to start() and stop() will result in undefined behavior."
             "\n")
        .def("asyncStop", &PyPeriodicTask::asyncStop,
             "asyncStop()\n"
             "Can be called from within the callback function\n"
             "Request for periodic task to stop asynchronously")
        .def("compensateCallbackTime", &PyPeriodicTask::compensateCallbackTime,
             "compensateCallbackTime(compensate)\n"
             ":param compensate: If true, call interval will take into account call duration to maintain the period.")
        .def("setName", &PyPeriodicTask::setName,
             "setName(name)\n"
             "Set name for debugging and tracking purpose")
        .def("isRunning", &PyPeriodicTask::isRunning,
             "isRunning() -> bool\n"
             ":return: true is task is running\n")
        .def("isStopping", &PyPeriodicTask::isStopping,
             "isStopping() -> bool\n"
             ":return: whether state is stopping or stopped.\n"
             "\n"
             "Can be called from within the callback to know if stop() or asyncStop()  was called.")
          ;
    }

  }
}
<|endoftext|>


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

//=============================================================================
// INCLUDES
//=============================================================================
#include "SimmPathMatrix.h"
#include <OpenSim/Simulation/Model/AbstractBody.h>
#include <ostream>

//=============================================================================
// STATICS
//=============================================================================
using namespace std;
using namespace OpenSim;

const int SimmPathMatrix::cSizeFactor = 2;
const int SimmPathMatrix::cHash1 = 27;
const int SimmPathMatrix::cHash2 = 7;

//=============================================================================
// CONSTRUCTOR(S) AND DESTRUCTOR
//=============================================================================
//_____________________________________________________________________________
/**
 * Default constructor.
 */
SimmPathMatrix::SimmPathMatrix(int size)
{
	if (size > 0)
	{
		_size = size * size * 2;
		_hashTable.reserve(_size);
		_factor = size;
		for (int i = 0; i < _size; i++)
			_hashTable[i] = NULL;
	}
	else
		_size = 0;
}

//_____________________________________________________________________________
/**
 * Destructor.
 */
SimmPathMatrix::~SimmPathMatrix()
{
	deletePaths();
}

//=============================================================================
// GET AND SET
//=============================================================================
//_____________________________________________________________________________
/**
 * Get a SimmPath between two bodies.
 *
 * @param aFromBody the first body
 * @param aToBody the second body
 * @return Pointer to the SimmPath from aFromBody to aToBody
 */
SimmPath* SimmPathMatrix::getSimmPath(const AbstractBody* aFromBody, const AbstractBody* aToBody) const
{
	int index = hash(aFromBody, aToBody);

	assert(index >= 0);

	return _hashTable[index];
}

//_____________________________________________________________________________
/**
 * Get a JointPath between two bodies.
 *
 * @param aFromBody the first body
 * @param aToBody the second body
 * @return Pointer to the JointPath from aFromBody to aToBody
 */
const JointPath* SimmPathMatrix::getPath(const AbstractBody* aFromBody, const AbstractBody* aToBody) const
{
	int index = hash(aFromBody, aToBody);

	assert(index >= 0);

	if (_hashTable[index])
		return &_hashTable[index]->getPath();

	return NULL;
}

//_____________________________________________________________________________
/**
 * Set the path between two bodies.
 *
 * @param aFromBody the first body
 * @param aToBody the second body
 * @param Pointer to the JointPath from aFromBody to aToBody
 */
void SimmPathMatrix::setPath(const AbstractBody* aFromBody, const AbstractBody* aToBody, JointPath aPath)
{
	int index = hash(aFromBody, aToBody);

	assert(index >= 0);

	_hashTable[index] = new SimmPath(aPath, aFromBody, aToBody);
}

//=============================================================================
// UTILITY
//=============================================================================
//_____________________________________________________________________________
/**
 * Delete all paths in the SimmPathMatrix.
 */
void SimmPathMatrix::deletePaths()
{
	for (unsigned int i = 0; i < _hashTable.size(); i++)
	{
		if (_hashTable[i])
			delete _hashTable[i];
	}

	_size = 0;
}

//_____________________________________________________________________________
/**
 * Initialize the has table which holds the paths
 */
void SimmPathMatrix::initTable(int size)
{
	/* If there are already some paths stored in the hash table, delete them. */
	if (_size > 0)
		deletePaths();

	/* Make room for the new paths. */
	if (size > 0)
	{
		_size = size * size * cSizeFactor;
		_hashTable.reserve(_size);
		_factor = size;
		for (int i = 0; i < _size; i++)
			_hashTable[i] = NULL;
	}
}

void SimmPathMatrix::invalidate()
{
	for (int i = 0; i < _size; i++)
		if (_hashTable[i])
			_hashTable[i]->invalidate();
}

//_____________________________________________________________________________
/**
 * Hash a pair of bodies
 *
 * @param aFromBody the first body
 * @param aToBody the second body
 * @return The integer hash value
 */
int SimmPathMatrix::hash(const AbstractBody* aFromBody, const AbstractBody* aToBody) const
{
	int hash_value = ((int)(intptr_t)aFromBody / cHash1 + (int)(intptr_t)aToBody) / cHash2 % _size;

	SimmPath* hashEntry;
	for (int i = 0; i < _size; i++)
	{
		hashEntry = _hashTable[hash_value];
		if ( hashEntry == NULL ||
			(hashEntry->getFromBody() == aFromBody && hashEntry->getToBody() == aToBody))
		{
			return hash_value;
		}
		hash_value++;
		if (hash_value >= _size)
			hash_value = 0;
	}

	return -1;
}

void SimmPathMatrix::peteTest() const
{
	cout << "SimmPathMatrix:" << endl;
	for (int i = 0; i < _size; i++)
	{
		if (_hashTable[i])
			_hashTable[i]->peteTest();
		else
			cout << "slot " << i << " is empty." << endl;
	}
}
<commit_msg>avoid ending up with negatie hash_value on a 64 bit machine<commit_after>// SimmPathMatrix.cpp
// Author: Peter Loan
/*
 * Copyright (c) 2006, Stanford University. All rights reserved.
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject
 * to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included
 * in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
 * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
 * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
 * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

//=============================================================================
// INCLUDES
//=============================================================================
#include "SimmPathMatrix.h"
#include <OpenSim/Simulation/Model/AbstractBody.h>
#include <ostream>

//=============================================================================
// STATICS
//=============================================================================
using namespace std;
using namespace OpenSim;

const int SimmPathMatrix::cSizeFactor = 2;
const int SimmPathMatrix::cHash1 = 27;
const int SimmPathMatrix::cHash2 = 7;

//=============================================================================
// CONSTRUCTOR(S) AND DESTRUCTOR
//=============================================================================
//_____________________________________________________________________________
/**
 * Default constructor.
 */
SimmPathMatrix::SimmPathMatrix(int size)
{
	if (size > 0)
	{
		_size = size * size * 2;
		_hashTable.reserve(_size);
		_factor = size;
		for (int i = 0; i < _size; i++)
			_hashTable[i] = NULL;
	}
	else
		_size = 0;
}

//_____________________________________________________________________________
/**
 * Destructor.
 */
SimmPathMatrix::~SimmPathMatrix()
{
	deletePaths();
}

//=============================================================================
// GET AND SET
//=============================================================================
//_____________________________________________________________________________
/**
 * Get a SimmPath between two bodies.
 *
 * @param aFromBody the first body
 * @param aToBody the second body
 * @return Pointer to the SimmPath from aFromBody to aToBody
 */
SimmPath* SimmPathMatrix::getSimmPath(const AbstractBody* aFromBody, const AbstractBody* aToBody) const
{
	int index = hash(aFromBody, aToBody);

	assert(index >= 0);

	return _hashTable[index];
}

//_____________________________________________________________________________
/**
 * Get a JointPath between two bodies.
 *
 * @param aFromBody the first body
 * @param aToBody the second body
 * @return Pointer to the JointPath from aFromBody to aToBody
 */
const JointPath* SimmPathMatrix::getPath(const AbstractBody* aFromBody, const AbstractBody* aToBody) const
{
	int index = hash(aFromBody, aToBody);

	assert(index >= 0);

	if (_hashTable[index])
		return &_hashTable[index]->getPath();

	return NULL;
}

//_____________________________________________________________________________
/**
 * Set the path between two bodies.
 *
 * @param aFromBody the first body
 * @param aToBody the second body
 * @param Pointer to the JointPath from aFromBody to aToBody
 */
void SimmPathMatrix::setPath(const AbstractBody* aFromBody, const AbstractBody* aToBody, JointPath aPath)
{
	int index = hash(aFromBody, aToBody);

	assert(index >= 0);

	_hashTable[index] = new SimmPath(aPath, aFromBody, aToBody);
}

//=============================================================================
// UTILITY
//=============================================================================
//_____________________________________________________________________________
/**
 * Delete all paths in the SimmPathMatrix.
 */
void SimmPathMatrix::deletePaths()
{
	for (unsigned int i = 0; i < _hashTable.size(); i++)
	{
		if (_hashTable[i])
			delete _hashTable[i];
	}

	_size = 0;
}

//_____________________________________________________________________________
/**
 * Initialize the has table which holds the paths
 */
void SimmPathMatrix::initTable(int size)
{
	/* If there are already some paths stored in the hash table, delete them. */
	if (_size > 0)
		deletePaths();

	/* Make room for the new paths. */
	if (size > 0)
	{
		_size = size * size * cSizeFactor;
		_hashTable.reserve(_size);
		_factor = size;
		for (int i = 0; i < _size; i++)
			_hashTable[i] = NULL;
	}
}

void SimmPathMatrix::invalidate()
{
	for (int i = 0; i < _size; i++)
		if (_hashTable[i])
			_hashTable[i]->invalidate();
}

//_____________________________________________________________________________
/**
 * Hash a pair of bodies
 *
 * @param aFromBody the first body
 * @param aToBody the second body
 * @return The integer hash value
 */
int SimmPathMatrix::hash(const AbstractBody* aFromBody, const AbstractBody* aToBody) const
{
	unsigned int hash_value = ((unsigned int)(uintptr_t)aFromBody / cHash1 + (unsigned int)(uintptr_t)aToBody) / cHash2 % _size;

	SimmPath* hashEntry;
	for (int i = 0; i < _size; i++)
	{
		hashEntry = _hashTable[hash_value];
		if ( hashEntry == NULL ||
			(hashEntry->getFromBody() == aFromBody && hashEntry->getToBody() == aToBody))
		{
			return hash_value;
		}
		hash_value++;
		if (hash_value >= (unsigned int)_size)
			hash_value = 0;
	}

	return -1;
}

void SimmPathMatrix::peteTest() const
{
	cout << "SimmPathMatrix:" << endl;
	for (int i = 0; i < _size; i++)
	{
		if (_hashTable[i])
			_hashTable[i]->peteTest();
		else
			cout << "slot " << i << " is empty." << endl;
	}
}
<|endoftext|>


<commit_before>#include <kv/Heine.hpp>
#include <kv/qAiry.hpp>
#include <kv/qBessel.hpp>
#include <cmath>
typedef kv::interval<double> itv;
typedef kv::complex< kv::interval<double> > cp;
using namespace std;
int main()
{
  cout.precision(17);
  int n=20;
  itv x,nu,q,xx;
  q="0.8";
  nu=1.5;
  x=2.5;
  for(int i=1;i<=n;i++){
    xx=mid(x);
    x=xx-kv::Jackson2(itv(xx),itv(nu),itv(q))*(1-q)*xx
      /(kv::Jackson2(itv(xx),itv(nu),itv(q))-kv::Jackson2(itv(q*xx),itv(nu),itv(q)))
      +(1-xx*(kv::Jackson2(itv(x),itv(nu),itv(q))-kv::Jackson2(itv(q*x),itv(nu),itv(q)))
	/x/(kv::Jackson2(itv(xx),itv(nu),itv(q))-kv::Jackson2(itv(q*xx),itv(nu),itv(q))))*(x-xx);
  cout<<x<<endl;
  cout<<"value of J2"<<kv::Jackson2(itv(x),itv(nu),itv(q))<<endl;
  }
}
<commit_msg>Update J2qKrawczyk.cc<commit_after>#include <kv/Heine.hpp>
#include <kv/qAiry.hpp>
#include <kv/qBessel.hpp>
#include <cmath>
typedef kv::interval<double> itv;
typedef kv::complex< kv::interval<double> > cp;
using namespace std;
int main()
{
  cout.precision(17);
  int n=20;
  itv x,nu,q,xx;
  q="0.8";
  nu=1.5;
  x=2.5;
  for(int i=1;i<=n;i++){
    xx=mid(x);
    x=xx-kv::Jackson2(itv(xx),itv(nu),itv(q))*(1-q)*xx
      /(kv::Jackson2(itv(xx),itv(nu),itv(q))-kv::Jackson2(itv(q*xx),itv(nu),itv(q)))
      +(1-xx*(kv::Jackson2(itv(x),itv(nu),itv(q))-kv::Jackson2(itv(q*x),itv(nu),itv(q)))
	/x/(kv::Jackson2(itv(xx),itv(nu),itv(q))-kv::Jackson2(itv(q*xx),itv(nu),itv(q))))*(x-xx);
  cout<<x<<endl;
  cout<<"value of J2 inf"<<kv::Jackson2(itv(x.lower()),itv(nu),itv(q))<<endl;
  cout<<"value of J2 sup"<<kv::Jackson2(itv(x.upper()),itv(nu),itv(q))<<endl;
  cout<<"value of J2 mid"<<kv::Jackson2(itv(mid(x)),itv(nu),itv(q))<<endl;
  }
}
<|endoftext|>