b311480e |
1 | // Copyright (c) 1995-1999 Matra Datavision |
973c2be1 |
2 | // Copyright (c) 1999-2014 OPEN CASCADE SAS |
b311480e |
3 | // |
973c2be1 |
4 | // This file is part of Open CASCADE Technology software library. |
b311480e |
5 | // |
d5f74e42 |
6 | // This library is free software; you can redistribute it and/or modify it under |
7 | // the terms of the GNU Lesser General Public License version 2.1 as published |
973c2be1 |
8 | // by the Free Software Foundation, with special exception defined in the file |
9 | // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT |
10 | // distribution for complete text of the license and disclaimer of any warranty. |
b311480e |
11 | // |
973c2be1 |
12 | // Alternatively, this file may be used under the terms of Open CASCADE |
13 | // commercial license or contractual agreement. |
b311480e |
14 | |
7fd59977 |
15 | // JCV 30/08/90 Modif passage version C++ 2.0 sur Sun |
16 | // JCV 1/10/90 Changement de nom du package vgeom -> gp |
17 | // JCV 4/10/90 codage sur la forme de la transformation shape,Scaling,negative |
18 | // JCV 10/12/90 Modif introduction des classes Mat et XYZ dans gp |
19 | |
20 | #define No_Standard_OutOfRange |
21 | |
22 | #include <gp_Trsf.ixx> |
23 | #include <gp.hxx> |
24 | #include <Standard_ConstructionError.hxx> |
25 | |
bead40f2 |
26 | |
27 | //======================================================================= |
28 | //function : gp_Trsf |
29 | //purpose : Constructor from 2d |
30 | //======================================================================= |
31 | |
32 | gp_Trsf::gp_Trsf (const gp_Trsf2d& T) : |
33 | scale(T.ScaleFactor()), |
34 | shape(T.Form()), |
35 | loc(T.TranslationPart().X(),T.TranslationPart().Y(), 0.0) |
36 | { |
37 | const gp_Mat2d& M = T.HVectorialPart(); |
38 | matrix(1,1) = M(1,1); |
39 | matrix(1,2) = M(1,2); |
40 | matrix(2,1) = M(2,1); |
41 | matrix(2,2) = M(2,2); |
42 | matrix(3,3) = 1.; |
43 | if (shape == gp_Ax1Mirror) |
44 | { |
45 | scale = 1; |
46 | matrix.Multiply(-1); |
47 | } |
48 | } |
49 | |
7fd59977 |
50 | //======================================================================= |
51 | //function : SetMirror |
52 | //purpose : |
53 | //======================================================================= |
54 | |
55 | void gp_Trsf::SetMirror (const gp_Ax1& A1) |
56 | { |
57 | shape = gp_Ax1Mirror; |
58 | scale = 1; |
59 | loc = A1.Location().XYZ(); |
60 | matrix.SetDot(A1.Direction().XYZ()); |
61 | matrix.Multiply(-2); |
62 | matrix.SetDiagonal (matrix.Value (1,1) + 1, |
63 | matrix.Value (2,2) + 1, |
64 | matrix.Value (3,3) + 1); |
65 | |
66 | loc.Multiply (matrix); |
67 | loc.Add (A1.Location().XYZ()); |
68 | matrix.Multiply(-1); |
69 | } |
70 | |
71 | //======================================================================= |
72 | //function : SetMirror |
73 | //purpose : |
74 | //======================================================================= |
75 | |
76 | void gp_Trsf::SetMirror (const gp_Ax2& A2) |
77 | { |
78 | shape = gp_Ax2Mirror; |
79 | scale = -1; |
80 | loc = A2.Location().XYZ(); |
81 | matrix.SetDot(A2.Direction().XYZ()); |
82 | matrix.Multiply(2); |
83 | matrix.SetDiagonal (matrix.Value (1,1) - 1, |
84 | matrix.Value (2,2) - 1, |
85 | matrix.Value (3,3) - 1); |
86 | |
87 | loc.Multiply (matrix); |
88 | loc.Add (A2.Location().XYZ()); |
89 | } |
90 | |
91 | //======================================================================= |
92 | //function : SetRotation |
93 | //purpose : |
94 | //======================================================================= |
95 | |
96 | void gp_Trsf::SetRotation (const gp_Ax1& A1, |
97 | const Standard_Real Ang) |
98 | { |
99 | shape = gp_Rotation; |
100 | scale = 1.; |
101 | loc = A1.Location().XYZ(); |
102 | matrix.SetRotation (A1.Direction().XYZ(), Ang); |
103 | loc.Reverse (); |
104 | loc.Multiply (matrix); |
105 | loc.Add (A1.Location().XYZ()); |
106 | } |
107 | |
108 | //======================================================================= |
109 | //function : SetRotation |
110 | //purpose : |
111 | //======================================================================= |
112 | |
113 | void gp_Trsf::SetRotation (const gp_Quaternion& R) |
114 | { |
115 | shape = gp_Rotation; |
116 | scale = 1.; |
117 | loc.SetCoord (0., 0., 0.); |
118 | matrix = R.GetMatrix(); |
119 | } |
120 | |
121 | //======================================================================= |
122 | //function : SetScale |
123 | //purpose : |
124 | //======================================================================= |
125 | |
126 | void gp_Trsf::SetScale (const gp_Pnt& P, const Standard_Real S) |
127 | { |
128 | shape = gp_Scale; |
129 | scale = S; |
130 | loc = P.XYZ(); |
131 | Standard_Real As = scale; |
132 | if (As < 0) As = - As; |
133 | Standard_ConstructionError_Raise_if |
134 | (As <= gp::Resolution(),"gp_Trsf::SetScaleFactor"); |
135 | matrix.SetIdentity (); |
136 | loc.Multiply (1-S); |
137 | } |
138 | |
139 | //======================================================================= |
140 | //function : SetTransformation |
141 | //purpose : |
142 | //======================================================================= |
143 | |
144 | void gp_Trsf::SetTransformation (const gp_Ax3& FromA1, |
145 | const gp_Ax3& ToA2) |
146 | { |
147 | shape = gp_CompoundTrsf; |
148 | scale = 1.0; |
149 | // matrix from XOY ToA2 : |
150 | matrix.SetCol (1, ToA2.XDirection().XYZ()); |
151 | matrix.SetCol (2, ToA2.YDirection().XYZ()); |
152 | matrix.SetCol (3, ToA2.Direction().XYZ()); |
153 | loc = ToA2.Location().XYZ(); |
154 | matrix.Transpose(); |
155 | loc.Multiply (matrix); |
156 | loc.Reverse (); |
157 | |
158 | // matrix FromA1 to XOY : |
159 | const gp_XYZ& xDir = FromA1.XDirection().XYZ(); |
160 | const gp_XYZ& yDir = FromA1.YDirection().XYZ(); |
161 | const gp_XYZ& zDir = FromA1.Direction().XYZ(); |
162 | |
163 | gp_Mat MA1 (xDir, yDir, zDir); |
164 | gp_XYZ MA1loc = FromA1.Location().XYZ(); |
165 | |
166 | // matrix * MA1 => FromA1 ToA2 : |
167 | MA1loc.Multiply (matrix); |
168 | loc.Add (MA1loc); |
169 | matrix.Multiply (MA1); |
170 | } |
171 | |
172 | void gp_Trsf::SetTransformation (const gp_Ax3& A3) |
173 | { |
174 | shape = gp_CompoundTrsf; |
175 | scale = 1.0; |
176 | loc = A3.Location().XYZ(); |
177 | matrix.SetCols (A3.XDirection().XYZ(), |
178 | A3.YDirection().XYZ(), |
179 | A3. Direction().XYZ()); |
180 | matrix.Transpose(); |
181 | loc.Multiply (matrix); |
182 | loc.Reverse (); |
183 | } |
184 | |
185 | //======================================================================= |
186 | //function : SetTransformation |
187 | //purpose : |
188 | //======================================================================= |
189 | |
190 | void gp_Trsf::SetTransformation (const gp_Quaternion& R, const gp_Vec& T) |
191 | { |
192 | shape = gp_CompoundTrsf; |
193 | scale = 1.; |
194 | loc = T.XYZ(); |
195 | matrix = R.GetMatrix(); |
196 | } |
197 | |
198 | //======================================================================= |
199 | //function : SetDisplacement |
200 | //purpose : |
201 | //======================================================================= |
202 | |
203 | void gp_Trsf::SetDisplacement (const gp_Ax3& FromA1, |
204 | const gp_Ax3& ToA2) |
205 | { |
206 | shape = gp_CompoundTrsf; |
207 | scale = 1.0; |
208 | // matrix from ToA2 to XOY : |
209 | matrix.SetCol (1, ToA2.XDirection().XYZ()); |
210 | matrix.SetCol (2, ToA2.YDirection().XYZ()); |
211 | matrix.SetCol (3, ToA2.Direction().XYZ()); |
212 | loc = ToA2.Location().XYZ(); |
213 | // matrix XOY to FromA1 : |
214 | const gp_XYZ& xDir = FromA1.XDirection().XYZ(); |
215 | const gp_XYZ& yDir = FromA1.YDirection().XYZ(); |
216 | const gp_XYZ& zDir = FromA1.Direction().XYZ(); |
217 | gp_Mat MA1 (xDir, yDir, zDir); |
218 | MA1.Transpose(); |
219 | gp_XYZ MA1loc = FromA1.Location().XYZ(); |
220 | MA1loc.Multiply (MA1); |
221 | MA1loc.Reverse(); |
222 | // matrix * MA1 |
223 | MA1loc.Multiply (matrix); |
224 | loc.Add (MA1loc); |
225 | matrix.Multiply (MA1); |
226 | } |
227 | |
228 | //======================================================================= |
229 | //function : SetTranslationPart |
230 | //purpose : |
231 | //======================================================================= |
232 | |
233 | void gp_Trsf::SetTranslationPart (const gp_Vec& V) { |
234 | |
235 | loc = V.XYZ(); |
57922171 |
236 | const Standard_Boolean locnull = (loc.SquareModulus() < gp::Resolution()); |
7fd59977 |
237 | |
238 | switch (shape) { |
239 | |
240 | case gp_Identity : |
241 | if (!locnull) shape = gp_Translation; |
242 | break; |
243 | |
244 | case gp_Translation : |
245 | if (locnull) shape = gp_Identity; |
246 | break; |
247 | |
248 | case gp_Rotation : |
249 | case gp_PntMirror : |
250 | case gp_Ax1Mirror : |
251 | case gp_Ax2Mirror : |
252 | case gp_Scale : |
253 | case gp_CompoundTrsf : |
254 | case gp_Other : |
57922171 |
255 | if (!locnull) { |
256 | shape = gp_CompoundTrsf; |
257 | } |
7fd59977 |
258 | break; |
259 | } |
260 | } |
261 | |
262 | //======================================================================= |
263 | //function : SetScaleFactor |
264 | //purpose : |
265 | //======================================================================= |
266 | |
267 | void gp_Trsf::SetScaleFactor (const Standard_Real S) |
268 | { |
269 | Standard_Real As = S; |
270 | if (As < 0) As = - As; |
271 | Standard_ConstructionError_Raise_if |
272 | (As <= gp::Resolution(),"gp_Trsf::SetScaleFactor"); |
273 | scale = S; |
274 | As = scale - 1.; |
275 | if (As < 0) As = - As; |
276 | Standard_Boolean unit = As <= gp::Resolution(); |
277 | As = scale + 1.; |
278 | if (As < 0) As = - As; |
279 | Standard_Boolean munit = As <= gp::Resolution(); |
280 | |
281 | switch (shape) { |
282 | case gp_Identity : |
283 | case gp_Translation : |
284 | if (!unit) shape = gp_Scale; |
285 | if (munit) shape = gp_PntMirror; |
286 | break; |
287 | case gp_Rotation : |
288 | if (!unit) shape = gp_CompoundTrsf; |
289 | break; |
290 | case gp_PntMirror : |
291 | case gp_Ax1Mirror : |
292 | case gp_Ax2Mirror : |
293 | if (!munit) shape = gp_Scale; |
294 | if (unit) shape = gp_Identity; |
295 | break; |
296 | case gp_Scale : |
297 | if (unit) shape = gp_Identity; |
298 | if (munit) shape = gp_PntMirror; |
299 | break; |
300 | case gp_CompoundTrsf : |
301 | break; |
302 | case gp_Other : |
303 | break; |
304 | } |
305 | } |
306 | |
307 | //======================================================================= |
308 | //function : SetValues |
309 | //purpose : |
310 | // 06-01-1998 modified by PMN : On utilise TolDist pour evaluer si les coeffs |
311 | // sont nuls : c'est toujours mieux que gp::Resolution ! |
312 | //======================================================================= |
313 | |
314 | void gp_Trsf::SetValues(const Standard_Real a11, |
315 | const Standard_Real a12, |
316 | const Standard_Real a13, |
317 | const Standard_Real a14, |
318 | const Standard_Real a21, |
319 | const Standard_Real a22, |
320 | const Standard_Real a23, |
321 | const Standard_Real a24, |
322 | const Standard_Real a31, |
323 | const Standard_Real a32, |
324 | const Standard_Real a33, |
325 | const Standard_Real a34, |
326 | // const Standard_Real Tolang, |
327 | const Standard_Real , |
328 | const Standard_Real |
329 | #ifndef No_Exception |
330 | TolDist |
331 | #endif |
332 | ) |
333 | { |
334 | gp_XYZ col1(a11,a21,a31); |
335 | gp_XYZ col2(a12,a22,a32); |
336 | gp_XYZ col3(a13,a23,a33); |
337 | gp_XYZ col4(a14,a24,a34); |
338 | // compute the determinant |
339 | gp_Mat M(col1,col2,col3); |
340 | Standard_Real s = M.Determinant(); |
341 | Standard_Real As = s; |
342 | if (As < 0) As = - As; |
343 | Standard_ConstructionError_Raise_if |
344 | (As < gp::Resolution(),"gp_Trsf::SeValues, null determinant"); |
345 | if (s > 0) |
346 | s = Pow(s,1./3.); |
347 | else |
348 | s = -Pow(-s,1./3.); |
349 | M.Divide(s); |
350 | |
351 | // check if the matrix is a rotation matrix |
352 | // the transposition should be the invert. |
353 | gp_Mat TM(M); |
354 | TM.Transpose(); |
355 | TM.Multiply(M); |
356 | // |
357 | // don t trust the initial values ! |
358 | // |
359 | gp_Mat anIdentity ; |
360 | anIdentity.SetIdentity() ; |
361 | TM.Subtract(anIdentity); |
362 | As = TM.Value(1,1); |
363 | if (As < 0) As = - As; |
364 | Standard_ConstructionError_Raise_if |
365 | (As > TolDist,"gp_Trsf::SeValues, non uniform"); |
366 | As = TM.Value(1,2); |
367 | if (As < 0) As = - As; |
368 | Standard_ConstructionError_Raise_if |
369 | (As > TolDist,"gp_Trsf::SeValues, non uniform"); |
370 | As = TM.Value(1,3); |
371 | if (As < 0) As = - As; |
372 | Standard_ConstructionError_Raise_if |
373 | (As > TolDist,"gp_Trsf::SeValues, non uniform"); |
374 | As = TM.Value(2,1); |
375 | if (As < 0) As = - As; |
376 | Standard_ConstructionError_Raise_if |
377 | (As > TolDist,"gp_Trsf::SeValues, non uniform"); |
378 | As = TM.Value(2,2); |
379 | if (As < 0) As = - As; |
380 | Standard_ConstructionError_Raise_if |
381 | (As > TolDist,"gp_Trsf::SeValues, non uniform"); |
382 | As = TM.Value(2,3); |
383 | if (As < 0) As = - As; |
384 | Standard_ConstructionError_Raise_if |
385 | (As > TolDist,"gp_Trsf::SeValues, non uniform"); |
386 | As = TM.Value(3,1); |
387 | if (As < 0) As = - As; |
388 | Standard_ConstructionError_Raise_if |
389 | (As > TolDist,"gp_Trsf::SeValues, non uniform"); |
390 | As = TM.Value(3,2); |
391 | if (As < 0) As = - As; |
392 | Standard_ConstructionError_Raise_if |
393 | (As > TolDist,"gp_Trsf::SeValues, non uniform"); |
394 | As = TM.Value(3,3); |
395 | if (As < 0) As = - As; |
396 | Standard_ConstructionError_Raise_if |
397 | (As > TolDist,"gp_Trsf::SeValues, non uniform"); |
398 | scale = s; |
399 | shape = gp_CompoundTrsf; |
400 | matrix = M; |
401 | loc = col4; |
402 | } |
403 | |
404 | //======================================================================= |
405 | //function : GetRotation |
406 | //purpose : |
407 | //======================================================================= |
408 | |
409 | gp_Quaternion gp_Trsf::GetRotation () const |
410 | { |
411 | return gp_Quaternion (matrix); |
412 | } |
413 | |
414 | //======================================================================= |
415 | //function : VectorialPart |
416 | //purpose : |
417 | //======================================================================= |
418 | |
419 | gp_Mat gp_Trsf::VectorialPart () const |
420 | { |
421 | if (scale == 1.0) return matrix; |
422 | gp_Mat M = matrix; |
423 | if (shape == gp_Scale || shape == gp_PntMirror) |
424 | M.SetDiagonal(scale*M.Value(1,1), |
425 | scale*M.Value(2,2), |
426 | scale*M.Value(3,3)); |
427 | else |
428 | M.Multiply (scale); |
429 | return M; |
430 | } |
431 | |
432 | //======================================================================= |
433 | //function : Invert |
434 | //purpose : |
435 | //======================================================================= |
436 | |
437 | void gp_Trsf::Invert() |
438 | { |
439 | // -1 |
440 | // X' = scale * R * X + T => X = (R / scale) * ( X' - T) |
441 | // |
442 | // Pour les gp_Trsf puisque le scale est extrait de la gp_Matrice R |
443 | // on a toujours determinant (R) = 1 et R-1 = R transposee. |
444 | if (shape == gp_Identity) { } |
445 | else if (shape == gp_Translation || shape == gp_PntMirror) loc.Reverse(); |
446 | else if (shape == gp_Scale) { |
447 | Standard_Real As = scale; |
448 | if (As < 0) As = - As; |
449 | Standard_ConstructionError_Raise_if |
450 | (As <= gp::Resolution(),""); |
451 | scale = 1.0 / scale; |
452 | loc.Multiply (-scale); |
453 | } |
454 | else { |
455 | Standard_Real As = scale; |
456 | if (As < 0) As = - As; |
457 | Standard_ConstructionError_Raise_if |
458 | (As <= gp::Resolution(),""); |
459 | scale = 1.0 / scale; |
460 | matrix.Transpose (); |
461 | loc.Multiply (matrix); |
462 | loc.Multiply (-scale); |
463 | } |
464 | } |
465 | |
466 | //======================================================================= |
467 | //function : Multiply |
468 | //purpose : |
469 | //======================================================================= |
470 | |
471 | void gp_Trsf::Multiply(const gp_Trsf& T) |
472 | { |
473 | if (T.shape == gp_Identity) { } |
474 | else if (shape == gp_Identity) { |
475 | shape = T.shape; |
476 | scale = T.scale; |
477 | loc = T.loc; |
478 | matrix = T.matrix; |
479 | } |
480 | else if (shape == gp_Rotation && T.shape == gp_Rotation) { |
f6f03db9 |
481 | if (T.loc.X() != 0.0 || T.loc.Y() != 0.0 || T.loc.Z() != 0.0) { |
7fd59977 |
482 | loc.Add (T.loc.Multiplied (matrix)); |
483 | } |
484 | matrix.Multiply(T.matrix); |
485 | } |
486 | else if (shape == gp_Translation && T.shape == gp_Translation) { |
487 | loc.Add (T.loc); |
488 | } |
489 | else if (shape == gp_Scale && T.shape == gp_Scale) { |
490 | loc.Add (T.loc.Multiplied(scale)); |
491 | scale = scale * T.scale; |
492 | } |
493 | else if (shape == gp_PntMirror && T.shape == gp_PntMirror) { |
494 | scale = 1.0; |
495 | shape = gp_Translation; |
496 | loc.Add (T.loc.Reversed()); |
497 | } |
498 | else if (shape == gp_Ax1Mirror && T.shape == gp_Ax1Mirror) { |
499 | shape = gp_Rotation; |
500 | loc.Add (T.loc.Multiplied (matrix)); |
501 | matrix.Multiply(T.matrix); |
502 | } |
503 | else if ((shape == gp_CompoundTrsf || shape == gp_Rotation || |
504 | shape == gp_Ax1Mirror || shape == gp_Ax2Mirror) |
505 | && T.shape == gp_Translation) { |
506 | gp_XYZ Tloc(T.loc); |
507 | Tloc.Multiply(matrix); |
508 | if (scale != 1.0) { Tloc.Multiply(scale); } |
509 | loc.Add (Tloc); |
510 | } |
511 | else if ((shape == gp_Scale || shape == gp_PntMirror) |
512 | && T.shape == gp_Translation) { |
513 | gp_XYZ Tloc(T.loc); |
514 | Tloc.Multiply (scale); |
515 | loc.Add (Tloc); |
516 | } |
517 | else if (shape == gp_Translation && |
518 | (T.shape == gp_CompoundTrsf || T.shape == gp_Rotation || |
519 | T.shape == gp_Ax1Mirror || T.shape == gp_Ax2Mirror)) { |
520 | shape = gp_CompoundTrsf; |
521 | scale = T.scale; |
522 | loc.Add (T.loc); |
523 | matrix = T.matrix; |
524 | } |
525 | else if (shape == gp_Translation && |
526 | (T.shape == gp_Scale || T.shape == gp_PntMirror)) { |
527 | shape = T.shape; |
528 | loc.Add (T.loc); |
529 | scale = T.scale; |
530 | } |
531 | else if ((shape == gp_PntMirror || shape == gp_Scale) && |
532 | (T.shape == gp_PntMirror || T.shape == gp_Scale)) { |
533 | shape = gp_CompoundTrsf; |
534 | gp_XYZ Tloc(T.loc); |
535 | Tloc.Multiply (scale); |
536 | loc.Add (Tloc); |
537 | scale = scale * T.scale; |
538 | } |
539 | else if ((shape == gp_CompoundTrsf || shape == gp_Rotation || |
540 | shape == gp_Ax1Mirror || shape == gp_Ax2Mirror) |
541 | && (T.shape == gp_Scale || T.shape == gp_PntMirror)) { |
542 | shape = gp_CompoundTrsf; |
543 | gp_XYZ Tloc(T.loc); |
544 | if (scale == 1.0) { |
545 | scale = T.scale; |
546 | Tloc.Multiply(matrix); |
547 | } |
548 | else { |
549 | Tloc.Multiply (matrix); |
550 | Tloc.Multiply (scale); |
551 | scale = scale * T.scale; |
552 | } |
553 | loc.Add (Tloc); |
554 | } |
555 | else if ((T.shape == gp_CompoundTrsf || T.shape == gp_Rotation || |
556 | T.shape == gp_Ax1Mirror || T.shape == gp_Ax2Mirror) |
557 | && (shape == gp_Scale || shape == gp_PntMirror)) { |
558 | shape = gp_CompoundTrsf; |
559 | gp_XYZ Tloc(T.loc); |
560 | Tloc.Multiply(scale); |
561 | loc.Add (Tloc); |
562 | scale = scale * T.scale; |
563 | matrix = T.matrix; |
564 | } |
565 | else { |
566 | shape = gp_CompoundTrsf; |
567 | gp_XYZ Tloc(T.loc); |
568 | Tloc.Multiply (matrix); |
569 | if (scale != 1.0) { |
570 | Tloc.Multiply (scale); |
571 | scale = scale * T.scale; |
572 | } |
573 | else { scale = T.scale; } |
574 | loc.Add (Tloc); |
575 | matrix.Multiply(T.matrix); |
576 | } |
577 | } |
578 | |
579 | //======================================================================= |
580 | //function : Power |
581 | //purpose : |
582 | //======================================================================= |
583 | |
584 | void gp_Trsf::Power (const Standard_Integer N) |
585 | { |
586 | if (shape == gp_Identity) { } |
587 | else { |
588 | if (N == 0) { |
589 | scale = 1.0; |
590 | shape = gp_Identity; |
591 | matrix.SetIdentity(); |
592 | loc = gp_XYZ (0.0, 0.0, 0.0); |
593 | } |
594 | else if (N == 1) { } |
595 | else if (N == -1) { Invert(); } |
596 | else { |
597 | if (N < 0) { Invert(); } |
598 | if (shape == gp_Translation) { |
599 | Standard_Integer Npower = N; |
600 | if (Npower < 0) Npower = - Npower; |
601 | Npower--; |
602 | gp_XYZ Temploc = loc; |
302f96fb |
603 | for(;;) { |
7fd59977 |
604 | if (IsOdd(Npower)) loc.Add (Temploc); |
605 | if (Npower == 1) break; |
606 | Temploc.Add (Temploc); |
607 | Npower = Npower/2; |
608 | } |
609 | } |
610 | else if (shape == gp_Scale) { |
611 | Standard_Integer Npower = N; |
612 | if (Npower < 0) Npower = - Npower; |
613 | Npower--; |
614 | gp_XYZ Temploc = loc; |
615 | Standard_Real Tempscale = scale; |
302f96fb |
616 | for(;;) { |
7fd59977 |
617 | if (IsOdd(Npower)) { |
618 | loc.Add (Temploc.Multiplied(scale)); |
619 | scale = scale * Tempscale; |
620 | } |
621 | if (Npower == 1) break; |
622 | Temploc.Add (Temploc.Multiplied(Tempscale)); |
623 | Tempscale = Tempscale * Tempscale; |
624 | Npower = Npower/2; |
625 | } |
626 | } |
627 | else if (shape == gp_Rotation) { |
628 | Standard_Integer Npower = N; |
629 | if (Npower < 0) Npower = - Npower; |
630 | Npower--; |
631 | gp_Mat Tempmatrix (matrix); |
632 | if (loc.X() == 0.0 && loc.Y() == 0.0 && loc.Z() == 0.0) { |
302f96fb |
633 | for(;;) { |
7fd59977 |
634 | if (IsOdd(Npower)) matrix.Multiply (Tempmatrix); |
635 | if (Npower == 1) break; |
636 | Tempmatrix.Multiply (Tempmatrix); |
637 | Npower = Npower/2; |
638 | } |
639 | } |
640 | else { |
641 | gp_XYZ Temploc = loc; |
302f96fb |
642 | for(;;) { |
7fd59977 |
643 | if (IsOdd(Npower)) { |
644 | loc.Add (Temploc.Multiplied (matrix)); |
645 | matrix.Multiply (Tempmatrix); |
646 | } |
647 | if (Npower == 1) break; |
648 | Temploc.Add (Temploc.Multiplied (Tempmatrix)); |
649 | Tempmatrix.Multiply (Tempmatrix); |
650 | Npower = Npower/2; |
651 | } |
652 | } |
653 | } |
654 | else if (shape == gp_PntMirror || shape == gp_Ax1Mirror || |
655 | shape == gp_Ax2Mirror) { |
656 | if (IsEven (N)) { |
657 | shape = gp_Identity; |
658 | scale = 1.0; |
659 | matrix.SetIdentity (); |
660 | loc.SetX(0); |
661 | loc.SetY(0); |
662 | loc.SetZ(0); |
663 | } |
664 | } |
665 | else { |
666 | shape = gp_CompoundTrsf; |
667 | Standard_Integer Npower = N; |
668 | if (Npower < 0) Npower = - Npower; |
669 | Npower--; |
670 | gp_XYZ Temploc = loc; |
671 | Standard_Real Tempscale = scale; |
672 | gp_Mat Tempmatrix (matrix); |
302f96fb |
673 | for(;;) { |
7fd59977 |
674 | if (IsOdd(Npower)) { |
675 | loc.Add ((Temploc.Multiplied (matrix)).Multiplied (scale)); |
676 | scale = scale * Tempscale; |
677 | matrix.Multiply (Tempmatrix); |
678 | } |
679 | if (Npower == 1) break; |
680 | Tempscale = Tempscale * Tempscale; |
681 | Temploc.Add ( (Temploc.Multiplied (Tempmatrix)).Multiplied |
682 | (Tempscale) |
683 | ); |
684 | Tempmatrix.Multiply (Tempmatrix); |
685 | Npower = Npower/2; |
686 | } |
687 | } |
688 | } |
689 | } |
690 | } |
691 | |
692 | //======================================================================= |
693 | //function : PreMultiply |
694 | //purpose : |
695 | //======================================================================= |
696 | |
697 | void gp_Trsf::PreMultiply (const gp_Trsf& T) |
698 | { |
699 | if (T.shape == gp_Identity) { } |
700 | else if (shape == gp_Identity) { |
701 | shape = T.shape; |
702 | scale = T.scale; |
703 | loc = T.loc; |
704 | matrix = T.matrix; |
705 | } |
706 | else if (shape == gp_Rotation && T.shape == gp_Rotation) { |
707 | loc.Multiply (T.matrix); |
708 | loc.Add (T.loc); |
709 | matrix.PreMultiply(T.matrix); |
710 | } |
711 | else if (shape == gp_Translation && T.shape == gp_Translation) { |
712 | loc.Add (T.loc); |
713 | } |
714 | else if (shape == gp_Scale && T.shape == gp_Scale) { |
715 | loc.Multiply (T.scale); |
716 | loc.Add (T.loc); |
717 | scale = scale * T.scale; |
718 | } |
719 | else if (shape == gp_PntMirror && T.shape == gp_PntMirror) { |
720 | scale = 1.0; |
721 | shape = gp_Translation; |
722 | loc.Reverse(); |
723 | loc.Add (T.loc); |
724 | } |
725 | else if (shape == gp_Ax1Mirror && T.shape == gp_Ax1Mirror) { |
726 | shape = gp_Rotation; |
727 | loc.Multiply (T.matrix); |
728 | loc.Add (T.loc); |
729 | matrix.PreMultiply(T.matrix); |
730 | } |
731 | else if ((shape == gp_CompoundTrsf || shape == gp_Rotation || |
732 | shape == gp_Ax1Mirror || shape == gp_Ax2Mirror) |
733 | && T.shape == gp_Translation) { |
734 | loc.Add (T.loc); |
735 | } |
736 | else if ((shape == gp_Scale || shape == gp_PntMirror) |
737 | && T.shape == gp_Translation) { |
738 | loc.Add (T.loc); |
739 | } |
740 | else if (shape == gp_Translation && |
741 | (T.shape == gp_CompoundTrsf || T.shape == gp_Rotation |
742 | || T.shape == gp_Ax1Mirror || T.shape == gp_Ax2Mirror)) { |
743 | shape = gp_CompoundTrsf; |
744 | matrix = T.matrix; |
745 | if (T.scale == 1.0) loc.Multiply (T.matrix); |
746 | else { |
747 | scale = T.scale; |
748 | loc.Multiply (matrix); |
749 | loc.Multiply (scale); |
750 | } |
751 | loc.Add (T.loc); |
752 | } |
753 | else if ((T.shape == gp_Scale || T.shape == gp_PntMirror) |
754 | && shape == gp_Translation) { |
755 | loc.Multiply (T.scale); |
756 | loc.Add (T.loc); |
757 | scale = T.scale; |
758 | shape = T.shape; |
759 | } |
760 | else if ((shape == gp_PntMirror || shape == gp_Scale) && |
761 | (T.shape == gp_PntMirror || T.shape == gp_Scale)) { |
762 | shape = gp_CompoundTrsf; |
763 | loc.Multiply (T.scale); |
764 | loc.Add (T.loc); |
765 | scale = scale * T.scale; |
766 | } |
767 | else if ((shape == gp_CompoundTrsf || shape == gp_Rotation || |
768 | shape == gp_Ax1Mirror || shape == gp_Ax2Mirror) |
769 | && (T.shape == gp_Scale || T.shape == gp_PntMirror)) { |
770 | shape = gp_CompoundTrsf; |
771 | loc.Multiply (T.scale); |
772 | loc.Add (T.loc); |
773 | scale = scale * T.scale; |
774 | } |
775 | else if ((T.shape == gp_CompoundTrsf || T.shape == gp_Rotation || |
776 | T.shape == gp_Ax1Mirror || T.shape == gp_Ax2Mirror) |
777 | && (shape == gp_Scale || shape == gp_PntMirror)) { |
778 | shape = gp_CompoundTrsf; |
779 | matrix = T.matrix; |
780 | if (T.scale == 1.0) loc.Multiply (T.matrix); |
781 | else { |
782 | loc.Multiply (matrix); |
783 | loc.Multiply (T.scale); |
784 | scale = T.scale * scale; |
785 | } |
786 | loc.Add (T.loc); |
787 | } |
788 | else { |
789 | shape = gp_CompoundTrsf; |
790 | loc.Multiply (T.matrix); |
791 | if (T.scale != 1.0) { |
792 | loc.Multiply (T.scale); scale = scale * T.scale; |
793 | } |
794 | loc.Add (T.loc); |
795 | matrix.PreMultiply(T.matrix); |
796 | } |
797 | } |
798 | |
799 | //======================================================================= |
800 | //function : GetRotation |
801 | //purpose : algorithm from A.Korn, M.Korn, "Mathematical Handbook for |
802 | // scientists and Engineers" McGraw-Hill, 1961, ch.14.10-2. |
803 | //======================================================================= |
804 | |
805 | Standard_Boolean gp_Trsf::GetRotation (gp_XYZ& theAxis, |
806 | Standard_Real& theAngle) const |
807 | { |
808 | gp_Quaternion Q = GetRotation(); |
809 | gp_Vec aVec; |
810 | Q.GetVectorAndAngle (aVec, theAngle); |
811 | theAxis = aVec.XYZ(); |
812 | return Standard_True; |
813 | } |