Tensor HeightField::operator()(math::Vector2f const & p) const
{
	static const int dx = 2, dy = 2;
	
	if (m_image.isNull())
	{
		return Tensor();
	}
	
	QPoint  ip = m_image.toImageCoords(p).toPoint();

	if (! QRect(QPoint(0,0), m_image.size()-QSize(dx,dy)).contains(ip))
	{
		return Tensor();
	}
	
	QRgb pix0 = m_image.pixel(ip);
	float f0 = QColor::fromRgb(pix0).valueF();
	
	QRgb pix1 = m_image.pixel(ip + QPoint(dx,0));
	float f1 = QColor::fromRgb(pix1).valueF();

	QRgb pix2 = m_image.pixel(ip + QPoint(0,dy));
	float f2 = QColor::fromRgb(pix2).valueF();

	qreal dHx = 100.0f * (f1 - f0);
	qreal dHy = 100.0f * (f2 - f0);

	qreal f = atan2f(dHy, dHx) + M_PI/2.0f;
	qreal r = sqrtf(pow2(dHx) + pow2(dHy));

	return Tensor(r, f);
}

    typename traits::unit_vector<Value>::type
    inline unit_vector(std::size_t k, std::size_t n)
    {
	using math::zero; using math::one;
	dense_vector<Value> v(n, zero(Value()));
	v[k]= one(Value());
	return v;
    }

math::Tensor BasisField::operator()(math::Vector2f const & p) const
{
	if (0 == m_singularityType)
	{
		return scale * m_regularValue;
	}
	else
	{
		Q_ASSERT(m_singularityType < NumSingularityTypes);

		Vector2f v = (p - p0).normalized();

		float x = v(0);
		float y = v(1);

		switch (m_singularityType)
		{
			case SingularityType_Center:
				return scale * math::Tensor::fromValues(pow2(y)-pow2(x), -2.0f*x*y);
			case SingularityType_Wedge:
				return scale * math::Tensor::fromValues(x, y);
			case SingularityType_Node:
				return scale * math::Tensor::fromValues(pow2(x)-pow2(y), 2.0f*x*y);
			case SingularityType_Trisector:
				return scale * math::Tensor::fromValues(x, -y);
			case SingularityType_Saddle:
				return scale * math::Tensor::fromValues(pow2(x)-pow2(y), -2.0f*x*y);
			case SingularityType_Focus:
				return scale * math::Tensor::fromValues(pow2(y)-pow2(x),  2.0f*x*y);
			default:
				// not reached, hopefully
				return math::Tensor();
		}
	}
}

dense2D<typename mtl::Collection
<typename mtl::Collection<VVector>::value_type
>::value_type, parameters<> >
inline orthogonalize_factors(VVector& v, tag::vector)
{
    using ::mtl::two_norm;
    using math::zero;
    using mtl::size1D;
    typedef typename mtl::Collection<VVector>::size_type  Size;
    typedef typename mtl::Collection<VVector>::value_type Vector;
    typedef typename mtl::Collection<Vector>::value_type  Scalar;

    dense2D<Scalar, parameters<> > tau(size1D(v), size1D(v));
    tau= zero(Scalar());

    for (Size j= 0; j < size1D(v); ++j) {
        for (Size i= 0; i < j; ++i) {
            Scalar t= dot(entry1D(v, i), entry1D(v, j)) / tau[i][i];
            tau[i][j]= t;
            entry1D(v, j)-= t * entry1D(v, i);
        }
        tau[j][j]= dot(entry1D(v, j), entry1D(v, j));
    }
    return tau;
}

Tensor BasisSumField::operator()(Vector2f const & p) const
{
	ElementList::const_iterator it;
	unsigned int i;
	
	unsigned int numElements = m_elements.size();
	
	if (numElements == 0)
	{
		return Tensor();
	}
	
	typedef float real;
	typedef real realv[numElements];
	
	// calculate distances
	//
	realv dists;
	real  distsSum = 0.0f;
	for (i = 0, it = m_elements.begin(); i < numElements; ++i, ++it)
	{
		Element * bf = *it;
		
		real d = (p - bf->p0).norm();
		
		if (d == 0.0) d = 0.001;
		
		dists[i]  = d;
		distsSum += d;
	}
	
	// calculate "nearness" value
	//
	realv nears;
	real  nearsSum = 0.0f;
	for (i = 0; i < numElements; ++i)
	{
		real n = math::pow2(distsSum / dists[i]);
		
		nears[i]  = n;
		nearsSum += n;
	}
	
	// calculate vector sum
	//
	math::Tensor t;
	for (i = 0, it = m_elements.begin(); i < numElements; ++i, ++it)
	{
		Element * bf = *it;
		
		float w = nears[i] / nearsSum;
		
		t += w * rbf(p, bf->p0, decay()) * bf->scale * (*bf)(p);
	}
	
	return t;
}

