<commit_before>// The libMesh Finite Element Library.
// Copyright (C) 2002-2014 Benjamin S. Kirk, John W. Peterson, Roy H. Stogner

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// 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 GNU
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

#include "libmesh/libmesh_common.h"
#include "libmesh/diff_solver.h"
#include "libmesh/newton_solver.h"
#include "libmesh/implicit_system.h"
namespace libMesh
{

DiffSolver::DiffSolver (sys_type& s)
  : ParallelObject(s),
    max_linear_iterations(1000),
    max_nonlinear_iterations(100),
    quiet(true),
    verbose(false),
    continue_after_max_iterations(true),
    continue_after_backtrack_failure(false),
    absolute_residual_tolerance(0.),
    relative_residual_tolerance(0.),
    absolute_step_tolerance(0.),
    relative_step_tolerance(0.),
    initial_linear_tolerance(1e-12),
    minimum_linear_tolerance(TOLERANCE*TOLERANCE),
    max_solution_norm(0.),
    max_residual_norm(0.),
    _system (s),
    _solve_result(INVALID_SOLVE_RESULT)
{
}

AutoPtr<DiffSolver> DiffSolver::build (sys_type& s)
{
  return AutoPtr<DiffSolver>(new NewtonSolver(s));
}

void DiffSolver::reinit ()
{
  // Reset the max_step_size and max_residual_norm for a new mesh
  max_solution_norm = 0.;
  max_residual_norm = 0.;
}

void DiffSolver::init ()
{
  // Reset the max_step_size and max_residual_norm for a new problem
  max_solution_norm = 0.;
  max_residual_norm = 0.;
}

} // namespace libMesh
<commit_msg>Don't start with undefined iteration counts<commit_after>// The libMesh Finite Element Library.
// Copyright (C) 2002-2014 Benjamin S. Kirk, John W. Peterson, Roy H. Stogner

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// 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 GNU
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

#include "libmesh/libmesh_common.h"
#include "libmesh/diff_solver.h"
#include "libmesh/newton_solver.h"
#include "libmesh/implicit_system.h"
namespace libMesh
{

DiffSolver::DiffSolver (sys_type& s)
  : ParallelObject(s),
    max_linear_iterations(1000),
    max_nonlinear_iterations(100),
    quiet(true),
    verbose(false),
    continue_after_max_iterations(true),
    continue_after_backtrack_failure(false),
    absolute_residual_tolerance(0.),
    relative_residual_tolerance(0.),
    absolute_step_tolerance(0.),
    relative_step_tolerance(0.),
    initial_linear_tolerance(1e-12),
    minimum_linear_tolerance(TOLERANCE*TOLERANCE),
    max_solution_norm(0.),
    max_residual_norm(0.),
    _outer_iterations(0),
    _inner_iterations(0),
    _system (s),
    _solve_result(INVALID_SOLVE_RESULT)
{
}

AutoPtr<DiffSolver> DiffSolver::build (sys_type& s)
{
  return AutoPtr<DiffSolver>(new NewtonSolver(s));
}

void DiffSolver::reinit ()
{
  // Reset the max_step_size and max_residual_norm for a new mesh
  max_solution_norm = 0.;
  max_residual_norm = 0.;
}

void DiffSolver::init ()
{
  // Reset the max_step_size and max_residual_norm for a new problem
  max_solution_norm = 0.;
  max_residual_norm = 0.;
}

} // namespace libMesh
<|endoftext|>


<commit_before>/** @file sortable-serialise.cc
 * @brief Serialise floating point values to string which sort the same way.
 */
/* Copyright (C) 2007,2009,2015,2016 Olly Betts
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program 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
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA
 */

#include <cfloat>
#include <cmath>
#include <cstring>
#include <string>

#include "sortable_serialise.h"

size_t
sortable_serialise_(long double value, char * buf)
{
	long double mantissa;
	int exponent;

	// Negative infinity.
	if (value < -LDBL_MAX) return 0;

	mantissa = frexpl(value, &exponent);

	/* Deal with zero specially.
	 *
	 * IEEE representation of long doubles uses 15 bits for the exponent, with a
	 * bias of 16383.  We bias this by subtracting 8, and non-IEEE
	 * representations may allow higher exponents, so allow exponents down to
	 * -32759 - if smaller exponents are possible anywhere, we underflow such
	 *  numbers to 0.
	 */
	if (mantissa == 0.0 || exponent < -(LDBL_MAX_EXP + LDBL_MAX_EXP - 1 - 8)) {
		*buf = '\x80';
		return 1;
	}

	bool negative = (mantissa < 0);
	if (negative) mantissa = -mantissa;

	// Infinity, or extremely large non-IEEE representation.
	if (value > LDBL_MAX || exponent > LDBL_MAX_EXP + LDBL_MAX_EXP - 1 + 8) {
		if (negative) {
			// This can only happen with a non-IEEE representation, because
			// we've already tested for value < -LDBL_MAX
			return 0;
		} else {
			memset(buf, '\xff', 18);
			return 18;
		}
	}

	// Encoding:
	//
	// [ 7 | 6 | 5 | 4 3 2 1 0]
	//   Sm  Se  Le
	//
	// Sm stores the sign of the mantissa: 1 = positive or zero, 0 = negative.
	// Se stores the sign of the exponent: Sm for positive/zero, !Sm for neg.
	// Le stores the length of the exponent: !Se for 7 bits, Se for 15 bits.
	unsigned char next = (negative ? 0 : 0xe0);

	// Bias the exponent by 8 so that more small integers get short encodings.
	exponent -= 8;
	bool exponent_negative = (exponent < 0);
	if (exponent_negative) {
		exponent = -exponent;
		next ^= 0x60;
	}

	size_t len = 0;

	/* We store the exponent in 7 or 15 bits.  If the number is negative, we
	 * flip all the bits of the exponent, since larger negative numbers should
	 * sort first.
	 *
	 * If the exponent is negative, we flip the bits of the exponent, since
	 * larger negative exponents should sort first (unless the number is
	 * negative, in which case they should sort later).
	 */
	//if (!(exponent >= 0)) std::cerr << "S=" << "ASSERT 1" << std::endl;
	if (exponent < 128) {
		next ^= 0x20;
		// Put the top 5 bits of the exponent into the lower 5 bits of the
		// first byte:
		next |= static_cast<unsigned char>(exponent >> 2);
		if (negative ^ exponent_negative) next ^= 0x1f;
		buf[len++] = next;

		// And the lower 2 bits of the exponent go into the upper 2 bits
		// of the second byte:
		next = static_cast<unsigned char>(exponent) << 6;
		if (negative ^ exponent_negative) next ^= 0xc0;

	} else {
		//if (!((exponent >> 15) == 0)) std::cerr << "S=" << "ASSERT 2" << std::endl;
		// Put the top 5 bits of the exponent into the lower 5 bits of the
		// first byte:
		next |= static_cast<unsigned char>(exponent >> 10);
		if (negative ^ exponent_negative) next ^= 0x1f;
		buf[len++] = next;

		// Put the bits 3-10 of the exponent into the second byte:
		next = static_cast<unsigned char>(exponent >> 2);
		if (negative ^ exponent_negative) next ^= 0xff;
		buf[len++] = next;

		// And the lower 2 bits of the exponent go into the upper 2 bits
		// of the third byte:
		next = static_cast<unsigned char>(exponent) << 6;
		if (negative ^ exponent_negative) next ^= 0xc0;
	}

	// Convert the 112 (or 113) bits of the mantissa into two 32-bit words.

	mantissa *= (negative ? 1073741824.0 : 2147483648.0);  // 1<<30 : 1<<31
	unsigned word1 = static_cast<unsigned>(mantissa);
	mantissa -= word1;

	mantissa *= 4294967296.0L;  // 1<<32
	unsigned word2 = static_cast<unsigned>(mantissa);
	mantissa -= word2;

	mantissa *= 4294967296.0L;  // 1<<32
	unsigned word3 = static_cast<unsigned>(mantissa);
	mantissa -= word3;

	mantissa *= 4294967296.0L;  // 1<<32
	unsigned word4 = static_cast<unsigned>(mantissa);

	// If the number is positive, the first bit will always be set because 0.5
	// <= mantissa < 1, unless mantissa is zero, which we handle specially
	// above).  If the number is negative, we negate the mantissa instead of
	// flipping all the bits, so in the case of 0.5, the first bit isn't set
	// so we need to store it explicitly.  But for the cost of one extra
	// leading bit, we can save several trailing 0xff bytes in lots of common
	// cases.
	//if (!(negative || (word1 & (1<<30)))) std::cerr << "S=" << "ASSERT 3" << std::endl;
	if (negative) {
		// We negate the mantissa for negative numbers, so that the sort order
		// is reversed (since larger negative numbers should come first).
		word1 = -word1;
		if (word2 != 0 || word3 != 0 || word4 != 0) ++word1;
		word2 = -word2;
		if (word3 != 0 || word4 != 0) ++word2;
		word3 = -word3;
		if (word4 != 0) ++word3;
		word4 = -word4;
	}

	word1 &= 0x3fffffff;
	next |= static_cast<unsigned char>(word1 >> 24);
	buf[len++] = next;

	if (word1 != 0) {
		buf[len++] = char(word1 >> 16);
		buf[len++] = char(word1 >> 8);
		buf[len++] = char(word1);
	}

	if (word2 != 0 || word3 != 0 || word4 != 0) {
		buf[len++] = char(word2 >> 24);
		buf[len++] = char(word2 >> 16);
		buf[len++] = char(word2 >> 8);
		buf[len++] = char(word2);
	}

	if (word3 != 0 || word4 != 0) {
		buf[len++] = char(word3 >> 24);
		buf[len++] = char(word3 >> 16);
		buf[len++] = char(word3 >> 8);
		buf[len++] = char(word3);
	}

	if (word4 != 0) {
		buf[len++] = char(word4 >> 24);
		buf[len++] = char(word4 >> 16);
		buf[len++] = char(word4 >> 8);
		buf[len++] = char(word4);
	}

	// Finally, we can chop off any trailing zero bytes.
	while (len > 0 && buf[len - 1] == '\0') {
		--len;
	}

	return len;
}

std::string sortable_serialise(long double value) {
	char buf[18];
	return std::string(buf, sortable_serialise_(value, buf));
}

/// Get a number from the character at a given position in a string, returning
/// 0 if the string isn't long enough.
static inline unsigned char
numfromstr(const std::string & str, std::string::size_type pos)
{
	return (pos < str.size()) ? static_cast<unsigned char>(str[pos]) : '\0';
}

