← Back to branch summary

~ubuntu-branches/ubuntu/precise/stellarium/precise

« back to all changes in this revision

Viewing changes to src/core/VecMath.hpp

  • Committer: Bazaar Package Importer
  • Author(s): Scott Howard
  • Date: 2010-02-15 20:48:39 UTC
  • mfrom: (1.1.9 upstream)
  • Revision ID: james.westby@ubuntu.com-20100215204839-u3qgbv60rho997yk
Tags: 0.10.3-0ubuntu1
https://launchpad.net/bugs/480553
* New upstream release.
  - fixes intel rendering bug (LP: #480553)

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/core/VecMath.hpp
26
26
 
27
27
#include <cmath>
28
28
#include <QString>
29
 
 
 
29
 
30
30
template<class T> class Vector2;
31
31
template<class T> class Vector3;
32
32
template<class T> class Vector4;
71
71
template<class T> class Vector2
72
72
{
73
73
public:
74
 
    inline Vector2();
75
 
    inline Vector2(const Vector2<T>&);
76
 
    inline Vector2(T, T);
 
74
        inline Vector2();
 
75
        inline Vector2(const Vector2<T>&);
 
76
        inline Vector2(T, T);
77
77
 
78
78
        inline Vector2& operator=(const Vector2<T>&);
79
79
        inline Vector2& operator=(const T*);
80
 
    inline void set(T, T);
 
80
        inline void set(T, T);
81
81
 
82
82
        inline bool operator==(const Vector2<T>&) const;
83
83
        inline bool operator!=(const Vector2<T>&) const;
84
84
 
85
85
        inline const T& operator[](int x) const;
86
 
    inline T& operator[](int);
 
86
        inline T& operator[](int);
87
87
        inline operator const T*() const;
88
88
        inline operator T*();
89
89
 
90
 
    inline Vector2& operator+=(const Vector2<T>&);
91
 
    inline Vector2& operator-=(const Vector2<T>&);
92
 
    inline Vector2& operator*=(T);
 
90
        inline Vector2& operator+=(const Vector2<T>&);
 
91
        inline Vector2& operator-=(const Vector2<T>&);
 
92
        inline Vector2& operator*=(T);
93
93
        inline Vector2& operator/=(T);
94
94
 
95
 
    inline Vector2 operator-(const Vector2<T>&) const;
96
 
    inline Vector2 operator+(const Vector2<T>&) const;
 
95
        inline Vector2 operator-(const Vector2<T>&) const;
 
96
        inline Vector2 operator+(const Vector2<T>&) const;
97
97
 
98
98
        inline Vector2 operator-() const;
99
99
        inline Vector2 operator+() const;
102
102
        inline Vector2 operator/(T) const;
103
103
 
104
104
 
105
 
    inline T dot(const Vector2<T>&) const;
106
 
 
107
 
    inline T length() const;
108
 
    inline T lengthSquared() const;
109
 
    inline void normalize();
110
 
 
111
 
    T v[2];
 
105
        inline T dot(const Vector2<T>&) const;
 
106
 
 
107
        inline T length() const;
 
108
        inline T lengthSquared() const;
 
109
        inline void normalize();
 
110
 
 
111
        T v[2];
112
112
};
113
113
 
 
114
 
114
115
//! @class Vector3
115
116
//! A templatized 3d vector compatible with openGL.
116
117
//! Use Vec3d or Vec3f typdef for vectors of double and float respectively.
117
118
template<class T> class Vector3
118
119
{
119
120
public:
120
 
    inline Vector3();
121
 
    inline Vector3(const Vector3&);
 
121
        inline Vector3();
 
122
        inline Vector3(const Vector3&);
122
123
        template <class T2> inline Vector3(const Vector3<T2>&);
123
 
    inline Vector3(T, T, T);
 
124
        inline Vector3(T, T, T);
124
125
        inline Vector3(T);
125
 
        
 
126
 
126
127
        inline Vector3& operator=(const Vector3&);
127
128
        inline Vector3& operator=(const T*);
128
129
        template <class T2> inline Vector3& operator=(const Vector3<T2>&);
129
 
    inline void set(T, T, T);
 
130
        inline void set(T, T, T);
130
131
 
131
132
        inline bool operator==(const Vector3<T>&) const;
132
133
        inline bool operator!=(const Vector3<T>&) const;
133
134
 
134
 
    inline T& operator[](int);
135
 
    inline const T& operator[](int) const;
 
135
        inline T& operator[](int);
 
136
        inline const T& operator[](int) const;
136
137
        inline operator const T*() const;
137
138
        inline operator T*();
 
139
        inline const T* data() const {return v;}
 
140
        inline T* data() {return v;}
138
141
 
139
 
    inline Vector3& operator+=(const Vector3<T>&);
140
 
    inline Vector3& operator-=(const Vector3<T>&);
141
 
    inline Vector3& operator*=(T);
 
142
        inline Vector3& operator+=(const Vector3<T>&);
 
143
        inline Vector3& operator-=(const Vector3<T>&);
 
144
        inline Vector3& operator*=(T);
142
145
        inline Vector3& operator/=(T);
143
146
 
144
 
    inline Vector3 operator-(const Vector3<T>&) const;
145
 
    inline Vector3 operator+(const Vector3<T>&) const;
 
147
        inline Vector3 operator-(const Vector3<T>&) const;
 
148
        inline Vector3 operator+(const Vector3<T>&) const;
146
149
 
147
150
        inline Vector3 operator-() const;
148
151
        inline Vector3 operator+() const;
151
154
        inline Vector3 operator/(T) const;
152
155
 
153
156
 
154
 
    inline T dot(const Vector3<T>&) const;
 
157
        inline T dot(const Vector3<T>&) const;
155
158
        inline Vector3 operator^(const Vector3<T>&) const;
156
159
 
157
160
        // Return latitude in rad
158
161
        inline T latitude() const;
159
162
        // Return longitude in rad
160
163
        inline T longitude() const;
161
 
        
 
164
 
162
165
        // Distance in radian between two
163
166
        inline T angle(const Vector3<T>&) const;
164
167
 
165
 
    inline T length() const;
166
 
    inline T lengthSquared() const;
167
 
    inline void normalize();
 
168
        inline T length() const;
 
169
        inline T lengthSquared() const;
 
170
        inline void normalize();
168
171
 
169
172
        inline void transfo4d(const Mat4d&);
170
173
        inline void transfo4d(const Mat4f&);
171
174
        T v[3];         // The 3 values
172
 
        
 
175
 
173
176
        QString toString() const {return QString("[%1, %2, %3]").arg(v[0]).arg(v[1]).arg(v[2]);}
 
177
        QString toStringLonLat() const {return QString("[") + QString::number(longitude()*180./M_PI, 'g', 12) + "," + QString::number(latitude()*180./M_PI, 'g', 12)+"]";}
174
178
};
175
179
 
176
180
 
180
184
template<class T> class Vector4
181
185
{
182
186
public:
183
 
    inline Vector4();
184
 
    inline Vector4(const Vector4<T>&);
 
187
        inline Vector4();
 
188
        inline Vector4(const Vector4<T>&);
185
189
        inline Vector4(const Vector3<T>&);
186
190
        inline Vector4(T, T, T);
187
191
        inline Vector4(T, T, T, T);
189
193
        inline Vector4& operator=(const Vector4<T>&);
190
194
        inline Vector4& operator=(const Vector3<T>&);
191
195
        inline Vector4& operator=(const T*);
192
 
    inline void set(T, T, T, T);
 
196
        inline void set(T, T, T, T);
193
197
 
194
198
        inline bool operator==(const Vector4<T>&) const;
195
199
        inline bool operator!=(const Vector4<T>&) const;
196
200
 
197
 
    inline T& operator[](int);
198
 
    inline const T& operator[](int) const;
 
201
        inline T& operator[](int);
 
202
        inline const T& operator[](int) const;
199
203
        inline operator T*();
200
204
        inline operator const T*() const;
201
205
 
202
 
    inline Vector4& operator+=(const Vector4<T>&);
203
 
    inline Vector4& operator-=(const Vector4<T>&);
204
 
    inline Vector4& operator*=(T);
 
206
        inline Vector4& operator+=(const Vector4<T>&);
 
207
        inline Vector4& operator-=(const Vector4<T>&);
 
208
        inline Vector4& operator*=(T);
205
209
        inline Vector4& operator/=(T);
206
210
 
207
 
    inline Vector4 operator-(const Vector4<T>&) const;
208
 
    inline Vector4 operator+(const Vector4<T>&) const;
 
211
        inline Vector4 operator-(const Vector4<T>&) const;
 
212
        inline Vector4 operator+(const Vector4<T>&) const;
209
213
 
210
214
        inline Vector4 operator-() const;
211
215
        inline Vector4 operator+() const;
214
218
        inline Vector4 operator/(T) const;
215
219
 
216
220
 
217
 
    inline T dot(const Vector4<T>&) const;
 
221
        inline T dot(const Vector4<T>&) const;
218
222
 
219
 
    inline T length() const;
220
 
    inline T lengthSquared() const;
221
 
    inline void normalize();
 
223
        inline T length() const;
 
224
        inline T lengthSquared() const;
 
225
        inline void normalize();
222
226
 
223
227
        inline void transfo4d(const Mat4d&);
224
228
 
231
235
template<class T> class Matrix4
232
236
{
233
237
 public:
234
 
    Matrix4();
235
 
    Matrix4(const Matrix4<T>& m);
236
 
    Matrix4(T,T,T,T,T,T,T,T,T,T,T,T,T,T,T,T);
237
 
    Matrix4(const T*);
238
 
    Matrix4(const Vector4<T>&, const Vector4<T>&,
239
 
            const Vector4<T>&, const Vector4<T>&);
 
238
        Matrix4();
 
239
        Matrix4(const Matrix4<T>& m);
 
240
        Matrix4(T,T,T,T,T,T,T,T,T,T,T,T,T,T,T,T);
 
241
        Matrix4(const T*);
 
242
        Matrix4(const Vector4<T>&, const Vector4<T>&,
 
243
                        const Vector4<T>&, const Vector4<T>&);
240
244
 
241
245
        inline Matrix4& operator=(const Matrix4<T>&);
242
246
        inline Matrix4& operator=(const T*);
243
247
        inline void set(T,T,T,T,T,T,T,T,T,T,T,T,T,T,T,T);
244
248
 
245
 
    inline T* operator[](int);
 
249
        inline T& operator[](int);
246
250
        inline operator T*();
247
251
        inline operator const T*() const;
248
252
 
249
 
    inline Matrix4 operator-(const Matrix4<T>&) const;
250
 
    inline Matrix4 operator+(const Matrix4<T>&) const;
251
 
    inline Matrix4 operator*(const Matrix4<T>&) const;
 
253
        inline Matrix4 operator-(const Matrix4<T>&) const;
 
254
        inline Matrix4 operator+(const Matrix4<T>&) const;
 
255
        inline Matrix4 operator*(const Matrix4<T>&) const;
252
256
 
253
 
    inline Vector3<T> operator*(const Vector3<T>&) const;
254
 
    inline Vector3<T> multiplyWithoutTranslation(const Vector3<T>& a) const;
 
257
        inline Vector3<T> operator*(const Vector3<T>&) const;
 
258
        inline Vector3<T> multiplyWithoutTranslation(const Vector3<T>& a) const;
255
259
        inline Vector4<T> operator*(const Vector4<T>&) const;
256
 
        
 
260
 
257
261
        inline void transfo(Vector3<T>&) const;
258
 
        
259
 
    static Matrix4<T> identity();
260
 
    static Matrix4<T> translation(const Vector3<T>&);
261
 
 
262
 
    //    static Matrix4<T> rotation(const Vector3<T>&);
263
 
    static Matrix4<T> rotation(const Vector3<T>&, T);
264
 
    static Matrix4<T> xrotation(T);
265
 
    static Matrix4<T> yrotation(T);
266
 
    static Matrix4<T> zrotation(T);
267
 
    static Matrix4<T> scaling(const Vector3<T>&);
268
 
    static Matrix4<T> scaling(T);
269
 
 
270
 
    Matrix4<T> transpose() const;
271
 
    Matrix4<T> inverse() const;
 
262
 
 
263
        static Matrix4<T> identity();
 
264
        static Matrix4<T> translation(const Vector3<T>&);
 
265
 
 
266
        //    static Matrix4<T> rotation(const Vector3<T>&);
 
267
        static Matrix4<T> rotation(const Vector3<T>&, T);
 
268
        static Matrix4<T> xrotation(T);
 
269
        static Matrix4<T> yrotation(T);
 
270
        static Matrix4<T> zrotation(T);
 
271
        static Matrix4<T> scaling(const Vector3<T>&);
 
272
        static Matrix4<T> scaling(T);
 
273
 
 
274
        Matrix4<T> transpose() const;
 
275
        Matrix4<T> inverse() const;
272
276
 
273
277
        inline void print(void) const;
274
278
 
275
 
    T r[16];
 
279
        T r[16];
276
280
};
277
281
 
 
282
//! Serialization routines.
 
283
template<class T> QDataStream& operator<<(QDataStream& out, const Vector2<T>& v) {out << v[0] << v[1]; return out;}
 
284
template<class T> QDataStream& operator<<(QDataStream& out, const Vector3<T>& v) {out << v[0] << v[1] << v[2]; return out;}
 
285
template<class T> QDataStream& operator<<(QDataStream& out, const Vector4<T>& v) {out << v[0] << v[1] << v[2] << v[3]; return out;}
 
286
template<class T> QDataStream& operator<<(QDataStream& out, const Matrix4<T>& m) {out << m[0] << m[1] << m[2] << m[3] << m[4] << m[5] << m[6] << m[7] << m[8] << m[9] << m[10] << m[11] << m[12] << m[13] << m[14] << m[15]; return out;}
278
287
 
 
288
template<class T> QDataStream& operator>>(QDataStream& in, Vector2<T>& v) {in >> v[0] >> v[1]; return in;}
 
289
template<class T> QDataStream& operator>>(QDataStream& in, Vector3<T>& v) {in >> v[0] >> v[1] >> v[2]; return in;}
 
290
template<class T> QDataStream& operator>>(QDataStream& in, Vector4<T>& v) {in >> v[0] >> v[1] >> v[2] >> v[3]; return in;}
 
291
template<class T> QDataStream& operator>>(QDataStream& in, Matrix4<T>& m) {in >> m[0] >> m[1] >> m[2] >> m[3] >> m[4] >> m[5] >> m[6] >> m[7] >> m[8] >> m[9] >> m[10] >> m[11] >> m[12] >> m[13] >> m[14] >> m[15]; return in;}
279
292
 
280
293
////////////////////////// Vector2 class methods ///////////////////////////////
281
294
 
294
307
template<class T> Vector2<T>& Vector2<T>::operator=(const Vector2<T>& a)
295
308
{
296
309
        v[0]=a.v[0]; v[1]=a.v[1];
297
 
    return *this;
 
310
        return *this;
298
311
}
299
312
 
300
313
template<class T> Vector2<T>& Vector2<T>::operator=(const T* a)
301
314
{
302
315
        v[0]=a[0]; v[1]=a[1];
303
 
    return *this;
 
316
        return *this;
304
317
}
305
318
 
306
319
template<class T> void Vector2<T>::set(T x, T y)
342
355
 
343
356
template<class T> Vector2<T>& Vector2<T>::operator+=(const Vector2<T>& a)
344
357
{
345
 
    v[0] += a.v[0]; v[1] += a.v[1];
346
 
    return *this;
 
358
        v[0] += a.v[0]; v[1] += a.v[1];
 
359
        return *this;
347
360
}
348
361
 
349
362
template<class T> Vector2<T>& Vector2<T>::operator-=(const Vector2<T>& a)
350
363
{
351
 
    v[0] -= a.v[0]; v[1] -= a.v[1];
352
 
    return *this;
 
364
        v[0] -= a.v[0]; v[1] -= a.v[1];
 
365
        return *this;
353
366
}
354
367
 
355
368
template<class T> Vector2<T>& Vector2<T>::operator*=(T s)
356
369
{
357
 
    v[0] *= s; v[1] *= s;
358
 
    return *this;
 
370
        v[0] *= s; v[1] *= s;
 
371
        return *this;
359
372
}
360
373
 
361
374
template<class T> Vector2<T> Vector2<T>::operator-() const
362
375
{
363
 
    return Vector2<T>(-v[0], -v[1]);
 
376
        return Vector2<T>(-v[0], -v[1]);
364
377
}
365
378
 
366
379
template<class T> Vector2<T> Vector2<T>::operator+() const
367
380
{
368
 
    return *this;
 
381
        return *this;
369
382
}
370
383
 
371
384
template<class T> Vector2<T> Vector2<T>::operator+(const Vector2<T>& b) const
372
385
{
373
 
    return Vector2<T>(v[0] + b.v[0], v[1] + b.v[1]);
 
386
        return Vector2<T>(v[0] + b.v[0], v[1] + b.v[1]);
374
387
}
375
388
 
376
389
template<class T> Vector2<T> Vector2<T>::operator-(const Vector2<T>& b) const
377
390
{
378
 
    return Vector2<T>(v[0] - b.v[0], v[1] - b.v[1]);
 
391
        return Vector2<T>(v[0] - b.v[0], v[1] - b.v[1]);
379
392
}
380
393
 
381
394
template<class T> Vector2<T> Vector2<T>::operator*(T s) const
382
395
{
383
 
    return Vector2<T>(s * v[0], s * v[1]);
 
396
        return Vector2<T>(s * v[0], s * v[1]);
384
397
}
385
398
 
386
399
template<class T> Vector2<T> Vector2<T>::operator/(T s) const
387
400
{
388
 
    return Vector2<T>(v[0]/s, v[1]/s);
 
401
        return Vector2<T>(v[0]/s, v[1]/s);
389
402
}
390
403
 
391
404
 
392
405
template<class T> T Vector2<T>::dot(const Vector2<T>& b) const
393
406
{
394
 
    return v[0] * b.v[0] + v[1] * b.v[1];
 
407
        return v[0] * b.v[0] + v[1] * b.v[1];
395
408
}
396
409
 
397
410
 
398
411
template<class T> T Vector2<T>::length() const
399
412
{
400
 
    return (T) std::sqrt(v[0] * v[0] + v[1] * v[1]);
 
413
        return (T) std::sqrt(v[0] * v[0] + v[1] * v[1]);
401
414
}
402
415
 
403
416
template<class T> T Vector2<T>::lengthSquared() const
404
417
{
405
 
    return v[0] * v[0] + v[1] * v[1];
 
418
        return v[0] * v[0] + v[1] * v[1];
406
419
}
407
420
 
408
421
template<class T> void Vector2<T>::normalize()
409
422
{
410
 
    T s = (T) 1 / std::sqrt(v[0] * v[0] + v[1] * v[1]);
411
 
    v[0] *= s;
412
 
    v[1] *= s;
 
423
        T s = (T) 1 / std::sqrt(v[0] * v[0] + v[1] * v[1]);
 
424
        v[0] *= s;
 
425
        v[1] *= s;
413
426
}
414
427
 
415
 
// template<class T> 
 
428
// template<class T>
416
429
// std::ostream& operator<<(std::ostream &o,const Vector2<T> &v) {
417
430
//   return o << '[' << v[0] << ',' << v[1] << ']';
418
431
// }
444
457
template<class T> Vector3<T>& Vector3<T>::operator=(const Vector3& a)
445
458
{
446
459
        v[0]=a.v[0]; v[1]=a.v[1]; v[2]=a.v[2];
447
 
    return *this;
 
460
        return *this;
448
461
}
449
462
 
450
463
template<class T> template <class T2> Vector3<T>& Vector3<T>::operator=(const Vector3<T2>& a)
451
464
{
452
465
        v[0]=a.v[0]; v[1]=a.v[1]; v[2]=a.v[2];
453
 
    return *this;
 
466
        return *this;
454
467
}
455
468
 
456
469
template<class T> Vector3<T>& Vector3<T>::operator=(const T* a)
457
470
{
458
471
        v[0]=a[0]; v[1]=a[1]; v[2]=a[2];
459
 
    return *this;
 
472
        return *this;
460
473
}
461
474
 
462
475
template<class T> void Vector3<T>::set(T x, T y, T z)
498
511
 
499
512
template<class T> Vector3<T>& Vector3<T>::operator+=(const Vector3<T>& a)
500
513
{
501
 
    v[0] += a.v[0]; v[1] += a.v[1]; v[2] += a.v[2];
502
 
    return *this;
 
514
        v[0] += a.v[0]; v[1] += a.v[1]; v[2] += a.v[2];
 
515
        return *this;
503
516
}
504
517
 
505
518
template<class T> Vector3<T>& Vector3<T>::operator-=(const Vector3<T>& a)
506
519
{
507
 
    v[0] -= a.v[0]; v[1] -= a.v[1]; v[2] -= a.v[2];
508
 
    return *this;
 
520
        v[0] -= a.v[0]; v[1] -= a.v[1]; v[2] -= a.v[2];
 
521
        return *this;
509
522
}
510
523
 
511
524
template<class T> Vector3<T>& Vector3<T>::operator*=(T s)
512
525
{
513
 
    v[0] *= s; v[1] *= s; v[2] *= s;
514
 
    return *this;
 
526
        v[0] *= s; v[1] *= s; v[2] *= s;
 
527
        return *this;
515
528
}
516
529
 
517
530
template<class T> Vector3<T>& Vector3<T>::operator/=(T s)
522
535
 
523
536
template<class T> Vector3<T> Vector3<T>::operator-() const
524
537
{
525
 
    return Vector3<T>(-v[0], -v[1], -v[2]);
 
538
        return Vector3<T>(-v[0], -v[1], -v[2]);
526
539
}
527
540
 
528
541
template<class T> Vector3<T> Vector3<T>::operator+() const
529
542
{
530
 
    return *this;
 
543
        return *this;
531
544
}
532
545
 
533
546
template<class T> Vector3<T> Vector3<T>::operator+(const Vector3<T>& b) const
534
547
{
535
 
    return Vector3<T>(v[0] + b.v[0], v[1] + b.v[1], v[2] + b.v[2]);
 
548
        return Vector3<T>(v[0] + b.v[0], v[1] + b.v[1], v[2] + b.v[2]);
536
549
}
537
550
 
538
551
template<class T> Vector3<T> Vector3<T>::operator-(const Vector3<T>& b) const
539
552
{
540
 
    return Vector3<T>(v[0] - b.v[0], v[1] - b.v[1], v[2] - b.v[2]);
 
553
        return Vector3<T>(v[0] - b.v[0], v[1] - b.v[1], v[2] - b.v[2]);
541
554
}
542
555
 
543
556
template<class T> Vector3<T> Vector3<T>::operator*(T s) const
544
557
{
545
 
    return Vector3<T>(s * v[0], s * v[1], s * v[2]);
 
558
        return Vector3<T>(s * v[0], s * v[1], s * v[2]);
546
559
}
547
560
 
548
561
template<class T> Vector3<T> Vector3<T>::operator/(T s) const
549
562
{
550
 
    return Vector3<T>(v[0]/s, v[1]/s, v[2]/s);
 
563
        return Vector3<T>(v[0]/s, v[1]/s, v[2]/s);
551
564
}
552
565
 
553
566
 
554
567
template<class T> T Vector3<T>::dot(const Vector3<T>& b) const
555
568
{
556
 
    return v[0] * b.v[0] + v[1] * b.v[1] + v[2] * b.v[2];
 
569
        return v[0] * b.v[0] + v[1] * b.v[1] + v[2] * b.v[2];
557
570
}
558
571
 
559
572
 
560
573
// cross product
561
574
template<class T> Vector3<T> Vector3<T>::operator^(const Vector3<T>& b) const
562
575
{
563
 
    return Vector3<T>(v[1] * b.v[2] - v[2] * b.v[1],
564
 
                      v[2] * b.v[0] - v[0] * b.v[2],
565
 
                      v[0] * b.v[1] - v[1] * b.v[0]);
 
576
        return Vector3<T>(v[1] * b.v[2] - v[2] * b.v[1],
 
577
                                          v[2] * b.v[0] - v[0] * b.v[2],
 
578
                                          v[0] * b.v[1] - v[1] * b.v[0]);
566
579
}
567
580
 
568
581
// Angle in radian between two normalized vectors
573
586
 
574
587
template<class T> T Vector3<T>::length() const
575
588
{
576
 
    return (T) sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
 
589
        return (T) sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
577
590
}
578
591
 
579
592
template<class T> T Vector3<T>::lengthSquared() const
580
593
{
581
 
    return v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
 
594
        return v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
582
595
}
583
596
 
584
597
template<class T> void Vector3<T>::normalize()
585
598
{
586
 
    T s = (T) (1. / std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]));
587
 
    v[0] *= s;
588
 
    v[1] *= s;
589
 
    v[2] *= s;
 
599
        T s = (T) (1. / std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]));
 
