fixed_point.h

Go to the documentation of this file.

/* Calf DSP Library
 * DSP primitives.
 *
 * Copyright (C) 2001-2007 Krzysztof Foltman
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This program 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 program; if not, write to the
 * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, 
 * Boston, MA 02111-1307, USA.
 */
#ifndef __CALF_FIXED_POINT_H
#define __CALF_FIXED_POINT_H
 
namespace dsp {
 
inline uint32_t shr(uint32_t v, int bits = 1) { return v>>bits; };
inline int32_t shr(int32_t v, int bits = 1) { return v>>bits; };
inline uint64_t shr(uint64_t v, int bits = 1) { return v>>bits; };
inline int64_t shr(int64_t v, int bits = 1) { return v>>bits; };
inline float shr(float v, int bits = 1) { return v*(1.0/(1<<bits)); };
inline double shr(double v, int bits = 1) { return v*(1.0/(1<<bits)); };
template<class T, int FracBits>
inline T shr(T v, int bits = 1) {
    v.set(v >> bits);
    return v;
}
 
template<class T, int FracBits> class fixed_point {
    T value;
    enum { IntBits = (sizeof(T)*8) - FracBits };
 
public:
    inline fixed_point() {
    }

    template<class U, int FracBits2> inline fixed_point(const fixed_point<U, FracBits2> &v) {
        if (FracBits == FracBits2) value = v.get();
        else if (FracBits > FracBits2) value = v.get() << abs(FracBits - FracBits2);
        else value = v.get() >> abs(FracBits - FracBits2);
    }
 
    /* this would be way too confusing, it wouldn't be obvious if it expects a whole fixed point or an integer part
    explicit inline fixed_point(T v) {
        this->value = v;
    }
    */
    explicit inline fixed_point(double v) {
        value = (T)(v*one());
    }
    
    static inline fixed_point from_base(const T &v) 
    {
        fixed_point result;
        result.value = v;
        return result;
    }
    
    inline static T one() {
        return (T)(1) << FracBits;
    }
 
    inline void set(T value) {
        this->value = value;
    }
      
    inline T get() const {
        return value;
    }
      
    inline operator double() const {
        return value * (1.0/one());
    }
    
    inline fixed_point &operator=(double v) {
        value = (T)(v*one());
        return *this;
    }
    
    template<class U, int FracBits2> static inline T rebase(const fixed_point<U, FracBits2> &v) {
        if (FracBits == FracBits2) 
            return v.get();
        if (FracBits > FracBits2) 
            return v.get() << abs(FracBits - FracBits2);
        return v.get() >> abs(FracBits2 - FracBits);
    }
    
    template<class U, int FracBits2> inline fixed_point &operator=(const fixed_point<U, FracBits2> &v) {
        value = rebase<U, FracBits2>(v);
        return *this;
    }
 
    template<class U, int FracBits2> inline fixed_point &operator+=(const fixed_point<U, FracBits2> &v) {
        value += rebase<U, FracBits2>(v);
        return *this;
    }
 
    template<class U, int FracBits2> inline fixed_point &operator-=(const fixed_point<U, FracBits2> &v) {
        value -= rebase<U, FracBits2>(v);
        return *this;
    }
 
    template<class U, int FracBits2> inline fixed_point operator+(const fixed_point<U, FracBits2> &v) const {
        fixed_point fpv;
        fpv.set(value + rebase<U, FracBits2>(v));
        return fpv;
    }
 
    template<class U, int FracBits2> inline fixed_point operator-(const fixed_point<U, FracBits2> &v) const {
        fixed_point fpv;
        fpv.set(value - rebase<U, FracBits2>(v));
        return fpv;
    }

    template<class U, int FracBits2> inline fixed_point operator*(const fixed_point<U, FracBits2> &v) const {
        fixed_point tmp;
        tmp.set(((int64_t)value) * v.get() >> FracBits2);
        return tmp;
    }

    template<class U, int FracBits2, class BigType> inline fixed_point& operator*=(const fixed_point<U, FracBits2> &v) {
        value = (T)(((BigType)value) * v.get() >> FracBits2);
        return *this;
    }
 
    inline fixed_point operator+(int v) const {
        fixed_point tmp;
        tmp.set(value + (v << FracBits));
        return tmp;
    }
 
    inline fixed_point operator-(int v) const {
        fixed_point tmp;
        tmp.set(value - (v << FracBits));
        return tmp;
    }
 
    inline fixed_point operator*(int v) const {
        fixed_point tmp;
        tmp.value = value*v;
        return tmp;
    }
 
    inline fixed_point& operator+=(int v) {
        value += (v << FracBits);
        return *this;
    }
 
    inline fixed_point& operator-=(int v) {
        value -= (v << FracBits);
        return *this;
    }
 
    inline fixed_point& operator*=(int v) {
        value *= v;
        return *this;
    }

    inline T ipart() const {
        return value >> FracBits;
    }

    inline unsigned int uipart() const {
        return ((unsigned)value) >> FracBits;
    }

    inline unsigned int ui64part() const {
        return ((uint64_t)value) >> FracBits;
    }

    inline T fpart() const {
        return value & ((1 << FracBits)-1);
    }

    template<int Bits>
    inline T fpart() const {
        int fbits = value & ((1 << FracBits)-1);
        if (Bits == FracBits) return fbits;
        int shift = abs(Bits-FracBits);
        return (Bits < FracBits) ? (fbits >> shift) : (fbits << shift);
    }

    inline double fpart_as_double() const {
        return (value & ((1 << FracBits)-1)) * (1.0 / (1 << FracBits));
    }

    template<class U, int UseBits, class MulType> 
    inline U lerp_by_fract_int(U v1, U v2) const {
        int fp = fpart<UseBits>();
        assert ( fp >=0 && fp <= (1LL<<UseBits));
        return v1 + shr(((MulType)(v2-v1) * fp), UseBits);
    }
 
    template<class U, int UseBits> 
    inline U lerp_table_lookup_int(U* data) const {
        unsigned int pos = uipart();
        return lerp_by_fract_int<U, UseBits>(data[pos], data[pos+1]);
    }

    template<class U, int UseBits> 
    inline U lerp_table_lookup_int_shift(U* data, unsigned int shift) {
        unsigned int pos = (uipart() + shift) & ((1u << IntBits) - 1);
        return lerp_by_fract_int<U, UseBits>(data[pos], data[pos+1]);
    }
 
    template<class U> 
    inline U lerp_table_lookup_float(U* data) const {
        unsigned int pos = uipart();
        return data[pos] + (data[pos+1]-data[pos]) * fpart_as_double();
    }
 
    template<class U> 
    inline U lerp_table_lookup_float_mask(U* data, unsigned int mask) const {
        unsigned int pos = ui64part() & mask;
        // printf("full = %lld pos = %d + %f\n", value, pos, fpart_as_double());
        return data[pos] + (data[pos+1]-data[pos]) * fpart_as_double();
    }
 
    template<class U, int UseBits, class MulType> 
    inline U lerp_ptr_lookup_int(U *data) const {
        unsigned int pos = ui64part();
        return lerp_by_fract_int<U, UseBits, MulType>(data[pos], data[pos+1]);
    }
 
    template<class U> 
    inline U lerp_ptr_lookup_float(U *data) const {
        unsigned int pos = ui64part();
        return data[pos] + (data[pos+1]-data[pos]) * fpart_as_double();
    }
};
 
template<class T, int FractBits>
inline fixed_point<T, FractBits> operator*(int v, fixed_point<T, FractBits> v2) {
    v2 *= v;
    return v2;
}

typedef fixed_point<unsigned long long int, 24> wpos;
 
};
 
#endif