void _gemm_x8s8s32x_convolution_fwd_t<src_type, dst_type>::pp_ker_t::operator ()
    (dst_data_t *dst, const acc_data_t *acc, const char *bias,
        const float *scales, float nslope, float sum_scale, float signed_scale,
        int g, size_t start, size_t end)
{
    using math::get_bias;

    if (end <= start)
        return;

    if (ker_) {
        // JIT
        ker_args args;
        size_t oc_offset = start % OC_;
        size_t os_offset = start / OC_;
        args.acc = acc + start;
        args.dst = dst + os_offset * dst_os_stride_ + oc_offset;
        args.bias = bias + (g * jcp_.oc + oc_offset) * bias_data_type_size_;
        args.scales = scales + scale_idx_mult_ * (g * jcp_.oc + oc_offset);
        args.nslope = nslope;
        args.sum_scale = sum_scale;
        args.signed_scale = signed_scale;
        args.len = end - start;
        args.oc_offset = oc_offset;
        ker_(&args);
    }
    else {
        // Fallback
        const size_t first_oc = start % OC_;
        const size_t last_oc = (end - 1) % OC_;
        const size_t first_os = start / OC_;
        const size_t last_os = (end - 1) / OC_;
        for (size_t os = first_os; os <= last_os; os++) {
            const size_t start_oc = (os == first_os) ? first_oc : 0;
            const size_t end_oc = (os == last_os) ? last_oc : OC_ - 1;
            for (size_t oc = start_oc; oc <= end_oc; oc++) {
                const size_t acc_off = os * jcp_.oc + oc;
                const size_t dst_off = os * dst_os_stride_ + oc;

                float d = (float)(acc[acc_off]);
                if (jcp_.signed_input)
                    d *= signed_scale;

                if (do_bias_)
                    d += get_bias(bias, g * jcp_.oc + oc,
                        bias_data_type_);

                d *= scales[(g * jcp_.oc + oc) * scale_idx_mult_];
                if (do_sum_)
                    d += sum_scale * dst[dst_off];
                if (do_relu_ && d < 0)
                    d *= nslope;
                dst[dst_off] = qz_a1b0<float, dst_data_t>()(d);
            }
        }
    }
};

    /// Constructor takes matrix reference
    explicit diagonal(const Matrix& A) : inv_diag(num_rows(A))
    {
	mtl::vampir_trace<5050> tracer;
	MTL_THROW_IF(num_rows(A) != num_cols(A), mtl::matrix_not_square());
	using math::reciprocal;

	for (size_type i= 0; i < num_rows(A); ++i)
	    inv_diag[i]= reciprocal(A[i][i]);
    }

    // To prevent that cout << A * B prints the element-wise product, suggestion by Hui Li
    // It is rather inefficient, esp. for multiple products (complexity increases with the number of arguments :-!)
    //    or sparse matrices. 
    // Better compute your product first and print it then when compute time is an issue,
    // this is ONLY for convenience.
    result_value_type
    operator()(std::size_t r, std::size_t c) const
    {
	using math::zero;
	MTL_THROW_IF(num_cols(first) != num_rows(second), incompatible_size());

	result_value_type ref, sum(zero(ref));
	for (std::size_t i= 0; i < num_cols(first); i++)
	    sum+= first(r, i) * second(i, c);
	return sum;
    }

    // Undefined if matrix is not symmetric 
    void factorize(const Matrix& A, mtl::tag::sparse)
    {
        using namespace mtl; using namespace mtl::tag;  using mtl::traits::range_generator;  
	using math::reciprocal; using mtl::matrix::upper;

        typedef typename range_generator<row, U_type>::type       cur_type;    
        typedef typename range_generator<nz, cur_type>::type      icur_type;            

	MTL_THROW_IF(num_rows(A) != num_cols(A), mtl::matrix_not_square());
	U= upper(A); crop(U);
	U_type L(lower(A)); // needed to find non-zeros in column

        typename mtl::traits::col<U_type>::type                   col(U), col_l(L);
        typename mtl::traits::value<U_type>::type                 value(U); 


	cur_type kc= begin<row>(U), kend= end<row>(U);
	for (size_type k= 0; kc != kend; ++kc, ++k) {

	    icur_type ic= begin<nz>(kc), iend= end<nz>(kc);
	    MTL_DEBUG_THROW_IF(col(*ic) != k, mtl::missing_diagonal());

	    // U[k][k]= 1.0 / sqrt(U[k][k]);
	    value_type inv_dia= reciprocal(sqrt(value(*ic)));
	    value(*ic, inv_dia);
	    icur_type jbegin= ++ic;
	    for (; ic != iend; ++ic) {
		// U[k][i] *= U[k][k]
		value_type d= value(*ic) * inv_dia;
		value(*ic, d);
		size_type i= col(*ic);

		// find non-zeros U[j][i] below U[k][i] for j in (k, i]
		// 1. Go to ith row in L (== ith column in U)
		cur_type irow= begin<row>(L); irow+= i;
		// 2. Find nonzeros with col() in (k, i]
		icur_type jc= begin<nz>(irow), jend= end<nz>(irow);
		while (col_l(*jc) <= k) ++jc;
		while (col_l(*--jend) > i);
		++jend; 
		
		for (; jc != jend; ++jc) {
		    size_type j= col_l(*jc);
		    U.lvalue(j, i)-= d * U[k][j];
		}
		// std::cout << "U after eliminating U[" << i << "][" << k << "] =\n" << U;
	    }
	}
    }

