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 | |
d5477f8c |
22 | #include <gp_Trsf.hxx> |
42cf5bc1 |
23 | |
7fd59977 |
24 | #include <gp.hxx> |
42cf5bc1 |
25 | #include <gp_Ax3.hxx> |
26 | #include <gp_GTrsf.hxx> |
27 | #include <gp_Mat.hxx> |
28 | #include <gp_Pnt.hxx> |
29 | #include <gp_Quaternion.hxx> |
42cf5bc1 |
30 | #include <gp_Trsf2d.hxx> |
31 | #include <gp_Vec.hxx> |
32 | #include <gp_XYZ.hxx> |
7fd59977 |
33 | #include <Standard_ConstructionError.hxx> |
42cf5bc1 |
34 | #include <Standard_OutOfRange.hxx> |
0904aa63 |
35 | #include <Standard_Dump.hxx> |
bead40f2 |
36 | |
37 | //======================================================================= |
38 | //function : gp_Trsf |
39 | //purpose : Constructor from 2d |
40 | //======================================================================= |
bead40f2 |
41 | gp_Trsf::gp_Trsf (const gp_Trsf2d& T) : |
42 | scale(T.ScaleFactor()), |
43 | shape(T.Form()), |
44 | loc(T.TranslationPart().X(),T.TranslationPart().Y(), 0.0) |
45 | { |
46 | const gp_Mat2d& M = T.HVectorialPart(); |
47 | matrix(1,1) = M(1,1); |
48 | matrix(1,2) = M(1,2); |
49 | matrix(2,1) = M(2,1); |
50 | matrix(2,2) = M(2,2); |
51 | matrix(3,3) = 1.; |
52 | if (shape == gp_Ax1Mirror) |
53 | { |
54 | scale = 1; |
55 | matrix.Multiply(-1); |
56 | } |
57 | } |
58 | |
7fd59977 |
59 | //======================================================================= |
60 | //function : SetMirror |
61 | //purpose : |
62 | //======================================================================= |
63 | |
64 | void gp_Trsf::SetMirror (const gp_Ax1& A1) |
65 | { |
66 | shape = gp_Ax1Mirror; |
67 | scale = 1; |
68 | loc = A1.Location().XYZ(); |
69 | matrix.SetDot(A1.Direction().XYZ()); |
70 | matrix.Multiply(-2); |
71 | matrix.SetDiagonal (matrix.Value (1,1) + 1, |
72 | matrix.Value (2,2) + 1, |
73 | matrix.Value (3,3) + 1); |
74 | |
75 | loc.Multiply (matrix); |
76 | loc.Add (A1.Location().XYZ()); |
77 | matrix.Multiply(-1); |
78 | } |
79 | |
80 | //======================================================================= |
81 | //function : SetMirror |
82 | //purpose : |
83 | //======================================================================= |
84 | |
85 | void gp_Trsf::SetMirror (const gp_Ax2& A2) |
86 | { |
87 | shape = gp_Ax2Mirror; |
88 | scale = -1; |
89 | loc = A2.Location().XYZ(); |
90 | matrix.SetDot(A2.Direction().XYZ()); |
91 | matrix.Multiply(2); |
92 | matrix.SetDiagonal (matrix.Value (1,1) - 1, |
93 | matrix.Value (2,2) - 1, |
94 | matrix.Value (3,3) - 1); |
95 | |
96 | loc.Multiply (matrix); |
97 | loc.Add (A2.Location().XYZ()); |
98 | } |
99 | |
100 | //======================================================================= |
101 | //function : SetRotation |
102 | //purpose : |
103 | //======================================================================= |
104 | |
105 | void gp_Trsf::SetRotation (const gp_Ax1& A1, |
106 | const Standard_Real Ang) |
107 | { |
108 | shape = gp_Rotation; |
109 | scale = 1.; |
110 | loc = A1.Location().XYZ(); |
111 | matrix.SetRotation (A1.Direction().XYZ(), Ang); |
112 | loc.Reverse (); |
113 | loc.Multiply (matrix); |
114 | loc.Add (A1.Location().XYZ()); |
115 | } |
116 | |
117 | //======================================================================= |
118 | //function : SetRotation |
119 | //purpose : |
120 | //======================================================================= |
121 | |
122 | void gp_Trsf::SetRotation (const gp_Quaternion& R) |
123 | { |
124 | shape = gp_Rotation; |
125 | scale = 1.; |
126 | loc.SetCoord (0., 0., 0.); |
127 | matrix = R.GetMatrix(); |
128 | } |
129 | |
dbc8becf |
130 | //======================================================================= |
131 | //function : SetRotationPart |
132 | //purpose : |
133 | //======================================================================= |
134 | void gp_Trsf::SetRotationPart (const gp_Quaternion& theR) |
135 | { |
136 | const bool hasRotation = !theR.IsEqual (gp_Quaternion()); |
137 | if (hasRotation) |
138 | { |
139 | matrix = theR.GetMatrix(); |
140 | } |
141 | else |
142 | { |
143 | matrix.SetIdentity(); |
144 | } |
145 | |
146 | switch (shape) |
147 | { |
148 | case gp_Identity: |
149 | { |
150 | if (hasRotation) |
151 | { |
152 | shape = gp_Rotation; |
153 | } |
154 | break; |
155 | } |
156 | case gp_Rotation: |
157 | { |
158 | if (!hasRotation) |
159 | { |
160 | shape = gp_Identity; |
161 | } |
162 | break; |
163 | } |
164 | case gp_Translation: |
165 | case gp_PntMirror: |
166 | case gp_Ax1Mirror: |
167 | case gp_Ax2Mirror: |
168 | case gp_Scale: |
169 | case gp_CompoundTrsf: |
170 | case gp_Other: |
171 | { |
172 | if (hasRotation) |
173 | { |
174 | shape = gp_CompoundTrsf; |
175 | } |
176 | break; |
177 | } |
178 | } |
179 | } |
180 | |
7fd59977 |
181 | //======================================================================= |
182 | //function : SetScale |
183 | //purpose : |
184 | //======================================================================= |
185 | |
186 | void gp_Trsf::SetScale (const gp_Pnt& P, const Standard_Real S) |
187 | { |
188 | shape = gp_Scale; |
189 | scale = S; |
190 | loc = P.XYZ(); |
191 | Standard_Real As = scale; |
192 | if (As < 0) As = - As; |
193 | Standard_ConstructionError_Raise_if |
194 | (As <= gp::Resolution(),"gp_Trsf::SetScaleFactor"); |
195 | matrix.SetIdentity (); |
196 | loc.Multiply (1-S); |
197 | } |
198 | |
199 | //======================================================================= |
200 | //function : SetTransformation |
201 | //purpose : |
202 | //======================================================================= |
203 | |
204 | void gp_Trsf::SetTransformation (const gp_Ax3& FromA1, |
205 | const gp_Ax3& ToA2) |
206 | { |
207 | shape = gp_CompoundTrsf; |
208 | scale = 1.0; |
209 | // matrix from XOY ToA2 : |
c16c60a4 |
210 | matrix.SetRows (ToA2.XDirection().XYZ(), |
211 | ToA2.YDirection().XYZ(), |
212 | ToA2. Direction().XYZ()); |
7fd59977 |
213 | loc = ToA2.Location().XYZ(); |
7fd59977 |
214 | loc.Multiply (matrix); |
215 | loc.Reverse (); |
216 | |
217 | // matrix FromA1 to XOY : |
218 | const gp_XYZ& xDir = FromA1.XDirection().XYZ(); |
219 | const gp_XYZ& yDir = FromA1.YDirection().XYZ(); |
220 | const gp_XYZ& zDir = FromA1.Direction().XYZ(); |
221 | |
222 | gp_Mat MA1 (xDir, yDir, zDir); |
223 | gp_XYZ MA1loc = FromA1.Location().XYZ(); |
224 | |
225 | // matrix * MA1 => FromA1 ToA2 : |
226 | MA1loc.Multiply (matrix); |
227 | loc.Add (MA1loc); |
228 | matrix.Multiply (MA1); |
229 | } |
230 | |
231 | void gp_Trsf::SetTransformation (const gp_Ax3& A3) |
232 | { |
233 | shape = gp_CompoundTrsf; |
234 | scale = 1.0; |
c16c60a4 |
235 | matrix.SetRows (A3.XDirection().XYZ(), |
7fd59977 |
236 | A3.YDirection().XYZ(), |
237 | A3. Direction().XYZ()); |
c16c60a4 |
238 | loc = A3.Location().XYZ(); |
7fd59977 |
239 | loc.Multiply (matrix); |
240 | loc.Reverse (); |
241 | } |
242 | |
243 | //======================================================================= |
244 | //function : SetTransformation |
245 | //purpose : |
246 | //======================================================================= |
247 | |
248 | void gp_Trsf::SetTransformation (const gp_Quaternion& R, const gp_Vec& T) |
249 | { |
250 | shape = gp_CompoundTrsf; |
251 | scale = 1.; |
252 | loc = T.XYZ(); |
253 | matrix = R.GetMatrix(); |
254 | } |
255 | |
256 | //======================================================================= |
257 | //function : SetDisplacement |
258 | //purpose : |
259 | //======================================================================= |
260 | |
261 | void gp_Trsf::SetDisplacement (const gp_Ax3& FromA1, |
262 | const gp_Ax3& ToA2) |
263 | { |
264 | shape = gp_CompoundTrsf; |
265 | scale = 1.0; |
266 | // matrix from ToA2 to XOY : |
267 | matrix.SetCol (1, ToA2.XDirection().XYZ()); |
268 | matrix.SetCol (2, ToA2.YDirection().XYZ()); |
269 | matrix.SetCol (3, ToA2.Direction().XYZ()); |
270 | loc = ToA2.Location().XYZ(); |
271 | // matrix XOY to FromA1 : |
272 | const gp_XYZ& xDir = FromA1.XDirection().XYZ(); |
273 | const gp_XYZ& yDir = FromA1.YDirection().XYZ(); |
274 | const gp_XYZ& zDir = FromA1.Direction().XYZ(); |
275 | gp_Mat MA1 (xDir, yDir, zDir); |
276 | MA1.Transpose(); |
277 | gp_XYZ MA1loc = FromA1.Location().XYZ(); |
278 | MA1loc.Multiply (MA1); |
279 | MA1loc.Reverse(); |
280 | // matrix * MA1 |
281 | MA1loc.Multiply (matrix); |
282 | loc.Add (MA1loc); |
283 | matrix.Multiply (MA1); |
284 | } |
285 | |
286 | //======================================================================= |
287 | //function : SetTranslationPart |
288 | //purpose : |
289 | //======================================================================= |
290 | |
291 | void gp_Trsf::SetTranslationPart (const gp_Vec& V) { |
292 | |
293 | loc = V.XYZ(); |
57922171 |
294 | const Standard_Boolean locnull = (loc.SquareModulus() < gp::Resolution()); |
7fd59977 |
295 | |
296 | switch (shape) { |
297 | |
298 | case gp_Identity : |
299 | if (!locnull) shape = gp_Translation; |
300 | break; |
301 | |
302 | case gp_Translation : |
303 | if (locnull) shape = gp_Identity; |
304 | break; |
305 | |
306 | case gp_Rotation : |
307 | case gp_PntMirror : |
308 | case gp_Ax1Mirror : |
309 | case gp_Ax2Mirror : |
310 | case gp_Scale : |
311 | case gp_CompoundTrsf : |
312 | case gp_Other : |
57922171 |
313 | if (!locnull) { |
314 | shape = gp_CompoundTrsf; |
315 | } |
7fd59977 |
316 | break; |
317 | } |
318 | } |
319 | |
320 | //======================================================================= |
321 | //function : SetScaleFactor |
322 | //purpose : |
323 | //======================================================================= |
324 | |
325 | void gp_Trsf::SetScaleFactor (const Standard_Real S) |
326 | { |
327 | Standard_Real As = S; |
328 | if (As < 0) As = - As; |
329 | Standard_ConstructionError_Raise_if |
330 | (As <= gp::Resolution(),"gp_Trsf::SetScaleFactor"); |
331 | scale = S; |
332 | As = scale - 1.; |
333 | if (As < 0) As = - As; |
9256a43d |
334 | Standard_Boolean unit = As <= gp::Resolution(); // = (scale == 1) |
7fd59977 |
335 | As = scale + 1.; |
336 | if (As < 0) As = - As; |
9256a43d |
337 | Standard_Boolean munit = As <= gp::Resolution(); // = (scale == -1) |
7fd59977 |
338 | |
339 | switch (shape) { |
340 | case gp_Identity : |
341 | case gp_Translation : |
342 | if (!unit) shape = gp_Scale; |
343 | if (munit) shape = gp_PntMirror; |
344 | break; |
345 | case gp_Rotation : |
346 | if (!unit) shape = gp_CompoundTrsf; |
347 | break; |
348 | case gp_PntMirror : |
349 | case gp_Ax1Mirror : |
350 | case gp_Ax2Mirror : |
351 | if (!munit) shape = gp_Scale; |
352 | if (unit) shape = gp_Identity; |
353 | break; |
354 | case gp_Scale : |
355 | if (unit) shape = gp_Identity; |
356 | if (munit) shape = gp_PntMirror; |
357 | break; |
358 | case gp_CompoundTrsf : |
359 | break; |
360 | case gp_Other : |
361 | break; |
362 | } |
363 | } |
364 | |
365 | //======================================================================= |
366 | //function : SetValues |
367 | //purpose : |
368 | // 06-01-1998 modified by PMN : On utilise TolDist pour evaluer si les coeffs |
369 | // sont nuls : c'est toujours mieux que gp::Resolution ! |
370 | //======================================================================= |
371 | |
372 | void gp_Trsf::SetValues(const Standard_Real a11, |
373 | const Standard_Real a12, |
374 | const Standard_Real a13, |
375 | const Standard_Real a14, |
376 | const Standard_Real a21, |
377 | const Standard_Real a22, |
378 | const Standard_Real a23, |
379 | const Standard_Real a24, |
380 | const Standard_Real a31, |
381 | const Standard_Real a32, |
382 | const Standard_Real a33, |
7a8c6a36 |
383 | const Standard_Real a34) |
7fd59977 |
384 | { |
385 | gp_XYZ col1(a11,a21,a31); |
386 | gp_XYZ col2(a12,a22,a32); |
387 | gp_XYZ col3(a13,a23,a33); |
388 | gp_XYZ col4(a14,a24,a34); |
389 | // compute the determinant |
390 | gp_Mat M(col1,col2,col3); |
391 | Standard_Real s = M.Determinant(); |
392 | Standard_Real As = s; |
393 | if (As < 0) As = - As; |
394 | Standard_ConstructionError_Raise_if |
7a8c6a36 |
395 | (As < gp::Resolution(),"gp_Trsf::SetValues, null determinant"); |
7fd59977 |
396 | if (s > 0) |
397 | s = Pow(s,1./3.); |
398 | else |
399 | s = -Pow(-s,1./3.); |
400 | M.Divide(s); |
401 | |
7fd59977 |
402 | scale = s; |
403 | shape = gp_CompoundTrsf; |
7a8c6a36 |
404 | |
7fd59977 |
405 | matrix = M; |
7a8c6a36 |
406 | Orthogonalize(); |
407 | |
7fd59977 |
408 | loc = col4; |
409 | } |
410 | |
411 | //======================================================================= |
412 | //function : GetRotation |
413 | //purpose : |
414 | //======================================================================= |
415 | |
416 | gp_Quaternion gp_Trsf::GetRotation () const |
417 | { |
418 | return gp_Quaternion (matrix); |
419 | } |
420 | |
421 | //======================================================================= |
422 | //function : VectorialPart |
423 | //purpose : |
424 | //======================================================================= |
425 | |
426 | gp_Mat gp_Trsf::VectorialPart () const |
427 | { |
428 | if (scale == 1.0) return matrix; |
429 | gp_Mat M = matrix; |
430 | if (shape == gp_Scale || shape == gp_PntMirror) |
431 | M.SetDiagonal(scale*M.Value(1,1), |
432 | scale*M.Value(2,2), |
433 | scale*M.Value(3,3)); |
434 | else |
435 | M.Multiply (scale); |
436 | return M; |
437 | } |
438 | |
439 | //======================================================================= |
440 | //function : Invert |
441 | //purpose : |
442 | //======================================================================= |
443 | |
444 | void gp_Trsf::Invert() |
445 | { |
446 | // -1 |
447 | // X' = scale * R * X + T => X = (R / scale) * ( X' - T) |
448 | // |
449 | // Pour les gp_Trsf puisque le scale est extrait de la gp_Matrice R |
450 | // on a toujours determinant (R) = 1 et R-1 = R transposee. |
451 | if (shape == gp_Identity) { } |
452 | else if (shape == gp_Translation || shape == gp_PntMirror) loc.Reverse(); |
453 | else if (shape == gp_Scale) { |
c16c60a4 |
454 | Standard_ConstructionError_Raise_if (Abs(scale) <= gp::Resolution(), "gp_Trsf::Invert() - transformation has zero scale"); |
7fd59977 |
455 | scale = 1.0 / scale; |
456 | loc.Multiply (-scale); |
457 | } |
458 | else { |
c16c60a4 |
459 | Standard_ConstructionError_Raise_if (Abs(scale) <= gp::Resolution(), "gp_Trsf::Invert() - transformation has zero scale"); |
7fd59977 |
460 | scale = 1.0 / scale; |
461 | matrix.Transpose (); |
462 | loc.Multiply (matrix); |
463 | loc.Multiply (-scale); |
464 | } |
465 | } |
466 | |
467 | //======================================================================= |
468 | //function : Multiply |
469 | //purpose : |
470 | //======================================================================= |
471 | |
472 | void gp_Trsf::Multiply(const gp_Trsf& T) |
473 | { |
474 | if (T.shape == gp_Identity) { } |
475 | else if (shape == gp_Identity) { |
476 | shape = T.shape; |
477 | scale = T.scale; |
478 | loc = T.loc; |
479 | matrix = T.matrix; |
480 | } |
481 | else if (shape == gp_Rotation && T.shape == gp_Rotation) { |
f6f03db9 |
482 | if (T.loc.X() != 0.0 || T.loc.Y() != 0.0 || T.loc.Z() != 0.0) { |
7fd59977 |
483 | loc.Add (T.loc.Multiplied (matrix)); |
484 | } |
485 | matrix.Multiply(T.matrix); |
486 | } |
487 | else if (shape == gp_Translation && T.shape == gp_Translation) { |
488 | loc.Add (T.loc); |
489 | } |
490 | else if (shape == gp_Scale && T.shape == gp_Scale) { |
491 | loc.Add (T.loc.Multiplied(scale)); |
492 | scale = scale * T.scale; |
493 | } |
494 | else if (shape == gp_PntMirror && T.shape == gp_PntMirror) { |
495 | scale = 1.0; |
496 | shape = gp_Translation; |
497 | loc.Add (T.loc.Reversed()); |
498 | } |
499 | else if (shape == gp_Ax1Mirror && T.shape == gp_Ax1Mirror) { |
500 | shape = gp_Rotation; |
501 | loc.Add (T.loc.Multiplied (matrix)); |
502 | matrix.Multiply(T.matrix); |
503 | } |
504 | else if ((shape == gp_CompoundTrsf || shape == gp_Rotation || |
505 | shape == gp_Ax1Mirror || shape == gp_Ax2Mirror) |
506 | && T.shape == gp_Translation) { |
507 | gp_XYZ Tloc(T.loc); |
508 | Tloc.Multiply(matrix); |
509 | if (scale != 1.0) { Tloc.Multiply(scale); } |
510 | loc.Add (Tloc); |
511 | } |
512 | else if ((shape == gp_Scale || shape == gp_PntMirror) |
513 | && T.shape == gp_Translation) { |
514 | gp_XYZ Tloc(T.loc); |
515 | Tloc.Multiply (scale); |
516 | loc.Add (Tloc); |
517 | } |
518 | else if (shape == gp_Translation && |
519 | (T.shape == gp_CompoundTrsf || T.shape == gp_Rotation || |
520 | T.shape == gp_Ax1Mirror || T.shape == gp_Ax2Mirror)) { |
521 | shape = gp_CompoundTrsf; |
522 | scale = T.scale; |
523 | loc.Add (T.loc); |
524 | matrix = T.matrix; |
525 | } |
526 | else if (shape == gp_Translation && |
527 | (T.shape == gp_Scale || T.shape == gp_PntMirror)) { |
528 | shape = T.shape; |
529 | loc.Add (T.loc); |
530 | scale = T.scale; |
531 | } |
532 | else if ((shape == gp_PntMirror || shape == gp_Scale) && |
533 | (T.shape == gp_PntMirror || T.shape == gp_Scale)) { |
534 | shape = gp_CompoundTrsf; |
535 | gp_XYZ Tloc(T.loc); |
536 | Tloc.Multiply (scale); |
537 | loc.Add (Tloc); |
538 | scale = scale * T.scale; |
539 | } |
540 | else if ((shape == gp_CompoundTrsf || shape == gp_Rotation || |
541 | shape == gp_Ax1Mirror || shape == gp_Ax2Mirror) |
542 | && (T.shape == gp_Scale || T.shape == gp_PntMirror)) { |
543 | shape = gp_CompoundTrsf; |
544 | gp_XYZ Tloc(T.loc); |
545 | if (scale == 1.0) { |
546 | scale = T.scale; |
547 | Tloc.Multiply(matrix); |
548 | } |
549 | else { |
550 | Tloc.Multiply (matrix); |
551 | Tloc.Multiply (scale); |
552 | scale = scale * T.scale; |
553 | } |
554 | loc.Add (Tloc); |
555 | } |
556 | else if ((T.shape == gp_CompoundTrsf || T.shape == gp_Rotation || |
557 | T.shape == gp_Ax1Mirror || T.shape == gp_Ax2Mirror) |
558 | && (shape == gp_Scale || shape == gp_PntMirror)) { |
559 | shape = gp_CompoundTrsf; |
560 | gp_XYZ Tloc(T.loc); |
561 | Tloc.Multiply(scale); |
562 | loc.Add (Tloc); |
563 | scale = scale * T.scale; |
564 | matrix = T.matrix; |
565 | } |
566 | else { |
567 | shape = gp_CompoundTrsf; |
568 | gp_XYZ Tloc(T.loc); |
569 | Tloc.Multiply (matrix); |
570 | if (scale != 1.0) { |
571 | Tloc.Multiply (scale); |
572 | scale = scale * T.scale; |
573 | } |
574 | else { scale = T.scale; } |
575 | loc.Add (Tloc); |
576 | matrix.Multiply(T.matrix); |
577 | } |
578 | } |
579 | |
580 | //======================================================================= |
581 | //function : Power |
582 | //purpose : |
583 | //======================================================================= |
584 | |
585 | void gp_Trsf::Power (const Standard_Integer N) |
586 | { |
587 | if (shape == gp_Identity) { } |
588 | else { |
589 | if (N == 0) { |
590 | scale = 1.0; |
591 | shape = gp_Identity; |
592 | matrix.SetIdentity(); |
593 | loc = gp_XYZ (0.0, 0.0, 0.0); |
594 | } |
595 | else if (N == 1) { } |
596 | else if (N == -1) { Invert(); } |
597 | else { |
598 | if (N < 0) { Invert(); } |
599 | if (shape == gp_Translation) { |
600 | Standard_Integer Npower = N; |
601 | if (Npower < 0) Npower = - Npower; |
602 | Npower--; |
603 | gp_XYZ Temploc = loc; |
302f96fb |
604 | for(;;) { |
7fd59977 |
605 | if (IsOdd(Npower)) loc.Add (Temploc); |
606 | if (Npower == 1) break; |
607 | Temploc.Add (Temploc); |
608 | Npower = Npower/2; |
609 | } |
610 | } |
611 | else if (shape == gp_Scale) { |
612 | Standard_Integer Npower = N; |
613 | if (Npower < 0) Npower = - Npower; |
614 | Npower--; |
615 | gp_XYZ Temploc = loc; |
616 | Standard_Real Tempscale = scale; |
302f96fb |
617 | for(;;) { |
7fd59977 |
618 | if (IsOdd(Npower)) { |
619 | loc.Add (Temploc.Multiplied(scale)); |
620 | scale = scale * Tempscale; |
621 | } |
622 | if (Npower == 1) break; |
623 | Temploc.Add (Temploc.Multiplied(Tempscale)); |
624 | Tempscale = Tempscale * Tempscale; |
625 | Npower = Npower/2; |
626 | } |
627 | } |
628 | else if (shape == gp_Rotation) { |
629 | Standard_Integer Npower = N; |
630 | if (Npower < 0) Npower = - Npower; |
631 | Npower--; |
632 | gp_Mat Tempmatrix (matrix); |
633 | if (loc.X() == 0.0 && loc.Y() == 0.0 && loc.Z() == 0.0) { |
302f96fb |
634 | for(;;) { |
7fd59977 |
635 | if (IsOdd(Npower)) matrix.Multiply (Tempmatrix); |
636 | if (Npower == 1) break; |
637 | Tempmatrix.Multiply (Tempmatrix); |
638 | Npower = Npower/2; |
639 | } |
640 | } |
641 | else { |
642 | gp_XYZ Temploc = loc; |
302f96fb |
643 | for(;;) { |
7fd59977 |
644 | if (IsOdd(Npower)) { |
645 | loc.Add (Temploc.Multiplied (matrix)); |
646 | matrix.Multiply (Tempmatrix); |
647 | } |
648 | if (Npower == 1) break; |
649 | Temploc.Add (Temploc.Multiplied (Tempmatrix)); |
650 | Tempmatrix.Multiply (Tempmatrix); |
651 | Npower = Npower/2; |
652 | } |
653 | } |
654 | } |
655 | else if (shape == gp_PntMirror || shape == gp_Ax1Mirror || |
656 | shape == gp_Ax2Mirror) { |
657 | if (IsEven (N)) { |
658 | shape = gp_Identity; |
659 | scale = 1.0; |
660 | matrix.SetIdentity (); |
661 | loc.SetX(0); |
662 | loc.SetY(0); |
663 | loc.SetZ(0); |
664 | } |
665 | } |
666 | else { |
667 | shape = gp_CompoundTrsf; |
668 | Standard_Integer Npower = N; |
669 | if (Npower < 0) Npower = - Npower; |
670 | Npower--; |
671 | gp_XYZ Temploc = loc; |
672 | Standard_Real Tempscale = scale; |
673 | gp_Mat Tempmatrix (matrix); |
302f96fb |
674 | for(;;) { |
7fd59977 |
675 | if (IsOdd(Npower)) { |
676 | loc.Add ((Temploc.Multiplied (matrix)).Multiplied (scale)); |
677 | scale = scale * Tempscale; |
678 | matrix.Multiply (Tempmatrix); |
679 | } |
680 | if (Npower == 1) break; |
681 | Tempscale = Tempscale * Tempscale; |
682 | Temploc.Add ( (Temploc.Multiplied (Tempmatrix)).Multiplied |
683 | (Tempscale) |
684 | ); |
685 | Tempmatrix.Multiply (Tempmatrix); |
686 | Npower = Npower/2; |
687 | } |
688 | } |
689 | } |
690 | } |
691 | } |
692 | |
693 | //======================================================================= |
694 | //function : PreMultiply |
695 | //purpose : |
696 | //======================================================================= |
697 | |
698 | void gp_Trsf::PreMultiply (const gp_Trsf& T) |
699 | { |
700 | if (T.shape == gp_Identity) { } |
701 | else if (shape == gp_Identity) { |
702 | shape = T.shape; |
703 | scale = T.scale; |
704 | loc = T.loc; |
705 | matrix = T.matrix; |
706 | } |
707 | else if (shape == gp_Rotation && T.shape == gp_Rotation) { |
708 | loc.Multiply (T.matrix); |
709 | loc.Add (T.loc); |
710 | matrix.PreMultiply(T.matrix); |
711 | } |
712 | else if (shape == gp_Translation && T.shape == gp_Translation) { |
713 | loc.Add (T.loc); |
714 | } |
715 | else if (shape == gp_Scale && T.shape == gp_Scale) { |
716 | loc.Multiply (T.scale); |
717 | loc.Add (T.loc); |
718 | scale = scale * T.scale; |
719 | } |
720 | else if (shape == gp_PntMirror && T.shape == gp_PntMirror) { |
721 | scale = 1.0; |
722 | shape = gp_Translation; |
723 | loc.Reverse(); |
724 | loc.Add (T.loc); |
725 | } |
726 | else if (shape == gp_Ax1Mirror && T.shape == gp_Ax1Mirror) { |
727 | shape = gp_Rotation; |
728 | loc.Multiply (T.matrix); |
729 | loc.Add (T.loc); |
730 | matrix.PreMultiply(T.matrix); |
731 | } |
732 | else if ((shape == gp_CompoundTrsf || shape == gp_Rotation || |
733 | shape == gp_Ax1Mirror || shape == gp_Ax2Mirror) |
734 | && T.shape == gp_Translation) { |
735 | loc.Add (T.loc); |
736 | } |
737 | else if ((shape == gp_Scale || shape == gp_PntMirror) |
738 | && T.shape == gp_Translation) { |
739 | loc.Add (T.loc); |
740 | } |
741 | else if (shape == gp_Translation && |
742 | (T.shape == gp_CompoundTrsf || T.shape == gp_Rotation |
743 | || T.shape == gp_Ax1Mirror || T.shape == gp_Ax2Mirror)) { |
744 | shape = gp_CompoundTrsf; |
745 | matrix = T.matrix; |
746 | if (T.scale == 1.0) loc.Multiply (T.matrix); |
747 | else { |
748 | scale = T.scale; |
749 | loc.Multiply (matrix); |
750 | loc.Multiply (scale); |
751 | } |
752 | loc.Add (T.loc); |
753 | } |
754 | else if ((T.shape == gp_Scale || T.shape == gp_PntMirror) |
755 | && shape == gp_Translation) { |
756 | loc.Multiply (T.scale); |
757 | loc.Add (T.loc); |
758 | scale = T.scale; |
759 | shape = T.shape; |
760 | } |
761 | else if ((shape == gp_PntMirror || shape == gp_Scale) && |
762 | (T.shape == gp_PntMirror || T.shape == gp_Scale)) { |
763 | shape = gp_CompoundTrsf; |
764 | loc.Multiply (T.scale); |
765 | loc.Add (T.loc); |
766 | scale = scale * T.scale; |
767 | } |
768 | else if ((shape == gp_CompoundTrsf || shape == gp_Rotation || |
769 | shape == gp_Ax1Mirror || shape == gp_Ax2Mirror) |
770 | && (T.shape == gp_Scale || T.shape == gp_PntMirror)) { |
771 | shape = gp_CompoundTrsf; |
772 | loc.Multiply (T.scale); |
773 | loc.Add (T.loc); |
774 | scale = scale * T.scale; |
775 | } |
776 | else if ((T.shape == gp_CompoundTrsf || T.shape == gp_Rotation || |
777 | T.shape == gp_Ax1Mirror || T.shape == gp_Ax2Mirror) |
778 | && (shape == gp_Scale || shape == gp_PntMirror)) { |
779 | shape = gp_CompoundTrsf; |
780 | matrix = T.matrix; |
781 | if (T.scale == 1.0) loc.Multiply (T.matrix); |
782 | else { |
783 | loc.Multiply (matrix); |
784 | loc.Multiply (T.scale); |
785 | scale = T.scale * scale; |
786 | } |
787 | loc.Add (T.loc); |
788 | } |
789 | else { |
790 | shape = gp_CompoundTrsf; |
791 | loc.Multiply (T.matrix); |
792 | if (T.scale != 1.0) { |
793 | loc.Multiply (T.scale); scale = scale * T.scale; |
794 | } |
795 | loc.Add (T.loc); |
796 | matrix.PreMultiply(T.matrix); |
797 | } |
798 | } |
799 | |
800 | //======================================================================= |
801 | //function : GetRotation |
802 | //purpose : algorithm from A.Korn, M.Korn, "Mathematical Handbook for |
803 | // scientists and Engineers" McGraw-Hill, 1961, ch.14.10-2. |
804 | //======================================================================= |
805 | |
806 | Standard_Boolean gp_Trsf::GetRotation (gp_XYZ& theAxis, |
807 | Standard_Real& theAngle) const |
808 | { |
809 | gp_Quaternion Q = GetRotation(); |
810 | gp_Vec aVec; |
811 | Q.GetVectorAndAngle (aVec, theAngle); |
812 | theAxis = aVec.XYZ(); |
813 | return Standard_True; |
814 | } |
7a8c6a36 |
815 | |
816 | //======================================================================= |
817 | //function : Orthogonalize |
818 | //purpose : |
4dba155d |
819 | //ATTENTION!!! |
820 | // Orthogonalization is not equivalent transformation. Therefore, |
821 | // transformation with source matrix and with orthogonalized matrix can |
822 | // lead to different results for one shape. Consequently, source matrix must |
823 | // be close to orthogonalized matrix for reducing these differences. |
7a8c6a36 |
824 | //======================================================================= |
825 | void gp_Trsf::Orthogonalize() |
826 | { |
4dba155d |
827 | //Matrix M is called orthogonal if and only if |
828 | // M*Transpose(M) == E |
829 | //where E is identity matrix. |
830 | |
831 | //Set of all rows (as of all columns) of matrix M (for gp_Trsf class) is |
832 | //orthonormal basis. If this condition is not satisfied then the basis can be |
833 | //orthonormalized in accordance with below described algorithm. |
834 | |
835 | //In 3D-space, we have the linear span of three basis vectors: V1, V2 and V3. |
836 | //Correspond orthonormalized basis is formed by vectors Vn1, Vn2 and Vn3. |
837 | |
838 | //In this case, |
839 | // Vn_{i}*Vn_{j} = (i == j)? 1 : 0. |
840 | |
841 | //The algorithm includes following steps: |
842 | |
843 | //1. Normalize V1 vector: |
844 | // V1n=V1/|V1|; |
845 | // |
846 | //2. Let |
847 | // V2n=V2-m*V1n. |
848 | // |
849 | //After multiplication two parts of this equation by V1n, |
850 | //we will have following equation: |
851 | // 0=V2*V1n-m <==> m=V2*V1n. |
852 | // |
853 | //Consequently, |
854 | // V2n=V2-(V2*V1n)*V1n. |
855 | |
856 | //3. Let |
857 | // V3n=V3-m1*V1n-m2*V2n. |
858 | // |
859 | //After multiplication two parts of this equation by V1n, |
860 | //we will have following equation: |
861 | // 0=V3*V1n-m1 <==> m1=V3*V1n. |
862 | // |
863 | //After multiplication two parts of main equation by V2n, |
864 | //we will have following equation: |
865 | // 0=V3*V2n-m2 <==> m2=V3*V2n. |
866 | // |
867 | //In conclusion, |
868 | // V3n=V3-(V3*V1n)*V1n-(V3*V2n)*V2n. |
869 | |
7a8c6a36 |
870 | gp_Mat aTM(matrix); |
871 | |
872 | gp_XYZ aV1 = aTM.Column(1); |
873 | gp_XYZ aV2 = aTM.Column(2); |
874 | gp_XYZ aV3 = aTM.Column(3); |
875 | |
876 | aV1.Normalize(); |
877 | |
878 | aV2 -= aV1*(aV2.Dot(aV1)); |
879 | aV2.Normalize(); |
880 | |
881 | aV3 -= aV1*(aV3.Dot(aV1)) + aV2*(aV3.Dot(aV2)); |
882 | aV3.Normalize(); |
883 | |
884 | aTM.SetCols(aV1, aV2, aV3); |
885 | |
886 | aV1 = aTM.Row(1); |
887 | aV2 = aTM.Row(2); |
888 | aV3 = aTM.Row(3); |
889 | |
890 | aV1.Normalize(); |
891 | |
892 | aV2 -= aV1*(aV2.Dot(aV1)); |
893 | aV2.Normalize(); |
894 | |
895 | aV3 -= aV1*(aV3.Dot(aV1)) + aV2*(aV3.Dot(aV2)); |
896 | aV3.Normalize(); |
897 | |
898 | aTM.SetRows(aV1, aV2, aV3); |
899 | |
900 | matrix = aTM; |
901 | } |
0904aa63 |
902 | |
903 | //======================================================================= |
904 | //function : DumpJson |
905 | //purpose : |
906 | //======================================================================= |
bc73b006 |
907 | void gp_Trsf::DumpJson (Standard_OStream& theOStream, Standard_Integer) const |
0904aa63 |
908 | { |
bc73b006 |
909 | OCCT_DUMP_VECTOR_CLASS (theOStream, "Location", 3, loc.X(), loc.Y(), loc.Z()) |
910 | OCCT_DUMP_VECTOR_CLASS (theOStream, "Matrix", 9, matrix.Value(1, 1), matrix.Value(1, 2), matrix.Value(1, 3), |
911 | matrix.Value(2, 1), matrix.Value(2, 2), matrix.Value(2, 3), |
912 | matrix.Value(3, 1), matrix.Value(3, 2), matrix.Value(3, 3)) |
913 | OCCT_DUMP_FIELD_VALUE_NUMERICAL (theOStream, shape) |
914 | OCCT_DUMP_FIELD_VALUE_NUMERICAL (theOStream, scale) |
0904aa63 |
915 | } |
6b63dc83 |
916 | |
917 | //======================================================================= |
918 | //function : InitFromJson |
919 | //purpose : |
920 | //======================================================================= |
921 | Standard_Boolean gp_Trsf::InitFromJson (const Standard_SStream& theSStream, Standard_Integer& theStreamPos) |
922 | { |
923 | Standard_Integer aPos = theStreamPos; |
924 | TCollection_AsciiString aStreamStr = Standard_Dump::Text (theSStream); |
925 | |
926 | gp_XYZ anXYZLoc; |
927 | OCCT_INIT_VECTOR_CLASS (aStreamStr, "Location", aPos, 3, |
928 | &anXYZLoc.ChangeCoord (1), &anXYZLoc.ChangeCoord (2), &anXYZLoc.ChangeCoord (3)) |
929 | SetTranslation (anXYZLoc); |
930 | |
931 | Standard_Real mymatrix[3][3]; |
932 | OCCT_INIT_VECTOR_CLASS (aStreamStr, "Matrix", aPos, 9, &mymatrix[0][0], &mymatrix[0][1], &mymatrix[0][2], |
933 | &mymatrix[1][0], &mymatrix[1][1], &mymatrix[1][2], |
934 | &mymatrix[2][0], &mymatrix[2][1], &mymatrix[2][2]) |
935 | for (int i = 0; i < 3; i++) |
936 | { |
937 | for (int j = 0; j < 3; j++) |
938 | { |
939 | matrix.SetValue (i + 1, j + 1, mymatrix[i][j]); |
940 | } |
941 | } |
942 | |
943 | Standard_Real ashape; |
944 | OCCT_INIT_FIELD_VALUE_INTEGER (aStreamStr, aPos, ashape); |
945 | shape = (gp_TrsfForm)((Standard_Integer)ashape); |
946 | |
947 | OCCT_INIT_FIELD_VALUE_REAL (aStreamStr, aPos, scale); |
948 | |
949 | theStreamPos = aPos; |
950 | return Standard_True; |
951 | } |