<commit_before>//-----------------------------------------------------------------------------
// HIMG, by Marcus Geelnard, 2015
//
// This is free and unencumbered software released into the public domain.
//
// See LICENSE for details.
//-----------------------------------------------------------------------------

#include "huffman.h"

namespace himg {

namespace {

// A symbol is an 8-bit unigned number.
typedef uint16_t Symbol;
const int kSymbolSize = 9;
const int kNumSymbols = 261;

// Special symbols for RLE.
const Symbol kSymTwoZeros = 256;        // 2            (0 bits)
const Symbol kSymUpTo6Zeros = 257;      // 3 - 6        (2 bits)
const Symbol kSymUpTo22Zeros = 258;     // 7 - 22       (4 bits)
const Symbol kSymUpTo278Zeros = 259;    // 23 - 278     (8 bits)
const Symbol kSymUpTo16662Zeros = 260;  // 279 - 16662  (14 bits)

// The maximum number of nodes in the Huffman tree (branch nodes + leaf nodes).
const int kMaxTreeNodes = (kNumSymbols * 2) - 1;

// The maximum size of the tree representation (there are two additional bits
// per leaf node, representing the branches in the tree).
const int kMaxTreeDataSize = ((2 + kSymbolSize) * kNumSymbols + 7) / 8;

class InBitstream {
 public:
  // Initialize a bitstream.
  explicit InBitstream(const uint8_t *buf) : m_byte_ptr(buf), m_bit_pos(0) {}

  // Read one bit from a bitstream.
  int ReadBit() {
    // Get current stream state.
    const uint8_t *buf = m_byte_ptr;
    int bit = m_bit_pos;

    // Extract bit.
    int x = (*buf & (1 << (7 - bit))) ? 1 : 0;
    bit = (bit + 1) & 7;
    if (!bit) {
      ++buf;
    }

    // Store new stream state.
    m_bit_pos = bit;
    m_byte_ptr = buf;

    return x;
  }

  // Read bits from a bitstream.
  uint32_t ReadBits(int bits) {
    // TODO(m): Optimize this.
    uint32_t x = 0;
    for (int k = 0; k < bits; ++k)
      x = (x << 1) | ReadBit();

    return x;
  }

 private:
  const uint8_t *m_byte_ptr;
  int m_bit_pos;
};

class OutBitstream {
 public:
  // Initialize a bitstream.
  explicit OutBitstream(uint8_t *buf)
      : m_base_ptr(buf), m_byte_ptr(buf), m_bit_pos(0) {}

  // Write bits to a bitstream.
  void WriteBits(uint32_t x, int bits) {
    // Get current stream state.
    uint8_t *buf = m_byte_ptr;
    int bit = m_bit_pos;

    // Append bits.
    uint32_t mask = 1 << (bits - 1);
    for (int count = 0; count < bits; ++count) {
      *buf = (*buf & (0xff ^ (1 << (7 - bit)))) +
             ((x & mask ? 1 : 0) << (7 - bit));
      x <<= 1;
      bit = (bit + 1) & 7;
      if (!bit) {
        ++buf;
      }
    }

    // Store new stream state.
    m_byte_ptr = buf;
    m_bit_pos = bit;
  }

  int Size() const {
    int total_bytes = static_cast<int>(m_byte_ptr - m_base_ptr);
    if (m_bit_pos > 0) {
      ++total_bytes;
    }
    return total_bytes;
  }

 private:
  uint8_t *m_base_ptr;
  uint8_t *m_byte_ptr;
  int m_bit_pos;
};

// Used by the encoder for building the optimal Huffman tree.
struct SymbolInfo {
  Symbol symbol;
  int count;
  uint32_t code;
  int bits;
};

struct EncodeNode {
  EncodeNode *child_a, *child_b;
  int count;
  int symbol;
};

struct DecodeNode {
  DecodeNode *child_a, *child_b;
  int symbol;
};

// Calculate (sorted) histogram for a block of data.
void Histogram(const uint8_t *in, SymbolInfo *symbols, int size) {
  // Clear/init histogram.
  for (int k = 0; k < kNumSymbols; ++k) {
    symbols[k].symbol = static_cast<Symbol>(k);
    symbols[k].count = 0;
    symbols[k].code = 0;
    symbols[k].bits = 0;
  }

  // Build histogram.
  for (int k = 0; k < size;) {
    Symbol symbol = static_cast<Symbol>(in[k]);

    // Possible RLE?
    if (symbol == 0) {
      int zeros;
      for (zeros = 1; zeros < 16662 && (k + zeros) < size; ++zeros) {
        if (in[k + zeros] != 0)
          break;
      }
      if (zeros == 1) {
        symbols[0].count++;
      } else if (zeros == 2) {
        symbols[kSymTwoZeros].count++;
      } else if (zeros <= 6) {
        symbols[kSymUpTo6Zeros].count++;
      } else if (zeros <= 22) {
        symbols[kSymUpTo22Zeros].count++;
      } else if (zeros <= 278) {
        symbols[kSymUpTo278Zeros].count++;
      } else {
        symbols[kSymUpTo16662Zeros].count++;
      }
      k += zeros;
    } else {
      symbols[symbol].count++;
      k++;
    }
  }
}

// Store a Huffman tree in the output stream and in a look-up-table (a symbol
// array).
void StoreTree(EncodeNode *node,
               SymbolInfo *symbols,
               OutBitstream *stream,
               uint32_t code,
               int bits) {
  // Is this a leaf node?
  if (node->symbol >= 0) {
    // Append symbol to tree description.
    stream->WriteBits(1, 1);
    stream->WriteBits(static_cast<uint32_t>(node->symbol), kSymbolSize);

    // Find symbol index.
    int sym_idx;
    for (sym_idx = 0; sym_idx < kNumSymbols; ++sym_idx) {
      if (symbols[sym_idx].symbol == static_cast<Symbol>(node->symbol))
        break;
    }

    // Store code info in symbol array.
    symbols[sym_idx].code = code;
    symbols[sym_idx].bits = bits;
    return;
  } else {
    // This was not a leaf node.
    stream->WriteBits(0, 1);
  }

  // Branch A.
  StoreTree(node->child_a, symbols, stream, (code << 1) + 0, bits + 1);

  // Branch B.
  StoreTree(node->child_b, symbols, stream, (code << 1) + 1, bits + 1);
}

// Generate a Huffman tree.
void MakeTree(SymbolInfo *sym, OutBitstream *stream) {
  // Initialize all leaf nodes.
  EncodeNode nodes[kMaxTreeNodes];
  int num_symbols = 0;
  for (int k = 0; k < kNumSymbols; ++k) {
    if (sym[k].count > 0) {
      nodes[num_symbols].symbol = static_cast<int>(sym[k].symbol);
      nodes[num_symbols].count = sym[k].count;
      nodes[num_symbols].child_a = nullptr;
      nodes[num_symbols].child_b = nullptr;
      ++num_symbols;
    }
  }

  // Build tree by joining the lightest nodes until there is only one node left
  // (the root node).
  EncodeNode *root = nullptr;
  int nodes_left = num_symbols;
  int next_idx = num_symbols;
  while (nodes_left > 1) {
    // Find the two lightest nodes.
    EncodeNode *node_1 = nullptr;
    EncodeNode *node_2 = nullptr;
    for (int k = 0; k < next_idx; ++k) {
      if (nodes[k].count > 0) {
        if (!node_1 || (nodes[k].count <= node_1->count)) {
          node_2 = node_1;
          node_1 = &nodes[k];
        } else if (!node_2 || (nodes[k].count <= node_2->count)) {
          node_2 = &nodes[k];
        }
      }
    }

    // Join the two nodes into a new parent node.
    root = &nodes[next_idx];
    root->child_a = node_1;
    root->child_b = node_2;
    root->count = node_1->count + node_2->count;
    root->symbol = -1;
    node_1->count = 0;
    node_2->count = 0;
    ++next_idx;
    --nodes_left;
  }

  // Store the tree in the output stream, and in the sym[] array (the latter is
  // used as a look-up-table for faster encoding).
  if (root) {
    StoreTree(root, sym, stream, 0, 0);
  } else {
    // Special case: only one symbol => no binary tree.
    root = &nodes[0];
    StoreTree(root, sym, stream, 0, 1);
  }
}

// Recover a Huffman tree from a bitstream.
DecodeNode *RecoverTree(DecodeNode *nodes, InBitstream *stream, int *nodenum) {
  // Pick a node from the node array.
  DecodeNode *this_node = &nodes[*nodenum];
  *nodenum = *nodenum + 1;

  // Clear the node.
  this_node->symbol = -1;
  this_node->child_a = nullptr;
  this_node->child_b = nullptr;

  // Is this a leaf node?
  if (stream->ReadBit()) {
    // Get symbol from tree description and store in lead node.
    this_node->symbol = static_cast<int>(stream->ReadBits(kSymbolSize));

    return this_node;
  }

  // Get branch A.
  this_node->child_a = RecoverTree(nodes, stream, nodenum);

  // Get branch B.
  this_node->child_b = RecoverTree(nodes, stream, nodenum);

  return this_node;
}

}  // namespace

int Huffman::MaxCompressedSize(int uncompressed_size) {
  return uncompressed_size + kMaxTreeDataSize;
}

int Huffman::Compress(uint8_t *out, const uint8_t *in, int in_size) {
  // Do we have anything to compress?
  if (in_size < 1)
    return 0;

  // Initialize bitstream.
  OutBitstream stream(out);

  // Calculate and sort histogram for input data.
  SymbolInfo symbols[kNumSymbols];
  Histogram(in, symbols, in_size);

  // Build Huffman tree.
  MakeTree(symbols, &stream);

  // Sort histogram - first symbol first (bubble sort).
  // TODO(m): Quick-sort.
  int swaps;
  do {
    swaps = 0;
    for (int k = 0; k < kNumSymbols - 1; ++k) {
      if (symbols[k].symbol > symbols[k + 1].symbol) {
        SymbolInfo tmp = symbols[k];
        symbols[k] = symbols[k + 1];
        symbols[k + 1] = tmp;
        swaps = 1;
      }
    }
  } while (swaps);

  // Encode input stream.
  for (int k = 0; k < in_size;) {
    uint8_t symbol = in[k];

    // Possible RLE?
    if (symbol == 0) {
      int zeros;
      for (zeros = 1; zeros < 16662 && (k + zeros) < in_size; ++zeros) {
        if (in[k + zeros] != 0)
          break;
      }
      if (zeros == 1) {
        stream.WriteBits(symbols[0].code, symbols[0].bits);
      } else if (zeros == 2) {
        stream.WriteBits(symbols[kSymTwoZeros].code,
                         symbols[kSymTwoZeros].bits);
      } else if (zeros <= 6) {
        uint32_t count = static_cast<uint32_t>(zeros - 3);
        stream.WriteBits(symbols[kSymUpTo6Zeros].code,
                         symbols[kSymUpTo6Zeros].bits);
        stream.WriteBits(count, 2);
      } else if (zeros <= 22) {
        uint32_t count = static_cast<uint32_t>(zeros - 7);
        stream.WriteBits(symbols[kSymUpTo22Zeros].code,
                         symbols[kSymUpTo22Zeros].bits);
        stream.WriteBits(count, 4);
      } else if (zeros <= 278) {
        uint32_t count = static_cast<uint32_t>(zeros - 23);
        stream.WriteBits(symbols[kSymUpTo278Zeros].code,
                         symbols[kSymUpTo278Zeros].bits);
        stream.WriteBits(count, 8);
      } else {
        uint32_t count = static_cast<uint32_t>(zeros - 279);
        stream.WriteBits(symbols[kSymUpTo16662Zeros].code,
                         symbols[kSymUpTo16662Zeros].bits);
        stream.WriteBits(count, 14);
      }
      k += zeros;
    } else {
      stream.WriteBits(symbols[symbol].code, symbols[symbol].bits);
      k++;
    }
  }

  // Calculate size of output data.
  return stream.Size();
}

void Huffman::Uncompress(uint8_t *out,
                         const uint8_t *in,
                         int in_size,
                         int out_size) {
  // Do we have anything to decompress?
  if (in_size < 1)
    return;

  // Initialize bitstream.
  InBitstream stream(in);

  // Recover Huffman tree.
  int node_count = 0;
  DecodeNode nodes[kMaxTreeNodes];
  DecodeNode *root = RecoverTree(nodes, &stream, &node_count);

  // Decode input stream.
  uint8_t *buf = out;
  for (int k = 0; k < out_size;) {
    // Traverse tree until we find a matching leaf node.
    // TODO(m): Improve the performance!
    DecodeNode *node = root;
    while (node->symbol < 0) {
      // Get next node.
      if (stream.ReadBit())
        node = node->child_b;
      else
        node = node->child_a;
    }

    // Decode as RLE or plain copy.
    uint8_t symbol;
    int count;
    switch (node->symbol) {
      case kSymTwoZeros: {
        symbol = 0;
        count = 2;
        break;
      }
      case kSymUpTo6Zeros: {
        symbol = 0;
        count = static_cast<int>(stream.ReadBits(2)) + 3;
        break;
      }
      case kSymUpTo22Zeros: {
        symbol = 0;
        count = static_cast<int>(stream.ReadBits(4)) + 7;
        break;
      }
      case kSymUpTo278Zeros: {
        symbol = 0;
        count = static_cast<int>(stream.ReadBits(8)) + 23;
        break;
      }
      case kSymUpTo16662Zeros: {
        symbol = 0;
        count = static_cast<int>(stream.ReadBits(14)) + 279;
        break;
      }
      default: {
        symbol = static_cast<uint8_t>(node->symbol);
        count = 1;
        break;
      }
    }

    for (int i = 0; i < count && (k + i) < out_size; ++i)
      *buf++ = symbol;
    k += count;
  }
}

}  // namespace himg
<commit_msg>Fixed typos<commit_after>//-----------------------------------------------------------------------------
// HIMG, by Marcus Geelnard, 2015
//
// This is free and unencumbered software released into the public domain.
//
// See LICENSE for details.
//-----------------------------------------------------------------------------