typename Collection<Matrix>::value_type
inline trace(const Matrix& matrix)
{
    using math::zero;
    typedef typename Collection<Matrix>::value_type value_type;

    MTL_THROW_IF(num_rows(matrix) != num_cols(matrix), matrix_not_square());

    // If matrix is empty then the result is the identity from the default-constructed value
    if (num_rows(matrix) == 0) {
	value_type ref;
	return zero(ref);
    }

    value_type value= matrix[0][0];
    for (unsigned i= 1; i < num_rows(matrix); i++)
	value+= matrix[i][i];	
    return value;
}

    void factorize(const Matrix& A, L_type& L, U_type& U, boost::mpl::true_)
    {
	using namespace mtl; using namespace mtl::tag;  using mtl::traits::range_generator;  
	using math::reciprocal; 
	MTL_THROW_IF(num_rows(A) != num_cols(A), mtl::matrix_not_square());
	mtl::vampir_trace<5038> tracer;

	typedef typename mtl::Collection<Matrix>::value_type      value_type;
	typedef typename mtl::Collection<Matrix>::size_type       size_type;
	typedef mtl::mat::parameters<mtl::row_major, mtl::index::c_index, mtl::non_fixed::dimensions, false, size_type> para;
	typedef mtl::mat::compressed2D<value_type, para>  LU_type;
	LU_type LU(A);

	typedef typename range_generator<row, LU_type>::type      cur_type;    
	typedef typename range_generator<nz, cur_type>::type      icur_type;            
	typename mtl::traits::col<LU_type>::type                  col(LU);
	typename mtl::traits::value<LU_type>::type                value(LU); 
	mtl::vec::dense_vector<value_type, mtl::vec::parameters<> > inv_dia(num_rows(A));
	cur_type ic= begin<row>(LU), iend= end<row>(LU);
	for (size_type i= 0; ic != iend; ++ic, ++i) {

	    for (icur_type kc= begin<nz>(ic), kend= end<nz>(ic); kc != kend; ++kc) {
		size_type k= col(*kc);
		if (k == i) break;

		value_type aik= value(*kc) * inv_dia[k];
		value(*kc, aik);

		for (icur_type jc= kc + 1; jc != kend; ++jc)
		    value(*jc, value(*jc) - aik * LU[k][col(*jc)]);
		// std::cout << "LU after eliminating A[" << i << "][" << k << "] =\n" << LU;			  
	    }
	    inv_dia[i]= reciprocal(LU[i][i]);
	}
	invert_diagonal(LU); 
	L= strict_lower(LU);
	U= upper(LU);
    }  

 Scalar finiteDifferenceIncrement() const
 {
   using math::sqrt;
   return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
 }

    static inline void init(Value& value)
    {
	using math::zero;
	value= zero(value);
    }

    /// Value of matrix entry
    value_type operator()(size_type r, size_type c) const
    {
	using math::zero;
	utilities::maybe<size_type> o= offset(r, c);
	return o ? data[o.value()] : zero(value_type());
    }

CRay2 CRay2::operator+ (const math::CVector2& Other) const
{
    return CRay2(this->Start + Other, this->End + Other);
}