600
        v[0] *= s;
 
601
        v[1] *= s;
 
602
        v[2] *= s;
590
603
}
591
604
 
592
605
template<class T> void Vector3<T>::transfo4d(const Mat4d& m)
593
606
{
594
 
        (*this)=m*(*this);
 
607
        const T v0 = v[0];
 
608
        const T v1 = v[1];
 
609
        v[0]=m.r[0]*v0 + m.r[4]*v1 + m.r[8]*v[2] + m.r[12];
 
610
        v[1]=m.r[1]*v0 + m.r[5]*v1 +  m.r[9]*v[2] + m.r[13];
 
611
        v[2]=m.r[2]*v0 + m.r[6]*v1 + m.r[10]*v[2] + m.r[14];
595
612
}
596
613
 
597
614
template<class T> void Vector3<T>::transfo4d(const Mat4f& m)
598
615
{
599
 
        (*this)=m*(*this);
 
616
        const T v0 = v[0];
 
617
        const T v1 = v[1];
 
618
        v[0]=m.r[0]*v0 + m.r[4]*v1 + m.r[8]*v[2] + m.r[12];
 
619
        v[1]=m.r[1]*v0 + m.r[5]*v1 +  m.r[9]*v[2] + m.r[13];
 
620
        v[2]=m.r[2]*v0 + m.r[6]*v1 + m.r[10]*v[2] + m.r[14];
600
621
}
601
622
 
