<commit_before>/**
 * KeybdInput 1.0.3
 *
 * @author Roger Lima (rogerlima@outlook.com)
 * @date 31/aug/2014
 * @update 11/feb/2016
 * @desc Reads the keyboard input them according to the format specified (only works on Windows)
 * @example
	int day, month, year;
	KeybdInput< int > userin;

	userin.solicit(
		"Type a date (btw 01/01/1900 and 31/12/2009): ",
		std::regex( "([0-2]?[0-9]|3[0-1])/(0?[1-9]|1[012])/(19[0-9]{2}|200[0-9])" ),
		{ &day, &month, &year },
		false,
		false,
		false,
		"/"
	);
*/

#pragma once
#include <conio.h>
#include <iostream>
#include <regex>
#include <sstream>
#include <vector>
#include <Windows.h>

template< typename T >
class KeybdInput {
private:
	// Stores the values to be used in KeybdInput::reset()
	bool _instverify, _reset, _ispw;
	size_t _inmaxsize;
	std::regex _restriction;
	std::string _rmsg, _sep;
	std::vector< T * > _references = {};

	// Stores the user input
	std::string input;

	// Receives the current X and Y cursor position from get_cursor_position()
	std::vector< short > cursor_position = { 0, 0 };

	// Erase the input of user
	// @param [{size_t=input.size()}] Erase range
	void erase_input( size_t = 0 );

	// Get the console cursor position and pass to the cursor_position
	void get_cursor_position();

	// Set the console cursor position
	// @param {short} X position of cursor
	// @param [{short=cursor_position[ 1 ]}] Y position of cursor
	void set_cursor_position( short, short = 0 );

	// Split a string
	// @param {std::string} Target string
	// @param [{std::string=""}] Separator
	std::vector< std::string > splitstr( std::string str, std::string separator = "" );

	// Set the reference with input value
	// @param {const std::string&} Input value
	// @param {T*} Target place
	void set_reference( const std::string&, T * );

	// Clear all values of references
	// @param {T*} Target place
	void clear_references( std::vector< T * > );
public:
	// Clipboard (arrow up or down to show the last inputs)
	std::vector< std::string > clipboard;

	// Requires the user input again
	// @param [{std::string=_rmsg}] Request message
	void reset( std::string = "" );

	// Requires the keyboard input
	// @param {string} Request message
	// @param {regex} The regex
	// @param {vector< T * >} The place(s) where it will be stored the input
	// @param [{bool=false}] Instant verify
	// @param [{bool=false}] Reset possibility
	// @param [{bool=false}] Is password
	// @param [{std::string=" "}] Separator
	// @param [{size_t=1000}] Input size
	void solicit( std::string, std::regex, std::vector< T * >, bool = false, bool = false, bool = false, std::string = " ", size_t = 1000 );
};

template< typename T >
void KeybdInput< T >::erase_input( size_t erase_range = 0 ) {
	// Default erase range
	if ( !erase_range )
		erase_range = input.size();

	for ( size_t i = 0; i < erase_range; i++ )
		std::cout << "\b \b";

	input = "";
};

template < typename T >
void KeybdInput< T >::set_reference( const std::string& value, T *target ) {
	std::stringstream convert;
	convert << value;
	convert >> *target;
};

void KeybdInput< std::string >::set_reference( const std::string& value, std::string *target ) {
	*target = value;
};

template < typename T >
void KeybdInput< T >::clear_references( std::vector< T * > target ) {
	for ( size_t i = 0; i < _references.size(); *target[ i++ ] = 0 );
};

void KeybdInput< std::string >::clear_references( std::vector< std::string * > target ) {
	for ( size_t i = 0; i < _references.size(); *target[ i++ ] = "" );
};

template< typename T >
void KeybdInput< T >::get_cursor_position() {
	CONSOLE_SCREEN_BUFFER_INFO screen_buffer_info;
	HANDLE hStd = GetStdHandle( STD_OUTPUT_HANDLE );

	if ( !GetConsoleScreenBufferInfo( hStd, &screen_buffer_info ) )
		MessageBox( NULL, L"An error occurred getting the console cursor position.", L"KeybdInput ERROR", NULL );

	cursor_position[ 0 ] = screen_buffer_info.dwCursorPosition.X;
	cursor_position[ 1 ] = screen_buffer_info.dwCursorPosition.Y;
};

template< typename T >
void KeybdInput< T >::set_cursor_position( short x, short y = 0 ) {
	if ( !SetConsoleCursorPosition( GetStdHandle( STD_OUTPUT_HANDLE ), { x, y ? y : cursor_position[ 1 ] } ) )
		MessageBox( NULL, L"An error occurred setting the console cursor position.", L"KeybdInput ERROR", NULL );
};

template< typename T >
std::vector< std::string > KeybdInput< T >::splitstr( std::string str, std::string separator = "" ) {
	size_t index;
	std::vector< std::string > elems;

	while ( ( index = str.find( separator ) ) != std::string::npos && str.size() > 1 ) {
		elems.push_back( str.substr( 0, index ? index : 1 ) );
		str.erase( 0, index + ( !separator.size() ? 1 : separator.size() ) );
	}

	elems.push_back( str );

	return elems;
};

template< typename T >
void KeybdInput< T >::reset( std::string msg = "" ) {
	// Default request message
	if ( msg == "" && _rmsg != "" )
		msg = _rmsg;

	if ( !_reset ) {
		MessageBox( NULL, L"Can't possible execute KeybdInput::reset() without set reset_possibility=true in KeybdInput::solicit().", L"KeybdInput ERROR", NULL );
		return;
	}

	// Clear previously set values
	clear_references( _references );

	// Sets the cursor in the previous line, erase all and requires input again
	get_cursor_position();
	set_cursor_position( static_cast< short >( msg.size() + input.size() ), cursor_position[ 1 ] - 1 );
	erase_input( msg.size() + input.size() );
	solicit( msg, std::regex( _restriction ), _references, _instverify, _reset, _ispw, _sep, _inmaxsize );
};

template< typename T >
void KeybdInput< T >::solicit( std::string request_msg, std::regex restriction, std::vector< T * > references, bool instant_verify, bool reset_possibility, bool is_password, std::string separator, size_t input_max_size ) {
	static size_t clipboard_index = 0;
	size_t i, cursor_pos_x,
		inputLength = 0;
	char key = 0;
	bool is_function_key = false,
		arrow = false,
		waiting_input = true;
	std::string input_subtr;
	std::vector< std::string > inputParts;

	if ( references.size() == 0 ) {
		MessageBox( NULL, L"\"refereces\" param need be set with at least one member.", L"KeybdInput ERROR", NULL );
		return;
	}

	input = "";
	std::cout << request_msg;

	// Retrieves the values to be used in KeybdInput::reset()
	if ( reset_possibility ) {
		_rmsg = request_msg;
		_restriction = restriction;
		_references = references;
		_instverify = instant_verify;
		_reset = reset_possibility;
		_ispw = is_password;
		_sep = separator;
		_inmaxsize = input_max_size;
	}

	// Clears previous data
	else if ( _reset ) {
		_rmsg = "";
		_restriction = "";
		_references = {};
		_instverify = false;
		_reset = false;
		_ispw = false;
		_sep = " ";
		_inmaxsize = 1000;
	}

	while ( waiting_input ) {
		key = _getch();

		// Arrow keys prefix
		if ( key == -32 ) {
			arrow = true;
			continue;
		}

		// Prevents function keys
		if ( key == 0 ) {
			is_function_key = true;
			continue;
		} else if ( is_function_key ) {
			is_function_key = false;
			continue;
		}

		// Arrows
		if ( arrow ) {
			get_cursor_position();

			// LEFT
			if ( key == 75 && static_cast< size_t >( cursor_position[ 0 ] ) > request_msg.size() )
				set_cursor_position( cursor_position[ 0 ] - 1 );

			// RIGHT
			else if ( key == 77 && static_cast< size_t >( cursor_position[ 0 ] ) < input.size() + request_msg.size() )
				set_cursor_position( cursor_position[ 0 ] + 1 );

			// UP
			else if ( key == 72 && !is_password && clipboard.size() > 0 && clipboard_index > 0 ) {
				erase_input();
				std::cout << clipboard[ --clipboard_index ];

				input = clipboard[ clipboard_index ];
				inputLength = clipboard[ clipboard_index ].size();
			}

			// DOWN
			else if ( key == 80 && !is_password && clipboard.size() > 0 && clipboard_index < clipboard.size() - 1 ) {
				erase_input();
				std::cout << clipboard[ ++clipboard_index ];

				input = clipboard[ clipboard_index ];
				inputLength = clipboard[ clipboard_index ].size();

				if ( clipboard_index >= clipboard.size() )
					clipboard_index = clipboard.size() - 1;
			}

		}

		// Valid character
		else if ( key != 8 && key != 13 ) {
			// Inserts the character in current cursor position
			get_cursor_position();
			input.insert( cursor_position[ 0 ] - request_msg.size(), std::string( 1, key ) );

			// If the user input satisfy the restrictions, removes the character
			if ( instant_verify && ( input.size() > input_max_size || !std::regex_match( input, restriction ) ) ) {
				input.erase( cursor_position[ 0 ] - request_msg.size(), 1 );
			} else {
				// Appends the character if cursor is at the end, otherwise, interleaves
				if ( cursor_position[ 0 ] == ( request_msg.size() + input.size() - 1 ) ) {
					std::cout << ( ( is_password ) ? '*' : key );
				} else {
					input_subtr = input.substr( input.size() - ( ( request_msg.size() + input.size() ) - cursor_position[ 0 ] - 1 ), input.size() );

					std::cout << ( ( is_password ) ? '*' : key )
						<< ( ( is_password ) ? std::string( input_subtr.size(), '*' ) : input_subtr );
					set_cursor_position( cursor_position[ 0 ] + 1 );
				}

				inputLength++;
			}
		}

		// ENTER
		else if ( key == 13 && inputLength ) {
			// If the user input satisfy the restrictions, clear input
			if ( input.size() > input_max_size || !std::regex_match( input, restriction ) ) {
				erase_input();
				inputLength = 0;
			} else {
				// Trim left and right
				input.erase( 0, input.find_first_not_of( ' ' ) );
				input.erase( input.find_last_not_of( ' ' ) + 1 );

				if ( references.size() == 1 )
					set_reference( input, references[ 0 ] );
				else
					for ( i = 0, inputParts = splitstr( input, separator ); i < references.size(); i++ )
						if ( i < inputParts.size() )
							set_reference( inputParts[ i ], references[ i ] );

				std::cout << std::endl;

				// Prevents repetition on clipboard and don't save if it's a password
				if ( !is_password && ( clipboard.size() == 0 || input != clipboard[ clipboard.size() - 1 ] ) ) {
					clipboard.push_back( input );
					clipboard_index = clipboard.size();
				}

				waiting_input = false;
			}
		}

		// BACKSPACE
		else if ( key == 8 && inputLength ) {
			get_cursor_position();
			cursor_pos_x = cursor_position[ 0 ];

			if ( cursor_pos_x == ( request_msg.size() + input.size() - 1 ) && inputLength == 1 )
				continue;

			std::cout << "\b \b";
			input.erase( cursor_pos_x - request_msg.size() - 1, 1 );
			inputLength--;

			// If the cursor isn't at the end
			if ( cursor_pos_x <= ( request_msg.size() + input.size() ) ) {
				// Put the cursor at the start and rewrites the input
				set_cursor_position( static_cast< short >( request_msg.size() ) );
				std::cout << ( ( is_password ) ? std::string( input.size(), '*' ) : input );

				// Put the cursor at the end and erase the last char
				set_cursor_position( static_cast< short >( request_msg.size() + input.size() + 1 ) );
				std::cout << "\b \b";

				// Put the cursor at the original position
				set_cursor_position( static_cast< short >( cursor_pos_x - 1 ) );
			}
		}

		arrow = false;
	}
};
<commit_msg>Buf fix<commit_after>/**
 * KeybdInput 1.0.3
 *
 * @author Roger Lima (rogerlima@outlook.com)
 * @date 31/aug/2014
 * @update 19/feb/2016
 * @desc Reads the keyboard input them according to the format specified (only works on Windows)
 * @example
	int day, month, year;
	KeybdInput< int > userin;

	userin.solicit(
		"Type a date (btw 01/01/1900 and 31/12/2009): ",
		std::regex( "([0-2]?[0-9]|3[0-1])/(0?[1-9]|1[012])/(19[0-9]{2}|200[0-9])" ),
		{ &day, &month, &year },
		false,
		false,
		false,
		"/"
	);
*/

