<commit_before>#include <cassert>
#include <cmath>
#include <stdexcept>
#include <ostream>
#include "reasoning/kanade_reasoner.h"

using namespace std;

namespace Tracking {

////
//// class KanadeIlp
////
  KanadeIlp::~KanadeIlp() {
    env_.end();
  }

  size_t KanadeIlp::add_init_hypothesis(size_t idx, double cost) {
    assert(idx < n_tracklets_);
    x_.add(IloBoolVar( obj_(cost) + c_[n_tracklets_ + idx](1) ));
    n_hypotheses_ += 1;
    return n_hypotheses_ - 1;
  }

  size_t KanadeIlp::add_term_hypothesis(size_t idx, double cost) {
    assert(idx < n_tracklets_);
    x_.add(IloBoolVar( obj_(cost) + c_[idx](1) ));
    n_hypotheses_ += 1;
    return n_hypotheses_ - 1;
  }

  size_t KanadeIlp::add_trans_hypothesis(size_t from_idx, size_t to_idx, double cost) {
    assert(from_idx < n_tracklets_);
    assert(to_idx < n_tracklets_);
    x_.add(IloBoolVar( obj_(cost) + c_[from_idx](1) + c_[n_tracklets_ + to_idx](1) ));
    n_hypotheses_ += 1;
    return n_hypotheses_ - 1;
  }

  size_t KanadeIlp::add_div_hypothesis(size_t from_idx, size_t to_idx1, size_t to_idx2, double cost) {
    assert(from_idx < n_tracklets_);
    assert(to_idx1 < n_tracklets_);
    assert(to_idx2 < n_tracklets_);
    x_.add(IloBoolVar( obj_(cost) + c_[from_idx](1) + c_[n_tracklets_ + to_idx1](1) + c_[n_tracklets_ + to_idx2](1) ));
    n_hypotheses_ += 1;
    return n_hypotheses_ - 1;
  }

  size_t KanadeIlp::add_fp_hypothesis(size_t idx, double cost) {
    assert(idx < n_tracklets_);
    x_.add(IloBoolVar( obj_(cost) + c_[idx](1) + c_[n_tracklets_ + idx](1)));
    n_hypotheses_ += 1;
    return n_hypotheses_ - 1;
  }

  KanadeIlp& KanadeIlp::solve() {
    // assemble model
    model_.add(obj_);
    model_.add(x_);
    model_.add(c_);

    // solve
    cplex_.extract(model_);
    if( !cplex_.solve() ) {
      throw runtime_error("KanadeIlp::solve(): cplex.solve() failed");
    }
    env_.out() << "Kanade solution status = " << cplex_.getStatus() << "\n";
    env_.out() << "Kanade objective value: " << cplex_.getObjValue() << "\n";
    cplex_.getValues(sol_, x_);
    //env_.out() << "Kanade solution: " << sol_ << "\n";

    return *this;
  }

  size_t KanadeIlp::solution_size() {
    size_t size = static_cast<size_t>(sol_.getSize());
    assert(size == n_hypotheses_);
    return size;
  }

  bool KanadeIlp::hypothesis_is_active( size_t idx ) {
    assert(idx < n_hypotheses_);
    return static_cast<bool>(sol_[idx]);
  }

  ////
  //// class Kanade
  ////
  Kanade::~Kanade() {
    if(ilp_ != NULL) {
      delete ilp_;
      ilp_ = NULL;
    }
  }

  void Kanade::formulate( const HypothesesGraph& g ) {
    reset();

    // flase positive hypotheses
    size_t count = 0;
    for(HypothesesGraph::NodeIt n(g); n!=lemon::INVALID; ++n) {
      tracklet_idx_map_[n] = count;
      hyp2type_[count] = FP;
      fp2node_[count] = n;
      ++count;
    }
    vector<double> fp_costs(lemon::countNodes( g ), log(misdetection_rate_));

    ilp_ = new KanadeIlp( fp_costs.begin(), fp_costs.end());

    for(HypothesesGraph::NodeIt n(g); n!=lemon::INVALID; ++n) {
      add_hypotheses(g, n);
    }
  }

  void Kanade::infer() {
}

  void Kanade::conclude( HypothesesGraph& /*g*/ ) {
}

  void Kanade::reset() {
    if(ilp_ != NULL) {
      delete ilp_;
      ilp_ = NULL;
    }
  }

  Kanade& Kanade::add_hypotheses( const HypothesesGraph& g, const HypothesesGraph::Node& n) {
    double COST = 100.2;
    size_t hyp = 0;

    // init hypothesis
    hyp = ilp_->add_init_hypothesis( tracklet_idx_map_[n], COST );
    hyp2type_[hyp] = INIT;

    // collect and count outgoing arcs
    vector<HypothesesGraph::Arc> arcs;
    size_t count = 0;
    for(HypothesesGraph::OutArcIt a(g, n); a != lemon::INVALID; ++a) {
      arcs.push_back(a);
      ++count;
    }

    if(count == 0) {
      hyp = ilp_->add_term_hypothesis(tracklet_idx_map_[n], COST );
      hyp2type_[hyp] = TERM;
    } else if(count == 1) {
      hyp = ilp_->add_term_hypothesis( tracklet_idx_map_[n], COST );
      hyp2type_[hyp] = TERM;

      hyp = ilp_->add_trans_hypothesis( tracklet_idx_map_[n], tracklet_idx_map_[g.target(arcs[0])], COST);
      hyp2type_[hyp] = TRANS;
      trans2arc_[hyp] = arcs[0];
    } else {
      hyp = ilp_->add_term_hypothesis( tracklet_idx_map_[n], COST );
      hyp2type_[hyp] = TERM;

      // translation hypotheses
      for(size_t i = 0; i < count; ++i) {
	hyp = ilp_->add_trans_hypothesis( tracklet_idx_map_[n], tracklet_idx_map_[g.target(arcs[i])], COST);
	hyp2type_[hyp] = TRANS;
	trans2arc_[hyp] = arcs[i];
      }

      // division hypotheses
      for(size_t i = 0; i < count - 1; ++i) {
	for(size_t j = i; j < count; ++j) {
	  hyp = ilp_->add_div_hypothesis( tracklet_idx_map_[n],
				    tracklet_idx_map_[g.target(arcs[i])],
				    tracklet_idx_map_[g.target(arcs[j])],
				    COST);
	  hyp2type_[hyp] = DIV;
	  div2arcs_[hyp] = pair<HypothesesGraph::Arc, HypothesesGraph::Arc>(arcs[i], arcs[j]);
	}
      }
    }

    return *this;
  }

} /* namespace Tracking */
<commit_msg>implemented Kanade::conclude()<commit_after>#include <cassert>
#include <cmath>
#include <stdexcept>
#include <ostream>
#include "reasoning/kanade_reasoner.h"

using namespace std;

namespace Tracking {

////
//// class KanadeIlp
////
  KanadeIlp::~KanadeIlp() {
    env_.end();
  }

  size_t KanadeIlp::add_init_hypothesis(size_t idx, double cost) {
    assert(idx < n_tracklets_);
    x_.add(IloBoolVar( obj_(cost) + c_[n_tracklets_ + idx](1) ));
    n_hypotheses_ += 1;
    return n_hypotheses_ - 1;
  }

  size_t KanadeIlp::add_term_hypothesis(size_t idx, double cost) {
    assert(idx < n_tracklets_);
    x_.add(IloBoolVar( obj_(cost) + c_[idx](1) ));
    n_hypotheses_ += 1;
    return n_hypotheses_ - 1;
  }

  size_t KanadeIlp::add_trans_hypothesis(size_t from_idx, size_t to_idx, double cost) {
    assert(from_idx < n_tracklets_);
    assert(to_idx < n_tracklets_);
    x_.add(IloBoolVar( obj_(cost) + c_[from_idx](1) + c_[n_tracklets_ + to_idx](1) ));
    n_hypotheses_ += 1;
    return n_hypotheses_ - 1;
  }

  size_t KanadeIlp::add_div_hypothesis(size_t from_idx, size_t to_idx1, size_t to_idx2, double cost) {
    assert(from_idx < n_tracklets_);
    assert(to_idx1 < n_tracklets_);
    assert(to_idx2 < n_tracklets_);
    x_.add(IloBoolVar( obj_(cost) + c_[from_idx](1) + c_[n_tracklets_ + to_idx1](1) + c_[n_tracklets_ + to_idx2](1) ));
    n_hypotheses_ += 1;
    return n_hypotheses_ - 1;
  }

  size_t KanadeIlp::add_fp_hypothesis(size_t idx, double cost) {
    assert(idx < n_tracklets_);
    x_.add(IloBoolVar( obj_(cost) + c_[idx](1) + c_[n_tracklets_ + idx](1)));
    n_hypotheses_ += 1;
    return n_hypotheses_ - 1;
  }

  KanadeIlp& KanadeIlp::solve() {
    // assemble model
    model_.add(obj_);
    model_.add(x_);
    model_.add(c_);

    // solve
    cplex_.extract(model_);
    if( !cplex_.solve() ) {
      throw runtime_error("KanadeIlp::solve(): cplex.solve() failed");
    }
    env_.out() << "Kanade solution status = " << cplex_.getStatus() << "\n";
    env_.out() << "Kanade objective value: " << cplex_.getObjValue() << "\n";
    cplex_.getValues(sol_, x_);
    //env_.out() << "Kanade solution: " << sol_ << "\n";

    return *this;
  }

  size_t KanadeIlp::solution_size() {
    size_t size = static_cast<size_t>(sol_.getSize());
    assert(size == n_hypotheses_);
    return size;
  }

  bool KanadeIlp::hypothesis_is_active( size_t idx ) {
    assert(idx < n_hypotheses_);
    return static_cast<bool>(sol_[idx]);
  }

  ////
  //// class Kanade
  ////
  Kanade::~Kanade() {
    if(ilp_ != NULL) {
      delete ilp_;
      ilp_ = NULL;
    }
  }

  void Kanade::formulate( const HypothesesGraph& g ) {
    reset();

    // flase positive hypotheses
    size_t count = 0;
    for(HypothesesGraph::NodeIt n(g); n!=lemon::INVALID; ++n) {
      tracklet_idx_map_[n] = count;
      hyp2type_[count] = FP;
      fp2node_[count] = n;
      ++count;
    }
    vector<double> fp_costs(lemon::countNodes( g ), log(misdetection_rate_));

    ilp_ = new KanadeIlp( fp_costs.begin(), fp_costs.end());

    for(HypothesesGraph::NodeIt n(g); n!=lemon::INVALID; ++n) {
      add_hypotheses(g, n);
    }
  }

  void Kanade::infer() {
    ilp_->solve();
  }

  void Kanade::conclude( HypothesesGraph& g ) {
    // add active property to graph and init nodes/arcs as active/inactive
    g.add(node_active()).add(arc_active());
    property_map<node_active, HypothesesGraph::base_graph>::type& active_nodes = g.get(node_active());
    property_map<arc_active, HypothesesGraph::base_graph>::type& active_arcs = g.get(arc_active());

    for(HypothesesGraph::NodeIt n(g); n!=lemon::INVALID; ++n) {
      active_nodes.set(n, true);
    }
    for(HypothesesGraph::ArcIt a(g); a!=lemon::INVALID; ++a) {
      active_arcs.set(a, false);
    }

    // go thru all hyps
    for(size_t hyp = 0; hyp < ilp_->solution_size(); ++hyp) {
      if(ilp_->hypothesis_is_active( hyp )) {
	  // mark corresponding nodes/arcs as active
	  switch( hyp2type_[hyp] ) {
	  case TERM:
	    // do nothing, because all arcs are inactive by default
	    break;
	  case INIT:
	    // do nothing, because all arcs are inactive by default
	    break;
	  case TRANS:
	    active_arcs.set(trans2arc_[hyp], true);
	    break;
	  case DIV:
	    active_arcs.set(div2arcs_[hyp].first, true);
	    active_arcs.set(div2arcs_[hyp].second, true);
	    break;
	  case FP:
	    active_nodes.set(fp2node_[hyp], false);
	    break;
	  default:
	    throw runtime_error("Kanade::conclude(): unknown hypothesis type");
	    break;
	  }
	}
    }
  }