long double
sortable_unserialise(const std::string & value)
{
	// Zero.
	if (value.size() == 1 && value[0] == '\x80') return 0.0;

	// Positive infinity.
	if (value.size() == 18 &&
		memcmp(value.data(), "\xff\xff\xff\xff\xff\xff\xff\xff\xff", 18) == 0) {
		// #ifdef INFINITY
		// 		// INFINITY is C99.  Oddly, it's of type "float" so sanity check in
		// 		// case it doesn't cast to double as infinity (apparently some
		// 		// implementations have this problem).
		// 		if (long double(INFINITY) > HUGE_VAL) return INFINITY;
		// #endif
		return HUGE_VAL;
	}

	// Negative infinity.
	if (value.empty()) {
//		#ifdef INFINITY
//				if (long double(INFINITY) > HUGE_VAL) return -INFINITY;
//		#endif
		return -HUGE_VAL;
	}

	unsigned char first = numfromstr(value, 0);
	size_t i = 0;

	first ^= static_cast<unsigned char>(first & 0xc0) >> 1;
	bool negative = !(first & 0x80);
	bool exponent_negative = (first & 0x40);
	bool explen = !(first & 0x20);
	int exponent = first & 0x1f;
	if (!explen) {
		first = numfromstr(value, ++i);
		exponent <<= 2;
		exponent |= (first >> 6);
		if (negative ^ exponent_negative) exponent ^= 0x07f;
	} else {
		first = numfromstr(value, ++i);
		exponent <<= 8;
		exponent |= first;
		first = numfromstr(value, ++i);
		exponent <<= 2;
		exponent |= (first >> 6);
		if (negative ^ exponent_negative) exponent ^= 0x7fff;
	}

	unsigned word1;
	word1 = (unsigned(first & 0x3f) << 24);
	word1 |= numfromstr(value, ++i) << 16;
	word1 |= numfromstr(value, ++i) << 8;
	word1 |= numfromstr(value, ++i);

	unsigned word2 = 0;
	if (i < value.size()) {
		word2 = numfromstr(value, ++i) << 24;
		word2 |= numfromstr(value, ++i) << 16;
		word2 |= numfromstr(value, ++i) << 8;
		word2 |= numfromstr(value, ++i);
	}

	unsigned word3 = 0;
	if (i < value.size()) {
		word3 = numfromstr(value, ++i) << 24;
		word3 |= numfromstr(value, ++i) << 16;
		word3 |= numfromstr(value, ++i) << 8;
		word3 |= numfromstr(value, ++i);
	}

	unsigned word4 = 0;
	if (i < value.size()) {
		word4 = numfromstr(value, ++i) << 24;
		word4 |= numfromstr(value, ++i) << 16;
		word4 |= numfromstr(value, ++i) << 8;
		word4 |= numfromstr(value, ++i);
	}

	if (negative) {
		word1 = -word1;
		if (word2 != 0 || word3 != 0 || word4 != 0) ++word1;
		word2 = -word2;
		if (word3 != 0 || word4 != 0) ++word2;
		word3 = -word3;
		if (word4 != 0) ++word3;
		word4 = -word4;
		//if (!((word1 & 0xf0000000) != 0)) std::cerr << "U=" << "ASSERT 4" << std::endl;
		word1 &= 0x3fffffff;
	}
	if (!negative) word1 |= 1<<30;

	long double mantissa = 0;
	if (word4) mantissa += word4 / 79228162514264337593543950336.0L; // 1<<96
	if (word3) mantissa += word3 / 18446744073709551616.0L; // 1<<64
	if (word2) mantissa += word2 / 4294967296.0L; // 1<<32
	if (word1) mantissa += word1;
	mantissa /= (negative ? 1073741824.0 : 2147483648.0);  // 1<<30 : 1<<31

	if (exponent_negative) exponent = -exponent;
	exponent += 8;

	if (negative) mantissa = -mantissa;

	// We use scalbnl() since it's equivalent to ldexp() when FLT_RADIX == 2
	// (which we currently assume), except that ldexp() will set errno if the
	// result overflows or underflows, which isn't really desirable here.
	return scalbnl(mantissa, exponent);
}
<commit_msg>Asserts in sortable serialise<commit_after>/** @file sortable-serialise.cc
 * @brief Serialise floating point values to string which sort the same way.
 */
/* Copyright (C) 2007,2009,2015,2016 Olly Betts
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program 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
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA
 */

#include <assert.h>
#include <cfloat>
#include <cmath>
#include <cstring>
#include <string>

#include "sortable_serialise.h"

size_t
sortable_serialise_(long double value, char * buf)
{
	long double mantissa;
	int exponent;

	// Negative infinity.
	if (value < -LDBL_MAX) return 0;

	mantissa = frexpl(value, &exponent);

	/* Deal with zero specially.
	 *
	 * IEEE representation of long doubles uses 15 bits for the exponent, with a
	 * bias of 16383.  We bias this by subtracting 8, and non-IEEE
	 * representations may allow higher exponents, so allow exponents down to
	 * -32759 - if smaller exponents are possible anywhere, we underflow such
	 *  numbers to 0.
	 */
	if (mantissa == 0.0 || exponent < -(LDBL_MAX_EXP + LDBL_MAX_EXP - 1 - 8)) {
		*buf = '\x80';
		return 1;
	}

	bool negative = (mantissa < 0);
	if (negative) mantissa = -mantissa;

	// Infinity, or extremely large non-IEEE representation.
	if (value > LDBL_MAX || exponent > LDBL_MAX_EXP + LDBL_MAX_EXP - 1 + 8) {
		if (negative) {
			// This can only happen with a non-IEEE representation, because
			// we've already tested for value < -LDBL_MAX
			return 0;
		} else {
			memset(buf, '\xff', 18);
			return 18;
		}
	}

	// Encoding:
	//
	// [ 7 | 6 | 5 | 4 3 2 1 0]
	//   Sm  Se  Le
	//
	// Sm stores the sign of the mantissa: 1 = positive or zero, 0 = negative.
	// Se stores the sign of the exponent: Sm for positive/zero, !Sm for neg.
	// Le stores the length of the exponent: !Se for 7 bits, Se for 15 bits.
	unsigned char next = (negative ? 0 : 0xe0);

	// Bias the exponent by 8 so that more small integers get short encodings.
	exponent -= 8;
	bool exponent_negative = (exponent < 0);
	if (exponent_negative) {
		exponent = -exponent;
		next ^= 0x60;
	}

	size_t len = 0;

	/* We store the exponent in 7 or 15 bits.  If the number is negative, we
	 * flip all the bits of the exponent, since larger negative numbers should
	 * sort first.
	 *
	 * If the exponent is negative, we flip the bits of the exponent, since
	 * larger negative exponents should sort first (unless the number is
	 * negative, in which case they should sort later).
	 */

	assert(exponent >= 0);
	if (exponent < 128) {
		next ^= 0x20;
		// Put the top 5 bits of the exponent into the lower 5 bits of the
		// first byte:
		next |= static_cast<unsigned char>(exponent >> 2);
		if (negative ^ exponent_negative) next ^= 0x1f;
		buf[len++] = next;

		// And the lower 2 bits of the exponent go into the upper 2 bits
		// of the second byte:
		next = static_cast<unsigned char>(exponent) << 6;
		if (negative ^ exponent_negative) next ^= 0xc0;

	} else {
		assert((exponent >> 15) == 0);
		// Put the top 5 bits of the exponent into the lower 5 bits of the
		// first byte:
		next |= static_cast<unsigned char>(exponent >> 10);
		if (negative ^ exponent_negative) next ^= 0x1f;
		buf[len++] = next;

		// Put the bits 3-10 of the exponent into the second byte:
		next = static_cast<unsigned char>(exponent >> 2);
		if (negative ^ exponent_negative) next ^= 0xff;
		buf[len++] = next;

		// And the lower 2 bits of the exponent go into the upper 2 bits
		// of the third byte:
		next = static_cast<unsigned char>(exponent) << 6;
		if (negative ^ exponent_negative) next ^= 0xc0;
	}

	// Convert the 112 (or 113) bits of the mantissa into two 32-bit words.

	mantissa *= (negative ? 1073741824.0 : 2147483648.0);  // 1<<30 : 1<<31
	unsigned word1 = static_cast<unsigned>(mantissa);
	mantissa -= word1;

	mantissa *= 4294967296.0L;  // 1<<32
	unsigned word2 = static_cast<unsigned>(mantissa);
	mantissa -= word2;

	mantissa *= 4294967296.0L;  // 1<<32
	unsigned word3 = static_cast<unsigned>(mantissa);
	mantissa -= word3;

	mantissa *= 4294967296.0L;  // 1<<32
	unsigned word4 = static_cast<unsigned>(mantissa);

	// If the number is positive, the first bit will always be set because 0.5
	// <= mantissa < 1, unless mantissa is zero, which we handle specially
	// above).  If the number is negative, we negate the mantissa instead of
	// flipping all the bits, so in the case of 0.5, the first bit isn't set
	// so we need to store it explicitly.  But for the cost of one extra
	// leading bit, we can save several trailing 0xff bytes in lots of common
	// cases.

	assert(negative || (word1 & (1<<30)));
	if (negative) {
		// We negate the mantissa for negative numbers, so that the sort order
		// is reversed (since larger negative numbers should come first).
		word1 = -word1;
		if (word2 != 0 || word3 != 0 || word4 != 0) ++word1;
		word2 = -word2;
		if (word3 != 0 || word4 != 0) ++word2;
		word3 = -word3;
		if (word4 != 0) ++word3;
		word4 = -word4;
	}

	word1 &= 0x3fffffff;
	next |= static_cast<unsigned char>(word1 >> 24);
	buf[len++] = next;

	if (word1 != 0) {
		buf[len++] = char(word1 >> 16);
		buf[len++] = char(word1 >> 8);
		buf[len++] = char(word1);
	}

	if (word2 != 0 || word3 != 0 || word4 != 0) {
		buf[len++] = char(word2 >> 24);
		buf[len++] = char(word2 >> 16);
		buf[len++] = char(word2 >> 8);
		buf[len++] = char(word2);
	}

	if (word3 != 0 || word4 != 0) {
		buf[len++] = char(word3 >> 24);
		buf[len++] = char(word3 >> 16);
		buf[len++] = char(word3 >> 8);
		buf[len++] = char(word3);
	}

	if (word4 != 0) {
		buf[len++] = char(word4 >> 24);
		buf[len++] = char(word4 >> 16);
		buf[len++] = char(word4 >> 8);
		buf[len++] = char(word4);
	}

	// Finally, we can chop off any trailing zero bytes.
	while (len > 0 && buf[len - 1] == '\0') {
		--len;
	}

	return len;
}

std::string sortable_serialise(long double value) {
	char buf[18];
	return std::string(buf, sortable_serialise_(value, buf));
}

/// Get a number from the character at a given position in a string, returning
/// 0 if the string isn't long enough.
static inline unsigned char
numfromstr(const std::string & str, std::string::size_type pos)
{
	return (pos < str.size()) ? static_cast<unsigned char>(str[pos]) : '\0';
}

long double
sortable_unserialise(const std::string & value)
{
	// Zero.
	if (value.size() == 1 && value[0] == '\x80') return 0.0;

	// Positive infinity.
	if (value.size() == 18 &&
		memcmp(value.data(), "\xff\xff\xff\xff\xff\xff\xff\xff\xff", 18) == 0) {
		// #ifdef INFINITY
		// 		// INFINITY is C99.  Oddly, it's of type "float" so sanity check in
		// 		// case it doesn't cast to double as infinity (apparently some
		// 		// implementations have this problem).
		// 		if (long double(INFINITY) > HUGE_VAL) return INFINITY;
		// #endif
		return HUGE_VAL;
	}

	// Negative infinity.
	if (value.empty()) {
//		#ifdef INFINITY
//				if (long double(INFINITY) > HUGE_VAL) return -INFINITY;
//		#endif
		return -HUGE_VAL;
	}

	unsigned char first = numfromstr(value, 0);
	size_t i = 0;

	first ^= static_cast<unsigned char>(first & 0xc0) >> 1;
	bool negative = !(first & 0x80);
	bool exponent_negative = (first & 0x40);
	bool explen = !(first & 0x20);
	int exponent = first & 0x1f;
	if (!explen) {
		first = numfromstr(value, ++i);
		exponent <<= 2;
		exponent |= (first >> 6);
		if (negative ^ exponent_negative) exponent ^= 0x07f;
	} else {
		first = numfromstr(value, ++i);
		exponent <<= 8;
		exponent |= first;
		first = numfromstr(value, ++i);
		exponent <<= 2;
		exponent |= (first >> 6);
		if (negative ^ exponent_negative) exponent ^= 0x7fff;
	}

	unsigned word1;
	word1 = (unsigned(first & 0x3f) << 24);
	word1 |= numfromstr(value, ++i) << 16;
	word1 |= numfromstr(value, ++i) << 8;
	word1 |= numfromstr(value, ++i);

	unsigned word2 = 0;
	if (i < value.size()) {
		word2 = numfromstr(value, ++i) << 24;
		word2 |= numfromstr(value, ++i) << 16;
		word2 |= numfromstr(value, ++i) << 8;
		word2 |= numfromstr(value, ++i);
	}

	unsigned word3 = 0;
	if (i < value.size()) {
		word3 = numfromstr(value, ++i) << 24;
		word3 |= numfromstr(value, ++i) << 16;
		word3 |= numfromstr(value, ++i) << 8;
		word3 |= numfromstr(value, ++i);
	}

	unsigned word4 = 0;
	if (i < value.size()) {
		word4 = numfromstr(value, ++i) << 24;
		word4 |= numfromstr(value, ++i) << 16;
		word4 |= numfromstr(value, ++i) << 8;
		word4 |= numfromstr(value, ++i);
	}

	if (negative) {
		word1 = -word1;
		if (word2 != 0 || word3 != 0 || word4 != 0) ++word1;
		word2 = -word2;
		if (word3 != 0 || word4 != 0) ++word2;
		word3 = -word3;
		if (word4 != 0) ++word3;
		word4 = -word4;
		assert((word1 & 0xf0000000) != 0);
		word1 &= 0x3fffffff;
	}
	if (!negative) word1 |= 1<<30;

	long double mantissa = 0;
	if (word4) mantissa += word4 / 79228162514264337593543950336.0L; // 1<<96
	if (word3) mantissa += word3 / 18446744073709551616.0L; // 1<<64
	if (word2) mantissa += word2 / 4294967296.0L; // 1<<32
	if (word1) mantissa += word1;
	mantissa /= (negative ? 1073741824.0 : 2147483648.0);  // 1<<30 : 1<<31

	if (exponent_negative) exponent = -exponent;
	exponent += 8;

	if (negative) mantissa = -mantissa;

	// We use scalbnl() since it's equivalent to ldexp() when FLT_RADIX == 2
	// (which we currently assume), except that ldexp() will set errno if the
	// result overflows or underflows, which isn't really desirable here.
	return scalbnl(mantissa, exponent);
}
<|endoftext|>


