<commit_before>/* Copyright (c) 2014 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 mc_att_control_base.cpp
 *
 * MC Attitude Controller : Control and math code
 *
 * @author Tobias Naegeli <naegelit@student.ethz.ch>
 * @author Lorenz Meier <lm@inf.ethz.ch>
 * @author Anton Babushkin <anton.babushkin@me.com>
 * @author Thomas Gubler <thomasgubler@gmail.com>
 * @author Julian Oes <julian@oes.ch>
 * @author Roman Bapst <bapstr@ethz.ch>
 *
 */

#include "mc_att_control_base.h"
#include <geo/geo.h>
#include <math.h>
#include <lib/mathlib/mathlib.h>

#ifdef CONFIG_ARCH_ARM
#else
#include <cmath>
using namespace std;
#endif

MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
	_publish_att_sp(false)

{
	memset(&_v_att_sp_mod, 0, sizeof(_v_att_sp_mod));
	memset(&_v_rates_sp_mod, 0, sizeof(_v_rates_sp_mod));
	memset(&_actuators, 0, sizeof(_actuators));

	_params.att_p.zero();
	_params.rate_p.zero();
	_params.rate_i.zero();
	_params.rate_d.zero();
	_params.yaw_ff = 0.0f;
	_params.yaw_rate_max = 0.0f;
	_params.man_roll_max = 0.0f;
	_params.man_pitch_max = 0.0f;
	_params.man_yaw_max = 0.0f;
	_params.acro_rate_max.zero();

	_rates_prev.zero();
	_rates_sp.zero();
	_rates_int.zero();
	_thrust_sp = 0.0f;
	_att_control.zero();

	_I.identity();
}

MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase()
{
}

void MulticopterAttitudeControlBase::control_attitude(float dt)
{
	float yaw_sp_move_rate = 0.0f;
	_publish_att_sp = false;

	if (_v_control_mode->data().flag_control_manual_enabled) {
		/* manual input, set or modify attitude setpoint */

		if (_v_control_mode->data().flag_control_velocity_enabled
		    || _v_control_mode->data().flag_control_climb_rate_enabled) {
			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
			memcpy(&_v_att_sp_mod, &_v_att_sp->data(), sizeof(_v_att_sp_mod));
		}

		if (!_v_control_mode->data().flag_control_climb_rate_enabled) {
			/* pass throttle directly if not in altitude stabilized mode */
			_v_att_sp_mod.data().thrust = _manual_control_sp->data().z;
			_publish_att_sp = true;
		}

		if (!_armed->data().armed) {
			/* reset yaw setpoint when disarmed */
			_reset_yaw_sp = true;
		}

		/* move yaw setpoint in all modes */
		if (_v_att_sp_mod.data().thrust < 0.1f) {
			// TODO
			//if (_status.condition_landed) {
			/* reset yaw setpoint if on ground */
			//	reset_yaw_sp = true;
			//}
		} else {
			/* move yaw setpoint */
			yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max;
			_v_att_sp_mod.data().yaw_body = _wrap_pi(
						     _v_att_sp_mod.data().yaw_body + yaw_sp_move_rate * dt);
			float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
			float yaw_offs = _wrap_pi(_v_att_sp_mod.data().yaw_body - _v_att->data().yaw);

			if (yaw_offs < -yaw_offs_max) {
				_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw - yaw_offs_max);

			} else if (yaw_offs > yaw_offs_max) {
				_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw + yaw_offs_max);
			}

			_v_att_sp_mod.data().R_valid = false;
			// _publish_att_sp = true;
		}

		/* reset yaw setpint to current position if needed */
		if (_reset_yaw_sp) {
			_reset_yaw_sp = false;
			_v_att_sp_mod.data().yaw_body = _v_att->data().yaw;
			_v_att_sp_mod.data().R_valid = false;
			// _publish_att_sp = true;
		}

		if (!_v_control_mode->data().flag_control_velocity_enabled) {
			/* update attitude setpoint if not in position control mode */
			_v_att_sp_mod.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max;
			_v_att_sp_mod.data().pitch_body = -_manual_control_sp->data().x
					       * _params.man_pitch_max;
			_v_att_sp_mod.data().R_valid = false;
			// _publish_att_sp = true;
		}

	} else {
		/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
		memcpy(&_v_att_sp_mod, &_v_att_sp->data(), sizeof(_v_att_sp_mod));

		/* reset yaw setpoint after non-manual control mode */
		_reset_yaw_sp = true;
	}

	_thrust_sp = _v_att_sp_mod.data().thrust;

	/* construct attitude setpoint rotation matrix */
	math::Matrix<3, 3> R_sp;

	if (_v_att_sp_mod.data().R_valid) {
		/* rotation matrix in _att_sp is valid, use it */
		R_sp.set(&_v_att_sp_mod.data().R_body[0]);

	} else {
		/* rotation matrix in _att_sp is not valid, use euler angles instead */
		R_sp.from_euler(_v_att_sp_mod.data().roll_body, _v_att_sp_mod.data().pitch_body,
				_v_att_sp_mod.data().yaw_body);

		/* copy rotation matrix back to setpoint struct */
		memcpy(&_v_att_sp_mod.data().R_body[0], &R_sp.data[0][0],
		       sizeof(_v_att_sp_mod.data().R_body));
		_v_att_sp_mod.data().R_valid = true;
	}

	/* rotation matrix for current state */
	math::Matrix<3, 3> R;
	R.set(_v_att->data().R);

	/* all input data is ready, run controller itself */

	/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
	math::Vector < 3 > R_z(R(0, 2), R(1, 2), R(2, 2));
	math::Vector < 3 > R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));

	/* axis and sin(angle) of desired rotation */
	math::Vector < 3 > e_R = R.transposed() * (R_z % R_sp_z);

	/* calculate angle error */
	float e_R_z_sin = e_R.length();
	float e_R_z_cos = R_z * R_sp_z;

	/* calculate weight for yaw control */
	float yaw_w = R_sp(2, 2) * R_sp(2, 2);

	/* calculate rotation matrix after roll/pitch only rotation */
	math::Matrix<3, 3> R_rp;

	if (e_R_z_sin > 0.0f) {
		/* get axis-angle representation */
		float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
		math::Vector < 3 > e_R_z_axis = e_R / e_R_z_sin;

		e_R = e_R_z_axis * e_R_z_angle;

		/* cross product matrix for e_R_axis */
		math::Matrix<3, 3> e_R_cp;
		e_R_cp.zero();
		e_R_cp(0, 1) = -e_R_z_axis(2);
		e_R_cp(0, 2) = e_R_z_axis(1);
		e_R_cp(1, 0) = e_R_z_axis(2);
		e_R_cp(1, 2) = -e_R_z_axis(0);
		e_R_cp(2, 0) = -e_R_z_axis(1);
		e_R_cp(2, 1) = e_R_z_axis(0);

		/* rotation matrix for roll/pitch only rotation */
		R_rp = R
		       * (_I + e_R_cp * e_R_z_sin
			  + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));

	} else {
		/* zero roll/pitch rotation */
		R_rp = R;
	}

	/* R_rp and R_sp has the same Z axis, calculate yaw error */
	math::Vector < 3 > R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
	math::Vector < 3 > R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
	e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;

	if (e_R_z_cos < 0.0f) {
		/* for large thrust vector rotations use another rotation method:
		 * calculate angle and axis for R -> R_sp rotation directly */
		math::Quaternion q;
		q.from_dcm(R.transposed() * R_sp);
		math::Vector < 3 > e_R_d = q.imag();
		e_R_d.normalize();
		e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));

		/* use fusion of Z axis based rotation and direct rotation */
		float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
		e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
	}

	/* calculate angular rates setpoint */
	_rates_sp = _params.att_p.emult(e_R);

	/* limit yaw rate */
	_rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max,
				       _params.yaw_rate_max);

	/* feed forward yaw setpoint rate */
	_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
}

