<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. Neither the name PX4 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
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
/**
 * @file followme.cpp
 *
 * Helper class to track and follow a given position
 *
 * @author Jimmy Johnson <catch22@fastmail.net>
 */

#include "follow_target.h"

#include <string.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <fcntl.h>

#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>

#include <uORB/uORB.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/follow_target.h>
#include <lib/geo/geo.h>

#include "navigator.h"

FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
	MissionBlock(navigator, name),
	_navigator(navigator),
	_param_min_alt(this, "MIS_TAKEOFF_ALT", false),
	_follow_target_state(WAIT_FOR_TARGET_POSITION),
	_follow_target_sub(-1),
	_step_time_in_ms(0.0f),
	_target_updates(0),
	_last_update_time(0),
	_current_target_motion( {}),
			_previous_target_motion({})
{
	updateParams();
	_current_vel.zero();
	_step_vel.zero();
	_target_vel.zero();
	_target_distance.zero();
}

FollowTarget::~FollowTarget()
{
}

void FollowTarget::on_inactive()
{
	reset_target_validity();
}

void FollowTarget::on_activation()
{
	if (_follow_target_sub < 0) {
		_follow_target_sub = orb_subscribe(ORB_ID(follow_target));
	}
}

void FollowTarget::on_active()
{
	struct map_projection_reference_s target_ref;
	math::Vector<3> target_position(0, 0, 0);
	uint64_t current_time = hrt_absolute_time();
	bool _radius_entered = false;
	bool _radius_exited = false;
	bool updated = false;
	float dt_ms = 0;

	orb_check(_follow_target_sub, &updated);

	if (updated) {

		_target_updates++;

		// save last known motion topic

		_previous_target_motion = _current_target_motion;

		orb_copy(ORB_ID(follow_target), _follow_target_sub, &_current_target_motion);

	} else if (((current_time - _previous_target_motion.timestamp) / 1000 / 1000) > TARGET_TIMEOUT_S
		   && target_velocity_valid()) {
		reset_target_validity();
	}

	// update target velocity

	if (target_velocity_valid() && updated) {

		dt_ms = ((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000);

		// get last gps known reference for target

		map_projection_init(&target_ref, _previous_target_motion.lat, _previous_target_motion.lon);

		// calculate distance the target has moved

		map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &(target_position(0)),
				       &(target_position(1)));

		// update the average velocity of the target based on the position

		_target_vel = target_position / (dt_ms / 1000.0f);

		// to keep the velocity increase/decrease smooth
		// calculate how many velocity increments/decrements
		// it will take to reach the targets velocity
		// with the given amount of steps also add a feed forward input that adjusts the
		// velocity as the position gap increases since
		// just traveling at the exact velocity of the target will not
		// get any closer to the target

		_step_vel = (_target_vel - _current_vel) + _target_distance * FF_K;
		_step_vel /= (dt_ms / 1000.0f * (float) INTERPOLATION_PNTS);
		_step_time_in_ms = dt_ms / (float) INTERPOLATION_PNTS;
	}

	// update distance to target

	if (target_position_valid()) {

		// get distance to target

		map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
		map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0),
				       &_target_distance(1));

		// are we within the target acceptance radius?
		// give a buffer to exit/enter the radius to give the velocity controller
		// a chance to catch up

		_radius_exited = (_target_distance.length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f);
		_radius_entered = (_target_distance.length() < (float) TARGET_ACCEPTANCE_RADIUS_M);
	}

	// update state machine

	switch (_follow_target_state) {

	case TRACK_POSITION: {

			if (_radius_entered == true) {
				_follow_target_state = TRACK_VELOCITY;

			} else if (target_velocity_valid()) {
				set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN);
				// keep the current velocity updated with the target velocity for when it's needed
				_current_vel = _target_vel;
				update_position_sp(true, true);
			}

			break;
		}

	case TRACK_VELOCITY: {

			if (_radius_exited == true) {
				_follow_target_state = TRACK_POSITION;

			} else if (target_velocity_valid()) {
				set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN);

				if ((current_time - _last_update_time) / 1000 >= _step_time_in_ms) {
					_current_vel += _step_vel;
					_last_update_time = current_time;
				}

				update_position_sp(true, false);
			}

			break;
		}

	case WAIT_FOR_TARGET_POSITION: {

			// Climb to the minimum altitude
			// and wait until a position is received

			follow_target_s target = { };

			// for now set the target at the minimum height above the uav

			target.lat = _navigator->get_global_position()->lat;
			target.lon = _navigator->get_global_position()->lon;
			target.alt = 0.0F;

			set_follow_target_item(&_mission_item, _param_min_alt.get(), target, NAN);

			update_position_sp(false, false);

			if (is_mission_item_reached() && target_velocity_valid()) {
				_follow_target_state = TRACK_POSITION;
			}

			break;
		}
	}
}

void FollowTarget::update_position_sp(bool use_velocity, bool use_position)
{
	// convert mission item to current setpoint

	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

	// activate line following in pos control if position is valid

	pos_sp_triplet->previous.valid = use_position;
	pos_sp_triplet->previous = pos_sp_triplet->current;
	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
	pos_sp_triplet->current.position_valid = use_position;
	pos_sp_triplet->current.velocity_valid = use_velocity;
	pos_sp_triplet->current.vx = _current_vel(0);
	pos_sp_triplet->current.vy = _current_vel(1);
	pos_sp_triplet->next.valid = false;

	_navigator->set_position_setpoint_triplet_updated();
}

void FollowTarget::reset_target_validity()
{
	_previous_target_motion = {};
	_current_target_motion = {};
	_target_updates = 0;
	_current_vel.zero();
	_step_vel.zero();
	_target_vel.zero();
	_target_distance.zero();
	reset_mission_item_reached();
	_follow_target_state = WAIT_FOR_TARGET_POSITION;
}

bool FollowTarget::target_velocity_valid()
{
	// need at least 2 data points for velocity estimate
	return (_target_updates >= 2);
}

bool FollowTarget::target_position_valid()
{
	// need at least 1 data point for position estimate
	return (_target_updates >= 1);
}
<commit_msg>Fix wrong header location<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. Neither the name PX4 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
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
/**
 * @file followme.cpp
 *
 * Helper class to track and follow a given position
 *
 * @author Jimmy Johnson <catch22@fastmail.net>
 */

#include "follow_target.h"

#include <string.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <fcntl.h>

#include <systemlib/err.h>

#include <uORB/uORB.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/follow_target.h>
#include <lib/geo/geo.h>

#include "navigator.h"

FollowTarget::FollowTarget(Navigator *navigator, const char *name) :
	MissionBlock(navigator, name),
	_navigator(navigator),
	_param_min_alt(this, "MIS_TAKEOFF_ALT", false),
	_follow_target_state(WAIT_FOR_TARGET_POSITION),
	_follow_target_sub(-1),
	_step_time_in_ms(0.0f),
	_target_updates(0),
	_last_update_time(0),
	_current_target_motion( {}),
			_previous_target_motion({})
{
	updateParams();
	_current_vel.zero();
	_step_vel.zero();
	_target_vel.zero();
	_target_distance.zero();
}

FollowTarget::~FollowTarget()
{
}

void FollowTarget::on_inactive()
{
	reset_target_validity();
}

void FollowTarget::on_activation()
{
	if (_follow_target_sub < 0) {
		_follow_target_sub = orb_subscribe(ORB_ID(follow_target));
	}
}

void FollowTarget::on_active()
{
	struct map_projection_reference_s target_ref;
	math::Vector<3> target_position(0, 0, 0);
	uint64_t current_time = hrt_absolute_time();
	bool _radius_entered = false;
	bool _radius_exited = false;
	bool updated = false;
	float dt_ms = 0;

	orb_check(_follow_target_sub, &updated);

	if (updated) {

		_target_updates++;

		// save last known motion topic

		_previous_target_motion = _current_target_motion;

		orb_copy(ORB_ID(follow_target), _follow_target_sub, &_current_target_motion);

	} else if (((current_time - _previous_target_motion.timestamp) / 1000 / 1000) > TARGET_TIMEOUT_S
		   && target_velocity_valid()) {
		reset_target_validity();
	}

	// update target velocity

	if (target_velocity_valid() && updated) {

		dt_ms = ((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000);

		// get last gps known reference for target

		map_projection_init(&target_ref, _previous_target_motion.lat, _previous_target_motion.lon);

		// calculate distance the target has moved

		map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &(target_position(0)),
				       &(target_position(1)));

		// update the average velocity of the target based on the position

		_target_vel = target_position / (dt_ms / 1000.0f);

		// to keep the velocity increase/decrease smooth
		// calculate how many velocity increments/decrements
		// it will take to reach the targets velocity
		// with the given amount of steps also add a feed forward input that adjusts the
		// velocity as the position gap increases since
		// just traveling at the exact velocity of the target will not
		// get any closer to the target

		_step_vel = (_target_vel - _current_vel) + _target_distance * FF_K;
		_step_vel /= (dt_ms / 1000.0f * (float) INTERPOLATION_PNTS);
		_step_time_in_ms = dt_ms / (float) INTERPOLATION_PNTS;
	}

	// update distance to target

	if (target_position_valid()) {

		// get distance to target

		map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
		map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0),
				       &_target_distance(1));

		// are we within the target acceptance radius?
		// give a buffer to exit/enter the radius to give the velocity controller
		// a chance to catch up

		_radius_exited = (_target_distance.length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f);
		_radius_entered = (_target_distance.length() < (float) TARGET_ACCEPTANCE_RADIUS_M);
	}

	// update state machine

	switch (_follow_target_state) {

	case TRACK_POSITION: {

			if (_radius_entered == true) {
				_follow_target_state = TRACK_VELOCITY;

			} else if (target_velocity_valid()) {
				set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN);
				// keep the current velocity updated with the target velocity for when it's needed
				_current_vel = _target_vel;
				update_position_sp(true, true);
			}

			break;
		}

	case TRACK_VELOCITY: {

			if (_radius_exited == true) {
				_follow_target_state = TRACK_POSITION;

			} else if (target_velocity_valid()) {
				set_follow_target_item(&_mission_item, _param_min_alt.get(), _current_target_motion, NAN);

				if ((current_time - _last_update_time) / 1000 >= _step_time_in_ms) {
					_current_vel += _step_vel;
					_last_update_time = current_time;
				}

				update_position_sp(true, false);
			}

			break;
		}

	case WAIT_FOR_TARGET_POSITION: {

			// Climb to the minimum altitude
			// and wait until a position is received

			follow_target_s target = { };

			// for now set the target at the minimum height above the uav

			target.lat = _navigator->get_global_position()->lat;
			target.lon = _navigator->get_global_position()->lon;
			target.alt = 0.0F;

			set_follow_target_item(&_mission_item, _param_min_alt.get(), target, NAN);

			update_position_sp(false, false);

			if (is_mission_item_reached() && target_velocity_valid()) {
				_follow_target_state = TRACK_POSITION;
			}

			break;
		}
	}
}