#include "huffman.h"

namespace himg {

namespace {

// A symbol is a 9-bit unsigned number.
typedef uint16_t Symbol;
const int kSymbolSize = 9;
const int kNumSymbols = 261;

// Special symbols for RLE.
const Symbol kSymTwoZeros = 256;        // 2            (0 bits)
const Symbol kSymUpTo6Zeros = 257;      // 3 - 6        (2 bits)
const Symbol kSymUpTo22Zeros = 258;     // 7 - 22       (4 bits)
const Symbol kSymUpTo278Zeros = 259;    // 23 - 278     (8 bits)
const Symbol kSymUpTo16662Zeros = 260;  // 279 - 16662  (14 bits)

// The maximum number of nodes in the Huffman tree (branch nodes + leaf nodes).
const int kMaxTreeNodes = (kNumSymbols * 2) - 1;

// The maximum size of the tree representation (there are two additional bits
// per leaf node, representing the branches in the tree).
const int kMaxTreeDataSize = ((2 + kSymbolSize) * kNumSymbols + 7) / 8;

class InBitstream {
 public:
  // Initialize a bitstream.
  explicit InBitstream(const uint8_t *buf) : m_byte_ptr(buf), m_bit_pos(0) {}

  // Read one bit from a bitstream.
  int ReadBit() {
    // Get current stream state.
    const uint8_t *buf = m_byte_ptr;
    int bit = m_bit_pos;

    // Extract bit.
    int x = (*buf & (1 << (7 - bit))) ? 1 : 0;
    bit = (bit + 1) & 7;
    if (!bit) {
      ++buf;
    }

    // Store new stream state.
    m_bit_pos = bit;
    m_byte_ptr = buf;

    return x;
  }

  // Read bits from a bitstream.
  uint32_t ReadBits(int bits) {
    // TODO(m): Optimize this.
    uint32_t x = 0;
    for (int k = 0; k < bits; ++k)
      x = (x << 1) | ReadBit();

    return x;
  }

 private:
  const uint8_t *m_byte_ptr;
  int m_bit_pos;
};

class OutBitstream {
 public:
  // Initialize a bitstream.
  explicit OutBitstream(uint8_t *buf)
      : m_base_ptr(buf), m_byte_ptr(buf), m_bit_pos(0) {}

  // Write bits to a bitstream.
  void WriteBits(uint32_t x, int bits) {
    // Get current stream state.
    uint8_t *buf = m_byte_ptr;
    int bit = m_bit_pos;

    // Append bits.
    uint32_t mask = 1 << (bits - 1);
    for (int count = 0; count < bits; ++count) {
      *buf = (*buf & (0xff ^ (1 << (7 - bit)))) +
             ((x & mask ? 1 : 0) << (7 - bit));
      x <<= 1;
      bit = (bit + 1) & 7;
      if (!bit) {
        ++buf;
      }
    }

    // Store new stream state.
    m_byte_ptr = buf;
    m_bit_pos = bit;
  }

  int Size() const {
    int total_bytes = static_cast<int>(m_byte_ptr - m_base_ptr);
    if (m_bit_pos > 0) {
      ++total_bytes;
    }
    return total_bytes;
  }

 private:
  uint8_t *m_base_ptr;
  uint8_t *m_byte_ptr;
  int m_bit_pos;
};

// Used by the encoder for building the optimal Huffman tree.
struct SymbolInfo {
  Symbol symbol;
  int count;
  uint32_t code;
  int bits;
};

struct EncodeNode {
  EncodeNode *child_a, *child_b;
  int count;
  int symbol;
};

struct DecodeNode {
  DecodeNode *child_a, *child_b;
  int symbol;
};

// Calculate (sorted) histogram for a block of data.
void Histogram(const uint8_t *in, SymbolInfo *symbols, int size) {
  // Clear/init histogram.
  for (int k = 0; k < kNumSymbols; ++k) {
    symbols[k].symbol = static_cast<Symbol>(k);
    symbols[k].count = 0;
    symbols[k].code = 0;
    symbols[k].bits = 0;
  }

  // Build histogram.
  for (int k = 0; k < size;) {
    Symbol symbol = static_cast<Symbol>(in[k]);

    // Possible RLE?
    if (symbol == 0) {
      int zeros;
      for (zeros = 1; zeros < 16662 && (k + zeros) < size; ++zeros) {
        if (in[k + zeros] != 0)
          break;
      }
      if (zeros == 1) {
        symbols[0].count++;
      } else if (zeros == 2) {
        symbols[kSymTwoZeros].count++;
      } else if (zeros <= 6) {
        symbols[kSymUpTo6Zeros].count++;
      } else if (zeros <= 22) {
        symbols[kSymUpTo22Zeros].count++;
      } else if (zeros <= 278) {
        symbols[kSymUpTo278Zeros].count++;
      } else {
        symbols[kSymUpTo16662Zeros].count++;
      }
      k += zeros;
    } else {
      symbols[symbol].count++;
      k++;
    }
  }
}

// Store a Huffman tree in the output stream and in a look-up-table (a symbol
// array).
void StoreTree(EncodeNode *node,
               SymbolInfo *symbols,
               OutBitstream *stream,
               uint32_t code,
               int bits) {
  // Is this a leaf node?
  if (node->symbol >= 0) {
    // Append symbol to tree description.
    stream->WriteBits(1, 1);
    stream->WriteBits(static_cast<uint32_t>(node->symbol), kSymbolSize);

    // Find symbol index.
    int sym_idx;
    for (sym_idx = 0; sym_idx < kNumSymbols; ++sym_idx) {
      if (symbols[sym_idx].symbol == static_cast<Symbol>(node->symbol))
        break;
    }

    // Store code info in symbol array.
    symbols[sym_idx].code = code;
    symbols[sym_idx].bits = bits;
    return;
  } else {
    // This was not a leaf node.
    stream->WriteBits(0, 1);
  }

  // Branch A.
  StoreTree(node->child_a, symbols, stream, (code << 1) + 0, bits + 1);

  // Branch B.
  StoreTree(node->child_b, symbols, stream, (code << 1) + 1, bits + 1);
}

// Generate a Huffman tree.
void MakeTree(SymbolInfo *sym, OutBitstream *stream) {
  // Initialize all leaf nodes.
  EncodeNode nodes[kMaxTreeNodes];
  int num_symbols = 0;
  for (int k = 0; k < kNumSymbols; ++k) {
    if (sym[k].count > 0) {
      nodes[num_symbols].symbol = static_cast<int>(sym[k].symbol);
      nodes[num_symbols].count = sym[k].count;
      nodes[num_symbols].child_a = nullptr;
      nodes[num_symbols].child_b = nullptr;
      ++num_symbols;
    }
  }

  // Build tree by joining the lightest nodes until there is only one node left
  // (the root node).
  EncodeNode *root = nullptr;
  int nodes_left = num_symbols;
  int next_idx = num_symbols;
  while (nodes_left > 1) {
    // Find the two lightest nodes.
    EncodeNode *node_1 = nullptr;
    EncodeNode *node_2 = nullptr;
    for (int k = 0; k < next_idx; ++k) {
      if (nodes[k].count > 0) {
        if (!node_1 || (nodes[k].count <= node_1->count)) {
          node_2 = node_1;
          node_1 = &nodes[k];
        } else if (!node_2 || (nodes[k].count <= node_2->count)) {
          node_2 = &nodes[k];
        }
      }
    }

    // Join the two nodes into a new parent node.
    root = &nodes[next_idx];
    root->child_a = node_1;
    root->child_b = node_2;
    root->count = node_1->count + node_2->count;
    root->symbol = -1;
    node_1->count = 0;
    node_2->count = 0;
    ++next_idx;
    --nodes_left;
  }

  // Store the tree in the output stream, and in the sym[] array (the latter is
  // used as a look-up-table for faster encoding).
  if (root) {
    StoreTree(root, sym, stream, 0, 0);
  } else {
    // Special case: only one symbol => no binary tree.
    root = &nodes[0];
    StoreTree(root, sym, stream, 0, 1);
  }
}

// Recover a Huffman tree from a bitstream.
DecodeNode *RecoverTree(DecodeNode *nodes, InBitstream *stream, int *nodenum) {
  // Pick a node from the node array.
  DecodeNode *this_node = &nodes[*nodenum];
  *nodenum = *nodenum + 1;

  // Clear the node.
  this_node->symbol = -1;
  this_node->child_a = nullptr;
  this_node->child_b = nullptr;

  // Is this a leaf node?
  if (stream->ReadBit()) {
    // Get symbol from tree description and store in lead node.
    this_node->symbol = static_cast<int>(stream->ReadBits(kSymbolSize));

    return this_node;
  }

  // Get branch A.
  this_node->child_a = RecoverTree(nodes, stream, nodenum);

  // Get branch B.
  this_node->child_b = RecoverTree(nodes, stream, nodenum);

  return this_node;
}

}  // namespace

int Huffman::MaxCompressedSize(int uncompressed_size) {
  return uncompressed_size + kMaxTreeDataSize;
}

int Huffman::Compress(uint8_t *out, const uint8_t *in, int in_size) {
  // Do we have anything to compress?
  if (in_size < 1)
    return 0;

  // Initialize bitstream.
  OutBitstream stream(out);

  // Calculate and sort histogram for input data.
  SymbolInfo symbols[kNumSymbols];
  Histogram(in, symbols, in_size);

  // Build Huffman tree.
  MakeTree(symbols, &stream);

  // Sort histogram - first symbol first (bubble sort).
  // TODO(m): Quick-sort.
  bool swaps;
  do {
    swaps = false;
    for (int k = 0; k < kNumSymbols - 1; ++k) {
      if (symbols[k].symbol > symbols[k + 1].symbol) {
        SymbolInfo tmp = symbols[k];
        symbols[k] = symbols[k + 1];
        symbols[k + 1] = tmp;
        swaps = true;
      }
    }
  } while (swaps);

  // Encode input stream.
  for (int k = 0; k < in_size;) {
    uint8_t symbol = in[k];

    // Possible RLE?
    if (symbol == 0) {
      int zeros;
      for (zeros = 1; zeros < 16662 && (k + zeros) < in_size; ++zeros) {
        if (in[k + zeros] != 0)
          break;
      }
      if (zeros == 1) {
        stream.WriteBits(symbols[0].code, symbols[0].bits);
      } else if (zeros == 2) {
        stream.WriteBits(symbols[kSymTwoZeros].code,
                         symbols[kSymTwoZeros].bits);
      } else if (zeros <= 6) {
        uint32_t count = static_cast<uint32_t>(zeros - 3);
        stream.WriteBits(symbols[kSymUpTo6Zeros].code,
                         symbols[kSymUpTo6Zeros].bits);
        stream.WriteBits(count, 2);
      } else if (zeros <= 22) {
        uint32_t count = static_cast<uint32_t>(zeros - 7);
        stream.WriteBits(symbols[kSymUpTo22Zeros].code,
                         symbols[kSymUpTo22Zeros].bits);
        stream.WriteBits(count, 4);
      } else if (zeros <= 278) {
        uint32_t count = static_cast<uint32_t>(zeros - 23);
        stream.WriteBits(symbols[kSymUpTo278Zeros].code,
                         symbols[kSymUpTo278Zeros].bits);
        stream.WriteBits(count, 8);
      } else {
        uint32_t count = static_cast<uint32_t>(zeros - 279);
        stream.WriteBits(symbols[kSymUpTo16662Zeros].code,
                         symbols[kSymUpTo16662Zeros].bits);
        stream.WriteBits(count, 14);
      }
      k += zeros;
    } else {
      stream.WriteBits(symbols[symbol].code, symbols[symbol].bits);
      k++;
    }
  }

  // Calculate size of output data.
  return stream.Size();
}

void Huffman::Uncompress(uint8_t *out,
                         const uint8_t *in,
                         int in_size,
                         int out_size) {
  // Do we have anything to decompress?
  if (in_size < 1)
    return;

  // Initialize bitstream.
  InBitstream stream(in);

  // Recover Huffman tree.
  int node_count = 0;
  DecodeNode nodes[kMaxTreeNodes];
  DecodeNode *root = RecoverTree(nodes, &stream, &node_count);

  // Decode input stream.
  uint8_t *buf = out;
  for (int k = 0; k < out_size;) {
    // Traverse tree until we find a matching leaf node.
    // TODO(m): Improve the performance!
    DecodeNode *node = root;
    while (node->symbol < 0) {
      // Get next node.
      if (stream.ReadBit())
        node = node->child_b;
      else
        node = node->child_a;
    }

    // Decode as RLE or plain copy.
    uint8_t symbol;
    int count;
    switch (node->symbol) {
      case kSymTwoZeros: {
        symbol = 0;
        count = 2;
        break;
      }
      case kSymUpTo6Zeros: {
        symbol = 0;
        count = static_cast<int>(stream.ReadBits(2)) + 3;
        break;
      }
      case kSymUpTo22Zeros: {
        symbol = 0;
        count = static_cast<int>(stream.ReadBits(4)) + 7;
        break;
      }
      case kSymUpTo278Zeros: {
        symbol = 0;
        count = static_cast<int>(stream.ReadBits(8)) + 23;
        break;
      }
      case kSymUpTo16662Zeros: {
        symbol = 0;
        count = static_cast<int>(stream.ReadBits(14)) + 279;
        break;
      }
      default: {
        symbol = static_cast<uint8_t>(node->symbol);
        count = 1;
        break;
      }
    }

    for (int i = 0; i < count && (k + i) < out_size; ++i)
      *buf++ = symbol;
    k += count;
  }
}

}  // namespace himg
<|endoftext|>


<commit_before>////////////////////////////////////////////////////////////////////////////////
/// Reaper
///
/// Copyright (c) 2015-2018 Thibault Schueller
/// This file is distributed under the MIT License
////////////////////////////////////////////////////////////////////////////////