void MulticopterAttitudeControlBase::control_attitude_rates(float dt)
{
	/* reset integral if disarmed */
	if (!_armed->data().armed || !_v_status->data().is_rotary_wing) {
		_rates_int.zero();
	}

	/* current body angular rates */
	math::Vector < 3 > rates;
	rates(0) = _v_att->data().rollspeed;
	rates(1) = _v_att->data().pitchspeed;
	rates(2) = _v_att->data().yawspeed;

	/* angular rates error */
	math::Vector < 3 > rates_err = _rates_sp - rates;
	_att_control = _params.rate_p.emult(rates_err)
		       + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
	_rates_prev = rates;

	/* update integral only if not saturated on low limit */
	if (_thrust_sp > MIN_TAKEOFF_THRUST) {
		for (int i = 0; i < 3; i++) {
			if (fabsf(_att_control(i)) < _thrust_sp) {
				float rate_i = _rates_int(i)
					       + _params.rate_i(i) * rates_err(i) * dt;

				if (isfinite(
					    rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
					_rates_int(i) = rate_i;
				}
			}
		}
	}

}

void MulticopterAttitudeControlBase::set_actuator_controls()
{
	_actuators.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
	_actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
	_actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
	_actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
}
<commit_msg>mc attctl multiplatform: remove memsets<commit_after>/* Copyright (c) 2014 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 mc_att_control_base.cpp
 *
 * MC Attitude Controller : Control and math code
 *
 * @author Tobias Naegeli <naegelit@student.ethz.ch>
 * @author Lorenz Meier <lm@inf.ethz.ch>
 * @author Anton Babushkin <anton.babushkin@me.com>
 * @author Thomas Gubler <thomasgubler@gmail.com>
 * @author Julian Oes <julian@oes.ch>
 * @author Roman Bapst <bapstr@ethz.ch>
 *
 */

#include "mc_att_control_base.h"
#include <geo/geo.h>
#include <math.h>
#include <lib/mathlib/mathlib.h>

#ifdef CONFIG_ARCH_ARM
#else
#include <cmath>
using namespace std;
#endif

MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
	_v_att_sp_mod(),
	_v_rates_sp_mod(),
	_actuators(),
	_publish_att_sp(false)

{
	_params.att_p.zero();
	_params.rate_p.zero();
	_params.rate_i.zero();
	_params.rate_d.zero();
	_params.yaw_ff = 0.0f;
	_params.yaw_rate_max = 0.0f;
	_params.man_roll_max = 0.0f;
	_params.man_pitch_max = 0.0f;
	_params.man_yaw_max = 0.0f;
	_params.acro_rate_max.zero();

	_rates_prev.zero();
	_rates_sp.zero();
	_rates_int.zero();
	_thrust_sp = 0.0f;
	_att_control.zero();

	_I.identity();
}

MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase()
{
}

void MulticopterAttitudeControlBase::control_attitude(float dt)
{
	float yaw_sp_move_rate = 0.0f;
	_publish_att_sp = false;

	if (_v_control_mode->data().flag_control_manual_enabled) {
		/* manual input, set or modify attitude setpoint */

		if (_v_control_mode->data().flag_control_velocity_enabled
		    || _v_control_mode->data().flag_control_climb_rate_enabled) {
			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
			memcpy(&_v_att_sp_mod, &_v_att_sp->data(), sizeof(_v_att_sp_mod));
		}

		if (!_v_control_mode->data().flag_control_climb_rate_enabled) {
			/* pass throttle directly if not in altitude stabilized mode */
			_v_att_sp_mod.data().thrust = _manual_control_sp->data().z;
			_publish_att_sp = true;
		}

		if (!_armed->data().armed) {
			/* reset yaw setpoint when disarmed */
			_reset_yaw_sp = true;
		}

		/* move yaw setpoint in all modes */
		if (_v_att_sp_mod.data().thrust < 0.1f) {
			// TODO
			//if (_status.condition_landed) {
			/* reset yaw setpoint if on ground */
			//	reset_yaw_sp = true;
			//}
		} else {
			/* move yaw setpoint */
			yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max;
			_v_att_sp_mod.data().yaw_body = _wrap_pi(
						     _v_att_sp_mod.data().yaw_body + yaw_sp_move_rate * dt);
			float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
			float yaw_offs = _wrap_pi(_v_att_sp_mod.data().yaw_body - _v_att->data().yaw);

			if (yaw_offs < -yaw_offs_max) {
				_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw - yaw_offs_max);

			} else if (yaw_offs > yaw_offs_max) {
				_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw + yaw_offs_max);
			}

			_v_att_sp_mod.data().R_valid = false;
			// _publish_att_sp = true;
		}

		/* reset yaw setpint to current position if needed */
		if (_reset_yaw_sp) {
			_reset_yaw_sp = false;
			_v_att_sp_mod.data().yaw_body = _v_att->data().yaw;
			_v_att_sp_mod.data().R_valid = false;
			// _publish_att_sp = true;
		}

		if (!_v_control_mode->data().flag_control_velocity_enabled) {
			/* update attitude setpoint if not in position control mode */
			_v_att_sp_mod.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max;
			_v_att_sp_mod.data().pitch_body = -_manual_control_sp->data().x
					       * _params.man_pitch_max;
			_v_att_sp_mod.data().R_valid = false;
			// _publish_att_sp = true;
		}

	} else {
		/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
		memcpy(&_v_att_sp_mod, &_v_att_sp->data(), sizeof(_v_att_sp_mod));

		/* reset yaw setpoint after non-manual control mode */
		_reset_yaw_sp = true;
	}

	_thrust_sp = _v_att_sp_mod.data().thrust;

	/* construct attitude setpoint rotation matrix */
	math::Matrix<3, 3> R_sp;

	if (_v_att_sp_mod.data().R_valid) {
		/* rotation matrix in _att_sp is valid, use it */
		R_sp.set(&_v_att_sp_mod.data().R_body[0]);

	} else {
		/* rotation matrix in _att_sp is not valid, use euler angles instead */
		R_sp.from_euler(_v_att_sp_mod.data().roll_body, _v_att_sp_mod.data().pitch_body,
				_v_att_sp_mod.data().yaw_body);

		/* copy rotation matrix back to setpoint struct */
		memcpy(&_v_att_sp_mod.data().R_body[0], &R_sp.data[0][0],
		       sizeof(_v_att_sp_mod.data().R_body));
		_v_att_sp_mod.data().R_valid = true;
	}

	/* rotation matrix for current state */
	math::Matrix<3, 3> R;
	R.set(_v_att->data().R);

	/* all input data is ready, run controller itself */

	/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
	math::Vector < 3 > R_z(R(0, 2), R(1, 2), R(2, 2));
	math::Vector < 3 > R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));

	/* axis and sin(angle) of desired rotation */
	math::Vector < 3 > e_R = R.transposed() * (R_z % R_sp_z);

	/* calculate angle error */
	float e_R_z_sin = e_R.length();
	float e_R_z_cos = R_z * R_sp_z;

	/* calculate weight for yaw control */
	float yaw_w = R_sp(2, 2) * R_sp(2, 2);

	/* calculate rotation matrix after roll/pitch only rotation */
	math::Matrix<3, 3> R_rp;

	if (e_R_z_sin > 0.0f) {
		/* get axis-angle representation */
		float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
		math::Vector < 3 > e_R_z_axis = e_R / e_R_z_sin;

		e_R = e_R_z_axis * e_R_z_angle;

		/* cross product matrix for e_R_axis */
		math::Matrix<3, 3> e_R_cp;
		e_R_cp.zero();
		e_R_cp(0, 1) = -e_R_z_axis(2);
		e_R_cp(0, 2) = e_R_z_axis(1);
		e_R_cp(1, 0) = e_R_z_axis(2);
		e_R_cp(1, 2) = -e_R_z_axis(0);
		e_R_cp(2, 0) = -e_R_z_axis(1);
		e_R_cp(2, 1) = e_R_z_axis(0);

		/* rotation matrix for roll/pitch only rotation */
		R_rp = R
		       * (_I + e_R_cp * e_R_z_sin
			  + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));

	} else {
		/* zero roll/pitch rotation */
		R_rp = R;
	}

	/* R_rp and R_sp has the same Z axis, calculate yaw error */
	math::Vector < 3 > R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
	math::Vector < 3 > R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
	e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;

	if (e_R_z_cos < 0.0f) {
		/* for large thrust vector rotations use another rotation method:
		 * calculate angle and axis for R -> R_sp rotation directly */
		math::Quaternion q;
		q.from_dcm(R.transposed() * R_sp);
		math::Vector < 3 > e_R_d = q.imag();
		e_R_d.normalize();
		e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));

		/* use fusion of Z axis based rotation and direct rotation */
		float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
		e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
	}

	/* calculate angular rates setpoint */
	_rates_sp = _params.att_p.emult(e_R);

	/* limit yaw rate */
	_rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max,
				       _params.yaw_rate_max);

	/* feed forward yaw setpoint rate */
	_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
}

void MulticopterAttitudeControlBase::control_attitude_rates(float dt)
{
	/* reset integral if disarmed */
	if (!_armed->data().armed || !_v_status->data().is_rotary_wing) {
		_rates_int.zero();
	}

	/* current body angular rates */
	math::Vector < 3 > rates;
	rates(0) = _v_att->data().rollspeed;
	rates(1) = _v_att->data().pitchspeed;
	rates(2) = _v_att->data().yawspeed;

	/* angular rates error */
	math::Vector < 3 > rates_err = _rates_sp - rates;
	_att_control = _params.rate_p.emult(rates_err)
		       + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
	_rates_prev = rates;

	/* update integral only if not saturated on low limit */
	if (_thrust_sp > MIN_TAKEOFF_THRUST) {
		for (int i = 0; i < 3; i++) {
			if (fabsf(_att_control(i)) < _thrust_sp) {
				float rate_i = _rates_int(i)
					       + _params.rate_i(i) * rates_err(i) * dt;

				if (isfinite(
					    rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
					_rates_int(i) = rate_i;
				}
			}
		}
	}

}

void MulticopterAttitudeControlBase::set_actuator_controls()
{
	_actuators.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
	_actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
	_actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
	_actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
}
<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2019 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.
 *
 ****************************************************************************/

#include "VehicleAcceleration.hpp"

#include <px4_platform_common/log.h>

using namespace matrix;
using namespace time_literals;

namespace sensors
{

VehicleAcceleration::VehicleAcceleration() :
	ModuleParams(nullptr),
	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_controllers)
{
	_lp_filter.set_cutoff_frequency(kInitialRateHz, _param_imu_accel_cutoff.get());
}

VehicleAcceleration::~VehicleAcceleration()
{
	Stop();
}

bool VehicleAcceleration::Start()
{
	// force initial updates
	ParametersUpdate(true);

	// sensor_selection needed to change the active sensor if the primary stops updating
	if (!_sensor_selection_sub.registerCallback()) {
		PX4_ERR("sensor_selection callback registration failed");
		return false;
	}

	if (!SensorSelectionUpdate(true)) {
		ScheduleDelayed(10_ms);
	}

	return true;
}

void VehicleAcceleration::Stop()
{
	// clear all registered callbacks
	for (auto &sub : _sensor_sub) {
		sub.unregisterCallback();
	}

	_sensor_selection_sub.unregisterCallback();

	Deinit();
}

void VehicleAcceleration::CheckFilters()
{
	if (_interval_count > 1000) {
		bool reset_filters = false;

		// calculate sensor update rate
		const float sample_interval_avg = _interval_sum / _interval_count;

		if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.0f)) {

			_update_rate_hz = 1.e6f / sample_interval_avg;

			// check if sample rate error is greater than 1%
			if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
				reset_filters = true;
			}

			if (reset_filters || (_required_sample_updates == 0)) {
				if (_param_imu_integ_rate.get() > 0) {
					// determine number of sensor samples that will get closest to the desired rate
					const float configured_interval_us = 1e6f / _param_imu_integ_rate.get();
					const uint8_t samples = math::constrain(roundf(configured_interval_us / sample_interval_avg), 1.f,
										(float)sensor_accel_s::ORB_QUEUE_LENGTH);

					_sensor_sub[_selected_sensor_sub_index].set_required_updates(samples);
					_required_sample_updates = samples;

				} else {
					_sensor_sub[_selected_sensor_sub_index].set_required_updates(1);
					_required_sample_updates = 1;
				}
			}
		}

		if (!reset_filters) {
			// accel low pass cutoff frequency changed
			if (fabsf(_lp_filter.get_cutoff_freq() - _param_imu_accel_cutoff.get()) > 0.01f) {
				reset_filters = true;
			}
		}

		if (reset_filters) {
			PX4_DEBUG("resetting filters, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz);
			_filter_sample_rate = _update_rate_hz;

			// update software low pass filters
			_lp_filter.set_cutoff_frequency(_filter_sample_rate, _param_imu_accel_cutoff.get());
			_lp_filter.reset(_acceleration_prev);
		}

		// reset sample interval accumulator
		_timestamp_sample_last = 0;
	}
}

