1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81
|
#include <iostream>
#include <iomanip>
#include <vector>
#include <cmath>
#include <cassert>
using namespace std;
using matrix = vector< vector<double> >;
const double PI = 4.0 * atan( 1.0 );
//======================================================================
// Rotation matrix for axis n, rotation angle in radians
// Sense of rotation is right-handed screw when looking in the direction of n
matrix rotationMatrix( vector<double> n, double angle )
{
double C = cos( angle ), S = sin( angle );
// Normalise n
double mag = sqrt( n[0] * n[0] + n[1] * n[1] + n[2] * n[2] );
n[0] /= mag; n[1] /= mag; n[2] /= mag;
// Create rotation matrix
matrix R( 3, vector<double>( 3 ) );
R[0][0] = C + n[0] * n[0] * ( 1.0 - C ) ;
R[0][1] = n[0] * n[1] * ( 1.0 - C ) - n[2] * S;
R[0][2] = n[0] * n[2] * ( 1.0 - C ) + n[1] * S;
R[1][1] = C + n[1] * n[1] * ( 1.0 - C ) ;
R[1][2] = n[1] * n[2] * ( 1.0 - C ) - n[0] * S;
R[1][0] = n[1] * n[0] * ( 1.0 - C ) + n[2] * S;
R[2][2] = C + n[2] * n[2] * ( 1.0 - C ) ;
R[2][0] = n[2] * n[0] * ( 1.0 - C ) - n[1] * S;
R[2][1] = n[2] * n[1] * ( 1.0 - C ) + n[0] * S;
return R;
}
//======================================================================
ostream & operator << ( ostream &out, const vector<double> &V )
{
out << fixed << setprecision( 4 );
for ( double e : V ) out << setw( 8 ) << e << ' ';
return out;
}
//======================================================================
vector<double> matmul( const matrix &M, vector<double> x )
{
int m = M.size(), n = M[0].size(); assert( n == x.size() );
vector<double> y( m, 0.0 );
for ( int i = 0; i < m; i++ )
{
for ( int j = 0; j < n; j++ ) y[i] += M[i][j] * x[j];
}
return y;
}
//======================================================================
int main()
{
vector<double> n = { 0.0, 0.0, 1.0 }; // rotation axis (here, z axis)
double degrees = 50; // rotation angle in degrees (anticlockwise seen from +z)
matrix R = rotationMatrix( n, degrees * PI / 180.0 );
// Set some vertices of a cuboid
vector< vector<double> > vertices = { { 0, 0, 0 }, { 1, 0, 0 }, { 1, 1, 0 }, { 0, 1, 0 },
{ 0, 0, 1 }, { 1, 0, 1 }, { 1, 1, 1 }, { 0, 1, 1 } };
cout << "Points before rotation:\n";
for ( auto pt : vertices ) cout << pt << '\n';
cout << "\nPoints after rotation:\n";
for ( auto pt : vertices ) cout << matmul( R, pt ) << '\n';
}
|