#include "pch/stdafx.h"
#include <doctest/doctest.h>

#include "core/memory/Allocator.h"

TEST_CASE("Offset alignment")
{
    SUBCASE("Alignment16")
    {
        const std::size_t alignmentMultiples = 4;
        const std::size_t alignment = 16;

        CHECK(0 == alignOffset(0, alignment));

        for (std::size_t base = 0; base < alignmentMultiples; ++base)
        {
            std::size_t desiredAlignedOffset = (base + 1) * alignment;

            for (std::size_t i = 1; i <= alignment; ++i)
            {
                CHECK(desiredAlignedOffset == alignOffset(base * alignment + i, alignment));
            }
        }
    }
}
<commit_msg>core: more alignment tests<commit_after>////////////////////////////////////////////////////////////////////////////////
/// Reaper
///
/// Copyright (c) 2015-2018 Thibault Schueller
/// This file is distributed under the MIT License
////////////////////////////////////////////////////////////////////////////////

#include "pch/stdafx.h"
#include <doctest/doctest.h>

#include "core/memory/Allocator.h"

TEST_CASE("Offset alignment")
{
    SUBCASE("Basic")
    {
        CHECK(0 == alignOffset(0, 16));
        CHECK(16 == alignOffset(1, 16));

        CHECK(4 == alignOffset(3, 4));
        CHECK(4 == alignOffset(4, 4));
        CHECK(5 == alignOffset(5, 1));
    }

    SUBCASE("Alignment16")
    {
        const std::size_t alignmentMultiples = 4;
        const std::size_t alignment = 16;

        CHECK(0 == alignOffset(0, alignment));

        for (std::size_t base = 0; base < alignmentMultiples; ++base)
        {
            std::size_t desiredAlignedOffset = (base + 1) * alignment;

            for (std::size_t i = 1; i <= alignment; ++i)
            {
                CHECK(desiredAlignedOffset == alignOffset(base * alignment + i, alignment));
            }
        }
    }
}
<|endoftext|>


<commit_before>//     Copyright 2016, Kay Hayen, mailto:kay.hayen@gmail.com
//
//     Part of "Nuitka", an optimizing Python compiler that is compatible and
//     integrates with CPython, but also works on its own.
//
//     Licensed under the Apache License, Version 2.0 (the "License");
//     you may not use this file except in compliance with the License.
//     You may obtain a copy of the License at
//
//        http://www.apache.org/licenses/LICENSE-2.0
//
//     Unless required by applicable law or agreed to in writing, software
//     distributed under the License is distributed on an "AS IS" BASIS,
//     WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//     See the License for the specific language governing permissions and
//     limitations under the License.
//
/* This is responsible for updating parts of CPython to better work with Nuitka
 * by replacing CPython implementations with enhanced versions.
 */

#include "nuitka/prelude.h"

#if PYTHON_VERSION >= 300
extern PyObject *const_str_plain_inspect;
extern PyObject *const_str_plain_site;
extern PyObject *const_int_0;

static PyObject *module_inspect;
#if PYTHON_VERSION >= 350
static PyObject *module_types;
extern PyObject *const_str_plain_types;
#endif

static char *kwlist[] = { (char *)"object", NULL };

static PyObject *old_getgeneratorstate = NULL;

static PyObject *_inspect_getgeneratorstate_replacement( PyObject *self, PyObject *args, PyObject *kwds )
{
    PyObject *object;

    if ( !PyArg_ParseTupleAndKeywords( args, kwds, "O:getgeneratorstate", kwlist, &object, NULL ))
    {
        return NULL;
    }

    if ( Nuitka_Generator_Check( object ) )
    {
        struct Nuitka_GeneratorObject *generator = (struct Nuitka_GeneratorObject *)object;

        if ( generator->m_running )
        {
            return PyObject_GetAttrString( module_inspect, "GEN_RUNNING" );
        }
        else if ( generator->m_status == status_Finished )
        {
            return PyObject_GetAttrString( module_inspect, "GEN_CLOSED" );
        }
        else if ( generator->m_status == status_Unused )
        {
            return PyObject_GetAttrString( module_inspect, "GEN_CREATED" );
        }
        else
        {
           return PyObject_GetAttrString( module_inspect, "GEN_SUSPENDED" );
        }
    }
    else
    {
        return old_getgeneratorstate->ob_type->tp_call( old_getgeneratorstate, args, kwds );
    }
}

#if PYTHON_VERSION >= 350
static PyObject *old_getcoroutinestate = NULL;

static PyObject *_inspect_getcoroutinestate_replacement( PyObject *self, PyObject *args, PyObject *kwds )
{
    PyObject *object;

    if ( !PyArg_ParseTupleAndKeywords( args, kwds, "O:getcoroutinestate", kwlist, &object, NULL ))
    {
        return NULL;
    }

    if ( Nuitka_Coroutine_Check( object ) )
    {
        struct Nuitka_CoroutineObject *coroutine = (struct Nuitka_CoroutineObject *)object;

        if ( coroutine->m_running )
        {
            return PyObject_GetAttrString( module_inspect, "CORO_RUNNING" );
        }
        else if ( coroutine->m_status == status_Finished )
        {
            return PyObject_GetAttrString( module_inspect, "CORO_CLOSED" );
        }
        else if ( coroutine->m_status == status_Unused )
        {
            return PyObject_GetAttrString( module_inspect, "CORO_CREATED" );
        }
        else
        {
           return PyObject_GetAttrString( module_inspect, "CORO_SUSPENDED" );
        }
    }
    else
    {
        return old_getcoroutinestate->ob_type->tp_call( old_getcoroutinestate, args, kwds );
    }
}

static PyObject *old_types_coroutine = NULL;

static char *kwlist_func[] = { (char *)"func", NULL };

static PyObject *_types_coroutine_replacement( PyObject *self, PyObject *args, PyObject *kwds )
{
    PyObject *func;

    if ( !PyArg_ParseTupleAndKeywords( args, kwds, "O:coroutine", kwlist_func, &func, NULL ))
    {
        return NULL;
    }

    if ( Nuitka_Function_Check( func ) )
    {
        struct Nuitka_FunctionObject *function = (struct Nuitka_FunctionObject *)func;

        if ( function->m_code_object->co_flags & CO_GENERATOR )
        {
            function->m_code_object->co_flags |= 0x100;
        }
    }

    return old_types_coroutine->ob_type->tp_call( old_types_coroutine, args, kwds );
}

#endif

#endif

#if PYTHON_VERSION >= 300
static PyMethodDef _method_def_inspect_getgeneratorstate_replacement =
{
    "getgeneratorstate",
    (PyCFunction)_inspect_getgeneratorstate_replacement,
    METH_VARARGS | METH_KEYWORDS,
    NULL
};

#if PYTHON_VERSION >= 350
static PyMethodDef _method_def_inspect_getcoroutinestate_replacement =
{
    "getcoroutinestate",
    (PyCFunction)_inspect_getcoroutinestate_replacement,
    METH_VARARGS | METH_KEYWORDS,
    NULL
};

static PyMethodDef _method_def_types_coroutine_replacement =
{
    "coroutine",
    (PyCFunction)_types_coroutine_replacement,
    METH_VARARGS | METH_KEYWORDS,
    NULL
};

#endif

/* Replace inspect functions with ones that handle compiles types too. */
void patchInspectModule( void )
{
#if PYTHON_VERSION >= 300
#ifdef _NUITKA_EXE
    // May need to import the "site" module, because otherwise the patching can
    // fail with it being unable to load it.
    if ( Py_NoSiteFlag == 0 )
    {
        PyObject *site_module = IMPORT_MODULE( const_str_plain_site, Py_None, Py_None, const_tuple_empty, const_int_0 );

        if ( site_module == NULL )
        {
            // Ignore "ImportError", having a "site" module is not a must.
            CLEAR_ERROR_OCCURRED();
        }
    }
#endif

    module_inspect = IMPORT_MODULE( const_str_plain_inspect, Py_None, Py_None, const_tuple_empty, const_int_0 );

    if ( module_inspect == NULL )
    {
        PyErr_PrintEx( 0 );
        Py_Exit( 1 );
    }
    CHECK_OBJECT( module_inspect );

    // Patch "inspect.getgeneratorstate" unless it is already patched.
    old_getgeneratorstate = PyObject_GetAttrString( module_inspect, "getgeneratorstate" );
    CHECK_OBJECT( old_getgeneratorstate );

    if ( PyFunction_Check( old_getgeneratorstate ) )
    {
        PyObject *inspect_getgeneratorstate_replacement = PyCFunction_New( &_method_def_inspect_getgeneratorstate_replacement, NULL );
        CHECK_OBJECT( inspect_getgeneratorstate_replacement );

        PyObject_SetAttrString( module_inspect, "getgeneratorstate", inspect_getgeneratorstate_replacement );
    }

#if PYTHON_VERSION >= 350
    // Patch "inspect.getcoroutinestate" unless it is already patched.
    old_getcoroutinestate = PyObject_GetAttrString( module_inspect, "getcoroutinestate" );
    CHECK_OBJECT( old_getcoroutinestate );

    if ( PyFunction_Check( old_getcoroutinestate ) )
    {
        PyObject *inspect_getcoroutinestate_replacement = PyCFunction_New( &_method_def_inspect_getcoroutinestate_replacement, NULL );
        CHECK_OBJECT( inspect_getcoroutinestate_replacement );

        PyObject_SetAttrString( module_inspect, "getcoroutinestate", inspect_getcoroutinestate_replacement );
    }

    module_types = IMPORT_MODULE( const_str_plain_types, Py_None, Py_None, const_tuple_empty, const_int_0 );

    if ( module_types == NULL )
    {
        PyErr_PrintEx( 0 );
        Py_Exit( 1 );
    }
    CHECK_OBJECT( module_types );

    // Patch "types.coroutine" unless it is already patched.
    old_types_coroutine = PyObject_GetAttrString( module_types, "coroutine" );
    CHECK_OBJECT( old_types_coroutine );

    if ( PyFunction_Check( old_types_coroutine ) )
    {
        PyObject *types_coroutine_replacement = PyCFunction_New( &_method_def_types_coroutine_replacement, NULL );
        CHECK_OBJECT( types_coroutine_replacement );

        PyObject_SetAttrString( module_types, "coroutine", types_coroutine_replacement );
    }

    static char const *wrapper_enhancement_code = "\n\
import types\n\
_old_GeneratorWrapper = types._GeneratorWrapper\n\
class GeneratorWrapperEnhanced(_old_GeneratorWrapper):\n\
    def __init__(self, gen):\n\
        _old_GeneratorWrapper.__init__(self, gen)\n\
\n\
        if hasattr(gen, 'gi_code'):\n\
            if gen.gi_code.co_flags & 0x0020:\n\
                self._GeneratorWrapper__isgen = True\n\
\n\
types._GeneratorWrapper = GeneratorWrapperEnhanced\
";

    PyObject *wrapper_enhencement_codeobject = Py_CompileString( wrapper_enhancement_code, "<exec>", Py_file_input );
    CHECK_OBJECT( wrapper_enhencement_codeobject );

    PyImport_ExecCodeModuleEx( "_types_patch", wrapper_enhencement_codeobject, "<frozen>" );

#endif

#endif

}
#endif

extern int Nuitka_IsInstance( PyObject *inst, PyObject *cls );
extern PyObject *original_isinstance;

static PyObject *_builtin_isinstance_replacement( PyObject *self, PyObject *args )
{
    PyObject *inst, *cls;

    if (unlikely( PyArg_UnpackTuple(args, "isinstance", 2, 2, &inst, &cls) == 0 ))
    {
        return NULL;
    }

    int res = Nuitka_IsInstance( inst, cls );

    if (unlikely( res < 0 ))
    {
        return NULL;
    }

    return PyBool_FromLong( res );
}

static PyMethodDef _method_def_builtin_isinstance_replacement =
{
    "isinstance",
    (PyCFunction)_builtin_isinstance_replacement,
    METH_VARARGS,
    NULL
};

extern PyModuleObject *builtin_module;

void patchBuiltinModule()
{
#if defined(_NUITKA_MODULE)
    static bool init_done = false;

    if (init_done == true) return;
    init_done = true;
#endif
    CHECK_OBJECT( (PyObject *)builtin_module );

    // Patch "inspect.isinstance" unless it is already patched.
    original_isinstance = PyObject_GetAttrString( (PyObject *)builtin_module, "isinstance" );
    CHECK_OBJECT( original_isinstance );

    // Copy the doc attribute over, needed for "inspect.signature" at least.
    if ( PyCFunction_Check( original_isinstance ))
    {
        _method_def_builtin_isinstance_replacement.ml_doc = ((PyCFunctionObject *)original_isinstance)->m_ml->ml_doc;
    }

    PyObject *builtin_isinstance_replacement = PyCFunction_New( &_method_def_builtin_isinstance_replacement, NULL );
    CHECK_OBJECT( builtin_isinstance_replacement );

    PyObject_SetAttrString( (PyObject *)builtin_module, "isinstance", builtin_isinstance_replacement );
}

static richcmpfunc original_PyType_tp_richcompare = NULL;

static PyObject *Nuitka_type_tp_richcompare( PyObject *a, PyObject *b, int op )
{
    if (likely( op == Py_EQ || op == Py_NE ))
    {
        if ( a == (PyObject *)&Nuitka_Function_Type )
        {
            a = (PyObject *)&PyFunction_Type;
        }
        else if ( a == (PyObject *)&Nuitka_Method_Type )
        {
            a = (PyObject *)&PyMethod_Type;
        }
        else if ( a == (PyObject *)&Nuitka_Generator_Type )
        {
            a = (PyObject *)&PyGen_Type;
        }

        if ( b == (PyObject *)&Nuitka_Function_Type )
        {
            b = (PyObject *)&PyFunction_Type;
        }
        else if ( b == (PyObject *)&Nuitka_Method_Type )
        {
            b = (PyObject *)&PyMethod_Type;
        }
        else if ( b == (PyObject *)&Nuitka_Generator_Type )
        {
            b = (PyObject *)&PyGen_Type;
        }
    }

    CHECK_OBJECT( a );
    CHECK_OBJECT( b );

    assert( original_PyType_tp_richcompare );

    return original_PyType_tp_richcompare( a, b, op );
}