<commit_before>// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
// Copyright (C) 2010 Jitse Niesen <jitse@maths.leeds.ac.uk>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen 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 GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#include "main.h"
#include <limits>
#include <Eigen/Eigenvalues>

#ifdef HAS_GSL
#include "gsl_helper.h"
#endif

template<typename MatrixType> void selfadjointeigensolver(const MatrixType& m)
{
  /* this test covers the following files:
     EigenSolver.h, SelfAdjointEigenSolver.h (and indirectly: Tridiagonalization.h)
  */
  int rows = m.rows();
  int cols = m.cols();

  typedef typename MatrixType::Scalar Scalar;
  typedef typename NumTraits<Scalar>::Real RealScalar;
  typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType;
  typedef Matrix<RealScalar, MatrixType::RowsAtCompileTime, 1> RealVectorType;
  typedef typename std::complex<typename NumTraits<typename MatrixType::Scalar>::Real> Complex;

  RealScalar largerEps = 10*test_precision<RealScalar>();

  MatrixType a = MatrixType::Random(rows,cols);
  MatrixType a1 = MatrixType::Random(rows,cols);
  MatrixType symmA =  a.adjoint() * a + a1.adjoint() * a1;

  MatrixType b = MatrixType::Random(rows,cols);
  MatrixType b1 = MatrixType::Random(rows,cols);
  MatrixType symmB = b.adjoint() * b + b1.adjoint() * b1;

  SelfAdjointEigenSolver<MatrixType> eiSymm(symmA);
  // generalized eigen pb
  SelfAdjointEigenSolver<MatrixType> eiSymmGen(symmA, symmB);

  #ifdef HAS_GSL
  if (ei_is_same_type<RealScalar,double>::ret)
  {
    typedef GslTraits<Scalar> Gsl;
    typename Gsl::Matrix gEvec=0, gSymmA=0, gSymmB=0;
    typename GslTraits<RealScalar>::Vector gEval=0;
    RealVectorType _eval;
    MatrixType _evec;
    convert<MatrixType>(symmA, gSymmA);
    convert<MatrixType>(symmB, gSymmB);
    convert<MatrixType>(symmA, gEvec);
    gEval = GslTraits<RealScalar>::createVector(rows);

    Gsl::eigen_symm(gSymmA, gEval, gEvec);
    convert(gEval, _eval);
    convert(gEvec, _evec);

    // test gsl itself !
    VERIFY((symmA * _evec).isApprox(_evec * _eval.asDiagonal(), largerEps));

    // compare with eigen
    VERIFY_IS_APPROX(_eval, eiSymm.eigenvalues());
    VERIFY_IS_APPROX(_evec.cwiseAbs(), eiSymm.eigenvectors().cwiseAbs());

    // generalized pb
    Gsl::eigen_symm_gen(gSymmA, gSymmB, gEval, gEvec);
    convert(gEval, _eval);
    convert(gEvec, _evec);
    // test GSL itself:
    VERIFY((symmA * _evec).isApprox(symmB * (_evec * _eval.asDiagonal()), largerEps));

    // compare with eigen
//     std::cerr << _eval.transpose() << "\n" << eiSymmGen.eigenvalues().transpose() << "\n\n";
//     std::cerr << _evec.format(6) << "\n\n" << eiSymmGen.eigenvectors().format(6) << "\n\n\n";
    VERIFY_IS_APPROX(_eval, eiSymmGen.eigenvalues());
    VERIFY_IS_APPROX(_evec.cwiseAbs(), eiSymmGen.eigenvectors().cwiseAbs());

    Gsl::free(gSymmA);
    Gsl::free(gSymmB);
    GslTraits<RealScalar>::free(gEval);
    Gsl::free(gEvec);
  }
  #endif

  VERIFY_IS_EQUAL(eiSymm.info(), Success);
  VERIFY((symmA * eiSymm.eigenvectors()).isApprox(
          eiSymm.eigenvectors() * eiSymm.eigenvalues().asDiagonal(), largerEps));
  VERIFY_IS_APPROX(symmA.template selfadjointView<Lower>().eigenvalues(), eiSymm.eigenvalues());

  SelfAdjointEigenSolver<MatrixType> eiSymmNoEivecs(symmA, false);
  VERIFY_IS_EQUAL(eiSymmNoEivecs.info(), Success);
  VERIFY_IS_APPROX(eiSymm.eigenvalues(), eiSymmNoEivecs.eigenvalues());

  // generalized eigen problem Ax = lBx
  VERIFY_IS_EQUAL(eiSymmGen.info(), Success);
  VERIFY((symmA * eiSymmGen.eigenvectors()).isApprox(
          symmB * (eiSymmGen.eigenvectors() * eiSymmGen.eigenvalues().asDiagonal()), largerEps));

  MatrixType sqrtSymmA = eiSymm.operatorSqrt();
  VERIFY_IS_APPROX(symmA, sqrtSymmA*sqrtSymmA);
  VERIFY_IS_APPROX(sqrtSymmA, symmA*eiSymm.operatorInverseSqrt());

  MatrixType id = MatrixType::Identity(rows, cols);
  VERIFY_IS_APPROX(id.template selfadjointView<Lower>().operatorNorm(), RealScalar(1));

  SelfAdjointEigenSolver<MatrixType> eiSymmUninitialized;
  VERIFY_RAISES_ASSERT(eiSymmUninitialized.info());
  VERIFY_RAISES_ASSERT(eiSymmUninitialized.eigenvalues());
  VERIFY_RAISES_ASSERT(eiSymmUninitialized.eigenvectors());
  VERIFY_RAISES_ASSERT(eiSymmUninitialized.operatorSqrt());
  VERIFY_RAISES_ASSERT(eiSymmUninitialized.operatorInverseSqrt());

  eiSymmUninitialized.compute(symmA, false);
  VERIFY_RAISES_ASSERT(eiSymmUninitialized.eigenvectors());
  VERIFY_RAISES_ASSERT(eiSymmUninitialized.operatorSqrt());
  VERIFY_RAISES_ASSERT(eiSymmUninitialized.operatorInverseSqrt());

  if (rows > 1)
  {
    // Test matrix with NaN
    symmA(0,0) = std::numeric_limits<typename MatrixType::RealScalar>::quiet_NaN();
    SelfAdjointEigenSolver<MatrixType> eiSymmNaN(symmA);
    VERIFY_IS_EQUAL(eiSymmNaN.info(), NoConvergence);
  }
}

void test_eigensolver_selfadjoint()
{
  for(int i = 0; i < g_repeat; i++) {
    // very important to test a 3x3 matrix since we provide a special path for it
    CALL_SUBTEST_1( selfadjointeigensolver(Matrix3f()) );
    CALL_SUBTEST_2( selfadjointeigensolver(Matrix4d()) );
    CALL_SUBTEST_3( selfadjointeigensolver(MatrixXf(10,10)) );
    CALL_SUBTEST_4( selfadjointeigensolver(MatrixXd(19,19)) );
    CALL_SUBTEST_5( selfadjointeigensolver(MatrixXcd(17,17)) );

    // some trivial but implementation-wise tricky cases
    CALL_SUBTEST_4( selfadjointeigensolver(MatrixXd(1,1)) );
    CALL_SUBTEST_4( selfadjointeigensolver(MatrixXd(2,2)) );
    CALL_SUBTEST_6( selfadjointeigensolver(Matrix<double,1,1>()) );
    CALL_SUBTEST_7( selfadjointeigensolver(Matrix<double,2,2>()) );
  }

  // Test problem size constructors
  CALL_SUBTEST_8(SelfAdjointEigenSolver<MatrixXf>(10));
  CALL_SUBTEST_8(Tridiagonalization<MatrixXf>(10));
}

<commit_msg>fix unit test when GSL is enabled<commit_after>// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
// Copyright (C) 2010 Jitse Niesen <jitse@maths.leeds.ac.uk>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen 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 GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#include "main.h"
#include <limits>
#include <Eigen/Eigenvalues>

#ifdef HAS_GSL
#include "gsl_helper.h"
#endif

template<typename MatrixType> void selfadjointeigensolver(const MatrixType& m)
{
  /* this test covers the following files:
     EigenSolver.h, SelfAdjointEigenSolver.h (and indirectly: Tridiagonalization.h)
  */
  int rows = m.rows();
  int cols = m.cols();

  typedef typename MatrixType::Scalar Scalar;
  typedef typename NumTraits<Scalar>::Real RealScalar;
  typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType;
  typedef Matrix<RealScalar, MatrixType::RowsAtCompileTime, 1> RealVectorType;
  typedef typename std::complex<typename NumTraits<typename MatrixType::Scalar>::Real> Complex;

  RealScalar largerEps = 10*test_precision<RealScalar>();

  MatrixType a = MatrixType::Random(rows,cols);
  MatrixType a1 = MatrixType::Random(rows,cols);
  MatrixType symmA =  a.adjoint() * a + a1.adjoint() * a1;

  MatrixType b = MatrixType::Random(rows,cols);
  MatrixType b1 = MatrixType::Random(rows,cols);
  MatrixType symmB = b.adjoint() * b + b1.adjoint() * b1;

  SelfAdjointEigenSolver<MatrixType> eiSymm(symmA);
  // generalized eigen pb
  SelfAdjointEigenSolver<MatrixType> eiSymmGen(symmA, symmB);

  #ifdef HAS_GSL
  if (ei_is_same_type<RealScalar,double>::ret)
  {
    typedef GslTraits<Scalar> Gsl;
    typename Gsl::Matrix gEvec=0, gSymmA=0, gSymmB=0;
    typename GslTraits<RealScalar>::Vector gEval=0;
    RealVectorType _eval;
    MatrixType _evec;
    convert<MatrixType>(symmA, gSymmA);
    convert<MatrixType>(symmB, gSymmB);
    convert<MatrixType>(symmA, gEvec);
    gEval = GslTraits<RealScalar>::createVector(rows);

    Gsl::eigen_symm(gSymmA, gEval, gEvec);
    convert(gEval, _eval);
    convert(gEvec, _evec);

    // test gsl itself !
    VERIFY((symmA * _evec).isApprox(_evec * _eval.asDiagonal(), largerEps));

    // compare with eigen
    VERIFY_IS_APPROX(_eval, eiSymm.eigenvalues());
    VERIFY_IS_APPROX(_evec.cwiseAbs(), eiSymm.eigenvectors().cwiseAbs());

    // generalized pb
    Gsl::eigen_symm_gen(gSymmA, gSymmB, gEval, gEvec);
    convert(gEval, _eval);
    convert(gEvec, _evec);
    // test GSL itself:
    VERIFY((symmA * _evec).isApprox(symmB * (_evec * _eval.asDiagonal()), largerEps));

    // compare with eigen
    MatrixType normalized_eivec = eiSymmGen.eigenvectors()*eiSymmGen.eigenvectors().colwise().norm().asDiagonal().inverse();
    VERIFY_IS_APPROX(_eval, eiSymmGen.eigenvalues());
    VERIFY_IS_APPROX(_evec.cwiseAbs(), normalized_eivec.cwiseAbs());

    Gsl::free(gSymmA);
    Gsl::free(gSymmB);
    GslTraits<RealScalar>::free(gEval);
    Gsl::free(gEvec);
  }
  #endif

  VERIFY_IS_EQUAL(eiSymm.info(), Success);
  VERIFY((symmA * eiSymm.eigenvectors()).isApprox(
          eiSymm.eigenvectors() * eiSymm.eigenvalues().asDiagonal(), largerEps));
  VERIFY_IS_APPROX(symmA.template selfadjointView<Lower>().eigenvalues(), eiSymm.eigenvalues());

  SelfAdjointEigenSolver<MatrixType> eiSymmNoEivecs(symmA, false);
  VERIFY_IS_EQUAL(eiSymmNoEivecs.info(), Success);
  VERIFY_IS_APPROX(eiSymm.eigenvalues(), eiSymmNoEivecs.eigenvalues());

  // generalized eigen problem Ax = lBx
  VERIFY_IS_EQUAL(eiSymmGen.info(), Success);
  VERIFY((symmA * eiSymmGen.eigenvectors()).isApprox(
          symmB * (eiSymmGen.eigenvectors() * eiSymmGen.eigenvalues().asDiagonal()), largerEps));

  MatrixType sqrtSymmA = eiSymm.operatorSqrt();
  VERIFY_IS_APPROX(symmA, sqrtSymmA*sqrtSymmA);
  VERIFY_IS_APPROX(sqrtSymmA, symmA*eiSymm.operatorInverseSqrt());

  MatrixType id = MatrixType::Identity(rows, cols);
  VERIFY_IS_APPROX(id.template selfadjointView<Lower>().operatorNorm(), RealScalar(1));

  SelfAdjointEigenSolver<MatrixType> eiSymmUninitialized;
  VERIFY_RAISES_ASSERT(eiSymmUninitialized.info());
  VERIFY_RAISES_ASSERT(eiSymmUninitialized.eigenvalues());
  VERIFY_RAISES_ASSERT(eiSymmUninitialized.eigenvectors());
  VERIFY_RAISES_ASSERT(eiSymmUninitialized.operatorSqrt());
  VERIFY_RAISES_ASSERT(eiSymmUninitialized.operatorInverseSqrt());

  eiSymmUninitialized.compute(symmA, false);
  VERIFY_RAISES_ASSERT(eiSymmUninitialized.eigenvectors());
  VERIFY_RAISES_ASSERT(eiSymmUninitialized.operatorSqrt());
  VERIFY_RAISES_ASSERT(eiSymmUninitialized.operatorInverseSqrt());

  if (rows > 1)
  {
    // Test matrix with NaN
    symmA(0,0) = std::numeric_limits<typename MatrixType::RealScalar>::quiet_NaN();
    SelfAdjointEigenSolver<MatrixType> eiSymmNaN(symmA);
    VERIFY_IS_EQUAL(eiSymmNaN.info(), NoConvergence);
  }
}