602
623
// Return latitude in rad
611
632
        return std::atan2(v[1],v[0]);
612
633
}
613
634
 
614
 
// template<class T> 
615
 
// std::ostream& operator<<(std::ostream &o,const Vector3<T> &v) {
616
 
//   return o << '[' << v[0] << ',' << v[1] << ',' << v[2] << ']';
617
 
// }
618
635
 
619
 
// template<class T> 
620
 
// std::istream& operator>> (std::istream& is, Vector3<T> &v) {
621
 
//      while(is.get()!='[' && !is.eof()) {;}
622
 
//      assert(!is.eof() && "Vector must start with a '['");
623
 
//      is >> v[0];
624
 
//      is.ignore(256, ',');
625
 
//      is >> v[1];
626
 
//      is.ignore(256, ',');
627
 
//      is >> v[2];
628
 
//      while(is.get()!=']' && !is.eof()) {;}
629
 
//      assert(!is.eof() && "Vector must be terminated by a ']'");
630
 
//      return is;
631
 
// }
632
 
                
633
636
////////////////////////// Vector4 class methods ///////////////////////////////
634
637
 
635
638
template<class T> Vector4<T>::Vector4() {}
657
660
template<class T> Vector4<T>& Vector4<T>::operator=(const Vector4<T>& a)
658
661
{
659
662
        v[0]=a.v[0]; v[1]=a.v[1]; v[2]=a.v[2]; v[3]=a.v[3];
660
 
    return *this;
 
663
        return *this;
661
664
}
662
665
 