  void Kanade::reset() {
    if(ilp_ != NULL) {
      delete ilp_;
      ilp_ = NULL;
    }

    tracklet_idx_map_.clear();
    hyp2type_.clear();
    fp2node_.clear();
    trans2arc_.clear();
    div2arcs_.clear();
  }

  Kanade& Kanade::add_hypotheses( const HypothesesGraph& g, const HypothesesGraph::Node& n) {
    double COST = 100.2;
    size_t hyp = 0;

    // init hypothesis
    hyp = ilp_->add_init_hypothesis( tracklet_idx_map_[n], COST );
    hyp2type_[hyp] = INIT;

    // collect and count outgoing arcs
    vector<HypothesesGraph::Arc> arcs;
    size_t count = 0;
    for(HypothesesGraph::OutArcIt a(g, n); a != lemon::INVALID; ++a) {
      arcs.push_back(a);
      ++count;
    }

    if(count == 0) {
      hyp = ilp_->add_term_hypothesis(tracklet_idx_map_[n], COST );
      hyp2type_[hyp] = TERM;
    } else if(count == 1) {
      hyp = ilp_->add_term_hypothesis( tracklet_idx_map_[n], COST );
      hyp2type_[hyp] = TERM;

      hyp = ilp_->add_trans_hypothesis( tracklet_idx_map_[n], tracklet_idx_map_[g.target(arcs[0])], COST);
      hyp2type_[hyp] = TRANS;
      trans2arc_[hyp] = arcs[0];
    } else {
      hyp = ilp_->add_term_hypothesis( tracklet_idx_map_[n], COST );
      hyp2type_[hyp] = TERM;

      // translation hypotheses
      for(size_t i = 0; i < count; ++i) {
	hyp = ilp_->add_trans_hypothesis( tracklet_idx_map_[n], tracklet_idx_map_[g.target(arcs[i])], COST);
	hyp2type_[hyp] = TRANS;
	trans2arc_[hyp] = arcs[i];
      }

      // division hypotheses
      for(size_t i = 0; i < count - 1; ++i) {
	for(size_t j = i; j < count; ++j) {
	  hyp = ilp_->add_div_hypothesis( tracklet_idx_map_[n],
				    tracklet_idx_map_[g.target(arcs[i])],
				    tracklet_idx_map_[g.target(arcs[j])],
				    COST);
	  hyp2type_[hyp] = DIV;
	  div2arcs_[hyp] = pair<HypothesesGraph::Arc, HypothesesGraph::Arc>(arcs[i], arcs[j]);
	}
      }
    }

    return *this;
  }

} /* namespace Tracking */
<|endoftext|>


<commit_before>// Copyright 2015 The Bazel Authors. All rights reserved.
//
// 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.

#if defined(__FreeBSD__)
# define HAVE_EXTATTR
# define HAVE_SYSCTLBYNAME
#elif defined(__OpenBSD__)
// No sys/extattr.h or sysctlbyname on this platform.
#else
# error This BSD is not supported
#endif

#include "src/main/native/unix_jni.h"

#include <assert.h>
#include <errno.h>
#include <fcntl.h>
#include <stdlib.h>
#include <string.h>
#if defined(HAVE_EXTATTR)
# include <sys/extattr.h>
#endif
#include <sys/param.h>
#include <sys/stat.h>
#include <sys/sysctl.h>
#include <sys/types.h>

#include <string>

namespace blaze_jni {

using std::string;

// See unix_jni.h.
string ErrorMessage(int error_number) {
  char buf[1024] = "";
  if (strerror_r(error_number, buf, sizeof buf) < 0) {
    snprintf(buf, sizeof buf, "strerror_r(%d): errno %d", error_number, errno);
  }

  return string(buf);
}

int portable_fstatat(int dirfd, char *name, portable_stat_struct *statbuf,
                     int flags) {
  return fstatat(dirfd, name, statbuf, flags);
}

int StatSeconds(const portable_stat_struct &statbuf, StatTimes t) {
  switch (t) {
    case STAT_ATIME:
      return statbuf.st_atime;
    case STAT_CTIME:
      return statbuf.st_ctime;
    case STAT_MTIME:
      return statbuf.st_mtime;
    default:
      CHECK(false);
  }
}

int StatNanoSeconds(const portable_stat_struct &statbuf, StatTimes t) {
  switch (t) {
    case STAT_ATIME:
      return statbuf.st_atimespec.tv_nsec;
    case STAT_CTIME:
      return statbuf.st_ctimespec.tv_nsec;
    case STAT_MTIME:
      return statbuf.st_mtimespec.tv_nsec;
    default:
      CHECK(false);
  }
}

ssize_t portable_getxattr(const char *path, const char *name, void *value,
                          size_t size, bool *attr_not_found) {
#if (HAVE_EXTATTR)
  ssize_t result =
      extattr_get_file(path, EXTATTR_NAMESPACE_SYSTEM, name, value, size);
  *attr_not_found = (errno == ENOATTR);
  return result;
#else
  *attr_not_found = true;
  return -1;
#endif
}

ssize_t portable_lgetxattr(const char *path, const char *name, void *value,
                           size_t size, bool *attr_not_found) {
#if (HAVE_EXTATTR)
  ssize_t result =
      extattr_get_link(path, EXTATTR_NAMESPACE_SYSTEM, name, value, size);
  *attr_not_found = (errno == ENOATTR);
  return result;
#else
  *attr_not_found = true;
  return -1;
#endif
}

int portable_sysctlbyname(const char *name_chars, long *mibp, size_t *sizep) {
#if (HAVE_SYSCTLBYNAME)
  return sysctlbyname(name_chars, mibp, sizep, NULL, 0);
#else
  errno = ENOSYS;
  return -1;
#endif
}

int portable_push_disable_sleep() {
  // Currently not supported.
  // https://wiki.freebsd.org/SuspendResume
  return -1;
}

int portable_pop_disable_sleep() {
  // Currently not supported.
  // https://wiki.freebsd.org/SuspendResume
  return -1;
}

int portable_suspend_count() {
  // Currently not implemented.
  return 0;
}

int portable_memory_pressure_warning_count() {
  // Currently not implemented.
  return 0;
}

int portable_memory_pressure_critical_count() {
  // Currently not implemented.
  return 0;
}

}  // namespace blaze_jni
<commit_msg>Un-break the build on FreeBSD and OpenBSD.<commit_after>// Copyright 2015 The Bazel Authors. All rights reserved.
//
// 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.

#if defined(__FreeBSD__)
# define HAVE_EXTATTR
# define HAVE_SYSCTLBYNAME
#elif defined(__OpenBSD__)
// No sys/extattr.h or sysctlbyname on this platform.
#else
# error This BSD is not supported
#endif

#include "src/main/native/unix_jni.h"

#include <assert.h>
#include <errno.h>
#include <fcntl.h>
#include <stdlib.h>
#include <string.h>
#if defined(HAVE_EXTATTR)
# include <sys/extattr.h>
#endif
#include <sys/param.h>
#include <sys/stat.h>
#include <sys/sysctl.h>
#include <sys/types.h>

#include <string>

namespace blaze_jni {

using std::string;

// See unix_jni.h.
string ErrorMessage(int error_number) {
  char buf[1024] = "";
  if (strerror_r(error_number, buf, sizeof buf) < 0) {
    snprintf(buf, sizeof buf, "strerror_r(%d): errno %d", error_number, errno);
  }

  return string(buf);
}

int portable_fstatat(int dirfd, char *name, portable_stat_struct *statbuf,
                     int flags) {
  return fstatat(dirfd, name, statbuf, flags);
}

int StatSeconds(const portable_stat_struct &statbuf, StatTimes t) {
  switch (t) {
    case STAT_ATIME:
      return statbuf.st_atime;
    case STAT_CTIME:
      return statbuf.st_ctime;
    case STAT_MTIME:
      return statbuf.st_mtime;
    default:
      CHECK(false);
  }
}

int StatNanoSeconds(const portable_stat_struct &statbuf, StatTimes t) {
  switch (t) {
    case STAT_ATIME:
      return statbuf.st_atimespec.tv_nsec;
    case STAT_CTIME:
      return statbuf.st_ctimespec.tv_nsec;
    case STAT_MTIME:
      return statbuf.st_mtimespec.tv_nsec;
    default:
      CHECK(false);
  }
}

ssize_t portable_getxattr(const char *path, const char *name, void *value,
                          size_t size, bool *attr_not_found) {
#if defined(HAVE_EXTATTR)
  ssize_t result =
      extattr_get_file(path, EXTATTR_NAMESPACE_SYSTEM, name, value, size);
  *attr_not_found = (errno == ENOATTR);
  return result;
#else
  *attr_not_found = true;
  return -1;
#endif
}

ssize_t portable_lgetxattr(const char *path, const char *name, void *value,
                           size_t size, bool *attr_not_found) {
#if defined(HAVE_EXTATTR)
  ssize_t result =
      extattr_get_link(path, EXTATTR_NAMESPACE_SYSTEM, name, value, size);
  *attr_not_found = (errno == ENOATTR);
  return result;
#else
  *attr_not_found = true;
  return -1;
#endif
}

int portable_sysctlbyname(const char *name_chars, long *mibp, size_t *sizep) {
#if defined(HAVE_SYSCTLBYNAME)
  return sysctlbyname(name_chars, mibp, sizep, NULL, 0);
#else
  errno = ENOSYS;
  return -1;
#endif
}

int portable_push_disable_sleep() {
  // Currently not supported.
  // https://wiki.freebsd.org/SuspendResume
  return -1;
}

int portable_pop_disable_sleep() {
  // Currently not supported.
  // https://wiki.freebsd.org/SuspendResume
  return -1;
}

int portable_suspend_count() {
  // Currently not implemented.
  return 0;
}

int portable_memory_pressure_warning_count() {
  // Currently not implemented.
  return 0;
}

int portable_memory_pressure_critical_count() {
  // Currently not implemented.
  return 0;
}

}  // namespace blaze_jni
<|endoftext|>


<commit_before><commit_msg>Minor formatting edit.<commit_after><|endoftext|>


<commit_before><commit_msg>client: fix logger deregistration<commit_after><|endoftext|>


<commit_before><commit_msg>Using alias instead of typedef<commit_after><|endoftext|>


<commit_before><commit_msg>fix lazy_entry stream out operator to not leave the stream in hexadecimal mode<commit_after><|endoftext|>


<commit_before><commit_msg>Set new node id in notify() to enable caching to work properly. Fix by pederb.<commit_after><|endoftext|>


<commit_before><commit_msg>added header inclusion for exit() in Malloc.hh<commit_after><|endoftext|>


<commit_before>
#ifndef __COMMON_HPP__
#define __COMMON_HPP__

/// Disable copy constructor and assignment operator.
/// Put this in your class' private declarations.
/// (from google public C++ coding standards)
#define DISALLOW_COPY_AND_ASSIGN( Name )	\
  Name( const Name & );				\
  void operator=( const Name & )

/// Sign extension.
/// From Stanford bit-twiddling hacks.
template <typename T, unsigned B>
inline T signextend(const T x)
{
  struct {T x:B;} s;
  return s.x = x;
}