void test_eigensolver_selfadjoint()
{
  for(int i = 0; i < g_repeat; i++) {
    // very important to test a 3x3 matrix since we provide a special path for it
    CALL_SUBTEST_1( selfadjointeigensolver(Matrix3f()) );
    CALL_SUBTEST_2( selfadjointeigensolver(Matrix4d()) );
    CALL_SUBTEST_3( selfadjointeigensolver(MatrixXf(10,10)) );
    CALL_SUBTEST_4( selfadjointeigensolver(MatrixXd(19,19)) );
    CALL_SUBTEST_5( selfadjointeigensolver(MatrixXcd(17,17)) );

    // some trivial but implementation-wise tricky cases
    CALL_SUBTEST_4( selfadjointeigensolver(MatrixXd(1,1)) );
    CALL_SUBTEST_4( selfadjointeigensolver(MatrixXd(2,2)) );
    CALL_SUBTEST_6( selfadjointeigensolver(Matrix<double,1,1>()) );
    CALL_SUBTEST_7( selfadjointeigensolver(Matrix<double,2,2>()) );
  }

  // Test problem size constructors
  CALL_SUBTEST_8(SelfAdjointEigenSolver<MatrixXf>(10));
  CALL_SUBTEST_8(Tridiagonalization<MatrixXf>(10));
}

<|endoftext|>


<commit_before>/******************************************************************************
The MIT License(MIT)

Embedded Template Library.
https://github.com/ETLCPP/etl
https://www.etlcpp.com

Copyright(c) 2020 jwellbelove

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files(the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and / or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions :

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
******************************************************************************/

#include "UnitTest++/UnitTest++.h"

#include <thread>
#include <vector>
#include <numeric>
#include <array>
#include <algorithm>
#include <queue>
#include <atomic>

#include "etl/atomic.h"
#include "etl/queue_spsc_atomic.h"
#include "etl/buffer_descriptors.h"

#if defined(ETL_TARGET_OS_WINDOWS)
#include <Windows.h>
#endif

#define REALTIME_TEST 0

namespace
{
  constexpr size_t BUFFER_SIZE = 16U;
  constexpr size_t N_BUFFERS = 4U;
  constexpr size_t DATA_COUNT = BUFFER_SIZE / 2;

  using BD = etl::buffer_descriptors<char, BUFFER_SIZE, N_BUFFERS, std::atomic_char>;

  char buffers[N_BUFFERS][BUFFER_SIZE];

  //***********************************
  struct Receiver
  {
    void receive(BD::notification n)
    {
      pbuffer = n.get_descriptor().data();
      count = n.get_count();
    }

    void clear()
    {
      pbuffer = nullptr;
      count = 0U;
    }

    BD::pointer   pbuffer;
    BD::size_type count;
  };

  Receiver receiver;

  SUITE(test_buffer_descriptors)
  {
    ////*************************************************************************
    //TEST(test_constructor_plus_buffer)
    //{
    //  BD bd(&buffers[0][0]);

    //  CHECK_EQUAL(N_BUFFERS, bd.N_BUFFERS);
    //  CHECK_EQUAL(BUFFER_SIZE, bd.BUFFER_SIZE);
    //  CHECK(!bd.is_valid());
    //}

    ////*************************************************************************
    //TEST(test_constructor_plus_buffer_and_callback)
    //{
    //  receiver.clear();
    //  BD::callback_type callback = BD::callback_type::create<Receiver, &Receiver::receive>(receiver);

    //  BD bd(&buffers[0][0], callback);

    //  CHECK_EQUAL(N_BUFFERS, bd.N_BUFFERS);
    //  CHECK_EQUAL(BUFFER_SIZE, bd.BUFFER_SIZE);
    //  CHECK(bd.is_valid());
    //}

    ////*************************************************************************
    //TEST(test_constructor_plus_buffer_set_callback)
    //{
    //  receiver.clear();
    //  BD::callback_type callback = BD::callback_type::create<Receiver, &Receiver::receive>(receiver);

    //  BD bd(&buffers[0][0]);
    //  bd.set_callback(callback);

    //  CHECK_EQUAL(N_BUFFERS, bd.N_BUFFERS);
    //  CHECK_EQUAL(BUFFER_SIZE, bd.BUFFER_SIZE);
    //  CHECK(bd.is_valid());
    //}

    //*************************************************************************
    TEST(test_buffers)
    {
      BD bd(&buffers[0][0]);

      for (size_t i = 0U; i < N_BUFFERS; ++i)
      {
        BD::descriptor desc = bd.allocate();

        CHECK(desc.is_valid());

        //CHECK_EQUAL(BUFFER_SIZE, desc.max_size());

        char* a = &buffers[i][0];
        char* b = desc.data();

        //CHECK_EQUAL(a, b);
      }
    }
    //
    //    //*************************************************************************
    //    TEST(test_buffers_with_allocate_fill)
    //    {
    //      std::array<char, BUFFER_SIZE> test =
    //      {
    //        char(0xFF), char(0xFF), char(0xFF), char(0xFF), char(0xFF), char(0xFF), char(0xFF), char(0xFF),
    //        char(0xFF), char(0xFF), char(0xFF), char(0xFF), char(0xFF), char(0xFF), char(0xFF), char(0xFF)
    //      };
    //
    //      BD bd(&buffers[0][0]);
    //
    //      for (size_t i = 0U; i < N_BUFFERS; ++i)
    //      {
    //        BD::descriptor desc = bd.allocate(char(0xFF));
    //
    //        CHECK_EQUAL(BUFFER_SIZE, desc.max_size());
    //        CHECK_EQUAL(uintptr_t(&buffers[i][0]), uintptr_t(desc.data()));
    //        CHECK_ARRAY_EQUAL(test.data(), desc.data(), BUFFER_SIZE);
    //      }
    //    }
    //
    //    //*************************************************************************
    //    TEST(test_notifications)
    //    {
    //      std::array<char, BUFFER_SIZE> test = { 0, 1, 2, 3, 4, 5, 6, 7, 0, 0, 0, 0, 0, 0, 0, 0 };
    //
    //      std::fill(&buffers[0][0], &buffers[N_BUFFERS - 1][0] + BUFFER_SIZE , 0U);
    //
    //      receiver.clear();
    //      BD::callback_type callback = BD::callback_type::create<Receiver, &Receiver::receive>(receiver);
    //
    //      BD bd(&buffers[0][0], callback);
    //
    //      for (size_t i = 0U; i < N_BUFFERS; ++i)
    //      {
    //        BD::descriptor desc = bd.allocate();
    //
    //        CHECK(desc.is_valid());
    //
    //        std::copy(test.begin(), test.begin() + DATA_COUNT, desc.data());
    //        bd.notify(BD::notification(desc, DATA_COUNT));
    //        desc.release();
    //
    //        CHECK_EQUAL(DATA_COUNT, receiver.count);
    //        CHECK_EQUAL(uintptr_t(&buffers[i][0]), uintptr_t(receiver.pbuffer));
    //        CHECK_ARRAY_EQUAL(test.data(), desc.data(), BUFFER_SIZE);
    //      }
    //    }
    //
    //    //*************************************************************************
    //    TEST(test_allocate_overflow)
    //    {
    //      BD bd(&buffers[0][0]);
    //
    //      // Use up all of the descriptors.
    //      for (size_t i = 0U; i < N_BUFFERS; ++i)
    //      {
    //        BD::descriptor desc = bd.allocate();
    //        CHECK(desc.is_valid());
    //      }
    //
    //      BD::descriptor desc = bd.allocate();
    //      CHECK(!desc.is_valid());
    //    }
    //
    //    //*************************************************************************
    //    TEST(test_allocate_release_rollover)
    //    {
    //      std::queue<BD::descriptor> desc_queue;
    //
    //      BD bd(&buffers[0][0]);
    //
    //      // Use up all of the descriptors, then release/allocate for the rest.
    //      for (size_t i = 0U; i < (N_BUFFERS * 2); ++i)
    //      {
    //        BD::descriptor desc = bd.allocate();
    //        desc_queue.push(desc);
    //
    //        CHECK(desc.is_valid());
    //
    //        if (i >= (N_BUFFERS - 1))
    //        {
    //          desc_queue.front().release();
    //          desc_queue.pop();
    //        }
    //      }
    //    }
    //
    //    //*************************************************************************
    //    TEST(test_descriptors)
    //    {
    //      BD bd(&buffers[0][0]);
    //
    //      BD::descriptor desc1 = bd.allocate();
    //      BD::descriptor desc2 = bd.allocate();
    //      BD::descriptor desc3 = bd.allocate();
    //      BD::descriptor desc4 = bd.allocate();
    //
    //      CHECK(desc1.is_allocated());
    //      CHECK(desc2.is_allocated());
    //      CHECK(desc3.is_allocated());
    //      CHECK(desc4.is_allocated());
    //
    //      CHECK(desc1.data() == &buffers[0][0]);
    //      CHECK(desc2.data() == &buffers[1][0]);
    //      CHECK(desc3.data() == &buffers[2][0]);
    //      CHECK(desc4.data() == &buffers[3][0]);
    //
    //      CHECK(desc1.max_size() == BUFFER_SIZE);
    //      CHECK(desc2.max_size() == BUFFER_SIZE);
    //      CHECK(desc3.max_size() == BUFFER_SIZE);
    //      CHECK(desc4.max_size() == BUFFER_SIZE);
    //
    //      CHECK(desc1.MAX_SIZE == BUFFER_SIZE);
    //      CHECK(desc2.MAX_SIZE == BUFFER_SIZE);
    //      CHECK(desc3.MAX_SIZE == BUFFER_SIZE);
    //      CHECK(desc4.MAX_SIZE == BUFFER_SIZE);
    //
    //      CHECK(desc1.is_valid());
    //      CHECK(desc2.is_valid());
    //      CHECK(desc3.is_valid());
    //      CHECK(desc4.is_valid());
    //
    //      desc1.release();
    //      desc2.release();
    //      desc3.release();
    //      desc4.release();
    //
    //      CHECK(desc1.is_released());
    //      CHECK(desc2.is_released());
    //      CHECK(desc3.is_released());
    //      CHECK(desc4.is_released());
    //    }
    //
    //    //*************************************************************************
    //#if REALTIME_TEST
    //
    //#if defined(ETL_TARGET_OS_WINDOWS) // Only Windows priority is currently supported
    //#define RAISE_THREAD_PRIORITY  SetThreadPriority(GetCurrentThread(), THREAD_PRIORITY_HIGHEST)
    //#define FIX_PROCESSOR_AFFINITY1 SetThreadAffinityMask(GetCurrentThread(), 1)
    //#define FIX_PROCESSOR_AFFINITY2 SetThreadAffinityMask(GetCurrentThread(), 2)
    //#else
    //#define RAISE_THREAD_PRIORITY
    //#define FIX_PROCESSOR_AFFINITY1
    //#define FIX_PROCESSOR_AFFINITY2
    //#endif
    //
    //    std::atomic_bool start = false;
    //
    //    //*********************************
    //    struct Notification
    //    {
    //      BD::descriptor    desc;
    //      BD::size_type count;
    //    };
    //
    //    constexpr int N_ITERATIONS = 1000000;
    //
    //    etl::queue_spsc_atomic<BD::notification, N_ITERATIONS + 100> desc_queue;
    //
    //    //*********************************
    //    void Callback(BD::notification n)
    //    {
    //      desc_queue.push(n);
    //    }
    //
    //    //*********************************
    //    void Producer()
    //    {
    //      static char buffers[N_BUFFERS][BUFFER_SIZE];
    //
    //      BD bd(&buffers[0][0], BD::callback_type::create<Callback>());
    //
    //      RAISE_THREAD_PRIORITY;
    //      FIX_PROCESSOR_AFFINITY1;
    //
    //      // Wait for the start flag.
    //      while (!start);
    //
    //      int errors = 0;
    //
    //      for (int i = 0; i < N_ITERATIONS; ++i)
    //      {
    //        BD::descriptor desc;
    //
    //        // Wait until we can allocate a descriptor.
    //        do
    //        {
    //          desc = bd.allocate();
    //        } while (desc.is_valid() == false);
    //
    //        if (!desc.is_allocated())
    //        {
    //          ++errors;
    //        }
    //
    //        // Send a notification to the callback function.
    //        bd.notify(BD::notification(desc, BUFFER_SIZE));
    //      }
    //
    //      CHECK_EQUAL(0, errors);
    //    }
    //
    //    //*********************************
    //    void Consumer()
    //    {
    //      RAISE_THREAD_PRIORITY;
    //      FIX_PROCESSOR_AFFINITY2;
    //
    //      // Wait for the start flag.
    //      while (!start);
    //
    //      int errors = 0;
    //
    //      for (int i = 0; i < N_ITERATIONS;)
    //      {
    //        BD::notification notification;
    //
    //        // Try to get a notification from the queue.
    //        if (desc_queue.pop(notification))
    //        {
    //          CHECK_EQUAL(BUFFER_SIZE, notification.get_count());
    //          CHECK(notification.get_descriptor().is_allocated());
    //
    //          if (!notification.get_descriptor().is_allocated())
    //          {
    //            ++errors;
    //          }
    //
    //          notification.get_descriptor().release();
    //          ++i;
    //        }
    //
    //        CHECK_EQUAL(0, errors);
    //      }
    //    }
    //
    //    //*********************************
    //    TEST(test_multi_thread)
    //    {
    //      std::thread t1(Producer);
    //      std::thread t2(Consumer);
    //
    //      start = true;
    //
    //      t1.join();
    //      t2.join();
    //    }
    //#endif
  };
}
<commit_msg>Refactor buffer_descriptors test<commit_after>/******************************************************************************
The MIT License(MIT)

Embedded Template Library.
https://github.com/ETLCPP/etl
https://www.etlcpp.com

Copyright(c) 2020 jwellbelove

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files(the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and / or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions :

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
******************************************************************************/

