BELFEM 0.9.0
Berkeley Lab Finite Element Framework
Loading...
Searching...
No Matches
fn_rotation_matrix.hpp
Go to the documentation of this file.
1/*
2 * BELFEM -- The Berkeley Lab Finite Element Framework
3 * Copyright (c) 2026, The Regents of the University of California,
4 * through Lawrence Berkeley National Laboratory (subject to receipt of any required
5 * approvals from the U.S. Dept. of Energy). All rights reserved.
6 *
7 * Developers: Christian Messe, Gregory Giard
8 *
9 * See the top-level LICENSE file for the complete license and disclaimer.
10 */
11
12#ifndef BELFEM_FN_ROTATION_MATRIX_HPP
13#define BELFEM_FN_ROTATION_MATRIX_HPP
14
15#include "cl_Vector.hpp"
16#include "cl_Matrix.hpp"
17#include "assert.hpp"
18
19namespace belfem
20{
21//------------------------------------------------------------------------------
22
31 template < typename T >
32 void
33 rotation_matrix(
34 const Vector< T > & aAxis,
35 const T & aAngle,
36 Matrix< real > & aMatrix )
37 {
38 BELFEM_ASSERT( aAxis.length() == 3,
39 "Input vector must be of length 3." );
40
41 BELFEM_ASSERT( aMatrix.n_rows() == 3 && aMatrix.n_cols() == 3,
42 "Input matrix must be a 3x3 matrix" );
43
44 T c = std::cos( aAngle );
45 T s = std::sin( aAngle );
46 T d = 1.0 - c;
47
48#ifdef BELFEM_ARMADILLO
49 T * tData = aMatrix.data() ;
50
51 tData[ 0 ] = aAxis( 0 ) * aAxis ( 0 ) * d + c;
52 tData[ 1 ] = aAxis( 1 ) * aAxis ( 0 ) * d + aAxis( 2 ) * s;
53 tData[ 2 ] = aAxis( 2 ) * aAxis ( 0 ) * d - aAxis( 1 ) * s;
54
55 tData[ 3 ] = aAxis( 0 ) * aAxis ( 1 ) * d - aAxis( 2 ) * s;
56 tData[ 4 ] = aAxis( 1 ) * aAxis ( 1 ) * d + c;
57 tData[ 5 ] = aAxis( 2 ) * aAxis ( 1 ) * d + aAxis( 0 ) * s;
58
59 tData[ 6 ] = aAxis( 0 ) * aAxis ( 2 ) * d + aAxis( 1 ) * s;
60 tData[ 7 ] = aAxis( 1 ) * aAxis ( 2 ) * d - aAxis( 0 ) * s;
61 tData[ 8 ] = aAxis( 2 ) * aAxis ( 2 ) * d + c;
62#elif BELFEM_BLAZE
63 aMatrix( 0, 0 ) = aAxis( 0 ) * aAxis ( 0 ) * d + c;
64 aMatrix( 1, 0 ) = aAxis( 1 ) * aAxis ( 0 ) * d + aAxis( 2 ) * s;
65 aMatrix( 2, 0 ) = aAxis( 2 ) * aAxis ( 0 ) * d - aAxis( 1 ) * s;
66
67 aMatrix( 0, 1 ) = aAxis( 0 ) * aAxis ( 1 ) * d - aAxis( 2 ) * s;
68 aMatrix( 1, 1 ) = aAxis( 1 ) * aAxis ( 1 ) * d + c;
69 aMatrix( 2, 1 ) = aAxis( 2 ) * aAxis ( 1 ) * d + aAxis( 0 ) * s;
70
71 aMatrix( 0, 2 ) = aAxis( 0 ) * aAxis ( 2 ) * d + aAxis( 1 ) * s;
72 aMatrix( 1, 2 ) = aAxis( 1 ) * aAxis ( 2 ) * d - aAxis( 0 ) * s;
73 aMatrix( 2, 2 ) = aAxis( 2 ) * aAxis ( 2 ) * d + c;
74#endif
75
76 }
77
78//------------------------------------------------------------------------------
79
80 template < typename T >
81 void
82 rotation_matrix_strip(
83 const Vector< T > & aAxis,
84 const T & aAngle,
85 Matrix< real > & aMatrix )
86 {
87 BELFEM_ASSERT( aAxis.length() == 3,
88 "Input vector must be of length 3." );
89
90 BELFEM_ASSERT( aMatrix.n_rows() == 2 && aMatrix.n_cols() == 3,
91 "Input matrix must be a 2x3 matrix" );
92
93 T c = std::cos( aAngle );
94 T s = std::sin( aAngle );
95 T d = 1.0 - c;
96
97 aMatrix( 0, 0 ) = aAxis( 0 ) * aAxis ( 0 ) * d + c;
98 aMatrix( 1, 0 ) = aAxis( 1 ) * aAxis ( 0 ) * d + aAxis( 2 ) * s;
99 //aMatrix( 2, 0 ) = aAxis( 2 ) * aAxis ( 0 ) * d - aAxis( 1 ) * s;
100
101 aMatrix( 0, 1 ) = aAxis( 0 ) * aAxis ( 1 ) * d - aAxis( 2 ) * s;
102 aMatrix( 1, 1 ) = aAxis( 1 ) * aAxis ( 1 ) * d + c;
103 //aMatrix( 2, 1 ) = aAxis( 2 ) * aAxis ( 1 ) * d + aAxis( 0 ) * s;
104
105 aMatrix( 0, 2 ) = aAxis( 0 ) * aAxis ( 2 ) * d + aAxis( 1 ) * s;
106 aMatrix( 1, 2 ) = aAxis( 1 ) * aAxis ( 2 ) * d - aAxis( 0 ) * s;
107 //aMatrix( 2, 2 ) = aAxis( 2 ) * aAxis ( 2 ) * d + c;
108 }
109
110//------------------------------------------------------------------------------
111
122 template < typename T >
123 void
124 rotation_matrix(
125 const T & aYaw,
126 const T & aPitch,
127 const T & aRoll,
128 Matrix< real > & aMatrix )
129 {
130 BELFEM_ASSERT( aMatrix.n_rows() == 3 && aMatrix.n_cols() == 3,
131 "Input matrix must be a 3x3 matrix" );
132
133 const real sa = std::sin( aRoll );
134 const real ca = std::cos( aRoll );
135
136 const real sb = std::sin( aPitch );
137 const real cb = std::cos( aPitch );
138
139 const real sc = std::sin( aYaw );
140 const real cc = std::cos( aYaw );
141
142#ifdef BELFEM_ARMADILLO
143 T * tData = aMatrix.data() ;
144
145 tData[ 0 ] = ca*cb ;
146 tData[ 1 ] = cb*sa ;
147 tData[ 2 ] = -sb ;
148
149 tData[ 3 ] = ca*sb*sc - cc*sa ;
150 tData[ 4 ] = ca*cc + sa*sb*sc ;
151 tData[ 5 ] = cb*sc ;
152
153 tData[ 6 ] = sa*sc + ca*cc*sb ;
154 tData[ 7 ] = cc*sa*sb - ca*sc ;
155 tData[ 8 ] = cb*cc;
156#elif BELFEM_BLAZE
157 // Use the (i,j) accessor: Blaze pads columns for SIMD alignment, so
158 // the inter-column stride is not necessarily 3 and manual offsets
159 // into aMatrix.data() are unreliable.
160 aMatrix( 0, 0 ) = ca*cb ;
161 aMatrix( 1, 0 ) = cb*sa ;
162 aMatrix( 2, 0 ) = -sb ;
163
164 aMatrix( 0, 1 ) = ca*sb*sc - cc*sa ;
165 aMatrix( 1, 1 ) = ca*cc + sa*sb*sc ;
166 aMatrix( 2, 1 ) = cb*sc ;
167
168 aMatrix( 0, 2 ) = sa*sc + ca*cc*sb ;
169 aMatrix( 1, 2 ) = cc*sa*sb - ca*sc ;
170 aMatrix( 2, 2 ) = cb*cc;
171#endif
172
173//------------------------------------------------------------------------------
174 }
175}
176#endif //BELFEM_FN_ROTATION_MATRIX_HPP
BELFEM_ASSERT
#define BELFEM_ASSERT(aCheck,...)
Definition assert.hpp:244
cl_Matrix.hpp
cl_Vector.hpp
belfem::Matrix::n_rows
size_t n_rows() const
Definition cl_AR_Matrix.hpp:205
belfem::Matrix::n_cols
size_t n_cols() const
Definition cl_AR_Matrix.hpp:213
belfem::Matrix::data
T * data()
Definition cl_AR_Matrix.hpp:135
belfem::Vector
Column vector.
Definition cl_BZ_Vector.hpp:41
belfem::Vector::length
size_t length() const
get the length of the vector
Definition cl_AR_Vector.hpp:257
belfem
USER GUIDES:
Definition cl_Capacitor.cpp:16
belfem::rotation_matrix_strip
void rotation_matrix_strip(const Vector< T > &aAxis, const T &aAngle, Matrix< real > &aMatrix)
Definition fn_rotation_matrix.hpp:82
belfem::rotation_matrix
void rotation_matrix(const Vector< T > &aAxis, const T &aAngle, Matrix< real > &aMatrix)
rotate around an axis with an angle
Definition fn_rotation_matrix.hpp:33
belfem::real
double real
Definition typedefs.hpp:36