void FollowTarget::update_position_sp(bool use_velocity, bool use_position)
{
	// convert mission item to current setpoint

	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

	// activate line following in pos control if position is valid

	pos_sp_triplet->previous.valid = use_position;
	pos_sp_triplet->previous = pos_sp_triplet->current;
	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
	pos_sp_triplet->current.position_valid = use_position;
	pos_sp_triplet->current.velocity_valid = use_velocity;
	pos_sp_triplet->current.vx = _current_vel(0);
	pos_sp_triplet->current.vy = _current_vel(1);
	pos_sp_triplet->next.valid = false;

	_navigator->set_position_setpoint_triplet_updated();
}

void FollowTarget::reset_target_validity()
{
	_previous_target_motion = {};
	_current_target_motion = {};
	_target_updates = 0;
	_current_vel.zero();
	_step_vel.zero();
	_target_vel.zero();
	_target_distance.zero();
	reset_mission_item_reached();
	_follow_target_state = WAIT_FOR_TARGET_POSITION;
}

bool FollowTarget::target_velocity_valid()
{
	// need at least 2 data points for velocity estimate
	return (_target_updates >= 2);
}

bool FollowTarget::target_position_valid()
{
	// need at least 1 data point for position estimate
	return (_target_updates >= 1);
}
<|endoftext|>


<commit_before>#ifndef CLIENT_WS_HPP
#define	CLIENT_WS_HPP

#include "crypto.hpp"

#include <boost/asio.hpp>

#include <unordered_map>
#include <iostream>
#include <regex>
#include <random>

namespace SimpleWeb {
    template <class socket_type>
    class SocketClientBase {
    public:
        class Connection {
            friend class SocketClientBase<socket_type>;

        public:
            std::unordered_map<std::string, std::string> header;

            Connection(std::shared_ptr<socket_type> socket): socket(socket), closed(false) {}
        private:
            std::shared_ptr<socket_type> socket;

            std::atomic<bool> closed;
        };

        std::shared_ptr<Connection> connection;

        struct Message {
            std::shared_ptr<std::istream> data;
            size_t length;
            unsigned char fin_rsv_opcode;
        };

        std::function<void(void)> onopen;
        std::function<void(std::shared_ptr<Message>)> onmessage;
        std::function<void(const boost::system::error_code&)> onerror;
        std::function<void(int)> onclose;

        void start() {
            connect();

            asio_io_service.run();
        }

        void send(std::iostream& stream, const std::function<void(const boost::system::error_code&)>& callback=nullptr,
                unsigned char fin_rsv_opcode=129) {
            //Create mask
            std::vector<unsigned char> mask;
            mask.resize(4);
            std::uniform_int_distribution<unsigned char> dist;
            std::random_device rd;
            for(int c=0;c<4;c++) {
                mask[c]=dist(rd);
            }

            std::shared_ptr<boost::asio::streambuf> write_buffer(new boost::asio::streambuf);
            std::ostream response(write_buffer.get());

            stream.seekp(0, std::ios::end);
            size_t length=stream.tellp();
            stream.seekp(0, std::ios::beg);

            response.put(fin_rsv_opcode);
            //masked (first length byte>=128)
            if(length>=126) {
                int num_bytes;
                if(length>0xffff) {
                    num_bytes=8;
                    response.put(127+128);
                }
                else {
                    num_bytes=2;
                    response.put(126+128);
                }

                for(int c=num_bytes-1;c>=0;c--) {
                    response.put((length>>(8*c))%256);
                }
            }
            else
                response.put(length+128);

            for(int c=0;c<4;c++) {
                response.put(mask[c]);
            }

            for(size_t c=0;c<length;c++) {
                response.put(stream.get()^mask[c%4]);
            }

            //Need to copy the callback-function in case its destroyed
            boost::asio::async_write(*connection->socket, *write_buffer,
                    [this, write_buffer, callback](const boost::system::error_code& ec, size_t bytes_transferred) {
                if(callback) {
                    callback(ec);
                }
            });
        }

        void send_close(int status, const std::string& reason="") {
            //Send close only once (in case close is initiated by client)
            if(connection->closed.load()) {
                return;
            }
            connection->closed.store(true);

            std::stringstream response;

            response.put(status>>8);
            response.put(status%256);

            response << reason;

            //fin_rsv_opcode=136: message close
            send(response, [](const boost::system::error_code& ec){}, 136);
        }

    protected:
        const std::string ws_magic_string="258EAFA5-E914-47DA-95CA-C5AB0DC85B11";

        boost::asio::io_service asio_io_service;
        boost::asio::ip::tcp::endpoint asio_endpoint;
        boost::asio::ip::tcp::resolver asio_resolver;

        std::string host;
        unsigned short port;
        std::string path;

        SocketClientBase(const std::string& host_port_path, unsigned short default_port) :
                asio_resolver(asio_io_service) {
            std::regex e("^([^:/]+):?([0-9]*)(.*)$");

            std::smatch sm;

            if(std::regex_match(host_port_path, sm, e)) {
                host=sm[1];
                path=sm[3];
                port=default_port;
                if(sm[2]!="")
                    port=(unsigned short)std::stoul(sm[2]);
                asio_endpoint=boost::asio::ip::tcp::endpoint(boost::asio::ip::tcp::v4(), port);
            }
            else {
                throw std::invalid_argument("Error parsing host_port_path");
            }
        }

        virtual void connect()=0;

        void handshake(std::shared_ptr<socket_type> socket) {
            std::shared_ptr<boost::asio::streambuf> write_buffer(new boost::asio::streambuf);

            std::ostream request(write_buffer.get());

            request << "GET " << path << " HTTP/1.1" << "\r\n";
            request << "Host: " << host << "\r\n";
            request << "Upgrade: websocket\r\n";
            request << "Connection: Upgrade\r\n";

            //Make random 16-byte nonce
            std::string nonce;
            nonce.resize(16);
            std::uniform_int_distribution<unsigned char> dist;
            std::random_device rd;
            for(int c=0;c<16;c++)
                nonce[c]=dist(rd);

            std::string nonce_base64=Crypto::Base64::encode(nonce);
            request << "Sec-WebSocket-Key: " << nonce_base64 << "\r\n";
            request << "Sec-WebSocket-Version: 13\r\n";
            request << "\r\n";

            //test this to base64::decode(Sec-WebSocket-Accept)
            std::shared_ptr<std::string> accept_sha1(new std::string(Crypto::SHA1(nonce_base64+ws_magic_string)));

            boost::asio::async_write(*socket, *write_buffer,
                    [this, socket, write_buffer, accept_sha1]
                    (const boost::system::error_code& ec, size_t bytes_transferred) {
                if(!ec) {
                    std::shared_ptr<boost::asio::streambuf> read_buffer(new boost::asio::streambuf);

                    boost::asio::async_read_until(*socket, *read_buffer, "\r\n\r\n",
                            [this, socket, read_buffer, accept_sha1]
                            (const boost::system::error_code& ec, size_t bytes_transferred) {
                        if(!ec) {
                            //Convert to istream to extract string-lines
                            std::istream stream(read_buffer.get());

                            connection=std::make_shared<Connection>(socket);
                            parse_handshake(stream);
                            if(Crypto::Base64::decode(connection->header["Sec-WebSocket-Accept"])==*accept_sha1) {
                                if(onopen)
                                    onopen();
                                read_message(read_buffer);
                            }
                            else
                                throw std::invalid_argument("WebSocket handshake failed");
                        }
                    });
                }
                else
                    throw std::invalid_argument("Failed sending handshake");
            });
        }

        void parse_handshake(std::istream& stream) const {
            std::smatch sm;

            //Not parsing the first line
            std::string line;
            getline(stream, line);
            line.pop_back();

            bool matched;
            std::regex e("^([^:]*): ?(.*)$");
            //Parse the rest of the header
            do {
                getline(stream, line);
                line.pop_back();
                matched=std::regex_match(line, sm, e);
                if(matched) {
                    connection->header[sm[1]]=sm[2];
                }

            } while(matched==true);
        }

        void read_message(std::shared_ptr<boost::asio::streambuf> read_buffer) {
            boost::asio::async_read(*connection->socket, *read_buffer, boost::asio::transfer_exactly(2),
                    [this, read_buffer](const boost::system::error_code& ec, size_t bytes_transferred) {
                if(!ec) {
                    std::istream stream(read_buffer.get());

                    std::vector<unsigned char> num_bytes;
                    num_bytes.resize(2);
                    stream.read((char*)&num_bytes[0], 2);

                    unsigned char fin_rsv_opcode=num_bytes[0];

                    size_t length=(num_bytes[1]&127);

                    if(length==126) {
                        //2 next bytes is the size of content
                        boost::asio::async_read(*connection->socket, *read_buffer, boost::asio::transfer_exactly(2),
                                [this, read_buffer, fin_rsv_opcode]
                                (const boost::system::error_code& ec, size_t bytes_transferred) {
                            if(!ec) {
                                std::istream stream(read_buffer.get());

                                std::vector<unsigned char> length_bytes;
                                length_bytes.resize(2);
                                stream.read((char*)&length_bytes[0], 2);

                                size_t length=0;
                                int num_bytes=2;
                                for(int c=0;c<num_bytes;c++)
                                    length+=length_bytes[c]<<(8*(num_bytes-1-c));

                                read_message_content(read_buffer, length, fin_rsv_opcode);
                            }
                            else {
                                if(onerror)
                                    onerror(ec);
                            }
                        });
                    }
                    else if(length==127) {
                        //8 next bytes is the size of content
                        boost::asio::async_read(*connection->socket, *read_buffer, boost::asio::transfer_exactly(8),
                                [this, read_buffer, fin_rsv_opcode]
                                (const boost::system::error_code& ec, size_t bytes_transferred) {
                            if(!ec) {
                                std::istream stream(read_buffer.get());

                                std::vector<unsigned char> length_bytes;
                                length_bytes.resize(8);
                                stream.read((char*)&length_bytes[0], 8);

                                size_t length=0;
                                int num_bytes=8;
                                for(int c=0;c<num_bytes;c++)
                                    length+=length_bytes[c]<<(8*(num_bytes-1-c));

                                read_message_content(read_buffer, length, fin_rsv_opcode);
                            }
                            else {
                                if(onerror)
                                    onerror(ec);
                            }
                        });
                    }
                    else
                        read_message_content(read_buffer, length, fin_rsv_opcode);
                }
                else {
                    if(onerror)
                        onerror(ec);
                }
            });
        }