/// Base 2 log of 32-bit number.
/// Modified from Stanford bit twiddling hacks.
inline unsigned int log2( unsigned int v ) {
  register unsigned int r; // result of log2(v) will go here
  register unsigned int shift;

  r =     (v > 0xFFFF) << 4; v >>= r;
  shift = (v > 0xFF  ) << 3; v >>= shift; r |= shift;
  shift = (v > 0xF   ) << 2; v >>= shift; r |= shift;
  shift = (v > 0x3   ) << 1; v >>= shift; r |= shift;
  r |= (v >> 1);

  return r;
}

/// Read 64-bit timestamp counter.
#define rdtscll(val) do {					\
    unsigned int __a,__d;					\
    asm volatile("rdtsc" : "=a" (__a), "=d" (__d));		\
    (val) = ((unsigned long)__a) | (((unsigned long)__d)<<32);	\
  } while(0)

#endif
<commit_msg>Add magic identity function for resolving template function pointers in template functions<commit_after>
#ifndef __COMMON_HPP__
#define __COMMON_HPP__

/// Disable copy constructor and assignment operator.
/// Put this in your class' private declarations.
/// (from google public C++ coding standards)
#define DISALLOW_COPY_AND_ASSIGN( Name )	\
  Name( const Name & );				\
  void operator=( const Name & )

/// Sign extension.
/// From Stanford bit-twiddling hacks.
template <typename T, unsigned B>
inline T signextend(const T x)
{
  struct {T x:B;} s;
  return s.x = x;
}

/// Base 2 log of 32-bit number.
/// Modified from Stanford bit twiddling hacks.
inline unsigned int log2( unsigned int v ) {
  register unsigned int r; // result of log2(v) will go here
  register unsigned int shift;

  r =     (v > 0xFFFF) << 4; v >>= r;
  shift = (v > 0xFF  ) << 3; v >>= shift; r |= shift;
  shift = (v > 0xF   ) << 2; v >>= shift; r |= shift;
  shift = (v > 0x3   ) << 1; v >>= shift; r |= shift;
  r |= (v >> 1);

  return r;
}

/// Read 64-bit timestamp counter.
#define rdtscll(val) do {					\
    unsigned int __a,__d;					\
    asm volatile("rdtsc" : "=a" (__a), "=d" (__d));		\
    (val) = ((unsigned long)__a) | (((unsigned long)__d)<<32);	\
  } while(0)

/// OMGWTFBBQ SoftXMT magic identity function
/// Use this to get a pointer to a template function inside a template function, etc.
template< typename T >
T * SoftXMT_magic_identity_function(T * t) {
  return t;
}

#endif
<|endoftext|>


<commit_before>/** \copyright
 * Copyright (c) 2013, Stuart W Baker
 * All rights reserved.
 *
 * 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.
 *
 * \file NMRAnetVelocity.hxx
 * This file provides an implementation of velocity in NMRAnet terms.
 *
 * @author Stuart W. Baker
 * @date 2 August 2013
 */

#ifndef _NMRAnetVelocity_hxx_
#define _NMRAnetVelocity_hxx_

#include <cmath>
#include <cstdint>

#include "utils/macros.h"

extern "C" {
/* These come from the ieeehalfprecision.c */
int singles2halfp(void *target, void *source, int numel);
int halfp2singles(void *target, void *source, int numel);
}

/** Conversion factor for MPH. 1 mph = this many m/s. */
#define MPH_FACTOR 0.44704f

/** This type represents how velocity is seen on the wire (16 bit float).
 */
typedef uint16_t float16_t;

namespace NMRAnet
{

/** This class provides a mechanism for working with velocity in different
 *  forms.  A single precision floating point value is used internally to store
 *  velocity, but this class provides the ability to easily work with
 *  different velocity formats including DCC 14/28/128 speed step formats.
 *  NMRAnet velocity is represented as a floating point meters/sec.  The sign
 *  represents direction where negative is reverse and positive is forward.
 *  Sign is always preserved even when the result is 0.  For example,
 *  -7.0 + 7.0 = -0.0 and 7.0 - 7.0 = +0.0  Be careful thought, per the C
 *  standards, a negative zero (-.0.0) velocity compared to constant 0.0 will
 *  compare true.  Always use @ref direction() when trying to determine the
 *  sign or direction of travel.
 */
class Velocity
{
public:
    /** define an enumeration for direction
     */
    enum
    {
        FORWARD  = 0, /**< forward direction */
        POSITIVE = 0, /**< forward direction */
        REVERSE  = 1, /**< reverse direction */
        NEGATIVE = 1, /**< reverse direction */
    };

    /** Default constructor.
     */
    Velocity()
        : velocity(0)
    {
    }

    /** Basic constructor.
     * @param value starting value for Velocity.
     */
    Velocity(int value)
        : velocity(value)
    {
    }

    /** Basic constructor.
     * @param value starting value for Velocity.
     */
    Velocity(unsigned int value)
        : velocity(value)
    {
    }

    /** Basic constructor.
     * @param value starting value for Velocity.
     */
    Velocity(float value)
        : velocity(value)
    {
    }

    /** Basic constructor.
     * @param value starting value for Velocity.
     */
    Velocity(double value)
        : velocity(value)
    {
    }

    /** Constructor that takes the 16 bit wire format
     * @param value starting value for Velocity as IEEE half precision float.
     */
    Velocity(float16_t value)
        : velocity(halfp2singles(&velocity, &value, 1))
    {
    }

    /** Copy constructor. */
    Velocity(const Velocity& old_velocity)
        : velocity(old_velocity.velocity)
    {
    }

    /** Destructor does nothing. */
    ~Velocity() {}

    /** Return the speed independent of direction.
     * @return speed absolute value of velocity
     */
    float speed()
    {
        return fabsf(velocity);
    }

    /** Return the direction independent of speed.
     * @return direction FORWARD or REVERSE
     */
    int direction()
    {
        if (std::signbit(velocity))
        {
            return REVERSE;
        }
        return FORWARD;
    }

    void set_direction(int direction)
    {
        switch (direction)
        {
        case FORWARD:
            forward();
            break;
        case REVERSE:
            reverse();
            break;
        default:
            DIE("Unexpected direction value");
        }
    }

    /** Set the direction to forward. */
    void forward()
    {
        if (std::signbit(velocity))
        {
            velocity = -velocity;
        }
    }

    /** Set the direction to reverse. */
    void reverse()
    {
        if (!std::signbit(velocity))
        {
            velocity = -velocity;
        }
    }

    /** Convert the native meters/sec representation into mile per hour.
     * @return velocity represented as miles per hour
     */
    float mph()
    {
        return zero_adjust(velocity / MPH_FACTOR, velocity);
    }

    /** Sets the speed value from a given mph value. The sign of the mph value
     * is ignored. */
    void set_mph(float mph)
    {
        velocity = std::copysign(mph * MPH_FACTOR, velocity);
    }

    /** Get the speed in DCC 128 speed step format.
     *  The mapping from meters/sec is strait forward.  First convert to
     *  miles/hour, then each speed step represents 1 mile/hour.  Saturate at
     *  126 miles/hour.
     *
     *  bit 7:  direction
     *  bits 6..0:  0 = stopped, 1 = estop, 2 - 127 = speed steps 1 - 126
     *  @return DCC encoded speed steps
     */
    uint8_t get_dcc_128();

    /** Set the speed from DCC 128 speed step format.
     *  The mapping from meters/sec is strait forward.  First convert to
     *  miles/hour, then each speed step represents 1 mile/hour.  Saturate at
     *  126 miles/hour.
     *
     *  @param value bit 7:  direction
     *               bits 6..0:  0 = stopped, 1 = estop, 2 - 127 = speed steps 1 - 126
     */
    void set_dcc_128(uint8_t value);

    /** Get the speed in DCC 28 speed step format.
     *  This is a decimation of the 128 speed step mode.
     *
     *  bit 7..6:  fixed at b'01'
     *  bit 5:  direction
     *  bits 4:  speed step least significant bit
     *  bits 3..0:  speed step significatn bits 4..1
     *  @return DCC encoded speed steps
     */
    uint8_t get_dcc_28();

    /** Set the speed from DCC 28 speed step format.
     *  This is a decimation of the 128 speed step mode.
     *
     *  @param value bit 7..6:  fixed at b'01'
     *               bit 5:  direction
     *               bits 4:  speed step least significant bit
     *               bits 3..0:  speed step significatn bits 4..1
     */
    void set_dcc_28(uint8_t value);

    /** Get the speed in DCC 14 speed step format.
     *  This is a decimation of the 128 speed step mode.
     *
     *  bit 7..6:  fixed at b'01'
     *  bit 5:  direction
     *  bits 4:  reserved 0 for headlight
     *  bits 3..0:  0 = stopped, 4 - 31 = speed steps 1 - 28
     *  @return DCC encoded speed steps
     */
    uint8_t get_dcc_14();

    /** Set the speed from DCC 14 speed step format.
     *  This is a decimation of the 128 speed step mode.
     *
     *  @param value bit 7..6:  fixed at b'01'
     *               bit 5:  direction
     *               bits 4:  reserved 0 for headlight
     *               bits 3..0:  0 = stopped, 4 - 31 = speed steps 1 - 28
     */
    void set_dcc_14(uint8_t value);

    /** Get a wire version of the velocity.
     * @return IEEE half precision floating point representation of velocity
     */
    float16_t get_wire()
    {
        float16_t result;
        singles2halfp(&result, &velocity, 1);
        return result;
    }

    /** Set the value based on the wire version of velocity.
     * @param value IEEE half precision floating point representation of velocity
     */
    void set_wire(float16_t value)
    {
        halfp2singles(&velocity, &value, 1);
    }

    /** Overloaded addition operator. */
    Velocity operator + (const Velocity& v)
    {
        return Velocity(zero_adjust(velocity + v.velocity,velocity));
    }

    /** Overloaded addition operator. */
    Velocity operator + (const float& v)
    {
        return Velocity(zero_adjust(velocity + v,velocity));
    }

    /** Overloaded subtraction operator. */
    Velocity operator - (const Velocity& v)
    {
        return Velocity(zero_adjust(velocity - v.velocity,velocity));
    }

    /** Overloaded subtraction operator. */
    Velocity operator - (const float& v)
    {
        return Velocity(zero_adjust(velocity - v,velocity));
    }

    /** Overloaded multiplication operator. */
    Velocity operator * (const Velocity& v)
    {
        return Velocity(zero_adjust(velocity * v.velocity,velocity));
    }

    /** Overloaded multiplication operator. */
    Velocity operator * (const float& v)
    {
        return Velocity(zero_adjust(velocity * v,velocity));
    }

    /** Overloaded division operator. */
    Velocity operator / (const Velocity& v)
    {
        return Velocity(zero_adjust(velocity / v.velocity,velocity));
    }

    /** Overloaded division operator. */
    Velocity operator / (const float& v)
    {
        return Velocity(zero_adjust(velocity / v,velocity));
    }

    /** Overloaded pre-increement operator. */
    Velocity& operator ++ ()
    {
        velocity = zero_adjust(velocity + 1,velocity);
        return *this;
    }

    /** Overloaded post-increment operator. */
    Velocity operator ++ (int)
    {
        Velocity result(*this);
        velocity = zero_adjust(velocity + 1,velocity);
        return result;
    }

    /** Overloaded pre-decreement operator. */
    Velocity& operator -- ()
    {
        velocity = zero_adjust(velocity - 1,velocity);
        return *this;
    }

    /** Overloaded post-decrement operator. */
    Velocity operator -- (int)
    {
        Velocity result(*this);
        velocity = zero_adjust(velocity - 1,velocity);
        return result;
    }

