Lattice.cpp 11.7 KB
Newer Older
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
1
2
3
4
5
6
7
/*
 * Lattice.cpp
 *
 *  Created on: 08.04.2017
 *      Author: Yaro
 */

8
#include <pinkIndexer/Lattice.h>
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
9
10
11
12
13
14
15
16
17
18
19
20
21
22
#include <algorithm>
#include <assert.h>
#include <inttypes.h>
#include <limits>
#include <math.h>

#ifndef M_1_PI
#define M_1_PI 0.318309886183790671538
#endif // !M_1_PI


using namespace std;
using namespace Eigen;

23
namespace pinkIndexer
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
24
{
25
26
27
28
    static inline void minimize2DLattice(Matrix3f& basis);
    static inline void sortTwoColumns_ascending(Matrix3f& basis, Vector3f& squaredNorms, float colNumber1, float colNumber2);
    static inline void sortColumnsByNorm_ascending(Matrix3f& basis);
    static inline void getMinA(Matrix3f& basis, Vector3f& minA, float& minALengthSquared);
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
29

30
31
32
33
34
35
36
37
38
    Lattice::Lattice() {}

    Lattice::Lattice(const Matrix3f& basis)
        : basis(basis)
    {
    }
    Lattice::Lattice(const Vector3f& a, const Vector3f& b, const Vector3f& c)
    {
        basis << a, b, c;
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
39
40
    }

41
    static inline void minimize2DLattice(Matrix3f& basis)
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
42
    {
43
44
45
46
        if (basis.col(0).squaredNorm() < basis.col(1).squaredNorm())
        { // will always be true in the current arrangement!
            basis.col(0).swap(basis.col(1));
        }
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
47

48
49
50
51
        do
        {
            float r = round(basis.col(0).dot(basis.col(1)) / basis.col(1).squaredNorm());
            Vector3f tmp = basis.col(0) - r * basis.col(1);
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
52

53
54
55
56
57
58
            basis.col(0) = basis.col(1);
            basis.col(1) = tmp;
        } while (basis.col(1).squaredNorm() < basis.col(0).squaredNorm());
    }

    static inline void sortTwoColumns_ascending(Matrix3f& basis, Vector3f& squaredNorms, float colNumber1, float colNumber2)
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
59
    {
60
61
62
        if (squaredNorms[colNumber1] > squaredNorms[colNumber2])
        {
            basis.col(colNumber1).swap(basis.col(colNumber2));
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
63

64
65
66
67
            float tmp = squaredNorms[colNumber1];
            squaredNorms[colNumber1] = squaredNorms[colNumber2];
            squaredNorms[colNumber2] = tmp;
        }
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
68
69
    }

70
71
72
73
74
75
76
77
    static inline void sortColumnsByNorm_ascending(Matrix3f& basis)
    {
        // simple bubble sort implementation
        Vector3f squaredNorms = basis.colwise().squaredNorm();
        sortTwoColumns_ascending(basis, squaredNorms, 0, 1);
        sortTwoColumns_ascending(basis, squaredNorms, 1, 2);
        sortTwoColumns_ascending(basis, squaredNorms, 0, 1);
    }
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
78

79
80
81
82
83
    static inline void getMinA(Matrix3f& basis, Vector3f& minA, float& minALengthSquared)
    {
        const Vector3f& b1 = basis.col(0);
        const Vector3f& b2 = basis.col(1);
        const Vector3f& b3 = basis.col(2);
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
84

85
86
87
88
        float b232 = b2.dot(b3) / b2.squaredNorm();
        float b122 = b1.dot(b2) / b2.squaredNorm();
        float b131 = b1.dot(b3) / b1.squaredNorm();
        float b121 = b1.dot(b2) / b1.squaredNorm();
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
89

90
91
92
93
94
        float y2 = -(b232 - b122 * b131) / (1 - b121 * b122);
        float y1 = -(b131 - b121 * b232) / (1 - b121 * b122);

        minALengthSquared = numeric_limits<float>::max();
        for (float i_1 = -1; i_1 <= 1; i_1++)
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
95
        {
96
            for (float i_2 = -1; i_2 <= 1; i_2++)
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
97
            {
98
99
100
                float x1 = round(y1 + i_1);
                float x2 = round(y2 + i_2);
                if ((abs(x1 - y1) <= 1) & (abs(x2 - y2) <= 1))
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
101
                {
102
103
104
105
106
107
108
109
                    Vector3f a = b3 + x2 * b2 + x1 * b1;
                    const float aLengthSquared = a.squaredNorm();

                    if (aLengthSquared < minALengthSquared)
                    {
                        minA = a;
                        minALengthSquared = aLengthSquared;
                    }
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
110
111
112
113
114
                }
            }
        }
    }

115
116
117
118
    // algorithm implemented after http://www.csie.nuk.edu.tw/~cychen/Lattices/A%203-Dimensional%20Lattice%20Reduction%20Algorithm.pdf
    Lattice& Lattice::minimize()
    {
        assert(abs(basis.determinant()) >= 1e-10); // nonsingular (value for normal crystal... can bee too high!)
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
119

120
        Vector3f minA(0, 0, 0); // initialization not required, but if not done, compiler issues warning
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
121

122
123
124
125
        bool terminationFlag = false;
        while (terminationFlag == false)
        {
            sortColumnsByNorm_ascending(basis);
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
126

127
            minimize2DLattice(basis);
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
128

129
130
            float minALengthSquared;
            getMinA(basis, minA, minALengthSquared);
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
131

132
133
134
135
136
137
138
139
            if (minALengthSquared >= basis.col(2).squaredNorm())
            {
                terminationFlag = true;
            }
            else
            {
                basis.col(2) = minA;
            }
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
140
141
        }

142
        sortColumnsByNorm_ascending(basis);
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
143

144
145
        return *this;
    }
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
146

147
148
149
150
151
    Vector3f Lattice::getBasisVectorAngles_deg() const
    {
        const Vector3f& a = basis.col(0);
        const Vector3f& b = basis.col(1);
        const Vector3f& c = basis.col(2);
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
152

153
154
155
156
        Vector3f angles;
        angles[0] = atan2(b.cross(c).norm(), b.dot(c)) * (M_1_PI * 180);
        angles[1] = atan2(a.cross(c).norm(), a.dot(c)) * (M_1_PI * 180);
        angles[2] = atan2(a.cross(b).norm(), a.dot(b)) * (M_1_PI * 180);
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
157

158
159
        return angles;
    }
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
160

161
162
163
164
165
    Vector3f Lattice::getBasisVectorAnglesNormalized_deg() const
    {
        const Vector3f& a = basis.col(0);
        const Vector3f& b = basis.col(1);
        const Vector3f& c = basis.col(2);
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
166

167
168
169
170
        Vector3f angles;
        angles[0] = atan2(b.cross(c).norm(), abs(b.dot(c))) * (M_1_PI * 180);
        angles[1] = atan2(a.cross(c).norm(), abs(a.dot(c))) * (M_1_PI * 180);
        angles[2] = atan2(a.cross(b).norm(), abs(a.dot(b))) * (M_1_PI * 180);
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
171

172
173
        return angles;
    }
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
174

175
176
177
178
179
    std::ostream& operator<<(std::ostream& os, const Lattice& lattice)
    {
        os << lattice.basis;
        return os;
    }
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
180

181
182
183
184
    // reorder such, that reordered basis differs from the prototype lattice by a rotation
    void Lattice::reorder(const Lattice prototypeLattice)
    {
        Matrix3f prototypeBasis_inv = prototypeLattice.getBasis().inverse();
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
185

186
        // clang-format off
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
	Array<int, 3, 6> indexPermutations;
	indexPermutations <<
		0, 0, 1, 1, 2, 2,
		1, 2, 0, 2, 0, 1,
		2, 1, 2, 0, 1, 0;

	Array<float, 3, 8> signPermutations;
	signPermutations <<
		1,  1,  1,  1, -1, -1, -1, -1,
		1,  1, -1, -1,  1,  1, -1, -1,
		1, -1,  1, -1,  1, -1,  1, -1;
    

    Matrix3f permutedBasis;
    Matrix3f bestPermutedBasis;
	Matrix3f resultingRotationMatrix;
	float smallestDefect = numeric_limits<float>::max();
    for (int indexPermutation = 0; indexPermutation < 6; indexPermutation++)
    {
        for (int signPermutation = 0; signPermutation < 8; signPermutation++)
        {
            permutedBasis << basis.col(indexPermutations(0, indexPermutation))*signPermutations(0, signPermutation),
					         basis.col(indexPermutations(1, indexPermutation))*signPermutations(1, signPermutation),
						     basis.col(indexPermutations(2, indexPermutation))*signPermutations(2, signPermutation);

			resultingRotationMatrix = permutedBasis*prototypeBasis_inv;

			//cout << "resultingRotationMatrix \n" << resultingRotationMatrix << endl;

			float defect = (resultingRotationMatrix.transpose() * resultingRotationMatrix - Matrix3f::Identity()).squaredNorm();
			//cout << "defect: " << defect<<endl;
			if (defect < smallestDefect) {
				bestPermutedBasis = permutedBasis;
				smallestDefect = defect;
				//cout << "taken\n" << permutedBasis 
				//	<< endl << endl<< prototypeLattice.getBasis() 
				//	<< endl << endl << prototypeBasis_inv 
				//	<< endl << endl << resultingRotationMatrix << endl << endl  ;
			}
        }
    }

	basis = bestPermutedBasis;

231
232
        // clang-format on
    }
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
233

234
    void Lattice::reorder(const Eigen::Vector3f prototypeNorms, const Eigen::Vector3f prototypeAngles_deg)
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
235
    {
236
237
238
239
        // cout << "prototype angles" << endl << prototypeAngles_deg << endl;

        Vector3f prototypeAnglesNormalized_deg = prototypeAngles_deg;
        for (int i = 0; i < 3; i++)
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
240
        {
241
242
243
244
            if (prototypeAnglesNormalized_deg(i) > 90)
            {
                prototypeAnglesNormalized_deg(i) = 180 - prototypeAnglesNormalized_deg(i);
            }
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
245
246
        }

247
        // cout << "prototype angles normalized" << endl << prototypeAnglesNormalized_deg << endl;
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
248

249
250
251
        Array<float, 6, 1> prototypeLatticeParameters;
        prototypeLatticeParameters << prototypeNorms, prototypeAnglesNormalized_deg;
        Array<float, 6, 1> prototypeLatticeParametersInverse = 1.0f / prototypeLatticeParameters;
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
252

253
254
        Vector3f n = getBasisVectorNorms();
        Vector3f a = getBasisVectorAnglesNormalized_deg();
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
255

256
        // clang-format off
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
	Array<int, 3, 6> indexPermutations;
	indexPermutations <<
		0, 0, 1, 1, 2, 2,
		1, 2, 0, 2, 0, 1,
		2, 1, 2, 0, 1, 0;

	Array<float, 6, 6> allPermutations;
	auto& i = indexPermutations;
	allPermutations <<
		n[i(0, 0)], n[i(0, 1)], n[i(0, 2)], n[i(0, 3)], n[i(0, 4)], n[i(0, 5)],
		n[i(1, 0)], n[i(1, 1)], n[i(1, 2)], n[i(1, 3)], n[i(1, 4)], n[i(1, 5)],
		n[i(2, 0)], n[i(2, 1)], n[i(2, 2)], n[i(2, 3)], n[i(2, 4)], n[i(2, 5)],
		a[i(0, 0)], a[i(0, 1)], a[i(0, 2)], a[i(0, 3)], a[i(0, 4)], a[i(0, 5)],
		a[i(1, 0)], a[i(1, 1)], a[i(1, 2)], a[i(1, 3)], a[i(1, 4)], a[i(1, 5)],
		a[i(2, 0)], a[i(2, 1)], a[i(2, 2)], a[i(2, 3)], a[i(2, 4)], a[i(2, 5)];
272
        // clang-format on
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
273

274
275
276
        // find best permutation
        auto rasiduals = ((allPermutations.colwise() - prototypeLatticeParameters).colwise() * prototypeLatticeParametersInverse).abs();
        auto maxResiduals = rasiduals.colwise().maxCoeff();
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
277

278
279
        int bestPermutationIndex;
        maxResiduals.minCoeff(&bestPermutationIndex);
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
280

281
282
283
284
        Matrix3f reorderedBasis;
        reorderedBasis << basis.col(indexPermutations(0, bestPermutationIndex)), basis.col(indexPermutations(1, bestPermutationIndex)),
            basis.col(indexPermutations(2, bestPermutationIndex));
        basis = reorderedBasis;
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
285

286
287
288
289
        // change sign of vectors
        Matrix3f bestNegatedBasis;
        float bestNegatedBasisResidual = numeric_limits<float>::max();
        for (int l = -1; l <= 1; l += 2)
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
290
        {
291
292
293
294
295
            for (int m = -1; m <= 1; m += 2)
            {
                Matrix3f negatedBasis;
                negatedBasis << basis.col(0), basis.col(1) * l, basis.col(2) * m;
                Lattice negatedLattice(negatedBasis);
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
296

297
                float maxResidual = (negatedLattice.getBasisVectorAngles_deg() - prototypeAngles_deg).cwiseAbs().maxCoeff();
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
298

299
300
301
302
303
                if (maxResidual < bestNegatedBasisResidual)
                {
                    bestNegatedBasisResidual = maxResidual;
                    bestNegatedBasis = negatedBasis;
                }
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
304
305
            }
        }
306
        basis = bestNegatedBasis;
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
307

308
        // cout << "best residual: " << bestNegatedBasisResidual << endl;
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
309

310
311
        // cout << "angles" << endl << getBasisVectorAngles_deg() << endl;
    }
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
312

313
    void Lattice::normalizeAngles()
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
314
    {
315
        for (int l = -1; l <= 1; l += 2)
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
316
        {
317
            for (int m = -1; m <= 1; m += 2)
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
318
            {
319
320
321
322
323
324
325
326
327
                Matrix3f negatedBasis;
                negatedBasis << basis.col(0), basis.col(1) * l, basis.col(2) * m;
                Lattice negatedLattice(negatedBasis);

                if ((negatedLattice.getBasisVectorAngles_deg().array() <= 90).all())
                {
                    basis = negatedBasis;
                    return;
                }
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
328
329
330
            }
        }

331
332
333
        // not possible to have all < 90, get the combination with smallest sum
        float smallestSum = 3 * 180;
        for (int l = -1; l <= 1; l += 2)
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
334
        {
335
            for (int m = -1; m <= 1; m += 2)
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
336
            {
337
338
339
340
341
342
343
344
345
346
                Matrix3f negatedBasis;
                negatedBasis << basis.col(0), basis.col(1) * l, basis.col(2) * m;
                Lattice negatedLattice(negatedBasis);

                float anglesSum = negatedLattice.getBasisVectorAngles_deg().sum();
                if (anglesSum <= smallestSum)
                {
                    smallestSum = anglesSum;
                    basis = negatedBasis;
                }
Yaroslav Gevorkov's avatar
Yaroslav Gevorkov committed
347
348
349
            }
        }
    }
350
} // namespace pinkIndexer