        void read_message_content(std::shared_ptr<boost::asio::streambuf> read_buffer, size_t length, unsigned char fin_rsv_opcode) {
            boost::asio::async_read(*connection->socket, *read_buffer, boost::asio::transfer_exactly(length),
                    [this, read_buffer, length, fin_rsv_opcode]
                    (const boost::system::error_code& ec, size_t bytes_transferred) {
                if(!ec) {
                    std::shared_ptr<std::istream> message_data(new std::istream(read_buffer.get()));

                    //If connection close
                    if((fin_rsv_opcode&0x0f)==8) {
                        int status=0;
                        if(length>=2) {
                            unsigned char byte1=message_data->get();
                            unsigned char byte2=message_data->get();
                            status=(byte1<<8)+byte2;
                        }

                        send_close(status);
                        if(onclose)
                            onclose(status);
                        return;
                    }
                    //If ping
                    else if((fin_rsv_opcode&0x0f)==9) {
                        //send pong
                        std::stringstream empty_ss;
                        send(empty_ss, nullptr, fin_rsv_opcode+1);
                    }
                    else if(onmessage) {
                        std::shared_ptr<Message> message(new Message());
                        message->data=message_data;
                        message->length=length;
                        message->fin_rsv_opcode=fin_rsv_opcode;
                        onmessage(message);
                    }

                    //Next message
                    read_message(read_buffer);
                }
                else {
                    if(onerror)
                        onerror(ec);
                }
            });
        }
    };

    template<class socket_type>
    class Client : public SocketClientBase<socket_type> {};

    typedef boost::asio::ip::tcp::socket WS;

    template<>
    class Client<WS> : public SocketClientBase<WS> {
    public:
        Client(const std::string& server_port_path) : SocketClientBase<WS>::SocketClientBase(server_port_path, 80) {};

    private:
        void connect() {
            boost::asio::ip::tcp::resolver::query query(host, std::to_string(port));

            asio_resolver.async_resolve(query, [this]
                    (const boost::system::error_code &ec, boost::asio::ip::tcp::resolver::iterator it){
                if(!ec) {
                    std::shared_ptr<WS> socket(new WS(asio_io_service));

                    boost::asio::async_connect(*socket, it, [this, socket]
                            (const boost::system::error_code &ec, boost::asio::ip::tcp::resolver::iterator it){
                        if(!ec) {
                            handshake(socket);
                        }
                        else
                            throw std::invalid_argument(ec.message());
                    });
                }
                else
                    throw std::invalid_argument(ec.message());
            });
        }
    };
}

#endif	/* CLIENT_WS_HPP */<commit_msg>Added client close if message from server is masked<commit_after>#ifndef CLIENT_WS_HPP
#define	CLIENT_WS_HPP

#include "crypto.hpp"

#include <boost/asio.hpp>

#include <unordered_map>
#include <iostream>
#include <regex>
#include <random>

namespace SimpleWeb {
    template <class socket_type>
    class SocketClientBase {
    public:
        class Connection {
            friend class SocketClientBase<socket_type>;

        public:
            std::unordered_map<std::string, std::string> header;

            Connection(std::shared_ptr<socket_type> socket): socket(socket), closed(false) {}
        private:
            std::shared_ptr<socket_type> socket;

            std::atomic<bool> closed;
        };

        std::shared_ptr<Connection> connection;

        struct Message {
            std::shared_ptr<std::istream> data;
            size_t length;
            unsigned char fin_rsv_opcode;
        };

        std::function<void(void)> onopen;
        std::function<void(std::shared_ptr<Message>)> onmessage;
        std::function<void(const boost::system::error_code&)> onerror;
        std::function<void(int)> onclose;

        void start() {
            connect();

            asio_io_service.run();
        }

        void send(std::iostream& stream, const std::function<void(const boost::system::error_code&)>& callback=nullptr,
                unsigned char fin_rsv_opcode=129) {
            //Create mask
            std::vector<unsigned char> mask;
            mask.resize(4);
            std::uniform_int_distribution<unsigned char> dist;
            std::random_device rd;
            for(int c=0;c<4;c++) {
                mask[c]=dist(rd);
            }

            std::shared_ptr<boost::asio::streambuf> write_buffer(new boost::asio::streambuf);
            std::ostream response(write_buffer.get());

            stream.seekp(0, std::ios::end);
            size_t length=stream.tellp();
            stream.seekp(0, std::ios::beg);

            response.put(fin_rsv_opcode);
            //masked (first length byte>=128)
            if(length>=126) {
                int num_bytes;
                if(length>0xffff) {
                    num_bytes=8;
                    response.put(127+128);
                }
                else {
                    num_bytes=2;
                    response.put(126+128);
                }

                for(int c=num_bytes-1;c>=0;c--) {
                    response.put((length>>(8*c))%256);
                }
            }
            else
                response.put(length+128);

            for(int c=0;c<4;c++) {
                response.put(mask[c]);
            }

            for(size_t c=0;c<length;c++) {
                response.put(stream.get()^mask[c%4]);
            }

            //Need to copy the callback-function in case its destroyed
            boost::asio::async_write(*connection->socket, *write_buffer,
                    [this, write_buffer, callback](const boost::system::error_code& ec, size_t bytes_transferred) {
                if(callback) {
                    callback(ec);
                }
            });
        }

        void send_close(int status, const std::string& reason="") {
            //Send close only once (in case close is initiated by client)
            if(connection->closed.load()) {
                return;
            }
            connection->closed.store(true);

            std::stringstream response;

            response.put(status>>8);
            response.put(status%256);

            response << reason;

            //fin_rsv_opcode=136: message close
            send(response, [](const boost::system::error_code& ec){}, 136);
        }

    protected:
        const std::string ws_magic_string="258EAFA5-E914-47DA-95CA-C5AB0DC85B11";

        boost::asio::io_service asio_io_service;
        boost::asio::ip::tcp::endpoint asio_endpoint;
        boost::asio::ip::tcp::resolver asio_resolver;

        std::string host;
        unsigned short port;
        std::string path;

        SocketClientBase(const std::string& host_port_path, unsigned short default_port) :
                asio_resolver(asio_io_service) {
            std::regex e("^([^:/]+):?([0-9]*)(.*)$");

            std::smatch sm;

            if(std::regex_match(host_port_path, sm, e)) {
                host=sm[1];
                path=sm[3];
                port=default_port;
                if(sm[2]!="")
                    port=(unsigned short)std::stoul(sm[2]);
                asio_endpoint=boost::asio::ip::tcp::endpoint(boost::asio::ip::tcp::v4(), port);
            }
            else {
                throw std::invalid_argument("Error parsing host_port_path");
            }
        }

        virtual void connect()=0;

        void handshake(std::shared_ptr<socket_type> socket) {
            std::shared_ptr<boost::asio::streambuf> write_buffer(new boost::asio::streambuf);

            std::ostream request(write_buffer.get());

            request << "GET " << path << " HTTP/1.1" << "\r\n";
            request << "Host: " << host << "\r\n";
            request << "Upgrade: websocket\r\n";
            request << "Connection: Upgrade\r\n";

            //Make random 16-byte nonce
            std::string nonce;
            nonce.resize(16);
            std::uniform_int_distribution<unsigned char> dist;
            std::random_device rd;
            for(int c=0;c<16;c++)
                nonce[c]=dist(rd);

            std::string nonce_base64=Crypto::Base64::encode(nonce);
            request << "Sec-WebSocket-Key: " << nonce_base64 << "\r\n";
            request << "Sec-WebSocket-Version: 13\r\n";
            request << "\r\n";

            //test this to base64::decode(Sec-WebSocket-Accept)
            std::shared_ptr<std::string> accept_sha1(new std::string(Crypto::SHA1(nonce_base64+ws_magic_string)));

            boost::asio::async_write(*socket, *write_buffer,
                    [this, socket, write_buffer, accept_sha1]
                    (const boost::system::error_code& ec, size_t bytes_transferred) {
                if(!ec) {
                    std::shared_ptr<boost::asio::streambuf> read_buffer(new boost::asio::streambuf);

                    boost::asio::async_read_until(*socket, *read_buffer, "\r\n\r\n",
                            [this, socket, read_buffer, accept_sha1]
                            (const boost::system::error_code& ec, size_t bytes_transferred) {
                        if(!ec) {
                            //Convert to istream to extract string-lines
                            std::istream stream(read_buffer.get());

                            connection=std::make_shared<Connection>(socket);
                            parse_handshake(stream);
                            if(Crypto::Base64::decode(connection->header["Sec-WebSocket-Accept"])==*accept_sha1) {
                                if(onopen)
                                    onopen();
                                read_message(read_buffer);
                            }
                            else
                                throw std::invalid_argument("WebSocket handshake failed");
                        }
                    });
                }
                else
                    throw std::invalid_argument("Failed sending handshake");
            });
        }

        void parse_handshake(std::istream& stream) const {
            std::smatch sm;

            //Not parsing the first line
            std::string line;
            getline(stream, line);
            line.pop_back();

            bool matched;
            std::regex e("^([^:]*): ?(.*)$");
            //Parse the rest of the header
            do {
                getline(stream, line);
                line.pop_back();
                matched=std::regex_match(line, sm, e);
                if(matched) {
                    connection->header[sm[1]]=sm[2];
                }

            } while(matched==true);
        }

        void read_message(std::shared_ptr<boost::asio::streambuf> read_buffer) {
            boost::asio::async_read(*connection->socket, *read_buffer, boost::asio::transfer_exactly(2),
                    [this, read_buffer](const boost::system::error_code& ec, size_t bytes_transferred) {
                if(!ec) {
                    std::istream stream(read_buffer.get());

                    std::vector<unsigned char> num_bytes;
                    num_bytes.resize(2);
                    stream.read((char*)&num_bytes[0], 2);

                    unsigned char fin_rsv_opcode=num_bytes[0];

                    //Close connection if masked message from server (protocol error)
                    if(num_bytes[1]>=128) {
                        send_close(1002, "message from server masked");
                        if(onclose)
                            onclose(1002);
                        return;
                    }

                    size_t length=(num_bytes[1]&127);

                    if(length==126) {
                        //2 next bytes is the size of content
                        boost::asio::async_read(*connection->socket, *read_buffer, boost::asio::transfer_exactly(2),
                                [this, read_buffer, fin_rsv_opcode]
                                (const boost::system::error_code& ec, size_t bytes_transferred) {
                            if(!ec) {
                                std::istream stream(read_buffer.get());

                                std::vector<unsigned char> length_bytes;
                                length_bytes.resize(2);
                                stream.read((char*)&length_bytes[0], 2);

                                size_t length=0;
                                int num_bytes=2;
                                for(int c=0;c<num_bytes;c++)
                                    length+=length_bytes[c]<<(8*(num_bytes-1-c));

                                read_message_content(read_buffer, length, fin_rsv_opcode);
                            }
                            else {
                                if(onerror)
                                    onerror(ec);
                            }
                        });
                    }
                    else if(length==127) {
                        //8 next bytes is the size of content
                        boost::asio::async_read(*connection->socket, *read_buffer, boost::asio::transfer_exactly(8),
                                [this, read_buffer, fin_rsv_opcode]
                                (const boost::system::error_code& ec, size_t bytes_transferred) {
                            if(!ec) {
                                std::istream stream(read_buffer.get());

                                std::vector<unsigned char> length_bytes;
                                length_bytes.resize(8);
                                stream.read((char*)&length_bytes[0], 8);

                                size_t length=0;
                                int num_bytes=8;
                                for(int c=0;c<num_bytes;c++)
                                    length+=length_bytes[c]<<(8*(num_bytes-1-c));

                                read_message_content(read_buffer, length, fin_rsv_opcode);
                            }
                            else {
                                if(onerror)
                                    onerror(ec);
                            }
                        });
                    }
                    else
                        read_message_content(read_buffer, length, fin_rsv_opcode);
                }
                else {
                    if(onerror)
                        onerror(ec);
                }
            });
        }