void patchTypeComparison()
{
    if ( original_PyType_tp_richcompare == NULL )
    {
        original_PyType_tp_richcompare = PyType_Type.tp_richcompare;
        PyType_Type.tp_richcompare = Nuitka_type_tp_richcompare;
    }
}
<commit_msg>Delete InspectPatcher.cpp<commit_after><|endoftext|>


<commit_before>// Copyright © 2015 Mikko Ronkainen <firstname@mikkoronkainen.com>
// License: MIT, see the LICENSE file.

#include <chrono>
#include <ctime>
#include <iomanip>

#include "tclap/CmdLine.h"

#include "App.h"
#include "Utils/Log.h"
#include "Utils/Settings.h"
#include "Rendering/Framebuffer.h"
#include "CLRaytracing/CLManager.h"
#include "Runners/InteractiveRunner.h"
#include "Runners/ConsoleRunner.h"
#include "Runners/NetworkRunner.h"
#include "Raytracing/Raytracer.h"
#include "CLRaytracing/CLRaytracer.h"

#ifdef _WIN32
#include <windows.h>
#endif

using namespace Raycer;

int main(int argc, char** argv)
{
	return App().run(argc, argv);
}

#ifdef _WIN32
BOOL consoleCtrlHandler(DWORD fdwCtrlType)
{
	if (fdwCtrlType == CTRL_C_EVENT)
	{
		App::getConsoleRunner().interrupt();
		App::getNetworkRunner().interrupt();

		return true;
	}
	else
		return false;
}
#endif

int App::run(int argc, char** argv)
{
#ifdef _WIN32
	SetConsoleCtrlHandler((PHANDLER_ROUTINE)consoleCtrlHandler, TRUE);
#endif

	TCLAP::CmdLine cmd("Raycer", ' ', "1.0.0");
	TCLAP::SwitchArg interactiveSwitch("i", "interactive", "View the scene interactively", cmd, false);
	TCLAP::SwitchArg useOpenCLSwitch("l", "opencl", "Use OpenCL for raytracing", cmd, false);
	TCLAP::ValueArg<int> platformIdArg("p", "platform", "OpenCL platform to use", false, 0, "int", cmd);
	TCLAP::ValueArg<int> deviceTypeArg("t", "type", "OpenCL device type to use", false, 0, "int", cmd);
	TCLAP::ValueArg<int> deviceIdArg("d", "device", "OpenCL device to use", false, 0, "int", cmd);
	TCLAP::SwitchArg clientSwitch("c", "client", "Enable network client mode", cmd, false);
	TCLAP::SwitchArg serverSwitch("e", "server", "Enable network server mode", cmd, false);
	TCLAP::ValueArg<std::string> sceneFileNameArg("s", "scene", "Path to the scene file", false, "", "string", cmd);
	TCLAP::ValueArg<int> widthArg("w", "width", "Width of the output image or window", false, 0, "int", cmd);
	TCLAP::ValueArg<int> heightArg("h", "height", "Height of the output image or window", false, 0, "int", cmd);
	TCLAP::ValueArg<std::string> outputFileNameArg("o", "output", "Path to the output image file", false, "", "string", cmd);
	TCLAP::SwitchArg autoViewSwitch("v", "view", "View the image after completion", cmd, false);

	try
	{
		cmd.parse(argc, argv);
	}
	catch (const TCLAP::ArgException &ex)
	{
		std::cerr << "Command line parsing error: " << ex.error() << " for arg " << ex.argId() << std::endl;
		return -1;
	}

	try
	{
		Settings& settings = getSettings();
		InteractiveRunner& interactiveRunner = getInteractiveRunner();
		ConsoleRunner& consoleRunner = getConsoleRunner();
		NetworkRunner& networkRunner = getNetworkRunner();

		settings.load("settings.ini");

		if (interactiveSwitch.isSet())
			settings.general.interactive = interactiveSwitch.getValue();

		if (useOpenCLSwitch.isSet())
			settings.openCL.enabled = useOpenCLSwitch.getValue();

		if (platformIdArg.isSet())
			settings.openCL.platformId = platformIdArg.getValue();

		if (deviceTypeArg.isSet())
			settings.openCL.deviceType = deviceTypeArg.getValue();

		if (deviceIdArg.isSet())
			settings.openCL.deviceId = deviceIdArg.getValue();

		if (clientSwitch.isSet())
		{
			settings.network.isClient = clientSwitch.getValue();
			settings.network.isServer = !settings.network.isClient;
		}

		if (serverSwitch.isSet())
		{
			settings.network.isServer = serverSwitch.getValue();
			settings.network.isClient = !settings.network.isServer;
		}

		if (sceneFileNameArg.isSet())
			settings.scene.fileName = sceneFileNameArg.getValue();

		if (widthArg.isSet())
		{
			settings.image.width = widthArg.getValue();
			settings.window.width = widthArg.getValue();
		}

		if (heightArg.isSet())
		{
			settings.image.height = heightArg.getValue();
			settings.window.height = heightArg.getValue();
		}

		if (outputFileNameArg.isSet())
			settings.image.fileName = outputFileNameArg.getValue();

		if (autoViewSwitch.isSet())
			settings.image.autoView = autoViewSwitch.getValue();

		if (settings.general.interactive)
			return interactiveRunner.run();
		else
		{
			if (settings.network.isClient || settings.network.isServer)
				return networkRunner.run();
			else
				return consoleRunner.run();
		}
	}
	catch (const std::exception& ex)
	{
		std::cerr << "Error: " << ex.what() << std::endl;
		return -1;
	}
}

Log& App::getLog()
{
	static Log log("raycer.log");
	return log;
}

Settings& App::getSettings()
{
	static Settings settings;
	return settings;
}

Framebuffer& App::getFramebuffer()
{
	static Framebuffer framebuffer;
	return framebuffer;
}

CLManager& App::getCLManager()
{
	static CLManager clManager;
	return clManager;
}

InteractiveRunner& App::getInteractiveRunner()
{
	static InteractiveRunner interactiveRunner;
	return interactiveRunner;
}

ConsoleRunner& App::getConsoleRunner()
{
	static ConsoleRunner consoleRunner;
	return consoleRunner;
}

NetworkRunner& App::getNetworkRunner()
{
	static NetworkRunner networkRunner;
	return networkRunner;
}

Raytracer& App::getRaytracer()
{
	static Raytracer raytracer;
	return raytracer;
}

CLRaytracer& App::getCLRaytracer()
{
	static CLRaytracer clRaytracer;
	return clRaytracer;
}
<commit_msg>Updated command line options<commit_after>// Copyright © 2015 Mikko Ronkainen <firstname@mikkoronkainen.com>
// License: MIT, see the LICENSE file.

#include <chrono>
#include <ctime>
#include <iomanip>

#include "tclap/CmdLine.h"

#include "App.h"
#include "Utils/Log.h"
#include "Utils/Settings.h"
#include "Rendering/Framebuffer.h"
#include "CLRaytracing/CLManager.h"
#include "Runners/InteractiveRunner.h"
#include "Runners/ConsoleRunner.h"
#include "Runners/NetworkRunner.h"
#include "Raytracing/Raytracer.h"
#include "CLRaytracing/CLRaytracer.h"

#ifdef _WIN32
#include <windows.h>
#endif

using namespace Raycer;

int main(int argc, char** argv)
{
	return App().run(argc, argv);
}

#ifdef _WIN32
BOOL consoleCtrlHandler(DWORD fdwCtrlType)
{
	if (fdwCtrlType == CTRL_C_EVENT)
	{
		App::getConsoleRunner().interrupt();
		App::getNetworkRunner().interrupt();

		return true;
	}
	else
		return false;
}
#endif

int App::run(int argc, char** argv)
{
#ifdef _WIN32
	SetConsoleCtrlHandler((PHANDLER_ROUTINE)consoleCtrlHandler, TRUE);
#endif

	TCLAP::CmdLine cmd("Raycer", ' ', "1.0.0");
	TCLAP::SwitchArg interactiveSwitch("", "interactive", "View the scene interactively", cmd, false);
	TCLAP::SwitchArg nonInteractiveSwitch("", "non-interactive", "Force non-interactive mode", cmd, false);
	TCLAP::SwitchArg enableOpenCLSwitch("", "opencl", "Use OpenCL for raytracing", cmd, false);
	TCLAP::ValueArg<int> clPlatformIdArg("", "cl-platform", "OpenCL platform to use", false, 0, "int", cmd);
	TCLAP::ValueArg<int> clDeviceTypeArg("", "cl-device-type", "OpenCL device type to use", false, 0, "int", cmd);
	TCLAP::ValueArg<int> clDeviceIdArg("", "cl-device", "OpenCL device to use", false, 0, "int", cmd);
	TCLAP::SwitchArg clientSwitch("", "client", "Enable network client mode", cmd, false);
	TCLAP::SwitchArg serverSwitch("", "server", "Enable network server mode", cmd, false);
	TCLAP::ValueArg<std::string> sceneFileNameArg("s", "scene", "Path to the scene file", false, "", "string", cmd);
	TCLAP::ValueArg<int> widthArg("w", "width", "Width of the output image or window", false, 0, "int", cmd);
	TCLAP::ValueArg<int> heightArg("h", "height", "Height of the output image or window", false, 0, "int", cmd);
	TCLAP::ValueArg<std::string> outputFileNameArg("o", "output", "Path to the output image file", false, "", "string", cmd);
	TCLAP::SwitchArg autoViewSwitch("", "view", "Open the image automatically after completion", cmd, false);

	try
	{
		cmd.parse(argc, argv);
	}
	catch (const TCLAP::ArgException &ex)
	{
		std::cerr << "Command line parsing error: " << ex.error() << " for arg " << ex.argId() << std::endl;
		return -1;
	}

	try
	{
		Settings& settings = getSettings();
		InteractiveRunner& interactiveRunner = getInteractiveRunner();
		ConsoleRunner& consoleRunner = getConsoleRunner();
		NetworkRunner& networkRunner = getNetworkRunner();

		settings.load("settings.ini");

		if (interactiveSwitch.isSet())
			settings.general.interactive = true;

		if (nonInteractiveSwitch.isSet())
			settings.general.interactive = false;

		if (enableOpenCLSwitch.isSet())
			settings.openCL.enabled = true;

		if (clPlatformIdArg.isSet())
			settings.openCL.platformId = clPlatformIdArg.getValue();

		if (clDeviceTypeArg.isSet())
			settings.openCL.deviceType = clDeviceTypeArg.getValue();

		if (clDeviceIdArg.isSet())
			settings.openCL.deviceId = clDeviceIdArg.getValue();

		if (clientSwitch.isSet())
		{
			settings.network.isClient = true;
			settings.network.isServer = false;
		}

		if (serverSwitch.isSet())
		{
			settings.network.isServer = true;
			settings.network.isClient = false;
		}

		if (sceneFileNameArg.isSet())
			settings.scene.fileName = sceneFileNameArg.getValue();

		if (widthArg.isSet())
			settings.image.width = settings.window.width = widthArg.getValue();

		if (heightArg.isSet())
			settings.image.height = settings.window.height = heightArg.getValue();

		if (outputFileNameArg.isSet())
			settings.image.fileName = outputFileNameArg.getValue();

		if (autoViewSwitch.isSet())
			settings.image.autoView = true;

		if (settings.general.interactive)
			return interactiveRunner.run();
		else
		{
			if (settings.network.isClient || settings.network.isServer)
				return networkRunner.run();
			else
				return consoleRunner.run();
		}
	}
	catch (const std::exception& ex)
	{
		std::cerr << "Error: " << ex.what() << std::endl;
		return -1;
	}
}

Log& App::getLog()
{
	static Log log("raycer.log");
	return log;
}

Settings& App::getSettings()
{
	static Settings settings;
	return settings;
}

Framebuffer& App::getFramebuffer()
{
	static Framebuffer framebuffer;
	return framebuffer;
}

CLManager& App::getCLManager()
{
	static CLManager clManager;
	return clManager;
}

InteractiveRunner& App::getInteractiveRunner()
{
	static InteractiveRunner interactiveRunner;
	return interactiveRunner;
}

ConsoleRunner& App::getConsoleRunner()
{
	static ConsoleRunner consoleRunner;
	return consoleRunner;
}

NetworkRunner& App::getNetworkRunner()
{
	static NetworkRunner networkRunner;
	return networkRunner;
}

Raytracer& App::getRaytracer()
{
	static Raytracer raytracer;
	return raytracer;
}

CLRaytracer& App::getCLRaytracer()
{
	static CLRaytracer clRaytracer;
	return clRaytracer;
}
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2011, Georgia Tech Research Corporation
 * All rights reserved.
 *
 * Author(s): Sehoon Ha <sehoon.ha@gmail.com>
 * Date: 06/12/2011
 *
 * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab
 *
 * Directed by Prof. C. Karen Liu and Prof. Mike Stilman
 * <karenliu@cc.gatech.edu> <mstilman@cc.gatech.edu>
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * 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 HOLDER 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.
 */

#include "TrfmTranslate.h"
#include "Dof.h"

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

namespace kinematics {
    TrfmTranslate::TrfmTranslate(Dof *x, Dof *y, Dof *z, const char* _name){
        mDofs.resize(3);
        mDofs[0]=x;
        mDofs[1]=y;
        mDofs[2]=z;
        mDofs[0]->setTrans(this);
        mDofs[1]->setTrans(this);
        mDofs[2]->setTrans(this);
        mType = Transformation::T_TRANSLATE;
        if(_name!=NULL)
            strcpy(mName, _name);
        else
            strcpy(mName, "translate");
    }