#include "UnitTest++/UnitTest++.h"

#include <thread>
#include <vector>
#include <numeric>
#include <array>
#include <algorithm>
#include <queue>
#include <atomic>

#include "etl/atomic.h"
#include "etl/queue_spsc_atomic.h"
#include "etl/buffer_descriptors.h"

#if defined(ETL_TARGET_OS_WINDOWS)
#include <Windows.h>
#endif

#define REALTIME_TEST 0

namespace
{
  constexpr size_t BUFFER_SIZE = 16U;
  constexpr size_t N_BUFFERS = 4U;
  constexpr size_t DATA_COUNT = BUFFER_SIZE / 2;

  using BD = etl::buffer_descriptors<char, BUFFER_SIZE, N_BUFFERS, std::atomic_char>;

  char buffers[N_BUFFERS][BUFFER_SIZE];

  //***********************************
  struct Receiver
  {
    void receive(BD::notification n)
    {
      pbuffer = n.get_descriptor().data();
      count = n.get_count();
    }

    void clear()
    {
      pbuffer = nullptr;
      count = 0U;
    }

    BD::pointer   pbuffer;
    BD::size_type count;
  };

  Receiver receiver;

  SUITE(test_buffer_descriptors)
  {
    ////*************************************************************************
    //TEST(test_constructor_plus_buffer)
    //{
    //  BD bd(&buffers[0][0]);

    //  CHECK_EQUAL(N_BUFFERS, bd.N_BUFFERS);
    //  CHECK_EQUAL(BUFFER_SIZE, bd.BUFFER_SIZE);
    //  CHECK(!bd.is_valid());
    //}

    ////*************************************************************************
    //TEST(test_constructor_plus_buffer_and_callback)
    //{
    //  receiver.clear();
    //  BD::callback_type callback = BD::callback_type::create<Receiver, &Receiver::receive>(receiver);

    //  BD bd(&buffers[0][0], callback);

    //  CHECK_EQUAL(N_BUFFERS, bd.N_BUFFERS);
    //  CHECK_EQUAL(BUFFER_SIZE, bd.BUFFER_SIZE);
    //  CHECK(bd.is_valid());
    //}

    ////*************************************************************************
    //TEST(test_constructor_plus_buffer_set_callback)
    //{
    //  receiver.clear();
    //  BD::callback_type callback = BD::callback_type::create<Receiver, &Receiver::receive>(receiver);

    //  BD bd(&buffers[0][0]);
    //  bd.set_callback(callback);

    //  CHECK_EQUAL(N_BUFFERS, bd.N_BUFFERS);
    //  CHECK_EQUAL(BUFFER_SIZE, bd.BUFFER_SIZE);
    //  CHECK(bd.is_valid());
    //}

    //*************************************************************************
    TEST(test_buffers)
    {
      BD bd(&buffers[0][0]);

      for (size_t i = 0U; i < N_BUFFERS; ++i)
      {
        BD::descriptor desc = bd.allocate();

        CHECK(desc.is_valid());
        if (!desc.is_valid())
        {
          CHECK_EQUAL(100000, i);
        }

        //CHECK_EQUAL(BUFFER_SIZE, desc.max_size());

        char* a = &buffers[i][0];
        char* b = desc.data();

        //CHECK_EQUAL(a, b);
      }
    }
    //
    //    //*************************************************************************
    //    TEST(test_buffers_with_allocate_fill)
    //    {
    //      std::array<char, BUFFER_SIZE> test =
    //      {
    //        char(0xFF), char(0xFF), char(0xFF), char(0xFF), char(0xFF), char(0xFF), char(0xFF), char(0xFF),
    //        char(0xFF), char(0xFF), char(0xFF), char(0xFF), char(0xFF), char(0xFF), char(0xFF), char(0xFF)
    //      };
    //
    //      BD bd(&buffers[0][0]);
    //
    //      for (size_t i = 0U; i < N_BUFFERS; ++i)
    //      {
    //        BD::descriptor desc = bd.allocate(char(0xFF));
    //
    //        CHECK_EQUAL(BUFFER_SIZE, desc.max_size());
    //        CHECK_EQUAL(uintptr_t(&buffers[i][0]), uintptr_t(desc.data()));
    //        CHECK_ARRAY_EQUAL(test.data(), desc.data(), BUFFER_SIZE);
    //      }
    //    }
    //
    //    //*************************************************************************
    //    TEST(test_notifications)
    //    {
    //      std::array<char, BUFFER_SIZE> test = { 0, 1, 2, 3, 4, 5, 6, 7, 0, 0, 0, 0, 0, 0, 0, 0 };
    //
    //      std::fill(&buffers[0][0], &buffers[N_BUFFERS - 1][0] + BUFFER_SIZE , 0U);
    //
    //      receiver.clear();
    //      BD::callback_type callback = BD::callback_type::create<Receiver, &Receiver::receive>(receiver);
    //
    //      BD bd(&buffers[0][0], callback);
    //
    //      for (size_t i = 0U; i < N_BUFFERS; ++i)
    //      {
    //        BD::descriptor desc = bd.allocate();
    //
    //        CHECK(desc.is_valid());
    //
    //        std::copy(test.begin(), test.begin() + DATA_COUNT, desc.data());
    //        bd.notify(BD::notification(desc, DATA_COUNT));
    //        desc.release();
    //
    //        CHECK_EQUAL(DATA_COUNT, receiver.count);
    //        CHECK_EQUAL(uintptr_t(&buffers[i][0]), uintptr_t(receiver.pbuffer));
    //        CHECK_ARRAY_EQUAL(test.data(), desc.data(), BUFFER_SIZE);
    //      }
    //    }
    //
    //    //*************************************************************************
    //    TEST(test_allocate_overflow)
    //    {
    //      BD bd(&buffers[0][0]);
    //
    //      // Use up all of the descriptors.
    //      for (size_t i = 0U; i < N_BUFFERS; ++i)
    //      {
    //        BD::descriptor desc = bd.allocate();
    //        CHECK(desc.is_valid());
    //      }
    //
    //      BD::descriptor desc = bd.allocate();
    //      CHECK(!desc.is_valid());
    //    }
    //
    //    //*************************************************************************
    //    TEST(test_allocate_release_rollover)
    //    {
    //      std::queue<BD::descriptor> desc_queue;
    //
    //      BD bd(&buffers[0][0]);
    //
    //      // Use up all of the descriptors, then release/allocate for the rest.
    //      for (size_t i = 0U; i < (N_BUFFERS * 2); ++i)
    //      {
    //        BD::descriptor desc = bd.allocate();
    //        desc_queue.push(desc);
    //
    //        CHECK(desc.is_valid());
    //
    //        if (i >= (N_BUFFERS - 1))
    //        {
    //          desc_queue.front().release();
    //          desc_queue.pop();
    //        }
    //      }
    //    }
    //
    //    //*************************************************************************
    //    TEST(test_descriptors)
    //    {
    //      BD bd(&buffers[0][0]);
    //
    //      BD::descriptor desc1 = bd.allocate();
    //      BD::descriptor desc2 = bd.allocate();
    //      BD::descriptor desc3 = bd.allocate();
    //      BD::descriptor desc4 = bd.allocate();
    //
    //      CHECK(desc1.is_allocated());
    //      CHECK(desc2.is_allocated());
    //      CHECK(desc3.is_allocated());
    //      CHECK(desc4.is_allocated());
    //
    //      CHECK(desc1.data() == &buffers[0][0]);
    //      CHECK(desc2.data() == &buffers[1][0]);
    //      CHECK(desc3.data() == &buffers[2][0]);
    //      CHECK(desc4.data() == &buffers[3][0]);
    //
    //      CHECK(desc1.max_size() == BUFFER_SIZE);
    //      CHECK(desc2.max_size() == BUFFER_SIZE);
    //      CHECK(desc3.max_size() == BUFFER_SIZE);
    //      CHECK(desc4.max_size() == BUFFER_SIZE);
    //
    //      CHECK(desc1.MAX_SIZE == BUFFER_SIZE);
    //      CHECK(desc2.MAX_SIZE == BUFFER_SIZE);
    //      CHECK(desc3.MAX_SIZE == BUFFER_SIZE);
    //      CHECK(desc4.MAX_SIZE == BUFFER_SIZE);
    //
    //      CHECK(desc1.is_valid());
    //      CHECK(desc2.is_valid());
    //      CHECK(desc3.is_valid());
    //      CHECK(desc4.is_valid());
    //
    //      desc1.release();
    //      desc2.release();
    //      desc3.release();
    //      desc4.release();
    //
    //      CHECK(desc1.is_released());
    //      CHECK(desc2.is_released());
    //      CHECK(desc3.is_released());
    //      CHECK(desc4.is_released());
    //    }
    //
    //    //*************************************************************************
    //#if REALTIME_TEST
    //
    //#if defined(ETL_TARGET_OS_WINDOWS) // Only Windows priority is currently supported
    //#define RAISE_THREAD_PRIORITY  SetThreadPriority(GetCurrentThread(), THREAD_PRIORITY_HIGHEST)
    //#define FIX_PROCESSOR_AFFINITY1 SetThreadAffinityMask(GetCurrentThread(), 1)
    //#define FIX_PROCESSOR_AFFINITY2 SetThreadAffinityMask(GetCurrentThread(), 2)
    //#else
    //#define RAISE_THREAD_PRIORITY
    //#define FIX_PROCESSOR_AFFINITY1
    //#define FIX_PROCESSOR_AFFINITY2
    //#endif
    //
    //    std::atomic_bool start = false;
    //
    //    //*********************************
    //    struct Notification
    //    {
    //      BD::descriptor    desc;
    //      BD::size_type count;
    //    };
    //
    //    constexpr int N_ITERATIONS = 1000000;
    //
    //    etl::queue_spsc_atomic<BD::notification, N_ITERATIONS + 100> desc_queue;
    //
    //    //*********************************
    //    void Callback(BD::notification n)
    //    {
    //      desc_queue.push(n);
    //    }
    //
    //    //*********************************
    //    void Producer()
    //    {
    //      static char buffers[N_BUFFERS][BUFFER_SIZE];
    //
    //      BD bd(&buffers[0][0], BD::callback_type::create<Callback>());
    //
    //      RAISE_THREAD_PRIORITY;
    //      FIX_PROCESSOR_AFFINITY1;
    //
    //      // Wait for the start flag.
    //      while (!start);
    //
    //      int errors = 0;
    //
    //      for (int i = 0; i < N_ITERATIONS; ++i)
    //      {
    //        BD::descriptor desc;
    //
    //        // Wait until we can allocate a descriptor.
    //        do
    //        {
    //          desc = bd.allocate();
    //        } while (desc.is_valid() == false);
    //
    //        if (!desc.is_allocated())
    //        {
    //          ++errors;
    //        }
    //
    //        // Send a notification to the callback function.
    //        bd.notify(BD::notification(desc, BUFFER_SIZE));
    //      }
    //
    //      CHECK_EQUAL(0, errors);
    //    }
    //
    //    //*********************************
    //    void Consumer()
    //    {
    //      RAISE_THREAD_PRIORITY;
    //      FIX_PROCESSOR_AFFINITY2;
    //
    //      // Wait for the start flag.
    //      while (!start);
    //
    //      int errors = 0;
    //
    //      for (int i = 0; i < N_ITERATIONS;)
    //      {
    //        BD::notification notification;
    //
    //        // Try to get a notification from the queue.
    //        if (desc_queue.pop(notification))
    //        {
    //          CHECK_EQUAL(BUFFER_SIZE, notification.get_count());
    //          CHECK(notification.get_descriptor().is_allocated());
    //
    //          if (!notification.get_descriptor().is_allocated())
    //          {
    //            ++errors;
    //          }
    //
    //          notification.get_descriptor().release();
    //          ++i;
    //        }
    //
    //        CHECK_EQUAL(0, errors);
    //      }
    //    }
    //
    //    //*********************************
    //    TEST(test_multi_thread)
    //    {
    //      std::thread t1(Producer);
    //      std::thread t2(Consumer);
    //
    //      start = true;
    //
    //      t1.join();
    //      t2.join();
    //    }
    //#endif
  };
}
<|endoftext|>