    /** Overloaded addition equals operator. */
    Velocity& operator += (const Velocity& v)
    {
        velocity = zero_adjust(velocity + v.velocity,velocity);
        return *this;
    }

    /** Overloaded addition equals operator. */
    Velocity& operator += (const float& v)
    {
        velocity = zero_adjust(velocity + v,velocity);
        return *this;
    }

    /** Overloaded subtraction equals operator. */
    Velocity& operator -= (const Velocity& v)
    {
        velocity = zero_adjust(velocity - v.velocity,velocity);
        return *this;
    }

    /** Overloaded subtraction equals operator. */
    Velocity& operator -= (const float& v)
    {
        velocity = zero_adjust(velocity - v,velocity);
        return *this;
    }

    /** Overloaded multiplication equals operator. */
    Velocity& operator *= (const Velocity& v)
    {
        velocity = zero_adjust(velocity * v.velocity,velocity);
        return *this;
    }

    /** Overloaded multiplication equals operator. */
    Velocity& operator *= (const float& v)
    {
        velocity = zero_adjust(velocity * v,velocity);
        return *this;
    }

    /** Overloaded division equals operator. */
    Velocity& operator /= (const Velocity& v)
    {
        velocity = zero_adjust(velocity / v.velocity,velocity);
        return *this;
    }

    /** Overloaded division equals operator. */
    Velocity& operator /= (const float& v)
    {
        velocity = zero_adjust(velocity / v,velocity);
        return *this;
    }

    /** Overloaded equals operator. */
    Velocity& operator = (const Velocity& v)
    {
        velocity = v.velocity;
        return *this;
    }

    /** Overloaded equals operator. */
    Velocity& operator = (const float& v)
    {
        velocity = v;
        return *this;
    }

    /** Overloaded equals equals operator */
    bool operator == (const Velocity& v)
    {
        return (v.velocity == velocity);
    }

    /** Overloaded equals equals operator */
    bool operator == (const float& v)
    {
        return (v == velocity);
    }

    /** Overloaded not equals operator */
    bool operator != (const Velocity& v)
    {
        return (v.velocity != velocity);
    }

    /** Overloaded not equals operator */
    bool operator != (const float& v)
    {
        return (v != velocity);
    }

private:
    /** Floating point representation of velocity. */
    float velocity;

    /** Adjust for a math result of negative 0.
     * @param value value of math result
     * @param old original value before expression
     */
    float zero_adjust(float value, float old)
    {
        if (value == 0)
        {
            return copysign(value, old);
        }
        return value;
    }
};

}; /* namespace NMRAnet */

#endif /* _NMRAnetVelocity_hxx_ */
<commit_msg>makes mph() return a non-negative value, similar to speed(). Users can always use the direction() function to get the actual direction.<commit_after>/** \copyright
 * Copyright (c) 2013, Stuart W Baker
 * All rights reserved.
 *
 * 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.
 *
 * \file NMRAnetVelocity.hxx
 * This file provides an implementation of velocity in NMRAnet terms.
 *
 * @author Stuart W. Baker
 * @date 2 August 2013
 */

#ifndef _NMRAnetVelocity_hxx_
#define _NMRAnetVelocity_hxx_

#include <cmath>
#include <cstdint>

#include "utils/macros.h"

extern "C" {
/* These come from the ieeehalfprecision.c */
int singles2halfp(void *target, void *source, int numel);
int halfp2singles(void *target, void *source, int numel);
}

/** Conversion factor for MPH. 1 mph = this many m/s. */
#define MPH_FACTOR 0.44704f

/** This type represents how velocity is seen on the wire (16 bit float).
 */
typedef uint16_t float16_t;

namespace NMRAnet
{

/** This class provides a mechanism for working with velocity in different
 *  forms.  A single precision floating point value is used internally to store
 *  velocity, but this class provides the ability to easily work with
 *  different velocity formats including DCC 14/28/128 speed step formats.
 *  NMRAnet velocity is represented as a floating point meters/sec.  The sign
 *  represents direction where negative is reverse and positive is forward.
 *  Sign is always preserved even when the result is 0.  For example,
 *  -7.0 + 7.0 = -0.0 and 7.0 - 7.0 = +0.0  Be careful thought, per the C
 *  standards, a negative zero (-.0.0) velocity compared to constant 0.0 will
 *  compare true.  Always use @ref direction() when trying to determine the
 *  sign or direction of travel.
 */
class Velocity
{
public:
    /** define an enumeration for direction
     */
    enum
    {
        FORWARD  = 0, /**< forward direction */
        POSITIVE = 0, /**< forward direction */
        REVERSE  = 1, /**< reverse direction */
        NEGATIVE = 1, /**< reverse direction */
    };

    /** Default constructor.
     */
    Velocity()
        : velocity(0)
    {
    }

    /** Basic constructor.
     * @param value starting value for Velocity.
     */
    Velocity(int value)
        : velocity(value)
    {
    }

    /** Basic constructor.
     * @param value starting value for Velocity.
     */
    Velocity(unsigned int value)
        : velocity(value)
    {
    }

    /** Basic constructor.
     * @param value starting value for Velocity.
     */
    Velocity(float value)
        : velocity(value)
    {
    }

    /** Basic constructor.
     * @param value starting value for Velocity.
     */
    Velocity(double value)
        : velocity(value)
    {
    }

    /** Constructor that takes the 16 bit wire format
     * @param value starting value for Velocity as IEEE half precision float.
     */
    Velocity(float16_t value)
        : velocity(halfp2singles(&velocity, &value, 1))
    {
    }

    /** Copy constructor. */
    Velocity(const Velocity& old_velocity)
        : velocity(old_velocity.velocity)
    {
    }

    /** Destructor does nothing. */
    ~Velocity() {}

    /** Return the speed independent of direction.
     * @return speed absolute value of velocity
     */
    float speed()
    {
        return fabsf(velocity);
    }

    /** Return the direction independent of speed.
     * @return direction FORWARD or REVERSE
     */
    int direction()
    {
        if (std::signbit(velocity))
        {
            return REVERSE;
        }
        return FORWARD;
    }

    void set_direction(int direction)
    {
        switch (direction)
        {
        case FORWARD:
            forward();
            break;
        case REVERSE:
            reverse();
            break;
        default:
            DIE("Unexpected direction value");
        }
    }

    /** Set the direction to forward. */
    void forward()
    {
        if (std::signbit(velocity))
        {
            velocity = -velocity;
        }
    }

    /** Set the direction to reverse. */
    void reverse()
    {
        if (!std::signbit(velocity))
        {
            velocity = -velocity;
        }
    }

    /** Convert the native meters/sec representation into mile per hour.
     * @return velocity represented as miles per hour. Always non-negative.
     */
    float mph()
    {
        return speed() / MPH_FACTOR;
    }

    /** Sets the speed value from a given mph value. The sign of the mph value
     * is ignored. */
    void set_mph(float mph)
    {
        velocity = std::copysign(mph * MPH_FACTOR, velocity);
    }

    /** Get the speed in DCC 128 speed step format.
     *  The mapping from meters/sec is strait forward.  First convert to
     *  miles/hour, then each speed step represents 1 mile/hour.  Saturate at
     *  126 miles/hour.
     *
     *  bit 7:  direction
     *  bits 6..0:  0 = stopped, 1 = estop, 2 - 127 = speed steps 1 - 126
     *  @return DCC encoded speed steps
     */
    uint8_t get_dcc_128();

    /** Set the speed from DCC 128 speed step format.
     *  The mapping from meters/sec is strait forward.  First convert to
     *  miles/hour, then each speed step represents 1 mile/hour.  Saturate at
     *  126 miles/hour.
     *
     *  @param value bit 7:  direction
     *               bits 6..0:  0 = stopped, 1 = estop, 2 - 127 = speed steps 1 - 126
     */
    void set_dcc_128(uint8_t value);

    /** Get the speed in DCC 28 speed step format.
     *  This is a decimation of the 128 speed step mode.
     *
     *  bit 7..6:  fixed at b'01'
     *  bit 5:  direction
     *  bits 4:  speed step least significant bit
     *  bits 3..0:  speed step significatn bits 4..1
     *  @return DCC encoded speed steps
     */
    uint8_t get_dcc_28();

    /** Set the speed from DCC 28 speed step format.
     *  This is a decimation of the 128 speed step mode.
     *
     *  @param value bit 7..6:  fixed at b'01'
     *               bit 5:  direction
     *               bits 4:  speed step least significant bit
     *               bits 3..0:  speed step significatn bits 4..1
     */
    void set_dcc_28(uint8_t value);

    /** Get the speed in DCC 14 speed step format.
     *  This is a decimation of the 128 speed step mode.
     *
     *  bit 7..6:  fixed at b'01'
     *  bit 5:  direction
     *  bits 4:  reserved 0 for headlight
     *  bits 3..0:  0 = stopped, 4 - 31 = speed steps 1 - 28
     *  @return DCC encoded speed steps
     */
    uint8_t get_dcc_14();

    /** Set the speed from DCC 14 speed step format.
     *  This is a decimation of the 128 speed step mode.
     *
     *  @param value bit 7..6:  fixed at b'01'
     *               bit 5:  direction
     *               bits 4:  reserved 0 for headlight
     *               bits 3..0:  0 = stopped, 4 - 31 = speed steps 1 - 28
     */
    void set_dcc_14(uint8_t value);

    /** Get a wire version of the velocity.
     * @return IEEE half precision floating point representation of velocity
     */
    float16_t get_wire()
    {
        float16_t result;
        singles2halfp(&result, &velocity, 1);
        return result;
    }

    /** Set the value based on the wire version of velocity.
     * @param value IEEE half precision floating point representation of velocity
     */
    void set_wire(float16_t value)
    {
        halfp2singles(&velocity, &value, 1);
    }

    /** Overloaded addition operator. */
    Velocity operator + (const Velocity& v)
    {
        return Velocity(zero_adjust(velocity + v.velocity,velocity));
    }

    /** Overloaded addition operator. */
    Velocity operator + (const float& v)
    {
        return Velocity(zero_adjust(velocity + v,velocity));
    }

    /** Overloaded subtraction operator. */
    Velocity operator - (const Velocity& v)
    {
        return Velocity(zero_adjust(velocity - v.velocity,velocity));
    }

    /** Overloaded subtraction operator. */
    Velocity operator - (const float& v)
    {
        return Velocity(zero_adjust(velocity - v,velocity));
    }

    /** Overloaded multiplication operator. */
    Velocity operator * (const Velocity& v)
    {
        return Velocity(zero_adjust(velocity * v.velocity,velocity));
    }

    /** Overloaded multiplication operator. */
    Velocity operator * (const float& v)
    {
        return Velocity(zero_adjust(velocity * v,velocity));
    }

    /** Overloaded division operator. */
    Velocity operator / (const Velocity& v)
    {
        return Velocity(zero_adjust(velocity / v.velocity,velocity));
    }

    /** Overloaded division operator. */
    Velocity operator / (const float& v)
    {
        return Velocity(zero_adjust(velocity / v,velocity));
    }

    /** Overloaded pre-increement operator. */
    Velocity& operator ++ ()
    {
        velocity = zero_adjust(velocity + 1,velocity);
        return *this;
    }

    /** Overloaded post-increment operator. */
    Velocity operator ++ (int)
    {
        Velocity result(*this);
        velocity = zero_adjust(velocity + 1,velocity);
        return result;
    }

    /** Overloaded pre-decreement operator. */
    Velocity& operator -- ()
    {
        velocity = zero_adjust(velocity - 1,velocity);
        return *this;
    }