    void TrfmTranslate::applyGLTransform(renderer::RenderInterface* _ri) const{
		if (_ri)
			_ri->translate(Vector3d(mDofs[0]->getValue(), mDofs[1]->getValue(), mDofs[2]->getValue()));
    }

    void TrfmTranslate::computeTransform(){
        mTransform.setZero();
        mTransform(0, 0) = 1.0;
        mTransform(1, 1) = 1.0;
        mTransform(2, 2) = 1.0;
        mTransform(3, 3) = 1.0;
        for(unsigned int i=0; i<mDofs.size(); i++){
            mTransform(i, 3) = mDofs[i]->getValue();
        }
    }

    Matrix4d TrfmTranslate::getDeriv(const Dof *q){
        Matrix4d ret = Matrix4d::Zero();
        for(unsigned int i=0; i<mDofs.size(); i++)
            if(mDofs[i] == q){
                ret(i, 3) = 1.0;
                break;
            }
        return ret;
    }

    void TrfmTranslate::applyDeriv(const Dof* q, Vector3d& v){
        for(unsigned int i=0; i<mDofs.size(); i++){
            if(mDofs[i] != q) v(i) = 0;
            else v(i) = 1;
        }
    }

    void TrfmTranslate::applyDeriv(const Dof* q, Matrix4d& m){
        for(unsigned int i=0; i<mDofs.size(); i++){
            if(mDofs[i] != q) m.row(i).setZero();
            else m.row(i) = m.row(3);
        }
        m.row(3).setZero();
    }

    Matrix4d TrfmTranslate::getSecondDeriv( const Dof *q1, const Dof *q2 ) {
        return Eigen::Matrix4d::Zero(); // always zero
    }

    void TrfmTranslate::applySecondDeriv( const Dof* q1, const Dof* q2, Eigen::Vector3d& v ) {
        v.setZero();
    }

    void TrfmTranslate::applySecondDeriv( const Dof* q1, const Dof* q2, Eigen::Matrix4d& m ) {
        m.setZero();
    }

    Matrix4d TrfmTranslate::getInvTransform(){
        Matrix4d ret = Matrix4d::Ones();
        for(unsigned int i=0; i<mDofs.size(); i++)
            ret(i, 3) = -mDofs[i]->getValue();
        return ret;
    }

    void TrfmTranslate::applyTransform(Vector3d& v){
        for(unsigned int i=0; i<mDofs.size(); i++)
            v(i) +=mDofs[i]->getValue();
    }

    void TrfmTranslate::applyInvTransform(Vector3d& v){
        for(unsigned int i=0; i<mDofs.size(); i++)
            v(i) -= mDofs[i]->getValue();
    }

    void TrfmTranslate::applyTransform(Matrix4d& m){
        for(unsigned int i=0; i<mDofs.size(); i++) {
            m.row(i) += mDofs[i]->getValue() * m.row(3);
        }
    }

    void TrfmTranslate::applyInvTransform(Matrix4d& m){
        for(unsigned int i=0; i<mDofs.size(); i++) {
            m.row(i) -= mDofs[i]->getValue() * m.row(3);
        }
    }

    TrfmTranslateX::TrfmTranslateX(Dof *x, const char *_name){
        mDofs.resize(1);
        mDofs[0]=x;
        mDofs[0]->setTrans(this);
        mType = Transformation::T_TRANSLATEX;
        strcpy(mName, _name);
        if(_name!=NULL)
            strcpy(mName, _name);
        else
            strcpy(mName, "TranslateX");
    }

    void TrfmTranslateX::applyGLTransform(renderer::RenderInterface* _ri) const{
		if (_ri)
			_ri->translate(Vector3d(mDofs[0]->getValue(), 0, 0));
	}

    void TrfmTranslateX::computeTransform(){
        mTransform.setZero();
        mTransform(0, 0) = 1.0;
        mTransform(1, 1) = 1.0;
        mTransform(2, 2) = 1.0;
        mTransform(3, 3) = 1.0;
        mTransform(A_X, 3) = mDofs[0]->getValue();
    }

    Matrix4d TrfmTranslateX::getDeriv(const Dof *q){
        Matrix4d ret = Matrix4d::Zero();
        if(mDofs[0] == q)
            ret(A_X, 3) = 1.0;
        return ret;
    }

    void TrfmTranslateX::applyDeriv(const Dof* q, Vector3d& v){
        if(mDofs[0] != q) v=Vector3d::Zero();
        else {
            v = Vector3d(1, 0, 0);
            // v = Vector3d::Zero();
            // v(0) = 1;
        }
    }

    void TrfmTranslateX::applyDeriv(const Dof* q, Matrix4d& m){
        if(mDofs[0] != q) m=Matrix4d::Zero();
        else for(int i=0; i<4; i++){
                if(i==A_X) m.row(i) = m.row(3);
                else m.row(i).setZero();
            }
    }

    Matrix4d TrfmTranslateX::getSecondDeriv( const Dof *q1, const Dof *q2 ) {
        return Eigen::Matrix4d::Zero(); // always zero
    }

    void TrfmTranslateX::applySecondDeriv( const Dof* q1, const Dof* q2, Eigen::Vector3d& v ) {
        v.setZero();
    }

    void TrfmTranslateX::applySecondDeriv( const Dof* q1, const Dof* q2, Eigen::Matrix4d& m ) {
        m.setZero();
    }

    Matrix4d TrfmTranslateX::getInvTransform(){
        Matrix4d ret = Matrix4d::Ones();
        ret(A_X, 3) = -mDofs[0]->getValue();
        return ret;
    }

    void TrfmTranslateX::applyTransform(Vector3d& v){
        v(A_X) += mDofs[0]->getValue();
    }

    void TrfmTranslateX::applyInvTransform(Vector3d& v){
        v(A_X) -= mDofs[0]->getValue();
    }

    void TrfmTranslateX::applyTransform(Matrix4d& m){
        m.row(A_X) += mDofs[0]->getValue() * m.row(3);
    }

    void TrfmTranslateX::applyInvTransform(Matrix4d& m){
        m.row(A_X) -= mDofs[0]->getValue() * m.row(3);
    }

    TrfmTranslateY::TrfmTranslateY(Dof *y, const char *_name){
        mDofs.resize(1);
        mDofs[0]=y;
        mDofs[0]->setTrans(this);
        mType = Transformation::T_TRANSLATEY;
        strcpy(mName, _name);
        if(_name!=NULL)
            strcpy(mName, _name);
        else
            strcpy(mName, "TranslateY");
    }

    void TrfmTranslateY::applyGLTransform(renderer::RenderInterface* _ri) const {
		if (_ri)
			_ri->translate(Vector3d(0, mDofs[0]->getValue(), 0));
    }

    void TrfmTranslateY::computeTransform(){
        mTransform.setZero();
        mTransform(0, 0) = 1.0;
        mTransform(1, 1) = 1.0;
        mTransform(2, 2) = 1.0;
        mTransform(3, 3) = 1.0;
        mTransform(A_Y, 3) = mDofs[0]->getValue();
    }

    Matrix4d TrfmTranslateY::getDeriv(const Dof *q){
        Matrix4d ret = Matrix4d::Zero();
        if(mDofs[0] == q)
            ret(A_Y, 3) = 1.0;
        return ret;
    }

    void TrfmTranslateY::applyDeriv(const Dof* q, Vector3d& v){
        if(mDofs[0] != q) v=Vector3d::Zero();
        else {
            v = Vector3d(0, 1, 0);
        }
    }

    void TrfmTranslateY::applyDeriv(const Dof* q, Matrix4d& m){
        if(mDofs[0] != q) m=Matrix4d::Zero();
        else for(int i=0; i<4; i++){
                if(i==A_Y) m.row(i)=m.row(3);
                else m.row(i).setZero();
            }
    }

    Matrix4d TrfmTranslateY::getSecondDeriv( const Dof *q1, const Dof *q2 ) {
        return Eigen::Matrix4d::Zero(); // always zero
    }

    void TrfmTranslateY::applySecondDeriv( const Dof* q1, const Dof* q2, Eigen::Vector3d& v ) {
        v.setZero();
    }

    void TrfmTranslateY::applySecondDeriv( const Dof* q1, const Dof* q2, Eigen::Matrix4d& m ) {
        m.setZero();
    }

    Matrix4d TrfmTranslateY::getInvTransform(){
        Matrix4d ret = Matrix4d::Ones();
        ret(A_Y, 3) = -mDofs[0]->getValue();
        return ret;
    }

    void TrfmTranslateY::applyTransform(Vector3d& v){
        v(A_Y) += mDofs[0]->getValue();
    }

    void TrfmTranslateY::applyInvTransform(Vector3d& v){
        v(A_Y) -= mDofs[0]->getValue();
    }

    void TrfmTranslateY::applyTransform(Matrix4d& m){
        m.row(A_Y) += mDofs[0]->getValue() * m.row(3);
    }

    void TrfmTranslateY::applyInvTransform(Matrix4d& m){
        m.row(A_Y) -= mDofs[0]->getValue() * m.row(3);
    }

    TrfmTranslateZ::TrfmTranslateZ(Dof *z, const char *_name){
        mDofs.resize(1);
        mDofs[0]=z;
        mDofs[0]->setTrans(this);
        mType = Transformation::T_TRANSLATEZ;
        strcpy(mName, _name);
        if(_name!=NULL)
            strcpy(mName, _name);
        else
            strcpy(mName, "TranslateZ");
    }

    void TrfmTranslateZ::applyGLTransform(renderer::RenderInterface* _ri) const {
		if (_ri)
			_ri->translate(Vector3d(0, 0, mDofs[0]->getValue()));
	}

    void TrfmTranslateZ::computeTransform(){
        mTransform.setZero();
        mTransform(0, 0) = 1.0;
        mTransform(1, 1) = 1.0;
        mTransform(2, 2) = 1.0;
        mTransform(3, 3) = 1.0;
        mTransform(A_Z, 3) = mDofs[0]->getValue();
    }

    Matrix4d TrfmTranslateZ::getDeriv(const Dof *q){
        Matrix4d ret = Matrix4d::Zero();
        if(mDofs[0] == q)
            ret(A_Z, 3) = 1.0;
        return ret;
    }

    void TrfmTranslateZ::applyDeriv(const Dof* q, Vector3d& v){
        if(mDofs[0] != q) v=Vector3d::Zero();
        else v=Vector3d(0,0,1);
    }

    void TrfmTranslateZ::applyDeriv(const Dof* q, Matrix4d& m){
        if(mDofs[0] != q) m = Matrix4d::Zero();
        else for(int i=0; i<4; i++){
                if(i==A_Z) m.row(i) = m.row(3);
                else m.row(i).setZero();
            }
    }

    Matrix4d TrfmTranslateZ::getSecondDeriv( const Dof *q1, const Dof *q2 ) {
        return Eigen::Matrix4d::Zero(); // always zero
    }

    void TrfmTranslateZ::applySecondDeriv( const Dof* q1, const Dof* q2, Eigen::Vector3d& v ) {
        v.setZero();
    }

    void TrfmTranslateZ::applySecondDeriv( const Dof* q1, const Dof* q2, Eigen::Matrix4d& m ) {
        m.setZero();
    }

    Matrix4d TrfmTranslateZ::getInvTransform(){
        Matrix4d ret = Matrix4d::Ones();
        ret(A_Z, 3) = -mDofs[0]->getValue();
        return ret;
    }

    void TrfmTranslateZ::applyTransform(Vector3d& v){
        v(A_Z) += mDofs[0]->getValue();
    }

    void TrfmTranslateZ::applyInvTransform(Vector3d& v){
        v(A_Z) -= mDofs[0]->getValue();
    }

    void TrfmTranslateZ::applyTransform(Matrix4d& m){
        m.row(A_Z) += mDofs[0]->getValue() * m.row(3);
    }

    void TrfmTranslateZ::applyInvTransform(Matrix4d& m){
        m.row(A_Z) -= mDofs[0]->getValue() * m.row(3);
    }

} // namespace kinematics
<commit_msg>Cannot copy a NULL name.<commit_after>/*
 * Copyright (c) 2011, Georgia Tech Research Corporation
 * All rights reserved.
 *
 * Author(s): Sehoon Ha <sehoon.ha@gmail.com>
 * Date: 06/12/2011
 *
 * Geoorgia Tech Graphics Lab and Humanoid Robotics Lab
 *
 * Directed by Prof. C. Karen Liu and Prof. Mike Stilman
 * <karenliu@cc.gatech.edu> <mstilman@cc.gatech.edu>
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * 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 HOLDER 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.
 */

#include "TrfmTranslate.h"
#include "Dof.h"

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

namespace kinematics {
    TrfmTranslate::TrfmTranslate(Dof *x, Dof *y, Dof *z, const char* _name){
        mDofs.resize(3);
        mDofs[0]=x;
        mDofs[1]=y;
        mDofs[2]=z;
        mDofs[0]->setTrans(this);
        mDofs[1]->setTrans(this);
        mDofs[2]->setTrans(this);
        mType = Transformation::T_TRANSLATE;
        if(_name!=NULL)
            strcpy(mName, _name);
        else
            strcpy(mName, "translate");
    }

    void TrfmTranslate::applyGLTransform(renderer::RenderInterface* _ri) const{
		if (_ri)
			_ri->translate(Vector3d(mDofs[0]->getValue(), mDofs[1]->getValue(), mDofs[2]->getValue()));
    }

    void TrfmTranslate::computeTransform(){
        mTransform.setZero();
        mTransform(0, 0) = 1.0;
        mTransform(1, 1) = 1.0;
        mTransform(2, 2) = 1.0;
        mTransform(3, 3) = 1.0;
        for(unsigned int i=0; i<mDofs.size(); i++){
            mTransform(i, 3) = mDofs[i]->getValue();
        }
    }

    Matrix4d TrfmTranslate::getDeriv(const Dof *q){
        Matrix4d ret = Matrix4d::Zero();
        for(unsigned int i=0; i<mDofs.size(); i++)
            if(mDofs[i] == q){
                ret(i, 3) = 1.0;
                break;
            }
        return ret;
    }