663
666
template<class T> Vector4<T>& Vector4<T>::operator=(const Vector3<T>& a)
664
667
{
665
668
        v[0]=a.v[0]; v[1]=a.v[1]; v[2]=a.v[2]; v[3]=1;
666
 
    return *this;
 
669
        return *this;
667
670
}
668
671
 
669
672
template<class T> Vector4<T>& Vector4<T>::operator=(const T* a)
670
673
{
671
674
        v[0]=a[0]; v[1]=a[1]; v[2]=a[2]; v[3]=a[3];
672
 
    return *this;
 
675
        return *this;
673
676
}
674
677
 
675
678
template<class T> void Vector4<T>::set(T x, T y, T z, T a)
709
712
 
710
713
template<class T> Vector4<T>& Vector4<T>::operator+=(const Vector4<T>& a)
711
714
{
712
 
    v[0] += a.v[0]; v[1] += a.v[1]; v[2] += a.v[2]; v[3] += a.v[3];
713
 
    return *this;
 
715
        v[0] += a.v[0]; v[1] += a.v[1]; v[2] += a.v[2]; v[3] += a.v[3];
 
716
        return *this;
714
717
}
715
718
 
716
719
template<class T> Vector4<T>& Vector4<T>::operator-=(const Vector4<T>& a)
717
720
{
718
 
    v[0] -= a.v[0]; v[1] -= a.v[1]; v[2] -= a.v[2]; v[3] -= a/v[3];
719
 
    return *this;
 
721
        v[0] -= a.v[0]; v[1] -= a.v[1]; v[2] -= a.v[2]; v[3] -= a/v[3];
 
722
        return *this;
720
723
}
721
724
 
722
725
template<class T> Vector4<T>& Vector4<T>::operator*=(T s)
723
726
{
724
 
    v[0] *= s; v[1] *= s; v[2] *= s; v[3] *= s;
725
 
    return *this;
 
727
        v[0] *= s; v[1] *= s; v[2] *= s; v[3] *= s;
 
728
        return *this;
726
729
}
727
730
 