    /** Overloaded post-decrement operator. */
    Velocity operator -- (int)
    {
        Velocity result(*this);
        velocity = zero_adjust(velocity - 1,velocity);
        return result;
    }

    /** Overloaded addition equals operator. */
    Velocity& operator += (const Velocity& v)
    {
        velocity = zero_adjust(velocity + v.velocity,velocity);
        return *this;
    }

    /** Overloaded addition equals operator. */
    Velocity& operator += (const float& v)
    {
        velocity = zero_adjust(velocity + v,velocity);
        return *this;
    }

    /** Overloaded subtraction equals operator. */
    Velocity& operator -= (const Velocity& v)
    {
        velocity = zero_adjust(velocity - v.velocity,velocity);
        return *this;
    }

    /** Overloaded subtraction equals operator. */
    Velocity& operator -= (const float& v)
    {
        velocity = zero_adjust(velocity - v,velocity);
        return *this;
    }

    /** Overloaded multiplication equals operator. */
    Velocity& operator *= (const Velocity& v)
    {
        velocity = zero_adjust(velocity * v.velocity,velocity);
        return *this;
    }

    /** Overloaded multiplication equals operator. */
    Velocity& operator *= (const float& v)
    {
        velocity = zero_adjust(velocity * v,velocity);
        return *this;
    }

    /** Overloaded division equals operator. */
    Velocity& operator /= (const Velocity& v)
    {
        velocity = zero_adjust(velocity / v.velocity,velocity);
        return *this;
    }

    /** Overloaded division equals operator. */
    Velocity& operator /= (const float& v)
    {
        velocity = zero_adjust(velocity / v,velocity);
        return *this;
    }

    /** Overloaded equals operator. */
    Velocity& operator = (const Velocity& v)
    {
        velocity = v.velocity;
        return *this;
    }

    /** Overloaded equals operator. */
    Velocity& operator = (const float& v)
    {
        velocity = v;
        return *this;
    }

    /** Overloaded equals equals operator */
    bool operator == (const Velocity& v)
    {
        return (v.velocity == velocity);
    }

    /** Overloaded equals equals operator */
    bool operator == (const float& v)
    {
        return (v == velocity);
    }

    /** Overloaded not equals operator */
    bool operator != (const Velocity& v)
    {
        return (v.velocity != velocity);
    }

    /** Overloaded not equals operator */
    bool operator != (const float& v)
    {
        return (v != velocity);
    }

private:
    /** Floating point representation of velocity. */
    float velocity;

    /** Adjust for a math result of negative 0.
     * @param value value of math result
     * @param old original value before expression
     */
    float zero_adjust(float value, float old)
    {
        if (value == 0)
        {
            return copysign(value, old);
        }
        return value;
    }
};

}; /* namespace NMRAnet */

#endif /* _NMRAnetVelocity_hxx_ */
<|endoftext|>


<commit_before>//
//  server.cpp
//  rdt
//
//  Created by Chris Orcutt on 11/12/15.
//  Copyright © 2015 Chris Orcutt. All rights reserved.
//

#include "Error.hpp"
#include "GBNServerProtocol.hpp"
#include "Header.hpp"
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <pthread.h>

using namespace std;

void * receiveAck(void *);
void sendValidPackets(GBNServerProtocol *);
void resendValidPackets(GBNServerProtocol *);
void sendSynack(GBNServerProtocol *);

int main(int argc, const char ** argv){

// !! Begin Command Line Argument Parsing !!

  if (argc > 10){
    Error::usage();
  }

  int port = 45000;
  bool verbose = false;
  bool printSent = false;
  bool printReceived = false;
  int windowSize = 10;
  int timeoutInterval = 2;

  for (int index = 0; index < argc; index++){

    //indicate port number
    if (strcmp(argv[index], "--port") == 0 || strcmp(argv[index], "-p") == 0){
      if (index + 1 == argc) {
        Error::usage();
      }
      port = atoi(argv[index + 1]);
    }

    //indicate window size
    if (strcmp(argv[index], "--window") == 0 || strcmp(argv[index], "-w") == 0){
      if (index + 1 == argc) {
        Error::usage();
      }
      windowSize = atoi(argv[index + 1]);
    }

    //indicate timeout interval
    if (strcmp(argv[index], "--timeout") == 0 || strcmp(argv[index], "-t") == 0){
      if (index + 1 == argc) {
        Error::usage();
      }
      timeoutInterval = atoi(argv[index + 1]);
    }

    //print all sent data
    if (strcmp(argv[index], "--print-sent") == 0 || strcmp(argv[index], "-ps") == 0){
      printSent = true;
    }

    //print all received data
    if (strcmp(argv[index], "--print-recv") == 0 || strcmp(argv[index], "-pr") == 0){
      printReceived = true;
    }

    //print all data
    if (strcmp(argv[index], "--print-all") == 0 || strcmp(argv[index], "-pa") == 0){
      printReceived = true;
      printSent = true;
    }

    //print server protocol
    if (strcmp(argv[index], "--verbose") == 0 || strcmp(argv[index], "-v") == 0){
      verbose = true;
    }
  }

// !! Begin GBN Server Initialization  !!

  GBNServerProtocol server = GBNServerProtocol(windowSize, timeoutInterval, port);
  server.communicator.printReceieved = printReceived;
  server.communicator.printSent = printSent;
  server.verbose = verbose;

// !! Begin Handshake !!

  if (server.verbose){
    cout << "Waiting for connection on port " << server.communicator.socket.port << "." << endl;
  }

  //waiting for client to request connection
  while (!server.receivedSyn());
  if (server.verbose){
    cout << "Received: Syn." << endl;
    cout << "Sent: Synack." << endl << endl;
  }
  sendSynack(&server);

  //wait for synack ack
  pthread_t receiveAckThread;
  if (pthread_create(&receiveAckThread, NULL, receiveAck, &server)) {
    cout << "Error creating ack thread." << endl;
    exit(1);
  }

  //resend synack at timeout
  while (server.timeoutTimer.valid){
    if (server.timeoutTimer.elapsedTime() >= server.timeoutInterval){
      if (server.verbose){
        cout << "Timed out!" << endl;
        cout << "Sent: Synack." << endl << endl;
      }
      sendSynack(&server);
    }
  }

  //join thread when ack received
  if (pthread_join(receiveAckThread, NULL)) {
    cout << "Error joining ack thread" << endl;
    exit(1);
  }

// !! Begin Data Transmission !!

  //send data
  while (server.keepAlive && server.currentWindowBase < server.totalChunks ){
    if (server.verbose){
      cout << "Current window base: " << server.currentWindowBase << ", Total packets: " << server.totalChunks << endl << endl;
    }
    server.timeoutTimer.valid = true;
    sendValidPackets(&server);

    //wait for ack
    if (pthread_create(&receiveAckThread, NULL, receiveAck, &server)) {
      cout << "Error creating ack thread" << endl;
      exit(1);
    }

    //resend data at timeout
    while (server.keepAlive && server.timeoutTimer.valid){
      if (server.timeoutTimer.elapsedTime() >= server.timeoutInterval){
        if (server.verbose){
          cout << "Timed out!" << endl << "Resending packets " << server.currentWindowBase << " to " << server.currentWindowBase + server.windowSize - 1 << endl;
        }
        resendValidPackets(&server);
      }
    }

    if (!server.keepAlive){
      return 0;
    }

    //join thread when ack received
    if (pthread_join(receiveAckThread, NULL)) {
      cout << "Error joining receive thread" << endl;
      exit(1);
    }
  }

  return 0;
}

//receives ack in new thread
void * receiveAck(void * aserver){
  GBNServerProtocol * server = (GBNServerProtocol*)aserver;
  while(!server->receivedAck());
  if (server->verbose){
    cout << "Received: Ack " << server->receivedAckNum <<  endl << endl;
  }
  server->currentWindowBase = server->receivedAckNum;
  server->timeoutTimer.stop();
  server->timeoutTimer.valid = false;
  return NULL;
}

//sends all packets in current window
void resendValidPackets(GBNServerProtocol * server){
  for (int packetNum = server->currentWindowBase; packetNum < server->currentWindowBase + server->windowSize; packetNum++){
    if (packetNum < server->totalChunks){
      if (server->verbose){
        cout << "Sent: Packet number " << packetNum << endl;
      }
      server->timeoutTimer.start();
      server->sendData(packetNum);
    }
  }
  if (server->verbose){
    cout << endl;
  }
}

//TODO: refactorable with resendValidPackets!
void sendValidPackets(GBNServerProtocol * server){
  for (int packetNum = server->currentWindowBase; packetNum < server->currentWindowBase + server->windowSize; packetNum++){
    if (packetNum < server->totalChunks && server->packetState[packetNum] == Unsent){
      if (server->verbose){
        cout << "Sent: Packet number " << packetNum << endl;
      }
      server->packetState[packetNum] = Sent;
      server->timeoutTimer.start();
      server->sendData(packetNum);
    }
  }
  if (server->verbose){
    cout << endl;
  }
}

//sends synack
void sendSynack(GBNServerProtocol * server){
  server->timeoutTimer.start();
  server->sendSynack();
}

<commit_msg>Update server to recovery from race condition<commit_after>//
//  server.cpp
//  rdt
//
//  Created by Chris Orcutt on 11/12/15.
//  Copyright © 2015 Chris Orcutt. All rights reserved.
//

#include "Error.hpp"
#include "GBNServerProtocol.hpp"
#include "Header.hpp"
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <pthread.h>

using namespace std;

void * receiveAck(void *);
void sendValidPackets(GBNServerProtocol *);
void resendValidPackets(GBNServerProtocol *);
void sendSynack(GBNServerProtocol *);