        void read_message_content(std::shared_ptr<boost::asio::streambuf> read_buffer, size_t length, unsigned char fin_rsv_opcode) {
            boost::asio::async_read(*connection->socket, *read_buffer, boost::asio::transfer_exactly(length),
                    [this, read_buffer, length, fin_rsv_opcode]
                    (const boost::system::error_code& ec, size_t bytes_transferred) {
                if(!ec) {
                    std::shared_ptr<std::istream> message_data(new std::istream(read_buffer.get()));

                    //If connection close
                    if((fin_rsv_opcode&0x0f)==8) {
                        int status=0;
                        if(length>=2) {
                            unsigned char byte1=message_data->get();
                            unsigned char byte2=message_data->get();
                            status=(byte1<<8)+byte2;
                        }

                        send_close(status);
                        if(onclose)
                            onclose(status);
                        return;
                    }
                    //If ping
                    else if((fin_rsv_opcode&0x0f)==9) {
                        //send pong
                        std::stringstream empty_ss;
                        send(empty_ss, nullptr, fin_rsv_opcode+1);
                    }
                    else if(onmessage) {
                        std::shared_ptr<Message> message(new Message());
                        message->data=message_data;
                        message->length=length;
                        message->fin_rsv_opcode=fin_rsv_opcode;
                        onmessage(message);
                    }

                    //Next message
                    read_message(read_buffer);
                }
                else {
                    if(onerror)
                        onerror(ec);
                }
            });
        }
    };

    template<class socket_type>
    class Client : public SocketClientBase<socket_type> {};

    typedef boost::asio::ip::tcp::socket WS;

    template<>
    class Client<WS> : public SocketClientBase<WS> {
    public:
        Client(const std::string& server_port_path) : SocketClientBase<WS>::SocketClientBase(server_port_path, 80) {};

    private:
        void connect() {
            boost::asio::ip::tcp::resolver::query query(host, std::to_string(port));

            asio_resolver.async_resolve(query, [this]
                    (const boost::system::error_code &ec, boost::asio::ip::tcp::resolver::iterator it){
                if(!ec) {
                    std::shared_ptr<WS> socket(new WS(asio_io_service));

                    boost::asio::async_connect(*socket, it, [this, socket]
                            (const boost::system::error_code &ec, boost::asio::ip::tcp::resolver::iterator it){
                        if(!ec) {
                            handshake(socket);
                        }
                        else
                            throw std::invalid_argument(ec.message());
                    });
                }
                else
                    throw std::invalid_argument(ec.message());
            });
        }
    };
}

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


<commit_before>// Copyright 2014 the V8 project authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "src/compiler/move-optimizer.h"

namespace v8 {
namespace internal {
namespace compiler {

namespace {

typedef std::pair<InstructionOperand, InstructionOperand> MoveKey;
typedef ZoneMap<MoveKey, unsigned> MoveMap;
typedef ZoneSet<InstructionOperand> OperandSet;

bool GapsCanMoveOver(Instruction* instr) {
  return instr->IsSourcePosition() || instr->IsNop();
}

int FindFirstNonEmptySlot(Instruction* instr) {
  int i = Instruction::FIRST_GAP_POSITION;
  for (; i <= Instruction::LAST_GAP_POSITION; i++) {
    auto move = instr->parallel_moves()[i];
    if (move == nullptr) continue;
    auto move_ops = move->move_operands();
    auto op = move_ops->begin();
    for (; op != move_ops->end(); ++op) {
      if (!op->IsRedundant()) break;
      op->Eliminate();
    }
    if (op != move_ops->end()) break;  // Found non-redundant move.
    move_ops->Rewind(0);               // Clear this redundant move.
  }
  return i;
}

}  // namepace

MoveOptimizer::MoveOptimizer(Zone* local_zone, InstructionSequence* code)
    : local_zone_(local_zone),
      code_(code),
      to_finalize_(local_zone),
      temp_vector_0_(local_zone),
      temp_vector_1_(local_zone) {}

void MoveOptimizer::Run() {
  for (auto* block : code()->instruction_blocks()) {
    CompressBlock(block);
  }
  for (auto block : code()->instruction_blocks()) {
    if (block->PredecessorCount() <= 1) continue;
    OptimizeMerge(block);
  }
  for (auto gap : to_finalize_) {
    FinalizeMoves(gap);
  }
}

void MoveOptimizer::CompressMoves(MoveOpVector* eliminated, ParallelMove* left,
                                  ParallelMove* right) {
  DCHECK(eliminated->empty());
  auto move_ops = right->move_operands();
  if (!left->move_operands()->is_empty()) {
    // Modify the right moves in place and collect moves that will be killed by
    // merging the two gaps.
    for (auto op = move_ops->begin(); op != move_ops->end(); ++op) {
      if (op->IsRedundant()) continue;
      auto to_eliminate = left->PrepareInsertAfter(op);
      if (to_eliminate != nullptr) eliminated->push_back(to_eliminate);
    }
    // Eliminate dead moves.  Must happen before insertion of new moves as the
    // contents of eliminated are pointers into a list.
    for (auto to_eliminate : *eliminated) {
      to_eliminate->Eliminate();
    }
    eliminated->clear();
  }
  // Add all possibly modified moves from right side.
  for (auto op = move_ops->begin(); op != move_ops->end(); ++op) {
    if (op->IsRedundant()) continue;
    left->move_operands()->Add(*op, code_zone());
  }
  // Nuke right.
  move_ops->Rewind(0);
}

// Smash all consecutive moves into the left most move slot and accumulate them
// as much as possible across instructions.
void MoveOptimizer::CompressBlock(InstructionBlock* block) {
  auto temp_vector = temp_vector_0();
  DCHECK(temp_vector.empty());
  Instruction* prev_instr = nullptr;
  for (int index = block->code_start(); index < block->code_end(); ++index) {
    auto instr = code()->instructions()[index];
    int i = FindFirstNonEmptySlot(instr);
    if (i <= Instruction::LAST_GAP_POSITION) {
      // Move the first non-empty gap to position 0.
      std::swap(instr->parallel_moves()[0], instr->parallel_moves()[i]);
      auto left = instr->parallel_moves()[0];
      // Compress everything into position 0.
      for (++i; i <= Instruction::LAST_GAP_POSITION; ++i) {
        auto move = instr->parallel_moves()[i];
        if (move == nullptr) continue;
        CompressMoves(&temp_vector, left, move);
      }
      if (prev_instr != nullptr) {
        // Smash left into prev_instr, killing left.
        auto pred_moves = prev_instr->parallel_moves()[0];
        CompressMoves(&temp_vector, pred_moves, left);
      }
    }
    if (prev_instr != nullptr) {
      // Slide prev_instr down so we always know where to look for it.
      std::swap(prev_instr->parallel_moves()[0], instr->parallel_moves()[0]);
    }
    prev_instr = instr->parallel_moves()[0] == nullptr ? nullptr : instr;
    if (GapsCanMoveOver(instr)) continue;
    if (prev_instr != nullptr) {
      to_finalize_.push_back(prev_instr);
      prev_instr = nullptr;
    }
  }
  if (prev_instr != nullptr) {
    to_finalize_.push_back(prev_instr);
  }
}

Instruction* MoveOptimizer::LastInstruction(InstructionBlock* block) {
  return code()->instructions()[block->last_instruction_index()];
}

void MoveOptimizer::OptimizeMerge(InstructionBlock* block) {
  DCHECK(block->PredecessorCount() > 1);
  // Ensure that the last instruction in all incoming blocks don't contain
  // things that would prevent moving gap moves across them.
  for (auto pred_index : block->predecessors()) {
    auto pred = code()->InstructionBlockAt(pred_index);
    auto last_instr = code()->instructions()[pred->last_instruction_index()];
    if (last_instr->IsSourcePosition()) continue;
    if (last_instr->IsCall()) return;
    if (last_instr->TempCount() != 0) return;
    if (last_instr->OutputCount() != 0) return;
    for (size_t i = 0; i < last_instr->InputCount(); ++i) {
      auto op = last_instr->InputAt(i);
      if (!op->IsConstant() && !op->IsImmediate()) return;
    }
  }
  // TODO(dcarney): pass a ZonePool down for this?
  MoveMap move_map(local_zone());
  size_t correct_counts = 0;
  // Accumulate set of shared moves.
  for (auto pred_index : block->predecessors()) {
    auto pred = code()->InstructionBlockAt(pred_index);
    auto instr = LastInstruction(pred);
    if (instr->parallel_moves()[0] == nullptr ||
        instr->parallel_moves()[0]->move_operands()->is_empty()) {
      return;
    }
    auto move_ops = instr->parallel_moves()[0]->move_operands();
    for (auto op = move_ops->begin(); op != move_ops->end(); ++op) {
      if (op->IsRedundant()) continue;
      auto src = *op->source();
      auto dst = *op->destination();
      MoveKey key = {src, dst};
      auto res = move_map.insert(std::make_pair(key, 1));
      if (!res.second) {
        res.first->second++;
        if (res.first->second == block->PredecessorCount()) {
          correct_counts++;
        }
      }
    }
  }
  if (move_map.empty() || correct_counts != move_map.size()) return;
  // Find insertion point.
  Instruction* instr = nullptr;
  for (int i = block->first_instruction_index();
       i <= block->last_instruction_index(); ++i) {
    instr = code()->instructions()[i];
    if (!GapsCanMoveOver(instr) || !instr->AreMovesRedundant()) break;
  }
  DCHECK(instr != nullptr);
  bool gap_initialized = true;
  if (instr->parallel_moves()[0] == nullptr ||
      instr->parallel_moves()[0]->move_operands()->is_empty()) {
    to_finalize_.push_back(instr);
  } else {
    // Will compress after insertion.
    gap_initialized = false;
    std::swap(instr->parallel_moves()[0], instr->parallel_moves()[1]);
  }
  auto move = instr->GetOrCreateParallelMove(
      static_cast<Instruction::GapPosition>(0), code_zone());
  // Delete relevant entries in predecessors and move everything to block.
  bool first_iteration = true;
  for (auto pred_index : block->predecessors()) {
    auto pred = code()->InstructionBlockAt(pred_index);
    auto instr = LastInstruction(pred);
    auto move_ops = instr->parallel_moves()[0]->move_operands();
    for (auto op = move_ops->begin(); op != move_ops->end(); ++op) {
      if (op->IsRedundant()) continue;
      MoveKey key = {*op->source(), *op->destination()};
      auto it = move_map.find(key);
      USE(it);
      DCHECK(it != move_map.end());
      if (first_iteration) {
        move->AddMove(op->source(), op->destination(), code_zone());
      }
      op->Eliminate();
    }
    first_iteration = false;
  }
  // Compress.
  if (!gap_initialized) {
    CompressMoves(&temp_vector_0(), instr->parallel_moves()[0],
                  instr->parallel_moves()[1]);
  }
}

// Split multiple loads of the same constant or stack slot off into the second
// slot and keep remaining moves in the first slot.
void MoveOptimizer::FinalizeMoves(Instruction* instr) {
  auto loads = temp_vector_0();
  DCHECK(loads.empty());
  auto new_moves = temp_vector_1();
  DCHECK(new_moves.empty());
  auto move_ops = instr->parallel_moves()[0]->move_operands();
  for (auto move = move_ops->begin(); move != move_ops->end(); ++move) {
    if (move->IsRedundant()) {
      move->Eliminate();
      continue;
    }
    if (!(move->source()->IsConstant() || move->source()->IsStackSlot() ||
          move->source()->IsDoubleStackSlot()))
      continue;
    // Search for existing move to this slot.
    MoveOperands* found = nullptr;
    for (auto load : loads) {
      if (load->source()->Equals(move->source())) {
        found = load;
        break;
      }
    }
    // Not found so insert.
    if (found == nullptr) {
      loads.push_back(move);
      // Replace source with copy for later use.
      auto dest = move->destination();
      move->set_destination(InstructionOperand::New(code_zone(), *dest));
      continue;
    }
    if ((found->destination()->IsStackSlot() ||
         found->destination()->IsDoubleStackSlot()) &&
        !(move->destination()->IsStackSlot() ||
          move->destination()->IsDoubleStackSlot())) {
      // Found a better source for this load.  Smash it in place to affect other
      // loads that have already been split.
      auto next_dest =
          InstructionOperand::New(code_zone(), *found->destination());
      auto dest = move->destination();
      InstructionOperand::ReplaceWith(found->destination(), dest);
      move->set_destination(next_dest);
    }
    // move from load destination.
    move->set_source(found->destination());
    new_moves.push_back(move);
  }
  loads.clear();
  if (new_moves.empty()) return;
  // Insert all new moves into slot 1.
  auto slot_1 = instr->GetOrCreateParallelMove(
      static_cast<Instruction::GapPosition>(1), code_zone());
  DCHECK(slot_1->move_operands()->is_empty());
  slot_1->move_operands()->AddBlock(MoveOperands(nullptr, nullptr),
                                    static_cast<int>(new_moves.size()),
                                    code_zone());
  auto it = slot_1->move_operands()->begin();
  for (auto new_move : new_moves) {
    std::swap(*new_move, *it);
    ++it;
  }
  DCHECK_EQ(it, slot_1->move_operands()->end());
  new_moves.clear();
}

}  // namespace compiler
}  // namespace internal
}  // namespace v8
<commit_msg>fix variable shadowing<commit_after>// Copyright 2014 the V8 project authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "src/compiler/move-optimizer.h"