    void TrfmTranslate::applyDeriv(const Dof* q, Vector3d& v){
        for(unsigned int i=0; i<mDofs.size(); i++){
            if(mDofs[i] != q) v(i) = 0;
            else v(i) = 1;
        }
    }

    void TrfmTranslate::applyDeriv(const Dof* q, Matrix4d& m){
        for(unsigned int i=0; i<mDofs.size(); i++){
            if(mDofs[i] != q) m.row(i).setZero();
            else m.row(i) = m.row(3);
        }
        m.row(3).setZero();
    }

    Matrix4d TrfmTranslate::getSecondDeriv( const Dof *q1, const Dof *q2 ) {
        return Eigen::Matrix4d::Zero(); // always zero
    }

    void TrfmTranslate::applySecondDeriv( const Dof* q1, const Dof* q2, Eigen::Vector3d& v ) {
        v.setZero();
    }

    void TrfmTranslate::applySecondDeriv( const Dof* q1, const Dof* q2, Eigen::Matrix4d& m ) {
        m.setZero();
    }

    Matrix4d TrfmTranslate::getInvTransform(){
        Matrix4d ret = Matrix4d::Ones();
        for(unsigned int i=0; i<mDofs.size(); i++)
            ret(i, 3) = -mDofs[i]->getValue();
        return ret;
    }

    void TrfmTranslate::applyTransform(Vector3d& v){
        for(unsigned int i=0; i<mDofs.size(); i++)
            v(i) +=mDofs[i]->getValue();
    }

    void TrfmTranslate::applyInvTransform(Vector3d& v){
        for(unsigned int i=0; i<mDofs.size(); i++)
            v(i) -= mDofs[i]->getValue();
    }

    void TrfmTranslate::applyTransform(Matrix4d& m){
        for(unsigned int i=0; i<mDofs.size(); i++) {
            m.row(i) += mDofs[i]->getValue() * m.row(3);
        }
    }

    void TrfmTranslate::applyInvTransform(Matrix4d& m){
        for(unsigned int i=0; i<mDofs.size(); i++) {
            m.row(i) -= mDofs[i]->getValue() * m.row(3);
        }
    }

    TrfmTranslateX::TrfmTranslateX(Dof *x, const char *_name){
        mDofs.resize(1);
        mDofs[0]=x;
        mDofs[0]->setTrans(this);
        mType = Transformation::T_TRANSLATEX;
        if(_name!=NULL)
            strcpy(mName, _name);
        else
            strcpy(mName, "TranslateX");
    }

    void TrfmTranslateX::applyGLTransform(renderer::RenderInterface* _ri) const{
		if (_ri)
			_ri->translate(Vector3d(mDofs[0]->getValue(), 0, 0));
	}

    void TrfmTranslateX::computeTransform(){
        mTransform.setZero();
        mTransform(0, 0) = 1.0;
        mTransform(1, 1) = 1.0;
        mTransform(2, 2) = 1.0;
        mTransform(3, 3) = 1.0;
        mTransform(A_X, 3) = mDofs[0]->getValue();
    }

    Matrix4d TrfmTranslateX::getDeriv(const Dof *q){
        Matrix4d ret = Matrix4d::Zero();
        if(mDofs[0] == q)
            ret(A_X, 3) = 1.0;
        return ret;
    }

    void TrfmTranslateX::applyDeriv(const Dof* q, Vector3d& v){
        if(mDofs[0] != q) v=Vector3d::Zero();
        else {
            v = Vector3d(1, 0, 0);
            // v = Vector3d::Zero();
            // v(0) = 1;
        }
    }

    void TrfmTranslateX::applyDeriv(const Dof* q, Matrix4d& m){
        if(mDofs[0] != q) m=Matrix4d::Zero();
        else for(int i=0; i<4; i++){
                if(i==A_X) m.row(i) = m.row(3);
                else m.row(i).setZero();
            }
    }

    Matrix4d TrfmTranslateX::getSecondDeriv( const Dof *q1, const Dof *q2 ) {
        return Eigen::Matrix4d::Zero(); // always zero
    }

    void TrfmTranslateX::applySecondDeriv( const Dof* q1, const Dof* q2, Eigen::Vector3d& v ) {
        v.setZero();
    }

    void TrfmTranslateX::applySecondDeriv( const Dof* q1, const Dof* q2, Eigen::Matrix4d& m ) {
        m.setZero();
    }

    Matrix4d TrfmTranslateX::getInvTransform(){
        Matrix4d ret = Matrix4d::Ones();
        ret(A_X, 3) = -mDofs[0]->getValue();
        return ret;
    }

    void TrfmTranslateX::applyTransform(Vector3d& v){
        v(A_X) += mDofs[0]->getValue();
    }

    void TrfmTranslateX::applyInvTransform(Vector3d& v){
        v(A_X) -= mDofs[0]->getValue();
    }

    void TrfmTranslateX::applyTransform(Matrix4d& m){
        m.row(A_X) += mDofs[0]->getValue() * m.row(3);
    }

    void TrfmTranslateX::applyInvTransform(Matrix4d& m){
        m.row(A_X) -= mDofs[0]->getValue() * m.row(3);
    }

    TrfmTranslateY::TrfmTranslateY(Dof *y, const char *_name){
        mDofs.resize(1);
        mDofs[0]=y;
        mDofs[0]->setTrans(this);
        mType = Transformation::T_TRANSLATEY;
        if(_name!=NULL)
            strcpy(mName, _name);
        else
            strcpy(mName, "TranslateY");
    }

    void TrfmTranslateY::applyGLTransform(renderer::RenderInterface* _ri) const {
		if (_ri)
			_ri->translate(Vector3d(0, mDofs[0]->getValue(), 0));
    }

    void TrfmTranslateY::computeTransform(){
        mTransform.setZero();
        mTransform(0, 0) = 1.0;
        mTransform(1, 1) = 1.0;
        mTransform(2, 2) = 1.0;
        mTransform(3, 3) = 1.0;
        mTransform(A_Y, 3) = mDofs[0]->getValue();
    }

    Matrix4d TrfmTranslateY::getDeriv(const Dof *q){
        Matrix4d ret = Matrix4d::Zero();
        if(mDofs[0] == q)
            ret(A_Y, 3) = 1.0;
        return ret;
    }

    void TrfmTranslateY::applyDeriv(const Dof* q, Vector3d& v){
        if(mDofs[0] != q) v=Vector3d::Zero();
        else {
            v = Vector3d(0, 1, 0);
        }
    }

    void TrfmTranslateY::applyDeriv(const Dof* q, Matrix4d& m){
        if(mDofs[0] != q) m=Matrix4d::Zero();
        else for(int i=0; i<4; i++){
                if(i==A_Y) m.row(i)=m.row(3);
                else m.row(i).setZero();
            }
    }

    Matrix4d TrfmTranslateY::getSecondDeriv( const Dof *q1, const Dof *q2 ) {
        return Eigen::Matrix4d::Zero(); // always zero
    }

    void TrfmTranslateY::applySecondDeriv( const Dof* q1, const Dof* q2, Eigen::Vector3d& v ) {
        v.setZero();
    }

    void TrfmTranslateY::applySecondDeriv( const Dof* q1, const Dof* q2, Eigen::Matrix4d& m ) {
        m.setZero();
    }

    Matrix4d TrfmTranslateY::getInvTransform(){
        Matrix4d ret = Matrix4d::Ones();
        ret(A_Y, 3) = -mDofs[0]->getValue();
        return ret;
    }

    void TrfmTranslateY::applyTransform(Vector3d& v){
        v(A_Y) += mDofs[0]->getValue();
    }

    void TrfmTranslateY::applyInvTransform(Vector3d& v){
        v(A_Y) -= mDofs[0]->getValue();
    }

    void TrfmTranslateY::applyTransform(Matrix4d& m){
        m.row(A_Y) += mDofs[0]->getValue() * m.row(3);
    }

    void TrfmTranslateY::applyInvTransform(Matrix4d& m){
        m.row(A_Y) -= mDofs[0]->getValue() * m.row(3);
    }

    TrfmTranslateZ::TrfmTranslateZ(Dof *z, const char *_name){
        mDofs.resize(1);
        mDofs[0]=z;
        mDofs[0]->setTrans(this);
        mType = Transformation::T_TRANSLATEZ;
        if(_name!=NULL)
            strcpy(mName, _name);
        else
            strcpy(mName, "TranslateZ");
    }

    void TrfmTranslateZ::applyGLTransform(renderer::RenderInterface* _ri) const {
		if (_ri)
			_ri->translate(Vector3d(0, 0, mDofs[0]->getValue()));
	}

    void TrfmTranslateZ::computeTransform(){
        mTransform.setZero();
        mTransform(0, 0) = 1.0;
        mTransform(1, 1) = 1.0;
        mTransform(2, 2) = 1.0;
        mTransform(3, 3) = 1.0;
        mTransform(A_Z, 3) = mDofs[0]->getValue();
    }

    Matrix4d TrfmTranslateZ::getDeriv(const Dof *q){
        Matrix4d ret = Matrix4d::Zero();
        if(mDofs[0] == q)
            ret(A_Z, 3) = 1.0;
        return ret;
    }

    void TrfmTranslateZ::applyDeriv(const Dof* q, Vector3d& v){
        if(mDofs[0] != q) v=Vector3d::Zero();
        else v=Vector3d(0,0,1);
    }

    void TrfmTranslateZ::applyDeriv(const Dof* q, Matrix4d& m){
        if(mDofs[0] != q) m = Matrix4d::Zero();
        else for(int i=0; i<4; i++){
                if(i==A_Z) m.row(i) = m.row(3);
                else m.row(i).setZero();
            }
    }

    Matrix4d TrfmTranslateZ::getSecondDeriv( const Dof *q1, const Dof *q2 ) {
        return Eigen::Matrix4d::Zero(); // always zero
    }

    void TrfmTranslateZ::applySecondDeriv( const Dof* q1, const Dof* q2, Eigen::Vector3d& v ) {
        v.setZero();
    }

    void TrfmTranslateZ::applySecondDeriv( const Dof* q1, const Dof* q2, Eigen::Matrix4d& m ) {
        m.setZero();
    }

    Matrix4d TrfmTranslateZ::getInvTransform(){
        Matrix4d ret = Matrix4d::Ones();
        ret(A_Z, 3) = -mDofs[0]->getValue();
        return ret;
    }

    void TrfmTranslateZ::applyTransform(Vector3d& v){
        v(A_Z) += mDofs[0]->getValue();
    }

    void TrfmTranslateZ::applyInvTransform(Vector3d& v){
        v(A_Z) -= mDofs[0]->getValue();
    }

    void TrfmTranslateZ::applyTransform(Matrix4d& m){
        m.row(A_Z) += mDofs[0]->getValue() * m.row(3);
    }

    void TrfmTranslateZ::applyInvTransform(Matrix4d& m){
        m.row(A_Z) -= mDofs[0]->getValue() * m.row(3);
    }

} // namespace kinematics
<|endoftext|>


<commit_before>/* Copyright 2007-2015 QReal Research Group
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. */

#include <QtCore/QCoreApplication>
#include <QtCore/QDateTime>

#include "twoDModel/engine/model/timeline.h"
#include "modelTimer.h"

using namespace twoDModel::model;

Timeline::Timeline(QObject *parent)
	: QObject(parent)
	, mSpeedFactor(normalSpeedFactor)
	, mCyclesCount(0)
	, mIsStarted(false)
	, mTimestamp(0)
{
	connect(&mTimer, SIGNAL(timeout()), this, SLOT(onTimer()));
	mTimer.setInterval(defaultRealTimeInterval);
}

void Timeline::start()
{
	if (!mIsStarted) {
		mIsStarted = true;
		emit started();
		gotoNextFrame();
	}
}

void Timeline::stop(qReal::interpretation::StopReason reason)
{
	if (mIsStarted) {
		mIsStarted = false;
		QCoreApplication::processEvents();
		emit beforeStop(reason);
		mTimer.stop();
		emit stopped(reason);
	}
}

void Timeline::onTimer()
{
	if (!mIsStarted) {
		mTimer.stop();
		return;
	}

	for (int i = 0; i < ticksPerCycle; ++i) {
		QCoreApplication::processEvents();
		if (mIsStarted) {
			mTimestamp += timeInterval;
			emit tick();
			++mCyclesCount;
			if (mCyclesCount >= mSpeedFactor) {
				mTimer.stop();
				mCyclesCount = 0;
				const int msFromFrameStart = static_cast<int>(QDateTime::currentMSecsSinceEpoch()
						- mFrameStartTimestamp);
				const int pauseBeforeFrameEnd = mFrameLength - msFromFrameStart;
				if (pauseBeforeFrameEnd > 0) {
					QTimer::singleShot(pauseBeforeFrameEnd - 1, this, SLOT(gotoNextFrame()));
				} else {
					gotoNextFrame();
				}

				return;
			}
		}
	}
}

void Timeline::gotoNextFrame()
{
	emit nextFrame();
	mFrameStartTimestamp = QDateTime::currentMSecsSinceEpoch();
	if (!mTimer.isActive()) {
		emit tick(); /// hack so that constraints init-on would start immediatly.
		mTimer.start();
	}
}

int Timeline::speedFactor() const
{
	return mSpeedFactor;
}

bool Timeline::isStarted() const
{
	return mIsStarted;
}

quint64 Timeline::timestamp() const
{
	return mTimestamp;
}

utils::AbstractTimer *Timeline::produceTimer()
{
	return new ModelTimer(this);
}

void Timeline::setImmediateMode(bool immediateMode)
{
	mTimer.setInterval(immediateMode ? 0 : defaultRealTimeInterval);
	setSpeedFactor(immediateMode ? immediateSpeedFactor : normalSpeedFactor);
	mFrameLength = immediateMode ? 0 : defaultFrameLength;
}