<commit_before>// ========================================================================== //
// This file is part of Sara, a basic set of libraries in C++ for computer
// vision.
//
// Copyright (C) 2019 David Ok <david.ok8@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License v. 2.0. If a copy of the MPL was not distributed with this file,
// you can obtain one at http://mozilla.org/MPL/2.0/.
// ========================================================================== //

#include <DO/Sara/Graphics.hpp>
#include <DO/Sara/ImageIO.hpp>
#include <DO/Sara/MultiViewGeometry/EpipolarGraph.hpp>
#include <DO/Sara/MultiViewGeometry/FeatureGraph.hpp>
#include <DO/Sara/MultiViewGeometry/Miscellaneous.hpp>

#include <DO/Sara/SfM/BuildingBlocks/EssentialMatrixEstimation.hpp>
#include <DO/Sara/SfM/BuildingBlocks/FundamentalMatrixEstimation.hpp>
#include <DO/Sara/SfM/BuildingBlocks/KeypointMatching.hpp>
#include <DO/Sara/SfM/BuildingBlocks/Triangulation.hpp>
#include <DO/Sara/SfM/Detectors/SIFT.hpp>

using namespace std;
using namespace DO::Sara;

using EEstimator = NisterFivePointAlgorithm;

// Detect or read SIFT keypoints.
auto get_keypoints(const Image<Rgb8>& image1,
                   const Image<Rgb8>& image2,
                   const std::string& keys1_filepath,
                   const std::string& keys2_filepath,
                   KeypointList<OERegion, float>& keys1,
                   KeypointList<OERegion, float>& keys2)
{
  print_stage("Computing/Reading keypoints");

#ifdef COMPUTE_KEYPOINTS
  keys1 = compute_sift_keypoints(image1.convert<float>());
  keys2 = compute_sift_keypoints(image2.convert<float>());
  cout << "Image 1: " << keys1.size() << " keypoints" << endl;
  cout << "Image 2: " << keys2.size() << " keypoints" << endl;

  const auto& [f1, d1] = keys1;
  const auto& [f2, d2] = keys2;

  write_keypoints(f1, d1, keys1_filepath);
  write_keypoints(f2, d2, keys2_filepath);
#else
  (void) image1;
  (void) image2;
  auto& [f1, d1] = keys1;
  auto& [f2, d2] = keys2;

  read_keypoints(f1, d1, keys1_filepath);
  read_keypoints(f2, d2, keys2_filepath);
#endif
}

GRAPHICS_MAIN()
{
  // Use the following data structure to load images, keypoints, camera
  // parameters.
  auto views = ViewAttributes{};

  // Load images.
  print_stage("Loading images...");
  const auto data_dir =
#ifdef __APPLE__
      std::string{"/Users/david/Desktop/Datasets/sfm/castle_int"};
#else
      std::string{"/home/david/Desktop/Datasets/sfm/castle_int"};
#endif
  views.image_paths = {
      data_dir + "/" + "0000.png",
      data_dir + "/" + "0001.png",
  };
  views.read_images();

  print_stage("Loading the internal camera matrices...");
  views.cameras.resize(2 /* views */);
  views.cameras[0].K =
      read_internal_camera_parameters(data_dir + "/" + "0000.png.K")
          .cast<double>();
  views.cameras[1].K =
      read_internal_camera_parameters(data_dir + "/" + "0001.png.K")
          .cast<double>();

  print_stage("Getting keypoints...");
  views.keypoints.resize(2 /* views */);
  get_keypoints(views.images[0], views.images[1],  //
                data_dir + "/" + "0000.key",       //
                data_dir + "/" + "0001.key",       //
                views.keypoints[0], views.keypoints[1]);

  // Use the following data structures to store the epipolar geometry data.
  auto epipolar_edges = EpipolarEdgeAttributes{};
  epipolar_edges.initialize_edges(2 /* views */);
  epipolar_edges.resize_fundamental_edge_list();
  epipolar_edges.resize_essential_edge_list();

  print_stage("Matching keypoints...");
  epipolar_edges.matches = {match(views.keypoints[0], views.keypoints[1])};
  const auto& matches = epipolar_edges.matches[0];

  print_stage("Performing data transformations...");
  // Invert the internal camera matrices.
  const auto K_inv = std::array<Matrix3d, 2>{views.cameras[0].K.inverse(),
                                             views.cameras[1].K.inverse()};
  // Tensors of image coordinates.
  const auto& f0 = features(views.keypoints[0]);
  const auto& f1 = features(views.keypoints[1]);
  const auto u = std::array{homogeneous(extract_centers(f0)).cast<double>(),
                            homogeneous(extract_centers(f1)).cast<double>()};
  // Tensors of camera coordinates.
  const auto un = std::array{apply_transform(K_inv[0], u[0]),
                             apply_transform(K_inv[1], u[1])};
  static_assert(std::is_same_v<decltype(un[0]), const Tensor_<double, 2>&>);
  // List the matches as a 2D-tensor where each row encodes a match 'm' as a
  // pair of point indices (i, j).
  const auto M = to_tensor(matches);

  print_stage("Estimating the essential matrix...");
  auto& E = epipolar_edges.E[0];
  auto& num_samples = epipolar_edges.E_num_samples[0];
  auto& err_thres = epipolar_edges.E_noise[0];
  auto& inliers = epipolar_edges.E_inliers[0];
  auto sample_best = Tensor_<int, 1>{};
  auto estimator = EEstimator{};
  auto distance = EpipolarDistance{};
  {
    num_samples = 1000;
    err_thres = 1e-3;
    std::tie(E, inliers, sample_best) =
        ransac(M, un[0], un[1], estimator, distance, num_samples, err_thres);
    E.matrix() = E.matrix().normalized();

    epipolar_edges.E_inliers[0] = inliers;
    epipolar_edges.E_best_samples[0] = sample_best;
  }

  // Calculate the fundamental matrix.
  print_stage("Computing the fundamental matrix...");
  auto& F = epipolar_edges.F[0];
  {
    F.matrix() = K_inv[1].transpose() * E.matrix() * K_inv[0];

    epipolar_edges.F_num_samples[0] = 1000;
    epipolar_edges.F_noise = epipolar_edges.E_noise;
    epipolar_edges.F_inliers = epipolar_edges.E_inliers;
    epipolar_edges.F_best_samples = epipolar_edges.E_best_samples;
  }

  // Extract the two-view geometry.
  print_stage("Estimating the two-view geometry...");
  epipolar_edges.two_view_geometries = {
      estimate_two_view_geometry(M, un[0], un[1], E, inliers, sample_best)
  };

  std::map<FeatureGID, int> from_2d_to_3d;

  // Populate the feature tracks.
  const auto [feature_graph, components] =
      populate_feature_tracks(views, epipolar_edges);

  // Keep feature tracks of size 2 at least.
  const auto feature_tracks = filter_feature_tracks(feature_graph, components);

  // Prepare the bundle adjustment problem formulation.
  //
  // 1. Count the number of 3D points.
  const auto num_points = static_cast<int>(feature_tracks.size());
  SARA_CHECK(num_points);

  // 2. Count the number of 2D observations.
  auto num_observations_per_points = std::vector<int>(num_points);
  std::transform(
      std::begin(feature_tracks), std::end(feature_tracks),
      std::begin(num_observations_per_points),
      [](const auto& track) { return static_cast<int>(track.size()); });

  const auto num_observations =
      std::accumulate(std::begin(num_observations_per_points),
                      std::end(num_observations_per_points), 0);
  SARA_CHECK(num_observations);

  // 3. Count the number of cameras, which should be equal to the number of
  //    images.
  auto image_ids = std::set<int>{};
  for (const auto& track : feature_tracks)
    for (const auto& f : track)
      image_ids.insert(f.image_id);

  const auto num_cameras = static_cast<int>(image_ids.size());
  SARA_CHECK(num_cameras);

  const auto num_parameters = 9 * num_cameras + 3 * num_points;
  SARA_CHECK(num_parameters);

  // 4. Transform the data for convenience.
  struct ObservationRef {
    FeatureGID gid;
    // TODO: needs the match_index;
    int camera_id;
    int point_id;
  };
  auto obs_refs = std::vector<ObservationRef>{};
  {
    obs_refs.reserve(num_observations);

    auto point_id = 0;
    for (const auto& track : feature_tracks)
    {
      for (const auto& f: track)
        obs_refs.push_back({f, f.image_id, point_id});
      ++point_id;
    }
  }

  // 5. Prepare the data for Ceres.
  auto observations = Tensor_<double, 2>{{num_observations, 2}};
  auto point_indices = std::vector<int>(num_observations);
  auto camera_indices = std::vector<int>(num_observations);
  auto parameters = std::vector<double>(num_parameters);
  for (int i = 0; i < num_observations; ++i)
  {
    const auto& ref = obs_refs[i];

    // Easy things first.
    point_indices[i] = ref.point_id;
    camera_indices[i] = ref.camera_id;

    // Initialize the 2D observations.
    const auto& image_id = ref.gid.image_id;
    const auto& local_id = ref.gid.local_id;
    const double x = un[image_id](local_id, 0);
    const double y = un[image_id](local_id, 1);
    observations(i, 0) = x;
    observations(i, 1) = y;

    // Initialize the 3D points.
    //parameters[9 * num_cameras + point_indices[i] + 0] =
  }

#ifdef SAVE_TWO_VIEW_GEOMETRY
  keep_cheiral_inliers_only(geometry, inliers);

  // Add the internal camera matrices to the camera.
  geometry.C1.K = K1;
  geometry.C2.K = K2;
  auto colors = extract_colors(image1, image2, geometry);
  save_to_hdf5(geometry, colors);
#endif

  // Inspect the fundamental matrix.
  print_stage("Inspecting the fundamental matrix estimation...");
  check_epipolar_constraints(views.images[0], views.images[1], F, matches,
                             sample_best, inliers,
                             /* display_step */ 20, /* wait_key */ true);

  return 0;
}
<commit_msg>WIP: bundle adjustment problem formulation in progress.<commit_after>// ========================================================================== //
// This file is part of Sara, a basic set of libraries in C++ for computer
// vision.
//
// Copyright (C) 2019 David Ok <david.ok8@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License v. 2.0. If a copy of the MPL was not distributed with this file,
// you can obtain one at http://mozilla.org/MPL/2.0/.
// ========================================================================== //