namespace v8 {
namespace internal {
namespace compiler {

namespace {

typedef std::pair<InstructionOperand, InstructionOperand> MoveKey;
typedef ZoneMap<MoveKey, unsigned> MoveMap;
typedef ZoneSet<InstructionOperand> OperandSet;

bool GapsCanMoveOver(Instruction* instr) {
  return instr->IsSourcePosition() || instr->IsNop();
}

int FindFirstNonEmptySlot(Instruction* instr) {
  int i = Instruction::FIRST_GAP_POSITION;
  for (; i <= Instruction::LAST_GAP_POSITION; i++) {
    auto move = instr->parallel_moves()[i];
    if (move == nullptr) continue;
    auto move_ops = move->move_operands();
    auto op = move_ops->begin();
    for (; op != move_ops->end(); ++op) {
      if (!op->IsRedundant()) break;
      op->Eliminate();
    }
    if (op != move_ops->end()) break;  // Found non-redundant move.
    move_ops->Rewind(0);               // Clear this redundant move.
  }
  return i;
}

}  // namepace

MoveOptimizer::MoveOptimizer(Zone* local_zone, InstructionSequence* code)
    : local_zone_(local_zone),
      code_(code),
      to_finalize_(local_zone),
      temp_vector_0_(local_zone),
      temp_vector_1_(local_zone) {}

void MoveOptimizer::Run() {
  for (auto* block : code()->instruction_blocks()) {
    CompressBlock(block);
  }
  for (auto block : code()->instruction_blocks()) {
    if (block->PredecessorCount() <= 1) continue;
    OptimizeMerge(block);
  }
  for (auto gap : to_finalize_) {
    FinalizeMoves(gap);
  }
}

void MoveOptimizer::CompressMoves(MoveOpVector* eliminated, ParallelMove* left,
                                  ParallelMove* right) {
  DCHECK(eliminated->empty());
  auto move_ops = right->move_operands();
  if (!left->move_operands()->is_empty()) {
    // Modify the right moves in place and collect moves that will be killed by
    // merging the two gaps.
    for (auto op = move_ops->begin(); op != move_ops->end(); ++op) {
      if (op->IsRedundant()) continue;
      auto to_eliminate = left->PrepareInsertAfter(op);
      if (to_eliminate != nullptr) eliminated->push_back(to_eliminate);
    }
    // Eliminate dead moves.  Must happen before insertion of new moves as the
    // contents of eliminated are pointers into a list.
    for (auto to_eliminate : *eliminated) {
      to_eliminate->Eliminate();
    }
    eliminated->clear();
  }
  // Add all possibly modified moves from right side.
  for (auto op = move_ops->begin(); op != move_ops->end(); ++op) {
    if (op->IsRedundant()) continue;
    left->move_operands()->Add(*op, code_zone());
  }
  // Nuke right.
  move_ops->Rewind(0);
}

// Smash all consecutive moves into the left most move slot and accumulate them
// as much as possible across instructions.
void MoveOptimizer::CompressBlock(InstructionBlock* block) {
  auto temp_vector = temp_vector_0();
  DCHECK(temp_vector.empty());
  Instruction* prev_instr = nullptr;
  for (int index = block->code_start(); index < block->code_end(); ++index) {
    auto instr = code()->instructions()[index];
    int i = FindFirstNonEmptySlot(instr);
    if (i <= Instruction::LAST_GAP_POSITION) {
      // Move the first non-empty gap to position 0.
      std::swap(instr->parallel_moves()[0], instr->parallel_moves()[i]);
      auto left = instr->parallel_moves()[0];
      // Compress everything into position 0.
      for (++i; i <= Instruction::LAST_GAP_POSITION; ++i) {
        auto move = instr->parallel_moves()[i];
        if (move == nullptr) continue;
        CompressMoves(&temp_vector, left, move);
      }
      if (prev_instr != nullptr) {
        // Smash left into prev_instr, killing left.
        auto pred_moves = prev_instr->parallel_moves()[0];
        CompressMoves(&temp_vector, pred_moves, left);
      }
    }
    if (prev_instr != nullptr) {
      // Slide prev_instr down so we always know where to look for it.
      std::swap(prev_instr->parallel_moves()[0], instr->parallel_moves()[0]);
    }
    prev_instr = instr->parallel_moves()[0] == nullptr ? nullptr : instr;
    if (GapsCanMoveOver(instr)) continue;
    if (prev_instr != nullptr) {
      to_finalize_.push_back(prev_instr);
      prev_instr = nullptr;
    }
  }
  if (prev_instr != nullptr) {
    to_finalize_.push_back(prev_instr);
  }
}

Instruction* MoveOptimizer::LastInstruction(InstructionBlock* block) {
  return code()->instructions()[block->last_instruction_index()];
}

void MoveOptimizer::OptimizeMerge(InstructionBlock* block) {
  DCHECK(block->PredecessorCount() > 1);
  // Ensure that the last instruction in all incoming blocks don't contain
  // things that would prevent moving gap moves across them.
  for (auto pred_index : block->predecessors()) {
    auto pred = code()->InstructionBlockAt(pred_index);
    auto last_instr = code()->instructions()[pred->last_instruction_index()];
    if (last_instr->IsSourcePosition()) continue;
    if (last_instr->IsCall()) return;
    if (last_instr->TempCount() != 0) return;
    if (last_instr->OutputCount() != 0) return;
    for (size_t i = 0; i < last_instr->InputCount(); ++i) {
      auto op = last_instr->InputAt(i);
      if (!op->IsConstant() && !op->IsImmediate()) return;
    }
  }
  // TODO(dcarney): pass a ZonePool down for this?
  MoveMap move_map(local_zone());
  size_t correct_counts = 0;
  // Accumulate set of shared moves.
  for (auto pred_index : block->predecessors()) {
    auto pred = code()->InstructionBlockAt(pred_index);
    auto instr = LastInstruction(pred);
    if (instr->parallel_moves()[0] == nullptr ||
        instr->parallel_moves()[0]->move_operands()->is_empty()) {
      return;
    }
    auto move_ops = instr->parallel_moves()[0]->move_operands();
    for (auto op = move_ops->begin(); op != move_ops->end(); ++op) {
      if (op->IsRedundant()) continue;
      auto src = *op->source();
      auto dst = *op->destination();
      MoveKey key = {src, dst};
      auto res = move_map.insert(std::make_pair(key, 1));
      if (!res.second) {
        res.first->second++;
        if (res.first->second == block->PredecessorCount()) {
          correct_counts++;
        }
      }
    }
  }
  if (move_map.empty() || correct_counts != move_map.size()) return;
  // Find insertion point.
  Instruction* instr = nullptr;
  for (int i = block->first_instruction_index();
       i <= block->last_instruction_index(); ++i) {
    instr = code()->instructions()[i];
    if (!GapsCanMoveOver(instr) || !instr->AreMovesRedundant()) break;
  }
  DCHECK(instr != nullptr);
  bool gap_initialized = true;
  if (instr->parallel_moves()[0] == nullptr ||
      instr->parallel_moves()[0]->move_operands()->is_empty()) {
    to_finalize_.push_back(instr);
  } else {
    // Will compress after insertion.
    gap_initialized = false;
    std::swap(instr->parallel_moves()[0], instr->parallel_moves()[1]);
  }
  auto move = instr->GetOrCreateParallelMove(
      static_cast<Instruction::GapPosition>(0), code_zone());
  // Delete relevant entries in predecessors and move everything to block.
  bool first_iteration = true;
  for (auto pred_index : block->predecessors()) {
    auto pred = code()->InstructionBlockAt(pred_index);
    auto move_ops = LastInstruction(pred)->parallel_moves()[0]->move_operands();
    for (auto op = move_ops->begin(); op != move_ops->end(); ++op) {
      if (op->IsRedundant()) continue;
      MoveKey key = {*op->source(), *op->destination()};
      auto it = move_map.find(key);
      USE(it);
      DCHECK(it != move_map.end());
      if (first_iteration) {
        move->AddMove(op->source(), op->destination(), code_zone());
      }
      op->Eliminate();
    }
    first_iteration = false;
  }
  // Compress.
  if (!gap_initialized) {
    CompressMoves(&temp_vector_0(), instr->parallel_moves()[0],
                  instr->parallel_moves()[1]);
  }
}

// Split multiple loads of the same constant or stack slot off into the second
// slot and keep remaining moves in the first slot.
void MoveOptimizer::FinalizeMoves(Instruction* instr) {
  auto loads = temp_vector_0();
  DCHECK(loads.empty());
  auto new_moves = temp_vector_1();
  DCHECK(new_moves.empty());
  auto move_ops = instr->parallel_moves()[0]->move_operands();
  for (auto move = move_ops->begin(); move != move_ops->end(); ++move) {
    if (move->IsRedundant()) {
      move->Eliminate();
      continue;
    }
    if (!(move->source()->IsConstant() || move->source()->IsStackSlot() ||
          move->source()->IsDoubleStackSlot()))
      continue;
    // Search for existing move to this slot.
    MoveOperands* found = nullptr;
    for (auto load : loads) {
      if (load->source()->Equals(move->source())) {
        found = load;
        break;
      }
    }
    // Not found so insert.
    if (found == nullptr) {
      loads.push_back(move);
      // Replace source with copy for later use.
      auto dest = move->destination();
      move->set_destination(InstructionOperand::New(code_zone(), *dest));
      continue;
    }
    if ((found->destination()->IsStackSlot() ||
         found->destination()->IsDoubleStackSlot()) &&
        !(move->destination()->IsStackSlot() ||
          move->destination()->IsDoubleStackSlot())) {
      // Found a better source for this load.  Smash it in place to affect other
      // loads that have already been split.
      auto next_dest =
          InstructionOperand::New(code_zone(), *found->destination());
      auto dest = move->destination();
      InstructionOperand::ReplaceWith(found->destination(), dest);
      move->set_destination(next_dest);
    }
    // move from load destination.
    move->set_source(found->destination());
    new_moves.push_back(move);
  }
  loads.clear();
  if (new_moves.empty()) return;
  // Insert all new moves into slot 1.
  auto slot_1 = instr->GetOrCreateParallelMove(
      static_cast<Instruction::GapPosition>(1), code_zone());
  DCHECK(slot_1->move_operands()->is_empty());
  slot_1->move_operands()->AddBlock(MoveOperands(nullptr, nullptr),
                                    static_cast<int>(new_moves.size()),
                                    code_zone());
  auto it = slot_1->move_operands()->begin();
  for (auto new_move : new_moves) {
    std::swap(*new_move, *it);
    ++it;
  }
  DCHECK_EQ(it, slot_1->move_operands()->end());
  new_moves.clear();
}

}  // namespace compiler
}  // namespace internal
}  // namespace v8
<|endoftext|>


