cvd_src/utility_helpers.h

#ifndef CVD_INCLUDE_UTILITY_HELPERS_H
#define CVD_INCLUDE_UTILITY_HELPERS_H

#include <cvd/utility.h>
#include <xmmintrin.h>
using namespace std;

namespace CVD{
    template <class F, class T1, class T2, int A, int M> inline void maybe_aligned_differences(const T1* a, const T1* b, T2* c, size_t count)
    {
    if (count < M*2) {
        F::unaligned_differences(a,b,c,count);
        return;
    }
    if (!is_aligned<A>(a)) {        
        size_t steps = steps_to_align<A>(a);
        F::unaligned_differences(a,b,c,steps);
        count -= steps;
        a += steps;
        b += steps;
        c += steps;
    }
    if (!is_aligned<A>(c) || count < M) {
        F::unaligned_differences(a,b,c,count);
        return;
    }   
    size_t block = (count/M)*M;
    F::aligned_differences(a,b,c,block);
    if (count > block) {
        F::unaligned_differences(a+block,b+block,c+block,count-block);
    }
    }    
    
    template <class F, class T1, class T2, int A, int M> inline void maybe_aligned_add_mul_add(const T1* a, const T1* b, const T1& c, T2* out, size_t count)
    {
    if (count < M*2) {
        F::unaligned_add_mul_add(a,b,c,out,count);
        return;
    }
    if (!is_aligned<A>(a)) {      
        size_t steps = steps_to_align<A>(a);
        F::unaligned_add_mul_add(a,b,c,out,steps);
        count -= steps;
        a += steps;
        b += steps;
        out += steps;
        if (count < M || !is_aligned<16>(out)) {
        F::unaligned_add_mul_add(a,b,c,out,count);
        return;
        }
    }
    else if (count < M || !is_aligned<16>(out)) {
        F::unaligned_add_mul_add(a,b,c,out,count);
        return;
    }
    size_t block = (count/M)*M;
    F::aligned_add_mul_add(a,b,c,out,block);
    if (count > block)
        F::unaligned_add_mul_add(a+block,b+block,c, out+block,count-block);
    }    

    template <class F, class T1, class T2, int A, int M> inline void maybe_aligned_assign_mul(const T1* a, const T1& c, T2* out, size_t count)
    {
    if (count < M*2) {
        F::unaligned_assign_mul(a,c,out,count);
        return;
    }
    if (!is_aligned<A>(a)) {      
        size_t steps = steps_to_align<A>(a);
        F::unaligned_assign_mul(a,c,out,steps);
        count -= steps;
        a += steps;
        out += steps;
        if (count < M) {
        F::unaligned_assign_mul(a,c,out,count);
        return;
        }
    }
    size_t block = (count/M)*M;
    F::aligned_assign_mul(a,c,out,block);
    if (count > block) {
        F::unaligned_assign_mul(a+block,c, out+block,count-block);
    }
    }    

    template <class F, class R, class T1, int A, int M> inline R maybe_aligned_inner_product(const T1* a, const T1* b, size_t count)
    {
    if (count < M*2) {
        return F::unaligned_inner_product(a,b,count);
    }
    R sum = 0;
    if (!is_aligned<A>(a)) {      
        size_t steps = steps_to_align<A>(a);
        sum = F::unaligned_inner_product(a,b,steps);
        count -= steps;
        a += steps;
        b += steps;
        if (count < M) {
        return sum + F::unaligned_inner_product(a,b,count);
        }
    }
    size_t block = (count/M)*M;
    sum += F::aligned_inner_product(a,b,block);
    if (count > block)
        sum += F::unaligned_inner_product(a+block,b+block,count-block);
    return sum;
    }    

    template <class F, class R, class T1, int A, int M> inline R maybe_aligned_ssd(const T1* a, const T1* b, size_t count)
    {
    if (count < M*2) {
        return F::unaligned_ssd(a,b,count);
    }
    R sum = 0;
    if (!is_aligned<A>(a)) {      
        size_t steps = steps_to_align<A>(a);
        sum = F::unaligned_ssd(a,b,steps);
        count -= steps;
        a += steps;
        b += steps;
        if (count < M) {
        return sum + F::unaligned_ssd(a,b,count);
        }
    }
    size_t block = (count/M)*M;
    sum += F::aligned_ssd(a,b,block);
    if (count > block)
        sum += F::unaligned_ssd(a+block,b+block,count-block);
    return sum;
    }    

template <class F, class T1, class T2, int A, int M> inline void maybe_aligned_square(const T1* in, T2* out, size_t count)
{
    if (count < M*2) {
    return F::unaligned_square(in,out,count);
    }
    if (!is_aligned<A>(in)) {
    size_t steps = steps_to_align<A>(in);
    F::unaligned_square(in,out,steps);
    count -= steps;
    in += steps;
    out += steps;
    if (count < M) {
        F::unaligned_square(in,out,count);
    }
    }
    size_t block = (count/M)*M;
    F::aligned_square(in,out,block);
    if (count > block)
    F::unaligned_square(in+block,out+block,count-block);
}    

template <class F, class T1, class T2, int A, int M> inline void maybe_aligned_subtract_square(const T1* in, T2* out, size_t count)
{
    if (count < M*2) {
    return F::unaligned_subtract_square(in,out,count);
    }
    if (!is_aligned<A>(in)) {
    size_t steps = steps_to_align<A>(in);
    F::unaligned_subtract_square(in,out,steps);
    count -= steps;
    in += steps;
    out += steps;
    if (count < M) {
        F::unaligned_subtract_square(in,out,count);
    }
    }
    size_t block = (count/M)*M;
    F::aligned_subtract_square(in,out,block);
    if (count > block)
    F::unaligned_subtract_square(in+block,out+block,count-block);
}    



}

#endif