#pragma once
#include <conio.h>
#include <iostream>
#include <regex>
#include <sstream>
#include <vector>
#include <Windows.h>

template< typename T >
class KeybdInput {
private:
	// Stores the values to be used in KeybdInput::reset()
	bool _instverify, _reset, _ispw;
	size_t _inmaxsize;
	std::regex _restriction;
	std::string _rmsg, _sep;
	std::vector< T * > _references = {};

	// Stores the user input
	std::string input;

	// Receives the current X and Y cursor position from get_cursor_position()
	std::vector< short > cursor_position = { 0, 0 };

	// Erase the input of user
	// @param [{size_t=input.size()}] Erase range
	void erase_input( size_t = 0 );

	// Get the console cursor position and pass to the cursor_position
	void get_cursor_position();

	// Set the console cursor position
	// @param {short} X position of cursor
	// @param [{short=cursor_position[ 1 ]}] Y position of cursor
	void set_cursor_position( short, short = 0 );

	// Split a string
	// @param {std::string} Target string
	// @param [{std::string=""}] Separator
	std::vector< std::string > splitstr( std::string str, std::string separator = "" );

	// Set the reference with input value
	// @param {const std::string&} Input value
	// @param {T*} Target place
	void set_reference( const std::string&, T * );

	// Clear all values of references
	// @param {T*} Target place
	void clear_references( std::vector< T * > );
public:
	// Clipboard (arrow up or down to show the last inputs)
	std::vector< std::string > clipboard;

	// Requires the user input again
	// @param [{std::string=_rmsg}] Request message
	void reset( std::string = "" );

	// Requires the keyboard input
	// @param {string} Request message
	// @param {regex} The regex
	// @param {vector< T * >} The place(s) where it will be stored the input
	// @param [{bool=false}] Instant verify
	// @param [{bool=false}] Reset possibility
	// @param [{bool=false}] Is password
	// @param [{std::string=" "}] Separator
	// @param [{size_t=1000}] Input size
	void solicit( std::string, std::regex, std::vector< T * >, bool = false, bool = false, bool = false, std::string = " ", size_t = 1000 );
};

template< typename T >
void KeybdInput< T >::erase_input( size_t erase_range = 0 ) {
	// Default erase range
	if ( !erase_range )
		erase_range = input.size();

	for ( size_t i = 0; i < erase_range; i++ )
		std::cout << "\b \b";

	input = "";
};

template < typename T >
void KeybdInput< T >::set_reference( const std::string& value, T *target ) {
	std::stringstream convert;
	convert << value;
	convert >> *target;
};

void KeybdInput< std::string >::set_reference( const std::string& value, std::string *target ) {
	*target = value;
};

template < typename T >
void KeybdInput< T >::clear_references( std::vector< T * > target ) {
	for ( size_t i = 0; i < _references.size(); *target[ i++ ] = 0 );
};

void KeybdInput< std::string >::clear_references( std::vector< std::string * > target ) {
	for ( size_t i = 0; i < _references.size(); *target[ i++ ] = "" );
};

template< typename T >
void KeybdInput< T >::get_cursor_position() {
	CONSOLE_SCREEN_BUFFER_INFO screen_buffer_info;
	HANDLE hStd = GetStdHandle( STD_OUTPUT_HANDLE );

	if ( !GetConsoleScreenBufferInfo( hStd, &screen_buffer_info ) )
		MessageBox( NULL, L"An error occurred getting the console cursor position.", L"KeybdInput ERROR", NULL );

	cursor_position[ 0 ] = screen_buffer_info.dwCursorPosition.X;
	cursor_position[ 1 ] = screen_buffer_info.dwCursorPosition.Y;
};

template< typename T >
void KeybdInput< T >::set_cursor_position( short x, short y = 0 ) {
	if ( !SetConsoleCursorPosition( GetStdHandle( STD_OUTPUT_HANDLE ), { x, y } ) )
		MessageBox( NULL, L"An error occurred setting the console cursor position.", L"KeybdInput ERROR", NULL );
};

template< typename T >
std::vector< std::string > KeybdInput< T >::splitstr( std::string str, std::string separator = "" ) {
	size_t index;
	std::vector< std::string > elems;

	while ( ( index = str.find( separator ) ) != std::string::npos && str.size() > 1 ) {
		elems.push_back( str.substr( 0, index ? index : 1 ) );
		str.erase( 0, index + ( !separator.size() ? 1 : separator.size() ) );
	}

	elems.push_back( str );

	return elems;
};

template< typename T >
void KeybdInput< T >::reset( std::string msg = "" ) {
	// Default request message
	if ( msg == "" && _rmsg != "" )
		msg = _rmsg;

	if ( !_reset ) {
		MessageBox( NULL, L"Can't possible execute KeybdInput::reset() without set reset_possibility=true in KeybdInput::solicit().", L"KeybdInput ERROR", NULL );
		return;
	}

	// Clear previously set values
	clear_references( _references );

	// Sets the cursor in the previous line, erase all and requires input again
	get_cursor_position();
	set_cursor_position( static_cast< short >( msg.size() + input.size() ), cursor_position[ 1 ] - 1 );
	erase_input( msg.size() + input.size() );
	solicit( msg, std::regex( _restriction ), _references, _instverify, _reset, _ispw, _sep, _inmaxsize );
};

template< typename T >
void KeybdInput< T >::solicit( std::string request_msg, std::regex restriction, std::vector< T * > references, bool instant_verify, bool reset_possibility, bool is_password, std::string separator, size_t input_max_size ) {
	static size_t clipboard_index = 0;
	size_t i, cursor_pos_x,
		inputLength = 0;
	char key = 0;
	bool is_function_key = false,
		arrow = false,
		waiting_input = true;
	std::string input_subtr;
	std::vector< std::string > inputParts;

	if ( references.size() == 0 ) {
		MessageBox( NULL, L"\"refereces\" param need be set with at least one member.", L"KeybdInput ERROR", NULL );
		return;
	}

	input = "";
	std::cout << request_msg;

	// Retrieves the values to be used in KeybdInput::reset()
	if ( reset_possibility ) {
		_rmsg = request_msg;
		_restriction = restriction;
		_references = references;
		_instverify = instant_verify;
		_reset = reset_possibility;
		_ispw = is_password;
		_sep = separator;
		_inmaxsize = input_max_size;
	}

	// Clears previous data
	else if ( _reset ) {
		_rmsg = "";
		_restriction = "";
		_references = {};
		_instverify = false;
		_reset = false;
		_ispw = false;
		_sep = " ";
		_inmaxsize = 1000;
	}

	while ( waiting_input ) {
		key = _getch();

		// Arrow keys prefix
		if ( key == -32 ) {
			arrow = true;
			continue;
		}

		// Prevents function keys
		if ( key == 0 ) {
			is_function_key = true;
			continue;
		} else if ( is_function_key ) {
			is_function_key = false;
			continue;
		}

		// Arrows
		if ( arrow ) {
			get_cursor_position();

			// LEFT
			if ( key == 75 && static_cast< size_t >( cursor_position[ 0 ] ) > request_msg.size() )
				set_cursor_position( cursor_position[ 0 ] - 1 );

			// RIGHT
			else if ( key == 77 && static_cast< size_t >( cursor_position[ 0 ] ) < input.size() + request_msg.size() )
				set_cursor_position( cursor_position[ 0 ] + 1 );

			// UP
			else if ( key == 72 && !is_password && clipboard.size() > 0 && clipboard_index > 0 ) {
				erase_input();
				std::cout << clipboard[ --clipboard_index ];

				input = clipboard[ clipboard_index ];
				inputLength = clipboard[ clipboard_index ].size();
			}

			// DOWN
			else if ( key == 80 && !is_password && clipboard.size() > 0 && clipboard_index < clipboard.size() - 1 ) {
				erase_input();
				std::cout << clipboard[ ++clipboard_index ];

				input = clipboard[ clipboard_index ];
				inputLength = clipboard[ clipboard_index ].size();

				if ( clipboard_index >= clipboard.size() )
					clipboard_index = clipboard.size() - 1;
			}

		}

		// Valid character
		else if ( key != 8 && key != 13 ) {
			// Inserts the character in current cursor position
			get_cursor_position();
			input.insert( cursor_position[ 0 ] - request_msg.size(), std::string( 1, key ) );

			// If the user input satisfy the restrictions, removes the character
			if ( instant_verify && ( input.size() > input_max_size || !std::regex_match( input, restriction ) ) ) {
				input.erase( cursor_position[ 0 ] - request_msg.size(), 1 );
			} else {
				// Appends the character if cursor is at the end, otherwise, interleaves
				if ( cursor_position[ 0 ] == ( request_msg.size() + input.size() - 1 ) ) {
					std::cout << ( ( is_password ) ? '*' : key );
				} else {
					input_subtr = input.substr( input.size() - ( ( request_msg.size() + input.size() ) - cursor_position[ 0 ] - 1 ), input.size() );

					std::cout << ( ( is_password ) ? '*' : key )
						<< ( ( is_password ) ? std::string( input_subtr.size(), '*' ) : input_subtr );
					set_cursor_position( cursor_position[ 0 ] + 1 );
				}

				inputLength++;
			}
		}

		// ENTER
		else if ( key == 13 && inputLength ) {
			// If the user input satisfy the restrictions, clear input
			if ( input.size() > input_max_size || !std::regex_match( input, restriction ) ) {
				erase_input();
				inputLength = 0;
			} else {
				// Trim left and right
				input.erase( 0, input.find_first_not_of( ' ' ) );
				input.erase( input.find_last_not_of( ' ' ) + 1 );

				if ( references.size() == 1 )
					set_reference( input, references[ 0 ] );
				else
					for ( i = 0, inputParts = splitstr( input, separator ); i < references.size(); i++ )
						if ( i < inputParts.size() )
							set_reference( inputParts[ i ], references[ i ] );

				std::cout << std::endl;

				// Prevents repetition on clipboard and don't save if it's a password
				if ( !is_password && ( clipboard.size() == 0 || input != clipboard[ clipboard.size() - 1 ] ) ) {
					clipboard.push_back( input );
					clipboard_index = clipboard.size();
				}

				waiting_input = false;
			}
		}

		// BACKSPACE
		else if ( key == 8 && inputLength ) {
			get_cursor_position();
			cursor_pos_x = cursor_position[ 0 ];

			if ( cursor_pos_x == ( request_msg.size() + input.size() - 1 ) && inputLength == 1 )
				continue;

			std::cout << "\b \b";
			input.erase( cursor_pos_x - request_msg.size() - 1, 1 );
			inputLength--;

			// If the cursor isn't at the end
			if ( cursor_pos_x <= ( request_msg.size() + input.size() ) ) {
				// Put the cursor at the start and rewrites the input
				set_cursor_position( static_cast< short >( request_msg.size() ) );
				std::cout << ( ( is_password ) ? std::string( input.size(), '*' ) : input );

				// Put the cursor at the end and erase the last char
				set_cursor_position( static_cast< short >( request_msg.size() + input.size() + 1 ) );
				std::cout << "\b \b";

				// Put the cursor at the original position
				set_cursor_position( static_cast< short >( cursor_pos_x - 1 ) );
			}
		}

		arrow = false;
	}
};
<|endoftext|>


<commit_before>#include "DoubleIntegrator.h"
#include "InterpolatorHelpers.h"
#include "ParabolicRamp.h"
#include "KinodynamicPath.h"
#include "CSetHelpers.h"
#include <sstream>
using namespace std;

void Convert(const ParabolicRamp::ParabolicRamp1D& ramp,Spline::PiecewisePolynomial& res)
{
  res.segments.resize(3);
  res.timeShift.resize(3);
  res.times.resize(4);
  res.times[0] = 0;
  res.times[1] = ramp.tswitch1;
  res.times[2] = ramp.tswitch2;
  res.times[3] = ramp.ttotal;
  res.segments[0].Resize(3);
  res.segments[0].coef[0] = ramp.x0;
  res.segments[0].coef[1] = ramp.dx0;
  res.segments[0].coef[2] = 0.5*ramp.a1;
  res.timeShift[0] = 0;
  res.segments[1].Resize(2);
  res.segments[1].coef[0] = ramp.Evaluate(ramp.tswitch1);
  res.segments[1].coef[1] = ramp.Derivative(ramp.tswitch1);
  res.timeShift[1] = ramp.tswitch1;
  res.segments[2].Resize(3);
  res.segments[2].coef[0] = ramp.x1;
  res.segments[2].coef[1] = ramp.dx1;
  res.segments[2].coef[2] = 0.5*ramp.a2;
  res.timeShift[2] = ramp.ttotal;

  if(ramp.ttotal == ramp.tswitch2) {
    res.times.erase(--res.times.end());
    res.segments.erase(--res.segments.end());
    res.timeShift.erase(--res.timeShift.end());
  }
  if(ramp.tswitch1 == ramp.tswitch2) {
    res.times.erase(++res.times.begin());
    res.segments.erase(++res.segments.begin());
    res.timeShift.erase(++res.timeShift.begin());
  }
  if(ramp.tswitch1 == 0 && res.segments.size()>1) {
    res.times.erase(res.times.begin());
    res.segments.erase(res.segments.begin());
    res.timeShift.erase(res.timeShift.begin());
  }
}