void VehicleAcceleration::SensorBiasUpdate(bool force)
{
	if (_estimator_sensor_bias_sub.updated() || force) {
		estimator_sensor_bias_s bias;

		if (_estimator_sensor_bias_sub.copy(&bias)) {
			if (bias.accel_device_id == _selected_sensor_device_id) {
				_bias = Vector3f{bias.accel_bias};

			} else {
				_bias.zero();
			}
		}
	}
}

bool VehicleAcceleration::SensorSelectionUpdate(bool force)
{
	if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) {
		sensor_selection_s sensor_selection{};
		_sensor_selection_sub.copy(&sensor_selection);

		if (_selected_sensor_device_id != sensor_selection.accel_device_id) {
			// clear all registered callbacks
			for (auto &sub : _sensor_sub) {
				sub.unregisterCallback();
			}

			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
				sensor_accel_s report{};
				_sensor_sub[i].copy(&report);

				if ((report.device_id != 0) && (report.device_id == sensor_selection.accel_device_id)) {
					if (_sensor_sub[i].registerCallback()) {
						PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor_sub_index, i);

						// record selected sensor (array index)
						_selected_sensor_sub_index = i;
						_selected_sensor_device_id = sensor_selection.accel_device_id;

						// clear bias and corrections
						_bias.zero();

						_calibration.set_device_id(report.device_id);

						// reset sample interval accumulator on sensor change
						_timestamp_sample_last = 0;
						_required_sample_updates = 0;

						return true;
					}
				}
			}

			PX4_ERR("unable to find or subscribe to selected sensor (%d)", sensor_selection.accel_device_id);
			_selected_sensor_device_id = 0;
			_selected_sensor_sub_index = 0;
		}
	}

	return false;
}

void VehicleAcceleration::ParametersUpdate(bool force)
{
	// Check if parameters have changed
	if (_params_sub.updated() || force) {
		// clear update
		parameter_update_s param_update;
		_params_sub.copy(&param_update);

		updateParams();

		_calibration.ParametersUpdate();
	}
}

void VehicleAcceleration::Run()
{
	// backup schedule
	ScheduleDelayed(10_ms);

	// update corrections first to set _selected_sensor
	bool selection_updated = SensorSelectionUpdate();

	_calibration.SensorCorrectionsUpdate(selection_updated);
	SensorBiasUpdate(selection_updated);
	ParametersUpdate();

	bool sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated();

	// process all outstanding messages
	while (sensor_updated || selection_updated) {
		selection_updated = false;

		sensor_accel_s sensor_data;

		if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) {

			if (sensor_updated) {
				// collect sample interval average for filters
				if ((_timestamp_sample_last > 0) && (sensor_data.timestamp_sample > _timestamp_sample_last)) {
					_interval_sum += (sensor_data.timestamp_sample - _timestamp_sample_last);
					_interval_count++;

				} else {
					_interval_sum = 0.f;
					_interval_count = 0.f;
				}

				_timestamp_sample_last = sensor_data.timestamp_sample;
			}

			CheckFilters();

			// Filter: apply low-pass
			const Vector3f accel_filtered = _lp_filter.apply(Vector3f{sensor_data.x, sensor_data.y, sensor_data.z});

			_acceleration_prev = accel_filtered;

			// publish once all new samples are processed
			sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated();

			if (!sensor_updated) {
				// correct for in-run bias errors
				const Vector3f accel = _calibration.Correct(accel_filtered) - _bias;

				// Publish vehicle_acceleration
				vehicle_acceleration_s v_acceleration;
				v_acceleration.timestamp_sample = sensor_data.timestamp_sample;
				accel.copyTo(v_acceleration.xyz);
				v_acceleration.timestamp = hrt_absolute_time();
				_vehicle_acceleration_pub.publish(v_acceleration);

				return;
			}
		}
	}
}

void VehicleAcceleration::PrintStatus()
{
	PX4_INFO("selected sensor: %d (%d), rate: %.1f Hz",
		 _selected_sensor_device_id, _selected_sensor_sub_index, (double)_update_rate_hz);
	PX4_INFO("estimated bias: [%.4f %.4f %.4f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));

	_calibration.PrintStatus();
}

} // namespace sensors
<commit_msg>sensors/vehicle_acceleration: apply calibration before filtering<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2019 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.
 *
 ****************************************************************************/

#include "VehicleAcceleration.hpp"

#include <px4_platform_common/log.h>

using namespace matrix;
using namespace time_literals;

namespace sensors
{

VehicleAcceleration::VehicleAcceleration() :
	ModuleParams(nullptr),
	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_controllers)
{
	_lp_filter.set_cutoff_frequency(kInitialRateHz, _param_imu_accel_cutoff.get());
}

VehicleAcceleration::~VehicleAcceleration()
{
	Stop();
}

bool VehicleAcceleration::Start()
{
	// force initial updates
	ParametersUpdate(true);

	// sensor_selection needed to change the active sensor if the primary stops updating
	if (!_sensor_selection_sub.registerCallback()) {
		PX4_ERR("sensor_selection callback registration failed");
		return false;
	}

	if (!SensorSelectionUpdate(true)) {
		ScheduleDelayed(10_ms);
	}

	return true;
}

void VehicleAcceleration::Stop()
{
	// clear all registered callbacks
	for (auto &sub : _sensor_sub) {
		sub.unregisterCallback();
	}

	_sensor_selection_sub.unregisterCallback();

	Deinit();
}

void VehicleAcceleration::CheckFilters()
{
	if (_interval_count > 1000) {
		bool reset_filters = false;

		// calculate sensor update rate
		const float sample_interval_avg = _interval_sum / _interval_count;

		if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.0f)) {

			_update_rate_hz = 1.e6f / sample_interval_avg;

			// check if sample rate error is greater than 1%
			if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
				reset_filters = true;
			}

			if (reset_filters || (_required_sample_updates == 0)) {
				if (_param_imu_integ_rate.get() > 0) {
					// determine number of sensor samples that will get closest to the desired rate
					const float configured_interval_us = 1e6f / _param_imu_integ_rate.get();
					const uint8_t samples = math::constrain(roundf(configured_interval_us / sample_interval_avg), 1.f,
										(float)sensor_accel_s::ORB_QUEUE_LENGTH);

					_sensor_sub[_selected_sensor_sub_index].set_required_updates(samples);
					_required_sample_updates = samples;

				} else {
					_sensor_sub[_selected_sensor_sub_index].set_required_updates(1);
					_required_sample_updates = 1;
				}
			}
		}

		if (!reset_filters) {
			// accel low pass cutoff frequency changed
			if (fabsf(_lp_filter.get_cutoff_freq() - _param_imu_accel_cutoff.get()) > 0.01f) {
				reset_filters = true;
			}
		}

		if (reset_filters) {
			PX4_DEBUG("resetting filters, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz);
			_filter_sample_rate = _update_rate_hz;

			// update software low pass filters
			_lp_filter.set_cutoff_frequency(_filter_sample_rate, _param_imu_accel_cutoff.get());
			_lp_filter.reset(_acceleration_prev);
		}

		// reset sample interval accumulator
		_timestamp_sample_last = 0;
	}
}

void VehicleAcceleration::SensorBiasUpdate(bool force)
{
	if (_estimator_sensor_bias_sub.updated() || force) {
		estimator_sensor_bias_s bias;

		if (_estimator_sensor_bias_sub.copy(&bias)) {
			if (bias.accel_device_id == _selected_sensor_device_id) {
				_bias = Vector3f{bias.accel_bias};

			} else {
				_bias.zero();
			}
		}
	}
}

bool VehicleAcceleration::SensorSelectionUpdate(bool force)
{
	if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) {
		sensor_selection_s sensor_selection{};
		_sensor_selection_sub.copy(&sensor_selection);

		if (_selected_sensor_device_id != sensor_selection.accel_device_id) {
			// clear all registered callbacks
			for (auto &sub : _sensor_sub) {
				sub.unregisterCallback();
			}

			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
				sensor_accel_s report{};
				_sensor_sub[i].copy(&report);

				if ((report.device_id != 0) && (report.device_id == sensor_selection.accel_device_id)) {
					if (_sensor_sub[i].registerCallback()) {
						PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor_sub_index, i);

						// record selected sensor (array index)
						_selected_sensor_sub_index = i;
						_selected_sensor_device_id = sensor_selection.accel_device_id;

						// clear bias and corrections
						_bias.zero();

						_calibration.set_device_id(report.device_id);

						// reset sample interval accumulator on sensor change
						_timestamp_sample_last = 0;
						_required_sample_updates = 0;

						return true;
					}
				}
			}

			PX4_ERR("unable to find or subscribe to selected sensor (%d)", sensor_selection.accel_device_id);
			_selected_sensor_device_id = 0;
			_selected_sensor_sub_index = 0;
		}
	}

	return false;
}

void VehicleAcceleration::ParametersUpdate(bool force)
{
	// Check if parameters have changed
	if (_params_sub.updated() || force) {
		// clear update
		parameter_update_s param_update;
		_params_sub.copy(&param_update);

		updateParams();

		_calibration.ParametersUpdate();
	}
}

void VehicleAcceleration::Run()
{
	// backup schedule
	ScheduleDelayed(10_ms);

	// update corrections first to set _selected_sensor
	bool selection_updated = SensorSelectionUpdate();

	_calibration.SensorCorrectionsUpdate(selection_updated);
	SensorBiasUpdate(selection_updated);
	ParametersUpdate();

	bool sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated();

	// process all outstanding messages
	while (sensor_updated || selection_updated) {
		selection_updated = false;

		sensor_accel_s sensor_data;

		if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) {

			if (sensor_updated) {
				// collect sample interval average for filters
				if ((_timestamp_sample_last > 0) && (sensor_data.timestamp_sample > _timestamp_sample_last)) {
					_interval_sum += (sensor_data.timestamp_sample - _timestamp_sample_last);
					_interval_count++;

				} else {
					_interval_sum = 0.f;
					_interval_count = 0.f;
				}

				_timestamp_sample_last = sensor_data.timestamp_sample;
			}

			CheckFilters();

			// Apply calibration and filter
			//  - calibration offsets, scale factors, and thermal scale (if available)
			//  - estimated in run bias (if available)
			//  - biquad low-pass filter
			const Vector3f accel_corrected = _calibration.Correct(Vector3f{sensor_data.x, sensor_data.y, sensor_data.z}) - _bias;
			const Vector3f accel_filtered = _lp_filter.apply(accel_corrected);

			_acceleration_prev = accel_corrected;

			// publish once all new samples are processed
			sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated();

			if (!sensor_updated) {
				// Publish vehicle_acceleration
				vehicle_acceleration_s v_acceleration;
				v_acceleration.timestamp_sample = sensor_data.timestamp_sample;
				accel_filtered.copyTo(v_acceleration.xyz);
				v_acceleration.timestamp = hrt_absolute_time();
				_vehicle_acceleration_pub.publish(v_acceleration);

				return;
			}
		}
	}
}