#include <DO/Sara/Graphics.hpp>
#include <DO/Sara/ImageIO.hpp>
#include <DO/Sara/MultiViewGeometry/EpipolarGraph.hpp>
#include <DO/Sara/MultiViewGeometry/FeatureGraph.hpp>
#include <DO/Sara/MultiViewGeometry/Miscellaneous.hpp>

#include <DO/Sara/SfM/BuildingBlocks/EssentialMatrixEstimation.hpp>
#include <DO/Sara/SfM/BuildingBlocks/FundamentalMatrixEstimation.hpp>
#include <DO/Sara/SfM/BuildingBlocks/KeypointMatching.hpp>
#include <DO/Sara/SfM/BuildingBlocks/Triangulation.hpp>
#include <DO/Sara/SfM/Detectors/SIFT.hpp>

using namespace std;
using namespace DO::Sara;

using EEstimator = NisterFivePointAlgorithm;

// Detect or read SIFT keypoints.
auto get_keypoints(const Image<Rgb8>& image1,
                   const Image<Rgb8>& image2,
                   const std::string& keys1_filepath,
                   const std::string& keys2_filepath,
                   KeypointList<OERegion, float>& keys1,
                   KeypointList<OERegion, float>& keys2)
{
  print_stage("Computing/Reading keypoints");

#ifdef COMPUTE_KEYPOINTS
  keys1 = compute_sift_keypoints(image1.convert<float>());
  keys2 = compute_sift_keypoints(image2.convert<float>());
  cout << "Image 1: " << keys1.size() << " keypoints" << endl;
  cout << "Image 2: " << keys2.size() << " keypoints" << endl;

  const auto& [f1, d1] = keys1;
  const auto& [f2, d2] = keys2;

  write_keypoints(f1, d1, keys1_filepath);
  write_keypoints(f2, d2, keys2_filepath);
#else
  (void) image1;
  (void) image2;
  auto& [f1, d1] = keys1;
  auto& [f2, d2] = keys2;

  read_keypoints(f1, d1, keys1_filepath);
  read_keypoints(f2, d2, keys2_filepath);
#endif
}

GRAPHICS_MAIN()
{
  // Use the following data structure to load images, keypoints, camera
  // parameters.
  auto views = ViewAttributes{};

  // Load images.
  print_stage("Loading images...");
  const auto data_dir =
#ifdef __APPLE__
      std::string{"/Users/david/Desktop/Datasets/sfm/castle_int"};
#else
      std::string{"/home/david/Desktop/Datasets/sfm/castle_int"};
#endif
  views.image_paths = {
      data_dir + "/" + "0000.png",
      data_dir + "/" + "0001.png",
  };
  views.read_images();

  print_stage("Loading the internal camera matrices...");
  views.cameras.resize(2 /* views */);
  views.cameras[0].K =
      read_internal_camera_parameters(data_dir + "/" + "0000.png.K")
          .cast<double>();
  views.cameras[1].K =
      read_internal_camera_parameters(data_dir + "/" + "0001.png.K")
          .cast<double>();

  print_stage("Getting keypoints...");
  views.keypoints.resize(2 /* views */);
  get_keypoints(views.images[0], views.images[1],  //
                data_dir + "/" + "0000.key",       //
                data_dir + "/" + "0001.key",       //
                views.keypoints[0], views.keypoints[1]);

  // Use the following data structures to store the epipolar geometry data.
  auto epipolar_edges = EpipolarEdgeAttributes{};
  epipolar_edges.initialize_edges(2 /* views */);
  epipolar_edges.resize_fundamental_edge_list();
  epipolar_edges.resize_essential_edge_list();

  print_stage("Matching keypoints...");
  epipolar_edges.matches = {match(views.keypoints[0], views.keypoints[1])};
  const auto& matches = epipolar_edges.matches[0];

  print_stage("Performing data transformations...");
  // Invert the internal camera matrices.
  const auto K_inv = std::array<Matrix3d, 2>{views.cameras[0].K.inverse(),
                                             views.cameras[1].K.inverse()};
  // Tensors of image coordinates.
  const auto& f0 = features(views.keypoints[0]);
  const auto& f1 = features(views.keypoints[1]);
  const auto u = std::array{homogeneous(extract_centers(f0)).cast<double>(),
                            homogeneous(extract_centers(f1)).cast<double>()};
  // Tensors of camera coordinates.
  const auto un = std::array{apply_transform(K_inv[0], u[0]),
                             apply_transform(K_inv[1], u[1])};
  static_assert(std::is_same_v<decltype(un[0]), const Tensor_<double, 2>&>);
  // List the matches as a 2D-tensor where each row encodes a match 'm' as a
  // pair of point indices (i, j).
  const auto M = to_tensor(matches);

  print_stage("Estimating the essential matrix...");
  auto& E = epipolar_edges.E[0];
  auto& num_samples = epipolar_edges.E_num_samples[0];
  auto& err_thres = epipolar_edges.E_noise[0];
  auto& inliers = epipolar_edges.E_inliers[0];
  auto sample_best = Tensor_<int, 1>{};
  auto estimator = EEstimator{};
  auto distance = EpipolarDistance{};
  {
    num_samples = 1000;
    err_thres = 1e-3;
    std::tie(E, inliers, sample_best) =
        ransac(M, un[0], un[1], estimator, distance, num_samples, err_thres);
    E.matrix() = E.matrix().normalized();

    epipolar_edges.E_inliers[0] = inliers;
    epipolar_edges.E_best_samples[0] = sample_best;
  }

  // Calculate the fundamental matrix.
  print_stage("Computing the fundamental matrix...");
  auto& F = epipolar_edges.F[0];
  {
    F.matrix() = K_inv[1].transpose() * E.matrix() * K_inv[0];

    epipolar_edges.F_num_samples[0] = 1000;
    epipolar_edges.F_noise = epipolar_edges.E_noise;
    epipolar_edges.F_inliers = epipolar_edges.E_inliers;
    epipolar_edges.F_best_samples = epipolar_edges.E_best_samples;
  }

  // Extract the two-view geometry.
  print_stage("Estimating the two-view geometry...");
  epipolar_edges.two_view_geometries = {
      estimate_two_view_geometry(M, un[0], un[1], E, inliers, sample_best)
  };

  std::map<FeatureGID, int> feature_gids_to_3d_point_indices;

  // Populate the feature tracks.
  const auto [feature_graph, components] =
      populate_feature_tracks(views, epipolar_edges);

  // Keep feature tracks of size 2 at least.
  const auto feature_tracks = filter_feature_tracks(feature_graph, components);

  // Prepare the bundle adjustment problem formulation.
  //
  // 1. Count the number of 3D points.
  const auto num_points = static_cast<int>(feature_tracks.size());
  SARA_CHECK(num_points);

  // 2. Count the number of 2D observations.
  auto num_observations_per_points = std::vector<int>(num_points);
  std::transform(
      std::begin(feature_tracks), std::end(feature_tracks),
      std::begin(num_observations_per_points),
      [](const auto& track) { return static_cast<int>(track.size()); });

  const auto num_observations =
      std::accumulate(std::begin(num_observations_per_points),
                      std::end(num_observations_per_points), 0);
  SARA_CHECK(num_observations);

  // 3. Count the number of cameras, which should be equal to the number of
  //    images.
  auto image_ids = std::set<int>{};
  for (const auto& track : feature_tracks)
    for (const auto& f : track)
      image_ids.insert(f.image_id);

  const auto num_cameras = static_cast<int>(image_ids.size());
  SARA_CHECK(num_cameras);

  const auto num_parameters = 9 * num_cameras + 3 * num_points;
  SARA_CHECK(num_parameters);

  // 4. Transform the data for convenience.
  struct ObservationRef {
    FeatureGID gid;
    // TODO: needs the match_index;
    int camera_id;
    int point_id;
  };
  auto obs_refs = std::vector<ObservationRef>{};
  {
    obs_refs.reserve(num_observations);

    auto point_id = 0;
    for (const auto& track : feature_tracks)
    {
      for (const auto& f: track)
        obs_refs.push_back({f, f.image_id, point_id});
      ++point_id;
    }
  }

  // 5. Prepare the data for Ceres.
  auto observations = Tensor_<double, 2>{{num_observations, 2}};
  auto point_indices = std::vector<int>(num_observations);
  auto camera_indices = std::vector<int>(num_observations);
  auto parameters = std::vector<double>(num_parameters);
  for (int i = 0; i < num_observations; ++i)
  {
    const auto& ref = obs_refs[i];

    // Easy things first.
    point_indices[i] = ref.point_id;
    camera_indices[i] = ref.camera_id;

    // Initialize the 2D observations.
    const auto& image_id = ref.gid.image_id;
    const auto& local_id = ref.gid.local_id;
    const double x = un[image_id](local_id, 0);
    const double y = un[image_id](local_id, 1);
    observations(i, 0) = x;
    observations(i, 1) = y;

    // Initialize the 3D points.
    //parameters[9 * num_cameras + point_indices[i] + 0] =
  }

#ifdef SAVE_TWO_VIEW_GEOMETRY
  keep_cheiral_inliers_only(geometry, inliers);

  // Add the internal camera matrices to the camera.
  geometry.C1.K = K1;
  geometry.C2.K = K2;
  auto colors = extract_colors(image1, image2, geometry);
  save_to_hdf5(geometry, colors);
#endif

  // Inspect the fundamental matrix.
  print_stage("Inspecting the fundamental matrix estimation...");
  check_epipolar_constraints(views.images[0], views.images[1], F, matches,
                             sample_best, inliers,
                             /* display_step */ 20, /* wait_key */ true);

  return 0;
}
<|endoftext|>


<commit_before>/* **********************************************************
 * Copyright (c) 2016-2019 Google, Inc.  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.
 *
 * * Neither the name of Google, Inc. nor the names of its contributors may be
 *   used to endorse or promote products derived from this software without
 *   specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL VMWARE, INC. OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 * DAMAGE.
 */

#include <assert.h>
#include "reader.h"
#include "../common/memref.h"
#include "../common/utils.h"

// Work around clang-format bug: no newline after return type for single-char operator.
// clang-format off
const memref_t &
reader_t::operator*()
// clang-format on
{
    return cur_ref;
}