728
731
template<class T> Vector4<T> Vector4<T>::operator-() const
729
732
{
730
 
    return Vector4<T>(-v[0], -v[1], -v[2], -v[3]);
 
733
        return Vector4<T>(-v[0], -v[1], -v[2], -v[3]);
731
734
}
732
735
 
733
736
template<class T> Vector4<T> Vector4<T>::operator+() const
734
737
{
735
 
    return *this;
 
738
        return *this;
736
739
}
737
740
 
738
741
template<class T> Vector4<T> Vector4<T>::operator+(const Vector4<T>& b) const
739
742
{
740
 
    return Vector4<T>(v[0] + b.v[0], v[1] + b.v[1], v[2] + b.v[2], v[3] + b.v[3]);
 
743
        return Vector4<T>(v[0] + b.v[0], v[1] + b.v[1], v[2] + b.v[2], v[3] + b.v[3]);
741
744
}
742
745
 
743
746
template<class T> Vector4<T> Vector4<T>::operator-(const Vector4<T>& b) const
744
747
{
745
 
    return Vector4<T>(v[0] - b.v[0], v[1] - b.v[1], v[2] - b.v[2], v[3] - b.v[3]);
 
748
        return Vector4<T>(v[0] - b.v[0], v[1] - b.v[1], v[2] - b.v[2], v[3] - b.v[3]);
746
749
}
747
750
 
748
751
template<class T> Vector4<T> Vector4<T>::operator*(T s) const
749
752
{
750
 
    return Vector4<T>(s * v[0], s * v[1], s * v[2], s * v[3]);
 
753
        return Vector4<T>(s * v[0], s * v[1], s * v[2], s * v[3]);
751
754
}
752
755
 
753
756
template<class T> Vector4<T> Vector4<T>::operator/(T s) const
754
757
{
755
 
    return Vector4<T>(v[0]/s, v[1]/s, v[2]/s, v[3]/s);
 
758
        return Vector4<T>(v[0]/s, v[1]/s, v[2]/s, v[3]/s);
756
759
}
757
760
 
758
761
template<class T> T Vector4<T>::dot(const Vector4<T>& b) const
759
762
{
760
 
    return v[0] * b.v[0] + v[1] * b.v[1] + v[2] * b.v[2] + v[3] * b.v[3];
 
763
        return v[0] * b.v[0] + v[1] * b.v[1] + v[2] * b.v[2] + v[3] * b.v[3];
761
764
}
762
765
 
763
766
template<class T> T Vector4<T>::length() const
764
767
{
765
 
    return (T) sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]);
 
768
        return (T) sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]);
766
769
}
767
770
 
768
771
template<class T> T Vector4<T>::lengthSquared() const
769
772
{
770
 
    return v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3];
 
773
        return v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3];
771
774
}
772
775
 
773
776
template<class T> void Vector4<T>::normalize()
774
777
{
775
 
    T s = (T) (1. / sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]));
776
 
    v[0] *= s;
777
 
    v[1] *= s;
778
 
    v[2] *= s;
 
778
        T s = (T) (1. / sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]));
 
779
        v[0] *= s;
 
780
        v[1] *= s;
 
781
        v[2] *= s;
779
782
        v[3] *= s;
780
783
}
781
784
 
784
787
        (*this)=m*(*this);
785
788
}
786
789
/*
787
 
template<class T> 
 
790
template<class T>
788
791
std::ostream& operator<<(std::ostream &o,const Vector4<T> &v) {
789
792
  return o << '[' << v[0] << ',' << v[1] << ',' << v[2] << ',' << v[3] << ']';
790
793
}*/
811
814
}
812
815
 
813
816
template<class T> Matrix4<T>::Matrix4(const Vector4<T>& v0,
814
 
                                      const Vector4<T>& v1,
815
 
                                      const Vector4<T>& v2,
816
 
                                      const Vector4<T>& v3)
 
817
                                                                          const Vector4<T>& v1,
 
818
                                                                          const Vector4<T>& v2,
 
819
                                                                          const Vector4<T>& v3)
817
820
{
818
 
    r[0] = v0.v[0];
819
 
    r[1] = v0.v[1];
820
 
    r[2] = v0.v[2];
821
 
    r[3] = v0.v[3];
822
 
    r[4] = v1.v[0];
823
 
    r[5] = v1.v[1];
824
 
    r[6] = v1.v[2];
825
 
    r[7] = v1.v[3];
826
 
    r[8] = v2.v[0];
827
 
    r[9] = v2.v[1];
828
 
    r[10] = v2.v[2];
829
 
    r[11] = v2.v[3];
830
 
    r[12] = v3.v[0];
831
 
    r[13] = v3.v[1];
832
 
    r[14] = v3.v[2];
833
 
    r[15] = v3.v[3];
 
821
        r[0] = v0.v[0];
 
822
        r[1] = v0.v[1];
 
823
        r[2] = v0.v[2];
 
824
        r[3] = v0.v[3];
 
825
        r[4] = v1.v[0];
 
826
        r[5] = v1.v[1];
 
827
        r[6] = v1.v[2];
 
828
        r[7] = v1.v[3];
 
829
        r[8] = v2.v[0];
 
830
        r[9] = v2.v[1];
 
831
        r[10] = v2.v[2];
 
832
        r[11] = v2.v[3];
 
833
        r[12] = v3.v[0];
 
834
        r[13] = v3.v[1];
 
835
        r[14] = v3.v[2];
 
836
        r[15] = v3.v[3];
834
837
}
835
838
 
836
839
 
846
849
        r[8]=i; r[9]=j; r[10]=k; r[11]=l; r[12]=m; r[13]=n; r[14]=o; r[15]=p;
847
850
}
848
851
 
849
 
template<class T> T* Matrix4<T>::operator[](int n)
 
852
template<class T> T& Matrix4<T>::operator[](int n)
850
853
{
851
 
    return &(r[n*4]);
 
854
        return r[n];
852
855
}
853
856
 
854
857
template<class T> Matrix4<T>::operator T*()
863
866
 