void VehicleAcceleration::PrintStatus()
{
	PX4_INFO("selected sensor: %d (%d), rate: %.1f Hz",
		 _selected_sensor_device_id, _selected_sensor_sub_index, (double)_update_rate_hz);
	PX4_INFO("estimated bias: [%.4f %.4f %.4f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));

	_calibration.PrintStatus();
}

} // namespace sensors
<|endoftext|>


<commit_before>/*  -*-c++-*-
 *  Copyright (C) 2008 Cedric Pinson <mornifle@plopbyte.net>
 *
 * This library is open source and may be redistributed and/or modified under
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * OpenSceneGraph Public License for more details.
 *
 *  Authors:
 *   Jeremy Moles  <jeremy@emperorlinux.com>
 *   Cedric Pinson <mornifle@plopbyte.net>
*/

#include <iostream>
#include <osg/io_utils>
#include <osg/Geometry>
#include <osg/Shape>
#include <osg/ShapeDrawable>
#include <osg/Material>
#include <osg/MatrixTransform>
#include <osgViewer/Viewer>
#include <osgViewer/ViewerEventHandlers>
#include <osgGA/TrackballManipulator>
#include <osgAnimation/Sampler>

  class AnimtkUpdateCallback : public osg::NodeCallback
  {
  public:
      META_Object(osgAnimation, AnimtkUpdateCallback);

      AnimtkUpdateCallback()
      {
          _sampler = new osgAnimation::Vec3CubicBezierSampler;
          _playing = false;
          _lastUpdate = 0;
      }
      AnimtkUpdateCallback(const AnimtkUpdateCallback& val, const osg::CopyOp& copyop = osg::CopyOp::SHALLOW_COPY):
          osg::Object(val, copyop),
          osg::NodeCallback(val, copyop),
          _sampler(val._sampler),
          _startTime(val._startTime),
          _currentTime(val._currentTime),
          _playing(val._playing),
          _lastUpdate(val._lastUpdate)
      {
      }

      /** Callback method called by the NodeVisitor when visiting a node.*/
      virtual void operator()(osg::Node* node, osg::NodeVisitor* nv)
      {
          if (nv->getVisitorType() == osg::NodeVisitor::UPDATE_VISITOR &&
              nv->getFrameStamp() &&
              nv->getFrameStamp()->getFrameNumber() != _lastUpdate)
          {

              _lastUpdate = nv->getFrameStamp()->getFrameNumber();
              _currentTime = osg::Timer::instance()->tick();

              if (_playing && _sampler.get() && _sampler->getKeyframeContainer())
              {
                  osg::MatrixTransform* transform = dynamic_cast<osg::MatrixTransform*>(node);
                  if (transform) {
                      osg::Vec3 result;
                      float t = osg::Timer::instance()->delta_s(_startTime, _currentTime);
                      float duration = _sampler->getEndTime() - _sampler->getStartTime();
                      t = fmod(t, duration);
                      t += _sampler->getStartTime();
                      _sampler->getValueAt(t, result);
                      transform->setMatrix(osg::Matrix::translate(result));
                  }
              }
          }
          // note, callback is responsible for scenegraph traversal so
          // they must call traverse(node,nv) to ensure that the
          // scene graph subtree (and associated callbacks) are traversed.
          traverse(node,nv);
      }

      void start() { _startTime = osg::Timer::instance()->tick(); _currentTime = _startTime; _playing = true;}
      void stop() { _currentTime = _startTime; _playing = false;}

      osg::ref_ptr<osgAnimation::Vec3CubicBezierSampler> _sampler;
      osg::Timer_t _startTime;
      osg::Timer_t _currentTime;
      bool _playing;
      unsigned int _lastUpdate;
  };

class AnimtkStateSetUpdateCallback : public osg::StateSet::Callback
{
public:
    META_Object(osgAnimation, AnimtkStateSetUpdateCallback);

    AnimtkStateSetUpdateCallback()
    {
        _sampler = new osgAnimation::Vec4LinearSampler;
        _playing = false;
        _lastUpdate = 0;
    }

    AnimtkStateSetUpdateCallback(const AnimtkStateSetUpdateCallback& val, const osg::CopyOp& copyop = osg::CopyOp::SHALLOW_COPY):
        osg::Object(val, copyop),
        osg::StateSet::Callback(val, copyop),
        _sampler(val._sampler),
        _startTime(val._startTime),
        _currentTime(val._currentTime),
        _playing(val._playing),
        _lastUpdate(val._lastUpdate)
    {
    }

    /** Callback method called by the NodeVisitor when visiting a node.*/
    virtual void operator()(osg::StateSet* state, osg::NodeVisitor* nv)
    {
        if (state &&
            nv->getVisitorType() == osg::NodeVisitor::UPDATE_VISITOR &&
            nv->getFrameStamp() &&
            nv->getFrameStamp()->getFrameNumber() != _lastUpdate) {

            _lastUpdate = nv->getFrameStamp()->getFrameNumber();
            _currentTime = osg::Timer::instance()->tick();

            if (_playing && _sampler.get() && _sampler->getKeyframeContainer())
            {
                osg::Material* material = dynamic_cast<osg::Material*>(state->getAttribute(osg::StateAttribute::MATERIAL));
                if (material)
                {
                    osg::Vec4 result;
                    float t = osg::Timer::instance()->delta_s(_startTime, _currentTime);
                    float duration = _sampler->getEndTime() - _sampler->getStartTime();
                    t = fmod(t, duration);
                    t += _sampler->getStartTime();
                    _sampler->getValueAt(t, result);
                    material->setDiffuse(osg::Material::FRONT_AND_BACK, result);
                }
            }
        }
    }

    void start() { _startTime = osg::Timer::instance()->tick(); _currentTime = _startTime; _playing = true;}
    void stop() { _currentTime = _startTime; _playing = false;}

    osg::ref_ptr<osgAnimation::Vec4LinearSampler> _sampler;
    osg::Timer_t _startTime;
    osg::Timer_t _currentTime;
    bool _playing;
    unsigned int _lastUpdate;
};

// This won't really give good results in any situation, but it does demonstrate
// on possible "fast" usage...
class MakePathTimeCallback: public AnimtkUpdateCallback
{
    osg::ref_ptr<osg::Geode> _geode;
    float _lastAdd;
    float _addSeconds;

public:
    MakePathTimeCallback(osg::Geode* geode):
        _geode(geode),
        _lastAdd(0.0f),
        _addSeconds(0.08f) {
    }

    virtual void operator()(osg::Node* node, osg::NodeVisitor* nv)
    {
        float t = osg::Timer::instance()->delta_s(_startTime, _currentTime);

        if(_lastAdd + _addSeconds <= t && t <= 8.0f)
        {
            osg::Vec3 pos;

            _sampler->getValueAt(t, pos);

            _geode->addDrawable(new osg::ShapeDrawable(new osg::Sphere(pos, 0.5f)));
            _geode->dirtyBound();

            _lastAdd += _addSeconds;
        }

        AnimtkUpdateCallback::operator()(node, nv);
    }
};

// This will give great results if you DO NOT have VSYNC enabled and can generate
// decent FPS.
class MakePathDistanceCallback: public AnimtkUpdateCallback
{
    osg::ref_ptr<osg::Geode> _geode;
    osg::Vec3 _lastAdd;
    float _threshold;
    unsigned int _count;

public:
    MakePathDistanceCallback(osg::Geode* geode):
        _geode(geode),
        _threshold(0.5f),
        _count(0) {}

    virtual void operator()(osg::Node* node, osg::NodeVisitor* nv)
    {
        static bool countReported = false;

        float t = osg::Timer::instance()->delta_s(_startTime, _currentTime);

        osg::Vec3 pos;

        _sampler->getValueAt(t, pos);

        osg::Vec3 distance = _lastAdd - pos;

        if(t <= 8.0f && distance.length() >= _threshold)
        {
            _geode->addDrawable(new osg::ShapeDrawable(new osg::Sphere(pos, 0.25f)));
            _lastAdd = pos;
            _count++;
        }
        else if(t > 8.0f)
        {
            if(!countReported) std::cout << "Created " << _count << " nodes." << std::endl;
            countReported = true;
        }

        AnimtkUpdateCallback::operator()(node, nv);
    }
};

osg::StateSet* setupStateSet()
{
    osg::StateSet* st = new osg::StateSet();

    st->setAttributeAndModes(new osg::Material(), true);
    st->setMode(GL_BLEND, true);

    AnimtkStateSetUpdateCallback* callback = new AnimtkStateSetUpdateCallback();
    osgAnimation::Vec4KeyframeContainer* keys = callback->_sampler->getOrCreateKeyframeContainer();
    keys->push_back(osgAnimation::Vec4Keyframe(0, osg::Vec4(1,0,0,1)));
    keys->push_back(osgAnimation::Vec4Keyframe(2, osg::Vec4(0.,1,0,1)));
    keys->push_back(osgAnimation::Vec4Keyframe(4, osg::Vec4(0,0,1,1)));
    keys->push_back(osgAnimation::Vec4Keyframe(6, osg::Vec4(0,0,1,1)));
    keys->push_back(osgAnimation::Vec4Keyframe(8, osg::Vec4(0,1,0,1)));
    keys->push_back(osgAnimation::Vec4Keyframe(10, osg::Vec4(1,0,0,1)));
    callback->start();
    st->setUpdateCallback(callback);

    return st;
}