void Convert(const ParabolicRamp::ParabolicRampND& ramp,Spline::PiecewisePolynomialND& res)
{
  res.elements.resize(ramp.ramps.size());
  for(size_t i=0;i<ramp.ramps.size();i++)
    Convert(ramp.ramps[i],res.elements[i]);
}

//concatenates the ramps
void Convert(const std::vector<ParabolicRamp::ParabolicRamp1D>& ramps,Spline::PiecewisePolynomial& res)
{
  assert(!ramps.empty());
  Convert(ramps[0],res);
  for(size_t i=1;i<ramps.size();i++) {
    Spline::PiecewisePolynomial suffix;
    Convert(ramps[i],suffix);
    res.Concat(suffix,true);
  }
}

void Convert(const std::vector<std::vector<ParabolicRamp::ParabolicRamp1D> >& ramps,Spline::PiecewisePolynomialND& res)
{
  res.elements.resize(ramps.size());
  for(size_t i=0;i<ramps.size();i++)
    Convert(ramps[i],res.elements[i]);
}

DoubleIntegratorControlSpace::DoubleIntegratorControlSpace(const SmartPointer<CSet>& uset,Real dtmax)
:IntegratedControlSpace(uset,dtmax,dtmax)
{}

Interpolator* DoubleIntegratorControlSpace::Simulate(const State& x0, const ControlInput& u)
{
  //do we want to have the interpolator in the range [0,1]?
  Vector q0,v0;
  CVSpace::GetState(x0,q0,v0);
  Real udt = u(0);
  ControlInput ubase;
  ubase.setRef(u,1,1,u.n-1);
  vector<Spline::Polynomial<double> > elements(x0.n);
  assert(q0.n == ubase.n);
  for(int i=0;i<x0.n;i++) {
    elements[i].Resize(2);
    elements[i].SetCoef(0,q0[i]);
    elements[i].SetCoef(1,v0[i]);
    elements[i].SetCoef(2,0.5*ubase[i]);
  }
  Spline::PiecewisePolynomialND path(elements,0,udt);
  return new PiecewisePolynomialInterpolator(path);
}

std::string DoubleIntegratorControlSpace::VariableName(int i)
{
  if(i==0) return "time_step";
  stringstream ss; ss<<"ddx"<<i-1; return ss.str();
}

void DoubleIntegratorControlSpace::Successor(const State& x0, const ControlInput& u,State& x1)
{
  Real udt = u(0);
  ControlInput ubase;
  ubase.setRef(u,1,1,u.n-1);
  EvalDynamics(x0,ubase,udt,x1);
}

void DoubleIntegratorControlSpace::Derivative(const State& x, const ControlInput& u,State& dx)
{
  Vector q,v;
  CVSpace::GetState(x,q,v);
  CVSpace::SetState(v,u,dx);
}

void DoubleIntegratorControlSpace::EvalDynamics(const State& x0, const ControlInput& ddx,Real t,State& x1)
{
  Vector q0,v0,q1,v1;
  CVSpace::GetState(x0,q0,v0);
  q1 = q0 + t*v0 + (0.5*t*t)*ddx;
  v1 = v0 + t*ddx;
  CVSpace::SetState(q1,v1,x1);
}

DoubleIntegratorKinodynamicSpace::DoubleIntegratorKinodynamicSpace(SmartPointer<CSpace> qspace,SmartPointer<CSpace> vspace,SmartPointer<CSet> uset,Real dtmax)
:KinodynamicSpace(new CVSpace(qspace,vspace),new DoubleIntegratorControlSpace(uset,dtmax)),visibilityEpsilon(1e-2)
{
  BoxSet* abset = dynamic_cast<BoxSet*>(&*uset);
  if(!abset) {
    printf("DoubleIntegratorKinodynamicSpace: No steering function available; accelerations are not box-bounded\n");
  }
  else {
    //create steering function
    PropertyMap props;
    vspace->Properties(props);
    vector<Real> vmin,vmax;
    if(!props.getArray("minimum",vmin) || !props.getArray("maximum",vmax)) {
      vmin.resize(abset->bmin.size(),-Inf);
      vmax.resize(abset->bmax.size(),Inf);
    }
    controlSpace->mySteeringFunction = new DoubleIntegratorBoxBoundedSteeringFunction(abset->bmax,vmax);
  }
}

EdgePlanner* DoubleIntegratorKinodynamicSpace::TrajectoryChecker(const ControlInput& u,const SmartPointer<Interpolator>& path)
{
  return new EpsilonEdgeChecker(GetStateSpace(),path,visibilityEpsilon);
}

void DoubleIntegratorKinodynamicSpace::SetVisibilityEpsilon(Real tol)
{
  visibilityEpsilon = tol;
}

DoubleIntegratorBoxBoundedSteeringFunction::DoubleIntegratorBoxBoundedSteeringFunction(const Vector& _amax,const Vector& _vmax)
:amax(vector<double>(_amax)),vmax(vector<double>(_vmax))
{}

void DoubleIntegratorBoxBoundedSteeringFunction::SetConfigurationBounds(const Config& _qmin,const Config& _qmax)
{
  qmin = _qmin;
  qmax = _qmax;
}

bool DoubleIntegratorBoxBoundedSteeringFunction::Connect(const State& x,const State& y,KinodynamicMilestonePath& path)
{
  Vector qx,vx,qy,vy;
  CVSpace::GetState(x,qx,vx);
  CVSpace::GetState(y,qy,vy);
  ParabolicRamp::ParabolicRampND ramp;
  ramp.x0 = qx;
  ramp.x1 = qy;
  ramp.dx0 = vx;
  ramp.dx1 = vy;
  if(!ramp.SolveMinTime(vector<double>(amax),vector<double>(vmax))) return false;
  path.milestones[0] = x;
  path.milestones[1] = y;
  //NOTE: THE CONTROL IS INVALID.  SHOULD WE SPLIT UP THE PATH INTO PIECEWISE QUADRATIC SEGMENTS?
  path.controls.resize(1);
  path.controls[0].resize(0);
  Spline::PiecewisePolynomialND ppath;
  Convert(ramp,ppath);
  path.paths.push_back(new PiecewisePolynomialInterpolator(ppath));
  return true;
}<commit_msg>Fixed DoubleIntegrator steering function problems<commit_after>#include "DoubleIntegrator.h"
#include "InterpolatorHelpers.h"
#include "ParabolicRamp.h"
#include "KinodynamicPath.h"
#include "CSetHelpers.h"
#include <iostream>
#include <sstream>
using namespace std;

void Convert(const ParabolicRamp::ParabolicRamp1D& ramp,Spline::PiecewisePolynomial& res)
{
  res.segments.resize(3);
  res.timeShift.resize(3);
  res.times.resize(4);
  res.times[0] = 0;
  res.times[1] = ramp.tswitch1;
  res.times[2] = ramp.tswitch2;
  res.times[3] = ramp.ttotal;
  res.segments[0].Resize(3);
  res.segments[0].coef[0] = ramp.x0;
  res.segments[0].coef[1] = ramp.dx0;
  res.segments[0].coef[2] = 0.5*ramp.a1;
  res.timeShift[0] = 0;
  res.segments[1].Resize(2);
  res.segments[1].coef[0] = ramp.Evaluate(ramp.tswitch1);
  res.segments[1].coef[1] = ramp.Derivative(ramp.tswitch1);
  res.timeShift[1] = ramp.tswitch1;
  res.segments[2].Resize(3);
  res.segments[2].coef[0] = ramp.x1;
  res.segments[2].coef[1] = ramp.dx1;
  res.segments[2].coef[2] = 0.5*ramp.a2;
  res.timeShift[2] = ramp.ttotal;

  if(ramp.ttotal == ramp.tswitch2) {
    res.times.erase(--res.times.end());
    res.segments.erase(--res.segments.end());
    res.timeShift.erase(--res.timeShift.end());
  }
  if(ramp.tswitch1 == ramp.tswitch2) {
    res.times.erase(++res.times.begin());
    res.segments.erase(++res.segments.begin());
    res.timeShift.erase(++res.timeShift.begin());
  }
  if(ramp.tswitch1 == 0 && res.segments.size()>1) {
    res.times.erase(res.times.begin());
    res.segments.erase(res.segments.begin());
    res.timeShift.erase(res.timeShift.begin());
  }
}

void Convert(const ParabolicRamp::ParabolicRampND& ramp,Spline::PiecewisePolynomialND& res)
{
  res.elements.resize(ramp.ramps.size());
  for(size_t i=0;i<ramp.ramps.size();i++)
    Convert(ramp.ramps[i],res.elements[i]);
}

//concatenates the ramps
void Convert(const std::vector<ParabolicRamp::ParabolicRamp1D>& ramps,Spline::PiecewisePolynomial& res)
{
  assert(!ramps.empty());
  Convert(ramps[0],res);
  for(size_t i=1;i<ramps.size();i++) {
    Spline::PiecewisePolynomial suffix;
    Convert(ramps[i],suffix);
    res.Concat(suffix,true);
  }
}

void Convert(const std::vector<std::vector<ParabolicRamp::ParabolicRamp1D> >& ramps,Spline::PiecewisePolynomialND& res)
{
  res.elements.resize(ramps.size());
  for(size_t i=0;i<ramps.size();i++)
    Convert(ramps[i],res.elements[i]);
}

DoubleIntegratorControlSpace::DoubleIntegratorControlSpace(const SmartPointer<CSet>& uset,Real dtmax)
:IntegratedControlSpace(uset,dtmax,dtmax)
{}

Interpolator* DoubleIntegratorControlSpace::Simulate(const State& x0, const ControlInput& u)
{
  //do we want to have the interpolator in the range [0,1]?
  Vector q0,v0;
  CVSpace::GetState(x0,q0,v0);
  Real udt = u(0);
  ControlInput ubase;
  ubase.setRef(u,1,1,u.n-1);
  vector<Spline::Polynomial<double> > elements(x0.n);
  assert(q0.n == ubase.n);
  for(int i=0;i<q0.n;i++) {
    elements[i].Resize(3);
    elements[i].SetCoef(0,q0[i]);
    elements[i].SetCoef(1,v0[i]);
    elements[i].SetCoef(2,0.5*ubase[i]);
  }
  for(int i=0;i<q0.n;i++) {
    elements[i+q0.n].Resize(2);
    elements[i+q0.n].SetCoef(0,v0[i]);
    elements[i+q0.n].SetCoef(1,ubase[i]);
  }
  Spline::PiecewisePolynomialND path(elements,0,udt);
  return new PiecewisePolynomialInterpolator(path);
}

std::string DoubleIntegratorControlSpace::VariableName(int i)
{
  if(i==0) return "time_step";
  stringstream ss; ss<<"ddx"<<i-1; return ss.str();
}

void DoubleIntegratorControlSpace::Successor(const State& x0, const ControlInput& u,State& x1)
{
  Real udt = u(0);
  ControlInput ubase;
  ubase.setRef(u,1,1,u.n-1);
  EvalDynamics(x0,ubase,udt,x1);
}

void DoubleIntegratorControlSpace::Derivative(const State& x, const ControlInput& u,State& dx)
{
  Vector q,v;
  CVSpace::GetState(x,q,v);
  CVSpace::SetState(v,u,dx);
}

void DoubleIntegratorControlSpace::EvalDynamics(const State& x0, const ControlInput& ddx,Real t,State& x1)
{
  Vector q0,v0,q1,v1;
  CVSpace::GetState(x0,q0,v0);
  q1 = q0 + t*v0 + (0.5*t*t)*ddx;
  v1 = v0 + t*ddx;
  CVSpace::SetState(q1,v1,x1);
}

