Trax3 3.1.0
trax track library
Loading...
Searching...
No Matches
Matrix.h
1// trax track library
2// AD 2014
3//
4// "Twilight driving gotta watch out for the roos"
5//
6// Methyl Ethel
7//
8// Copyright (c) 2025 Trend Redaktions- und Verlagsgesellschaft mbH
9// Copyright (c) 2019 Marc-Michael Horstmann
10//
11// Permission is hereby granted to any person obtaining a copy of this software
12// and associated source code (the "Software"), to use, view, and study the
13// Software for personal or internal business purposes, subject to the following
14// conditions:
15//
16// 1. Redistribution, modification, sublicensing, or commercial use of the
17// Software is NOT permitted without prior written consent from the copyright
18// holder.
19//
20// 2. The Software is provided "AS IS", without warranty of any kind, express
21// or implied.
22//
23// 3. All copies of the Software must retain this license notice.
24//
25// For further information, please contact: horstmann.marc@trendverlag.de
26
27#pragma once
28
29#include "Position.h"
30#include "PositionH.h"
31
32//#include <boost/numeric/ublas/matrix.hpp>
33//#include <boost/numeric/ublas/vector.hpp>
34//#include <boost/numeric/ublas/lu.hpp>
35
36// TODO: watch MPL2 license.
37#ifdef TRAX_OPEN_SOURCE
38# if defined(_MSC_VER)
39# pragma warning( disable: 4127 )
40# endif
41# include <Eigen/Dense>
42#endif
43
44#include <cassert>
45#include <algorithm>
46#include <cmath>
47
48
49namespace spat{
50
51 template<typename> struct Vector;
52 template<typename> struct Vector2D;
53 template<typename,typename> struct VectorBundle;
54 template<typename,typename> struct Frame;
55
61 template<typename Valtype, const unsigned short nCols, const unsigned short nRows >
62 class Matrix
63 {
64 public:
65 typedef Valtype value_type;
66
71 Matrix();
72 Matrix( const Matrix& matrix );
73 Matrix( Matrix&& matrix ) noexcept;
74 explicit Matrix( const std::initializer_list<Valtype>& elements );
75 ~Matrix() noexcept = default;
77
78
83 Matrix& operator=( const Matrix& matrix ) noexcept;
84
85 Matrix& operator=( Matrix&& matrix ) noexcept;
86
87 Matrix& operator=( const std::initializer_list<Valtype>& elements ) noexcept;
89
90
94 const Valtype& operator()( unsigned short col, unsigned short row ) const noexcept;
95
96 Valtype& operator()( unsigned short col, unsigned short row ) noexcept;
98
99
101 void SetNull() noexcept;
102
103
105 constexpr unsigned short Cols() const noexcept;
106
107
109 constexpr unsigned short Rows() const noexcept;
110
111
113 bool IsEqual( const Matrix& matrix, Valtype delta = 0 ) const noexcept;
114
115
117 bool operator==( const Matrix& matrix ) const noexcept;
118
119
121 bool operator!=( const Matrix& matrix ) const noexcept;
122
123
125 void operator*=( Valtype skalar ) noexcept;
126
127
129 void operator*=( const Matrix<Valtype,nCols,nCols>& matrix );
130
131
133 void operator/=( Valtype skalar ) noexcept;
134
135
137 void operator+=( const Matrix& matrix ) noexcept;
138
139
141 void operator-=( const Matrix& matrix ) noexcept;
142
143
146 const Valtype* ptr() const noexcept;
147
148
151 Matrix<Valtype,nCols-1,nRows-1> SubMatrix( unsigned short c, unsigned short r ) const;
152 private:
153 std::unique_ptr<Valtype[]> m;
154 };
155
156
158 template<typename Valtype,const unsigned short nCols> inline
159 constexpr Vector<Valtype> Column( const Matrix<Valtype,nCols,3>& m, unsigned short idx ) noexcept;
160
161
163 template<typename Valtype,const unsigned short nRows> inline
164 constexpr Vector<Valtype> Row( const Matrix<Valtype,3,nRows>& m, unsigned short idx ) noexcept;
165
166
168 template<typename Valtype, const unsigned short nCols, const unsigned short nRows >
169 constexpr Matrix<Valtype,nRows,nCols> Transposed( const Matrix<Valtype,nCols,nRows>& m ) noexcept;
170
171
174 template< typename Valtype,
175 typename Valtype2,
176 const unsigned short nCols,
177 const unsigned short nRows > inline
178 auto operator*( Valtype skalar, const Matrix<Valtype2,nCols,nRows>& matrix ) noexcept -> Matrix<decltype(Valtype{}*Valtype2{}),nCols,nRows>;
179
180
181 //template< typename Valtype,
182 // typename Valtype2,
183 // const unsigned short nCols,
184 // const unsigned short nRows,
185 // typename = std::enable_if<std::is_pod<Valtype2>::value, void>::type> inline
186 //auto operator*( const Matrix<Valtype,nCols,nRows>& matrix, Valtype2 skalar ) -> Matrix<decltype(Valtype{}*Valtype2{}),nCols,nRows>;
188
189
190 template< typename Valtype,
191 typename Valtype2,
192 unsigned short nColsFirst_RowsSecond,
193 unsigned short nRowsFirst,
194 unsigned short nColsSecond > inline
195 auto operator*( const Matrix<Valtype,nColsFirst_RowsSecond,nRowsFirst>& first,
196 const Matrix<Valtype2,nColsSecond,nColsFirst_RowsSecond>& second ) noexcept -> Matrix<decltype(Valtype{}*Valtype2{}),nColsSecond,nRowsFirst>;
197
198
199 template< typename Valtype,
200 typename Valtype2,
201 unsigned short nCols,
202 unsigned short nRows > inline
203 Matrix<Valtype,nCols,nRows>& operator*=( Matrix<Valtype,nCols,nRows>& first, Valtype2 skalar ) noexcept;
204
205
206 template< typename Valtype,
207 typename Valtype2,
208 unsigned short nCols,
209 unsigned short nRows > inline
210 Matrix<Valtype,nCols,nRows>& operator/=( Matrix<Valtype,nCols,nRows>& first, Valtype2 skalar ) noexcept;
211
212
213 template< typename Valtype,
214 const unsigned short nCols,
215 const unsigned short nRows > inline
216 Matrix<Valtype,nCols,nRows> operator+(
217 const Matrix<Valtype,nCols,nRows>& first,
218 const Matrix<Valtype,nCols,nRows>& second ) noexcept;
219
220
221 template< typename Valtype,
222 const unsigned short nCols,
223 const unsigned short nRows > inline
224 Matrix<Valtype,nCols,nRows> operator-(
225 const Matrix<Valtype,nCols,nRows>& first,
226 const Matrix<Valtype,nCols,nRows>& second ) noexcept;
227
228
229 template< typename Valtype,
230 const unsigned short nCols,
231 const unsigned short nRows > inline
232 constexpr Matrix<Valtype,nCols,nRows>& operator+( Matrix<Valtype,nCols,nRows>& matrix ) noexcept;
233
234
235 template< typename Valtype,
236 const unsigned short nCols,
237 const unsigned short nRows > inline
238 Matrix<Valtype,nCols,nRows>& operator-( Matrix<Valtype,nCols,nRows>& matrix ) noexcept;
240
241
246 template< typename Valtype,
247 typename Valtype2,
248 const unsigned short nCols,
249 const unsigned short nRows > inline
250 void copy_column_major( const Matrix<Valtype,nCols,nRows>& source, Valtype2* target ) noexcept;
251
252 template< typename Valtype,
253 typename Valtype2,
254 const unsigned short nCols,
255 const unsigned short nRows > inline
256 void copy_column_major( const Valtype2* source, Matrix<Valtype,nCols,nRows>& target ) noexcept;
257
258 template< typename Valtype,
259 typename Valtype2,
260 const unsigned short nCols,
261 const unsigned short nRows > inline
262 void copy_row_major( const Matrix<Valtype,nCols,nRows>& source, Valtype2* target ) noexcept;
263
264 template< typename Valtype,
265 typename Valtype2,
266 const unsigned short nCols,
267 const unsigned short nRows > inline
268 void copy_row_major( const Valtype2* source, Matrix<Valtype,nCols,nRows>& target ) noexcept;
270
271
276 template<typename Valtype, const unsigned short nColsAndRows>
277 class SquareMatrix : public Matrix<Valtype,nColsAndRows,nColsAndRows>
278 {
279 public:
280 using Basetype = Matrix<Valtype,nColsAndRows,nColsAndRows>;
281
286 // using Matrix<Valtype,nColsAndRows,nColsAndRows>::Matrix; //does not work ...
287 SquareMatrix() noexcept(false) = default;
288 SquareMatrix( const SquareMatrix& ) = default;
289 SquareMatrix( const Basetype& matrix );
290 SquareMatrix( Basetype&& matrix ) noexcept;
291 explicit SquareMatrix( const std::initializer_list<Valtype>& elements );
293
297 using Basetype::operator=;
299
300
301 using Basetype::operator();
302
303
306 void SetIdentity() noexcept;
307
308
310 bool IsIdentity( Valtype epsilon = 0 ) const noexcept;
311
312
314 bool IsDiagonal() const noexcept;
315
316
318 bool IsSymmetric( Valtype epsilon = 0 ) const noexcept;
319
320
322 SquareMatrix& Transpose() noexcept;
323
324
326 SquareMatrix& Invert();
327
328
332 Valtype Trace() const noexcept;
333 };
334
335
336 // IdentityMatrix not existing due to initializing problematic. Use SquareMatrix::SetIdentity() instead.
337
338
340 template<typename Valtype,typename Valtype2> constexpr
341 auto operator*( const SquareMatrix<Valtype,3>& m, const Vector<Valtype2>& v ) noexcept -> Vector<decltype(Valtype{}*Valtype2{})>;
342
343
345 template<typename Valtype, const unsigned short nColsAndRows > constexpr
346 SquareMatrix<Valtype,nColsAndRows> Inverted( const SquareMatrix<Valtype,nColsAndRows>& m );
347
348
350 template<typename Valtype, const unsigned short nColsAndRows > constexpr
351 SquareMatrix<Valtype,nColsAndRows> Transposed( const SquareMatrix<Valtype,nColsAndRows>& m );
352
353
356 template<typename Valtype, const unsigned short nColsAndRows > constexpr
357 auto Determinant( const SquareMatrix<Valtype,nColsAndRows>& m ) -> decltype(pow<nColsAndRows>(Valtype{}));
358
359
363 template<typename Valtype, const unsigned short nColsAndRows > constexpr
364 auto Adjungated( const SquareMatrix<Valtype,nColsAndRows>& m, unsigned short c, unsigned short r ) -> decltype(pow<nColsAndRows-1>(Valtype{}));
365
366
367 template<typename Valtype > constexpr
368 auto Adjungated( const SquareMatrix<Valtype,1>& m, unsigned short c, unsigned short r ) noexcept -> decltype(pow<0>(Valtype{}));
370
371
373 template<typename Valtype, const unsigned short nColsAndRows >
374 SquareMatrix<Valtype,nColsAndRows> AdjungatedMatrix( const SquareMatrix<Valtype,nColsAndRows>& m );
375
376
377 template<typename Valtype1,typename Valtype2>
378 auto operator*( const SquareMatrix<Valtype1,2>& m, const Vector2D<Valtype2>& v ) noexcept -> Vector2D<decltype(Valtype1{}*Valtype2{})>;
379
380
381 template<typename Valtype> class Rotation;
382
383
385 template<typename Valtype>
386 class Transformation : public SquareMatrix<Valtype,4>
387 {
388 public:
389 using Basetype = typename SquareMatrix<Valtype,4>::Basetype;
390
398 Transformation() = default;
399 Transformation( const SquareMatrix<Valtype,4>& matrix );
400 Transformation( SquareMatrix<Valtype,4>&& matrix ) noexcept;
401 Transformation( const Basetype& matrix );
402 Transformation( Basetype&& matrix ) noexcept;
403
404 template<typename Valtype2>
405 Transformation( const Rotation<Valtype2>& rot );
406 template<typename Valtype2,typename ValtypeT2>
407 explicit Transformation( const Frame<Valtype2,ValtypeT2>& frame );
408 explicit Transformation( const std::initializer_list<Valtype>& elements );
410
411
416 using SquareMatrix<Valtype,4>::operator=;
417
418 template<typename Valtype2>
419 Transformation& operator=( const Rotation<Valtype2>& rot ) noexcept;
420 template<typename Valtype2,typename ValtypeT2>
421 Transformation& operator=( const Frame<Valtype2,ValtypeT2>& frame ) noexcept;
423
424
425 using SquareMatrix<Valtype,4>::operator();
426
427
430 template<typename Valtype2>
431 PositionH<Valtype2> operator*( const PositionH<Valtype2>& p ) const noexcept;
432
433 template<typename Valtype2>
434 Position<Valtype2> operator*( const Position<Valtype2>& pos ) const noexcept;
435
436 template<typename Valtype2>
437 Vector<Valtype2> operator*( const Vector<Valtype2>& vec ) const noexcept;
439
440
441
449 void CreateTranslation( const Vector<Valtype>& translation ) noexcept;
450
451 void CreateTranslation( Valtype tx, Valtype ty, Valtype tz ) noexcept;
453
454
462 void CreateRotation( const Vector<Valtype>& rotation );
463
464 void CreateRotation( Valtype rx, Valtype ry, Valtype rz );
466
467
475 void CreateScaling( const Vector<Valtype>& scaling ) noexcept;
476
477 void CreateScaling( Valtype sx, Valtype sy, Valtype sz ) noexcept;
479
480
484 template<typename Valtype2>
485 void CreateMirror( const VectorBundle<Valtype2,Valtype>& mirror );
486
487
489 void LookAt( const Position<Valtype>& at,
490 const Position<Valtype>& eye,
491 const Vector<Valtype>& up ) noexcept;
492
494 void CreateViewport( Valtype Width,
495 Valtype Height,
496 Valtype MinZ,
497 Valtype MaxZ,
498 Valtype LeftMargin = 0.0f,
499 Valtype TopMargin = 0.0f ) noexcept;
500
501
507 bool CreateCameraProjection ( Valtype fovy, Valtype Aspect, Valtype zn, Valtype zf ) noexcept;
508
509
511 bool CreateOrthographicProjection( Valtype width, Valtype height, Valtype znear, Valtype zfar ) noexcept;
512
513
533 Vector<Valtype> TransformVector ( const Vector<Valtype>& vec ) const noexcept;
534
535
538 //Vector<Valtype> TransformNormal ( const Vector<Valtype>& vec ) const;
539
540
543 void GetTranslation( Transformation& T ) const noexcept;
544
545
548 bool GeRotation( Transformation& R ) const noexcept;
549
550
553 bool GetScaling( Transformation& S ) const noexcept;
554
555
558 bool Dismantle( Transformation& T, Transformation& R, Transformation& S ) const noexcept;
559
560
562 bool IsValid() const noexcept;
563 };
564
566 template<typename Valtype>
567 const Transformation<Valtype> IdentityTransformation{
568 Valtype{1}, Valtype{0}, Valtype{0}, Valtype{0},
569 Valtype{0}, Valtype{1}, Valtype{0}, Valtype{0},
570 Valtype{0}, Valtype{0}, Valtype{1}, Valtype{0},
571 Valtype{0}, Valtype{0}, Valtype{0}, Valtype{1} };
572
573
574 template< typename Valtype,
575 typename Valtype2 > inline
576 auto operator*( const Transformation<Valtype>& first,
577 const Transformation<Valtype2>& second ) noexcept -> Transformation<decltype(Valtype{}*Valtype2{})>;
578
579
588 template<typename Valtype>
589 bool Slerp( Transformation<Valtype>& out,
590 const Transformation<Valtype>& inA,
591 const Transformation<Valtype>& inB,
592 Valtype t );
593
594
600 template<typename Valtype, typename ValtypeT>
601 Frame<Valtype,ValtypeT> Invert( const Frame<Valtype,ValtypeT> frame );
602
603
605 template<typename Valtype>
606 class Rotation : public SquareMatrix<Valtype,3>
607 {
608 public:
609 using Basetype = typename SquareMatrix<Valtype,3>::Basetype;
610
617 Rotation() noexcept(false) = default;
618 Rotation( const SquareMatrix<Valtype,3>& matrix );
619 Rotation( SquareMatrix<Valtype,3>&& matrix ) noexcept;
620 Rotation( const Basetype& matrix );
621 Rotation( Basetype&& matrix ) noexcept;
622
623 explicit Rotation( const Vector<Valtype>& axis );
624 Rotation( Valtype q0, Valtype q1, Valtype q2, Valtype q3 ) noexcept; // from quarternion
625 template<typename ValtypeP>
626 explicit Rotation( const Frame<ValtypeP,Valtype>& frame );
627 template<typename Valtype2>
628 Rotation( const Transformation<Valtype2>& matrix );
629 explicit Rotation( Valtype r00, Valtype r10, Valtype r20,
630 Valtype r01, Valtype r11, Valtype r21,
631 Valtype r02, Valtype r12, Valtype r22 );
633
637 using SquareMatrix<Valtype,3>::operator=;
638
639 template<typename ValtypeP>
640 Rotation& operator=( const Frame<ValtypeP,Valtype>& frame ) noexcept;
642
643
644 using SquareMatrix<Valtype,3>::operator();
645
646
649 void CreateFromAxis( const Vector<Valtype>& axis );
650
651
654 void CreateFromAxis( Valtype rx, Valtype ry, Valtype rz ) noexcept;
655
656
660 void Rotate( Vector<Valtype>& v ) const noexcept;
661
662 template<typename ValtypeP>
663 void Rotate( Frame<ValtypeP,Valtype>& frame ) const noexcept;
665
666
669 Position<Valtype> operator*( const Position<Valtype>& p ) const noexcept;
670
671 Vector<Valtype> operator*( const Vector<Valtype>& v ) const noexcept;
673
674
677 Vector<Valtype> RotationAxis() const noexcept;
678
679
683 Valtype RotationAngle() const noexcept;
684 };
685
686
688 template<typename Valtype>
689 const Rotation<Valtype> IdentityRotation{ 1,0,0,
690 0,1,0,
691 0,0,1 };
692
693 template< typename Valtype,
694 typename Valtype2 > inline
695 auto operator*( const Rotation<Valtype>& first,
696 const Rotation<Valtype2>& second ) noexcept -> Rotation<decltype(Valtype{}*Valtype2{})>;
697
699template<typename Valtype, const unsigned short nCols, const unsigned short nRows >
700inline Matrix<Valtype,nCols,nRows>::Matrix()
701 : m{ std::make_unique<Valtype[]>(nCols*nRows) }
702{
703 static_assert( nCols, "Matrix: Number columes can not be zero" );
704 static_assert( nRows, "Matrix: Number of rows can not be zero" );
705}
706
707template<typename Valtype, const unsigned short nCols, const unsigned short nRows >
708Matrix<Valtype,nCols,nRows>::Matrix( const Matrix& matrix )
709 : m{ std::make_unique<Valtype[]>(nCols*nRows) }
710{
711 for( size_t Col = 0; Col < nCols; ++Col )
712 for( size_t Row = 0; Row < nRows; ++Row )
713 m[nRows * Col + Row] = matrix.m[nRows * Col + Row];
714}
715
716template<typename Valtype, const unsigned short nCols, const unsigned short nRows >
717inline Matrix<Valtype,nCols,nRows>::Matrix( Matrix&& matrix ) noexcept
718{
719 m = std::move(matrix.m);
720}
721
722template<typename Valtype,unsigned short nCols,unsigned short nRows>
723inline Matrix<Valtype,nCols,nRows>::Matrix( const std::initializer_list<Valtype>& elements )
724 : m{ std::make_unique<Valtype[]>(nCols*nRows) }
725{
726 static_assert( nCols, "Matrix: Number columes can not be zero" );
727 static_assert( nRows, "Matrix: Number of rows can not be zero" );
728
729 operator=( elements );
730}
731
732template<typename Valtype, const unsigned short nCols, const unsigned short nRows >
733Matrix<Valtype,nCols,nRows>& Matrix<Valtype,nCols,nRows>::operator=(
734 const Matrix& matrix ) noexcept
735{
736 for( size_t Col = 0; Col < nCols; ++Col )
737 for( size_t Row = 0; Row < nRows; ++Row )
738 m[nRows * Col + Row] = matrix.m[nRows * Col + Row];
739
740 return (*this);
741}
742
743template<typename Valtype, const unsigned short nCols, const unsigned short nRows >
744inline Matrix<Valtype,nCols,nRows>& Matrix<Valtype,nCols,nRows>::operator=(
745 Matrix&& matrix ) noexcept
746{
747 m = std::move(matrix.m);
748 return *this;
749}
750
751template<typename Valtype,unsigned short nCols,unsigned short nRows>
752inline Matrix<Valtype,nCols,nRows>& Matrix<Valtype,nCols,nRows>::operator=(
753 const std::initializer_list<Valtype>& elements ) noexcept
754{
755 auto it = elements.begin();
756 const auto end = elements.end();
757
758 // Fill in row-major order into column-major storage
759 for( size_t Row = 0; Row < nRows; ++Row ){
760 for( size_t Col = 0; Col < nCols; ++Col ){
761 m[nRows * Col + Row] = (it != end) ? *it++ : Valtype{0};
762 }
763 }
764 return *this;
765}
766
767template<typename Valtype, const unsigned short nCols, const unsigned short nRows >
768inline const Valtype& Matrix<Valtype,nCols,nRows>::operator()( unsigned short col, unsigned short row ) const noexcept{
769// assert( col < nCols && row < nRows );
770 return m[nRows * static_cast<size_t>(col) + row];
771}
772
773template<typename Valtype, const unsigned short nCols, const unsigned short nRows >
774inline Valtype& Matrix<Valtype,nCols,nRows>::operator()( unsigned short col, unsigned short row ) noexcept{
775// assert( col < nCols && row < nRows );
776 return m[nRows * static_cast<size_t>(col) + row];
777}
778
779template<typename Valtype, const unsigned short nCols, const unsigned short nRows >
780void Matrix<Valtype,nCols,nRows>::SetNull() noexcept{
781 for( size_t Col = 0; Col < nCols; ++Col )
782 for( size_t Row = 0; Row < nRows; ++Row )
783 m[nRows * Col + Row] = 0;
784}
785
786template<typename Valtype, const unsigned short nCols, const unsigned short nRows >
787inline constexpr unsigned short Matrix<Valtype,nCols,nRows>::Cols() const noexcept{
788 return nCols;
789}
790
791template<typename Valtype, const unsigned short nCols, const unsigned short nRows >
792inline constexpr unsigned short Matrix<Valtype,nCols,nRows>::Rows() const noexcept{
793 return nRows;
794}
795
796template<typename Valtype, const unsigned short nCols, const unsigned short nRows >
797bool Matrix<Valtype,nCols,nRows>::IsEqual( const Matrix& matrix, Valtype delta ) const noexcept{
798 for( unsigned short x = 0; x < nCols; ++x )
799 for( unsigned short y = 0; y < nRows; ++y )
800 if( !common::Equals( (*this)( x, y ), matrix( x, y ), delta ) )
801 return false;
802
803 return true;
804}
805
806template<typename Valtype, const unsigned short nCols, const unsigned short nRows >
807inline bool Matrix<Valtype,nCols,nRows>::operator==( const Matrix& matrix ) const noexcept{
808 return IsEqual( matrix, std::numeric_limits<Valtype>::epsilon() );
809}
810
811template<typename Valtype, const unsigned short nCols, const unsigned short nRows >
812inline bool Matrix<Valtype,nCols,nRows>::operator!=( const Matrix& matrix ) const noexcept{
813 return !operator==( matrix );
814}
815
816template<typename Valtype, const unsigned short nCols, const unsigned short nRows >
817inline void Matrix<Valtype,nCols,nRows>::operator*=( Valtype skalar ) noexcept{
818 for( size_t Col = 0; Col < nCols; ++Col )
819 for( size_t Row = 0; Row < nRows; ++Row )
820 m[nRows * Col + Row] *= skalar;
821}
822
823template<typename Valtype, const unsigned short nCols, const unsigned short nRows >
824inline void Matrix<Valtype,nCols,nRows>::operator*=( const Matrix<Valtype,nCols,nCols>& matrix ){
825 Matrix<Valtype,nCols,nRows> TempVal;
826 MatrixMultiply( *this, matrix, TempVal );
827 *this = TempVal;
828}
829
830template<typename Valtype, const unsigned short nCols, const unsigned short nRows >
831inline void Matrix<Valtype,nCols,nRows>::operator/=( Valtype skalar ) noexcept{
832 for( size_t Col = 0; Col < nCols; ++Col )
833 for( size_t Row = 0; Row < nRows; ++Row )
834 m[nRows * Col + Row] /= skalar;
835}
836
837template<typename Valtype, const unsigned short nCols, const unsigned short nRows >
838inline void Matrix<Valtype,nCols,nRows>::operator+=( const Matrix& matrix ) noexcept{
839 for( size_t Col = 0; Col < nCols; ++Col )
840 for( size_t Row = 0; Row < nRows; ++Row )
841 m[nRows * Col + Row] += matrix.m[nRows * Col + Row];
842}
843
844template<typename Valtype, const unsigned short nCols, const unsigned short nRows >
845inline void Matrix<Valtype,nCols,nRows>::operator-=( const Matrix& matrix ) noexcept{
846 for( size_t Col = 0; Col < nCols; ++Col )
847 for( size_t Row = 0; Row < nRows; ++Row )
848 m[nRows * Col + Row] -= matrix.m[nRows * Col + Row];
849}
850
851template<typename Valtype, const unsigned short nCols, const unsigned short nRows >
852inline const Valtype* Matrix<Valtype,nCols,nRows>::ptr() const noexcept{
853 return m.get();
854}
855
856template<typename Valtype, const unsigned short nCols, const unsigned short nRows >
857Matrix<Valtype,nCols-1,nRows-1> Matrix<Valtype,nCols,nRows>::SubMatrix( unsigned short c, unsigned short r ) const{
858 Matrix<Valtype,nCols-1,nRows-1> Result;
859 unsigned short x;
860 for( x = 0; x < c; ++x ){
861 unsigned short y;
862 for( y = 0; y < r; ++y )
863 Result( x, y ) = (*this)( x, y );
864 for( y++; y < nRows; ++y )
865 Result( x, y - 1u ) = (*this)( x, y );
866 }
867 for( x++; x < nCols; ++x ){
868 unsigned short y;
869 for( y = 0; y < r; ++y )
870 Result( x - 1u, y ) = (*this)( x, y );
871 for( y++; y < nRows; ++y )
872 Result( x - 1u, y - 1u ) = (*this)( x, y );
873 }
874 return Result;
875}
876
877template<typename Valtype,const unsigned short nCols>
878inline constexpr Vector<Valtype> Column( const Matrix<Valtype,nCols,3>& m, unsigned short idx ) noexcept{
879 return Vector<Valtype>{ m(idx,0), m(idx,1), m(idx,2) };
880}
881
882template<typename Valtype,const unsigned short nRows>
883inline constexpr Vector<Valtype> Row( const Matrix<Valtype,3,nRows>& m, unsigned short idx ) noexcept{
884 return Vector<Valtype>{ m(0,idx), m(1,idx), m(2,idx) };
885}
886
887template<typename Valtype, unsigned short nCols, unsigned short nRows>
888constexpr Matrix<Valtype,nRows,nCols> Transposed( const Matrix<Valtype,nCols,nRows>& m ) noexcept{
889 Matrix<Valtype,nRows,nCols> Result;
890 for( unsigned short c = 0; c < nCols; ++c )
891 for( unsigned short r = 0; r < nRows; ++r )
892 Result(r,c) = m(c,r);
893
894 return Result;
895}
896
897template<typename Valtype, typename Valtype2, const unsigned short nCols, const unsigned short nRows >
898inline auto operator*( Valtype scalar, const Matrix<Valtype2,nCols,nRows>& matrix ) noexcept -> Matrix<decltype(Valtype{}*Valtype2{}),nCols,nRows>{
899// static_assert( std::is_scalar<Valtype>::value, "Scalar multiplication only defined for scalar types!" );
900
901 Matrix<decltype(Valtype{}*Valtype2{}),nCols,nRows> Retval;
902 for( unsigned short c = 0; c < nCols; ++c )
903 for( unsigned short r = 0; r < nRows; ++r )
904 Retval(c,r) = scalar * matrix(c,r);
905
906 return Retval;
907}
908
909//
910//template<typename Valtype, typename Valtype2, const unsigned short nCols, const unsigned short nRows, typename >
911//inline auto operator*( const Matrix<Valtype,nCols,nRows>& matrix, Valtype2 skalar ) -> Matrix<decltype(Valtype{}*Valtype2{}),nCols,nRows>{
912// return skalar * matrix;
913//}
914
915template<
916 typename Valtype,
917 typename Valtype2,
918 unsigned short nColsFirst_RowsSecond,
919 unsigned short nRowsFirst,
920 unsigned short nColsSecond>
921inline auto operator*(
922 const Matrix<Valtype,nColsFirst_RowsSecond,nRowsFirst>& first,
923 const Matrix<Valtype2,nColsSecond,nColsFirst_RowsSecond>& second ) noexcept -> Matrix<decltype(Valtype{}*Valtype2{}),nColsSecond,nRowsFirst>
924{
925 using ResultType = decltype(Valtype{}*Valtype2{});
926
927 Matrix<ResultType,nColsSecond,nRowsFirst> result;
928 for( unsigned short x = 0; x < nColsSecond; ++x )
929 for( unsigned short y = 0; y < nRowsFirst; ++y )
930 {
931 result(x,y) = ResultType{0};
932 for( unsigned short z = 0; z < nColsFirst_RowsSecond; ++z )
933 result(x,y) += first(z,y) * second(x,z);
934 }
935
936 return result;
937}
938
939template< typename Valtype,
940 typename Valtype2,
941 unsigned short nCols,
942 unsigned short nRows > inline
943Matrix<Valtype,nCols,nRows>& operator*=( Matrix<Valtype,nCols,nRows>& matrix, Valtype2 skalar ) noexcept{
944 for( unsigned short c = 0; c < nCols; ++c )
945 for( unsigned short r = 0; r < nRows; ++r )
946 matrix(c,r) *= skalar;
947 return matrix;
948}
949
950template< typename Valtype,
951 typename Valtype2,
952 unsigned short nCols,
953 unsigned short nRows > inline
954Matrix<Valtype,nCols,nRows>& operator/=( Matrix<Valtype,nCols,nRows>& matrix, Valtype2 skalar ) noexcept{
955 for( unsigned short c = 0; c < nCols; ++c )
956 for( unsigned short r = 0; r < nRows; ++r )
957 matrix(c,r) /= skalar;
958 return matrix;
959}
960
961template<typename Valtype,unsigned short nCols,unsigned short nRows>
962inline Matrix<Valtype,nCols,nRows> operator+(const Matrix<Valtype,nCols,nRows>& first,const Matrix<Valtype,nCols,nRows>& second) noexcept{
963 Matrix<Valtype,nCols,nRows> result;
964 for( unsigned short c = 0; c < nCols; ++c )
965 for( unsigned short r = 0; r < nRows; ++r )
966 result(c,r) = first(c,r) + second(c,r);
967 return result;
968}
969
970template<typename Valtype,unsigned short nCols,unsigned short nRows>
971inline Matrix<Valtype,nCols,nRows> operator-(const Matrix<Valtype,nCols,nRows>& first,const Matrix<Valtype,nCols,nRows>& second) noexcept{
972 Matrix<Valtype,nCols,nRows> result;
973 for( unsigned short c = 0; c < nCols; ++c )
974 for( unsigned short r = 0; r < nRows; ++r )
975 result(c,r) = first(c,r) - second(c,r);
976 return result;
977}
978
979template<typename Valtype,unsigned short nCols,unsigned short nRows>
980inline constexpr Matrix<Valtype,nCols,nRows>& operator+( Matrix<Valtype,nCols,nRows>& matrix ) noexcept{
981 return matrix;
982}
983
984template<typename Valtype,unsigned short nCols,unsigned short nRows>
985inline Matrix<Valtype,nCols,nRows>& operator-( Matrix<Valtype,nCols,nRows>& matrix ) noexcept{
986 for( unsigned short c = 0; c < nCols; ++c )
987 for( unsigned short r = 0; r < nRows; ++r )
988 matrix(c,r) *= -1;
989 return matrix;
990}
991
992template< typename Valtype,
993 typename Valtype2,
994 const unsigned short nCols,
995 const unsigned short nRows > inline
996void copy_column_major( const Matrix<Valtype,nCols,nRows>& source, Valtype2* target ) noexcept{
997 assert( target );
998 for( unsigned short c = 0; c < nCols; ++c )
999 for( unsigned short r = 0; r < nRows; ++r )
1000 target[c*nRows+r] = static_cast<Valtype2>(source(c,r));
1001}
1002
1003template< typename Valtype,
1004 typename Valtype2,
1005 const unsigned short nCols,
1006 const unsigned short nRows > inline
1007void copy_column_major( const Valtype2* source, Matrix<Valtype,nCols,nRows>& target ) noexcept{
1008 assert( source );
1009 for( unsigned short c = 0; c < nCols; ++c )
1010 for( unsigned short r = 0; r < nRows; ++r )
1011 target(c,r) = static_cast<Valtype>(source[c*nRows+r]);
1012}
1013
1014template< typename Valtype,
1015 typename Valtype2,
1016 const unsigned short nCols,
1017 const unsigned short nRows > inline
1018void copy_row_major( const Matrix<Valtype,nCols,nRows>& source, Valtype2* target ) noexcept{
1019 assert( target );
1020 for( unsigned short r = 0; r < nRows; ++r )
1021 for( unsigned short c = 0; c < nCols; ++c )
1022 target[r*nCols+c] = static_cast<Valtype2>(source(c,r));
1023}
1024
1025template< typename Valtype,
1026 typename Valtype2,
1027 const unsigned short nCols,
1028 const unsigned short nRows > inline
1029void copy_row_major( const Valtype2* source, Matrix<Valtype,nCols,nRows>& target ) noexcept{
1030 assert( source );
1031 for( unsigned short r = 0; r < nRows; ++r )
1032 for( unsigned short c = 0; c < nCols; ++c )
1033 target(c,r) = static_cast<Valtype>(source[r*nCols+c]);
1034}
1036template<typename Valtype,unsigned short nColsAndRows>
1037inline SquareMatrix<Valtype,nColsAndRows>::SquareMatrix( const Basetype& matrix )
1038 : Basetype{matrix}
1039{
1040}
1041
1042template<typename Valtype,unsigned short nColsAndRows>
1043inline SquareMatrix<Valtype,nColsAndRows>::SquareMatrix( Basetype&& matrix ) noexcept
1044 : Basetype{std::move(matrix)}
1045{
1046}
1047
1048template<typename Valtype,unsigned short nColsAndRows>
1049inline SquareMatrix<Valtype,nColsAndRows>::SquareMatrix( const std::initializer_list<Valtype>& elements )
1050 : Basetype{elements}
1051{
1052}
1053
1054template<typename Valtype, const unsigned short nColsAndRows>
1055void SquareMatrix<Valtype,nColsAndRows>::SetIdentity() noexcept{
1056 for( unsigned short x = 0; x < nColsAndRows; ++x )
1057 for( unsigned short y = 0; y < nColsAndRows; ++y )
1058 (*this)(x,y) = (Valtype)((x == y) ? 1 : 0);
1059}
1060
1061template<typename Valtype, const unsigned short nColsAndRows>
1062bool SquareMatrix<Valtype,nColsAndRows>::IsIdentity( Valtype epsilon_ ) const noexcept{
1063 for( unsigned short x = 0; x < nColsAndRows; ++x )
1064 for( unsigned short y = 0; y < nColsAndRows; ++y )
1065 if( !common::Equals( (*this)(x,y), (Valtype)((x == y) ? 1 : 0), epsilon_ ) )
1066 return false;
1067 return true;
1068}
1069
1070template<typename Valtype, const unsigned short nColsAndRows>
1071bool SquareMatrix<Valtype,nColsAndRows>::IsDiagonal() const noexcept{
1072 for( unsigned short x = 0; x < nColsAndRows; ++x )
1073 for( unsigned short y = 0; y < nColsAndRows; ++y )
1074 if( x != y && (*this)(x,y) != Valtype{0} )
1075 return false;
1076
1077 return true;
1078}
1079
1080template<typename Valtype,unsigned short nColsAndRows>
1081inline bool SquareMatrix<Valtype,nColsAndRows>::IsSymmetric( Valtype epsilon_ ) const noexcept{
1082 for( unsigned short i = 0; i < nColsAndRows; ++i )
1083 for( unsigned short j = 0; j < nColsAndRows; ++j ){
1084 if( i == j )
1085 continue;
1086 if( !common::Equals( (*this)(i,j), (*this)(j,i), epsilon_ ) )
1087 return false;
1088 }
1089
1090 return true;
1091}
1092
1093template<typename Valtype, const unsigned short nColsAndRows>
1094SquareMatrix<Valtype,nColsAndRows>& SquareMatrix<Valtype,nColsAndRows>::Transpose() noexcept{
1095 for( unsigned short x = 0; x < nColsAndRows; ++x )
1096 for( unsigned short y = 0; y < x; ++y )
1097 std::swap( (*this)( x, y ), (*this)( y, x ) );
1098
1099 return *this;
1100}
1101
1102template<typename Valtype, const unsigned short nColsAndRows>
1103SquareMatrix<Valtype,nColsAndRows>& SquareMatrix<Valtype,nColsAndRows>::Invert(){
1104 Valtype d = Determinant(*this);
1105 if( d == Valtype{0} )
1106 throw std::logic_error( "no inverse matrix" );
1107
1108 *this = 1 / d * AdjungatedMatrix(*this).Transpose();
1109 return *this;
1110}
1111
1112template<typename Valtype, const unsigned short nColsAndRows>
1113Valtype SquareMatrix<Valtype,nColsAndRows>::Trace() const noexcept{
1114 Valtype T = 0;
1115 for( unsigned short z = 0; z < nColsAndRows; ++z )
1116 T += (*this)( z, z );
1117
1118 return T;
1119}
1120
1121template<typename Valtype,typename Valtype2> constexpr
1122inline auto operator*( const SquareMatrix<Valtype,3>& m, const Vector<Valtype2>& v ) noexcept -> Vector<decltype(Valtype{}*Valtype2{})>{
1123 return {
1124 m(0,0) * v.dx + m(1,0) * v.dy + m(2,0) * v.dz,
1125 m(0,1) * v.dx + m(1,1) * v.dy + m(2,1) * v.dz,
1126 m(0,2) * v.dx + m(1,2) * v.dy + m(2,2) * v.dz };
1127}
1128
1129template<typename Valtype,unsigned short nColsAndRows>
1130inline constexpr SquareMatrix<Valtype,nColsAndRows> Inverted( const SquareMatrix<Valtype,nColsAndRows>& m ){
1131 SquareMatrix<Valtype,nColsAndRows> result{ m };
1132 result.Invert();
1133 return result;
1134}
1135
1136template<typename Valtype, const unsigned short nColsAndRows > constexpr
1137inline SquareMatrix<Valtype,nColsAndRows> Transposed( const SquareMatrix<Valtype,nColsAndRows>& m ){
1138 SquareMatrix<Valtype,nColsAndRows> Result;
1139 for( unsigned short c = 0; c < nColsAndRows; ++c )
1140 for( unsigned short r = 0; r < nColsAndRows; ++r )
1141 Result(r,c) = m(c,r);
1142
1143 return Result;
1144}
1145
1146template<typename Valtype, const unsigned short nColsAndRows > constexpr
1147auto Determinant( const SquareMatrix<Valtype,nColsAndRows>& m ) -> decltype(pow<nColsAndRows>(Valtype{})){
1148 decltype(pow<nColsAndRows>(Valtype{})) D{0};
1149 for( unsigned short z = 0; z < nColsAndRows; ++z )
1150 D += m( z, 0 ) * Adjungated( m, z, 0 );
1151
1152 return D;
1153}
1154
1155template<typename Valtype, const unsigned short nColsAndRows > constexpr
1156auto Adjungated( const SquareMatrix<Valtype,nColsAndRows>& m, unsigned short c, unsigned short r ) -> decltype(pow<nColsAndRows-1>(Valtype{})){
1157 SquareMatrix<Valtype,nColsAndRows-1> sub{ m.SubMatrix( c, r ) };
1158 return (((r + c) % 2) ? -1 : 1 ) * Determinant( sub );
1159}
1160
1161template<typename Valtype > constexpr
1162auto Adjungated( const SquareMatrix<Valtype,1>&, unsigned short, unsigned short ) noexcept -> decltype(pow<0>(Valtype{})){
1163 return 1;
1164}
1165
1166template<typename Valtype, const unsigned short nColsAndRows >
1167SquareMatrix<Valtype,nColsAndRows> AdjungatedMatrix( const SquareMatrix<Valtype,nColsAndRows>& m ){
1168 SquareMatrix<Valtype,nColsAndRows> Retval;
1169 for( unsigned short x = 0; x < nColsAndRows; ++x )
1170 for( unsigned short y = 0; y < nColsAndRows; ++y )
1171 Retval( x, y ) = Adjungated( m, x, y );
1172
1173 return Retval;
1174}
1175
1176template<typename Valtype1,typename Valtype2>
1177inline auto operator*( const SquareMatrix<Valtype1,2>& m, const Vector2D<Valtype2>& v ) noexcept -> Vector2D<decltype(Valtype1{}*Valtype2{})>{
1178 return { m(0,0) * v.dx + m(1,0) * v.dy, m(0,1) * v.dx + m(1,1) * v.dy };
1179}
1181template<typename Valtype>
1182inline Transformation<Valtype>::Transformation( const SquareMatrix<Valtype,4>& matrix )
1183 : SquareMatrix<Valtype,4>{matrix}
1184{
1185}
1186
1187template<typename Valtype>
1188inline Transformation<Valtype>::Transformation( SquareMatrix<Valtype,4>&& matrix ) noexcept
1189 : SquareMatrix<Valtype,4>{std::move(matrix)}
1190{
1191}
1192
1193template<typename Valtype>
1194inline Transformation<Valtype>::Transformation( const Basetype& matrix )
1195 : SquareMatrix<Valtype,4>{matrix}
1196{
1197}
1198
1199template<typename Valtype>
1200inline Transformation<Valtype>::Transformation( Basetype&& matrix ) noexcept
1201 : SquareMatrix<Valtype,4>{std::move(matrix)}
1202{
1203}
1204
1205template<typename Valtype>
1206template<typename Valtype2>
1207inline Transformation<Valtype>::Transformation( const Rotation<Valtype2>& rot )
1208 : SquareMatrix<Valtype,4>{}
1209{
1210 operator=(rot);
1211}
1212
1213template<typename Valtype>
1214template<typename Valtype2,typename ValtypeT2>
1215inline Transformation<Valtype>::Transformation( const Frame<Valtype2,ValtypeT2>& frame )
1216 : SquareMatrix<Valtype,4>{}
1217{
1218 operator=( frame );
1219}
1220
1221template<typename Valtype>
1222inline Transformation<Valtype>::Transformation( const std::initializer_list<Valtype>& elements )
1223 : SquareMatrix<Valtype,4>{elements}
1224{
1225}
1226
1227template<typename Valtype>
1228template<typename Valtype2>
1229Transformation<Valtype>& Transformation<Valtype>::operator=(
1230 const Rotation<Valtype2>& rot ) noexcept
1231{
1232 operator()(0,0) = static_cast<Valtype>(rot(0,0));
1233 operator()(1,0) = static_cast<Valtype>(rot(1,0));
1234 operator()(2,0) = static_cast<Valtype>(rot(2,0));
1235 operator()(3,0) = 0;
1236
1237 operator()(0,1) = static_cast<Valtype>(rot(0,1));
1238 operator()(1,1) = static_cast<Valtype>(rot(1,1));
1239 operator()(2,1) = static_cast<Valtype>(rot(2,1));
1240 operator()(3,1) = 0;
1241
1242 operator()(0,2) = static_cast<Valtype>(rot(0,2));
1243 operator()(1,2) = static_cast<Valtype>(rot(1,2));
1244 operator()(2,2) = static_cast<Valtype>(rot(2,2));
1245 operator()(3,2) = 0;
1246
1247 operator()(0,3) = 0;
1248 operator()(1,3) = 0;
1249 operator()(2,3) = 0;
1250 operator()(3,3) = 1;
1251
1252 return *this;
1253}
1254
1255template<typename Valtype>
1256template<typename Valtype2,typename ValtypeT2>
1257Transformation<Valtype>& Transformation<Valtype>::operator=(
1258 const Frame<Valtype2,ValtypeT2>& frame ) noexcept
1259{
1260 operator()( 0, 0 ) = static_cast<Valtype>(frame.T.dx/ValtypeT2{1});
1261 operator()( 0, 1 ) = static_cast<Valtype>(frame.T.dy/ValtypeT2{1});
1262 operator()( 0, 2 ) = static_cast<Valtype>(frame.T.dz/ValtypeT2{1});
1263 operator()( 0, 3 ) = 0;
1264
1265 operator()( 1, 0 ) = static_cast<Valtype>(frame.N.dx/ValtypeT2{1});
1266 operator()( 1, 1 ) = static_cast<Valtype>(frame.N.dy/ValtypeT2{1});
1267 operator()( 1, 2 ) = static_cast<Valtype>(frame.N.dz/ValtypeT2{1});
1268 operator()( 1, 3 ) = 0;
1269
1270 operator()( 2, 0 ) = static_cast<Valtype>(frame.B.dx/ValtypeT2{1});
1271 operator()( 2, 1 ) = static_cast<Valtype>(frame.B.dy/ValtypeT2{1});
1272 operator()( 2, 2 ) = static_cast<Valtype>(frame.B.dz/ValtypeT2{1});
1273 operator()( 2, 3 ) = 0;
1274
1275 operator()( 3, 0 ) = static_cast<Valtype>(frame.P.x/Valtype2{1});
1276 operator()( 3, 1 ) = static_cast<Valtype>(frame.P.y/Valtype2{1});
1277 operator()( 3, 2 ) = static_cast<Valtype>(frame.P.z/Valtype2{1});
1278 operator()( 3, 3 ) = 1;
1279
1280 return *this;
1281}
1282
1283template<typename Valtype>
1284template<typename Valtype2>
1285inline PositionH<Valtype2> Transformation<Valtype>::operator*(
1286 const PositionH<Valtype2>& pos ) const noexcept
1287{
1288 return {
1289 (*this)(0,0) * pos.x + (*this)(1,0) * pos.y + (*this)(2,0) * pos.z + (*this)(3,0) * pos.w,
1290 (*this)(0,1) * pos.x + (*this)(1,1) * pos.y + (*this)(2,1) * pos.z + (*this)(3,1) * pos.w,
1291 (*this)(0,2) * pos.x + (*this)(1,2) * pos.y + (*this)(2,2) * pos.z + (*this)(3,2) * pos.w,
1292 (*this)(0,3) * pos.x + (*this)(1,3) * pos.y + (*this)(2,3) * pos.z + (*this)(3,3) * pos.w
1293 };
1294}
1295
1296template<typename Valtype>
1297template<typename Valtype2>
1298inline Position<Valtype2> Transformation<Valtype>::operator*(
1299 const Position<Valtype2>& pos ) const noexcept
1300{
1301 PositionH<Valtype> HPos{ pos };
1302 HPos = operator*( HPos );
1303 HPos.Homogenize();
1304 return { Valtype2{HPos.x}, Valtype2{HPos.y}, Valtype2{HPos.z} };
1305}
1306
1307template<typename Valtype>
1308template<typename Valtype2>
1309inline Vector<Valtype2> Transformation<Valtype>::operator*(
1310 const Vector<Valtype2>& dir ) const noexcept
1311{
1312 PositionH<Valtype> HPos{ dir };
1313 HPos = operator*( HPos );
1314 return { Valtype2{HPos.x}, Valtype2{HPos.y}, Valtype2{HPos.z} };
1315}
1316
1317template<typename Valtype>
1318inline void Transformation<Valtype>::CreateTranslation(
1319 const Vector<Valtype>& translation ) noexcept
1320{
1321 CreateTranslation( translation.dx, translation.dy, translation.dz );
1322}
1323
1324template<typename Valtype>
1325void Transformation<Valtype>::CreateTranslation(
1326 Valtype tx, Valtype ty, Valtype tz ) noexcept
1327{
1328 operator()(0,0) = 1;
1329 operator()(1,0) = 0;
1330 operator()(2,0) = 0;
1331 operator()(3,0) = tx;
1332
1333 operator()(0,1) = 0;
1334 operator()(1,1) = 1;
1335 operator()(2,1) = 0;
1336 operator()(3,1) = ty;
1337
1338 operator()(0,2) = 0;
1339 operator()(1,2) = 0;
1340 operator()(2,2) = 1;
1341 operator()(3,2) = tz;
1342
1343 operator()(0,3) = 0;
1344 operator()(1,3) = 0;
1345 operator()(2,3) = 0;
1346 operator()(3,3) = 1;
1347}
1348
1349template<typename Valtype>
1350void Transformation<Valtype>::CreateRotation( const Vector<Valtype>& rotation )
1351 // Source D. Paull. Charles River Media 2002
1352 // This at least is proven to work with the
1353 // rotations around the coordinate axes.
1354{
1355 Rotation<Valtype> R;
1356 R.CreateFromAxis(rotation);
1357
1358 operator()(0,0) = R(0,0);
1359 operator()(1,0) = R(1,0);
1360 operator()(2,0) = R(2,0);
1361 operator()(3,0) = 0;
1362
1363 operator()(0,1) = R(0,1);
1364 operator()(1,1) = R(1,1);
1365 operator()(2,1) = R(2,1);
1366 operator()(3,1) = 0;
1367
1368 operator()(0,2) = R(0,2);
1369 operator()(1,2) = R(1,2);
1370 operator()(2,2) = R(2,2);
1371 operator()(3,2) = 0;
1372
1373 operator()(0,3) = 0;
1374 operator()(1,3) = 0;
1375 operator()(2,3) = 0;
1376 operator()(3,3) = 1;
1377}
1378
1379template<typename Valtype>
1380inline void Transformation<Valtype>::CreateRotation( Valtype rx, Valtype ry, Valtype rz )
1381{
1382 CreateRotation( Vector<Valtype>( rx, ry, rz ) );
1383}
1384
1385template<typename Valtype>
1386inline void Transformation<Valtype>::CreateScaling( const Vector<Valtype>& scaling ) noexcept
1387{
1388 CreateScaling( scaling.dx, scaling.dy, scaling.dz );
1389}
1390
1391template<typename Valtype>
1392void Transformation<Valtype>::CreateScaling( Valtype sx, Valtype sy, Valtype sz ) noexcept
1393{
1394 operator()(0,0) = sx;
1395 operator()(1,0) = 0;
1396 operator()(2,0) = 0;
1397 operator()(3,0) = 0;
1398
1399 operator()(0,1) = 0;
1400 operator()(1,1) = sy;
1401 operator()(2,1) = 0;
1402 operator()(3,1) = 0;
1403
1404 operator()(0,2) = 0;
1405 operator()(1,2) = 0;
1406 operator()(2,2) = sz;
1407 operator()(3,2) = 0;
1408
1409 operator()(0,3) = 0;
1410 operator()(1,3) = 0;
1411 operator()(2,3) = 0;
1412 operator()(3,3) = 1;
1413}
1414
1415template<typename Valtype>
1416template<typename Valtype2>
1417inline void Transformation<Valtype>::CreateMirror( const VectorBundle<Valtype2,Valtype>& mirror ){
1418 assert( mirror.T.IsNormal() );
1419 Matrix<Valtype,1,4> N{ mirror.T.dx, mirror.T.dy, mirror.T.dz, (Origin3D<Valtype2> - mirror.P) * mirror.T / Valtype2{1} };
1420 *this = IdentityTransformation<Valtype> - 2*N*Transposed(N);
1421 operator()(0,3) = Valtype{0};
1422 operator()(1,3) = Valtype{0};
1423 operator()(2,3) = Valtype{0};
1424 operator()(3,3) = Valtype{1};
1425}
1426
1427template<typename Valtype>
1428void Transformation<Valtype>::LookAt( const Position<Valtype>& at,
1429 const Position<Valtype>& eye,
1430 const Vector<Valtype>& up ) noexcept
1431{
1432 // Vector directed at at;
1433 Vector<Valtype> d = at - eye;
1434 d.Normalize();
1435
1436 // Vector that really shows up, nearest to up
1437 // but othogonal to d;
1438 Vector<Valtype> u = up - (up*d)*d;
1439 u.Normalize();
1440
1441 Vector<Valtype> n = d % u;
1442 Vector<Valtype> e = eye - Position<Valtype>{0,0,0};
1443
1444 // To transform a vector x into new projected coords X,Y,Z
1445 // with respect to a right handed coord system (negative Z)
1446 // calculations are;
1447 // X = n(x - e)
1448 // Y = u(x - e)
1449 // Z = -d(x - e)
1450 // This makes:
1451
1452 *this = { n.dx, n.dy, n.dz, -n*e,
1453 u.dx, u.dy, u.dz, -u*e,
1454 -d.dx, -d.dy, -d.dz, d*e,
1455 0, 0, 0, 1 };
1456}
1457
1458template<typename Valtype>
1459void Transformation<Valtype>::CreateViewport(
1460 Valtype Width,
1461 Valtype Height,
1462 Valtype MinZ,
1463 Valtype MaxZ,
1464 Valtype LeftMargin,
1465 Valtype TopMargin ) noexcept
1466 // This assumes incomming data to be in a -1 to +1 cube in x/y
1467 // directions an 0 to -1 in z.
1468{
1469 *this = { Width / 2, 0, 0, LeftMargin + Width / 2,
1470 0, -Height / 2, 0, TopMargin + Height / 2,
1471 0, 0, MinZ - MaxZ, MinZ,
1472 0, 0, 0, 1 };
1473}
1474
1475template<typename Valtype>
1476bool Transformation<Valtype>::CreateCameraProjection(
1477 Valtype Fovy, Valtype Aspect, Valtype zn, Valtype zf ) noexcept
1478{
1479 if( zf <= zn ||
1480 zn <= 0.0f ||
1481 Aspect == 0.0f)
1482 return false;
1483
1484 Valtype n = zn;
1485 Valtype f = zf;
1486 Valtype w2 = n * tan( Fovy / 2 );
1487 Valtype h2 = w2 / Aspect;
1488
1489 if( w2 == 0.0f ||
1490 h2 == 0.0f )
1491 return false;
1492
1493 *this = { n/w2, 0, 0, 0,
1494 0, n/h2, 0, 0,
1495 0, 0, f/(f-n), -f*n/(f-n),
1496 0, 0, 1, 0 };
1497
1498 return true;
1499}
1500
1501template<typename Valtype>
1502bool Transformation<Valtype>::CreateOrthographicProjection(
1503 Valtype width, Valtype height, Valtype znear, Valtype zfar ) noexcept
1504{
1505 if( zfar <= znear ||
1506 znear <= 0.0f ||
1507 width <= 0.0f ||
1508 height <= 0.0f )
1509 return false;
1510
1511 Valtype n = znear;
1512 Valtype f = zfar;
1513 Valtype w = width;
1514 Valtype h = height;
1515
1516 *this = { 2/w, 0, 0, 0,
1517 0, 2/h, 0, 0,
1518 0, 0, -1/(f-n), -n/(f-n),
1519 0, 0, 0, 1 };
1520
1521 return true;
1522}
1523
1524template<typename Valtype>
1525Vector<Valtype> Transformation<Valtype>::TransformVector( const Vector<Valtype>& v ) const noexcept
1526{
1527 //If this assertion is not true a vector will not get transformed
1528 //as expected. The resulting vector would have a 4 - komponent. As
1529 //vectors are usually differences between two positions the resulting
1530 //vector would only with a 4 komponent be the difference vector between
1531 //the transformed positions. Without it, it is not usefull for most purposes.
1532 //Additionally be aware that normal vectors are covariant and have to
1533 //be transformed with the inverse transposed matrix.
1534 assert( (*this)(0,3) == 0 && (*this)(1,3) == 0 && (*this)(2,3) == 0 );
1535
1536 return Vector<Valtype>{
1537 operator()(0,0) * v.dx + operator()(1,0) * v.dy + operator()(2,0) * v.dz,
1538 operator()(0,1) * v.dx + operator()(1,1) * v.dy + operator()(2,1) * v.dz,
1539 operator()(0,2) * v.dx + operator()(1,2) * v.dy + operator()(2,2) * v.dz };
1540}
1541
1542//template<typename Valtype>
1543//Vector<Valtype> Transformation<Valtype>::TransformNormal( const Vector<Valtype>& v ) const
1544//{
1545// //If this assertion is not true a vector will not get transformed
1546// //as expected. Use the inverse transposed of the matrix used for position
1547// //transformation matrix to transform normals. Instead you may want to transform
1548// //the vector with TransformVector but be aware that this will not work correctly
1549// //for normals if scalings are involved.
1550// assert( (*this)(3,0) == 0 && (*this)(3,1) == 0 && (*this)(3,2) == 0 );
1551//
1552// Vector<Valtype> Retval(
1553// operator()(0,0) * v.dx + operator()(1,0) * v.dy + operator()(2,0) * v.dz,
1554// operator()(0,1) * v.dx + operator()(1,1) * v.dy + operator()(2,1) * v.dz,
1555// operator()(0,2) * v.dx + operator()(1,2) * v.dy + operator()(2,2) * v.dz );
1556//
1557// return Retval;
1558//}
1559
1560template<typename Valtype>
1561void Transformation<Valtype>::GetTranslation(
1562 Transformation& T ) const noexcept
1563{
1564 assert( IsValid() );
1565
1566 T(0,0) = 1;
1567 T(1,0) = 0;
1568 T(2,0) = 0;
1569
1570 T(0,1) = 0;
1571 T(1,1) = 1;
1572 T(2,1) = 0;
1573
1574 T(0,2) = 0;
1575 T(1,2) = 0;
1576 T(2,2) = 1;
1577
1578 T(3,0) = operator()(3,0);
1579 T(3,1) = operator()(3,1);
1580 T(3,2) = operator()(3,2);
1581 T(3,3) = 1;
1582
1583 T(0,3) = operator()(0,3);
1584 T(1,3) = operator()(1,3);
1585 T(2,3) = operator()(2,3);
1586}
1587
1588template<typename Valtype>
1589bool Transformation<Valtype>::GeRotation(
1590 Transformation& R ) const noexcept
1591{
1592 assert( IsValid() );
1593
1594 // M*ex = R*S*ex = R*sx -> sx = |M*ex| since |R*sx| = |sx|
1595 Valtype sx = std::sqrt( std::pow( operator()(0,0), Valtype{2} ) + std::pow( operator()(0,1), Valtype{2} ) + std::pow( operator()(0,2), Valtype{2} ) );
1596 Valtype sy = std::sqrt( std::pow( operator()(1,0), Valtype{2} ) + std::pow( operator()(1,1), Valtype{2} ) + std::pow( operator()(1,2), Valtype{2} ) );
1597 Valtype sz = std::sqrt( std::pow( operator()(2,0), Valtype{2} ) + std::pow( operator()(2,1), Valtype{2} ) + std::pow( operator()(2,2), Valtype{2} ) );
1598
1599 if( sx == 0 || sy == 0 || sz == 0 )
1600 // degenerated matrix
1601 return false;
1602
1603 // Invert Matrix:
1604 Valtype isx = 1/sx;
1605 Valtype isy = 1/sy;
1606 Valtype isz = 1/sz;
1607
1608 // R = M * Inv(S)
1609 R(0,0) = operator()(0,0) * isx;
1610 R(0,1) = operator()(0,1) * isx;
1611 R(0,2) = operator()(0,2) * isx;
1612 R(0,3) = 0;
1613
1614 R(1,0) = operator()(1,0) * isy;
1615 R(1,1) = operator()(1,1) * isy;
1616 R(1,2) = operator()(1,2) * isy;
1617 R(1,3) = 0;
1618
1619 R(2,0) = operator()(2,0) * isz;
1620 R(2,1) = operator()(2,1) * isz;
1621 R(2,2) = operator()(2,2) * isz;
1622 R(2,3) = 0;
1623
1624 R(3,0) = 0;
1625 R(3,1) = 0;
1626 R(3,2) = 0;
1627 R(3,3) = 1;
1628
1629 return true;
1630}
1631
1632template<typename Valtype>
1633bool Transformation<Valtype>::GetScaling(
1634 Transformation& S ) const noexcept
1635 // M*ex = R*S*ex = R*sx -> sx = |M*ex| since |R*sx| = |sx|
1636{
1637 assert( IsValid() );
1638
1639 S(0,0) = std::sqrt( std::pow( operator()(0,0), Valtype{2} ) + std::pow( operator()(0,1), Valtype{2} ) + std::pow( operator()(0,2), Valtype{2} ) );
1640 S(0,1) = 0;
1641 S(0,2) = 0;
1642 S(0,3) = 0;
1643
1644 S(1,0) = 0;
1645 S(1,1) = std::sqrt( std::pow( operator()(1,0), Valtype{2} ) + std::pow( operator()(1,1), Valtype{2} ) + std::pow( operator()(1,2), Valtype{2} ) );
1646 S(1,2) = 0;
1647 S(1,3) = 0;
1648
1649 S(2,0) = 0;
1650 S(2,1) = 0;
1651 S(2,2) = std::sqrt( std::pow( operator()(2,0), Valtype{2} ) + std::pow( operator()(2,1), Valtype{2} ) + std::pow( operator()(2,2), Valtype{2} ) );
1652 S(2,3) = 0;
1653
1654 S(3,0) = 0;
1655 S(3,1) = 0;
1656 S(3,2) = 0;
1657 S(3,3) = 1;
1658
1659 if( S(0,0) == 0 || S(1,1) == 0 || S(2,2) == 0 )
1660 return false;
1661
1662 return true;
1663}
1664
1665template<typename Valtype>
1666bool Transformation<Valtype>::Dismantle( Transformation& T,
1667 Transformation& R,
1668 Transformation& S ) const noexcept
1669{
1670 if( !GetScaling( S ) )
1671 return false;
1672
1673 GetTranslation( T );
1674
1675 // Invert Matrix:
1676 Valtype isx = 1/S(0,0);
1677 Valtype isy = 1/S(1,1);
1678 Valtype isz = 1/S(2,2);
1679
1680 // R = T * Inv(S)
1681 R(0,0) = operator()(0,0) * isx;
1682 R(0,1) = operator()(0,1) * isx;
1683 R(0,2) = operator()(0,2) * isx;
1684 R(0,3) = 0;
1685
1686 R(1,0) = operator()(1,0) * isy;
1687 R(1,1) = operator()(1,1) * isy;
1688 R(1,2) = operator()(1,2) * isy;
1689 R(1,3) = 0;
1690
1691 R(2,0) = operator()(2,0) * isz;
1692 R(2,1) = operator()(2,1) * isz;
1693 R(2,2) = operator()(2,2) * isz;
1694 R(2,3) = 0;
1695
1696 R(3,0) = 0;
1697 R(3,1) = 0;
1698 R(3,2) = 0;
1699 R(3,3) = 1;
1700
1701 return true;
1702}
1703
1704template<typename Valtype>
1705inline bool Transformation<Valtype>::IsValid() const noexcept
1706{
1707 if( !common::Equals(operator()(3,3), Valtype{1}, Valtype{0.001} ) )
1708 return false;
1709
1710 return true;
1711}
1712
1713template< typename Valtype,
1714 typename Valtype2 > inline
1715auto operator*( const Transformation<Valtype>& first,
1716 const Transformation<Valtype2>& second ) noexcept -> Transformation<decltype(Valtype{}*Valtype2{})>
1717{
1718 return operator*( static_cast<const Matrix<Valtype,4,4>&>( first ), static_cast<const Matrix<Valtype2,4,4>&>( second ) );
1719}
1720
1721template<typename Valtype>
1722bool Slerp( Transformation<Valtype>& out,
1723 const Transformation<Valtype>& A,
1724 const Transformation<Valtype>& B,
1725 Valtype t )
1726{
1727 assert( A.IsValid() );
1728 assert( B.IsValid() );
1729
1730 // B = M * A; means M transforms A to B;
1731 Transformation<Valtype> iA{ A };
1732 iA.Invert();
1733 Transformation<Valtype> M = B * iA;
1734
1735 Transformation<Valtype> T,R,S;
1736
1737 if( !M.Dismantle( T, R, S ) )
1738 return false;
1739
1740 // Interpolate translation;
1741 T(3,0) *= t;
1742 T(3,1) *= t;
1743 T(3,2) *= t;
1744
1745 T(0,3) *= t;
1746 T(1,3) *= t;
1747 T(2,3) *= t;
1748
1749 // Interpolate rotation spherical linearly (slerp);
1750 Rotation<Valtype> Rot{ R };
1751
1752 const Vector<Valtype> Axis = Rot.RotationAxis();
1753 Valtype Angle = Rot.RotationAngle();
1754
1755 Angle *= t;
1756 Rot.CreateFromAxis( Angle * Axis );
1757
1758 R = Rot;
1759
1760 // Interpolate the scalation;
1761 S(0,0) = 1 + (S(0,0) - 1) * t;
1762 S(1,1) = 1 + (S(1,1) - 1) * t;
1763 S(2,2) = 1 + (S(2,2) - 1) * t;
1764
1765 // put all together again;
1766 out = T * R * S * A;
1767
1768 assert( out.IsValid() );
1769 return true;
1770}
1771
1772template<typename Valtype, typename ValtypeT>
1773inline Frame<Valtype,ValtypeT> Invert( Frame<Valtype,ValtypeT> frame )
1774{
1775 Transformation<ValtypeT> T{ frame };
1776 T.Invert();
1777 frame = T;
1778 return frame;
1779}
1780
1781template<typename Valtype>
1782inline Rotation<Valtype>::Rotation( const SquareMatrix<Valtype,3>& matrix )
1783 : SquareMatrix<Valtype,3>{matrix}
1784{
1785}
1786
1787template<typename Valtype>
1788inline Rotation<Valtype>::Rotation( SquareMatrix<Valtype,3>&& matrix ) noexcept
1789 : SquareMatrix<Valtype,3>{std::move(matrix)}
1790{
1791}
1792
1793template<typename Valtype>
1794inline Rotation<Valtype>::Rotation( const Basetype& matrix )
1795 : SquareMatrix<Valtype,3>{matrix}
1796{
1797}
1798
1799template<typename Valtype>
1800inline Rotation<Valtype>::Rotation( Basetype&& matrix ) noexcept
1801 : SquareMatrix<Valtype,3>{std::move(matrix)}
1802{
1803}
1804
1805template<typename Valtype>
1806inline Rotation<Valtype>::Rotation( const Vector<Valtype>& axis )
1807 : SquareMatrix<Valtype,3>{}
1808{
1809 CreateFromAxis( axis );
1810}
1811
1812template<typename Valtype>
1813inline Rotation<Valtype>::Rotation( Valtype q0, Valtype q1, Valtype q2, Valtype q3 ) noexcept
1814 : SquareMatrix<Valtype,3>
1815{ 2*(q0*q0+q1*q1)-1, 2*(q1*q2-q0*q3), 2*(q1*q3+q0*q2),
1816 2*(q1*q2+q0*q3), 2*(q0*q0+q2*q2)-1, 2*(q2*q3-q0*q1),
1817 2*(q1*q3-q0*q2), 2*(q2*q3+q0*q1), 2*(q0*q0+q3*q3)-1 }
1818{
1819}
1820
1821template<typename Valtype>
1822template<typename ValtypeP>
1823Rotation<Valtype>::Rotation( const Frame<ValtypeP,Valtype>& frame )
1824 : SquareMatrix<Valtype,3>{}
1825{
1826 operator=( frame );
1827}
1828
1829template<typename Valtype>
1830template<typename Valtype2>
1831Rotation<Valtype>::Rotation( const Transformation<Valtype2>& tr )
1832 : SquareMatrix<Valtype,3>{}
1833{
1834 Transformation<Valtype2> R;
1835 tr.GeRotation( R );
1836
1837 operator()(0,0) = static_cast<Valtype>(R(0,0));
1838 operator()(1,0) = static_cast<Valtype>(R(1,0));
1839 operator()(2,0) = static_cast<Valtype>(R(2,0));
1840
1841 operator()(0,1) = static_cast<Valtype>(R(0,1));
1842 operator()(1,1) = static_cast<Valtype>(R(1,1));
1843 operator()(2,1) = static_cast<Valtype>(R(2,1));
1844
1845 operator()(0,2) = static_cast<Valtype>(R(0,2));
1846 operator()(1,2) = static_cast<Valtype>(R(1,2));
1847 operator()(2,2) = static_cast<Valtype>(R(2,2));
1848}
1849
1850template<typename Valtype>
1851inline Rotation<Valtype>::Rotation(
1852 Valtype r00, Valtype r10, Valtype r20,
1853 Valtype r01, Valtype r11, Valtype r21,
1854 Valtype r02, Valtype r12, Valtype r22 )
1855 : SquareMatrix<Valtype,3>{ r00, r10, r20,
1856 r01, r11, r21,
1857 r02, r12, r22 }
1858{
1859}
1860
1861template<typename Valtype>
1862template<typename ValtypeP>
1863inline Rotation<Valtype>& Rotation<Valtype>::operator=( const Frame<ValtypeP,Valtype>& frame ) noexcept
1864{
1865 operator()(0,0) = frame.T.dx;
1866 operator()(0,1) = frame.T.dy;
1867 operator()(0,2) = frame.T.dz;
1868
1869 operator()(1,0) = frame.N.dx;
1870 operator()(1,1) = frame.N.dy;
1871 operator()(1,2) = frame.N.dz;
1872
1873 operator()(2,0) = frame.B.dx;
1874 operator()(2,1) = frame.B.dy;
1875 operator()(2,2) = frame.B.dz;
1876
1877 return *this;
1878}
1879
1880template<typename Valtype>
1881void Rotation<Valtype>::CreateFromAxis( const Vector<Valtype>& a )
1882// Once and for all: this is a rotation with an axis according to the right-hand-rule. (Thumb
1883// points in vector direction, fingers show the rotation)
1884//
1885// a: rotation vector
1886// theta: rotating angle theta = |a|
1887// u: normalised rotation vector
1888// v: vector to be rotated
1889// v': rotated vector (v'=R*v;)
1890// vp: parallel part of v with respect to u
1891// vo: orthogonal part of v with respect to u
1892// I : Identity matrix
1893//
1894// it holds:
1895// v = vp + vo (1)
1896// vo = -u % (u % v) (2)
1897//
1898// if we construct an orthogonal base for the rotation with:
1899//
1900// rx = vo/|vo|
1901// ry = u % vo/|vo| = u % v/|vo|
1902// rz = u
1903//
1904// we can write vo' as
1905//
1906// vo' = |vo|*cos(theta) * rx + |vo|*sin(theta) * ry
1907// = cos(theta) * vo + sin(theta) * (u % v) (3)
1908//
1909// v' = vp'+ vo'
1910// = vp + vo'
1911// (3)
1912// = vp + cos(theta) * vo + sin(theta) * (u % v)
1913// = vp + vo - vo + cos(theta) * vo + sin(theta) * (u % v)
1914// = v - (1 - cos(theta)) * vo + sin(theta) * (u % v)
1915// (2)
1916// = v + (1 - cos(theta)) * u % (u % v) + sin(theta) * (u % v) (4)
1917//
1918// We can write
1919//
1920// (u2v3 - u3v2) ( 0 -u3 u2) (v1)
1921// u % v = (u3v1 - u1v3) = ( u3 0 -u1) * (v2) = S * v
1922// (u1v2 - u2v1) (-u2 u1 0) (v3)
1923// with
1924//
1925// 0 -u3 u2
1926// S = u3 0 -u1
1927// -u2 u1 0
1928//
1929// So (4) gives:
1930//
1931// R = I + sin(theta) * S + (1 - cos(theta)) * S * S
1932{
1933 Vector<Valtype> u{ a };
1934 Valtype theta = u.Normalize();
1935
1936 SquareMatrix<Valtype,3> S{ 0, -u.dz, u.dy,
1937 u.dz, 0, -u.dx,
1938 -u.dy, u.dx, 0 };
1939
1940 SquareMatrix<Valtype,3> I{ 1, 0, 0,
1941 0, 1, 0,
1942 0, 0, 1 };
1943
1944 *this = I + sin(theta) * S + (1 - cos(theta)) * S * S;
1945}
1946
1947template<typename Valtype>
1948inline void Rotation<Valtype>::CreateFromAxis( Valtype rx, Valtype ry, Valtype rz ) noexcept
1949{
1950 CreateFromAxis( Vector<Valtype>(rx,ry,rz) );
1951}
1952
1953template<typename Valtype>
1954inline void Rotation<Valtype>::Rotate( Vector<Valtype>& v ) const noexcept{
1955 v = operator*( v );
1956}
1957
1958template<typename Valtype>
1959template<typename ValtypeP>
1960inline void Rotation<Valtype>::Rotate( Frame<ValtypeP,Valtype>& frame ) const noexcept{
1961 frame.T = operator*( frame.T );
1962 frame.N = operator*( frame.N );
1963 frame.B = operator*( frame.B );
1964}
1965
1966template<typename Valtype>
1967inline Position<Valtype> Rotation<Valtype>::operator*(
1968 const Position<Valtype>& p ) const noexcept
1969{
1970 return Position<Valtype>{
1971 (*this)(0,0) * p.x + (*this)(1,0) * p.y + (*this)(2,0) * p.z,
1972 (*this)(0,1) * p.x + (*this)(1,1) * p.y + (*this)(2,1) * p.z,
1973 (*this)(0,2) * p.x + (*this)(1,2) * p.y + (*this)(2,2) * p.z };
1974}
1975
1976template<typename Valtype>
1977inline Vector<Valtype> Rotation<Valtype>::operator*(
1978 const Vector<Valtype>& v ) const noexcept
1979{
1980 return Vector<Valtype>{
1981 (*this)(0,0) * v.dx + (*this)(1,0) * v.dy + (*this)(2,0) * v.dz,
1982 (*this)(0,1) * v.dx + (*this)(1,1) * v.dy + (*this)(2,1) * v.dz,
1983 (*this)(0,2) * v.dx + (*this)(1,2) * v.dy + (*this)(2,2) * v.dz };
1984}
1985
1986//template<typename Valtype>
1987//inline Vector<Valtype> Rotation<Valtype>::RotationAxis() const noexcept
1988// // Source D. Paull. Charles River Media 2002
1989// // This at least is proven to work with the
1990// // rotations around the coordinate axes.
1992//{
1993// //Vector<Valtype> Axis{ (*this)(1,2) - (*this)(2,1),
1994// // (*this)(2,0) - (*this)(0,2),
1995// // (*this)(0,1) - (*this)(1,0) };
1996//
1997// //Axis.Normalize();
1998// //return Axis;
1999//
2000// namespace ublas = boost::numeric::ublas;
2001//
2002// // Define the rotation matrix R
2003// ublas::matrix<Valtype> R{3,3};
2004//
2005// // Assign values to the elements of R
2006// // Example:
2007// R(0,0) = (*this)(0,0); R(0,1) = (*this)(0,1); R(0,2) = (*this)(0,2);
2008// R(1,0) = (*this)(1,0); R(1,1) = (*this)(1,1); R(1,2) = (*this)(1,2);
2009// R(2,0) = (*this)(2,0); R(2,1) = (*this)(2,1); R(2,2) = (*this)(2,2);
2010//
2011// // Subtract identity matrix from R
2012// ublas::matrix<Valtype> A = R - ublas::identity_matrix<Valtype>(3);
2013//
2014// // Define the right-hand side vector (zero vector)
2015// ublas::vector<Valtype> b( 3, 0.0 );
2016//
2017// // Solve the system of linear equations Ax = b
2018// ublas::permutation_matrix<std::size_t> pm(A.size1());
2019// ublas::lu_factorize(A, pm);
2020// ublas::lu_substitute(A, pm, b);
2021//
2022// Vector<Valtype> axis{ b(0), b(1), b(2) };
2023// axis.Normalize();
2024// return axis;
2025//}
2026
2027template<typename Valtype>
2028inline Vector<Valtype> Rotation<Valtype>::RotationAxis() const noexcept
2029{
2030#ifdef TRAX_OPEN_SOURCE
2031 // Define the rotation matrix R
2032 Eigen::Matrix3d R;
2033
2034 // Assign values to the elements of R
2035 // Example:
2036 R(0,0) = (*this)(0,0); R(0,1) = (*this)(0,1); R(0,2) = (*this)(0,2);
2037 R(1,0) = (*this)(1,0); R(1,1) = (*this)(1,1); R(1,2) = (*this)(1,2);
2038 R(2,0) = (*this)(2,0); R(2,1) = (*this)(2,1); R(2,2) = (*this)(2,2);
2039
2040 // Subtract identity matrix from R
2041 Eigen::Matrix3d A = R - Eigen::Matrix3d::Identity();
2042
2043 // Define the right-hand side vector (zero vector)
2044 Eigen::Vector3d b{ 0.0, 0.0, 0.0 };
2045
2046 // Solve the system of linear equations Ax = b
2047 Eigen::JacobiSVD<Eigen::Matrix3d> svd{ A, Eigen::ComputeFullU | Eigen::ComputeFullV };
2048 Eigen::Vector3d x = svd.matrixV().col(2); // Extract the last column of the right singular vectors matrix
2049
2050 Vector<Valtype> axis{ static_cast<Valtype>(x(0)), static_cast<Valtype>(x(1)), static_cast<Valtype>(x(2)) };
2051 axis.Normalize();
2052 return axis;
2053#else
2054 return Null<Valtype>;
2055#endif
2056}
2057
2058template<typename Valtype>
2059Valtype Rotation<Valtype>::RotationAngle() const noexcept
2060{
2061 Valtype trace = SquareMatrix<Valtype,3>::Trace();
2062
2063 Valtype cosangle = common::Clamp( (trace-Valtype{1}) / 2, static_cast<Valtype>(-1), static_cast<Valtype>(+1) );
2064 return acos( cosangle );
2065}
2066
2067template< typename Valtype,
2068 typename Valtype2 > inline
2069auto operator*( const Rotation<Valtype>& first,
2070 const Rotation<Valtype2>& second ) noexcept -> Rotation<decltype(Valtype{}*Valtype2{})>
2071{
2072 return operator*( static_cast<const Matrix<Valtype,3,3>&>( first ), static_cast<const Matrix<Valtype2,3,3>&>( second ) );
2073}
2075} // namespace spat
spat::Matrix
Matrix template for arbitrary dimensions and value type.
Definition Matrix.h:63
spat::Matrix< Valtype, nColsAndRows, nColsAndRows >::SubMatrix
Matrix< Valtype, nCols-1, nRows-1 > SubMatrix(unsigned short c, unsigned short r) const
spat::Matrix::operator-=
void operator-=(const Matrix &matrix) noexcept
Matrix subtraction.
Definition Matrix.h:845
spat::Matrix< Valtype, nColsAndRows, nColsAndRows >::Rows
constexpr unsigned short Rows() const noexcept
spat::Matrix< Valtype, nColsAndRows, nColsAndRows >::Cols
constexpr unsigned short Cols() const noexcept
spat::Matrix::operator!=
bool operator!=(const Matrix &matrix) const noexcept
Comparison for inequality.
Definition Matrix.h:812
spat::Matrix::operator*=
void operator*=(Valtype skalar) noexcept
Matrix * Skalar multiplication.
Definition Matrix.h:817
spat::Matrix::operator/=
void operator/=(Valtype skalar) noexcept
Matrix / Skalar division.
Definition Matrix.h:831
spat::Matrix::operator+=
void operator+=(const Matrix &matrix) noexcept
Matrix addition.
Definition Matrix.h:838
spat::Matrix::operator==
bool operator==(const Matrix &matrix) const noexcept
Comparison for equality.
Definition Matrix.h:807
spat::Matrix< Valtype, nColsAndRows, nColsAndRows >::IsEqual
bool IsEqual(const Matrix &matrix, Valtype delta=0) const noexcept
spat::Matrix::SetNull
void SetNull() noexcept
Sets all elements to 0.
Definition Matrix.h:780
spat::Rotation
Rotation matrix.
Definition Matrix.h:607
spat::Rotation::RotationAngle
Valtype RotationAngle() const noexcept
Angle of the rotation described by this matrix.
Definition Matrix.h:2059
spat::Rotation::Basetype
typename SquareMatrix< Valtype, 3 >::Basetype Basetype
Utmost base type of this Rotation.
Definition Matrix.h:609
spat::Rotation::RotationAxis
Vector< Valtype > RotationAxis() const noexcept
Axis of the rotation described by this matrix.
Definition Matrix.h:2028
spat::Rotation::CreateFromAxis
void CreateFromAxis(const Vector< Valtype > &axis)
This constructs a rotation with an axis according to the right-hand-rule. (Thumb points in vector dir...
Definition Matrix.h:1881
spat::SquareMatrix
Square matrix with nColumns == nRows.
Definition Matrix.h:278
spat::SquareMatrix< Valtype, 3 >::Trace
Valtype Trace() const noexcept
Definition Matrix.h:1113
spat::SquareMatrix< Valtype, 3 >::SetIdentity
void SetIdentity() noexcept
Definition Matrix.h:1055
spat::SquareMatrix< Valtype, 3 >::Transpose
SquareMatrix & Transpose() noexcept
Definition Matrix.h:1094
spat::SquareMatrix< Valtype, 3 >::Invert
SquareMatrix & Invert()
Definition Matrix.h:1103
spat::SquareMatrix< Valtype, 3 >::IsSymmetric
bool IsSymmetric(Valtype epsilon=0) const noexcept
Definition Matrix.h:1081
spat::SquareMatrix< Valtype, 3 >::IsDiagonal
bool IsDiagonal() const noexcept
Definition Matrix.h:1071
spat::SquareMatrix::Basetype
Matrix< Valtype, nColsAndRows, nColsAndRows > Basetype
Utmost base type of this SquareMatrix.
Definition Matrix.h:280
spat::SquareMatrix< Valtype, 3 >::IsIdentity
bool IsIdentity(Valtype epsilon=0) const noexcept
Definition Matrix.h:1062
spat::Transformation
Homogenous transformation matrix.
Definition Matrix.h:387
spat::Transformation::IsValid
bool IsValid() const noexcept
Tests wether the matrix is a valid homogenous transformation.
Definition Matrix.h:1705
spat::Transformation::CreateMirror
void CreateMirror(const VectorBundle< Valtype2, Valtype > &mirror)
Create a view matrix that transforms to camera space.
Definition Matrix.h:1417
spat::Transformation::TransformVector
Vector< Valtype > TransformVector(const Vector< Valtype > &vec) const noexcept
Vector transformation. Use it for difference vectors between positions or dynamic vectors like veloci...
Definition Matrix.h:1525
spat::Transformation::GeRotation
bool GeRotation(Transformation &R) const noexcept
Calculates a rotational part of the transformation so that T * R * S == *this.
Definition Matrix.h:1589
spat::Transformation::GetScaling
bool GetScaling(Transformation &S) const noexcept
Calculates a scaling part of the transformation so that T * R * S == *this.
Definition Matrix.h:1633
spat::Transformation::CreateViewport
void CreateViewport(Valtype Width, Valtype Height, Valtype MinZ, Valtype MaxZ, Valtype LeftMargin=0.0f, Valtype TopMargin=0.0f) noexcept
Creates a viewport projection.
Definition Matrix.h:1459
spat::Transformation::LookAt
void LookAt(const Position< Valtype > &at, const Position< Valtype > &eye, const Vector< Valtype > &up) noexcept
Create a view matrix that transforms to camera space.
Definition Matrix.h:1428
spat::Transformation::CreateCameraProjection
bool CreateCameraProjection(Valtype fovy, Valtype Aspect, Valtype zn, Valtype zf) noexcept
Creates camera projection transformation.
Definition Matrix.h:1476
spat::Transformation::Dismantle
bool Dismantle(Transformation &T, Transformation &R, Transformation &S) const noexcept
Calculates translational, rotational and scaling part of the transformation so that T * R * S == *thi...
Definition Matrix.h:1666
spat::Transformation::CreateOrthographicProjection
bool CreateOrthographicProjection(Valtype width, Valtype height, Valtype znear, Valtype zfar) noexcept
Creates orthographic projection transformation.
Definition Matrix.h:1502
spat::Transformation::GetTranslation
void GetTranslation(Transformation &T) const noexcept
Kovariant vector use this for face normals or vectors denoting a rotation orientation.
Definition Matrix.h:1561
common::operator*
constexpr auto operator*(const Interval< Valtype > &i1, Valtype2 scalar) noexcept -> Interval< decltype(Valtype{} *Valtype2{})>
Interval operator.
Definition Interval.h:628
common::pow
constexpr T pow(T val) noexcept
power function with templated integer exponent.
Definition Helpers.h:61
common::Clamp
constexpr const T & Clamp(const T &v, const T &lo, const T &hi) noexcept
Clips a val to a specified range.
Definition Helpers.h:105
common::Equals
constexpr bool Equals(T a, T b, T epsilon) noexcept
Tests equality in the sense |a-b| < epsilon.
Definition Helpers.h:66
common::operator*=
constexpr Interval< Valtype > & operator*=(Interval< Valtype > &i1, Valtype2 scalar) noexcept
Interval operator.
Definition Interval.h:638
common::operator/=
constexpr Interval< Valtype > & operator/=(Interval< Valtype > &i1, Valtype2 scalar) noexcept
Interval operator.
Definition Interval.h:650
common::operator+
constexpr Interval< Valtype > operator+(const Interval< Valtype > &i1, const Interval< Valtype > &i2) noexcept
Interval operator.
Definition Interval.h:591
common::operator-
constexpr Interval< Valtype > operator-(const Interval< Valtype > &i1, Valtype l) noexcept
Interval operator.
Definition Interval.h:613
spat
The namespace provides classes and methods for spatial computations.
Definition Box.h:32
spat::operator*
Position< Valtype2 > operator*(const Frame< Valtype1, ValtypeT1 > &frame, const Position< Valtype2 > &p) noexcept
Frame operator.
Definition Frame.h:1089
spat::Invert
Frame< Valtype, ValtypeT > Invert(const Frame< Valtype, ValtypeT > frame)
Inverts the frame, so that FI * F == Identity.
Definition Matrix.h:1773
spat::Transposed
constexpr Matrix< Valtype, nRows, nCols > Transposed(const Matrix< Valtype, nCols, nRows > &m) noexcept
Definition Matrix.h:888
spat::Determinant
constexpr auto Determinant(const SquareMatrix< Valtype, nColsAndRows > &m) -> decltype(pow< nColsAndRows >(Valtype{}))
Determinant of the matrix.
Definition Matrix.h:1147
spat::IdentityRotation
const Rotation< Valtype > IdentityRotation
Rotation that leaves the transformed object unchanged.
Definition Matrix.h:689
spat::Row
constexpr Vector< Valtype > Row(const Matrix< Valtype, 3, nRows > &m, unsigned short idx) noexcept
Definition Matrix.h:883
spat::IdentityTransformation
const Transformation< Valtype > IdentityTransformation
Transformation that leaves the transformed object unchanged.
Definition Matrix.h:567
spat::Slerp
bool Slerp(Transformation< Valtype > &out, const Transformation< Valtype > &inA, const Transformation< Valtype > &inB, Valtype t)
Spherical interpolates two matrices weighting them by t.
Definition Matrix.h:1722
spat::AdjungatedMatrix
SquareMatrix< Valtype, nColsAndRows > AdjungatedMatrix(const SquareMatrix< Valtype, nColsAndRows > &m)
Matrix of adjungated values.
Definition Matrix.h:1167
spat::Inverted
constexpr SquareMatrix< Valtype, nColsAndRows > Inverted(const SquareMatrix< Valtype, nColsAndRows > &m)
Definition Matrix.h:1130
spat::Origin3D
constexpr Position< Valtype > Origin3D
Origin of coordinate system.
Definition Position.h:143
spat::Column
constexpr Vector< Valtype > Column(const Matrix< Valtype, nCols, 3 > &m, unsigned short idx) noexcept
Definition Matrix.h:878
std
STL namespace.
spat::Frame
A Frame ("TNBFrame") describes a location in 3d space and an orientation using a right handed coordin...
Definition Frame.h:52
spat::PositionH
Implements a 4D - position in homogenous coordinates.
Definition PositionH.h:41
spat::Position
Implements a 3D - position in cartesian coordinates.
Definition Position.h:46
spat::Vector2D
Implements a 2D - vector in cartesian coordinates.
Definition Vector2D.h:46
spat::VectorBundle
Implements a Vector bundle.
Definition VectorBundle.h:42
spat::VectorBundle::P
Position< Valtype > P
Base space postion.
Definition VectorBundle.h:43
spat::VectorBundle::T
Vector< ValtypeT > T
Tangent vector or x-axis.
Definition VectorBundle.h:44
spat::Vector
Implements a 3D - vector in cartesian coordinates.
Definition Vector.h:48
spat::Vector::dy
Valtype dy
cartesian y component.
Definition Vector.h:52
spat::Vector::dx
Valtype dx
cartesian x component.
Definition Vector.h:51
spat::Vector::dz
Valtype dz
cartesian z component.
Definition Vector.h:53
spat::Vector::Normalize
auto Normalize() noexcept -> decltype(Valtype{}/Valtype{})
Normalizes the vector to length 1.
Definition Vector.h:487