void Timeline::setSpeedFactor(int factor)
{
	if (mSpeedFactor != factor) {
		mSpeedFactor = factor;
		emit speedFactorChanged(factor);
	}
}
<commit_msg>fix tomestamp issue<commit_after>/* Copyright 2007-2015 QReal Research Group
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. */

#include <QtCore/QCoreApplication>
#include <QtCore/QDateTime>

#include "twoDModel/engine/model/timeline.h"
#include "modelTimer.h"

using namespace twoDModel::model;

Timeline::Timeline(QObject *parent)
	: QObject(parent)
	, mSpeedFactor(normalSpeedFactor)
	, mCyclesCount(0)
	, mIsStarted(false)
	, mTimestamp(0)
{
	connect(&mTimer, SIGNAL(timeout()), this, SLOT(onTimer()));
	mTimer.setInterval(defaultRealTimeInterval);
}

void Timeline::start()
{
	if (!mIsStarted) {
		mIsStarted = true;
		emit started();
		emit tick(); /// hack so that constraints init-on would start immediatly
		/// ideally they should be triggered on started()
		gotoNextFrame();
	}
}

void Timeline::stop(qReal::interpretation::StopReason reason)
{
	if (mIsStarted) {
		mIsStarted = false;
		QCoreApplication::processEvents();
		emit beforeStop(reason);
		mTimer.stop();
		emit stopped(reason);
	}
}

void Timeline::onTimer()
{
	if (!mIsStarted) {
		mTimer.stop();
		return;
	}

	for (int i = 0; i < ticksPerCycle; ++i) {
		QCoreApplication::processEvents();
		if (mIsStarted) {
			mTimestamp += timeInterval;
			emit tick();
			++mCyclesCount;
			if (mCyclesCount >= mSpeedFactor) {
				mTimer.stop();
				mCyclesCount = 0;
				const int msFromFrameStart = static_cast<int>(QDateTime::currentMSecsSinceEpoch()
						- mFrameStartTimestamp);
				const int pauseBeforeFrameEnd = mFrameLength - msFromFrameStart;
				if (pauseBeforeFrameEnd > 0) {
					QTimer::singleShot(pauseBeforeFrameEnd - 1, this, SLOT(gotoNextFrame()));
				} else {
					gotoNextFrame();
				}

				return;
			}
		}
	}
}

void Timeline::gotoNextFrame()
{
	emit nextFrame();
	mFrameStartTimestamp = QDateTime::currentMSecsSinceEpoch();
	if (!mTimer.isActive()) {
		mTimer.start();
	}
}

int Timeline::speedFactor() const
{
	return mSpeedFactor;
}

bool Timeline::isStarted() const
{
	return mIsStarted;
}

quint64 Timeline::timestamp() const
{
	return mTimestamp;
}

utils::AbstractTimer *Timeline::produceTimer()
{
	return new ModelTimer(this);
}

void Timeline::setImmediateMode(bool immediateMode)
{
	mTimer.setInterval(immediateMode ? 0 : defaultRealTimeInterval);
	setSpeedFactor(immediateMode ? immediateSpeedFactor : normalSpeedFactor);
	mFrameLength = immediateMode ? 0 : defaultFrameLength;
}

void Timeline::setSpeedFactor(int factor)
{
	if (mSpeedFactor != factor) {
		mSpeedFactor = factor;
		emit speedFactorChanged(factor);
	}
}
<|endoftext|>


<commit_before>/**
 * Based on: https://github.com/davetcoleman/ros_control_boilerplate
 */

#include <kulbu_hardware/kulbu_hardware_interface.h>
#include <fcntl.h>

#define SYSFS_GPIO_DIR  "/sys/class/gpio"
#define SYSFS_PWM_DIR   "/sys/devices/platform/pwm-ctrl"
#define MAX_BUF 256

// FIXME: Encapsulate this IO code.
int pwm_enable(unsigned int pwm, bool enable) {
  int fd;
  char buf[MAX_BUF];

  // First check current value, only update if changed.
  char curr[11];
  int status;
  snprintf(buf, sizeof(buf), SYSFS_PWM_DIR  "/enable%d", pwm);
  fd = open(buf, sizeof(buf), O_RDONLY);
  if (fd < 0) {
    ROS_ERROR_STREAM_NAMED("kulbu_hardware_interface",
      "pwm/enable/read");
    return fd;
  }
  read(fd, curr, sizeof(curr));
  close(fd);

  // "On" always ends with an "n". Quick and dirty.
  status = (curr[strlen(curr)-2] == 'n');

  if (status != enable) {
    ROS_DEBUG_STREAM_NAMED("kulbu_hardware_interface",
       "Chan: " << pwm << " Enable: " << enable);

    snprintf(buf, sizeof(buf), SYSFS_PWM_DIR  "/enable%d", pwm);
    fd = open(buf, O_WRONLY);
    if (fd < 0) {
      ROS_ERROR_STREAM_NAMED("kulbu_hardware_interface",
        "pwm/enable/write");
      return fd;
    }

    if (enable)
      write(fd, "1", 2);
    else
      write(fd, "0", 2);

    close(fd);
  }
  return 0;
}

int pwm_freq(unsigned int pwm, unsigned int freq) {
  int fd;
  char buf[MAX_BUF];

  // First check current value, only update if changed.
  char curr[11];
  int status;
  snprintf(buf, sizeof(buf), SYSFS_PWM_DIR  "/freq%d", pwm);
  fd = open(buf, sizeof(buf), O_RDONLY);
  if (fd < 0) {
    ROS_ERROR_STREAM_NAMED("kulbu_hardware_interface",
      "pwm/freq/read");
    return fd;
  }
  read(fd, curr, sizeof(curr));
  close(fd);

  if (atoi(curr) != freq && freq >= 1) {
    ROS_DEBUG_STREAM_NAMED("kulbu_hardware_interface",
       "Chan: " << pwm << " Freq: " << freq);

    snprintf(buf, sizeof(buf), SYSFS_PWM_DIR  "/freq%d", pwm);
    fd = open(buf, O_WRONLY);
    if (fd < 0) {
      ROS_ERROR_STREAM_NAMED("kulbu_hardware_interface",
        "pwm/freq/write");
      return fd;
    }

    int len = snprintf(buf, sizeof(buf), "%d", freq);
    write(fd, buf, len);
    close(fd);
  }
  return 0;
}

int pwm_duty(unsigned int pwm, unsigned int duty) {
  int fd;
  char buf[MAX_BUF];

  ROS_DEBUG_STREAM_NAMED("kulbu_hardware_interface",
     "Chan: " << pwm << " Duty: " << duty);

  snprintf(buf, sizeof(buf), SYSFS_PWM_DIR  "/duty%d", pwm);
  fd = open(buf, O_WRONLY);
  if (fd < 0) {
    ROS_ERROR_STREAM_NAMED("kulbu_hardware_interface",
      "pwm/duty/write");
    return fd;
  }

  int len = snprintf(buf, sizeof(buf), "%d", duty);
  write(fd, buf, len);
  close(fd);

  return 0;
}

/*
int gpio_export(unsigned int gpio) {
  int fd;
  char buf[MAX_BUF];

  ROS_DEBUG_STREAM_NAMED("kulbu_hardware_interface",
     "Export: " << gpio);

  fd = open(SYSFS_GPIO_DIR "/export", O_WRONLY);
  if (fd < 0) {
    ROS_ERROR_STREAM_NAMED("kulbu_hardware_interface",
      "gpio/export");
    return fd;
  }

  int len = snprintf(buf, sizeof(buf), "%d", gpio);
  write(fd, buf, len);
  close(fd);

  return 0;
}

int gpio_dir(unsigned int gpio, bool output) {
  int fd;
  char buf[MAX_BUF];

  ROS_DEBUG_STREAM_NAMED("kulbu_hardware_interface",
     "Dir: " << gpio << " Output: " << output);

  snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/direction", gpio);
  fd = open(buf, O_WRONLY);
  if (fd < 0) {
    ROS_ERROR_STREAM_NAMED("kulbu_hardware_interface",
      "gpio/direction");
    return fd;
  }

  if (output)
    write(fd, "out", 2);
  else
    write(fd, "in", 2);

  close(fd);
  return 0;
}
*/

int gpio_set(unsigned int gpio, unsigned int value) {
  int fd;
  char buf[MAX_BUF];

  ROS_DEBUG_STREAM_NAMED("kulbu_hardware_interface",
     "GPIO: " << gpio << " Value: " << value);

  snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/value", gpio);
  fd = open(buf, O_WRONLY);
  if (fd < 0) {
    ROS_ERROR_STREAM_NAMED("kulbu_hardware_interface",
      "gpio/set-value");
    return fd;
  }

  if (value)
    write(fd, "1", 2);
  else
    write(fd, "0", 2);

  close(fd);
  return 0;
}

namespace kulbu_hardware {

KulbuHardwareInterface::KulbuHardwareInterface(
  ros::NodeHandle& nh,
  int joint_mode)
  : nh_(nh),
  joint_mode_(joint_mode) {
  // Initialize shared memory and interfaces here
  init();  // this implementation loads from rosparam

  ROS_INFO_NAMED("kulbu_hardware_interface",
    "Loaded kulbu_hardware_interface.");
}

void KulbuHardwareInterface::init() {
  ROS_INFO_STREAM_NAMED("kulbu_hardware_interface",
    "Reading rosparams from namespace: " << nh_.getNamespace());

  nh_.getParam("hardware_interface/steps_per_m", steps_per_m_);
  if (!steps_per_m_ > 0) {
    ROS_FATAL_STREAM_NAMED("kulbu_hardware_interface",
      "No step per metre found on parameter server."
        << " Namespace: " << nh_.getNamespace());
    exit(-1);
  }

  // Get joint names
  nh_.getParam("hardware_interface/joints", joint_names_);
  if (joint_names_.size() == 0) {
    ROS_FATAL_STREAM_NAMED("kulbu_hardware_interface",
      "No joints found on parameter server"
        << " Namespace: " << nh_.getNamespace());
    exit(-1);
  }
  num_joints_ = joint_names_.size();

  // Get direction GPIO pins; 0 = forward, 1 = reverse.
  nh_.getParam("hardware_interface/dirs", pin_dirs_);
  if (pin_dirs_.size() == 0) {
    ROS_FATAL_STREAM_NAMED("kulbu_hardware_interface",
      "No GPIO direction pins found on parameter server."
        << " Namespace: " << nh_.getNamespace());
    exit(-1);
  }

  // Get PWM sysfs indexes.
  nh_.getParam("hardware_interface/steps", pin_steps_);
  if (pin_steps_.size() == 0) {
    ROS_FATAL_STREAM_NAMED("kulbu_hardware_interface",
      "No GPIO step pins found on parameter server."
        << " Namespace: " << nh_.getNamespace());
    exit(-1);
  }

  // Resize vectors
  joint_position_.resize(num_joints_);
  joint_velocity_.resize(num_joints_);
  joint_effort_.resize(num_joints_);
  joint_position_command_.resize(num_joints_);
  joint_velocity_command_.resize(num_joints_);
  joint_effort_command_.resize(num_joints_);

  // Initialize controller
  for (std::size_t i = 0; i < num_joints_; ++i) {
    ROS_DEBUG_STREAM_NAMED("kulbu_hardware_interface",
      "Loading joint name: " << joint_names_[i]);

    // Start with PWM turned off and 50/50 duty cycle.
    pwm_enable(pin_steps_[i], 0);
    pwm_duty(pin_steps_[i], 512);

    // Export GPIO pin for output.
    // FIXME: Permission denied? Manual export `gpio export 88 out`
    // gpio_export(pin_dirs_[i]);
    // gpio_dir(pin_dirs_[i], 1);

    // Set direction pins to forward by default.
    gpio_set(pin_dirs_[i], 0);

    // Create joint state interface
    joint_state_interface_.registerHandle(hardware_interface::JointStateHandle(
      joint_names_[i],
      &joint_position_[i],
      &joint_velocity_[i],
      &joint_effort_[i]));

    switch (joint_mode_) {
      case 0:
        // Create position joint interface
        position_joint_interface_
          .registerHandle(hardware_interface::JointHandle(
            joint_state_interface_.getHandle(
              joint_names_[i]),
              &joint_position_command_[i]));
        break;
      case 1:
        // Create velocity joint interface
        velocity_joint_interface_
          .registerHandle(hardware_interface::JointHandle(
            joint_state_interface_.getHandle(
              joint_names_[i]),
              &joint_velocity_command_[i]));
        break;
      case 2:
        // Create effort joint interface
        effort_joint_interface_
          .registerHandle(hardware_interface::JointHandle(
            joint_state_interface_.getHandle(
              joint_names_[i]),
              &joint_effort_command_[i]));
        break;
    }
  }

  registerInterface(&joint_state_interface_);  // From RobotHW base class.
  registerInterface(&position_joint_interface_);  // From RobotHW base class.
  registerInterface(&velocity_joint_interface_);  // From RobotHW base class.
  registerInterface(&effort_joint_interface_);  // From RobotHW base class.
}

void KulbuHardwareInterface::read(ros::Duration elapsed_time) {
  // TODO: Optical encoders.

  // Read the joint states from your hardware here
  // e.g.
  // for (std::size_t i = 0; i < num_joints_; ++i)
  // {
  //   joint_position_[i] = robot_api_.getJointPosition(i);
  //   joint_velocity_[i] = robot_api_.getJointVelocity(i);
  //   joint_effort_[i] = robot_api_.getJointEffort(i);
  // }
}

void KulbuHardwareInterface::write(ros::Duration elapsed_time) {
  int freq = 0;
  int dir = 0;

  // Send commands in different modes
  for (std::size_t i = 0; i < num_joints_; ++i) {
    // Skip joints with no pins defined
    if (pin_dirs_[i] == -1 || pin_steps_[i] == -1) continue;

    switch (joint_mode_) {
      case 1:  // hardware_interface::MODE_VELOCITY:
        // Calc PWM frequency.
        freq = joint_velocity_command_[i] * steps_per_m_;

       // DEBUG: instantly adding velocity to state
        joint_velocity_[i] = joint_velocity_command_[i];

        // Reverse flips direction.
        if (freq < 0) {
          freq = abs(freq);
          dir = 1;
        } else {
          dir = 0;
        }

        // Disable PWM when stopped.
        if (freq < 1) {
          freq = 0;
          pwm_enable(pin_steps_[i], 0);
        } else {
          pwm_enable(pin_steps_[i], 1);
        }

        // Set direction pin.
        gpio_set(pin_dirs_[i], dir);

        // Set PWM frequency.
        pwm_freq(pin_steps_[i], freq);

        ROS_DEBUG_STREAM_NAMED("kulbu_hardware_interface",
          "\ni: "     << i <<
          "\ncmd: "   << joint_velocity_command_[i] <<
          "\ndist: "  << steps_per_m_ <<
          "\nfreq: "  << freq <<
          "\ndir: "   << dir <<
          "\njoint: " << joint_names_[i] <<
          "\ndir: "   << pin_dirs_[i] <<
          "\nstep: "  << pin_steps_[i]);
        break;

      default:
        ROS_ERROR_STREAM_NAMED("kulbu_hardware_interface",
          "Joint mode not implemented");
        break;
    }
  }
}

}  // namespace kulbu_hardware
<commit_msg>Playing with sysfs<commit_after>/**
 * Based on: https://github.com/davetcoleman/ros_control_boilerplate
 */