int main(int argc, const char ** argv){

// !! Begin Command Line Argument Parsing !!

  if (argc > 9){
    Error::usage();
  }

  int port = 45000;
  bool verbose = false;
  bool printSent = false;
  bool printReceived = false;
  int windowSize = 10;
  int timeoutInterval = 2;

  for (int index = 1; index < argc; index++){

    //indicate port number
    if (strcmp(argv[index], "--port") == 0 || strcmp(argv[index], "-p") == 0){
      if (index + 1 == argc) {
        Error::usage();
      }
      port = atoi(argv[index + 1]);
      index++;
    }

    //indicate window size
    else if (strcmp(argv[index], "--window") == 0 || strcmp(argv[index], "-w") == 0){
      if (index + 1 == argc) {
        Error::usage();
      }
      windowSize = atoi(argv[index + 1]);
      index++;
    }

    //indicate timeout interval
    else if (strcmp(argv[index], "--timeout") == 0 || strcmp(argv[index], "-t") == 0){
      if (index + 1 == argc) {
        Error::usage();
      }
      timeoutInterval = atoi(argv[index + 1]);
      index++;
    }

    //print all sent data
    else if (strcmp(argv[index], "--print-sent") == 0 || strcmp(argv[index], "-ps") == 0){
      printSent = true;
    }

    //print all received data
    else if (strcmp(argv[index], "--print-recv") == 0 || strcmp(argv[index], "-pr") == 0){
      printReceived = true;
    }

    //print all data
    else if (strcmp(argv[index], "--print-all") == 0 || strcmp(argv[index], "-pa") == 0){
      printReceived = true;
      printSent = true;
    }

    //print server protocol
    else if (strcmp(argv[index], "--verbose") == 0 || strcmp(argv[index], "-v") == 0){
      verbose = true;
    }else {
      Error::usage();
      Error::exit(-1);
    }
  }

// !! Begin GBN Server Initialization  !!

  GBNServerProtocol server = GBNServerProtocol(windowSize, timeoutInterval, port);
  server.communicator.printReceieved = printReceived;
  server.communicator.printSent = printSent;
  server.verbose = verbose;

// !! Begin Handshake !!

  if (server.verbose){
    cout << "Waiting for connection on port " << server.communicator.socket.port << "." << endl;
  }

  //waiting for client to request connection
  while (!server.receivedSyn());
  if (server.verbose){
    cout << "Received: Syn." << endl;
    cout << "Sent: Synack." << endl << endl;
  }
  sendSynack(&server);

  //wait for synack ack
  pthread_t receiveSynackThread;
  if (pthread_create(&receiveSynackThread, NULL, receiveAck, &server)) {
    cout << "Error creating ack thread." << endl;
    exit(1);
  }

  //resend synack at timeout
  while (server.timeoutTimer.valid){
    if (server.timeoutTimer.elapsedTime() >= server.timeoutInterval){
      if (server.verbose){
        cout << "Timed out!" << endl;
        cout << "Sent: Synack." << endl << endl;
      }
      sendSynack(&server);
    }
  }

  //join thread when ack received
  if (pthread_join(receiveSynackThread, NULL)) {
    cout << "Error joining ack thread" << endl;
    exit(1);
  }

// !! Begin Data Transmission !!

  //send data
  while (server.keepAlive && (server.currentWindowBase < server.totalChunks) ){
    if (server.verbose){
      cout << "Current window base: " << server.currentWindowBase << ", Total packets: " << server.totalChunks << endl;
    }
    server.timeoutTimer.valid = true;
    sendValidPackets(&server);

    //wait for ack
    pthread_t receiveAckThread;
    if (pthread_create(&receiveAckThread, NULL, receiveAck, &server)) {
      cout << "Error creating ack thread" << endl;
      exit(1);
    }

    //resend data at timeout
    while (server.keepAlive && server.timeoutTimer.valid){
      if (!server.timeoutTimer.timing){
        server.timeoutTimer.start();
      }
      if (server.timeoutTimer.elapsedTime() >= server.timeoutInterval){
        if (server.verbose){
          cout << "Timed out!" << endl << "Resending packets " << server.currentWindowBase << " to " << server.currentWindowBase + server.windowSize - 1 << endl;
        }
        resendValidPackets(&server);
      }
    }

    if (!server.keepAlive){
      return 0;
    }

    //join thread when ack received
    if (pthread_join(receiveAckThread, NULL)) {
      cout << "Error joining receive thread" << endl;
      exit(1);
    }
  }
  return 0;
}

//receives ack in new thread
void * receiveAck(void * aserver){
  GBNServerProtocol * server = (GBNServerProtocol*)aserver;
  while(!server->receivedAck());
  if (server->verbose){
    cout << "Received: Ack " << server->receivedAckNum << endl;
  }
  server->currentWindowBase = server->receivedAckNum;
  server->timeoutTimer.stop();
  server->timeoutTimer.valid = false;
  return NULL;
}

//sends all packets in current window
void resendValidPackets(GBNServerProtocol * server){
  for (int packetNum = server->currentWindowBase; packetNum < server->currentWindowBase + server->windowSize; packetNum++){
    if (packetNum < server->totalChunks){
      if (server->verbose){
        cout << "Sent: Packet number " << packetNum << endl;
      }
      server->timeoutTimer.start();
      server->sendData(packetNum);
    }
  }
  if (server->verbose){
    cout << endl;
  }
}

//TODO: refactorable with resendValidPackets!
void sendValidPackets(GBNServerProtocol * server){
  for (int packetNum = server->currentWindowBase; packetNum < server->currentWindowBase + server->windowSize; packetNum++){
    if (packetNum < server->totalChunks && server->packetState[packetNum] == Unsent){
      if (server->verbose){
        cout << "Sent: Packet number " << packetNum << endl;
      }
      server->packetState[packetNum] = Sent;
      server->timeoutTimer.start();
      server->sendData(packetNum);
    }
  }
  if (server->verbose){
    cout << endl;
  }
}

//sends synack
void sendSynack(GBNServerProtocol * server){
  server->timeoutTimer.start();
  server->sendSynack();
}

<|endoftext|>


<commit_before>/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2003 Robert Osfield
 *
 * This library is open source and may be redistributed and/or modified under
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * OpenSceneGraph Public License for more details.
*/
#include <osg/Types>
#include <osg/Notify>
#include <osg/TriangleFunctor>

#include <osgUtil/TriStripVisitor>

#include <stdio.h>

#include "TriStrip_tri_stripper.h"

using namespace osg;
using namespace osgUtil;

// triangle functor.
struct TriangleAcumulatorFunctor
{

    triangle_stripper::tri_stripper::indices in_indices;
    const Vec3* _vbase;

    TriangleAcumulatorFunctor() : _vbase(0) {}

    void setCoords( const Vec3* vbase ) { _vbase = vbase; }

    inline void operator() ( const Vec3 &v1, const Vec3 &v2, const Vec3 &v3 )
    {
        int p1 = (int)(&v1-_vbase);
        int p2 = (int)(&v2-_vbase);
        int p3 = (int)(&v3-_vbase);
        if (p1==p2 || p1==p3 || p2==p3) return;
        in_indices.push_back(p1);
        in_indices.push_back(p2);
        in_indices.push_back(p3);
    }
};

void TriStripVisitor::stripify(Geometry& geom)
{

    if (geom.getNormalBinding()==osg::Geometry::BIND_PER_PRIMITIVE ||
        geom.getNormalBinding()==osg::Geometry::BIND_PER_PRIMITIVE_SET) return;

    if (geom.getColorBinding()==osg::Geometry::BIND_PER_PRIMITIVE ||
        geom.getColorBinding()==osg::Geometry::BIND_PER_PRIMITIVE_SET) return;

    if (geom.getSecondaryColorBinding()==osg::Geometry::BIND_PER_PRIMITIVE ||
        geom.getSecondaryColorBinding()==osg::Geometry::BIND_PER_PRIMITIVE_SET) return;

    if (geom.getFogCoordBinding()==osg::Geometry::BIND_PER_PRIMITIVE ||
        geom.getFogCoordBinding()==osg::Geometry::BIND_PER_PRIMITIVE_SET) return;

    unsigned int numSurfacePrimitives = 0;
    unsigned int numNonSurfacePrimitives = 0;

    Geometry::PrimitiveSetList& primitives = geom.getPrimitiveSetList();
    Geometry::PrimitiveSetList::iterator itr;
    for(itr=primitives.begin();
        itr!=primitives.end();
        ++itr)
    {
        switch((*itr)->getMode())
        {
            case(PrimitiveSet::TRIANGLES):
            case(PrimitiveSet::TRIANGLE_STRIP):
            case(PrimitiveSet::TRIANGLE_FAN):
            case(PrimitiveSet::QUADS):
            case(PrimitiveSet::QUAD_STRIP):
            case(PrimitiveSet::POLYGON):
                ++numSurfacePrimitives;
                break;
            default:
                ++numNonSurfacePrimitives;
                break;

        }
    }

    if (!numSurfacePrimitives) return;

    TriangleFunctor<TriangleAcumulatorFunctor> taf;

    Geometry::PrimitiveSetList new_primitives;
    new_primitives.reserve(primitives.size());

    for(itr=primitives.begin();
        itr!=primitives.end();
        ++itr)
    {
        switch((*itr)->getMode())
        {
            case(PrimitiveSet::TRIANGLES):
            case(PrimitiveSet::TRIANGLE_STRIP):
            case(PrimitiveSet::TRIANGLE_FAN):
            case(PrimitiveSet::QUADS):
            case(PrimitiveSet::QUAD_STRIP):
            case(PrimitiveSet::POLYGON):
                (*itr)->accept(taf);
                break;
            default:
                new_primitives.push_back(*itr);
                break;

        }
    }

    if (!taf.in_indices.empty())
    {
        int in_numVertices = -1;
        for(triangle_stripper::tri_stripper::indices::iterator itr=taf.in_indices.begin();
            itr!=taf.in_indices.end();
            ++itr)
        {
            if ((int)*itr>in_numVertices) in_numVertices=*itr;
        }
        // the largest indice is in_numVertices, but indices start at 0
        // so increment to give to the corrent number of verticies.
        ++in_numVertices;

        int in_cacheSize = 16;
        int in_minStripLength = 2;

        triangle_stripper::tri_stripper stripifier(taf.in_indices);
        stripifier.SetCacheSize(in_cacheSize);
        stripifier.SetMinStripSize(in_minStripLength);

        triangle_stripper::tri_stripper::primitives_vector outPrimitives;
        stripifier.Strip(&outPrimitives);

        for(triangle_stripper::tri_stripper::primitives_vector::iterator pitr=outPrimitives.begin();
            pitr!=outPrimitives.end();
            ++pitr)
        {
            osg::DrawElementsUShort* elements = new osg::DrawElementsUShort(pitr->m_Type);
            elements->insert(elements->end(),pitr->m_Indices.begin(),pitr->m_Indices.end());
            new_primitives.push_back(elements);
        }

        geom.setPrimitiveSetList(new_primitives);
    }
}

void TriStripVisitor::apply(Geode& geode)
{
    for(unsigned int i = 0; i < geode.getNumDrawables(); ++i )
    {
        osg::Geometry* geom = dynamic_cast<osg::Geometry*>(geode.getDrawable(i));
        if (geom) stripify(*geom);
    }
}
<commit_msg>Changed the template insert(,,) method for a std::copy() implemention as it seems that the Sun Forte compiler can't handle member templates!<commit_after>/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2003 Robert Osfield
 *
 * This library is open source and may be redistributed and/or modified under
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * OpenSceneGraph Public License for more details.
*/
#include <osg/Types>
#include <osg/Notify>
#include <osg/TriangleFunctor>

#include <osgUtil/TriStripVisitor>

#include <stdio.h>

#include "TriStrip_tri_stripper.h"

using namespace osg;
using namespace osgUtil;

// triangle functor.
struct TriangleAcumulatorFunctor
{

    triangle_stripper::tri_stripper::indices in_indices;
    const Vec3* _vbase;

    TriangleAcumulatorFunctor() : _vbase(0) {}

    void setCoords( const Vec3* vbase ) { _vbase = vbase; }

    inline void operator() ( const Vec3 &v1, const Vec3 &v2, const Vec3 &v3 )
    {
        int p1 = (int)(&v1-_vbase);
        int p2 = (int)(&v2-_vbase);
        int p3 = (int)(&v3-_vbase);
        if (p1==p2 || p1==p3 || p2==p3) return;
        in_indices.push_back(p1);
        in_indices.push_back(p2);
        in_indices.push_back(p3);
    }
};