<commit_before>/*################################################################################
  ##
  ##   Copyright (C) 2016-2017 Keith O'Hara
  ##
  ##   This file is part of the OptimLib C++ library.
  ##
  ##   OptimLib is free software: you can redistribute it and/or modify
  ##   it under the terms of the GNU General Public License as published by
  ##   the Free Software Foundation, either version 2 of the License, or
  ##   (at your option) any later version.
  ##
  ##   OptimLib 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.
  ##
  ################################################################################*/

/*
 * L-BFGS method for quasi-Newton-based non-linear optimization
 *
 * Keith O'Hara
 * 12/23/2016
 *
 * This version:
 * 08/20/2017
 */

#include "optim.hpp"

bool
optim::lbfgs_int(arma::vec& init_out_vals, std::function<double (const arma::vec& vals_inp, arma::vec* grad_out, void* opt_data)> opt_objfn, void* opt_data, opt_settings* settings_inp)
{   // notation: 'p' stands for '+1'.
    //
    bool success = false;

    const int n_vals = init_out_vals.n_elem;

    //
    // BFGS settings

    opt_settings settings;

    if (settings_inp) {
        settings = *settings_inp;
    }

    const int conv_failure_switch = settings.conv_failure_switch;
    const int iter_max = settings.iter_max;
    const double err_tol = settings.err_tol;

    const double wolfe_cons_1 = 1E-03; // line search tuning parameters
    const double wolfe_cons_2 = 0.90;

    const int par_M = settings.lbfgs_par_M;

    const bool vals_bound = settings.vals_bound;

    const arma::vec lower_bounds = settings.lower_bounds;
    const arma::vec upper_bounds = settings.upper_bounds;

    const arma::uvec bounds_type = determine_bounds_type(vals_bound, n_vals, lower_bounds, upper_bounds);

    // lambda function for box constraints

    std::function<double (const arma::vec& vals_inp, arma::vec* grad_out, void* box_data)> box_objfn = [opt_objfn, vals_bound, bounds_type, lower_bounds, upper_bounds] (const arma::vec& vals_inp, arma::vec* grad_out, void* opt_data) -> double {
        //

        if (vals_bound) {

            arma::vec vals_inv_trans = inv_transform(vals_inp, bounds_type, lower_bounds, upper_bounds);

            double ret;

            if (grad_out) {
                arma::vec grad_obj = *grad_out;

                ret = opt_objfn(vals_inv_trans,&grad_obj,opt_data);

                arma::mat jacob_matrix = jacobian_adjust(vals_inp,bounds_type,lower_bounds,upper_bounds);

                // *grad_out = jacob_matrix.t() * grad_obj; // correct gradient for transformation
                *grad_out = jacob_matrix * grad_obj; // no need for transpose as jacob_matrix is diagonal
            } else {
                ret = opt_objfn(vals_inv_trans,nullptr,opt_data);
            }

            return ret;
        } else {
            double ret = opt_objfn(vals_inp,grad_out,opt_data);

            return ret;
        }
    };

    //
    // initialization

    arma::vec x = init_out_vals;

    if (!x.is_finite()) {
        printf("lbfgs error: non-finite initial value(s).\n");

        return false;
    }

    if (vals_bound) { // should we transform the parameters?
	    x = transform(x, bounds_type, lower_bounds, upper_bounds);
    }

    arma::vec grad(n_vals); // gradient vector
    box_objfn(x,&grad,opt_data);

    // double err = arma::accu(arma::abs(grad));
    double err = arma::norm(grad, 2);
    if (err <= err_tol) {
        return true;
    }

    //
    // if ||gradient(initial values)|| > tolerance, then continue

    // double t_line = 1.0;    // initial line search value
    arma::vec d = - grad; // direction

    arma::vec x_p = x, grad_p = grad;

    line_search_mt(1.0, x_p, grad_p, d, &wolfe_cons_1, &wolfe_cons_2, box_objfn, opt_data);

    err = arma::norm(grad, 2);  // check updated values
    if (err <= err_tol) {
        init_out_vals = x_p;
        return true;
    }

    //
    // if ||gradient(x_p)|| > tolerance, then continue

    arma::vec s = x_p - x;
    arma::vec y = grad_p - grad;

    arma::mat s_mat(n_vals,par_M);
    arma::mat y_mat(n_vals,par_M);

    s_mat.col(0) = s;
    y_mat.col(0) = y;

    grad = grad_p;

    //
    // begin loop

    int iter = 0;

    while (err > err_tol && iter < iter_max) {
        iter++;
        //
        int par_M_act = std::min(iter,par_M);

        d = - lbfgs_recur(grad,s_mat,y_mat,par_M_act);

        line_search_mt(1.0, x_p, grad_p, d, &wolfe_cons_1, &wolfe_cons_2, box_objfn, opt_data);

        // err = arma::accu(arma::abs(grad_p));
        err = arma::norm(grad_p, 2);
        if (err <= err_tol) {
            break;
        }

        // if ||gradient(x_p)|| > tolerance, then continue
        s = x_p - x;
        y = grad_p - grad;

        err = arma::norm(s, 2);

        //

        x = x_p;
        grad = grad_p;

        s_mat.cols(1,par_M-1) = s_mat.cols(0,par_M-2);
        y_mat.cols(1,par_M-1) = y_mat.cols(0,par_M-2);

        s_mat.col(0) = s;
        y_mat.col(0) = y;
    }
    //
    if (vals_bound) {
	    x_p = inv_transform(x_p, bounds_type, lower_bounds, upper_bounds);
    }

    error_reporting(init_out_vals,x_p,opt_objfn,opt_data,success,err,err_tol,iter,iter_max,conv_failure_switch,settings_inp);
    //
    return success;
}

bool
optim::lbfgs(arma::vec& init_out_vals, std::function<double (const arma::vec& vals_inp, arma::vec* grad_out, void* opt_data)> opt_objfn, void* opt_data)
{
    return lbfgs_int(init_out_vals,opt_objfn,opt_data,nullptr);
}

bool
optim::lbfgs(arma::vec& init_out_vals, std::function<double (const arma::vec& vals_inp, arma::vec* grad_out, void* opt_data)> opt_objfn, void* opt_data, opt_settings& settings)
{
    return lbfgs_int(init_out_vals,opt_objfn,opt_data,&settings);
}

arma::vec
optim::lbfgs_recur(arma::vec q, const arma::mat& s_mat, const arma::mat& y_mat, const int M)
{
    arma::vec alpha_vec(M);

    double rho = 1.0;

    for (int i=0; i < M; i++) {
        rho = 1.0 / arma::dot(y_mat.col(i),s_mat.col(i));
        alpha_vec(i) = rho*arma::dot(s_mat.col(i),q);

        q -= alpha_vec(i)*y_mat.col(i);
    }

    arma::vec r = q * ( arma::dot(s_mat.col(0),y_mat.col(0)) / arma::dot(y_mat.col(0),y_mat.col(0)) );

    double beta = 1.0;

    for (int i = M - 1; i >= 0; i--) {
        rho = 1.0 / arma::dot(y_mat.col(i),s_mat.col(i));
        beta = rho*arma::dot(y_mat.col(i),r);

        r += (alpha_vec(i) - beta)*s_mat.col(i);
    }

    return r;
}
<commit_msg>add reference<commit_after>/*################################################################################
  ##
  ##   Copyright (C) 2016-2017 Keith O'Hara
  ##
  ##   This file is part of the OptimLib C++ library.
  ##
  ##   OptimLib is free software: you can redistribute it and/or modify
  ##   it under the terms of the GNU General Public License as published by
  ##   the Free Software Foundation, either version 2 of the License, or
  ##   (at your option) any later version.
  ##
  ##   OptimLib 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.
  ##
  ################################################################################*/

/*
 * L-BFGS method for quasi-Newton-based non-linear optimization
 *
 * Keith O'Hara
 * 12/23/2016
 *
 * This version:
 * 08/20/2017
 */