#include <kulbu_hardware/kulbu_hardware_interface.h>
#include <fcntl.h>

#define SYSFS_GPIO_DIR  "/sys/class/gpio"
#define SYSFS_PWM_DIR   "/sys/devices/platform/pwm-ctrl"
#define MAX_BUF 256

// FIXME: Encapsulate this IO code.
int pwm_enable(unsigned int pwm, bool enable) {
  int fd;
  char buf[MAX_BUF];

  // First check current value, only update if changed.
  char curr[11];
  int status;
  snprintf(buf, sizeof(buf), SYSFS_PWM_DIR  "/enable%d", pwm);
  fd = open(buf, sizeof(buf), O_RDONLY);
  if (fd < 0) {
    ROS_ERROR_STREAM_NAMED("kulbu_hardware_interface",
      "pwm/enable/read");
    return fd;
  }
  read(fd, curr, sizeof(curr));
  close(fd);

  // "On" always ends with an "n". Quick and dirty.
  status = (curr[strlen(curr)-2] == 'n');

  if (status != enable) {
    ROS_DEBUG_STREAM_NAMED("kulbu_hardware_interface",
       "Chan: " << pwm << " Enable: " << enable);

    snprintf(buf, sizeof(buf), SYSFS_PWM_DIR  "/enable%d", pwm);
    fd = open(buf, O_WRONLY);
    if (fd < 0) {
      ROS_ERROR_STREAM_NAMED("kulbu_hardware_interface",
        "pwm/enable/write");
      return fd;
    }

    if (enable)
      write(fd, "1", 2);
    else
      write(fd, "0", 2);

    close(fd);
  }
  return 0;
}

int pwm_freq(unsigned int pwm, unsigned int freq) {
  int fd;
  char buf[MAX_BUF];

  // First check current value, only update if changed.
  char curr[11];
  int status;
  snprintf(buf, sizeof(buf), SYSFS_PWM_DIR  "/freq%d", pwm);
  fd = open(buf, sizeof(buf), O_RDONLY);
  if (fd < 0) {
    ROS_ERROR_STREAM_NAMED("kulbu_hardware_interface",
      "pwm/freq/read");
    return fd;
  }
  read(fd, curr, sizeof(curr));
  close(fd);

  if (atoi(curr) != freq && freq >= 1) {
    ROS_DEBUG_STREAM_NAMED("kulbu_hardware_interface",
       "Chan: " << pwm << " Freq: " << freq);

    snprintf(buf, sizeof(buf), SYSFS_PWM_DIR  "/freq%d", pwm);
    fd = open(buf, O_WRONLY);
    if (fd < 0) {
      ROS_ERROR_STREAM_NAMED("kulbu_hardware_interface",
        "pwm/freq/write");
      return fd;
    }

    int len = snprintf(buf, sizeof(buf), "%d", freq);
    write(fd, buf, len);
    close(fd);
  }
  return 0;
}

int pwm_duty(unsigned int pwm, unsigned int duty) {
  int fd;
  char buf[MAX_BUF];

  ROS_DEBUG_STREAM_NAMED("kulbu_hardware_interface",
     "Chan: " << pwm << " Duty: " << duty);

  snprintf(buf, sizeof(buf), SYSFS_PWM_DIR  "/duty%d", pwm);
  fd = open(buf, O_WRONLY);
  if (fd < 0) {
    ROS_ERROR_STREAM_NAMED("kulbu_hardware_interface",
      "pwm/duty/write");
    return fd;
  }

  int len = snprintf(buf, sizeof(buf), "%d", duty);
  write(fd, buf, len);
  close(fd);

  return 0;
}

/*
int gpio_own(char *file) {
  uid_t uid = getuid();
  uid_t gid = getgid();

  if (chown (file, uid, gid) != 0) {
    if (errno == ENOENT) {
      ROS_WARN_STREAM_NAMED("kulbu_hardware_interface",
        "gpio/own File does not exist");
    } else {
      ROS_FATAL_STREAM_NAMED("kulbu_hardware_interface",
        "gpio/own Unable to change ownership "
        << file << " Error: " << strerror (errno));
      exit (1) ;
    }
  }

}

int gpio_export(unsigned int gpio) {
  int fd;
  char buf[MAX_BUF];

  ROS_DEBUG_STREAM_NAMED("kulbu_hardware_interface",
     "Export: " << gpio);

  fd = open(SYSFS_GPIO_DIR "/export", O_WRONLY);
  if (fd < 0) {
    ROS_ERROR_STREAM_NAMED("kulbu_hardware_interface",
      "gpio/export");
    return fd;
  }

  int len = snprintf(buf, sizeof(buf), "%d", gpio);
  write(fd, buf, len);
  close(fd);

  snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/value", gpio);
  gpio_own(buf);

  snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/edge", gpio);
  gpio_own(buf);

 return 0;
}

int gpio_dir(unsigned int gpio, bool output) {
  int fd;
  char buf[MAX_BUF];

  ROS_DEBUG_STREAM_NAMED("kulbu_hardware_interface",
     "Dir: " << gpio << " Output: " << output);

  snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/direction", gpio);
  fd = open(buf, O_WRONLY);
  if (fd < 0) {
    ROS_ERROR_STREAM_NAMED("kulbu_hardware_interface",
      "gpio/direction");
    return fd;
  }

  if (output)
    write(fd, "out", 2);
  else
    write(fd, "in", 2);

  close(fd);

  snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/direction", gpio);
  gpio_own(buf);

  return 0;
}
*/

int gpio_set(unsigned int gpio, unsigned int value) {
  int fd;
  char buf[MAX_BUF];

  ROS_DEBUG_STREAM_NAMED("kulbu_hardware_interface",
     "GPIO: " << gpio << " Value: " << value);

  snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/value", gpio);
  fd = open(buf, O_WRONLY);
  if (fd < 0) {
    ROS_ERROR_STREAM_NAMED("kulbu_hardware_interface",
      "gpio/set-value");
    return fd;
  }

  if (value)
    write(fd, "1", 2);
  else
    write(fd, "0", 2);

  close(fd);
  return 0;
}

namespace kulbu_hardware {

KulbuHardwareInterface::KulbuHardwareInterface(
  ros::NodeHandle& nh,
  int joint_mode)
  : nh_(nh),
  joint_mode_(joint_mode) {
  // Initialize shared memory and interfaces here
  init();  // this implementation loads from rosparam

  ROS_INFO_NAMED("kulbu_hardware_interface",
    "Loaded kulbu_hardware_interface.");
}

void KulbuHardwareInterface::init() {
  ROS_INFO_STREAM_NAMED("kulbu_hardware_interface",
    "Reading rosparams from namespace: " << nh_.getNamespace());

  nh_.getParam("hardware_interface/steps_per_m", steps_per_m_);
  if (!steps_per_m_ > 0) {
    ROS_FATAL_STREAM_NAMED("kulbu_hardware_interface",
      "No step per metre found on parameter server."
        << " Namespace: " << nh_.getNamespace());
    exit(-1);
  }

  // Get joint names
  nh_.getParam("hardware_interface/joints", joint_names_);
  if (joint_names_.size() == 0) {
    ROS_FATAL_STREAM_NAMED("kulbu_hardware_interface",
      "No joints found on parameter server"
        << " Namespace: " << nh_.getNamespace());
    exit(-1);
  }
  num_joints_ = joint_names_.size();

  // Get direction GPIO pins; 0 = forward, 1 = reverse.
  nh_.getParam("hardware_interface/dirs", pin_dirs_);
  if (pin_dirs_.size() == 0) {
    ROS_FATAL_STREAM_NAMED("kulbu_hardware_interface",
      "No GPIO direction pins found on parameter server."
        << " Namespace: " << nh_.getNamespace());
    exit(-1);
  }

  // Get PWM sysfs indexes.
  nh_.getParam("hardware_interface/steps", pin_steps_);
  if (pin_steps_.size() == 0) {
    ROS_FATAL_STREAM_NAMED("kulbu_hardware_interface",
      "No GPIO step pins found on parameter server."
        << " Namespace: " << nh_.getNamespace());
    exit(-1);
  }

  // Resize vectors
  joint_position_.resize(num_joints_);
  joint_velocity_.resize(num_joints_);
  joint_effort_.resize(num_joints_);
  joint_position_command_.resize(num_joints_);
  joint_velocity_command_.resize(num_joints_);
  joint_effort_command_.resize(num_joints_);

  // Initialize controller
  for (std::size_t i = 0; i < num_joints_; ++i) {
    ROS_DEBUG_STREAM_NAMED("kulbu_hardware_interface",
      "Loading joint name: " << joint_names_[i]);

    // Export GPIO pin for output.
    // FIXME: Permission denied? Manual export `gpio export 88 out`
    // gpio_export(pin_dirs_[i]);
    // gpio_dir(pin_dirs_[i], 1);

    // Start with PWM turned off and 50/50 duty cycle.
    pwm_enable(pin_steps_[i], 0);
    pwm_duty(pin_steps_[i], 512);

    // Set direction pins to forward by default.
    gpio_set(pin_dirs_[i], 0);

    // Create joint state interface
    joint_state_interface_.registerHandle(hardware_interface::JointStateHandle(
      joint_names_[i],
      &joint_position_[i],
      &joint_velocity_[i],
      &joint_effort_[i]));

    switch (joint_mode_) {
      case 0:
        // Create position joint interface
        position_joint_interface_
          .registerHandle(hardware_interface::JointHandle(
            joint_state_interface_.getHandle(
              joint_names_[i]),
              &joint_position_command_[i]));
        break;
      case 1:
        // Create velocity joint interface
        velocity_joint_interface_
          .registerHandle(hardware_interface::JointHandle(
            joint_state_interface_.getHandle(
              joint_names_[i]),
              &joint_velocity_command_[i]));
        break;
      case 2:
        // Create effort joint interface
        effort_joint_interface_
          .registerHandle(hardware_interface::JointHandle(
            joint_state_interface_.getHandle(
              joint_names_[i]),
              &joint_effort_command_[i]));
        break;
    }
  }

  registerInterface(&joint_state_interface_);  // From RobotHW base class.
  registerInterface(&position_joint_interface_);  // From RobotHW base class.
  registerInterface(&velocity_joint_interface_);  // From RobotHW base class.
  registerInterface(&effort_joint_interface_);  // From RobotHW base class.
}

void KulbuHardwareInterface::read(ros::Duration elapsed_time) {
  // TODO: Optical encoders.

  // Read the joint states from your hardware here
  // e.g.
  // for (std::size_t i = 0; i < num_joints_; ++i)
  // {
  //   joint_position_[i] = robot_api_.getJointPosition(i);
  //   joint_velocity_[i] = robot_api_.getJointVelocity(i);
  //   joint_effort_[i] = robot_api_.getJointEffort(i);
  // }
}

void KulbuHardwareInterface::write(ros::Duration elapsed_time) {
  int freq = 0;
  int dir = 0;

  // Send commands in different modes
  for (std::size_t i = 0; i < num_joints_; ++i) {
    // Skip joints with no pins defined
    if (pin_dirs_[i] == -1 || pin_steps_[i] == -1) continue;

    switch (joint_mode_) {
      case 1:  // hardware_interface::MODE_VELOCITY:
        // Calc PWM frequency.
        freq = joint_velocity_command_[i] * steps_per_m_;

       // DEBUG: instantly adding velocity to state
        joint_velocity_[i] = joint_velocity_command_[i];

        // Reverse flips direction.
        if (freq < 0) {
          freq = abs(freq);
          dir = 1;
        } else {
          dir = 0;
        }

        // Disable PWM when stopped.
        if (freq < 1) {
          freq = 0;
          pwm_enable(pin_steps_[i], 0);
        } else {
          pwm_enable(pin_steps_[i], 1);
        }

        // Set direction pin.
        gpio_set(pin_dirs_[i], dir);

        // Set PWM frequency.
        pwm_freq(pin_steps_[i], freq);

        ROS_DEBUG_STREAM_NAMED("kulbu_hardware_interface",
          "\ni: "     << i <<
          "\ncmd: "   << joint_velocity_command_[i] <<
          "\ndist: "  << steps_per_m_ <<
          "\nfreq: "  << freq <<
          "\ndir: "   << dir <<
          "\njoint: " << joint_names_[i] <<
          "\ndir: "   << pin_dirs_[i] <<
          "\nstep: "  << pin_steps_[i]);
        break;

      default:
        ROS_ERROR_STREAM_NAMED("kulbu_hardware_interface",
          "Joint mode not implemented");
        break;
    }
  }
}

}  // namespace kulbu_hardware
<|endoftext|>