DoubleIntegratorKinodynamicSpace::DoubleIntegratorKinodynamicSpace(SmartPointer<CSpace> qspace,SmartPointer<CSpace> vspace,SmartPointer<CSet> uset,Real dtmax)
:KinodynamicSpace(new CVSpace(qspace,vspace),new DoubleIntegratorControlSpace(uset,dtmax)),visibilityEpsilon(1e-2)
{
  BoxSet* abset = dynamic_cast<BoxSet*>(&*uset);
  if(!abset) {
    printf("DoubleIntegratorKinodynamicSpace: No steering function available; accelerations are not box-bounded\n");
  }
  else {
    //create steering function
    PropertyMap props;
    vspace->Properties(props);
    vector<Real> vmin,vmax;
    if(!props.getArray("minimum",vmin) || !props.getArray("maximum",vmax)) {
      vmin.resize(abset->bmin.size(),-Inf);
      vmax.resize(abset->bmax.size(),Inf);
    }
    controlSpace->mySteeringFunction = new DoubleIntegratorBoxBoundedSteeringFunction(abset->bmax,vmax);
  }
}

EdgePlanner* DoubleIntegratorKinodynamicSpace::TrajectoryChecker(const ControlInput& u,const SmartPointer<Interpolator>& path)
{
  return new EpsilonEdgeChecker(GetStateSpace(),path,visibilityEpsilon);
}

void DoubleIntegratorKinodynamicSpace::SetVisibilityEpsilon(Real tol)
{
  visibilityEpsilon = tol;
}

DoubleIntegratorBoxBoundedSteeringFunction::DoubleIntegratorBoxBoundedSteeringFunction(const Vector& _amax,const Vector& _vmax)
:amax(vector<double>(_amax)),vmax(vector<double>(_vmax))
{}

void DoubleIntegratorBoxBoundedSteeringFunction::SetConfigurationBounds(const Config& _qmin,const Config& _qmax)
{
  qmin = _qmin;
  qmax = _qmax;
}

bool DoubleIntegratorBoxBoundedSteeringFunction::Connect(const State& x,const State& y,KinodynamicMilestonePath& path)
{
  Vector qx,vx,qy,vy;
  CVSpace::GetState(x,qx,vx);
  CVSpace::GetState(y,qy,vy);
  ParabolicRamp::ParabolicRampND ramp;
  ramp.x0 = qx;
  ramp.x1 = qy;
  ramp.dx0 = vx;
  ramp.dx1 = vy;
  if(!ramp.SolveMinTime(vector<double>(amax),vector<double>(vmax))) return false;
  path.milestones.resize(2);
  path.milestones[0] = x;
  path.milestones[1] = y;
  //NOTE: THE CONTROL IS INVALID.  SHOULD WE SPLIT UP THE PATH INTO PIECEWISE QUADRATIC SEGMENTS?
  path.controls.resize(1);
  path.controls[0].resize(qx.n+1,0.0);
  path.controls[0][0] = ramp.endTime;
  Spline::PiecewisePolynomialND ppath;
  Convert(ramp,ppath);
  Spline::PiecewisePolynomialND dpath = ppath.Differentiate();
  //the polynomial needs derivative information too...
  ppath.elements.insert(ppath.elements.end(),dpath.elements.begin(),dpath.elements.end());
  path.paths.push_back(new PiecewisePolynomialInterpolator(ppath));
  return true;
}<|endoftext|>