osg::MatrixTransform* setupAnimtkNode(osg::Geode* staticGeode)
{
    osg::Vec3 v[5];

    v[0] = osg::Vec3(  0,   0,   0);
    v[1] = osg::Vec3(20, 40, 60);
    v[2] = osg::Vec3(40, 60, 20);
    v[3] = osg::Vec3(60, 20, 40);
    v[4] = osg::Vec3( 0,  0,  0);

    osg::MatrixTransform* node = new osg::MatrixTransform();
    AnimtkUpdateCallback* callback = new MakePathDistanceCallback(staticGeode);
    osgAnimation::Vec3CubicBezierKeyframeContainer* keys = callback->_sampler->getOrCreateKeyframeContainer();

    keys->push_back(osgAnimation::Vec3CubicBezierKeyframe(0, osgAnimation::Vec3CubicBezier(
                                                        v[0],
                                                        v[0] + (v[0] - v[3]),
                                                        v[1] - (v[1] - v[0])
                                                        )));

    keys->push_back(osgAnimation::Vec3CubicBezierKeyframe(2, osgAnimation::Vec3CubicBezier(
                                                        v[1],
                                                        v[1] + (v[1] - v[0]),
                                                        v[2] - (v[2] - v[1])
                                                        )));

    keys->push_back(osgAnimation::Vec3CubicBezierKeyframe(4, osgAnimation::Vec3CubicBezier(
                                                        v[2],
                                                        v[2] + (v[2] - v[1]),
                                                        v[3] - (v[3] - v[2])
                                                        )));

    keys->push_back(osgAnimation::Vec3CubicBezierKeyframe(6, osgAnimation::Vec3CubicBezier(
                                                        v[3],
                                                        v[3] + (v[3] - v[2]),
                                                        v[4] - (v[4] - v[3])
                                                        )));

    keys->push_back(osgAnimation::Vec3CubicBezierKeyframe(8, osgAnimation::Vec3CubicBezier(
                                                        v[4],
                                                        v[4] + (v[4] - v[3]),
                                                        v[0] - (v[0] - v[4])
                                                        )));

    callback->start();
    node->setUpdateCallback(callback);

    osg::Geode* geode = new osg::Geode();

    geode->setStateSet(setupStateSet());
    geode->addDrawable(new osg::ShapeDrawable(new osg::Sphere(osg::Vec3(0.0f, 0.0f, 0.0f), 2)));

    node->addChild(geode);

    return node;
}

int main(int argc, char** argv)
{
    osg::ArgumentParser arguments(&argc, argv);
    osgViewer::Viewer viewer(arguments);

    osgGA::TrackballManipulator* tbm = new osgGA::TrackballManipulator();

    viewer.setCameraManipulator(tbm);

    viewer.addEventHandler(new osgViewer::StatsHandler());
    viewer.addEventHandler(new osgViewer::WindowSizeHandler());

    osg::Group* root = new osg::Group();
    osg::Geode* geode = new osg::Geode();

    geode->setStateSet(setupStateSet());

    root->setInitialBound(osg::BoundingSphere(osg::Vec3(10,0,20), 50));
    root->addChild(setupAnimtkNode(geode));
    root->addChild(geode);

    viewer.setSceneData(root);

    // tbm->setDistance(150);

    return viewer.run();
}
<commit_msg>Fixed -Wextra warnings<commit_after>/*  -*-c++-*-
 *  Copyright (C) 2008 Cedric Pinson <mornifle@plopbyte.net>
 *
 * This library is open source and may be redistributed and/or modified under
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * OpenSceneGraph Public License for more details.
 *
 *  Authors:
 *   Jeremy Moles  <jeremy@emperorlinux.com>
 *   Cedric Pinson <mornifle@plopbyte.net>
*/

#include <iostream>
#include <osg/io_utils>
#include <osg/Geometry>
#include <osg/Shape>
#include <osg/ShapeDrawable>
#include <osg/Material>
#include <osg/MatrixTransform>
#include <osgViewer/Viewer>
#include <osgViewer/ViewerEventHandlers>
#include <osgGA/TrackballManipulator>
#include <osgAnimation/Sampler>

  class AnimtkUpdateCallback : public osg::NodeCallback
  {
  public:
      META_Object(osgAnimation, AnimtkUpdateCallback);

      AnimtkUpdateCallback()
      {
          _sampler = new osgAnimation::Vec3CubicBezierSampler;
          _playing = false;
          _lastUpdate = 0;
      }
      AnimtkUpdateCallback(const AnimtkUpdateCallback& val, const osg::CopyOp& copyop = osg::CopyOp::SHALLOW_COPY):
          osg::Object(val, copyop),
          osg::Callback(val, copyop),
          osg::NodeCallback(val, copyop),
          _sampler(val._sampler),
          _startTime(val._startTime),
          _currentTime(val._currentTime),
          _playing(val._playing),
          _lastUpdate(val._lastUpdate)
      {
      }

      /** Callback method called by the NodeVisitor when visiting a node.*/
      virtual void operator()(osg::Node* node, osg::NodeVisitor* nv)
      {
          if (nv->getVisitorType() == osg::NodeVisitor::UPDATE_VISITOR &&
              nv->getFrameStamp() &&
              nv->getFrameStamp()->getFrameNumber() != _lastUpdate)
          {

              _lastUpdate = nv->getFrameStamp()->getFrameNumber();
              _currentTime = osg::Timer::instance()->tick();

              if (_playing && _sampler.get() && _sampler->getKeyframeContainer())
              {
                  osg::MatrixTransform* transform = dynamic_cast<osg::MatrixTransform*>(node);
                  if (transform) {
                      osg::Vec3 result;
                      float t = osg::Timer::instance()->delta_s(_startTime, _currentTime);
                      float duration = _sampler->getEndTime() - _sampler->getStartTime();
                      t = fmod(t, duration);
                      t += _sampler->getStartTime();
                      _sampler->getValueAt(t, result);
                      transform->setMatrix(osg::Matrix::translate(result));
                  }
              }
          }
          // note, callback is responsible for scenegraph traversal so
          // they must call traverse(node,nv) to ensure that the
          // scene graph subtree (and associated callbacks) are traversed.
          traverse(node,nv);
      }

      void start() { _startTime = osg::Timer::instance()->tick(); _currentTime = _startTime; _playing = true;}
      void stop() { _currentTime = _startTime; _playing = false;}

      osg::ref_ptr<osgAnimation::Vec3CubicBezierSampler> _sampler;
      osg::Timer_t _startTime;
      osg::Timer_t _currentTime;
      bool _playing;
      unsigned int _lastUpdate;
  };

class AnimtkStateSetUpdateCallback : public osg::StateSet::Callback
{
public:
    META_Object(osgAnimation, AnimtkStateSetUpdateCallback);

    AnimtkStateSetUpdateCallback()
    {
        _sampler = new osgAnimation::Vec4LinearSampler;
        _playing = false;
        _lastUpdate = 0;
    }

    AnimtkStateSetUpdateCallback(const AnimtkStateSetUpdateCallback& val, const osg::CopyOp& copyop = osg::CopyOp::SHALLOW_COPY):
        osg::Object(val, copyop),
        osg::Callback(val, copyop),
        osg::StateSet::Callback(val, copyop),
        _sampler(val._sampler),
        _startTime(val._startTime),
        _currentTime(val._currentTime),
        _playing(val._playing),
        _lastUpdate(val._lastUpdate)
    {
    }

    /** Callback method called by the NodeVisitor when visiting a node.*/
    virtual void operator()(osg::StateSet* state, osg::NodeVisitor* nv)
    {
        if (state &&
            nv->getVisitorType() == osg::NodeVisitor::UPDATE_VISITOR &&
            nv->getFrameStamp() &&
            nv->getFrameStamp()->getFrameNumber() != _lastUpdate) {

            _lastUpdate = nv->getFrameStamp()->getFrameNumber();
            _currentTime = osg::Timer::instance()->tick();

            if (_playing && _sampler.get() && _sampler->getKeyframeContainer())
            {
                osg::Material* material = dynamic_cast<osg::Material*>(state->getAttribute(osg::StateAttribute::MATERIAL));
                if (material)
                {
                    osg::Vec4 result;
                    float t = osg::Timer::instance()->delta_s(_startTime, _currentTime);
                    float duration = _sampler->getEndTime() - _sampler->getStartTime();
                    t = fmod(t, duration);
                    t += _sampler->getStartTime();
                    _sampler->getValueAt(t, result);
                    material->setDiffuse(osg::Material::FRONT_AND_BACK, result);
                }
            }
        }
    }

    void start() { _startTime = osg::Timer::instance()->tick(); _currentTime = _startTime; _playing = true;}
    void stop() { _currentTime = _startTime; _playing = false;}

    osg::ref_ptr<osgAnimation::Vec4LinearSampler> _sampler;
    osg::Timer_t _startTime;
    osg::Timer_t _currentTime;
    bool _playing;
    unsigned int _lastUpdate;
};

// This won't really give good results in any situation, but it does demonstrate
// on possible "fast" usage...
class MakePathTimeCallback: public AnimtkUpdateCallback
{
    osg::ref_ptr<osg::Geode> _geode;
    float _lastAdd;
    float _addSeconds;

public:
    MakePathTimeCallback(osg::Geode* geode):
        _geode(geode),
        _lastAdd(0.0f),
        _addSeconds(0.08f) {
    }

    virtual void operator()(osg::Node* node, osg::NodeVisitor* nv)
    {
        float t = osg::Timer::instance()->delta_s(_startTime, _currentTime);

        if(_lastAdd + _addSeconds <= t && t <= 8.0f)
        {
            osg::Vec3 pos;

            _sampler->getValueAt(t, pos);

            _geode->addDrawable(new osg::ShapeDrawable(new osg::Sphere(pos, 0.5f)));
            _geode->dirtyBound();

            _lastAdd += _addSeconds;
        }

        AnimtkUpdateCallback::operator()(node, nv);
    }
};

// This will give great results if you DO NOT have VSYNC enabled and can generate
// decent FPS.
class MakePathDistanceCallback: public AnimtkUpdateCallback
{
    osg::ref_ptr<osg::Geode> _geode;
    osg::Vec3 _lastAdd;
    float _threshold;
    unsigned int _count;

public:
    MakePathDistanceCallback(osg::Geode* geode):
        _geode(geode),
        _threshold(0.5f),
        _count(0) {}

