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
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
|
// This file is part of ff3d - http://www.freefem.org/ff3d
// Copyright (C) 2001, 2002, 2003 Stphane Del Pino
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2, or (at your option)
// any later version.
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software Foundation,
// Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
// $Id: Rotation.cpp,v 1.2 2003/05/04 18:09:01 delpinux Exp $
#include <Rotation.hpp>
#include <cmath>
#include <sstream>
//! Applies the Rotation to the vector \a v.
TinyVector<3>& Rotation::operator()(TinyVector<3>& v) const
{
v = __matrix*v;
return (v);
}
//! Applies the inverse Rotation to the vector \a v.
TinyVector<3>& Rotation::inverse(TinyVector<3>& v) const
{
// As 'matrix' is composed of rotations, and
// as rotations are othogonal matrices
// the inverse of 'matrix' is its transposed
TinyVector<3> Temporary = 0;
for (size_t i=0; i<3; i++)
for (size_t j=0; j<3; j++)
Temporary[i]+=__matrix(j,i)*v[j];
v = Temporary;
return v;
}
/*! Constructs the Rotation for a set of given angles given in \a v by
builting the invert of the Rotation matrix since it is more usefull than the
Rotation matrix.
\remark The angles contained in r are expressed using degrees.
*/
Rotation::Rotation(const TinyVector<3>& r)
{
__type = rotation;
__angles = r;
const real_t deg2rad=4*std::atan(1.)/180.;
const real_t cosx = std::cos(deg2rad*r[0]);
const real_t cosy = std::cos(deg2rad*r[1]);
const real_t cosz = std::cos(deg2rad*r[2]);
const real_t sinx = std::sin(deg2rad*r[0]);
const real_t siny = std::sin(deg2rad*r[1]);
const real_t sinz = std::sin(deg2rad*r[2]);
__matrix(0,0) = cosy*cosz;
__matrix(0,1) = -sinz*cosx + sinx*siny*cosz;
__matrix(0,2) = sinx*sinz + cosx*siny*cosz;
__matrix(1,0) = cosy*sinz;
__matrix(1,1) = cosx*cosz + sinx*siny*sinz;
__matrix(1,2) = -sinx*cosy + cosx*siny*sinz;
__matrix(2,0) = -siny;
__matrix(2,1) = sinx*cosy;
__matrix(2,2) = cosx*cosy;
}
/*! Default constructor: the Rotation is the identity.
\warning this might not be pertinent and may be removed.
*/
Rotation::Rotation()
{
__type = rotation;
__angles = 0;
__matrix(0,0) = 1;
__matrix(1,0) = 0;
__matrix(2,0) = 0;
__matrix(0,1) = 0;
__matrix(1,1) = 1;
__matrix(2,1) = 0;
__matrix(0,2) = 0;
__matrix(1,2) = 0;
__matrix(2,2) = 1;
}
//! Copy constructor.
Rotation::Rotation(const Rotation& r)
{
__type = rotation;
__angles = r.__angles;
for (size_t i=0; i<3; i++)
for (size_t j=0; j<3; j++)
__matrix(i,j) = r.__matrix(i,j);
}
//! prints rotation information to a string.
std::string Rotation::povWrite() const
{
std::stringstream povs;
povs << "rotation <"
<< __angles[0]
<< ", "
<< __angles[1]
<< ", "
<< __angles[2]
<< ">";
povs << std::ends;
return povs.str();
}
|