864
867
template<class T> Matrix4<T> Matrix4<T>::identity()
865
868
{
866
 
    return Matrix4<T>(  1, 0, 0, 0,
 
869
        return Matrix4<T>(      1, 0, 0, 0,
867
870
                                                0, 1, 0, 0,
868
871
                                                0, 0, 1, 0,
869
872
                                                0, 0, 0, 1  );
872
875
 
873
876
template<class T> Matrix4<T> Matrix4<T>::translation(const Vector3<T>& a)
874
877
{
875
 
    return Matrix4<T>(  1, 0, 0, 0,
 
878
        return Matrix4<T>(      1, 0, 0, 0,
876
879
                                                0, 1, 0, 0,
877
880
                                                0, 0, 1, 0,
878
881
                                                a.v[0], a.v[1], a.v[2], 1);
879
882
}
880
883
 
881
 
template<class T> Matrix4<T> Matrix4<T>::rotation(const Vector3<T>& a,
882
 
                                                  T angle)
883
 
{
884
 
        Vec3d axis(a);
885
 
        axis.normalize();
886
 
//    T c = (T) cos(angle);
887
 
//    T s = (T) sin(angle);
888
 
//    T t = 1 - c;
889
 
//
890
 
//    return Matrix4<T>(Vector4<T>(t * axis.v[0] * axis.v[0] + c,
891
 
//                                 t * axis.v[0] * axis.v[1] - s * axis.v[2],
892
 
//                                 t * axis.v[0] * axis.v[2] + s * axis.v[1],
893
 
//                                 0),
894
 
//                      Vector4<T>(t * axis.v[0] * axis.v[1] + s * axis.v[2],
895
 
//                                 t * axis.v[1] * axis.v[1] + c,
896
 
//                                 t * axis.v[1] * axis.v[2] - s * axis.v[0],
897
 
//                                 0),
898
 
//                      Vector4<T>(t * axis.v[0] * axis.v[2] - s * axis.v[1],
899
 
//                                 t * axis.v[1] * axis.v[2] + s * axis.v[0],
900
 
//                                 t * axis.v[2] * axis.v[2] + c,
901
 
//                                 0),
902
 
//                      Vector4<T>(0, 0, 0, 1));
903
 
                      
904
 
        T sin_a = std::sin( angle / 2 );
905
 
        T cos_a = std::cos( angle / 2 );
906
 
        T X    = axis[0] * sin_a;
907
 
        T Y    = axis[1] * sin_a;
908
 
        T Z    = axis[2] * sin_a;
909
 
        T W    = cos_a;
910
 
        
911
 
        T xx      = X * X;
912
 
        T xy      = X * Y;
913
 
        T xz      = X * Z;
914
 
        T xw      = X * W;
915
 
        
916
 
        T yy      = Y * Y;
917
 
        T yz      = Y * Z;
918
 
        T yw      = Y * W;
919
 
        
920
 
        T zz      = Z * Z;
921
 
        T zw      = Z * W;
922
 
 
923
 
        return Matrix4<T>(
924
 
        1. - 2. * ( yy + zz ), 2. * ( xy + zw ),      2. * ( xz - yw ),      0.,
925
 
        2. * ( xy - zw ),      1. - 2. * ( xx + zz ), 2. * ( yz + xw ),      0., 
926
 
        2. * ( xz + yw ),      2. * ( yz - xw ),      1. - 2. * ( xx + yy ), 0., 
927
 
        0.,                    0.,                    0.,                    1.);
928
 
}
929
 
 
930
 
 
931
 
 
932
 
/*
933
 
template<class T> Matrix4<T> Matrix4<T>::rotation(const Vector3<T>& a)
934
 
{
935
 
    T c = (T) cos(angle);
936
 
    T s = (T) sin(angle);
937
 
        T d = 1-c;
938
 
         return Matrix4<T>(     a.v[0]*a.v[0]*d+c, a.v[1]*a.v[0]*d+a.v[2]*s, a.v[0]*a.v[2]*d-a.v[1]*s, 0,
939
 
                                                a.v[0]*a.v[1]*d-a.v[2]*s, a.v[1]*a.v[1]*d+c, a.v[1]*a.v[2]*d+a.v[0]*s, 0,
940
 
                                                a.v[0]*a.v[2]*d+a.v[1]*s, a.v[1]*a.v[2]*d-a.v[0]*s, a.v[2]*a.v[2]*d+c, 0,
 
884
 
 
885
template<class T> Matrix4<T> Matrix4<T>::rotation(const Vector3<T>& axis, T angle)
 
886
{
 
887
        Vec3d a(axis);
 
888
        a.normalize();
 
889
        const T c = (T) cos(angle);
 
890
        const T s = (T) sin(angle);
 
891
        const T d = 1-c;
 
892
        return Matrix4<T>(      a[0]*a[0]*d+c     , a[1]*a[0]*d+a[2]*s, a[0]*a[2]*d-a[1]*s, 0,
 
893
                                                a[0]*a[1]*d-a[2]*s, a[1]*a[1]*d+c     , a[1]*a[2]*d+a[0]*s, 0,
 
894
                                                a[0]*a[2]*d+a[1]*s, a[1]*a[2]*d-a[0]*s, a[2]*a[2]*d+c     , 0,
941
895
                                                0,0,0,1 );
942
896
}
943
 
*/
 
897
 
944
898
template<class T> Matrix4<T> Matrix4<T>::xrotation(T angle)
945
899
{
946
 
    T c = (T) cos(angle);
947
 
    T s = (T) sin(angle);
 
900
        T c = (T) cos(angle);
 
901
        T s = (T) sin(angle);
948
902
 
949
 
    return Matrix4<T>(1, 0, 0, 0,
950
 
                      0, c, s, 0,
951
 
                      0,-s, c, 0,
952
 
                      0, 0, 0, 1 );
 
903
        return Matrix4<T>(1, 0, 0, 0,
 
904
                                          0, c, s, 0,
 
905
                                          0,-s, c, 0,
 
906
                                          0, 0, 0, 1 );
953
907
}
954
908
 
955
909
 
956
910
template<class T> Matrix4<T> Matrix4<T>::yrotation(T angle)
957
911
{
958
 
    T c = (T) cos(angle);
959
 
    T s = (T) sin(angle);
 
912
        T c = (T) cos(angle);
 
913
        T s = (T) sin(angle);
960
914
 
961
 
    return Matrix4<T>( c, 0,-s, 0,
962
 
                       0, 1, 0, 0,
963
 
                       s, 0, c, 0,
964
 
                       0, 0, 0, 1 );
 
915
        return Matrix4<T>( c, 0,-s, 0,
 
916
                                           0, 1, 0, 0,
 
917
                                           s, 0, c, 0,
 
918
                                           0, 0, 0, 1 );
965
919
}
966
920
 
967
921
 
968
922
template<class T> Matrix4<T> Matrix4<T>::zrotation(T angle)
969
923
{
970
 
    T c = (T) cos(angle);
971
 
    T s = (T) sin(angle);
 
924
        T c = (T) cos(angle);
 
925
        T s = (T) sin(angle);
972
926
 
973
 
    return Matrix4<T>(c, s, 0, 0,
974
 
                     -s, c, 0, 0,
975
 
                      0, 0, 1, 0,
976
 
                      0, 0, 0, 1 );
 
927
        return Matrix4<T>(c, s, 0, 0,
 
928
                                         -s, c, 0, 0,
 
929
                                          0, 0, 1, 0,
 
930
                                          0, 0, 0, 1 );
977
931
}
978
932
 
979
933
 
980
934
template<class T> Matrix4<T> Matrix4<T>::scaling(const Vector3<T>& s)
981
935
{
982
 
    return Matrix4<T>(s[0], 0  , 0  , 0,
983
 
                      0   ,s[1], 0  , 0,
984
 
                      0   , 0  ,s[2], 0,
985
 
                      0   , 0  , 0  , 1);
 
936
        return Matrix4<T>(s[0], 0  , 0  , 0,
 
937
                                          0   ,s[1], 0  , 0,
 
938
                                          0   , 0  ,s[2], 0,
 
939
                                          0   , 0  , 0  , 1);
986
940
}
987
941
 
988
942
 
989
943
template<class T> Matrix4<T> Matrix4<T>::scaling(T scale)
990
944
{
991
 
    return scaling(Vector3<T>(scale, scale, scale));
 
945
        return scaling(Vector3<T>(scale, scale, scale));
992
946
}
993
947
 
994
948
// multiply column vector by a 4x4 matrix in homogeneous coordinate (use a[3]=1)
995
949
template<class T> Vector3<T> Matrix4<T>::operator*(const Vector3<T>& a) const
996
950
{
997
 
    return Vector3<T>(  r[0]*a.v[0] + r[4]*a.v[1] +  r[8]*a.v[2] + r[12],
 
951
        return Vector3<T>(      r[0]*a.v[0] + r[4]*a.v[1] +  r[8]*a.v[2] + r[12],
998
952
                                                r[1]*a.v[0] + r[5]*a.v[1] +  r[9]*a.v[2] + r[13],
999
953
                                                r[2]*a.v[0] + r[6]*a.v[1] + r[10]*a.v[2] + r[14] );
1000
954
}
1001
955
 
1002
956
template<class T> Vector3<T> Matrix4<T>::multiplyWithoutTranslation(const Vector3<T>& a) const
1003
957
{
1004
 
    return Vector3<T>(  r[0]*a.v[0] + r[4]*a.v[1] +  r[8]*a.v[2],
 
958
        return Vector3<T>(      r[0]*a.v[0] + r[4]*a.v[1] +  r[8]*a.v[2],
1005
959
                                                r[1]*a.v[0] + r[5]*a.v[1] +  r[9]*a.v[2],
1006
960
                                                r[2]*a.v[0] + r[6]*a.v[1] + r[10]*a.v[2] );
1007
961
}
1009
963
// multiply column vector by a 4x4 matrix in homogeneous coordinate (considere a[3]=1)
1010
964
template<class T> Vector4<T> Matrix4<T>::operator*(const Vector4<T>& a) const
1011
965
{
1012
 
    return Vector4<T>(  r[0]*a.v[0] + r[4]*a.v[1] +  r[8]*a.v[2] + r[12]*a.v[3],
 
966
        return Vector4<T>(      r[0]*a.v[0] + r[4]*a.v[1] +  r[8]*a.v[2] + r[12]*a.v[3],
1013
967
                                                r[1]*a.v[0] + r[5]*a.v[1] +  r[9]*a.v[2] + r[13]*a.v[3],
1014
968
                                                r[2]*a.v[0] + r[6]*a.v[1] + r[10]*a.v[2] + r[14]*a.v[3] );
1015
969
}
1023
977
 
1024
978
template<class T> Matrix4<T> Matrix4<T>::transpose() const
1025
979
{
1026
 
    return Matrix4<T>(  r[0], r[4], r[8],  r[12],
 
980
        return Matrix4<T>(      r[0], r[4], r[8],  r[12],
1027
981
                                                r[1], r[5], r[9],  r[13],
1028
982
                                                r[2], r[6], r[10], r[14],
1029
983
                                                r[3], r[7], r[11], r[15]);
1032
986
template<class T> Matrix4<T> Matrix4<T>::operator*(const Matrix4<T>& a) const
1033
987
{
1034
988
#define MATMUL(R, C) (r[R] * a.r[C] + r[R+4] * a.r[C+1] + r[R+8] * a.r[C+2] + r[R+12] * a.r[C+3])
1035
 
    return Matrix4<T>(  MATMUL(0,0), MATMUL(1,0), MATMUL(2,0), MATMUL(3,0),
 
989
        return Matrix4<T>(      MATMUL(0,0), MATMUL(1,0), MATMUL(2,0), MATMUL(3,0),
1036
990
                                                MATMUL(0,4), MATMUL(1,4), MATMUL(2,4), MATMUL(3,4),
1037
991
                                                MATMUL(0,8), MATMUL(1,8), MATMUL(2,8), MATMUL(3,8),
1038
992
                                                MATMUL(0,12), MATMUL(1,12), MATMUL(2,12), MATMUL(3,12) );
1042
996
 
1043
997
template<class T> Matrix4<T> Matrix4<T>::operator+(const Matrix4<T>& a) const
1044
998
{
1045
 
    return Matrix4<T>(  r[0]+a.r[0], r[1]+a.r[1], r[2]+a.r[2], r[3]+a.r[3],
 
999
        return Matrix4<T>(      r[0]+a.r[0], r[1]+a.r[1], r[2]+a.r[2], r[3]+a.r[3],
1046
1000
                                                r[4]+a.r[4], r[5]+a.r[5], r[6]+a.r[6], r[7]+a.r[7],
1047
1001
                                                r[8]+a.r[8], r[9]+a.r[9], r[10]+a.r[10], r[11]+a.r[11],
1048
1002
                                                r[12]+a.r[12], r[13]+a.r[13], r[14]+a.r[14], r[15]+a.r[15] );
1050
1004
 
1051
1005
template<class T> Matrix4<T> Matrix4<T>::operator-(const Matrix4<T>& a) const
1052
1006
{
1053
 
    return Matrix4<T>(  r[0]-a.r[0], r[1]-a.r[1], r[2]-a.r[2], r[3]-a.r[3],
 
1007
        return Matrix4<T>(      r[0]-a.r[0], r[1]-a.r[1], r[2]-a.r[2], r[3]-a.r[3],
1054
1008
                                                r[4]-a.r[4], r[5]-a.r[5], r[6]-a.r[6], r[7]-a.r[7],
1055
1009
                                                r[8]-a.r[8], r[9]-a.r[9], r[10]-a.r[10], r[11]-a.r[11],
1056
1010
                                                r[12]-a.r[12], r[13]-a.r[13], r[14]-a.r[14], r[15]-a.r[15] );
1078
1032
   r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];
1079
1033
 
1080
1034
   r0[0] = MAT(m, 0, 0), r0[1] = MAT(m, 0, 1),
1081
 
      r0[2] = MAT(m, 0, 2), r0[3] = MAT(m, 0, 3),
1082
 
      r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0,
1083
 
      r1[0] = MAT(m, 1, 0), r1[1] = MAT(m, 1, 1),
1084
 
      r1[2] = MAT(m, 1, 2), r1[3] = MAT(m, 1, 3),
1085
 
      r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0,
1086
 
      r2[0] = MAT(m, 2, 0), r2[1] = MAT(m, 2, 1),
1087
 
      r2[2] = MAT(m, 2, 2), r2[3] = MAT(m, 2, 3),
1088
 
      r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0,
1089
 
      r3[0] = MAT(m, 3, 0), r3[1] = MAT(m, 3, 1),
1090
 
      r3[2] = MAT(m, 3, 2), r3[3] = MAT(m, 3, 3),
1091
 
      r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0;
 
1035
          r0[2] = MAT(m, 0, 2), r0[3] = MAT(m, 0, 3),
 
1036
          r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0,
 
1037
          r1[0] = MAT(m, 1, 0), r1[1] = MAT(m, 1, 1),
 
1038
          r1[2] = MAT(m, 1, 2), r1[3] = MAT(m, 1, 3),
 
1039
          r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0,
 
1040
          r2[0] = MAT(m, 2, 0), r2[1] = MAT(m, 2, 1),
 
1041
          r2[2] = MAT(m, 2, 2), r2[3] = MAT(m, 2, 3),
 
1042
          r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0,
 
1043
          r3[0] = MAT(m, 3, 0), r3[1] = MAT(m, 3, 1),
 
1044
          r3[2] = MAT(m, 3, 2), r3[3] = MAT(m, 3, 3),
 
1045
          r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0;
1092
1046
 
1093
1047
   /* choose pivot - or die */
1094
1048
   if (fabs(r3[0]) > fabs(r2[0]))
1095
 
      SWAP_ROWS(r3, r2);
 
1049
          SWAP_ROWS(r3, r2);
1096
1050
   if (fabs(r2[0]) > fabs(r1[0]))
1097
 
      SWAP_ROWS(r2, r1);
 
1051
          SWAP_ROWS(r2, r1);
1098
1052
   if (fabs(r1[0]) > fabs(r0[0]))
1099
 
      SWAP_ROWS(r1, r0);
 
1053
          SWAP_ROWS(r1, r0);
1100
1054
   if (0.0 == r0[0])
1101
 
      return Matrix4<T>();
 
1055
          return Matrix4<T>();
1102
1056
 
1103
1057
   /* eliminate first variable     */
1104
1058
   m1 = r1[0] / r0[0];
1118
1072
   r3[3] -= m3 * s;
1119
1073
   s = r0[4];
1120
1074
   if (s != 0.0) {
1121
 
      r1[4] -= m1 * s;
1122
 
      r2[4] -= m2 * s;
1123
 
      r3[4] -= m3 * s;
 
1075
          r1[4] -= m1 * s;
 
1076
          r2[4] -= m2 * s;
 
1077
          r3[4] -= m3 * s;
1124
1078
   }
1125
1079
   s = r0[5];
1126
1080
   if (s != 0.0) {
1127
 
      r1[5] -= m1 * s;
1128
 
      r2[5] -= m2 * s;
1129
 
      r3[5] -= m3 * s;
 
1081
          r1[5] -= m1 * s;
 
1082
          r2[5] -= m2 * s;
 
1083
          r3[5] -= m3 * s;
1130
1084
   }
1131
1085
   s = r0[6];
1132
1086
   if (s != 0.0) {
1133
 
      r1[6] -= m1 * s;
1134
 
      r2[6] -= m2 * s;
1135
 
      r3[6] -= m3 * s;
 
1087
          r1[6] -= m1 * s;
 
1088
          r2[6] -= m2 * s;
 
1089
          r3[6] -= m3 * s;
1136
1090
   }
1137
1091
   s = r0[7];
1138
1092
   if (s != 0.0) {
1139
 
      r1[7] -= m1 * s;
1140
 
      r2[7] -= m2 * s;
1141
 
      r3[7] -= m3 * s;
 
1093
          r1[7] -= m1 * s;
 
1094
          r2[7] -= m2 * s;
 
1095
          r3[7] -= m3 * s;
1142
1096
   }
1143
1097
 
1144
1098
   /* choose pivot - or die */
1145
1099
   if (fabs(r3[1]) > fabs(r2[1]))
1146
 
      SWAP_ROWS(r3, r2);
 
1100
          SWAP_ROWS(r3, r2);
1147
1101
   if (fabs(r2[1]) > fabs(r1[1]))
1148
 
      SWAP_ROWS(r2, r1);
 
1102
          SWAP_ROWS(r2, r1);
1149
1103
   if (0.0 == r1[1])
1150
 
      return Matrix4<T>();
 
1104
          return Matrix4<T>();
1151
1105
 
1152
1106
   /* eliminate second variable */
1153
1107
   m2 = r2[1] / r1[1];
1158
1112
   r3[3] -= m3 * r1[3];
1159
1113
   s = r1[4];
1160
1114
   if (0.0 != s) {
1161
 
      r2[4] -= m2 * s;
1162
 
      r3[4] -= m3 * s;
 
1115
          r2[4] -= m2 * s;
 
1116
          r3[4] -= m3 * s;
1163
1117
   }
1164
1118
   s = r1[5];
1165
1119
   if (0.0 != s) {
1166
 
      r2[5] -= m2 * s;
1167
 
      r3[5] -= m3 * s;
 
1120
          r2[5] -= m2 * s;
 
1121
          r3[5] -= m3 * s;
1168
1122
   }
1169
1123
   s = r1[6];
1170
1124
   if (0.0 != s) {
1171
 
      r2[6] -= m2 * s;
1172
 
      r3[6] -= m3 * s;
 
1125
          r2[6] -= m2 * s;
 
1126
          r3[6] -= m3 * s;
1173
1127
   }
1174
1128
   s = r1[7];
1175
1129
   if (0.0 != s) {
1176
 
      r2[7] -= m2 * s;
1177
 
      r3[7] -= m3 * s;
 
1130
          r2[7] -= m2 * s;
 
1131
          r3[7] -= m3 * s;
1178
1132
   }
1179
1133
 
1180
1134
   /* choose pivot - or die */
1181
1135
   if (fabs(r3[2]) > fabs(r2[2]))
1182
 
      SWAP_ROWS(r3, r2);
 
1136
          SWAP_ROWS(r3, r2);
1183
1137
   if (0.0 == r2[2])
1184
 
      return Matrix4<T>();
 
1138
          return Matrix4<T>();
1185
1139
 
1186
1140
   /* eliminate third variable */
1187
1141
   m3 = r3[2] / r2[2];
1188
1142
   r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4],
1189
 
      r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6], r3[7] -= m3 * r2[7];
 
1143
          r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6], r3[7] -= m3 * r2[7];
1190
1144
 
1191
1145
   /* last check */
1192
1146
   if (0.0 == r3[3])
1193
 
      return Matrix4<T>();
 
1147
          return Matrix4<T>();
1194
1148
 
1195
1149
   s = 1.0 / r3[3];             /* now back substitute row 3 */
1196
1150
   r3[4] *= s;
1201
1155
   m2 = r2[3];                  /* now back substitute row 2 */
1202
1156
   s = 1.0 / r2[2];
1203
1157
   r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2),
1204
 
      r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2);
 
1158
          r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2);
1205
1159
   m1 = r1[3];
1206
1160
   r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1,
1207
 
      r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1;
 
1161
          r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1;
1208
1162
   m0 = r0[3];
1209
1163
   r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0,
1210
 
      r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0;
 
1164
          r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0;
1211
1165
 
1212
1166
   m1 = r1[2];                  /* now back substitute row 1 */
1213
1167
   s = 1.0 / r1[1];
1214
1168
   r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1),
1215
 
      r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1);
 
1169
          r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1);
1216
1170
   m0 = r0[2];
1217
1171
   r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0,
1218
 
      r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0;
 
1172
          r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0;
1219
1173
 
1220
1174
   m0 = r0[1];                  /* now back substitute row 0 */
1221
1175
   s = 1.0 / r0[0];
1222
1176
   r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0),
1223
 
      r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0);
 
1177
          r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0);
1224
1178
 
1225
1179
   MAT(out, 0, 0) = r0[4];
1226
1180
   MAT(out, 0, 1) = r0[5], MAT(out, 0, 2) = r0[6];
1241
1195
 
1242
1196
template<class T> void Matrix4<T>::print(void) const
1243
1197
{
1244
 
    printf("[%5.2lf %5.2lf %5.2lf %17.12le]\n"
1245
 
           "[%5.2lf %5.2lf %5.2lf %17.12le]\n"
1246
 
           "[%5.2lf %5.2lf %5.2lf %17.12le]\n"
1247
 
           "[%5.2lf %5.2lf %5.2lf %17.12le]\n\n",
1248
 
    r[0],r[4],r[8],r[12],
1249
 
    r[1],r[5],r[9],r[13],
1250
 
    r[2],r[6],r[10],r[14],
1251
 
    r[3],r[7],r[11],r[15]);
 
1198
        printf("[%5.2lf %5.2lf %5.2lf %17.12le]\n"
 
1199
                   "[%5.2lf %5.2lf %5.2lf %17.12le]\n"
 
1200
                   "[%5.2lf %5.2lf %5.2lf %17.12le]\n"
 
1201
                   "[%5.2lf %5.2lf %5.2lf %17.12le]\n\n",
 
1202
        r[0],r[4],r[8],r[12],
 
1203
        r[1],r[5],r[9],r[13],
 
1204
        r[2],r[6],r[10],r[14],
 
1205
        r[3],r[7],r[11],r[15]);
1252
1206
}
1253
1207
 
1254
1208