    virtual void operator()(osg::Node* node, osg::NodeVisitor* nv)
    {
        static bool countReported = false;

        float t = osg::Timer::instance()->delta_s(_startTime, _currentTime);

        osg::Vec3 pos;

        _sampler->getValueAt(t, pos);

        osg::Vec3 distance = _lastAdd - pos;

        if(t <= 8.0f && distance.length() >= _threshold)
        {
            _geode->addDrawable(new osg::ShapeDrawable(new osg::Sphere(pos, 0.25f)));
            _lastAdd = pos;
            _count++;
        }
        else if(t > 8.0f)
        {
            if(!countReported) std::cout << "Created " << _count << " nodes." << std::endl;
            countReported = true;
        }

        AnimtkUpdateCallback::operator()(node, nv);
    }
};

osg::StateSet* setupStateSet()
{
    osg::StateSet* st = new osg::StateSet();

    st->setAttributeAndModes(new osg::Material(), true);
    st->setMode(GL_BLEND, true);

    AnimtkStateSetUpdateCallback* callback = new AnimtkStateSetUpdateCallback();
    osgAnimation::Vec4KeyframeContainer* keys = callback->_sampler->getOrCreateKeyframeContainer();
    keys->push_back(osgAnimation::Vec4Keyframe(0, osg::Vec4(1,0,0,1)));
    keys->push_back(osgAnimation::Vec4Keyframe(2, osg::Vec4(0.,1,0,1)));
    keys->push_back(osgAnimation::Vec4Keyframe(4, osg::Vec4(0,0,1,1)));
    keys->push_back(osgAnimation::Vec4Keyframe(6, osg::Vec4(0,0,1,1)));
    keys->push_back(osgAnimation::Vec4Keyframe(8, osg::Vec4(0,1,0,1)));
    keys->push_back(osgAnimation::Vec4Keyframe(10, osg::Vec4(1,0,0,1)));
    callback->start();
    st->setUpdateCallback(callback);

    return st;
}

osg::MatrixTransform* setupAnimtkNode(osg::Geode* staticGeode)
{
    osg::Vec3 v[5];

    v[0] = osg::Vec3(  0,   0,   0);
    v[1] = osg::Vec3(20, 40, 60);
    v[2] = osg::Vec3(40, 60, 20);
    v[3] = osg::Vec3(60, 20, 40);
    v[4] = osg::Vec3( 0,  0,  0);

    osg::MatrixTransform* node = new osg::MatrixTransform();
    AnimtkUpdateCallback* callback = new MakePathDistanceCallback(staticGeode);
    osgAnimation::Vec3CubicBezierKeyframeContainer* keys = callback->_sampler->getOrCreateKeyframeContainer();

    keys->push_back(osgAnimation::Vec3CubicBezierKeyframe(0, osgAnimation::Vec3CubicBezier(
                                                        v[0],
                                                        v[0] + (v[0] - v[3]),
                                                        v[1] - (v[1] - v[0])
                                                        )));

    keys->push_back(osgAnimation::Vec3CubicBezierKeyframe(2, osgAnimation::Vec3CubicBezier(
                                                        v[1],
                                                        v[1] + (v[1] - v[0]),
                                                        v[2] - (v[2] - v[1])
                                                        )));

    keys->push_back(osgAnimation::Vec3CubicBezierKeyframe(4, osgAnimation::Vec3CubicBezier(
                                                        v[2],
                                                        v[2] + (v[2] - v[1]),
                                                        v[3] - (v[3] - v[2])
                                                        )));

    keys->push_back(osgAnimation::Vec3CubicBezierKeyframe(6, osgAnimation::Vec3CubicBezier(
                                                        v[3],
                                                        v[3] + (v[3] - v[2]),
                                                        v[4] - (v[4] - v[3])
                                                        )));

    keys->push_back(osgAnimation::Vec3CubicBezierKeyframe(8, osgAnimation::Vec3CubicBezier(
                                                        v[4],
                                                        v[4] + (v[4] - v[3]),
                                                        v[0] - (v[0] - v[4])
                                                        )));

    callback->start();
    node->setUpdateCallback(callback);

    osg::Geode* geode = new osg::Geode();

    geode->setStateSet(setupStateSet());
    geode->addDrawable(new osg::ShapeDrawable(new osg::Sphere(osg::Vec3(0.0f, 0.0f, 0.0f), 2)));

    node->addChild(geode);

    return node;
}

int main(int argc, char** argv)
{
    osg::ArgumentParser arguments(&argc, argv);
    osgViewer::Viewer viewer(arguments);

    osgGA::TrackballManipulator* tbm = new osgGA::TrackballManipulator();

    viewer.setCameraManipulator(tbm);

    viewer.addEventHandler(new osgViewer::StatsHandler());
    viewer.addEventHandler(new osgViewer::WindowSizeHandler());

    osg::Group* root = new osg::Group();
    osg::Geode* geode = new osg::Geode();

    geode->setStateSet(setupStateSet());

    root->setInitialBound(osg::BoundingSphere(osg::Vec3(10,0,20), 50));
    root->addChild(setupAnimtkNode(geode));
    root->addChild(geode);

    viewer.setSceneData(root);

    // tbm->setDistance(150);

    return viewer.run();
}
<|endoftext|>


<commit_before>#ifndef UNIT_HPP
#define UNIT_HPP

#include "internal/common.hpp"

#include <QObject>

namespace cutehmi {
namespace gui {

class CUTEHMI_GUI_API Units:
	public QObject
{
		Q_OBJECT

	public:
		static constexpr qreal INITIAL_QUADRAT = 40.0;

		static constexpr bool INITIAL_ROUND_STROKE_WIDTH = true;

		static constexpr qreal STANDARD_STROKE_WIDTH_QUADRAT_RATIO = 40.0;

		Q_PROPERTY(qreal quadrat READ quadrat WRITE setQuadrat NOTIFY quadratChanged)

		Q_PROPERTY(qreal strokeWidth READ strokeWidth WRITE setStrokeWidth NOTIFY strokeWidthChanged)

		Q_PROPERTY(bool roundStrokeWidth READ roundStrokeWidth WRITE setRoundStrokeWidth NOTIFY roundStrokeWidthChanged)

		Units(QObject * parent = nullptr);

		qreal quadrat() const;

		void setQuadrat(qreal quadrat);

		qreal strokeWidth() const;

		void setStrokeWidth(qreal strokeWidth);

		bool roundStrokeWidth() const;

		void setRoundStrokeWidth(bool roundStrokeWidth);

	signals:
		void quadratChanged();

		void strokeWidthChanged();

		void roundStrokeWidthChanged();

	private slots:
		void strokeWidthBinding();

	private:
		void setStrokeWidth(qreal strokeWidth, bool breakBinding);

		struct Members {
			qreal quadrat;
			qreal strokeWidth;
			bool roundStrokeWidth;
			QMetaObject::Connection strokeWidthBindingConnection;
		};

		MPtr<Members> m;
};

}
}

#endif // UNIT_HPP
<commit_msg>Change initial quadrat size.<commit_after>#ifndef UNIT_HPP
#define UNIT_HPP

#include "internal/common.hpp"

#include <QObject>

namespace cutehmi {
namespace gui {

class CUTEHMI_GUI_API Units:
	public QObject
{
		Q_OBJECT

	public:
		static constexpr qreal INITIAL_QUADRAT = 80.0;

		static constexpr bool INITIAL_ROUND_STROKE_WIDTH = true;

		static constexpr qreal STANDARD_STROKE_WIDTH_QUADRAT_RATIO = 40.0;

		Q_PROPERTY(qreal quadrat READ quadrat WRITE setQuadrat NOTIFY quadratChanged)

		Q_PROPERTY(qreal strokeWidth READ strokeWidth WRITE setStrokeWidth NOTIFY strokeWidthChanged)

		Q_PROPERTY(bool roundStrokeWidth READ roundStrokeWidth WRITE setRoundStrokeWidth NOTIFY roundStrokeWidthChanged)

		Units(QObject * parent = nullptr);

		qreal quadrat() const;

		void setQuadrat(qreal quadrat);

		qreal strokeWidth() const;

		void setStrokeWidth(qreal strokeWidth);

		bool roundStrokeWidth() const;

		void setRoundStrokeWidth(bool roundStrokeWidth);

	signals:
		void quadratChanged();

		void strokeWidthChanged();

		void roundStrokeWidthChanged();

	private slots:
		void strokeWidthBinding();

	private:
		void setStrokeWidth(qreal strokeWidth, bool breakBinding);

		struct Members {
			qreal quadrat;
			qreal strokeWidth;
			bool roundStrokeWidth;
			QMetaObject::Connection strokeWidthBindingConnection;
		};

		MPtr<Members> m;
};

}
}

#endif // UNIT_HPP
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  $RCSfile: loadlisteneradapter.hxx,v $
 *
 *  $Revision: 1.1 $
 *
 *  last change: $Author: fs $ $Date: 2001-10-22 07:32:20 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 by Sun Microsystems, Inc.
 *  901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License version 2.1, as published by the Free Software Foundation.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *  MA  02111-1307  USA
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (the "License"); You may not use this file
 *  except in compliance with the License. You may obtain a copy of the
 *  License at http://www.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc..
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/

#ifndef EXTENSIONS_BIB_LOADLISTENERADAPTER_HXX
#define EXTENSIONS_BIB_LOADLISTENERADAPTER_HXX

#ifndef _OSL_MUTEX_HXX_
#include <osl/mutex.hxx>
#endif

#ifndef _COM_SUN_STAR_LANG_XCOMPONENT_HPP_
#include <com/sun/star/lang/XComponent.hpp>
#endif
#ifndef _CPPUHELPER_IMPLBASE1_HXX_
#include <cppuhelper/implbase1.hxx>
#endif

#ifndef _COM_SUN_STAR_FORM_XLOADABLE_HPP_
#include <com/sun/star/form/XLoadable.hpp>
#endif

//.........................................................................
namespace bib
{
//.........................................................................

    //=====================================================================
    //= OComponentListener
    //=====================================================================
    class OComponentListener
    {
        friend class OComponentAdapterBase;

    private:
        OComponentAdapterBase*  m_pAdapter;
        ::osl::Mutex&           m_rMutex;

    protected:
        OComponentListener( ::osl::Mutex& _rMutex )
            :m_rMutex( _rMutex )
            ,m_pAdapter( NULL )
        {
        }

        virtual ~OComponentListener();

        // XEventListener equivalents
        virtual void _disposing( const ::com::sun::star::lang::EventObject& _rSource ) throw( ::com::sun::star::uno::RuntimeException );

    protected:
        void setAdapter( OComponentAdapterBase* _pAdapter );
    };