<commit_before>/* Authors: Lutong Wang and Bangqi Xu */
/*
 * Copyright (c) 2019, The Regents of the University of California
 * 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 the University 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 THE REGENTS 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 <fstream>
#include <iostream>

#include "db/tech/frTechObject.h"
#include "dr/FlexDR.h"
#include "frDesign.h"
#include "gc/FlexGC.h"
#include "global.h"
#include "gr/FlexGR.h"
#include "gui/gui.h"
#include "io/io.h"
#include "pa/FlexPA.h"
#include "rp/FlexRP.h"
#include "sta/StaMain.hh"
#include "ta/FlexTA.h"
#include "triton_route/TritonRoute.h"

using namespace std;
using namespace fr;
using namespace triton_route;

namespace sta {
// Tcl files encoded into strings.
extern const char* TritonRoute_tcl_inits[];
}  // namespace sta

extern "C" {
extern int Tritonroute_Init(Tcl_Interp* interp);
}

TritonRoute::TritonRoute()
    : debug_(std::make_unique<frDebugSettings>()),
      num_drvs_(-1),
      gui_(gui::Gui::get())
{
}

TritonRoute::~TritonRoute()
{
}

void TritonRoute::setDebugDR(bool on)
{
  debug_->debugDR = on;
}

void TritonRoute::setDebugMaze(bool on)
{
  debug_->debugMaze = on;
}

void TritonRoute::setDebugPA(bool on)
{
  debug_->debugPA = on;
}

void TritonRoute::setDebugNetName(const char* name)
{
  debug_->netName = name;
}

void TritonRoute::setDebugPinName(const char* name)
{
  debug_->pinName = name;
}

void TritonRoute::setDebugGCell(int x, int y)
{
  debug_->gcellX = x;
  debug_->gcellY = y;
}

void TritonRoute::setDebugIter(int iter)
{
  debug_->iter = iter;
}

void TritonRoute::setDebugPaMarkers(bool on)
{
  debug_->paMarkers = on;
}

int TritonRoute::getNumDRVs() const
{
  if (num_drvs_ < 0) {
    logger_->error(DRT, 2, "Detailed routing has not been run yet.");
  }
  return num_drvs_;
}

void TritonRoute::init(Tcl_Interp* tcl_interp,
                       odb::dbDatabase* db,
                       Logger* logger)
{
  db_ = db;
  logger_ = logger;
  design_ = std::make_unique<frDesign>(logger_);
  // Define swig TCL commands.
  Tritonroute_Init(tcl_interp);
  sta::evalTclInit(tcl_interp, sta::TritonRoute_tcl_inits);
}

void TritonRoute::init()
{
  if (DBPROCESSNODE == "GF14_13M_3Mx_2Cx_4Kx_2Hx_2Gx_LB") {
    VIAINPIN_BOTTOMLAYERNUM = 2;
    VIAINPIN_TOPLAYERNUM = 2;
    USENONPREFTRACKS = false;
    BOTTOM_ROUTING_LAYER = 4;
    TOP_ROUTING_LAYER = 18;
    ENABLE_VIA_GEN = false;
  }

  io::Parser parser(getDesign(), logger_);
  parser.readDb(db_);

  auto tech = getDesign()->getTech();
  if (!BOTTOM_ROUTING_LAYER_NAME.empty()) {
    frLayer* layer = tech->getLayer(BOTTOM_ROUTING_LAYER_NAME);
    if (layer) {
      BOTTOM_ROUTING_LAYER = layer->getLayerNum();
    } else {
      logger_->warn(utl::DRT,
                    251,
                    "bottomRoutingLayer {} not found",
                    BOTTOM_ROUTING_LAYER_NAME);
    }
  }

  if (!TOP_ROUTING_LAYER_NAME.empty()) {
    frLayer* layer = tech->getLayer(TOP_ROUTING_LAYER_NAME);
    if (layer) {
      TOP_ROUTING_LAYER = layer->getLayerNum();
    } else {
      logger_->warn(utl::DRT,
                    252,
                    "bottomRoutingLayer {} not found",
                    TOP_ROUTING_LAYER_NAME);
    }
  }

  if (GUIDE_FILE != string("")) {
    parser.readGuide();
  } else {
    ENABLE_VIA_GEN = false;
  }
  parser.postProcess();
  FlexPA pa(getDesign(), logger_);
  pa.setDebug(debug_.get(), db_);
  pa.main();
  if (GUIDE_FILE != string("")) {
    parser.postProcessGuide();
  }
  // GR-related
  parser.initRPin();
  cout << "BRANCH NDR_testing\n";
}

void TritonRoute::prep()
{
  FlexRP rp(getDesign(), getDesign()->getTech(), logger_);
  rp.main();
}

void TritonRoute::gr()
{
  FlexGR gr(getDesign(), logger_);
  gr.main(db_);
}

void TritonRoute::ta()
{
  FlexTA ta(getDesign(), logger_);
  ta.main();
}

void TritonRoute::dr()
{
  num_drvs_ = -1;
  FlexDR dr(getDesign(), logger_, db_);
  dr.setDebug(debug_.get());
  dr.main();
}

void TritonRoute::endFR()
{
  io::Writer writer(getDesign(), logger_);
  writer.updateDb(db_);
}

void TritonRoute::reportConstraints()
{
  getDesign()->getTech()->printAllConstraints(logger_);
}

int TritonRoute::main()
{
  init();
  if (GUIDE_FILE == string("")) {
    gr();
    io::Parser parser(getDesign(), logger_);
    GUIDE_FILE = OUTGUIDE_FILE;
    ENABLE_VIA_GEN = true;
    parser.readGuide();
    parser.initDefaultVias();
    parser.postProcessGuide();
  }
  prep();
  ta();
  dr();
  endFR();

  num_drvs_ = design_->getTopBlock()->getNumMarkers();

  return 0;
}

void TritonRoute::readParams(const string& fileName)
{
  int readParamCnt = 0;
  ifstream fin(fileName.c_str());
  string line;
  if (fin.is_open()) {
    while (fin.good()) {
      getline(fin, line);
      if (line[0] != '#') {
        char delimiter = ':';
        int pos = line.find(delimiter);
        string field = line.substr(0, pos);
        string value = line.substr(pos + 1);
        stringstream ss(value);
        if (field == "lef") {
          logger_->warn(utl::DRT, 148, "deprecated lef param in params file");
        } else if (field == "def") {
          logger_->warn(utl::DRT, 227, "deprecated def param in params file");
        } else if (field == "guide") {
          GUIDE_FILE = value;
          ++readParamCnt;
        } else if (field == "outputTA") {
          logger_->warn(
              utl::DRT, 171, "deprecated outputTA param in params file");
        } else if (field == "output") {
          logger_->warn(
              utl::DRT, 205, "deprecated output param in params file");
        } else if (field == "outputguide") {
          OUTGUIDE_FILE = value;
          ++readParamCnt;
        } else if (field == "outputMaze") {
          OUT_MAZE_FILE = value;
          ++readParamCnt;
        } else if (field == "outputDRC") {
          DRC_RPT_FILE = value;
          ++readParamCnt;
        } else if (field == "outputCMap") {
          CMAP_FILE = value;
          ++readParamCnt;
        } else if (field == "threads") {
          MAX_THREADS = atoi(value.c_str());
          ++readParamCnt;
        } else if (field == "verbose")
          VERBOSE = atoi(value.c_str());
        else if (field == "dbProcessNode") {
          DBPROCESSNODE = value;
          ++readParamCnt;
        } else if (field == "drouteViaInPinBottomLayerNum") {
          VIAINPIN_BOTTOMLAYERNUM = atoi(value.c_str());
          ++readParamCnt;
        } else if (field == "drouteViaInPinTopLayerNum") {
          VIAINPIN_TOPLAYERNUM = atoi(value.c_str());
          ++readParamCnt;
        } else if (field == "drouteEndIterNum") {
          END_ITERATION = atoi(value.c_str());
          ++readParamCnt;
        } else if (field == "OR_SEED") {
          OR_SEED = atoi(value.c_str());
          ++readParamCnt;
        } else if (field == "OR_K") {
          OR_K = atof(value.c_str());
          ++readParamCnt;
        } else if (field == "bottomRoutingLayer") {
          BOTTOM_ROUTING_LAYER_NAME = value;
          ++readParamCnt;
        } else if (field == "topRoutingLayer") {
          TOP_ROUTING_LAYER_NAME = value;
          ++readParamCnt;
        } else if (field == "initRouteShapeCost") {
          ROUTESHAPECOST = atoi(value.c_str());
          ++readParamCnt;
        }
      }
    }
    fin.close();
  }

  if (readParamCnt < 2) {
    logger_->error(DRT, 1, "Error reading param file: {}", fileName);
  }
}

bool fr::isPad(MacroClassEnum e)
{
  return e == MacroClassEnum::PAD || e == MacroClassEnum::PAD_INPUT
         || e == MacroClassEnum::PAD_OUTPUT || e == MacroClassEnum::PAD_INOUT
         || e == MacroClassEnum::PAD_POWER || e == MacroClassEnum::PAD_SPACER
         || e == MacroClassEnum::PAD_AREAIO;
}

bool fr::isEndcap(MacroClassEnum e)
{
  return e == MacroClassEnum::ENDCAP || e == MacroClassEnum::ENDCAP_PRE
         || e == MacroClassEnum::ENDCAP_POST
         || e == MacroClassEnum::ENDCAP_TOPLEFT
         || e == MacroClassEnum::ENDCAP_TOPRIGHT
         || e == MacroClassEnum::ENDCAP_BOTTOMLEFT
         || e == MacroClassEnum::ENDCAP_BOTTOMRIGHT;
}
<commit_msg>log "BRANCH NDR_testing" in logger<commit_after>/* Authors: Lutong Wang and Bangqi Xu */
/*
 * Copyright (c) 2019, The Regents of the University of California
 * 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 the University 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 THE REGENTS 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 <fstream>
#include <iostream>

#include "db/tech/frTechObject.h"
#include "dr/FlexDR.h"
#include "frDesign.h"
#include "gc/FlexGC.h"
#include "global.h"
#include "gr/FlexGR.h"
#include "gui/gui.h"
#include "io/io.h"
#include "pa/FlexPA.h"
#include "rp/FlexRP.h"
#include "sta/StaMain.hh"
#include "ta/FlexTA.h"
#include "triton_route/TritonRoute.h"

using namespace std;
using namespace fr;
using namespace triton_route;

namespace sta {
// Tcl files encoded into strings.
extern const char* TritonRoute_tcl_inits[];
}  // namespace sta

extern "C" {
extern int Tritonroute_Init(Tcl_Interp* interp);
}

TritonRoute::TritonRoute()
    : debug_(std::make_unique<frDebugSettings>()),
      num_drvs_(-1),
      gui_(gui::Gui::get())
{
}

TritonRoute::~TritonRoute()
{
}

void TritonRoute::setDebugDR(bool on)
{
  debug_->debugDR = on;
}

void TritonRoute::setDebugMaze(bool on)
{
  debug_->debugMaze = on;
}

void TritonRoute::setDebugPA(bool on)
{
  debug_->debugPA = on;
}

void TritonRoute::setDebugNetName(const char* name)
{
  debug_->netName = name;
}

void TritonRoute::setDebugPinName(const char* name)
{
  debug_->pinName = name;
}

void TritonRoute::setDebugGCell(int x, int y)
{
  debug_->gcellX = x;
  debug_->gcellY = y;
}

void TritonRoute::setDebugIter(int iter)
{
  debug_->iter = iter;
}

void TritonRoute::setDebugPaMarkers(bool on)
{
  debug_->paMarkers = on;
}

int TritonRoute::getNumDRVs() const
{
  if (num_drvs_ < 0) {
    logger_->error(DRT, 2, "Detailed routing has not been run yet.");
  }
  return num_drvs_;
}

void TritonRoute::init(Tcl_Interp* tcl_interp,
                       odb::dbDatabase* db,
                       Logger* logger)
{
  db_ = db;
  logger_ = logger;
  design_ = std::make_unique<frDesign>(logger_);
  // Define swig TCL commands.
  Tritonroute_Init(tcl_interp);
  sta::evalTclInit(tcl_interp, sta::TritonRoute_tcl_inits);
}

void TritonRoute::init()
{
  if (DBPROCESSNODE == "GF14_13M_3Mx_2Cx_4Kx_2Hx_2Gx_LB") {
    VIAINPIN_BOTTOMLAYERNUM = 2;
    VIAINPIN_TOPLAYERNUM = 2;
    USENONPREFTRACKS = false;
    BOTTOM_ROUTING_LAYER = 4;
    TOP_ROUTING_LAYER = 18;
    ENABLE_VIA_GEN = false;
  }

  io::Parser parser(getDesign(), logger_);
  parser.readDb(db_);

  auto tech = getDesign()->getTech();
  if (!BOTTOM_ROUTING_LAYER_NAME.empty()) {
    frLayer* layer = tech->getLayer(BOTTOM_ROUTING_LAYER_NAME);
    if (layer) {
      BOTTOM_ROUTING_LAYER = layer->getLayerNum();
    } else {
      logger_->warn(utl::DRT,
                    251,
                    "bottomRoutingLayer {} not found",
                    BOTTOM_ROUTING_LAYER_NAME);
    }
  }

  if (!TOP_ROUTING_LAYER_NAME.empty()) {
    frLayer* layer = tech->getLayer(TOP_ROUTING_LAYER_NAME);
    if (layer) {
      TOP_ROUTING_LAYER = layer->getLayerNum();
    } else {
      logger_->warn(utl::DRT,
                    252,
                    "bottomRoutingLayer {} not found",
                    TOP_ROUTING_LAYER_NAME);
    }
  }

  if (GUIDE_FILE != string("")) {
    parser.readGuide();
  } else {
    ENABLE_VIA_GEN = false;
  }
  parser.postProcess();
  FlexPA pa(getDesign(), logger_);
  pa.setDebug(debug_.get(), db_);
  pa.main();
  if (GUIDE_FILE != string("")) {
    parser.postProcessGuide();
  }
  // GR-related
  parser.initRPin();
  logger_->info(utl::DRT, -1, "BRANCH NDR_testing\n");
}

void TritonRoute::prep()
{
  FlexRP rp(getDesign(), getDesign()->getTech(), logger_);
  rp.main();
}

void TritonRoute::gr()
{
  FlexGR gr(getDesign(), logger_);
  gr.main(db_);
}

void TritonRoute::ta()
{
  FlexTA ta(getDesign(), logger_);
  ta.main();
}

void TritonRoute::dr()
{
  num_drvs_ = -1;
  FlexDR dr(getDesign(), logger_, db_);
  dr.setDebug(debug_.get());
  dr.main();
}

void TritonRoute::endFR()
{
  io::Writer writer(getDesign(), logger_);
  writer.updateDb(db_);
}

void TritonRoute::reportConstraints()
{
  getDesign()->getTech()->printAllConstraints(logger_);
}

int TritonRoute::main()
{
  init();
  if (GUIDE_FILE == string("")) {
    gr();
    io::Parser parser(getDesign(), logger_);
    GUIDE_FILE = OUTGUIDE_FILE;
    ENABLE_VIA_GEN = true;
    parser.readGuide();
    parser.initDefaultVias();
    parser.postProcessGuide();
  }
  prep();
  ta();
  dr();
  endFR();

  num_drvs_ = design_->getTopBlock()->getNumMarkers();

  return 0;
}

void TritonRoute::readParams(const string& fileName)
{
  int readParamCnt = 0;
  ifstream fin(fileName.c_str());
  string line;
  if (fin.is_open()) {
    while (fin.good()) {
      getline(fin, line);
      if (line[0] != '#') {
        char delimiter = ':';
        int pos = line.find(delimiter);
        string field = line.substr(0, pos);
        string value = line.substr(pos + 1);
        stringstream ss(value);
        if (field == "lef") {
          logger_->warn(utl::DRT, 148, "deprecated lef param in params file");
        } else if (field == "def") {
          logger_->warn(utl::DRT, 227, "deprecated def param in params file");
        } else if (field == "guide") {
          GUIDE_FILE = value;
          ++readParamCnt;
        } else if (field == "outputTA") {
          logger_->warn(
              utl::DRT, 171, "deprecated outputTA param in params file");
        } else if (field == "output") {
          logger_->warn(
              utl::DRT, 205, "deprecated output param in params file");
        } else if (field == "outputguide") {
          OUTGUIDE_FILE = value;
          ++readParamCnt;
        } else if (field == "outputMaze") {
          OUT_MAZE_FILE = value;
          ++readParamCnt;
        } else if (field == "outputDRC") {
          DRC_RPT_FILE = value;
          ++readParamCnt;
        } else if (field == "outputCMap") {
          CMAP_FILE = value;
          ++readParamCnt;
        } else if (field == "threads") {
          MAX_THREADS = atoi(value.c_str());
          ++readParamCnt;
        } else if (field == "verbose")
          VERBOSE = atoi(value.c_str());
        else if (field == "dbProcessNode") {
          DBPROCESSNODE = value;
          ++readParamCnt;
        } else if (field == "drouteViaInPinBottomLayerNum") {
          VIAINPIN_BOTTOMLAYERNUM = atoi(value.c_str());
          ++readParamCnt;
        } else if (field == "drouteViaInPinTopLayerNum") {
          VIAINPIN_TOPLAYERNUM = atoi(value.c_str());
          ++readParamCnt;
        } else if (field == "drouteEndIterNum") {
          END_ITERATION = atoi(value.c_str());
          ++readParamCnt;
        } else if (field == "OR_SEED") {
          OR_SEED = atoi(value.c_str());
          ++readParamCnt;
        } else if (field == "OR_K") {
          OR_K = atof(value.c_str());
          ++readParamCnt;
        } else if (field == "bottomRoutingLayer") {
          BOTTOM_ROUTING_LAYER_NAME = value;
          ++readParamCnt;
        } else if (field == "topRoutingLayer") {
          TOP_ROUTING_LAYER_NAME = value;
          ++readParamCnt;
        } else if (field == "initRouteShapeCost") {
          ROUTESHAPECOST = atoi(value.c_str());
          ++readParamCnt;
        }
      }
    }
    fin.close();
  }

  if (readParamCnt < 2) {
    logger_->error(DRT, 1, "Error reading param file: {}", fileName);
  }
}

bool fr::isPad(MacroClassEnum e)
{
  return e == MacroClassEnum::PAD || e == MacroClassEnum::PAD_INPUT
         || e == MacroClassEnum::PAD_OUTPUT || e == MacroClassEnum::PAD_INOUT
         || e == MacroClassEnum::PAD_POWER || e == MacroClassEnum::PAD_SPACER
         || e == MacroClassEnum::PAD_AREAIO;
}

bool fr::isEndcap(MacroClassEnum e)
{
  return e == MacroClassEnum::ENDCAP || e == MacroClassEnum::ENDCAP_PRE
         || e == MacroClassEnum::ENDCAP_POST
         || e == MacroClassEnum::ENDCAP_TOPLEFT
         || e == MacroClassEnum::ENDCAP_TOPRIGHT
         || e == MacroClassEnum::ENDCAP_BOTTOMLEFT
         || e == MacroClassEnum::ENDCAP_BOTTOMRIGHT;
}
<|endoftext|>


<commit_before>#include <sch/S_Polyhedron/Polyhedron_algorithms.h>

#include <sch/File_Parsing/SimplestParsing.h>

#include <fstream>
#include <stdexcept>
#include <string>
#include <algorithm>
#include <set>

//#define CD_POLYHEDRON_ALGORITHM_VERBOSE_MODE //VERBOSE mode (slows down the algorithm) default is commented

using namespace sch;

Polyhedron_algorithms::Polyhedron_algorithms(void)
    : fastVertexes_(0x0),
      lastVertexes_(0x0),
      numberOfVertices_(0)
{

}

Polyhedron_algorithms::Polyhedron_algorithms(const Polyhedron_algorithms &p)
  :triangles_(p.triangles_)
  ,fastVertexes_(0x0)
  ,lastVertexes_(0x0)
{
  for (unsigned i=0; i<p.vertexes_.size(); ++i)
  {
    vertexes_.push_back(p.vertexes_[i]->clone());
  }

  updateVertexNeighbors();

  updateFastArrays();
}

Polyhedron_algorithms::~Polyhedron_algorithms(void)
{
  if (fastVertexes_!=NULL)
  {
    delete[] fastVertexes_;
  }
  for (unsigned int i=0; i<vertexes_.size(); ++i)
  {
    delete vertexes_[i];
  }
}

const Polyhedron_algorithms& Polyhedron_algorithms::operator =(const Polyhedron_algorithms &p)
{
  if (this==&p)
  {
    return *this;
  }
  else
  {
    clear();
    triangles_=p.triangles_;

    for (unsigned i=0; i<p.vertexes_.size(); ++i)
    {
      vertexes_.push_back(p.vertexes_[i]->clone());
    }

    updateVertexNeighbors();

    updateFastArrays();

    return *this;
  }
}

size_t Polyhedron_algorithms::getEdgeKey(PolyhedronEdge e)
{
  return ((e.a<e.b)?(e.a*vertexes_.size()+e.b):(e.b*vertexes_.size()+e.a));
}

void Polyhedron_algorithms::fillEdges()
{
  edges_.clear();
  PolyhedronEdge edge;
  std::vector<PolyhedronEdge> triangleEdges;
  std::set<size_t, std::greater<int>> edgesSet;
  for(size_t i = 0; i < triangles_.size(); i++)
  {
    edge.a = triangles_[i].a;
    edge.b = triangles_[i].b;
    edge.computeEdge(vertexes_);
    triangleEdges.push_back(edge);

    edge.a = triangles_[i].b;
    edge.b = triangles_[i].c;
    edge.computeEdge(vertexes_);
    triangleEdges.push_back(edge);

    edge.a = triangles_[i].c;
    edge.b = triangles_[i].a;
    edge.computeEdge(vertexes_);
    triangleEdges.push_back(edge);

    for(auto j = triangleEdges.begin(); j != triangleEdges.end(); j++)
    {
      if(!edgesSet.count(getEdgeKey(*j)))
      {
        edges_.push_back(*j);
        edgesSet.insert(getEdgeKey(*j));
      }
    }
    triangleEdges.clear();
  }
}

void Polyhedron_algorithms::openFromFile(const std::string &filename)
{
  clear();

  std::ifstream is(filename.c_str());
  if(!is.is_open())
  {
    std::stringstream errmsg;
    errmsg << "EXCEPTION: Unable to open File " << filename << std::endl;
    throw std::invalid_argument(errmsg.str());
  }

  std::string line;

  // Discard first line
  std::getline(is, line);
  size_t nr_vertexes;
  size_t nr_faces;
  is>>nr_vertexes; //get the number of points
  vertexes_.reserve(nr_vertexes);
  is>>nr_faces;
  triangles_.reserve(nr_faces);
  // Discard the last number
  std::getline(is, line);
  for (size_t g=0; g<nr_vertexes; g++)
  {
    Scalar y[3];
    is >> y[0] >> y[1] >> y[2];//get the coords

    S_PolyhedronVertex *v;
    v=new S_PolyhedronVertex();
    v->setCoordinates(y[0],y[1],y[2]);
    v->setNumber(unsigned (vertexes_.size()));
    vertexes_.push_back(v);

  }

  //get the normals
  const size_t normalSearchLen = 13;
  const char normalSearch[normalSearchLen + 1] = "    - normal:";
  const size_t verticesSearchLen = 15;
  const char verticesSearch[verticesSearchLen + 1] = "    - vertices:";
  PolyhedronEdge edge;
  Vector3 p1, p2;
  while(std::getline(is, line).good())
  {
    if(line.substr(0, normalSearchLen) == normalSearch)
    {
      std::stringstream ss;
      ss << line.substr(normalSearchLen);
      Scalar y[3];

      PolyhedronTriangle t;

      ss>>y[0];
      ss>>y[1];
      ss>>y[2];

      t.normal.Set(y[0],y[1],y[2]);
      t.normal.normalize();

      while(std::getline(is, line).good())
      {
        if(line.substr(0, verticesSearchLen) == verticesSearch)
        {
          std::stringstream ss;
          ss << line.substr(verticesSearchLen);
          char c = '\0';
          while(c!='p' && ss.good()) { ss.get(c); }
          ss >> t.a;
          c = '\0';
          while(c!='p' && ss.good()) { ss.get(c); }
          ss >> t.b;
          c = '\0';
          while(c!='p' && ss.good()) { ss.get(c); }
          ss >> t.c;

          //updatingNeighbors
          vertexes_[t.a]->addNeighbor(vertexes_[t.b]);
          vertexes_[t.a]->addNeighbor(vertexes_[t.c]);

          vertexes_[t.b]->addNeighbor(vertexes_[t.a]);
          vertexes_[t.b]->addNeighbor(vertexes_[t.c]);

          vertexes_[t.c]->addNeighbor(vertexes_[t.a]);
          vertexes_[t.c]->addNeighbor(vertexes_[t.b]);

          triangles_.push_back(t);

          break;
        }
      }
    }
  }

  for (unsigned int i=0; i<vertexes_.size(); i++)
  {
    vertexes_[i]->updateFastArrays();
  }

  deleteVertexesWithoutNeighbors();

  fillEdges();
}

void Polyhedron_algorithms::updateVertexNeighbors()
{
  for (unsigned i=0; i<triangles_.size(); ++i)
  {
    //updatingNeighbors
    vertexes_[triangles_[i].a]->addNeighbor(vertexes_[triangles_[i].b]);
    vertexes_[triangles_[i].a]->addNeighbor(vertexes_[triangles_[i].c]);

    vertexes_[triangles_[i].b]->addNeighbor(vertexes_[triangles_[i].a]);
    vertexes_[triangles_[i].b]->addNeighbor(vertexes_[triangles_[i].c]);

    vertexes_[triangles_[i].c]->addNeighbor(vertexes_[triangles_[i].a]);
    vertexes_[triangles_[i].c]->addNeighbor(vertexes_[triangles_[i].b]);
  }

  for (unsigned i=0; i<vertexes_.size(); ++i)
  {
    vertexes_[i]->updateFastArrays();
  }
}

void Polyhedron_algorithms::clear()
{
  for (unsigned int i=0; i<vertexes_.size(); ++i)
  {
    delete vertexes_[i];
  }
  vertexes_.clear();
  triangles_.clear();
  edges_.clear();
  updateFastArrays();
}

void Polyhedron_algorithms::clearNeighbors()
{
  for (unsigned i=0; i<vertexes_.size(); ++i)
  {
    vertexes_[i]->clearNeighbors();
    vertexes_[i]->updateFastArrays();
  }
}

void Polyhedron_algorithms::updateFastArrays()
{
  if (fastVertexes_!=NULL)
  {
    delete[] fastVertexes_;
  }
  numberOfVertices_ = unsigned(vertexes_.size());
  if (numberOfVertices_ > 0)
  {
    fastVertexes_ = new S_PolyhedronVertex *[numberOfVertices_];
    for (unsigned int i = 0; i < numberOfVertices_; ++i)
    {
      fastVertexes_[i]=vertexes_[i];
    }

    lastVertexes_ = &(fastVertexes_[numberOfVertices_]);
  }
  else
  {
    fastVertexes_=lastVertexes_=NULL;
  }
}

Point3 Polyhedron_algorithms::naiveSupport(const Vector3&v)const
{
  S_PolyhedronVertex** current;

  current=fastVertexes_;

  Scalar supportH=(*current)->supportH(v);

  Vector3 best=(*current)->getCoordinates();

  current++;

  for (unsigned i = 1; i < numberOfVertices_; i++, current++)
  {
    if ((*current)->supportH(v) > supportH)
    {
      supportH = (*current)->supportH(v);
      best = (*current)->getCoordinates();
    }
  }

  return best;
}

Point3 Polyhedron_algorithms::support(const Vector3&v,int &lastFeature)const
{
  S_PolyhedronVertex* current;
  Scalar supportH;

  if (numberOfVertices_==0)
  {
    std::stringstream errmsg;
    errmsg << "The polyhedron is empty, impossible to compute support function " << std::endl;
    throw std::length_error(errmsg.str());
  }

  if (lastFeature==-1)
  {
    current=*fastVertexes_;
  }
  else
  {
    current=fastVertexes_[lastFeature];
  }

  bool b=current->isHere(v);

  unsigned iterations = 0;

  while (!b)
  {
    supportH= current->getNextVertexH();
    current = current->getNextVertex();
    b=current->isHere(v,supportH);
    ++iterations;

    /// if the number of iterations is bigger than the number of vertices it means that we entered an infinite loop
    ///the best is te return the support computed using the naive version
    if (iterations>numberOfVertices_)
    {
#ifdef CD_POLYHEDRON_ALGORITHM_VERBOSE_MODE
      std::cout << "Problem Support Polyhedron, Naive method triggered" << std::endl;
#endif
      return naiveSupport(v);
    }
  }

  lastFeature=current->getNumber();

  return current->getCoordinates();
}

void Polyhedron_algorithms::deleteVertexesWithoutNeighbors()
{
  int *cache=new int[vertexes_.size()];
  std::vector<S_PolyhedronVertex*> v;
  int index=0;

  for (unsigned i=0; i<vertexes_.size(); ++i)
  {
    if (vertexes_[i]->getNumNeighbors()>0)
    {
      v.push_back(vertexes_[i]);
      vertexes_[i]->setNumber(index);
      cache[i]=index++;
    }
    else
    {
      delete vertexes_[i];
      cache[i]=-1;
    }
  }

  for (unsigned i=0; i<triangles_.size(); ++i)
  {
    triangles_[i].a=cache[triangles_[i].a];
    triangles_[i].b=cache[triangles_[i].b];
    triangles_[i].c=cache[triangles_[i].c];
  }

  vertexes_=v;

  updateFastArrays();

  delete[] cache;

}
<commit_msg>Remove unused variables<commit_after>#include <sch/S_Polyhedron/Polyhedron_algorithms.h>

#include <sch/File_Parsing/SimplestParsing.h>

#include <fstream>
#include <stdexcept>
#include <string>
#include <algorithm>
#include <set>

//#define CD_POLYHEDRON_ALGORITHM_VERBOSE_MODE //VERBOSE mode (slows down the algorithm) default is commented

using namespace sch;

Polyhedron_algorithms::Polyhedron_algorithms(void)
    : fastVertexes_(0x0),
      lastVertexes_(0x0),
      numberOfVertices_(0)
{

}

Polyhedron_algorithms::Polyhedron_algorithms(const Polyhedron_algorithms &p)
  :triangles_(p.triangles_)
  ,fastVertexes_(0x0)
  ,lastVertexes_(0x0)
{
  for (unsigned i=0; i<p.vertexes_.size(); ++i)
  {
    vertexes_.push_back(p.vertexes_[i]->clone());
  }

  updateVertexNeighbors();

  updateFastArrays();
}

Polyhedron_algorithms::~Polyhedron_algorithms(void)
{
  if (fastVertexes_!=NULL)
  {
    delete[] fastVertexes_;
  }
  for (unsigned int i=0; i<vertexes_.size(); ++i)
  {
    delete vertexes_[i];
  }
}

const Polyhedron_algorithms& Polyhedron_algorithms::operator =(const Polyhedron_algorithms &p)
{
  if (this==&p)
  {
    return *this;
  }
  else
  {
    clear();
    triangles_=p.triangles_;

    for (unsigned i=0; i<p.vertexes_.size(); ++i)
    {
      vertexes_.push_back(p.vertexes_[i]->clone());
    }

    updateVertexNeighbors();

    updateFastArrays();

    return *this;
  }
}

size_t Polyhedron_algorithms::getEdgeKey(PolyhedronEdge e)
{
  return ((e.a<e.b)?(e.a*vertexes_.size()+e.b):(e.b*vertexes_.size()+e.a));
}

void Polyhedron_algorithms::fillEdges()
{
  edges_.clear();
  PolyhedronEdge edge;
  std::vector<PolyhedronEdge> triangleEdges;
  std::set<size_t, std::greater<int>> edgesSet;
  for(size_t i = 0; i < triangles_.size(); i++)
  {
    edge.a = triangles_[i].a;
    edge.b = triangles_[i].b;
    edge.computeEdge(vertexes_);
    triangleEdges.push_back(edge);

    edge.a = triangles_[i].b;
    edge.b = triangles_[i].c;
    edge.computeEdge(vertexes_);
    triangleEdges.push_back(edge);

    edge.a = triangles_[i].c;
    edge.b = triangles_[i].a;
    edge.computeEdge(vertexes_);
    triangleEdges.push_back(edge);

    for(auto j = triangleEdges.begin(); j != triangleEdges.end(); j++)
    {
      if(!edgesSet.count(getEdgeKey(*j)))
      {
        edges_.push_back(*j);
        edgesSet.insert(getEdgeKey(*j));
      }
    }
    triangleEdges.clear();
  }
}

void Polyhedron_algorithms::openFromFile(const std::string &filename)
{
  clear();

  std::ifstream is(filename.c_str());
  if(!is.is_open())
  {
    std::stringstream errmsg;
    errmsg << "EXCEPTION: Unable to open File " << filename << std::endl;
    throw std::invalid_argument(errmsg.str());
  }

  std::string line;

  // Discard first line
  std::getline(is, line);
  size_t nr_vertexes;
  size_t nr_faces;
  is>>nr_vertexes; //get the number of points
  vertexes_.reserve(nr_vertexes);
  is>>nr_faces;
  triangles_.reserve(nr_faces);
  // Discard the last number
  std::getline(is, line);
  for (size_t g=0; g<nr_vertexes; g++)
  {
    Scalar y[3];
    is >> y[0] >> y[1] >> y[2];//get the coords

    S_PolyhedronVertex *v;
    v=new S_PolyhedronVertex();
    v->setCoordinates(y[0],y[1],y[2]);
    v->setNumber(unsigned (vertexes_.size()));
    vertexes_.push_back(v);

  }

  //get the normals
  const size_t normalSearchLen = 13;
  const char normalSearch[normalSearchLen + 1] = "    - normal:";
  const size_t verticesSearchLen = 15;
  const char verticesSearch[verticesSearchLen + 1] = "    - vertices:";
  PolyhedronEdge edge;

  while(std::getline(is, line).good())
  {
    if(line.substr(0, normalSearchLen) == normalSearch)
    {
      std::stringstream ss;
      ss << line.substr(normalSearchLen);
      Scalar y[3];

      PolyhedronTriangle t;

      ss>>y[0];
      ss>>y[1];
      ss>>y[2];

      t.normal.Set(y[0],y[1],y[2]);
      t.normal.normalize();

      while(std::getline(is, line).good())
      {
        if(line.substr(0, verticesSearchLen) == verticesSearch)
        {
          std::stringstream ss;
          ss << line.substr(verticesSearchLen);
          char c = '\0';
          while(c!='p' && ss.good()) { ss.get(c); }
          ss >> t.a;
          c = '\0';
          while(c!='p' && ss.good()) { ss.get(c); }
          ss >> t.b;
          c = '\0';
          while(c!='p' && ss.good()) { ss.get(c); }
          ss >> t.c;

          //updatingNeighbors
          vertexes_[t.a]->addNeighbor(vertexes_[t.b]);
          vertexes_[t.a]->addNeighbor(vertexes_[t.c]);

          vertexes_[t.b]->addNeighbor(vertexes_[t.a]);
          vertexes_[t.b]->addNeighbor(vertexes_[t.c]);

          vertexes_[t.c]->addNeighbor(vertexes_[t.a]);
          vertexes_[t.c]->addNeighbor(vertexes_[t.b]);

          triangles_.push_back(t);

          break;
        }
      }
    }
  }

  for (unsigned int i=0; i<vertexes_.size(); i++)
  {
    vertexes_[i]->updateFastArrays();
  }

  deleteVertexesWithoutNeighbors();

  fillEdges();
}

void Polyhedron_algorithms::updateVertexNeighbors()
{
  for (unsigned i=0; i<triangles_.size(); ++i)
  {
    //updatingNeighbors
    vertexes_[triangles_[i].a]->addNeighbor(vertexes_[triangles_[i].b]);
    vertexes_[triangles_[i].a]->addNeighbor(vertexes_[triangles_[i].c]);

    vertexes_[triangles_[i].b]->addNeighbor(vertexes_[triangles_[i].a]);
    vertexes_[triangles_[i].b]->addNeighbor(vertexes_[triangles_[i].c]);

    vertexes_[triangles_[i].c]->addNeighbor(vertexes_[triangles_[i].a]);
    vertexes_[triangles_[i].c]->addNeighbor(vertexes_[triangles_[i].b]);
  }

  for (unsigned i=0; i<vertexes_.size(); ++i)
  {
    vertexes_[i]->updateFastArrays();
  }
}

void Polyhedron_algorithms::clear()
{
  for (unsigned int i=0; i<vertexes_.size(); ++i)
  {
    delete vertexes_[i];
  }
  vertexes_.clear();
  triangles_.clear();
  edges_.clear();
  updateFastArrays();
}

void Polyhedron_algorithms::clearNeighbors()
{
  for (unsigned i=0; i<vertexes_.size(); ++i)
  {
    vertexes_[i]->clearNeighbors();
    vertexes_[i]->updateFastArrays();
  }
}

void Polyhedron_algorithms::updateFastArrays()
{
  if (fastVertexes_!=NULL)
  {
    delete[] fastVertexes_;
  }
  numberOfVertices_ = unsigned(vertexes_.size());
  if (numberOfVertices_ > 0)
  {
    fastVertexes_ = new S_PolyhedronVertex *[numberOfVertices_];
    for (unsigned int i = 0; i < numberOfVertices_; ++i)
    {
      fastVertexes_[i]=vertexes_[i];
    }

    lastVertexes_ = &(fastVertexes_[numberOfVertices_]);
  }
  else
  {
    fastVertexes_=lastVertexes_=NULL;
  }
}

Point3 Polyhedron_algorithms::naiveSupport(const Vector3&v)const
{
  S_PolyhedronVertex** current;

  current=fastVertexes_;

  Scalar supportH=(*current)->supportH(v);

  Vector3 best=(*current)->getCoordinates();

  current++;

  for (unsigned i = 1; i < numberOfVertices_; i++, current++)
  {
    if ((*current)->supportH(v) > supportH)
    {
      supportH = (*current)->supportH(v);
      best = (*current)->getCoordinates();
    }
  }

  return best;
}

Point3 Polyhedron_algorithms::support(const Vector3&v,int &lastFeature)const
{
  S_PolyhedronVertex* current;
  Scalar supportH;

  if (numberOfVertices_==0)
  {
    std::stringstream errmsg;
    errmsg << "The polyhedron is empty, impossible to compute support function " << std::endl;
    throw std::length_error(errmsg.str());
  }

  if (lastFeature==-1)
  {
    current=*fastVertexes_;
  }
  else
  {
    current=fastVertexes_[lastFeature];
  }

  bool b=current->isHere(v);

  unsigned iterations = 0;

  while (!b)
  {
    supportH= current->getNextVertexH();
    current = current->getNextVertex();
    b=current->isHere(v,supportH);
    ++iterations;

    /// if the number of iterations is bigger than the number of vertices it means that we entered an infinite loop
    ///the best is te return the support computed using the naive version
    if (iterations>numberOfVertices_)
    {
#ifdef CD_POLYHEDRON_ALGORITHM_VERBOSE_MODE
      std::cout << "Problem Support Polyhedron, Naive method triggered" << std::endl;
#endif
      return naiveSupport(v);
    }
  }

  lastFeature=current->getNumber();

  return current->getCoordinates();
}

void Polyhedron_algorithms::deleteVertexesWithoutNeighbors()
{
  int *cache=new int[vertexes_.size()];
  std::vector<S_PolyhedronVertex*> v;
  int index=0;

  for (unsigned i=0; i<vertexes_.size(); ++i)
  {
    if (vertexes_[i]->getNumNeighbors()>0)
    {
      v.push_back(vertexes_[i]);
      vertexes_[i]->setNumber(index);
      cache[i]=index++;
    }
    else
    {
      delete vertexes_[i];
      cache[i]=-1;
    }
  }

  for (unsigned i=0; i<triangles_.size(); ++i)
  {
    triangles_[i].a=cache[triangles_[i].a];
    triangles_[i].b=cache[triangles_[i].b];
    triangles_[i].c=cache[triangles_[i].c];
  }

  vertexes_=v;

  updateFastArrays();

  delete[] cache;

}
<|endoftext|>


<commit_before>// bsls_assert.cpp                                                    -*-C++-*-
#include <bsls_assert.h>

#include <bsls_ident.h>
BSLS_IDENT("$Id$ $CSID$")

#include <bsls_asserttestexception.h>
#include <bsls_pointercastutil.h>
#include <bsls_types.h>
#include <bsls_log.h>
#include <bsls_logseverity.h>

#include <exception>

#include <cstdio>
#include <cstdlib>
#include <cstring>

#ifdef BSLS_PLATFORM_OS_AIX
#include <signal.h>
#endif

#ifdef BSLS_PLATFORM_OS_UNIX
#include <unistd.h>    // 'sleep'
#endif

#ifdef BSLS_PLATFORM_OS_WINDOWS
#include <windows.h>   // IsDebbugerPresent
#include <crtdbg.h>    // '_CrtSetReportMode', to suppress pop-ups

typedef unsigned long DWORD;

extern "C" {
    __declspec(dllimport) void __stdcall Sleep(DWORD dwMilliseconds);
};
#endif

#ifdef BSLS_ASSERT_NORETURN
#error BSLS_ASSERT_NORETURN must be a macro scoped locally to this file
#endif

// Note that a portable syntax for 'noreturn' will be available once we have
// access to conforming C++0x compilers.
//# define BSLS_ASSERT_NORETURN [[noreturn]]

#ifdef BSLS_PLATFORM_CMP_MSVC
#   define BSLS_ASSERT_NORETURN __declspec(noreturn)
#else
#   define BSLS_ASSERT_NORETURN
#endif

static
BloombergLP::bsls::AtomicOperations_Imp::AtomicTypes::Int g_failureReturnCount;

namespace BloombergLP {

#ifndef BDE_OMIT_INTERNAL_DEPRECATED
// We want to print the error message to 'stderr', not 'stdout'.   The old
// documentation for 'printError' is:
//..
//  Print a formatted error message to standard output.  (Most Bloomberg
//  processes will send standard output to a log file.)
//..
// TBD: find out whether 'stderr' goes to 'act.log'.
#endif // BDE_OMIT_INTERNAL_DEPRECATED

static
void printError(const char *text, const char *file, int line)
    // Print a formatted error message to 'stderr' using the specified
    // expression 'text', 'file' name, and 'line' number.  If either
    // 'text' or 'file' is empty ("") or null (0), replace it with some
    // informative, "human-readable" text, before formatting.
{
    if (!text) {
        text = "(* Unspecified Expression Text *)";
    }
    else if (!*text) {
        text = "(* Empty Expression Text *)";
    }

    if (!file) {
        file = "(* Unspecified File Name *)";
    }
    else if (!*file) {
        file = "(* Empty File Name *)";
    }

    bsls::Log::logFormattedMessage(
        bsls::LogSeverity::e_ERROR, file, line, "Assertion failed: %s", text);
}

namespace bsls {

                                // ------------
                                // class Assert
                                // ------------

// CLASS DATA
bsls::AtomicOperations::AtomicTypes::Pointer
    Assert::s_handler = {(void *) &Assert::failAbort};
bsls::AtomicOperations::AtomicTypes::Int Assert::s_lockedFlag = {0};

// CLASS METHODS
void Assert::setFailureHandlerRaw(Assert::Handler function)
{
    bsls::AtomicOperations::setPtrRelease(
        &s_handler, PointerCastUtil::cast<void *>(function));
}

void Assert::setFailureHandler(Assert::Handler function)
{
    if (!bsls::AtomicOperations::getIntRelaxed(&s_lockedFlag)) {
        setFailureHandlerRaw(function);
    }
}

void Assert::lockAssertAdministration()
{
    bsls::AtomicOperations::setIntRelaxed(&s_lockedFlag, 1);
}

Assert::Handler Assert::failureHandler()
{
    return (Handler) bsls::AtomicOperations::getPtrAcquire(&s_handler);
}

                       // Macro Dispatcher Method

BSLS_ASSERT_NORETURN_INVOKE_HANDLER
void Assert::invokeHandler(const char *text, const char *file, int line)
{
    Assert::Handler currentHandlerAddress = failureHandler();

    currentHandlerAddress(text, file, line);

    // The failure handler should not return.  If a returning failure handler
    // has been installed, alert the user that the program is continuing to
    // run.

    unsigned count = static_cast<unsigned>(
                AtomicOperations::incrementIntNvAcqRel(&g_failureReturnCount));

    if (BSLS_PERFORMANCEHINT_PREDICT_UNLIKELY(0 == (count & (count - 1)))) {
        BSLS_PERFORMANCEHINT_UNLIKELY_HINT;

        // Log when 'count' is a power of 2.

        if (count == (1 << 30)) {
            // Avoid undefined behavior by resetting the counter.

            AtomicOperations::setInt(&g_failureReturnCount, 1 << 29);
        }

        Log::logFormattedMessage(LogSeverity::e_FATAL,
                                 file,
                                 line,
                                 "BSLS_ASSERT failure: '%s'",
                                 text);

        BSLS_LOG_FATAL("Bad 'bsls_assert' configuration: "
                       "violation handler at %p failed to prevent program "
                       "from continuing.",
                       currentHandlerAddress);
    }

#ifdef BSLS_ASSERT_ENABLE_NORETURN_FOR_INVOKE_HANDLER
    std::abort();
#endif
}

                     // Standard Assertion-Failure Handlers

BSLS_ASSERT_NORETURN
void Assert::failAbort(const char *text, const char *file, int line)
{
    printError(text, file, line);

#ifndef BDE_OMIT_INTERNAL_DEPRECATED
    // See DRQS 8923441: The following is a work-around for a Fortran compiler bug.
#endif // BDE_OMIT_INTERNAL_DEPRECATED

#ifdef BSLS_PLATFORM_OS_AIX
    sigset_t newset;
    sigemptyset(&newset);
    sigaddset(&newset, SIGABRT);
#if defined(BDE_BUILD_TARGET_MT)
    pthread_sigmask(SIG_UNBLOCK, &newset, 0);
#else
    sigprocmask(SIG_UNBLOCK, &newset, 0);
#endif
#endif

#ifndef BDE_OMIT_INTERNAL_DEPRECATED
    // See DRQS 13882128: Note that (according to Oleg) the first line alone may be
    // sufficient.
#endif // BDE_OMIT_INTERNAL_DEPRECATED

#ifdef BSLS_PLATFORM_OS_WINDOWS
    // The following configures the runtime library on how to report asserts,
    // errors, and warnings in order to avoid pop-up windows when 'abort' is
    // called.
    if (!IsDebuggerPresent()) {
        _CrtSetReportMode(_CRT_ASSERT, 0);
        _CrtSetReportMode(_CRT_ERROR, 0);
        _CrtSetReportMode(_CRT_WARN, 0);
    }
#endif

    std::abort();
}

BSLS_ASSERT_NORETURN
void Assert::failSleep(const char *text, const char *file, int line)
{
    printError(text, file, line);

    volatile int sleepDuration = 1;

    while (1 == sleepDuration) {

#if defined(BSLS_PLATFORM_OS_UNIX)
        sleep(sleepDuration);
#elif defined(BSLS_PLATFORM_OS_WINDOWS)
        Sleep(sleepDuration * 1000);  // milliseconds
#else
        #error "Do not know how to sleep on this platform."
#endif

    }

    // We will never reach this line, but it is needed to let the compiler know
    // that this function does not return.

    std::abort();
}

BSLS_ASSERT_NORETURN
void Assert::failThrow(const char *text, const char *file, int line)
{

#ifdef BDE_BUILD_TARGET_EXC
    if (!std::uncaught_exception()) {
        throw AssertTestException(text, file, line);
    }
    else {
        bsls::Log::logMessage(bsls::LogSeverity::e_ERROR, file, line,
                "BSLS_ASSERT: An uncaught exception is pending;"
                " cannot throw 'AssertTestException'.");
    }
#endif

    failAbort(text, file, line);
}

}  // close package namespace

#undef BSLS_ASSERT_NORETURN

namespace bsls {

                    // -------------------------------
                    // class AssertFailureHandlerGuard
                    // -------------------------------

AssertFailureHandlerGuard::AssertFailureHandlerGuard(Assert::Handler temporary)
: d_original(Assert::failureHandler())
{
    Assert::setFailureHandlerRaw(temporary);
}

AssertFailureHandlerGuard::~AssertFailureHandlerGuard()
{
    Assert::setFailureHandlerRaw(d_original);
}

}  // close package namespace

}  // close enterprise namespace

// ----------------------------------------------------------------------------
// Copyright 2013 Bloomberg Finance L.P.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// ----------------------------- END-OF-FILE ----------------------------------
<commit_msg>Move handler return counter into 'invokeHandler'.<commit_after>// bsls_assert.cpp                                                    -*-C++-*-
#include <bsls_assert.h>

#include <bsls_ident.h>
BSLS_IDENT("$Id$ $CSID$")

#include <bsls_asserttestexception.h>
#include <bsls_pointercastutil.h>
#include <bsls_types.h>
#include <bsls_log.h>
#include <bsls_logseverity.h>

#include <exception>

#include <cstdio>
#include <cstdlib>
#include <cstring>

#ifdef BSLS_PLATFORM_OS_AIX
#include <signal.h>
#endif

#ifdef BSLS_PLATFORM_OS_UNIX
#include <unistd.h>    // 'sleep'
#endif

#ifdef BSLS_PLATFORM_OS_WINDOWS
#include <windows.h>   // IsDebbugerPresent
#include <crtdbg.h>    // '_CrtSetReportMode', to suppress pop-ups

typedef unsigned long DWORD;

extern "C" {
    __declspec(dllimport) void __stdcall Sleep(DWORD dwMilliseconds);
};
#endif

#ifdef BSLS_ASSERT_NORETURN
#error BSLS_ASSERT_NORETURN must be a macro scoped locally to this file
#endif

// Note that a portable syntax for 'noreturn' will be available once we have
// access to conforming C++0x compilers.
//# define BSLS_ASSERT_NORETURN [[noreturn]]

#ifdef BSLS_PLATFORM_CMP_MSVC
#   define BSLS_ASSERT_NORETURN __declspec(noreturn)
#else
#   define BSLS_ASSERT_NORETURN
#endif

namespace BloombergLP {

#ifndef BDE_OMIT_INTERNAL_DEPRECATED
// We want to print the error message to 'stderr', not 'stdout'.   The old
// documentation for 'printError' is:
//..
//  Print a formatted error message to standard output.  (Most Bloomberg
//  processes will send standard output to a log file.)
//..
// TBD: find out whether 'stderr' goes to 'act.log'.
#endif // BDE_OMIT_INTERNAL_DEPRECATED

static
void printError(const char *text, const char *file, int line)
    // Print a formatted error message to 'stderr' using the specified
    // expression 'text', 'file' name, and 'line' number.  If either
    // 'text' or 'file' is empty ("") or null (0), replace it with some
    // informative, "human-readable" text, before formatting.
{
    if (!text) {
        text = "(* Unspecified Expression Text *)";
    }
    else if (!*text) {
        text = "(* Empty Expression Text *)";
    }

    if (!file) {
        file = "(* Unspecified File Name *)";
    }
    else if (!*file) {
        file = "(* Empty File Name *)";
    }

    bsls::Log::logFormattedMessage(
        bsls::LogSeverity::e_ERROR, file, line, "Assertion failed: %s", text);
}

namespace bsls {

                                // ------------
                                // class Assert
                                // ------------

// CLASS DATA
bsls::AtomicOperations::AtomicTypes::Pointer
    Assert::s_handler = {(void *) &Assert::failAbort};
bsls::AtomicOperations::AtomicTypes::Int Assert::s_lockedFlag = {0};

// CLASS METHODS
void Assert::setFailureHandlerRaw(Assert::Handler function)
{
    bsls::AtomicOperations::setPtrRelease(
        &s_handler, PointerCastUtil::cast<void *>(function));
}

void Assert::setFailureHandler(Assert::Handler function)
{
    if (!bsls::AtomicOperations::getIntRelaxed(&s_lockedFlag)) {
        setFailureHandlerRaw(function);
    }
}

void Assert::lockAssertAdministration()
{
    bsls::AtomicOperations::setIntRelaxed(&s_lockedFlag, 1);
}

Assert::Handler Assert::failureHandler()
{
    return (Handler) bsls::AtomicOperations::getPtrAcquire(&s_handler);
}

                       // Macro Dispatcher Method

BSLS_ASSERT_NORETURN_INVOKE_HANDLER
void Assert::invokeHandler(const char *text, const char *file, int line)
{
    static AtomicOperations::AtomicTypes::Int failureReturnCount = {0};

    Assert::Handler currentHandlerAddress = failureHandler();

    currentHandlerAddress(text, file, line);

    // The failure handler should not return.  If a returning failure handler
    // has been installed, alert the user that the program is continuing to
    // run.

    unsigned count = static_cast<unsigned>(
                AtomicOperations::incrementIntNvAcqRel(&failureReturnCount));

    if (BSLS_PERFORMANCEHINT_PREDICT_UNLIKELY(0 == (count & (count - 1)))) {
        BSLS_PERFORMANCEHINT_UNLIKELY_HINT;

        // Log when 'count' is a power of 2.

        if (count == (1 << 30)) {
            // Avoid undefined behavior by resetting the counter.

            AtomicOperations::setInt(&failureReturnCount, 1 << 29);
        }

        Log::logFormattedMessage(LogSeverity::e_FATAL,
                                 file,
                                 line,
                                 "BSLS_ASSERT failure: '%s'",
                                 text);

        BSLS_LOG_FATAL("Bad 'bsls_assert' configuration: "
                       "violation handler at %p failed to prevent program "
                       "from continuing.",
                       currentHandlerAddress);
    }

#ifdef BSLS_ASSERT_ENABLE_NORETURN_FOR_INVOKE_HANDLER
    std::abort();
#endif
}

                     // Standard Assertion-Failure Handlers

BSLS_ASSERT_NORETURN
void Assert::failAbort(const char *text, const char *file, int line)
{
    printError(text, file, line);

#ifndef BDE_OMIT_INTERNAL_DEPRECATED
    // See DRQS 8923441: The following is a work-around for a Fortran compiler bug.
#endif // BDE_OMIT_INTERNAL_DEPRECATED

#ifdef BSLS_PLATFORM_OS_AIX
    sigset_t newset;
    sigemptyset(&newset);
    sigaddset(&newset, SIGABRT);
#if defined(BDE_BUILD_TARGET_MT)
    pthread_sigmask(SIG_UNBLOCK, &newset, 0);
#else
    sigprocmask(SIG_UNBLOCK, &newset, 0);
#endif
#endif

#ifndef BDE_OMIT_INTERNAL_DEPRECATED
    // See DRQS 13882128: Note that (according to Oleg) the first line alone may be
    // sufficient.
#endif // BDE_OMIT_INTERNAL_DEPRECATED

#ifdef BSLS_PLATFORM_OS_WINDOWS
    // The following configures the runtime library on how to report asserts,
    // errors, and warnings in order to avoid pop-up windows when 'abort' is
    // called.
    if (!IsDebuggerPresent()) {
        _CrtSetReportMode(_CRT_ASSERT, 0);
        _CrtSetReportMode(_CRT_ERROR, 0);
        _CrtSetReportMode(_CRT_WARN, 0);
    }
#endif

    std::abort();
}

BSLS_ASSERT_NORETURN
void Assert::failSleep(const char *text, const char *file, int line)
{
    printError(text, file, line);

    volatile int sleepDuration = 1;

    while (1 == sleepDuration) {

#if defined(BSLS_PLATFORM_OS_UNIX)
        sleep(sleepDuration);
#elif defined(BSLS_PLATFORM_OS_WINDOWS)
        Sleep(sleepDuration * 1000);  // milliseconds
#else
        #error "Do not know how to sleep on this platform."
#endif

    }

    // We will never reach this line, but it is needed to let the compiler know
    // that this function does not return.

    std::abort();
}

BSLS_ASSERT_NORETURN
void Assert::failThrow(const char *text, const char *file, int line)
{

#ifdef BDE_BUILD_TARGET_EXC
    if (!std::uncaught_exception()) {
        throw AssertTestException(text, file, line);
    }
    else {
        bsls::Log::logMessage(bsls::LogSeverity::e_ERROR, file, line,
                "BSLS_ASSERT: An uncaught exception is pending;"
                " cannot throw 'AssertTestException'.");
    }
#endif

    failAbort(text, file, line);
}

}  // close package namespace

#undef BSLS_ASSERT_NORETURN

namespace bsls {

                    // -------------------------------
                    // class AssertFailureHandlerGuard
                    // -------------------------------

AssertFailureHandlerGuard::AssertFailureHandlerGuard(Assert::Handler temporary)
: d_original(Assert::failureHandler())
{
    Assert::setFailureHandlerRaw(temporary);
}

AssertFailureHandlerGuard::~AssertFailureHandlerGuard()
{
    Assert::setFailureHandlerRaw(d_original);
}

}  // close package namespace

}  // close enterprise namespace

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


<commit_before>/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                       http://www.mrpt.org/                                |
   |                                                                           |
   |   Copyright (C) 2005-2012  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Machine Perception and Intelligent    |
   |      Robotics Lab, University of Malaga (Spain).                          |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT 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 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT 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 MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */

