Coupure prévue mardi 3 Août au matin pour maintenance du serveur. Nous faisons au mieux pour que celle-ci soit la plus brève possible.

frame.hpp 7.35 KB
Newer Older
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
/*******************************************************************************
* CGoGN: Combinatorial and Geometric modeling with Generic N-dimensional Maps  *
* version 0.1                                                                  *
* Copyright (C) 2009-2011, IGG Team, LSIIT, University of Strasbourg           *
*                                                                              *
* This library is free software; you can redistribute it and/or modify it      *
* under the terms of the GNU Lesser General Public License as published by the *
* Free Software Foundation; either version 2.1 of the License, or (at your     *
* option) any later version.                                                   *
*                                                                              *
* This library 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 Lesser General Public License  *
* for more details.                                                            *
*                                                                              *
* You should have received a copy of the GNU Lesser General Public License     *
* along with this library; if not, write to the Free Software Foundation,      *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.           *
*                                                                              *
* Web site: http://cgogn.u-strasbg.fr/                                         *
* Contact information: cgogn@unistra.fr                                        *
*                                                                              *
*******************************************************************************/

namespace CGoGN {

namespace Geom {

template<typename PFP>
Frame<PFP>::Frame(const VEC3& X, const VEC3& Y, const VEC3& Z)
{
32
33
34
	const VEC3 refX(Xx,Xy,Xz) ;
	const VEC3 refY(Yx,Yy,Yz) ;
	const VEC3 refZ(Zx,Zy,Zz) ;
35

36
37
	if (!isDirectOrthoNormalFrame<PFP>(X,Y,Z))
		return ;
38

39
40
41
	REAL& alpha = m_EulerAngles[0] ;
	REAL& beta = m_EulerAngles[1] ;
	REAL& gamma = m_EulerAngles[2] ;
42

43
	VEC3 lineOfNodes = refZ ^ Z ;
44
45
	if (lineOfNodes.norm2() < 1e-5) // if Z ~= m_Z
	{
46
		lineOfNodes = refX ; // = reference T
47
48
49
50
51
52
53
54
		alpha = 0 ;
		gamma = 0 ;
	}
	else
	{
		lineOfNodes.normalize() ;

		// angle between reference T and line of nodes
55
		alpha = (refY*lineOfNodes > 0 ? 1 : -1) * std::acos(std::max(std::min(REAL(1.0), refX*lineOfNodes ),REAL(-1.0))) ;
56
		// angle between reference normal and normal
57
		gamma = std::acos(std::max(std::min(REAL(1.0), refZ*Z ),REAL(-1.0))) ; // gamma is always positive because the direction of vector lineOfNodes=(reference normal)^(normal) (around which a rotation of angle beta is done later on) changes depending on the side on which they lay w.r.t eachother.
58
59
	}
	// angle between line of nodes and T
60
61
	beta = (Y*lineOfNodes > 0 ? -1 : 1) * std::acos(std::max(std::min(REAL(1.0), X*lineOfNodes ),REAL(-1.0))) ;
}
62

63
64
65
66
template<typename PFP>
Frame<PFP>::Frame(const VEC3& EulerAngles)
{
	m_EulerAngles = EulerAngles ;
67
68
69
}

template<typename PFP>
70
void Frame<PFP>::getFrame(VEC3& X, VEC3& Y, VEC3& Z) const
71
{
72
73
	const VEC3 refX(Xx,Xy,Xz) ;
	const VEC3 refZ(Zx,Zy,Zz) ;
74

75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
	// get known data
	const REAL& alpha = m_EulerAngles[0] ;
	const REAL& beta = m_EulerAngles[1] ;
	const REAL& gamma = m_EulerAngles[2] ;

	const VEC3 lineOfNodes = rotate<REAL>(refZ,alpha,refX) ; // rotation around reference normal of vector T
	Z = rotate<REAL>(lineOfNodes,gamma,refZ) ; // rotation around line of nodes of vector N
	X = rotate<REAL>(Z,beta,lineOfNodes) ; // rotation around new normal of vector represented by line of nodes
	Y = Z ^ X ;
}

template<typename PFP>
bool Frame<PFP>::equals(const Geom::Frame<PFP>& lf, REAL epsilon) const
{
	return (m_EulerAngles - lf.m_EulerAngles).norm2() < epsilon ;
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
}

template<typename PFP>
bool Frame<PFP>::operator==(const Frame<PFP>& lf) const
{
	return this->equals(lf) ;
}

template<typename PFP>
bool Frame<PFP>::operator!=(const Frame<PFP>& lf) const
{
	return !(this->equals(lf)) ;
}

template<typename PFP>
105
bool isNormalizedFrame(const typename PFP::VEC3& X, const typename PFP::VEC3& Y, const typename PFP::VEC3& Z, typename PFP::REAL epsilon)
106
{
107
	return X.isNormalized(epsilon) && Y.isNormalized(epsilon) && Z.isNormalized(epsilon) ;
108
109
110
}

template<typename PFP>
111
bool isOrthogonalFrame(const typename PFP::VEC3& X, const typename PFP::VEC3& Y, const typename PFP::VEC3& Z, typename PFP::REAL epsilon)
112
{
113
	return X.isOrthogonal(Y,epsilon) && X.isOrthogonal(Z,epsilon) && Y.isOrthogonal(Z,epsilon) ;
114
115
116
}

template<typename PFP>
117
bool isDirectFrame(const typename PFP::VEC3& X, const typename PFP::VEC3& Y, const typename PFP::VEC3& Z, typename PFP::REAL epsilon)
118
{
119
120
121
122
123
	typename PFP::VEC3 new_Y = Z ^ X ;		// direct
	typename PFP::VEC3 diffs = new_Y - Y ;		// differences with existing B
	typename PFP::REAL diffNorm = diffs.norm2() ;	// Norm of this differences vector

	return (diffNorm < epsilon) ;		// Verify that this difference is very small
124
125
126
}

template<typename PFP>
127
bool isDirectOrthoNormalFrame(const typename PFP::VEC3& X, const typename PFP::VEC3& Y, const typename PFP::VEC3& Z, typename PFP::REAL epsilon)
128
{
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
	if (!isNormalizedFrame<PFP>(X,Y,Z,epsilon))
	{
		CGoGNerr << "The Frame you want to create and compress is not normalized" << CGoGNendl ;
		return false ;
	}

	if (!isOrthogonalFrame<PFP>(X,Y,Z,epsilon))
	{
		CGoGNerr << "The Frame you want to create and compress is not orthogonal" << CGoGNendl ;
		return false ;
	}

	if (!isDirectFrame<PFP>(X,Y,Z,epsilon))
	{
		CGoGNerr << "The Frame you want to create and compress is not direct" << CGoGNendl ;
		return false ;
	}

	return true ;
148
149
150
151
}


template<typename REAL>
152
Geom::Vector<3,REAL> cartToSpherical (const Geom::Vector<3,REAL>& cart)
153
154
155
{
	Geom::Vector<3,REAL> res ;

156
157
158
	const REAL& x = cart[0] ;
	const REAL& y = cart[1] ;
	const REAL& z = cart[2] ;
159
160
161
162
163

	REAL& rho = res[0] ;
	REAL& theta = res[1] ;
	REAL& phi = res[2] ;

164
	rho = cart.norm() ;
165
166
167
168
169
170
171
172
173
	theta = ((y < 0) ? -1 : 1) * std::acos(x / REAL(sqrt(x*x + y*y)) )  ;
	if (isnan(theta))
		theta = 0.0 ;
	phi = std::asin(z) ;

	return res ;
}

template<typename REAL>
174
Geom::Vector<3,REAL> sphericalToCart (const Geom::Vector<3,REAL>& sph)
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
{
	Geom::Vector<3,REAL> res ;

	const REAL& rho = sph[0] ;
	const REAL& theta = sph[1] ;
	const REAL& phi = sph[2] ;

	REAL& x = res[0] ;
	REAL& y = res[1] ;
	REAL& z = res[2] ;

	x = rho*cos(theta)*cos(phi) ;
	y = rho*sin(theta)*cos(phi) ;
	z = rho*sin(phi) ;

	assert(-1.000001 < x && x < 1.000001) ;
	assert(-1.000001 < y && y < 1.000001) ;
	assert(-1.000001 < z && z < 1.000001) ;

	return res ;
}

template <typename REAL>
Geom::Vector<3,REAL> rotate (Geom::Vector<3,REAL> axis, REAL angle, Geom::Vector<3,REAL> vector)
{
200
	// Algorithm extracted from : http://inside.mines.edu/~gmurray/ArbitraryAxisRotation/ section 5
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
	axis.normalize() ;

	const REAL& u = axis[0] ;
	const REAL& v = axis[1] ;
	const REAL& w = axis[2] ;

	const REAL& x = vector[0] ;
	const REAL& y = vector[1] ;
	const REAL& z = vector[2] ;

	Geom::Vector<3,REAL> res ;
	REAL& xp = res[0] ;
	REAL& yp = res[1] ;
	REAL& zp = res[2] ;

	const REAL tmp1 = u*x+v*y+w*z ;
	const REAL cos = std::cos(angle) ;
	const REAL sin = std::sin(angle) ;

	xp = u*tmp1*(1-cos) + x*cos+(v*z-w*y)*sin ;
	yp = v*tmp1*(1-cos) + y*cos-(u*z-w*x)*sin ;
	zp = w*tmp1*(1-cos) + z*cos+(u*y-v*x)*sin ;

	return res ;
}

} // Geom

} // CGoGN