#include "optim.hpp"

bool
optim::lbfgs_int(arma::vec& init_out_vals, std::function<double (const arma::vec& vals_inp, arma::vec* grad_out, void* opt_data)> opt_objfn, void* opt_data, opt_settings* settings_inp)
{   // notation: 'p' stands for '+1'.
    //
    bool success = false;

    const int n_vals = init_out_vals.n_elem;

    //
    // BFGS settings

    opt_settings settings;

    if (settings_inp) {
        settings = *settings_inp;
    }

    const int conv_failure_switch = settings.conv_failure_switch;
    const int iter_max = settings.iter_max;
    const double err_tol = settings.err_tol;

    const double wolfe_cons_1 = 1E-03; // line search tuning parameters
    const double wolfe_cons_2 = 0.90;

    const int par_M = settings.lbfgs_par_M;

    const bool vals_bound = settings.vals_bound;

    const arma::vec lower_bounds = settings.lower_bounds;
    const arma::vec upper_bounds = settings.upper_bounds;

    const arma::uvec bounds_type = determine_bounds_type(vals_bound, n_vals, lower_bounds, upper_bounds);

    // lambda function for box constraints

    std::function<double (const arma::vec& vals_inp, arma::vec* grad_out, void* box_data)> box_objfn = [opt_objfn, vals_bound, bounds_type, lower_bounds, upper_bounds] (const arma::vec& vals_inp, arma::vec* grad_out, void* opt_data) -> double {
        //

        if (vals_bound) {

            arma::vec vals_inv_trans = inv_transform(vals_inp, bounds_type, lower_bounds, upper_bounds);

            double ret;

            if (grad_out) {
                arma::vec grad_obj = *grad_out;

                ret = opt_objfn(vals_inv_trans,&grad_obj,opt_data);

                arma::mat jacob_matrix = jacobian_adjust(vals_inp,bounds_type,lower_bounds,upper_bounds);

                // *grad_out = jacob_matrix.t() * grad_obj; // correct gradient for transformation
                *grad_out = jacob_matrix * grad_obj; // no need for transpose as jacob_matrix is diagonal
            } else {
                ret = opt_objfn(vals_inv_trans,nullptr,opt_data);
            }

            return ret;
        } else {
            double ret = opt_objfn(vals_inp,grad_out,opt_data);

            return ret;
        }
    };

    //
    // initialization

    arma::vec x = init_out_vals;

    if (!x.is_finite()) {
        printf("lbfgs error: non-finite initial value(s).\n");

        return false;
    }

    if (vals_bound) { // should we transform the parameters?
	    x = transform(x, bounds_type, lower_bounds, upper_bounds);
    }

    arma::vec grad(n_vals); // gradient vector
    box_objfn(x,&grad,opt_data);

    // double err = arma::accu(arma::abs(grad));
    double err = arma::norm(grad, 2);
    if (err <= err_tol) {
        return true;
    }

    //
    // if ||gradient(initial values)|| > tolerance, then continue

    // double t_line = 1.0;    // initial line search value
    arma::vec d = - grad; // direction

    arma::vec x_p = x, grad_p = grad;

    line_search_mt(1.0, x_p, grad_p, d, &wolfe_cons_1, &wolfe_cons_2, box_objfn, opt_data);

    err = arma::norm(grad, 2);  // check updated values
    if (err <= err_tol) {
        init_out_vals = x_p;
        return true;
    }

    //
    // if ||gradient(x_p)|| > tolerance, then continue

    arma::vec s = x_p - x;
    arma::vec y = grad_p - grad;

    arma::mat s_mat(n_vals,par_M);
    arma::mat y_mat(n_vals,par_M);

    s_mat.col(0) = s;
    y_mat.col(0) = y;

    grad = grad_p;

    //
    // begin loop

    int iter = 0;

    while (err > err_tol && iter < iter_max) {
        iter++;
        //
        int par_M_act = std::min(iter,par_M);

        d = - lbfgs_recur(grad,s_mat,y_mat,par_M_act);

        line_search_mt(1.0, x_p, grad_p, d, &wolfe_cons_1, &wolfe_cons_2, box_objfn, opt_data);

        // err = arma::accu(arma::abs(grad_p));
        err = arma::norm(grad_p, 2);
        if (err <= err_tol) {
            break;
        }

        // if ||gradient(x_p)|| > tolerance, then continue
        s = x_p - x;
        y = grad_p - grad;

        err = arma::norm(s, 2);

        //

        x = x_p;
        grad = grad_p;

        s_mat.cols(1,par_M-1) = s_mat.cols(0,par_M-2);
        y_mat.cols(1,par_M-1) = y_mat.cols(0,par_M-2);

        s_mat.col(0) = s;
        y_mat.col(0) = y;
    }
    //
    if (vals_bound) {
	    x_p = inv_transform(x_p, bounds_type, lower_bounds, upper_bounds);
    }

    error_reporting(init_out_vals,x_p,opt_objfn,opt_data,success,err,err_tol,iter,iter_max,conv_failure_switch,settings_inp);
    //
    return success;
}

bool
optim::lbfgs(arma::vec& init_out_vals, std::function<double (const arma::vec& vals_inp, arma::vec* grad_out, void* opt_data)> opt_objfn, void* opt_data)
{
    return lbfgs_int(init_out_vals,opt_objfn,opt_data,nullptr);
}

bool
optim::lbfgs(arma::vec& init_out_vals, std::function<double (const arma::vec& vals_inp, arma::vec* grad_out, void* opt_data)> opt_objfn, void* opt_data, opt_settings& settings)
{
    return lbfgs_int(init_out_vals,opt_objfn,opt_data,&settings);
}

// algorithm 7.4 of Nocedal and Wright (2006)
arma::vec
optim::lbfgs_recur(arma::vec q, const arma::mat& s_mat, const arma::mat& y_mat, const int M)
{
    arma::vec alpha_vec(M);

    double rho = 1.0;

    for (int i=0; i < M; i++) {
        rho = 1.0 / arma::dot(y_mat.col(i),s_mat.col(i));
        alpha_vec(i) = rho*arma::dot(s_mat.col(i),q);

        q -= alpha_vec(i)*y_mat.col(i);
    }

    arma::vec r = q * ( arma::dot(s_mat.col(0),y_mat.col(0)) / arma::dot(y_mat.col(0),y_mat.col(0)) );

    double beta = 1.0;

    for (int i = M - 1; i >= 0; i--) {
        rho = 1.0 / arma::dot(y_mat.col(i),s_mat.col(i));
        beta = rho*arma::dot(y_mat.col(i),r);

        r += (alpha_vec(i) - beta)*s_mat.col(i);
    }

    return r;
}
<|endoftext|>


<commit_before>/* bzflag
 * Copyright (c) 1993 - 2004 Tim Riker
 *
 * This package is free software;  you can redistribute it and/or
 * modify it under the terms of the license found in the file
 * named COPYING that should have accompanied this file.
 *
 * THIS PACKAGE IS PROVIDED ``AS IS'' AND WITHOUT ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED
 * WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */

#include "vectors.h"
#include "MeshSceneNodeGenerator.h"
#include "MeshObstacle.h"
#include "MeshFace.h"
#include "bzfgl.h"
#include "MeshPolySceneNode.h"
#include "DynamicColor.h"
#include "TextureManager.h"
#include "OpenGLMaterial.h"

//
// MeshSceneNodeGenerator
//

MeshSceneNodeGenerator::MeshSceneNodeGenerator(const MeshObstacle* _mesh)
{
  mesh = _mesh;
  faceNumber = 0;
  return;
}

MeshSceneNodeGenerator::~MeshSceneNodeGenerator()
{
  // do nothing
  return;
}

WallSceneNode* MeshSceneNodeGenerator::getNextNode(bool /*lod*/)
{
  int i;
  const MeshFace* face;

  // remove any faces on the ground that will not be displayed
  // also, return NULL if we are at the end of the face list
  while (true) {
    if (faceNumber >= mesh->getFaceCount()) {
      return NULL;
    }
    face = mesh->getFace(faceNumber);
    const float* plane = face->getPlane();
    if (plane[2] < -0.9f) {
      // plane is facing downwards
      float mins[3], maxs[3];
      face->getExtents(mins, maxs);
      if (maxs[2] < 0.001) {
        // plane is on or below the ground, ditch it
        faceNumber++;
        continue;
      } else {
        break;
      }
    } else {
      break;
    }
  }

  GLfloat base[3], sCorner[3], tCorner[3];
  memcpy (base, face->getVertex(0), sizeof(float[3]));
  memcpy (sCorner, face->getVertex(1), sizeof(float[3]));
  memcpy (tCorner, face->getVertex(2), sizeof(float[3]));

  GLfloat sEdge[3];
  GLfloat tEdge[3];
  sEdge[0] = sCorner[0] - base[0];
  sEdge[1] = sCorner[1] - base[1];
  sEdge[2] = sCorner[2] - base[2];
  tEdge[0] = tCorner[0] - base[0];
  tEdge[1] = tCorner[1] - base[1];
  tEdge[2] = tCorner[2] - base[2];

  // vertices
  const int vertexCount = face->getVertexCount();
  GLfloat3Array vertices(vertexCount);
  for (i = 0; i < vertexCount; i++) {
    memcpy (vertices[i], face->getVertex(i), sizeof(float[3]));
  }

  // normals
  int normalCount = 0;
  if (face->useNormals()) {
    normalCount = vertexCount;
  }
  GLfloat3Array normals(normalCount);
  for (i = 0; i < normalCount; i++) {
    memcpy (normals[i], face->getNormal(i), sizeof(float[3]));
  }

  // texcoords
  GLfloat2Array texcoords(vertexCount);
  if (face->useTexcoords()) {
    for (i = 0; i < vertexCount; i++) {
      memcpy (texcoords[i], face->getTexcoord(i), sizeof(float[2]));
    }
  } else {
    makeTexcoords (face->getPlane(), vertices, texcoords);
  }

  MeshPolySceneNode* node =
    new MeshPolySceneNode(face->getPlane(), vertices, normals, texcoords);

  const MeshMaterial* mat = face->getMaterial();
  setupNodeMaterial(node, mat);

  faceNumber++;

  return node;
}

