kis_fast_math.cpp 3.76 KB
Newer Older
Cyrille Berger's avatar
Cyrille Berger committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26
/*
 *  Copyright (c) 2010 Lukáš Tvrdý <lukast.dev@gmail.com>
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  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 General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 *
 *  adopted from here http://www.snippetcenter.org/en/a-fast-atan2-function-s1868.aspx
 */

#include "kis_fast_math.h"

#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <QtGlobal>
27
#include <QGlobalStatic>
Cyrille Berger's avatar
Cyrille Berger committed
28 29

// Algorithm from http://www.snippetcenter.org/en/a-fast-atan2-function-s1868.aspx
Cyrille Berger's avatar
Cyrille Berger committed
30
const qreal MAX_SECOND_DERIV_IN_RANGE = 0.6495;
Cyrille Berger's avatar
Cyrille Berger committed
31 32

/// precision
Cyrille Berger's avatar
Cyrille Berger committed
33
const qreal MAX_ERROR = 0.0001;
Cyrille Berger's avatar
Cyrille Berger committed
34 35 36

struct KisATanTable {

37
    KisATanTable() {
Cyrille Berger's avatar
Cyrille Berger committed
38
        qreal nf = ::sqrt(MAX_SECOND_DERIV_IN_RANGE / (8 * MAX_ERROR));
39 40
        NUM_ATAN_ENTRIES = int(nf) + 1;
        // Build table
41
        qreal y = 10.0;
Cyrille Berger's avatar
Cyrille Berger committed
42 43
        qreal x;
        ATanTable = new qreal[NUM_ATAN_ENTRIES + 1];
44
        ATanTable[0] = 0.0;
45 46
        for (quint32 i = 1; i <= NUM_ATAN_ENTRIES; i++) {
            x = (y / i) * NUM_ATAN_ENTRIES;
Cyrille Berger's avatar
Cyrille Berger committed
47
            ATanTable[i] = (qreal)::atan2(y, x);
Cyrille Berger's avatar
Cyrille Berger committed
48 49
        }

50 51 52 53 54
    }

    ~KisATanTable() {
        delete [] ATanTable;
    }
Cyrille Berger's avatar
Cyrille Berger committed
55

56
    quint32 NUM_ATAN_ENTRIES;
Cyrille Berger's avatar
Cyrille Berger committed
57
    qreal* ATanTable;
Cyrille Berger's avatar
Cyrille Berger committed
58 59
};

Boudewijn Rempt's avatar
Boudewijn Rempt committed
60
Q_GLOBAL_STATIC(KisATanTable, kisATanTable)
Cyrille Berger's avatar
Cyrille Berger committed
61

62
/// private functions
Cyrille Berger's avatar
Cyrille Berger committed
63

Cyrille Berger's avatar
Cyrille Berger committed
64
inline qreal interp(qreal r, qreal a, qreal b)
65 66 67
{
    return r*(b - a) + a;
}
Cyrille Berger's avatar
Cyrille Berger committed
68

Cyrille Berger's avatar
Cyrille Berger committed
69
inline qreal calcAngle(qreal x, qreal y)
Cyrille Berger's avatar
Cyrille Berger committed
70
{
71 72 73 74
    static qreal af = kisATanTable->NUM_ATAN_ENTRIES;
    static int ai = kisATanTable->NUM_ATAN_ENTRIES;
    static qreal* ATanTable = kisATanTable->ATanTable;
    qreal di = (y / x) * af;
75
    int i = (int)(di);
76 77
    if (i >= ai) return ::atan2(y, x);
    return interp(di - i, ATanTable[i], ATanTable[i+1]);
Cyrille Berger's avatar
Cyrille Berger committed
78 79
}

Cyrille Berger's avatar
Cyrille Berger committed
80
qreal KisFastMath::atan2(qreal y, qreal x)
Cyrille Berger's avatar
Cyrille Berger committed
81 82
{

83 84 85
    if (y == 0.0) { // the line is horizontal
        if (x >= 0.0) { // towards the right
            return(0.0);// the angle is 0
Cyrille Berger's avatar
Cyrille Berger committed
86 87
        }
        // toward the left
Cyrille Berger's avatar
Cyrille Berger committed
88
        return qreal(M_PI);
Cyrille Berger's avatar
Cyrille Berger committed
89
    } // we now know that y is not 0 check x
90 91
    if (x == 0.0) { // the line is vertical
        if (y > 0.0) {
Cyrille Berger's avatar
Cyrille Berger committed
92
            return M_PI_2;
Cyrille Berger's avatar
Cyrille Berger committed
93
        }
Cyrille Berger's avatar
Cyrille Berger committed
94
        return -M_PI_2;
Cyrille Berger's avatar
Cyrille Berger committed
95 96
    }
    // from here on we know that niether x nor y is 0
97
    if (x > 0.0) {
Cyrille Berger's avatar
Cyrille Berger committed
98
        // we are in quadrant 1 or 4
99
        if (y > 0.0) {
Cyrille Berger's avatar
Cyrille Berger committed
100 101 102
            // we are in quadrant 1
            // now figure out which side of the 45 degree line
            if (x > y) {
103
                return(calcAngle(x, y));
Cyrille Berger's avatar
Cyrille Berger committed
104
            }
Cyrille Berger's avatar
Cyrille Berger committed
105
            return(M_PI_2 - calcAngle(y, x));
Cyrille Berger's avatar
Cyrille Berger committed
106 107 108 109
        }
        // we are in quadrant 4
        y = -y;
        // now figure out which side of the 45 degree line
110 111
        if (x > y) {
            return(-calcAngle(x, y));
Cyrille Berger's avatar
Cyrille Berger committed
112
        }
Cyrille Berger's avatar
Cyrille Berger committed
113
        return(-M_PI_2 + calcAngle(y, x));
Cyrille Berger's avatar
Cyrille Berger committed
114 115 116 117
    }
    // we are in quadrant 2 or 3
    x = -x;
    // flip x so we can use it as a positive
118
    if (y > 0.0) {
Cyrille Berger's avatar
Cyrille Berger committed
119 120
        // we are in quadrant 2
        // now figure out which side of the 45 degree line
121
        if (x > y) {
Cyrille Berger's avatar
Cyrille Berger committed
122 123
            return(M_PI - calcAngle(x, y));
        } return(M_PI_2 + calcAngle(y, x));
Cyrille Berger's avatar
Cyrille Berger committed
124 125 126
    }
    // we are in quadrant 3
    y = -y;
luz paz's avatar
luz paz committed
127
    // flip y so we can use it as a positive
Cyrille Berger's avatar
Cyrille Berger committed
128
    // now figure out which side of the 45 degree line
129
    if (x > y) {
Cyrille Berger's avatar
Cyrille Berger committed
130 131
        return(-M_PI + calcAngle(x, y));
    } return(-M_PI_2 - calcAngle(y, x));
Cyrille Berger's avatar
Cyrille Berger committed
132
}