void TriStripVisitor::stripify(Geometry& geom)
{

    if (geom.getNormalBinding()==osg::Geometry::BIND_PER_PRIMITIVE ||
        geom.getNormalBinding()==osg::Geometry::BIND_PER_PRIMITIVE_SET) return;

    if (geom.getColorBinding()==osg::Geometry::BIND_PER_PRIMITIVE ||
        geom.getColorBinding()==osg::Geometry::BIND_PER_PRIMITIVE_SET) return;

    if (geom.getSecondaryColorBinding()==osg::Geometry::BIND_PER_PRIMITIVE ||
        geom.getSecondaryColorBinding()==osg::Geometry::BIND_PER_PRIMITIVE_SET) return;

    if (geom.getFogCoordBinding()==osg::Geometry::BIND_PER_PRIMITIVE ||
        geom.getFogCoordBinding()==osg::Geometry::BIND_PER_PRIMITIVE_SET) return;

    unsigned int numSurfacePrimitives = 0;
    unsigned int numNonSurfacePrimitives = 0;

    Geometry::PrimitiveSetList& primitives = geom.getPrimitiveSetList();
    Geometry::PrimitiveSetList::iterator itr;
    for(itr=primitives.begin();
        itr!=primitives.end();
        ++itr)
    {
        switch((*itr)->getMode())
        {
            case(PrimitiveSet::TRIANGLES):
            case(PrimitiveSet::TRIANGLE_STRIP):
            case(PrimitiveSet::TRIANGLE_FAN):
            case(PrimitiveSet::QUADS):
            case(PrimitiveSet::QUAD_STRIP):
            case(PrimitiveSet::POLYGON):
                ++numSurfacePrimitives;
                break;
            default:
                ++numNonSurfacePrimitives;
                break;

        }
    }

    if (!numSurfacePrimitives) return;

    TriangleFunctor<TriangleAcumulatorFunctor> taf;

    Geometry::PrimitiveSetList new_primitives;
    new_primitives.reserve(primitives.size());

    for(itr=primitives.begin();
        itr!=primitives.end();
        ++itr)
    {
        switch((*itr)->getMode())
        {
            case(PrimitiveSet::TRIANGLES):
            case(PrimitiveSet::TRIANGLE_STRIP):
            case(PrimitiveSet::TRIANGLE_FAN):
            case(PrimitiveSet::QUADS):
            case(PrimitiveSet::QUAD_STRIP):
            case(PrimitiveSet::POLYGON):
                (*itr)->accept(taf);
                break;
            default:
                new_primitives.push_back(*itr);
                break;

        }
    }

    if (!taf.in_indices.empty())
    {
        int in_numVertices = -1;
        for(triangle_stripper::tri_stripper::indices::iterator itr=taf.in_indices.begin();
            itr!=taf.in_indices.end();
            ++itr)
        {
            if ((int)*itr>in_numVertices) in_numVertices=*itr;
        }
        // the largest indice is in_numVertices, but indices start at 0
        // so increment to give to the corrent number of verticies.
        ++in_numVertices;

        int in_cacheSize = 16;
        int in_minStripLength = 2;

        triangle_stripper::tri_stripper stripifier(taf.in_indices);
        stripifier.SetCacheSize(in_cacheSize);
        stripifier.SetMinStripSize(in_minStripLength);

        triangle_stripper::tri_stripper::primitives_vector outPrimitives;
        stripifier.Strip(&outPrimitives);

        for(triangle_stripper::tri_stripper::primitives_vector::iterator pitr=outPrimitives.begin();
            pitr!=outPrimitives.end();
            ++pitr)
        {
            osg::DrawElementsUShort* elements = new osg::DrawElementsUShort(pitr->m_Type);
            elements->reserve(pitr->m_Indices.size());
            std::copy(pitr->m_Indices.begin(),pitr->m_Indices.end(),std::back_inserter(*elements));
            //elements->insert(elements->end(),);
            new_primitives.push_back(elements);
        }

        geom.setPrimitiveSetList(new_primitives);
    }
}

void TriStripVisitor::apply(Geode& geode)
{
    for(unsigned int i = 0; i < geode.getNumDrawables(); ++i )
    {
        osg::Geometry* geom = dynamic_cast<osg::Geometry*>(geode.getDrawable(i));
        if (geom) stripify(*geom);
    }
}
<|endoftext|>


<commit_before>#include <gtest/gtest.h>

#include "Call.cpp"

TEST(Call, CallCreation)
{
	Call* doubleCall = new Call(CallType::DOUBLE);
	ASSERT_TRUE(doubleCall->callType == CallType::DOUBLE);
	delete doubleCall;

	Call* redoubleCall = new Call(CallType::REDOUBLE);
	ASSERT_TRUE(redoubleCall->callType == CallType::REDOUBLE);
	delete redoubleCall;

	Call* passCall = new Call(CallType::PASS);
	ASSERT_TRUE(passCall->callType == CallType::PASS);
	delete passCall;
}

TEST(DeclarationCall, DeclarationCallCreation)
{
	DeclarationCall* declarationCall = new DeclarationCall(Level::ACE);
	ASSERT_TRUE(declarationCall->callType == CallType::DECLARATION);
	ASSERT_TRUE(declarationCall->level == Level::ACE);
	delete declarationCall;
}
<commit_msg>Delete CallTest.cpp<commit_after><|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * 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 <fstream>
#include <iomanip>
#include "modules/localization/msf/common/io/velodyne_utility.h"
#include "modules/localization/msf/local_tool/map_creation/poses_interpolation/poses_interpolation.h"

namespace apollo {
namespace localization {
namespace msf {
PosesInterpolation::PosesInterpolation() {}

bool PosesInterpolation::Init(const std::string &input_poses_path,
                              const std::string &ref_timestamps_path,
                              const std::string &out_poses_path,
                              const std::string &extrinsic_path) {
  this->input_poses_path_ = input_poses_path;
  this->ref_timestamps_path_ = ref_timestamps_path;
  this->out_poses_path_ = out_poses_path;
  this->extrinsic_path_ = extrinsic_path;

  bool success = velodyne::LoadExtrinsic(extrinsic_path_, &velodyne_extrinsic_);
  if (!success) {
    std::cerr << "Load lidar extrinsic failed." << std::endl;
    return false;
  }

  return true;
}

void PosesInterpolation::DoInterpolation() {
  // Load input poses
  std::vector<Eigen::Vector3d> input_stds;
  velodyne::LoadPosesAndStds(input_poses_path_, &input_poses_, &input_stds,
                             &input_poses_timestamps_);

  // Load pcd timestamp
  LoadPCDTimestamp();

  // Interpolation
  PoseInterpolationByTime(input_poses_, input_poses_timestamps_,
                          ref_timestamps_, ref_ids_, &out_indexes_,
                          &out_timestamps_, &out_poses_);

  // Write pcd poses
  WritePCDPoses();
}

void PosesInterpolation::LoadPCDTimestamp() {
  FILE *file = fopen(ref_timestamps_path_.c_str(), "r");
  if (file) {
    unsigned int index;
    double timestamp;
    constexpr int kSize = 2;
    while (fscanf(file, "%u %lf\n", &index, &timestamp) == kSize) {
      ref_timestamps_.push_back(timestamp);
      ref_ids_.push_back(index);
    }
    fclose(file);
  } else {
    std::cerr << "Can't open file to read: " << ref_timestamps_path_;
  }
}

void PosesInterpolation::WritePCDPoses() {
  std::ofstream fout;
  fout.open(out_poses_path_.c_str(), std::ofstream::out);
  fout.setf(std::ios::fixed, std::ios::floatfield);
  fout.precision(6);

  if (fout.is_open()) {
    for (size_t i = 0; i < out_poses_.size(); i++) {
      double timestamp = out_timestamps_[i];

      Eigen::Affine3d pose_tem = out_poses_[i] * velodyne_extrinsic_;
      Eigen::Quaterniond quatd(pose_tem.linear());
      Eigen::Translation3d transd(pose_tem.translation());
      double qx = quatd.x();
      double qy = quatd.y();
      double qz = quatd.z();
      double qr = quatd.w();

      fout << out_indexes_[i] << " " << timestamp << " " << transd.x() << " "
           << transd.y() << " " << transd.z() << " " << qx << " " << qy << " "
           << qz << " " << qr << "\n";
    }
    fout.close();
  } else {
    std::cerr << "Can't open file to write: " << out_poses_path_ << std::endl;
  }
}  // namespace msf

void PosesInterpolation::PoseInterpolationByTime(
    const std::vector<Eigen::Affine3d> &in_poses,
    const std::vector<double> &in_timestamps,
    const std::vector<double> &ref_timestamps,
    const std::vector<unsigned int> &ref_indexes,
    std::vector<unsigned int> *out_indexes, std::vector<double> *out_timestamps,
    std::vector<Eigen::Affine3d> *out_poses) {
  out_indexes->clear();
  out_timestamps->clear();
  out_poses->clear();

  unsigned int index = 0;
  for (size_t i = 0; i < ref_timestamps.size(); i++) {
    double ref_timestamp = ref_timestamps[i];
    unsigned int ref_index = ref_indexes[i];

    while (index < in_timestamps.size() &&
           in_timestamps.at(index) < ref_timestamp) {
      ++index;
    }

    if (index < in_timestamps.size()) {
      if (index >= 1) {
        double cur_timestamp = in_timestamps[index];
        double pre_timestamp = in_timestamps[index - 1];
        assert(cur_timestamp != pre_timestamp);

        double t =
            (cur_timestamp - ref_timestamp) / (cur_timestamp - pre_timestamp);
        assert(t >= 0.0);
        assert(t <= 1.0);

        Eigen::Affine3d pre_pose = in_poses[index - 1];
        Eigen::Affine3d cur_pose = in_poses[index];
        Eigen::Quaterniond pre_quatd(pre_pose.linear());
        Eigen::Translation3d pre_transd(pre_pose.translation());
        Eigen::Quaterniond cur_quatd(cur_pose.linear());
        Eigen::Translation3d cur_transd(cur_pose.translation());

        Eigen::Quaterniond res_quatd = pre_quatd.slerp(1 - t, cur_quatd);

        Eigen::Translation3d re_transd;
        re_transd.x() = pre_transd.x() * t + cur_transd.x() * (1 - t);
        re_transd.y() = pre_transd.y() * t + cur_transd.y() * (1 - t);
        re_transd.z() = pre_transd.z() * t + cur_transd.z() * (1 - t);

        out_poses->push_back(re_transd * res_quatd);
        out_indexes->push_back(ref_index);
        out_timestamps->push_back(ref_timestamp);
      }
    } else {
      std::cerr << "[ERROR] No more poses. Exit now." << std::endl;
      break;
    }
    std::cout << "Frame_id: " << i << std::endl;
  }
}

}  // namespace msf
}  // namespace localization
}  // namespace apollo
<commit_msg>Localization: fix order of fstream operation in PoseInterpolation<commit_after>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * 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 <fstream>
#include <iomanip>
#include "modules/localization/msf/common/io/velodyne_utility.h"
#include "modules/localization/msf/local_tool/map_creation/poses_interpolation/poses_interpolation.h"

