b311480e |
1 | // Copyright (c) 1999-2012 OPEN CASCADE SAS |
2 | // |
3 | // The content of this file is subject to the Open CASCADE Technology Public |
4 | // License Version 6.5 (the "License"). You may not use the content of this file |
5 | // except in compliance with the License. Please obtain a copy of the License |
6 | // at http://www.opencascade.org and read it completely before using this file. |
7 | // |
8 | // The Initial Developer of the Original Code is Open CASCADE S.A.S., having its |
9 | // main offices at: 1, place des Freres Montgolfier, 78280 Guyancourt, France. |
10 | // |
11 | // The Original Code and all software distributed under the License is |
12 | // distributed on an "AS IS" basis, without warranty of any kind, and the |
13 | // Initial Developer hereby disclaims all such warranties, including without |
14 | // limitation, any warranties of merchantability, fitness for a particular |
15 | // purpose or non-infringement. Please see the License for the specific terms |
16 | // and conditions governing the rights and limitations under the License. |
17 | |
7fd59977 |
18 | //======================================================================= |
19 | //function : gp_Quaternion |
20 | //purpose : |
21 | //======================================================================= |
22 | |
23 | inline gp_Quaternion::gp_Quaternion() |
24 | : x(0.0), y(0.0), z(0.0), w(1.0) |
25 | { |
26 | } |
27 | |
28 | //======================================================================= |
29 | //function : gp_Quaternion |
30 | //purpose : |
31 | //======================================================================= |
32 | |
33 | inline gp_Quaternion::gp_Quaternion (const Standard_Real x, const Standard_Real y, |
34 | const Standard_Real z, const Standard_Real w) |
35 | : x(x), y(y), z(z), w(w) |
36 | { |
37 | } |
38 | |
39 | //======================================================================= |
40 | //function : gp_Quaternion |
41 | //purpose : |
42 | //======================================================================= |
43 | |
44 | inline gp_Quaternion::gp_Quaternion (const gp_Quaternion& theToCopy) |
45 | : x(theToCopy.x), y(theToCopy.y), z(theToCopy.z), w(theToCopy.w) |
46 | { |
47 | } |
48 | |
49 | //======================================================================= |
50 | //function : gp_Quaternion |
51 | //purpose : |
52 | //======================================================================= |
53 | |
54 | inline gp_Quaternion::gp_Quaternion (const gp_Vec& theVecFrom, const gp_Vec& theVecTo) |
55 | { |
56 | SetRotation (theVecFrom, theVecTo); |
57 | } |
58 | |
59 | //======================================================================= |
60 | //function : gp_Quaternion |
61 | //purpose : |
62 | //======================================================================= |
63 | |
64 | inline gp_Quaternion::gp_Quaternion (const gp_Vec& theVecFrom, const gp_Vec& theVecTo, const gp_Vec& theHelpCrossVec) |
65 | { |
66 | SetRotation (theVecFrom, theVecTo, theHelpCrossVec); |
67 | } |
68 | |
69 | //======================================================================= |
70 | //function : gp_Quaternion |
71 | //purpose : |
72 | //======================================================================= |
73 | |
74 | inline gp_Quaternion::gp_Quaternion (const gp_Vec& theAxis, const Standard_Real theAngle) |
75 | { |
76 | SetVectorAndAngle (theAxis, theAngle); |
77 | } |
78 | |
79 | //======================================================================= |
80 | //function : gp_Quaternion |
81 | //purpose : |
82 | //======================================================================= |
83 | |
84 | inline gp_Quaternion::gp_Quaternion (const gp_Mat& theMat) |
85 | { |
86 | SetMatrix (theMat); |
87 | } |
88 | |
89 | //======================================================================= |
90 | //function : Set |
91 | //purpose : |
92 | //======================================================================= |
93 | |
94 | inline void gp_Quaternion::Set (Standard_Real x, Standard_Real y, |
95 | Standard_Real z, Standard_Real w) |
96 | { |
97 | this->x = x; |
98 | this->y = y; |
99 | this->z = z; |
100 | this->w = w; |
101 | } |
102 | |
103 | //======================================================================= |
104 | //function : Set |
105 | //purpose : |
106 | //======================================================================= |
107 | |
108 | inline void gp_Quaternion::Set (const gp_Quaternion& theQuaternion) |
109 | { |
110 | x = theQuaternion.x; |
111 | y = theQuaternion.y; |
112 | z = theQuaternion.z; |
113 | w = theQuaternion.w; |
114 | } |
115 | |
116 | //======================================================================= |
117 | //function : X |
118 | //purpose : |
119 | //======================================================================= |
120 | |
121 | inline Standard_Real gp_Quaternion::X() const |
122 | { |
123 | return x; |
124 | } |
125 | |
126 | //======================================================================= |
127 | //function : Y |
128 | //purpose : |
129 | //======================================================================= |
130 | |
131 | inline Standard_Real gp_Quaternion::Y() const |
132 | { |
133 | return y; |
134 | } |
135 | |
136 | //======================================================================= |
137 | //function : Z |
138 | //purpose : |
139 | //======================================================================= |
140 | |
141 | inline Standard_Real gp_Quaternion::Z() const |
142 | { |
143 | return z; |
144 | } |
145 | |
146 | //======================================================================= |
147 | //function : W |
148 | //purpose : |
149 | //======================================================================= |
150 | |
151 | inline Standard_Real gp_Quaternion::W() const |
152 | { |
153 | return w; |
154 | } |
155 | |
156 | //======================================================================= |
157 | //function : SetIdent |
158 | //purpose : |
159 | //======================================================================= |
160 | |
161 | inline void gp_Quaternion::SetIdent() |
162 | { |
163 | x = y = z = 0.0; |
164 | w = 1.0; |
165 | } |
166 | |
167 | //======================================================================= |
168 | //function : Reverse |
169 | //purpose : |
170 | //======================================================================= |
171 | |
172 | inline void gp_Quaternion::Reverse() |
173 | { |
174 | x = -x; |
175 | y = -y; |
176 | z = -z; |
177 | } |
178 | |
179 | //======================================================================= |
180 | //function : Reversed |
181 | //purpose : |
182 | //======================================================================= |
183 | |
184 | inline gp_Quaternion gp_Quaternion::Reversed() const |
185 | { |
186 | return gp_Quaternion (-x, -y, -z, w); |
187 | } |
188 | |
189 | //======================================================================= |
190 | //function : Scale |
191 | //purpose : |
192 | //======================================================================= |
193 | |
194 | inline void gp_Quaternion::Scale (const Standard_Real theScale) |
195 | { |
196 | x *= theScale; |
197 | y *= theScale; |
198 | z *= theScale; |
199 | w *= theScale; |
200 | } |
201 | |
202 | //======================================================================= |
203 | //function : Scaled |
204 | //purpose : |
205 | //======================================================================= |
206 | |
207 | inline gp_Quaternion gp_Quaternion::Scaled (const Standard_Real theScale) const |
208 | { |
209 | return gp_Quaternion (x * theScale, y * theScale, z * theScale, w * theScale); |
210 | } |
211 | |
212 | //======================================================================= |
213 | //function : SquareNorm |
214 | //purpose : |
215 | //======================================================================= |
216 | |
217 | inline Standard_Real gp_Quaternion::SquareNorm() const |
218 | { |
219 | return x * x + y * y + z * z + w * w; |
220 | } |
221 | |
222 | //======================================================================= |
223 | //function : Norm |
224 | //purpose : |
225 | //======================================================================= |
226 | |
227 | inline Standard_Real gp_Quaternion::Norm() const |
228 | { |
229 | return Sqrt (SquareNorm()); |
230 | } |
231 | |
232 | //======================================================================= |
233 | //function : Invert |
234 | //purpose : |
235 | //======================================================================= |
236 | |
237 | inline void gp_Quaternion::Invert() |
238 | { |
239 | Standard_Real in = 1.0 / SquareNorm(); |
240 | Set (-x * in, -y * in, -z * in, w * in); |
241 | } |
242 | |
243 | //======================================================================= |
244 | //function : Inverted |
245 | //purpose : |
246 | //======================================================================= |
247 | |
248 | inline gp_Quaternion gp_Quaternion::Inverted() const |
249 | { |
250 | Standard_Real in = 1.0 / SquareNorm(); |
251 | return gp_Quaternion (-x * in, -y * in, -z * in, w * in); |
252 | } |
253 | |
254 | //======================================================================= |
255 | //function : Normalized |
256 | //purpose : |
257 | //======================================================================= |
258 | |
259 | inline gp_Quaternion gp_Quaternion::Normalized() const |
260 | { |
261 | gp_Quaternion aNormilizedQ (*this); |
262 | aNormilizedQ.Normalize(); |
263 | return aNormilizedQ; |
264 | } |
265 | |
266 | //======================================================================= |
267 | //function : Negated |
268 | //purpose : |
269 | //======================================================================= |
270 | |
271 | inline gp_Quaternion gp_Quaternion::Negated () const |
272 | { |
273 | return gp_Quaternion (-x, -y, -z, -w); |
274 | } |
275 | |
276 | //======================================================================= |
277 | //function : Added |
278 | //purpose : |
279 | //======================================================================= |
280 | |
281 | inline gp_Quaternion gp_Quaternion::Added (const gp_Quaternion& theQ) const |
282 | { |
283 | return gp_Quaternion (x + theQ.x, y + theQ.y, z + theQ.z, w + theQ.w); |
284 | } |
285 | |
286 | //======================================================================= |
287 | //function : Subtracted |
288 | //purpose : |
289 | //======================================================================= |
290 | |
291 | inline gp_Quaternion gp_Quaternion::Subtracted (const gp_Quaternion& theQ) const |
292 | { |
293 | return gp_Quaternion (x - theQ.x, y - theQ.y, z - theQ.z, w - theQ.w); |
294 | } |
295 | |
296 | //======================================================================= |
297 | //function : Multiplied |
298 | //purpose : |
299 | //======================================================================= |
300 | |
301 | inline gp_Quaternion gp_Quaternion::Multiplied (const gp_Quaternion& theQ) const |
302 | { |
303 | return gp_Quaternion (w * theQ.x + x * theQ.w + y * theQ.z - z * theQ.y, |
304 | w * theQ.y + y * theQ.w + z * theQ.x - x * theQ.z, |
305 | w * theQ.z + z * theQ.w + x * theQ.y - y * theQ.x, |
306 | w * theQ.w - x * theQ.x - y * theQ.y - z * theQ.z); |
307 | // 16 multiplications 12 addidtions 0 variables |
308 | } |
309 | |
310 | //======================================================================= |
311 | //function : Add |
312 | //purpose : |
313 | //======================================================================= |
314 | |
315 | inline void gp_Quaternion::Add (const gp_Quaternion& theQ) |
316 | { |
317 | x += theQ.x; |
318 | y += theQ.y; |
319 | z += theQ.z; |
320 | w += theQ.w; |
321 | } |
322 | |
323 | //======================================================================= |
324 | //function : Subtract |
325 | //purpose : |
326 | //======================================================================= |
327 | |
328 | inline void gp_Quaternion::Subtract (const gp_Quaternion& theQ) |
329 | { |
330 | x -= theQ.x; |
331 | y -= theQ.y; |
332 | z -= theQ.z; |
333 | w -= theQ.w; |
334 | } |
335 | |
336 | //======================================================================= |
337 | //function : Multiply |
338 | //purpose : |
339 | //======================================================================= |
340 | |
341 | inline void gp_Quaternion::Multiply (const gp_Quaternion& theQ) |
342 | { |
343 | (*this) = Multiplied (theQ); // have no optimization here |
344 | } |
345 | |
346 | //======================================================================= |
347 | //function : Dot |
348 | //purpose : |
349 | //======================================================================= |
350 | |
351 | inline Standard_Real gp_Quaternion::Dot (const gp_Quaternion& theQ) const |
352 | { |
353 | return x * theQ.x + y * theQ.y + z * theQ.z + w * theQ.w; |
354 | } |
355 | |