#include <mrpt/vision.h>
#include <gtest/gtest.h>

#include "chessboard_stereo_camera_calib_internal.h"

using namespace mrpt;
using namespace mrpt::poses;
using namespace mrpt::utils;
using namespace mrpt::math;
using namespace mrpt::vision;
using namespace std;

class StereoCalibTests : public ::testing::Test {
protected:
	virtual void SetUp()
	{
	}
	virtual void TearDown() {  }

	void testStereoCalibJacobians(
		double x,double y, double z, double yaw, double pitch, double roll )
	{
		const CPose3D camPose = CPose3D(x2,y2,z2,yaw2,pitch2,roll2);

		// Prepare a test state:
		// --------------------------------------
		TCalibrationStereoImageList images;
		vector<size_t>   valid_image_pair_indices;
		vector<TPoint3D> obj_points;

		obj_points.push_back(TPoint3D(-0.4,0.3,0));
		valid_image_pair_indices.push_back(0);

		lm_stat_t  lm_stat(images, valid_image_pair_indices, obj_points);

		lm_stat.left_cam_poses.push_back( CPose3D(0,0,1) );

		// [fx fy cx cy k1 k2 k3 t1 t2]
		const double cam_l_params[9] = {600, 400, 320,240, 0,0,0,0,0 };
		const double cam_r_params[9] = {500, 300, 250,200, 0,0,0,0,0 };
		lm_stat.left_cam_params  = CArrayDouble<9>(cam_l_params);
		lm_stat.right_cam_params = CArrayDouble<9>(cam_r_params);

		// Evaluate theoretical Jacobians:
		TResidualJacobianList jacobs; // Theoretical output Jacobians
		mrpt::vision::recompute_errors_and_Jacobians( lm_stat, jacobs );

		// Compare:
		//EXPECT_NEAR(
	}

};

TEST_F(StereoCalibTests,Jacobians)
{
	testStereoCalibJacobians(0,0,0,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0));

}

<commit_msg>removed unimplemented unit test<commit_after><|endoftext|>