    //=====================================================================
    //= OComponentAdapterBase
    //=====================================================================
    class OComponentAdapterBase
    {
        friend class OComponentListener;

    private:
        ::com::sun::star::uno::Reference< ::com::sun::star::lang::XComponent >
                                            m_xComponent;
        OComponentListener*                 m_pListener;
        sal_Int32                           m_nLockCount;
        sal_Bool                            m_bListening    : 1;
        sal_Bool                            m_bAutoRelease  : 1;

        virtual ~OComponentAdapterBase();

        // impl method for dispose - virtual, 'cause you at least need to remove the listener from the broadcaster
        virtual void disposing() = 0;

    protected:
        // attribute access for derivees
        const ::com::sun::star::uno::Reference< ::com::sun::star::lang::XComponent >&
                                getComponent() const    { return m_xComponent; }
        OComponentListener*     getListener()           { return m_pListener; }

        // to be called by derivees which started listening at the component
        virtual void    startComponentListening() = 0;

    public:
        OComponentAdapterBase(
            const ::com::sun::star::uno::Reference< ::com::sun::star::lang::XComponent >& _rxComp,
            sal_Bool _bAutoRelease = sal_True
        );

        // late construction
        // can be called from within you ctor, to have you're object fully initialized at the moment of
        // the call (which would not be the case when calling this ctor)
        void Init( OComponentListener* _pListener );

    // base for ref-counting, implemented by OComponentAdapter
        virtual void SAL_CALL acquire(  ) throw () = 0;
        virtual void SAL_CALL release(  ) throw () = 0;

    // helper
        /// incremental lock
        void        lock();
        /// incremental unlock
        void        unlock();
        /// get the lock count
        sal_Int32   locked() const { return m_nLockCount; }

        /// dispose the object - stop listening and such
        void dispose();

    protected:
    // XEventListener
        virtual void SAL_CALL disposing( const  ::com::sun::star::lang::EventObject& Source ) throw( ::com::sun::star::uno::RuntimeException);
    };

    //=====================================================================
    //= OLoadListener
    //=====================================================================
    class OLoadListener : public OComponentListener
    {
        friend class OLoadListenerAdapter;

    protected:
        OLoadListener( ::osl::Mutex& _rMutex ) : OComponentListener( _rMutex ) { }

    // XLoadListener equivalents
        virtual void _loaded( const ::com::sun::star::lang::EventObject& aEvent ) = 0;
        virtual void _unloading( const ::com::sun::star::lang::EventObject& aEvent ) = 0;
        virtual void _unloaded( const ::com::sun::star::lang::EventObject& aEvent ) = 0;
        virtual void _reloading( const ::com::sun::star::lang::EventObject& aEvent ) = 0;
        virtual void _reloaded( const ::com::sun::star::lang::EventObject& aEvent ) = 0;
    };

    //=====================================================================
    //= OLoadListenerAdapter
    //=====================================================================
    typedef ::cppu::WeakImplHelper1< ::com::sun::star::form::XLoadListener >    OLoadListenerAdapter_Base;
    class OLoadListenerAdapter
        :public OLoadListenerAdapter_Base
        ,public OComponentAdapterBase
    {
    protected:
        OLoadListener*      getLoadListener( )  { return static_cast< OLoadListener* >( getListener() ); }

    protected:
        virtual void    disposing();
        virtual void    startComponentListening();

    public:
        OLoadListenerAdapter(
            const ::com::sun::star::uno::Reference< ::com::sun::star::form::XLoadable >& _rxLoadable,
            sal_Bool _bAutoRelease = sal_True
        );

        virtual void SAL_CALL acquire(  ) throw ();
        virtual void SAL_CALL release(  ) throw ();

    protected:
    // XEventListener
        virtual void SAL_CALL disposing( const  ::com::sun::star::lang::EventObject& _rSource ) throw( ::com::sun::star::uno::RuntimeException);

    // XLoadListener
        virtual void SAL_CALL loaded( const ::com::sun::star::lang::EventObject& aEvent ) throw (::com::sun::star::uno::RuntimeException);
        virtual void SAL_CALL unloading( const ::com::sun::star::lang::EventObject& aEvent ) throw (::com::sun::star::uno::RuntimeException);
        virtual void SAL_CALL unloaded( const ::com::sun::star::lang::EventObject& aEvent ) throw (::com::sun::star::uno::RuntimeException);
        virtual void SAL_CALL reloading( const ::com::sun::star::lang::EventObject& aEvent ) throw (::com::sun::star::uno::RuntimeException);
        virtual void SAL_CALL reloaded( const ::com::sun::star::lang::EventObject& aEvent ) throw (::com::sun::star::uno::RuntimeException);
    };

//.........................................................................
}   // namespace bib
//.........................................................................

#endif // EXTENSIONS_BIB_LOADLISTENERADAPTER_HXX

/*************************************************************************
 * history:
 *  $Log: not supported by cvs2svn $
 *
 *  Revision 1.0 17.10.01 12:15:24  fs
 ************************************************************************/

<commit_msg>#65293# dtor of OComponentAdapterBase protected instead of private<commit_after>/*************************************************************************
 *
 *  $RCSfile: loadlisteneradapter.hxx,v $
 *
 *  $Revision: 1.2 $
 *
 *  last change: $Author: fs $ $Date: 2001-10-23 13:20:53 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 by Sun Microsystems, Inc.
 *  901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License version 2.1, as published by the Free Software Foundation.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *  MA  02111-1307  USA
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (the "License"); You may not use this file
 *  except in compliance with the License. You may obtain a copy of the
 *  License at http://www.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc..
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/

#ifndef EXTENSIONS_BIB_LOADLISTENERADAPTER_HXX
#define EXTENSIONS_BIB_LOADLISTENERADAPTER_HXX

#ifndef _OSL_MUTEX_HXX_
#include <osl/mutex.hxx>
#endif

#ifndef _COM_SUN_STAR_LANG_XCOMPONENT_HPP_
#include <com/sun/star/lang/XComponent.hpp>
#endif
#ifndef _CPPUHELPER_IMPLBASE1_HXX_
#include <cppuhelper/implbase1.hxx>
#endif

#ifndef _COM_SUN_STAR_FORM_XLOADABLE_HPP_
#include <com/sun/star/form/XLoadable.hpp>
#endif

//.........................................................................
namespace bib
{
//.........................................................................

    //=====================================================================
    //= OComponentListener
    //=====================================================================
    class OComponentListener
    {
        friend class OComponentAdapterBase;

    private:
        OComponentAdapterBase*  m_pAdapter;
        ::osl::Mutex&           m_rMutex;

    protected:
        OComponentListener( ::osl::Mutex& _rMutex )
            :m_rMutex( _rMutex )
            ,m_pAdapter( NULL )
        {
        }

        virtual ~OComponentListener();

        // XEventListener equivalents
        virtual void _disposing( const ::com::sun::star::lang::EventObject& _rSource ) throw( ::com::sun::star::uno::RuntimeException );

    protected:
        void setAdapter( OComponentAdapterBase* _pAdapter );
    };

    //=====================================================================
    //= OComponentAdapterBase
    //=====================================================================
    class OComponentAdapterBase
    {
        friend class OComponentListener;

    private:
        ::com::sun::star::uno::Reference< ::com::sun::star::lang::XComponent >
                                            m_xComponent;
        OComponentListener*                 m_pListener;
        sal_Int32                           m_nLockCount;
        sal_Bool                            m_bListening    : 1;
        sal_Bool                            m_bAutoRelease  : 1;

        // impl method for dispose - virtual, 'cause you at least need to remove the listener from the broadcaster
        virtual void disposing() = 0;

    protected:
        // attribute access for derivees
        const ::com::sun::star::uno::Reference< ::com::sun::star::lang::XComponent >&
                                getComponent() const    { return m_xComponent; }
        OComponentListener*     getListener()           { return m_pListener; }

        // to be called by derivees which started listening at the component
        virtual void    startComponentListening() = 0;

        virtual ~OComponentAdapterBase();

    public:
        OComponentAdapterBase(
            const ::com::sun::star::uno::Reference< ::com::sun::star::lang::XComponent >& _rxComp,
            sal_Bool _bAutoRelease = sal_True
        );

        // late construction
        // can be called from within you ctor, to have you're object fully initialized at the moment of
        // the call (which would not be the case when calling this ctor)
        void Init( OComponentListener* _pListener );

    // base for ref-counting, implemented by OComponentAdapter
        virtual void SAL_CALL acquire(  ) throw () = 0;
        virtual void SAL_CALL release(  ) throw () = 0;

    // helper
        /// incremental lock
        void        lock();
        /// incremental unlock
        void        unlock();
        /// get the lock count
        sal_Int32   locked() const { return m_nLockCount; }

        /// dispose the object - stop listening and such
        void dispose();

    protected:
    // XEventListener
        virtual void SAL_CALL disposing( const  ::com::sun::star::lang::EventObject& Source ) throw( ::com::sun::star::uno::RuntimeException);
    };

    //=====================================================================
    //= OLoadListener
    //=====================================================================
    class OLoadListener : public OComponentListener
    {
        friend class OLoadListenerAdapter;

    protected:
        OLoadListener( ::osl::Mutex& _rMutex ) : OComponentListener( _rMutex ) { }

    // XLoadListener equivalents
        virtual void _loaded( const ::com::sun::star::lang::EventObject& aEvent ) = 0;
        virtual void _unloading( const ::com::sun::star::lang::EventObject& aEvent ) = 0;
        virtual void _unloaded( const ::com::sun::star::lang::EventObject& aEvent ) = 0;
        virtual void _reloading( const ::com::sun::star::lang::EventObject& aEvent ) = 0;
        virtual void _reloaded( const ::com::sun::star::lang::EventObject& aEvent ) = 0;
    };