/**
* @brief Draw a line
*
* Draws a straight line between the two points.
*
* @param x1 the x coordinate of the first point
* @param y2 the y coordinate of the first point
* @param x1 the x coordinate of the second point
* @param y2 the y coordinate of the second point
* @param color the Color to of the line
*/
void CatPictureApp::drawLine(int x1, int y1, int x2, int y2, Color8u* color)
// changed the names of the parametets, thought x1 would make more sense
// to the user than xF, fairly arbitrary though.. might just be personal preference
{
    //Implementation of Bresenham's line algorithm, derived from pseudocode
    //from Wikipedia.


    int dx = intMath->abs(x2 - x1);
    int dy = intMath->abs(y2 - y1);

    int sx = intMath->signum(x2 - x1);
    int sy = intMath->signum(y2 - y1);
    int err = dx - dy;
    int x = x1;
    int y = y1;

    while(true)
    {
        modify(color,x,y);
        if(x == x2 && y == y2)
            break;
        int e2 = 2 * err;
        if(e2 > -dy)
        {
            err -= dy;
            x += sx;
        }
        if(e2 < dx)
        {
            err += dx;
            y += sy;
        }
    }
}

/**
* Draws a straight line between the two points.
* Params:
	xI = Initial X
	yI = Initial Y
	xF = Final X
	yF = Final Y
	color = Color to draw.
*/
void CatPictureApp::drawLine(int xI, int yI, int xF, int yF, Color8u* color)
{
	//Implementation of Bresenham's line algorithm, derived from pseudocode
	//from Wikipedia.

	
	int dx = intMath->abs(xF - xI);
	int dy = intMath->abs(yF - yI);

	int sx = intMath->signum(xF - xI);
	int sy = intMath->signum(yF - yI);
	int err = dx - dy;
	int x = xI;
	int y = yI;

	while(true)
	{
		modify(color,x,y);
		if(x == xF && y == yF)
			break;
		int e2 = 2 * err;
		if(e2 > -dy)
		{
			err -= dy;
			x += sx;
		}
		if(e2 < dx)
		{
			err += dx;
			y += sy;
		}
	}
}

 Scalar getAbsoluteTimeInSeconds() const
 {
     // This is super wonky, should be improved someday
     const auto delta = chVTTimeElapsedSinceX(previous_time_systicks_);
     previous_time_systicks_ += delta;
     absolute_time_systicks_ += delta;
     return Scalar(absolute_time_systicks_) / Scalar(CH_CFG_ST_FREQUENCY);
 }

geom::aabb Object::getBounds() const {
    geom::aabb bounds(
        vec3f(
            vertices_[0], vertices_[1], vertices_[2]));

    for(int i=1; i<nVertices_; ++i) {
        bounds.extendToFit(
            vec3f(
                vertices_[3*i], vertices_[3*i+1], vertices_[3*i+2]));
    }

    return bounds;
}

void TECS::_update_speed_states(float airspeed_setpoint, float indicated_airspeed, float EAS2TAS)
{
	// Calculate the time in seconds since the last update and use the default time step value if out of bounds
	uint64_t now = ecl_absolute_time();
	const float dt = constrain((now - _speed_update_timestamp) * 1.0e-6f, DT_MIN, DT_MAX);

	// Convert equivalent airspeed quantities to true airspeed
	_EAS_setpoint = airspeed_setpoint;
	_TAS_setpoint  = _EAS_setpoint * EAS2TAS;
	_TAS_max   = _indicated_airspeed_max * EAS2TAS;
	_TAS_min   = _indicated_airspeed_min * EAS2TAS;

	// If airspeed measurements are not being used, fix the airspeed estimate to halfway between
	// min and max limits
	if (!ISFINITE(indicated_airspeed) || !airspeed_sensor_enabled()) {
		_EAS = 0.5f * (_indicated_airspeed_min + _indicated_airspeed_max);

	} else {
		_EAS = indicated_airspeed;
	}

	// If first time through or not flying, reset airspeed states
	if (_speed_update_timestamp == 0 || !_in_air) {
		_tas_rate_state = 0.0f;
		_tas_state = (_EAS * EAS2TAS);
	}

	// Obtain a smoothed airspeed estimate using a second order complementary filter

	// Update TAS rate state
	float tas_error = (_EAS * EAS2TAS) - _tas_state;
	float tas_rate_state_input = tas_error * _tas_estimate_freq * _tas_estimate_freq;

	// limit integrator input to prevent windup
	if (_tas_state < 3.1f) {
		tas_rate_state_input = max(tas_rate_state_input, 0.0f);

	}

	// Update TAS state
	_tas_rate_state = _tas_rate_state + tas_rate_state_input * dt;
	float tas_state_input = _tas_rate_state + _speed_derivative + tas_error * _tas_estimate_freq * 1.4142f;
	_tas_state = _tas_state + tas_state_input * dt;

	// Limit the airspeed state to a minimum of 3 m/s
	_tas_state = max(_tas_state, 3.0f);
	_speed_update_timestamp = now;

}