void MeshSceneNodeGenerator::setupNodeMaterial(MeshPolySceneNode* node,
                                               const MeshMaterial* mat)
{
  TextureManager &tm = TextureManager::instance();
  OpenGLMaterial glMaterial(mat->specular, mat->emission, mat->shininess);

  int faceTexture = -1;
  bool gotSpecifiedTexture = false;
  if (mat->useTexture) {
    if (mat->texture.size() > 0) {
      faceTexture = tm.getTextureID(mat->texture.c_str());
    }
    if (faceTexture >= 0) {
      gotSpecifiedTexture = true;
    } else {
      faceTexture = tm.getTextureID("mesh", false /* no failure reports */);
    }
  }

  // NOTE: the diffuse color is used, and not the ambient color
  //       could use the ambient color for non-lighted,and diffuse
  //       for lighted
  const DynamicColor* dyncol = DYNCOLORMGR.getColor(mat->dynamicColor);
  const GLfloat* dc = dyncol->getColor();
  node->setDynamicColor(dc);
  node->setColor(mat->diffuse); // redundant, see below
  node->setModulateColor(mat->diffuse);
  node->setLightedColor(mat->diffuse);
  node->setLightedModulateColor(mat->diffuse);
  node->setMaterial(glMaterial);
  node->setTexture(faceTexture);
  if (mat->useColorOnTexture || !gotSpecifiedTexture) {
    // modulate with the color if asked to, or
    // if the specified texture was not available
    node->setUseColorTexture(false);
  } else {
    node->setUseColorTexture(true);
  }
  node->setTextureMatrix(mat->textureMatrix);

  // deal with the blending setting for textures
  bool alpha = false;
  if ((faceTexture >= 0) && mat->useTextureAlpha) {
    const ImageInfo& imageInfo = tm.getInfo(faceTexture);
    alpha = imageInfo.alpha;
  }
  node->setBlending(alpha);
  node->setSphereMap(mat->useSphereMap);

  // the current color can also affect the blending.
  // if blending is disabled then the alpha value from
  // one of these colors is used to set the stipple value.
  // we'll just set it to the middle value.
  if (dc) {
    const float color[4] = { 1.0f, 1.0f, 1.0f, 0.5f }; // alpha value != 1.0f
    if (dyncol->canHaveAlpha()) {
      node->setColor(color); // trigger transparency check
      node->setModulateColor(color);
      node->setLightedColor(color);
      node->setLightedModulateColor(color);
    }
  }
}

bool MeshSceneNodeGenerator::makeTexcoords(const float* plane,
                                           const GLfloat3Array& vertices,
                                           GLfloat2Array& texcoords)
{
  float x[3], y[3];

  vec3sub (x, vertices[1], vertices[0]);
  vec3cross (y, plane, x);

  float len = vec3dot(x, x);
  if (len > 0.0f) {
    len = 1 / len;
    x[0] = x[0] * len;
    x[1] = x[1] * len;
    x[2] = x[2] * len;
  } else {
    return false;
  }

  len = vec3dot(y, y);
  if (len > 0.0f) {
    len = 1 / len;
    y[0] = y[0] * len;
    y[1] = y[1] * len;
    y[2] = y[2] * len;
  } else {
    return false;
  }

  const float uvScale = 4.0f;

  texcoords[0][0] = 0.0f;
  texcoords[0][1] = 0.0f;
  const int count = vertices.getSize();
  for (int i = 1; i < count; i++) {
    float delta[3];
    vec3sub (delta, vertices[i], vertices[0]);
    texcoords[i][0] = vec3dot(delta, x) / uvScale;
    texcoords[i][1] = vec3dot(delta, y) / uvScale;
  }

  return true;
}

// Local Variables: ***
// mode:C++ ***
// tab-width: 8 ***
// c-basic-offset: 2 ***
// indent-tabs-mode: t ***
// End: ***
// ex: shiftwidth=2 tabstop=8
<commit_msg>don't include invisible meshface scenenodes<commit_after>/* bzflag
 * Copyright (c) 1993 - 2004 Tim Riker
 *
 * This package is free software;  you can redistribute it and/or
 * modify it under the terms of the license found in the file
 * named COPYING that should have accompanied this file.
 *
 * THIS PACKAGE IS PROVIDED ``AS IS'' AND WITHOUT ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED
 * WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */

#include "vectors.h"
#include "MeshSceneNodeGenerator.h"
#include "MeshObstacle.h"
#include "MeshFace.h"
#include "bzfgl.h"
#include "MeshPolySceneNode.h"
#include "DynamicColor.h"
#include "TextureManager.h"
#include "OpenGLMaterial.h"

//
// MeshSceneNodeGenerator
//

MeshSceneNodeGenerator::MeshSceneNodeGenerator(const MeshObstacle* _mesh)
{
  mesh = _mesh;
  faceNumber = 0;
  return;
}

MeshSceneNodeGenerator::~MeshSceneNodeGenerator()
{
  // do nothing
  return;
}

WallSceneNode* MeshSceneNodeGenerator::getNextNode(bool /*lod*/)
{
  int i;
  const MeshFace* face;
  const MeshMaterial* mat;

  // remove any faces on the ground that will not be displayed
  // also, return NULL if we are at the end of the face list
  while (true) {
    if (faceNumber >= mesh->getFaceCount()) {
      return NULL;
    }
    face = mesh->getFace(faceNumber);
    mat = face->getMaterial();
    const DynamicColor* dyncol = DYNCOLORMGR.getColor(mat->dynamicColor);
    if ((mat->diffuse[3] == 0.0f) && (dyncol == NULL) &&
        !(mat->useTexture && !mat->useColorOnTexture)) {
      // face is invisible
      faceNumber++;
      continue;
    }
    const float* plane = face->getPlane();
    if (plane[2] < -0.9f) {
      // plane is facing downwards
      float mins[3], maxs[3];
      face->getExtents(mins, maxs);
      if (maxs[2] < 0.001) {
        // plane is on or below the ground, ditch it
        faceNumber++;
        continue;
      } else {
        break;
      }
    } else {
      break;
    }
  }

  GLfloat base[3], sCorner[3], tCorner[3];
  memcpy (base, face->getVertex(0), sizeof(float[3]));
  memcpy (sCorner, face->getVertex(1), sizeof(float[3]));
  memcpy (tCorner, face->getVertex(2), sizeof(float[3]));

  GLfloat sEdge[3];
  GLfloat tEdge[3];
  sEdge[0] = sCorner[0] - base[0];
  sEdge[1] = sCorner[1] - base[1];
  sEdge[2] = sCorner[2] - base[2];
  tEdge[0] = tCorner[0] - base[0];
  tEdge[1] = tCorner[1] - base[1];
  tEdge[2] = tCorner[2] - base[2];

  // vertices
  const int vertexCount = face->getVertexCount();
  GLfloat3Array vertices(vertexCount);
  for (i = 0; i < vertexCount; i++) {
    memcpy (vertices[i], face->getVertex(i), sizeof(float[3]));
  }

  // normals
  int normalCount = 0;
  if (face->useNormals()) {
    normalCount = vertexCount;
  }
  GLfloat3Array normals(normalCount);
  for (i = 0; i < normalCount; i++) {
    memcpy (normals[i], face->getNormal(i), sizeof(float[3]));
  }

  // texcoords
  GLfloat2Array texcoords(vertexCount);
  if (face->useTexcoords()) {
    for (i = 0; i < vertexCount; i++) {
      memcpy (texcoords[i], face->getTexcoord(i), sizeof(float[2]));
    }
  } else {
    makeTexcoords (face->getPlane(), vertices, texcoords);
  }

  MeshPolySceneNode* node =
    new MeshPolySceneNode(face->getPlane(), vertices, normals, texcoords);

  setupNodeMaterial(node, mat);

  faceNumber++;

  return node;
}

void MeshSceneNodeGenerator::setupNodeMaterial(MeshPolySceneNode* node,
                                               const MeshMaterial* mat)
{
  TextureManager &tm = TextureManager::instance();
  OpenGLMaterial glMaterial(mat->specular, mat->emission, mat->shininess);

  int faceTexture = -1;
  bool gotSpecifiedTexture = false;
  if (mat->useTexture) {
    if (mat->texture.size() > 0) {
      faceTexture = tm.getTextureID(mat->texture.c_str());
    }
    if (faceTexture >= 0) {
      gotSpecifiedTexture = true;
    } else {
      faceTexture = tm.getTextureID("mesh", false /* no failure reports */);
    }
  }

  // NOTE: the diffuse color is used, and not the ambient color
  //       could use the ambient color for non-lighted,and diffuse
  //       for lighted
  const DynamicColor* dyncol = DYNCOLORMGR.getColor(mat->dynamicColor);
  const GLfloat* dc = dyncol->getColor();
  node->setDynamicColor(dc);
  node->setColor(mat->diffuse); // redundant, see below
  node->setModulateColor(mat->diffuse);
  node->setLightedColor(mat->diffuse);
  node->setLightedModulateColor(mat->diffuse);
  node->setMaterial(glMaterial);
  node->setTexture(faceTexture);
  if (mat->useColorOnTexture || !gotSpecifiedTexture) {
    // modulate with the color if asked to, or
    // if the specified texture was not available
    node->setUseColorTexture(false);
  } else {
    node->setUseColorTexture(true);
  }
  node->setTextureMatrix(mat->textureMatrix);

  // deal with the blending setting for textures
  bool alpha = false;
  if ((faceTexture >= 0) && mat->useTextureAlpha) {
    const ImageInfo& imageInfo = tm.getInfo(faceTexture);
    alpha = imageInfo.alpha;
  }
  node->setBlending(alpha);
  node->setSphereMap(mat->useSphereMap);

  // the current color can also affect the blending.
  // if blending is disabled then the alpha value from
  // one of these colors is used to set the stipple value.
  // we'll just set it to the middle value.
  if (dc) {
    const float color[4] = { 1.0f, 1.0f, 1.0f, 0.5f }; // alpha value != 1.0f
    if (dyncol->canHaveAlpha()) {
      node->setColor(color); // trigger transparency check
      node->setModulateColor(color);
      node->setLightedColor(color);
      node->setLightedModulateColor(color);
    }
  }
}

bool MeshSceneNodeGenerator::makeTexcoords(const float* plane,
                                           const GLfloat3Array& vertices,
                                           GLfloat2Array& texcoords)
{
  float x[3], y[3];

  vec3sub (x, vertices[1], vertices[0]);
  vec3cross (y, plane, x);

  float len = vec3dot(x, x);
  if (len > 0.0f) {
    len = 1 / len;
    x[0] = x[0] * len;
    x[1] = x[1] * len;
    x[2] = x[2] * len;
  } else {
    return false;
  }

  len = vec3dot(y, y);
  if (len > 0.0f) {
    len = 1 / len;
    y[0] = y[0] * len;
    y[1] = y[1] * len;
    y[2] = y[2] * len;
  } else {
    return false;
  }

  const float uvScale = 4.0f;

  texcoords[0][0] = 0.0f;
  texcoords[0][1] = 0.0f;
  const int count = vertices.getSize();
  for (int i = 1; i < count; i++) {
    float delta[3];
    vec3sub (delta, vertices[i], vertices[0]);
    texcoords[i][0] = vec3dot(delta, x) / uvScale;
    texcoords[i][1] = vec3dot(delta, y) / uvScale;
  }

  return true;
}

// Local Variables: ***
// mode:C++ ***
// tab-width: 8 ***
// c-basic-offset: 2 ***
// indent-tabs-mode: t ***
// End: ***
// ex: shiftwidth=2 tabstop=8
<|endoftext|>