namespace apollo {
namespace localization {
namespace msf {
PosesInterpolation::PosesInterpolation() {}

bool PosesInterpolation::Init(const std::string &input_poses_path,
                              const std::string &ref_timestamps_path,
                              const std::string &out_poses_path,
                              const std::string &extrinsic_path) {
  this->input_poses_path_ = input_poses_path;
  this->ref_timestamps_path_ = ref_timestamps_path;
  this->out_poses_path_ = out_poses_path;
  this->extrinsic_path_ = extrinsic_path;

  bool success = velodyne::LoadExtrinsic(extrinsic_path_, &velodyne_extrinsic_);
  if (!success) {
    std::cerr << "Load lidar extrinsic failed." << std::endl;
    return false;
  }

  return true;
}

void PosesInterpolation::DoInterpolation() {
  // Load input poses
  std::vector<Eigen::Vector3d> input_stds;
  velodyne::LoadPosesAndStds(input_poses_path_, &input_poses_, &input_stds,
                             &input_poses_timestamps_);

  // Load pcd timestamp
  LoadPCDTimestamp();

  // Interpolation
  PoseInterpolationByTime(input_poses_, input_poses_timestamps_,
                          ref_timestamps_, ref_ids_, &out_indexes_,
                          &out_timestamps_, &out_poses_);

  // Write pcd poses
  WritePCDPoses();
}

void PosesInterpolation::LoadPCDTimestamp() {
  FILE *file = fopen(ref_timestamps_path_.c_str(), "r");
  if (file) {
    unsigned int index;
    double timestamp;
    constexpr int kSize = 2;
    while (fscanf(file, "%u %lf\n", &index, &timestamp) == kSize) {
      ref_timestamps_.push_back(timestamp);
      ref_ids_.push_back(index);
    }
    fclose(file);
  } else {
    std::cerr << "Can't open file to read: " << ref_timestamps_path_;
  }
}

void PosesInterpolation::WritePCDPoses() {
  std::ofstream fout;
  fout.open(out_poses_path_.c_str(), std::ofstream::out);
  fout.setf(std::ios::fixed, std::ios::floatfield);

  if (fout.is_open()) {
    for (size_t i = 0; i < out_poses_.size(); i++) {
      double timestamp = out_timestamps_[i];

      Eigen::Affine3d pose_tem = out_poses_[i] * velodyne_extrinsic_;
      Eigen::Quaterniond quatd(pose_tem.linear());
      Eigen::Translation3d transd(pose_tem.translation());
      double qx = quatd.x();
      double qy = quatd.y();
      double qz = quatd.z();
      double qr = quatd.w();

      fout.precision(6);
      fout << out_indexes_[i] << " " << timestamp << " " << transd.x() << " "
           << transd.y() << " " << transd.z() << " " << qx << " " << qy << " "
           << qz << " " << qr << "\n";
    }
    fout.close();
  } else {
    std::cerr << "Can't open file to write: " << out_poses_path_ << std::endl;
  }
}  // namespace msf

void PosesInterpolation::PoseInterpolationByTime(
    const std::vector<Eigen::Affine3d> &in_poses,
    const std::vector<double> &in_timestamps,
    const std::vector<double> &ref_timestamps,
    const std::vector<unsigned int> &ref_indexes,
    std::vector<unsigned int> *out_indexes, std::vector<double> *out_timestamps,
    std::vector<Eigen::Affine3d> *out_poses) {
  out_indexes->clear();
  out_timestamps->clear();
  out_poses->clear();

  unsigned int index = 0;
  for (size_t i = 0; i < ref_timestamps.size(); i++) {
    double ref_timestamp = ref_timestamps[i];
    unsigned int ref_index = ref_indexes[i];

    while (index < in_timestamps.size() &&
           in_timestamps.at(index) < ref_timestamp) {
      ++index;
    }

    if (index < in_timestamps.size()) {
      if (index >= 1) {
        double cur_timestamp = in_timestamps[index];
        double pre_timestamp = in_timestamps[index - 1];
        assert(cur_timestamp != pre_timestamp);

        double t =
            (cur_timestamp - ref_timestamp) / (cur_timestamp - pre_timestamp);
        assert(t >= 0.0);
        assert(t <= 1.0);

        Eigen::Affine3d pre_pose = in_poses[index - 1];
        Eigen::Affine3d cur_pose = in_poses[index];
        Eigen::Quaterniond pre_quatd(pre_pose.linear());
        Eigen::Translation3d pre_transd(pre_pose.translation());
        Eigen::Quaterniond cur_quatd(cur_pose.linear());
        Eigen::Translation3d cur_transd(cur_pose.translation());

        Eigen::Quaterniond res_quatd = pre_quatd.slerp(1 - t, cur_quatd);

        Eigen::Translation3d re_transd;
        re_transd.x() = pre_transd.x() * t + cur_transd.x() * (1 - t);
        re_transd.y() = pre_transd.y() * t + cur_transd.y() * (1 - t);
        re_transd.z() = pre_transd.z() * t + cur_transd.z() * (1 - t);

        out_poses->push_back(re_transd * res_quatd);
        out_indexes->push_back(ref_index);
        out_timestamps->push_back(ref_timestamp);
      }
    } else {
      std::cerr << "[ERROR] No more poses. Exit now." << std::endl;
      break;
    }
    std::cout << "Frame_id: " << i << std::endl;
  }
}

}  // namespace msf
}  // namespace localization
}  // namespace apollo
<|endoftext|>


<commit_before>#include <vector>

#include <Ptexture.h>

#include "ptexutils.hpp"
#include "helpers.hpp"

int ptex_utils::ptex_conform(const char* filename,
                             const char* output_filename,
                             int8_t downsteps,
                             int8_t clampsize,
                             bool change_datatype,
                             Ptex::DataType out_dt,
                             Ptex::String &err_msg)
{
    PtxPtr ptx(PtexTexture::open(filename, err_msg, 0));
    if (!ptx) {
        err_msg = "Can't open for reading " + std::string(filename) + ":" + err_msg;
        return -1;
    }

    downsteps = std::max(0, (int) downsteps);
    clampsize = std::max(0, (int) downsteps);

    Ptex::DataType input_dt = ptx->dataType();
    Ptex::DataType dt = change_datatype ? out_dt : input_dt;
    int nfaces = ptx->numFaces();
    int nchannels = ptx->numChannels();

    WriterPtr writer(PtexWriter::open(output_filename,
                                      ptx->meshType(),
                                      dt,
                                      nchannels,
                                      ptx->alphaChannel(),
                                      nfaces,
                                      err_msg));
    if (!writer) {
        err_msg = "Can't open for writing " + std::string(output_filename) + ":" + err_msg;
        return -1;
    }

    int8_t clamp_log = clampsize <= 0 ? 15 : clampsize;
    Ptex::Res clamp_res(clamp_log, clamp_log);

    const size_t input_pixel_size = Ptex::DataSize(ptx->dataType()) * nchannels;
    const size_t pixel_size = Ptex::DataSize(dt)*nchannels;

    std::vector<char> in_buffer(input_pixel_size*128*128);
    std::vector<char> out_buffer(pixel_size*128*128);
    std::vector<float> fdata(nchannels*128*128);

    for (int face_id = 0; face_id < nfaces; ++face_id) {
        Ptex::FaceInfo face_info = ptx->getFaceInfo(face_id);

        if (!face_info.isConstant()) {
            Ptex::Res res = face_info.res;
            res.ulog2 = res.ulog2 > 2 ? std::max(1, res.ulog2 - downsteps) : res.ulog2;
            res.vlog2 = res.vlog2 > 2 ? std::max(1, res.vlog2 - downsteps) : res.vlog2;
            res.clamp(clamp_res);
            face_info.res = res;
        }

        const size_t input_size = input_pixel_size * face_info.res.size();
        if (in_buffer.size() < input_size) {
            in_buffer.resize(input_size);
        }

        ptx->getData(face_id, in_buffer.data(), 0, face_info.res);

        if (dt == input_dt) {
            if (face_info.isConstant()) {
                writer->writeConstantFace(face_id, face_info, in_buffer.data());
            }
            else {
                writer->writeFace(face_id, face_info, in_buffer.data(), 0);
            }
        }
        else {
            const size_t out_size = pixel_size * face_info.res.size();
            const size_t float_size = nchannels*face_info.res.size();
            if (out_buffer.size() < out_size) {
                out_buffer.resize(out_size);
            }
            if (fdata.size() < float_size) {
                fdata.resize(float_size);
            }

            Ptex::ConvertToFloat(fdata.data(), in_buffer.data(), input_dt,
                                 nchannels*face_info.res.size());
            Ptex::ConvertFromFloat(out_buffer.data(), fdata.data(),
                                   dt, nchannels*face_info.res.size());

            if (face_info.isConstant()) {
                writer->writeConstantFace(face_id, face_info, out_buffer.data());
            }
            else {
                writer->writeFace(face_id, face_info, out_buffer.data(), 0);
            }
        }
    }

    MetaPtr meta_ptr(ptx->getMetaData());
    writer->writeMeta(meta_ptr.get());

    writer->setBorderModes(ptx->vBorderMode(), ptx->uBorderMode());

    if (!writer->close(err_msg)){
        err_msg = "Closing writer " + std::string(output_filename) + ":" + err_msg.c_str();
	return -1;
    }

    return 0;
}

<commit_msg>Fix clamping in ptex conform<commit_after>#include <vector>

#include <Ptexture.h>

#include "ptexutils.hpp"
#include "helpers.hpp"

int ptex_utils::ptex_conform(const char* filename,
                             const char* output_filename,
                             int8_t downsteps,
                             int8_t clampsize,
                             bool change_datatype,
                             Ptex::DataType out_dt,
                             Ptex::String &err_msg)
{
    PtxPtr ptx(PtexTexture::open(filename, err_msg, 0));
    if (!ptx) {
        err_msg = "Can't open for reading " + std::string(filename) + ":" + err_msg;
        return -1;
    }

    downsteps = std::max(0, (int) downsteps);
    clampsize = std::max(0, (int) clampsize);

    Ptex::DataType input_dt = ptx->dataType();
    Ptex::DataType dt = change_datatype ? out_dt : input_dt;
    int nfaces = ptx->numFaces();
    int nchannels = ptx->numChannels();

    WriterPtr writer(PtexWriter::open(output_filename,
                                      ptx->meshType(),
                                      dt,
                                      nchannels,
                                      ptx->alphaChannel(),
                                      nfaces,
                                      err_msg));
    if (!writer) {
        err_msg = "Can't open for writing " + std::string(output_filename) + ":" + err_msg;
        return -1;
    }

    int8_t clamp_log = clampsize <= 0 ? 15 : clampsize;
    Ptex::Res clamp_res(clamp_log, clamp_log);

    const size_t input_pixel_size = Ptex::DataSize(ptx->dataType()) * nchannels;
    const size_t pixel_size = Ptex::DataSize(dt)*nchannels;

    std::vector<char> in_buffer(input_pixel_size*128*128);
    std::vector<char> out_buffer(pixel_size*128*128);
    std::vector<float> fdata(nchannels*128*128);

    for (int face_id = 0; face_id < nfaces; ++face_id) {
        Ptex::FaceInfo face_info = ptx->getFaceInfo(face_id);

        if (!face_info.isConstant()) {
            Ptex::Res res = face_info.res;
            res.ulog2 = res.ulog2 > 2 ? std::max(1, res.ulog2 - downsteps) : res.ulog2;
            res.vlog2 = res.vlog2 > 2 ? std::max(1, res.vlog2 - downsteps) : res.vlog2;
            res.clamp(clamp_res);
            face_info.res = res;
        }

        const size_t input_size = input_pixel_size * face_info.res.size();
        if (in_buffer.size() < input_size) {
            in_buffer.resize(input_size);
        }

        ptx->getData(face_id, in_buffer.data(), 0, face_info.res);

        if (dt == input_dt) {
            if (face_info.isConstant()) {
                writer->writeConstantFace(face_id, face_info, in_buffer.data());
            }
            else {
                writer->writeFace(face_id, face_info, in_buffer.data(), 0);
            }
        }
        else {
            const size_t out_size = pixel_size * face_info.res.size();
            const size_t float_size = nchannels*face_info.res.size();
            if (out_buffer.size() < out_size) {
                out_buffer.resize(out_size);
            }
            if (fdata.size() < float_size) {
                fdata.resize(float_size);
            }

            Ptex::ConvertToFloat(fdata.data(), in_buffer.data(), input_dt,
                                 nchannels*face_info.res.size());
            Ptex::ConvertFromFloat(out_buffer.data(), fdata.data(),
                                   dt, nchannels*face_info.res.size());

            if (face_info.isConstant()) {
                writer->writeConstantFace(face_id, face_info, out_buffer.data());
            }
            else {
                writer->writeFace(face_id, face_info, out_buffer.data(), 0);
            }
        }
    }

    MetaPtr meta_ptr(ptx->getMetaData());
    writer->writeMeta(meta_ptr.get());

    writer->setBorderModes(ptx->vBorderMode(), ptx->uBorderMode());

    if (!writer->close(err_msg)){
        err_msg = "Closing writer " + std::string(output_filename) + ":" + err_msg.c_str();
	return -1;
    }

    return 0;
}

<|endoftext|>