    //=====================================================================
    //= OLoadListenerAdapter
    //=====================================================================
    typedef ::cppu::WeakImplHelper1< ::com::sun::star::form::XLoadListener >    OLoadListenerAdapter_Base;
    class OLoadListenerAdapter
        :public OLoadListenerAdapter_Base
        ,public OComponentAdapterBase
    {
    protected:
        OLoadListener*      getLoadListener( )  { return static_cast< OLoadListener* >( getListener() ); }

    protected:
        virtual void    disposing();
        virtual void    startComponentListening();

    public:
        OLoadListenerAdapter(
            const ::com::sun::star::uno::Reference< ::com::sun::star::form::XLoadable >& _rxLoadable,
            sal_Bool _bAutoRelease = sal_True
        );

        virtual void SAL_CALL acquire(  ) throw ();
        virtual void SAL_CALL release(  ) throw ();

    protected:
    // XEventListener
        virtual void SAL_CALL disposing( const  ::com::sun::star::lang::EventObject& _rSource ) throw( ::com::sun::star::uno::RuntimeException);

    // XLoadListener
        virtual void SAL_CALL loaded( const ::com::sun::star::lang::EventObject& aEvent ) throw (::com::sun::star::uno::RuntimeException);
        virtual void SAL_CALL unloading( const ::com::sun::star::lang::EventObject& aEvent ) throw (::com::sun::star::uno::RuntimeException);
        virtual void SAL_CALL unloaded( const ::com::sun::star::lang::EventObject& aEvent ) throw (::com::sun::star::uno::RuntimeException);
        virtual void SAL_CALL reloading( const ::com::sun::star::lang::EventObject& aEvent ) throw (::com::sun::star::uno::RuntimeException);
        virtual void SAL_CALL reloaded( const ::com::sun::star::lang::EventObject& aEvent ) throw (::com::sun::star::uno::RuntimeException);
    };

//.........................................................................
}   // namespace bib
//.........................................................................

#endif // EXTENSIONS_BIB_LOADLISTENERADAPTER_HXX

/*************************************************************************
 * history:
 *  $Log: not supported by cvs2svn $
 *  Revision 1.1  2001/10/22 07:32:20  fs
 *  initial checkin - UNO-to-nonUNO bridge for (load)listeners
 *
 *
 *  Revision 1.0 17.10.01 12:15:24  fs
 ************************************************************************/

<|endoftext|>


<commit_before>#ifndef HMLIB_ODEINT_STATEBREAKOBSERVER_INC
#define HMLIB_ODEINT_STATEBREAKOBSERVER_INC 100
#
#include <array>
#include "../utility.hpp"
namespace hmLib{
	namespace odeint{
		template<typename state_range_accessor_>
		struct equal_state_breaker {
		public:
			using state_range_accessor = state_range_accessor_;
		private:
			unsigned int Cnt;
			unsigned int Interval;
			state_range_accessor Accessor;
			double Tolerance;
		public:
			equal_state_breaker(unsigned int Interval_, state_range_accessor Accessor_, double Tolerance_) :Cnt(0), Interval(Interval_), Accessor(Accessor_), Tolerance(Tolerance_){}
			template<typename state_type, typename time_type>
			bool operator()(const state_type& x, time_type t) {
				if (++Cnt < Interval)return false;
				Cnt = 0;

				auto Range = Accessor();
				for (const auto& sx: Range) {
					if (hmLib::odeint::distance_norm_inf(x, sx) < Tolerance) {
						return true;
					}
				}

				return false;
			}
		};
		template<typename state_range_accessor_>
		struct cross_state_breaker {
		public:
			using state_range_accessor = state_range_accessor_;
		private:
			unsigned int Cnt;
			unsigned int Interval;
			state_range_accessor Accessor;
		private:
			template<typename state>
			bool is_cross_segment(const state& a1, const state& a2, const state& b1, const state& b2) {
				if (a1[0] >= a2[0]) {
					if ((a1[0] < b1[0] && a1[0] < b2[0]) || (a2[0] > b1[0] && a2[0] > b2[0])) {
						return false;
					}
				}
				else {
					if ((a2[0] < b1[0] && a2[0] < b2[0]) || (a1[0] > b1[0] && a1[0] > b2[0])) {
						return false;
					}
				}
				if (a1[1] >= a2[1]) {
					if ((a1[1] < b1[1] && a1[1] < b2[1]) || (a2[1] > b1[1] && a2[1] > b2[1])) {
						return false;
					}
				}
				else {
					if ((a2[1] < b1[1] && a2[1] < b2[1]) || (a1[1] > b1[1] && a1[1] > b2[1])) {
						return false;
					}
				}

				if (((a1[0] - a2[0]) * (b1[1] - a1[1]) + (a1[1] - a2[1]) * (a1[0] - b1[0])) *
					((a1[0] - a2[0]) * (b2[1] - a1[1]) + (a1[1] - a2[1]) * (a1[0] - b2[0])) > 0) {
					return false;
				}
				if (((b1[0] - b2[0]) * (a1[1] - b1[1]) + (b1[1] - b2[1]) * (b1[0] - a1[0])) *
					((b1[0] - b2[0]) * (a2[1] - b1[1]) + (b1[1] - b2[1]) * (b1[0] - a2[0])) > 0) {
					return false;
				}
				return true;
			}
		public:
			cross_state_breaker(unsigned int Interval_, state_range_accessor Accessor_) :Cnt(0), Interval(Interval_), Accessor(Accessor_){}
			template<typename state_type, typename time_type>
			bool operator()(const state_type& x, time_type t) {
				if (++Cnt < Interval)return false;
				Cnt = 0;

				auto Range = Accessor();
				for (const auto& sx : Range) {
					if (hmLib::odeint::distance_norm_inf(x, sx) < Tolerance) {
						return true;
					}
				}
				for (; Beg != End; ++Beg) {
					if (is_cross_segment(x, y, p, Beg->second))return true;
					p = Beg->second;
				}
				return false;
			}
		};
	}
}
#
#endif
<commit_msg>Add cross_state_breaker.<commit_after>#ifndef HMLIB_ODEINT_STATEBREAKOBSERVER_INC
#define HMLIB_ODEINT_STATEBREAKOBSERVER_INC 100
#
#include <array>
#include "../utility.hpp"
#include "../../geometry.hpp"
namespace hmLib{
	namespace odeint{
		template<typename state_range_accessor_>
		struct equal_state_breaker {
		public:
			using state_range_accessor = state_range_accessor_;
		private:
			unsigned int Cnt;
			unsigned int Interval;
			state_range_accessor Accessor;
			double Tolerance;
		public:
			equal_state_breaker(unsigned int Interval_, state_range_accessor Accessor_, double Tolerance_) :Cnt(0), Interval(Interval_), Accessor(Accessor_), Tolerance(Tolerance_){}
			template<typename state_type, typename time_type>
			bool operator()(const state_type& x, time_type t) {
				if (++Cnt < Interval)return false;
				Cnt = 0;

				auto Range = Accessor();
				for (const auto& sx: Range) {
					if (hmLib::odeint::distance_norm_inf(x, sx) < Tolerance) {
						return true;
					}
				}

				return false;
			}
		};
		template<typename state_range_accessor_>
		struct cross_state_breaker {
		public:
			using state_range_accessor = state_range_accessor_;
		private:
			unsigned int Cnt;
			unsigned int Interval;
			state_range_accessor Accessor;
		public:
			cross_state_breaker(unsigned int Interval_, state_range_accessor Accessor_) :Cnt(0), Interval(Interval_), Accessor(Accessor_){}
			template<typename state_type, typename time_type>
			bool operator()(const state_type& x, time_type t) {
				if (++Cnt < Interval)return false;
				Cnt = 0;

				auto Range = Accessor();
				auto Last = std::next(std::begin(Range), std::distance(std::begin(), std::end()) - 1);
				auto Prev = std::begin(Range);
				auto s1 = hmLib::plane_geometry::make_semgnet(hmLib::plane_geometry::make_point(x), hmLib::plane_geometry::make_point(*Last));
				for (auto Itr = std::next(Prev); Prev != Last; ++Itr, ++Prev) {
					auto s2 = hmLib::plane_geometry::make_semgnet(hmLib::plane_geometry::make_point(*Itr), hmLib::plane_geometry::make_point(*Prev));

					if(hmLib::plane_geometry::is_cross(s1, s2))return true;
				}
				return false;
			}
		};
	}
}
#
#endif
<|endoftext|>


<commit_before>#include "photoeffects.hpp"

using namespace cv;

int warmify(cv::InputArray src, cv::OutputArray dst, uchar delta)
{
    if (src.type() != CV_8UC3)
    {
        return 1;
    }
    Mat img_src(src.size(), CV_8UC3);
    img_src = src.getMat();
    Mat img_dst(img_src.size(), CV_8UC3);
    Vec3b intensity, intensity_new;
    int r = 0;
    int g = 0;

    for (int i = 0; i < img_src.rows; i++)
    {
        for (int j = 0; j < img_src.cols; j++)
        {
            intensity = img_src.at<Vec3b>(i, j);
            intensity_new.val[0] = intensity.val[0];
            g = intensity.val[1] + delta;
            if (g > 255) g = 255;
            r = intensity.val[2] + delta;
            if (r > 255) r = 255;
            intensity_new.val[1] = (uchar)g;
            intensity_new.val[2] = (uchar)r;
            img_dst.at<Vec3b>(i, j) = intensity_new;
        }
    }
    img_dst.convertTo(dst, img_src.type());
    return 0;
}

<commit_msg>applyed format code of warmify.cpp and used CV_Assert<commit_after>#include "photoeffects.hpp"

using namespace cv;

/*
// Function for applying warmify filter to image
//
// API
// int warmify(cv::InputArray src, cv::OutputArray dst, uchar delta);
// INPUT
// src- source image
// delta - value, increase a warm components of colors which on
// OUTPUT
// dst - the image with warmify filter effect
// RESULT
// Error status
*/

int warmify(cv::InputArray src, cv::OutputArray dst, uchar delta)
{
    CV_Assert(src.type() == CV_8UC3);

    Mat imgSrc(src.size(), CV_8UC3);
    imgSrc = src.getMat();
    Mat imgDst(imgSrc.size(), CV_8UC3);

    Vec3b intensity, intensityNew;
    int r = 0;
    int g = 0;

    for (int i = 0; i < imgSrc.rows; i++)
    {
        for (int j = 0; j < imgSrc.cols; j++)
        {
            intensity = imgSrc.at<Vec3b>(i, j);
            intensityNew.val[0] = intensity.val[0];
            g = intensity.val[1] + delta;
            if (g > 255) g = 255;
            r = intensity.val[2] + delta;
            if (r > 255) r = 255;
            intensityNew.val[1] = (uchar)g;
            intensityNew.val[2] = (uchar)r;
            imgDst.at<Vec3b>(i, j) = intensityNew;
        }
    }
    imgDst.convertTo(dst, CV_8UC3);
    return 0;
}

<|endoftext|>