reader_t &
reader_t::operator++()
{
    // We bail if we get a partial read, or EOF, or any error.
    while (true) {
        if (bundle_idx == 0 /*not in instr bundle*/)
            input_entry = read_next_entry();
        if (input_entry == NULL) {
            if (!at_eof) {
                ERRMSG("Trace is truncated\n");
                assert(false);
                at_eof = true; // bail
            }
            break;
        }
        if (input_entry->type == TRACE_TYPE_FOOTER) {
            VPRINT(this, 2, "At thread EOF\n");
            // We've already presented the thread exit entry to the analyzer.
            continue;
        }
        VPRINT(this, 4, "RECV: type=%d, size=%d, addr=%zd\n", input_entry->type,
               input_entry->size, input_entry->addr);
        bool have_memref = false;
        switch (input_entry->type) {
        case TRACE_TYPE_READ:
        case TRACE_TYPE_WRITE:
        case TRACE_TYPE_PREFETCH:
        case TRACE_TYPE_PREFETCHT0:
        case TRACE_TYPE_PREFETCHT1:
        case TRACE_TYPE_PREFETCHT2:
        case TRACE_TYPE_PREFETCHNTA:
        case TRACE_TYPE_PREFETCH_READ:
        case TRACE_TYPE_PREFETCH_WRITE:
        case TRACE_TYPE_PREFETCH_INSTR:
            have_memref = true;
            assert(cur_tid != 0 && cur_pid != 0);
            cur_ref.data.pid = cur_pid;
            cur_ref.data.tid = cur_tid;
            cur_ref.data.type = (trace_type_t)input_entry->type;
            cur_ref.data.size = input_entry->size;
            cur_ref.data.addr = input_entry->addr;
            // The trace stream always has the instr fetch first, which we
            // use to obtain the PC for subsequent data references.
            cur_ref.data.pc = cur_pc;
            break;
        case TRACE_TYPE_INSTR_MAYBE_FETCH:
            // While offline traces can convert rep string per-iter instrs into
            // no-fetch entries, online can't w/o extra work, so we do the work
            // here:
            if (prev_instr_addr == input_entry->addr)
                input_entry->type = TRACE_TYPE_INSTR_NO_FETCH;
            else
                input_entry->type = TRACE_TYPE_INSTR;
            ANNOTATE_FALLTHROUGH;
        case TRACE_TYPE_INSTR:
        case TRACE_TYPE_INSTR_DIRECT_JUMP:
        case TRACE_TYPE_INSTR_INDIRECT_JUMP:
        case TRACE_TYPE_INSTR_CONDITIONAL_JUMP:
        case TRACE_TYPE_INSTR_DIRECT_CALL:
        case TRACE_TYPE_INSTR_INDIRECT_CALL:
        case TRACE_TYPE_INSTR_RETURN:
        case TRACE_TYPE_INSTR_SYSENTER:
        case TRACE_TYPE_INSTR_NO_FETCH:
            assert(cur_tid != 0 && cur_pid != 0);
            if (input_entry->size == 0) {
                // Just an entry to tell us the PC of the subsequent memref,
                // used with -L0_filter where we don't reliably have icache
                // entries prior to data entries.
                cur_pc = input_entry->addr;
            } else {
                have_memref = true;
                cur_ref.instr.pid = cur_pid;
                cur_ref.instr.tid = cur_tid;
                cur_ref.instr.type = (trace_type_t)input_entry->type;
                cur_ref.instr.size = input_entry->size;
                cur_pc = input_entry->addr;
                cur_ref.instr.addr = cur_pc;
                next_pc = cur_pc + cur_ref.instr.size;
                prev_instr_addr = input_entry->addr;
            }
            break;
        case TRACE_TYPE_INSTR_BUNDLE:
            have_memref = true;
            // The trace stream always has the instr fetch first, which we
            // use to compute the starting PC for the subsequent instructions.
            assert(type_is_instr(cur_ref.instr.type) ||
                   cur_ref.instr.type == TRACE_TYPE_INSTR_NO_FETCH);
            cur_ref.instr.size = input_entry->length[bundle_idx++];
            cur_pc = next_pc;
            cur_ref.instr.addr = cur_pc;
            next_pc = cur_pc + cur_ref.instr.size;
            // input_entry->size stores the number of instrs in this bundle
            assert(input_entry->size <= sizeof(input_entry->length));
            if (bundle_idx == input_entry->size)
                bundle_idx = 0;
            break;
        case TRACE_TYPE_INSTR_FLUSH:
        case TRACE_TYPE_DATA_FLUSH:
            assert(cur_tid != 0 && cur_pid != 0);
            cur_ref.flush.pid = cur_pid;
            cur_ref.flush.tid = cur_tid;
            cur_ref.flush.type = (trace_type_t)input_entry->type;
            cur_ref.flush.size = input_entry->size;
            cur_ref.flush.addr = input_entry->addr;
            if (cur_ref.flush.size != 0)
                have_memref = true;
            break;
        case TRACE_TYPE_INSTR_FLUSH_END:
        case TRACE_TYPE_DATA_FLUSH_END:
            cur_ref.flush.size = input_entry->addr - cur_ref.flush.addr;
            have_memref = true;
            break;
        case TRACE_TYPE_THREAD:
            cur_tid = (memref_tid_t)input_entry->addr;
            // tid2pid might not be filled in yet: if so, we expect a
            // TRACE_TYPE_PID entry right after this one, and later asserts
            // will complain if it wasn't there.
            cur_pid = tid2pid[cur_tid];
            break;
        case TRACE_TYPE_THREAD_EXIT:
            cur_tid = (memref_tid_t)input_entry->addr;
            cur_pid = tid2pid[cur_tid];
            assert(cur_tid != 0 && cur_pid != 0);
            // We do pass this to the caller but only some fields are valid:
            cur_ref.exit.pid = cur_pid;
            cur_ref.exit.tid = cur_tid;
            cur_ref.exit.type = (trace_type_t)input_entry->type;
            have_memref = true;
            break;
        case TRACE_TYPE_PID:
            cur_pid = (memref_pid_t)input_entry->addr;
            // We do want to replace, in case of tid reuse.
            tid2pid[cur_tid] = cur_pid;
            break;
        case TRACE_TYPE_MARKER:
            have_memref = true;
            cur_ref.marker.type = (trace_type_t)input_entry->type;
            assert(cur_tid != 0 && cur_pid != 0);
            cur_ref.marker.pid = cur_pid;
            cur_ref.marker.tid = cur_tid;
            cur_ref.marker.marker_type = (trace_marker_type_t)input_entry->size;
            cur_ref.marker.marker_value = input_entry->addr;
            break;
        default:
            ERRMSG("Unknown trace entry type %d\n", input_entry->type);
            assert(false);
            at_eof = true; // bail
            break;
        }
        if (have_memref)
            break;
    }

    return *this;
}
<commit_msg>i#3320 cachesim assert: Add diagnostics for Appveyor (#3890)<commit_after>/* **********************************************************
 * Copyright (c) 2016-2019 Google, Inc.  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.
 *
 * * Neither the name of Google, Inc. nor the names of its contributors may be
 *   used to endorse or promote products derived from this software without
 *   specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL VMWARE, INC. OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 * DAMAGE.
 */

#include <assert.h>
#include "reader.h"
#include "../common/memref.h"
#include "../common/utils.h"

// Work around clang-format bug: no newline after return type for single-char operator.
// clang-format off
const memref_t &
reader_t::operator*()
// clang-format on
{
    return cur_ref;
}

reader_t &
reader_t::operator++()
{
    // We bail if we get a partial read, or EOF, or any error.
    while (true) {
        if (bundle_idx == 0 /*not in instr bundle*/)
            input_entry = read_next_entry();
        if (input_entry == NULL) {
            if (!at_eof) {
                ERRMSG("Trace is truncated\n");
                assert(false);
                at_eof = true; // bail
            }
            break;
        }
        if (input_entry->type == TRACE_TYPE_FOOTER) {
            VPRINT(this, 2, "At thread EOF\n");
            // We've already presented the thread exit entry to the analyzer.
            continue;
        }
        VPRINT(this, 4, "RECV: type=%d, size=%d, addr=%zd\n", input_entry->type,
               input_entry->size, input_entry->addr);
        bool have_memref = false;
        switch (input_entry->type) {
        case TRACE_TYPE_READ:
        case TRACE_TYPE_WRITE:
        case TRACE_TYPE_PREFETCH:
        case TRACE_TYPE_PREFETCHT0:
        case TRACE_TYPE_PREFETCHT1:
        case TRACE_TYPE_PREFETCHT2:
        case TRACE_TYPE_PREFETCHNTA:
        case TRACE_TYPE_PREFETCH_READ:
        case TRACE_TYPE_PREFETCH_WRITE:
        case TRACE_TYPE_PREFETCH_INSTR:
            have_memref = true;
            assert(cur_tid != 0 && cur_pid != 0);
            cur_ref.data.pid = cur_pid;
            cur_ref.data.tid = cur_tid;
            cur_ref.data.type = (trace_type_t)input_entry->type;
            cur_ref.data.size = input_entry->size;
            cur_ref.data.addr = input_entry->addr;
            // The trace stream always has the instr fetch first, which we
            // use to obtain the PC for subsequent data references.
            cur_ref.data.pc = cur_pc;
            break;
        case TRACE_TYPE_INSTR_MAYBE_FETCH:
            // While offline traces can convert rep string per-iter instrs into
            // no-fetch entries, online can't w/o extra work, so we do the work
            // here:
            if (prev_instr_addr == input_entry->addr)
                input_entry->type = TRACE_TYPE_INSTR_NO_FETCH;
            else
                input_entry->type = TRACE_TYPE_INSTR;
            ANNOTATE_FALLTHROUGH;
        case TRACE_TYPE_INSTR:
        case TRACE_TYPE_INSTR_DIRECT_JUMP:
        case TRACE_TYPE_INSTR_INDIRECT_JUMP:
        case TRACE_TYPE_INSTR_CONDITIONAL_JUMP:
        case TRACE_TYPE_INSTR_DIRECT_CALL:
        case TRACE_TYPE_INSTR_INDIRECT_CALL:
        case TRACE_TYPE_INSTR_RETURN:
        case TRACE_TYPE_INSTR_SYSENTER:
        case TRACE_TYPE_INSTR_NO_FETCH:
            assert(cur_tid != 0 && cur_pid != 0);
            if (input_entry->size == 0) {
                // Just an entry to tell us the PC of the subsequent memref,
                // used with -L0_filter where we don't reliably have icache
                // entries prior to data entries.
                cur_pc = input_entry->addr;
            } else {
                have_memref = true;
                cur_ref.instr.pid = cur_pid;
                cur_ref.instr.tid = cur_tid;
                cur_ref.instr.type = (trace_type_t)input_entry->type;
                cur_ref.instr.size = input_entry->size;
                cur_pc = input_entry->addr;
                cur_ref.instr.addr = cur_pc;
                next_pc = cur_pc + cur_ref.instr.size;
                prev_instr_addr = input_entry->addr;
            }
            break;
        case TRACE_TYPE_INSTR_BUNDLE:
            have_memref = true;
            // The trace stream always has the instr fetch first, which we
            // use to compute the starting PC for the subsequent instructions.
            if (!(type_is_instr(cur_ref.instr.type) ||
                  cur_ref.instr.type == TRACE_TYPE_INSTR_NO_FETCH)) {
                // XXX i#3320: Diagnostics to track down the elusive remaining case of
                // this assert on Appveyor.  We'll remove and replace with just the
                // assert once we have a fix.
                ERRMSG("Invalid trace entry type %d before a bundle\n",
                       cur_ref.instr.type);
                assert(type_is_instr(cur_ref.instr.type) ||
                       cur_ref.instr.type == TRACE_TYPE_INSTR_NO_FETCH);
            }
            cur_ref.instr.size = input_entry->length[bundle_idx++];
            cur_pc = next_pc;
            cur_ref.instr.addr = cur_pc;
            next_pc = cur_pc + cur_ref.instr.size;
            // input_entry->size stores the number of instrs in this bundle
            assert(input_entry->size <= sizeof(input_entry->length));
            if (bundle_idx == input_entry->size)
                bundle_idx = 0;
            break;
        case TRACE_TYPE_INSTR_FLUSH:
        case TRACE_TYPE_DATA_FLUSH:
            assert(cur_tid != 0 && cur_pid != 0);
            cur_ref.flush.pid = cur_pid;
            cur_ref.flush.tid = cur_tid;
            cur_ref.flush.type = (trace_type_t)input_entry->type;
            cur_ref.flush.size = input_entry->size;
            cur_ref.flush.addr = input_entry->addr;
            if (cur_ref.flush.size != 0)
                have_memref = true;
            break;
        case TRACE_TYPE_INSTR_FLUSH_END:
        case TRACE_TYPE_DATA_FLUSH_END:
            cur_ref.flush.size = input_entry->addr - cur_ref.flush.addr;
            have_memref = true;
            break;
        case TRACE_TYPE_THREAD:
            cur_tid = (memref_tid_t)input_entry->addr;
            // tid2pid might not be filled in yet: if so, we expect a
            // TRACE_TYPE_PID entry right after this one, and later asserts
            // will complain if it wasn't there.
            cur_pid = tid2pid[cur_tid];
            break;
        case TRACE_TYPE_THREAD_EXIT:
            cur_tid = (memref_tid_t)input_entry->addr;
            cur_pid = tid2pid[cur_tid];
            assert(cur_tid != 0 && cur_pid != 0);
            // We do pass this to the caller but only some fields are valid:
            cur_ref.exit.pid = cur_pid;
            cur_ref.exit.tid = cur_tid;
            cur_ref.exit.type = (trace_type_t)input_entry->type;
            have_memref = true;
            break;
        case TRACE_TYPE_PID:
            cur_pid = (memref_pid_t)input_entry->addr;
            // We do want to replace, in case of tid reuse.
            tid2pid[cur_tid] = cur_pid;
            break;
        case TRACE_TYPE_MARKER:
            have_memref = true;
            cur_ref.marker.type = (trace_type_t)input_entry->type;
            assert(cur_tid != 0 && cur_pid != 0);
            cur_ref.marker.pid = cur_pid;
            cur_ref.marker.tid = cur_tid;
            cur_ref.marker.marker_type = (trace_marker_type_t)input_entry->size;
            cur_ref.marker.marker_value = input_entry->addr;
            break;
        default:
            ERRMSG("Unknown trace entry type %d\n", input_entry->type);
            assert(false);
            at_eof = true; // bail
            break;
        }
        if (have_memref)
            break;
    }

    return *this;
}
<|endoftext|>