0030997: Foundation Classes - name correction of dump macros
[occt.git] / src / gp / gp_Mat.cxx
1 // Copyright (c) 1995-1999 Matra Datavision
2 // Copyright (c) 1999-2014 OPEN CASCADE SAS
3 //
4 // This file is part of Open CASCADE Technology software library.
5 //
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
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.
11 //
12 // Alternatively, this file may be used under the terms of Open CASCADE
13 // commercial license or contractual agreement.
14
15 //  10/09/97 : PMN : Correction BUC40192 (pb avec les matrices negatives)
16
17 #ifndef OCCT_DEBUG
18 #define No_Standard_OutOfRange
19 #define No_Standard_ConstructionError
20 #endif
21
22
23 #include <gp_GTrsf.hxx>
24 #include <gp_Mat.hxx>
25 #include <gp_Trsf.hxx>
26 #include <gp_XYZ.hxx>
27 #include <Standard_ConstructionError.hxx>
28 #include <Standard_OutOfRange.hxx>
29 #include <Standard_Dump.hxx>
30
31 #define M00 ((Standard_Real*)M)[0]
32 #define M01 ((Standard_Real*)M)[1]
33 #define M02 ((Standard_Real*)M)[2]
34 #define M10 ((Standard_Real*)M)[3]
35 #define M11 ((Standard_Real*)M)[4]
36 #define M12 ((Standard_Real*)M)[5]
37 #define M20 ((Standard_Real*)M)[6]
38 #define M21 ((Standard_Real*)M)[7]
39 #define M22 ((Standard_Real*)M)[8]
40
41 #define N00 ((Standard_Real*)N)[0]
42 #define N01 ((Standard_Real*)N)[1]
43 #define N02 ((Standard_Real*)N)[2]
44 #define N10 ((Standard_Real*)N)[3]
45 #define N11 ((Standard_Real*)N)[4]
46 #define N12 ((Standard_Real*)N)[5]
47 #define N20 ((Standard_Real*)N)[6]
48 #define N21 ((Standard_Real*)N)[7]
49 #define N22 ((Standard_Real*)N)[8]
50
51 gp_Mat::gp_Mat (const gp_XYZ& Col1,
52                 const gp_XYZ& Col2,
53                 const gp_XYZ& Col3)
54 {
55   const Standard_Address M = (Standard_Address)&(matrix[0][0]);
56   M00 = Col1.X(); M10 = Col1.Y(); M20 = Col1.Z();
57   M01 = Col2.X(); M11 = Col2.Y(); M21 = Col2.Z();
58   M02 = Col3.X(); M12 = Col3.Y(); M22 = Col3.Z();
59 }
60
61 void gp_Mat::SetCol (const Standard_Integer Col,
62                      const gp_XYZ& Value) {
63
64   Standard_OutOfRange_Raise_if (Col < 1 || Col > 3, " ");
65   const Standard_Address M = (Standard_Address)&(matrix[0][0]);
66   if      (Col == 1) {
67     M00 = Value.X(); M10 = Value.Y(); M20 = Value.Z();
68   }
69   else if (Col == 2) {
70     M01 = Value.X(); M11 = Value.Y(); M21 = Value.Z();
71   }
72   else {
73     M02 = Value.X(); M12 = Value.Y(); M22 = Value.Z();
74   }
75 }
76
77 void gp_Mat::SetCols (const gp_XYZ& Col1,
78                       const gp_XYZ& Col2,
79                       const gp_XYZ& Col3)
80 {
81   Mat00 = Col1.X(); Mat10 = Col1.Y(); Mat20 = Col1.Z();
82   Mat01 = Col2.X(); Mat11 = Col2.Y(); Mat21 = Col2.Z();
83   Mat02 = Col3.X(); Mat12 = Col3.Y(); Mat22 = Col3.Z();
84 }
85
86 void gp_Mat::SetCross (const gp_XYZ& Ref)
87 {
88   const Standard_Address M = (Standard_Address)&(matrix[0][0]);
89   Standard_Real X = Ref.X();
90   Standard_Real Y = Ref.Y();
91   Standard_Real Z = Ref.Z();
92   M00 = M11 = M22 = 0.0;
93   M01 = - Z;  
94   M02 = Y;
95   M12 = - X;
96   M10 = Z;
97   M20 = - Y;
98   M21 = X;
99 }
100
101 void gp_Mat::SetDot (const gp_XYZ& Ref)
102 {
103   const Standard_Address M = (Standard_Address)&(matrix[0][0]);
104   Standard_Real X = Ref.X();
105   Standard_Real Y = Ref.Y();
106   Standard_Real Z = Ref.Z();
107   M00 = X * X;
108   M11 = Y * Y;
109   M22 = Z * Z;
110   M01 = X * Y;
111   M02 = X * Z;
112   M12 = Y * Z;
113   M10 = M01;
114   M20 = M02;
115   M21 = M12;
116 }
117
118 void gp_Mat::SetRotation (const gp_XYZ& Axis,
119                           const Standard_Real Ang)
120 {
121   //    Rot = I + sin(Ang) * M + (1. - cos(Ang)) * M*M
122   //    avec  M . XYZ = Axis ^ XYZ
123
124 //  const Standard_Address M = (Standard_Address)&(matrix[0][0]);
125   gp_XYZ V = Axis.Normalized();
126   SetCross (V);
127   Multiply (sin(Ang));
128   gp_Mat Temp;
129   Temp.SetScale (1.0);
130   Add (Temp);
131   Standard_Real A = V.X();
132   Standard_Real B = V.Y();
133   Standard_Real C = V.Z();
134   Temp.SetRow (1, gp_XYZ(- C*C - B*B,      A*B,           A*C     ));
135   Temp.SetRow (2, gp_XYZ(     A*B,      -A*A - C*C,        B*C    ));
136   Temp.SetRow (3, gp_XYZ(     A*C,          B*C,       - A*A - B*B));
137   Temp.Multiply (1.0 - cos(Ang));
138   Add (Temp);
139 }
140
141 void gp_Mat::SetRow (const Standard_Integer Row,
142                      const gp_XYZ& Value)
143 {
144   Standard_OutOfRange_Raise_if (Row < 1 || Row > 3, " ");
145   const Standard_Address M = (Standard_Address)&(matrix[0][0]);
146   if      (Row == 1) {
147     M00 = Value.X(); M01 = Value.Y(); M02 = Value.Z();
148   }
149   else if (Row == 2) {
150     M10 = Value.X(); M11 = Value.Y(); M12 = Value.Z();
151   }
152   else {
153     M20 = Value.X(); M21 = Value.Y(); M22 = Value.Z();
154   }
155 }
156
157 void gp_Mat::SetRows (const gp_XYZ& Row1,
158                       const gp_XYZ& Row2,
159                       const gp_XYZ& Row3)
160 {
161   const Standard_Address M = (Standard_Address)&(matrix[0][0]);
162   M00 = Row1.X(); M01 = Row1.Y(); M02 = Row1.Z();
163   M10 = Row2.X(); M11 = Row2.Y(); M12 = Row2.Z();
164   M20 = Row3.X(); M21 = Row3.Y(); M22 = Row3.Z();
165 }
166
167 gp_XYZ gp_Mat::Column (const Standard_Integer Col) const
168 {
169   Standard_OutOfRange_Raise_if (Col < 1 || Col > 3, "gp_Mat::Column() - wrong index");
170   const Standard_Address M = (Standard_Address)&(matrix[0][0]);
171   if (Col == 1) return gp_XYZ (M00,M10,M20);
172   if (Col == 2) return gp_XYZ (M01,M11,M21);
173   return gp_XYZ (M02,M12,M22);
174 }
175
176 gp_XYZ gp_Mat::Diagonal () const
177 {
178   const Standard_Address M = (Standard_Address)&(matrix[0][0]);
179   return gp_XYZ (M00, M11, M22);
180 }
181
182 gp_XYZ gp_Mat::Row (const Standard_Integer Row) const
183 {
184   Standard_OutOfRange_Raise_if (Row < 1 || Row > 3, "gp_Mat::Row() - wrong index");
185   const Standard_Address M = (Standard_Address)&(matrix[0][0]);
186   if (Row == 1) return gp_XYZ (M00,M01,M02);
187   if (Row == 2) return gp_XYZ (M10,M11,M12);
188   return gp_XYZ (M20,M21,M22);
189 }
190
191 void gp_Mat::Invert ()
192
193   Standard_Real new_array[3][3] ;
194   const Standard_Address M = (Standard_Address)&(   matrix[0][0]);
195   const Standard_Address N = (Standard_Address)&(new_array[0][0]);  
196
197   //
198   // calcul de  la transposee de la commatrice
199   //
200   N00 = M11 * M22 - M12 * M21 ;
201   N10 = -(M10 * M22 - M20 * M12) ;
202   N20 = M10 * M21 - M20 * M11 ;
203   N01 = - (M01 * M22 - M21 * M02) ;
204   N11 = M00 * M22 - M20 * M02 ;
205   N21 = -(M00 * M21 - M20 * M01) ;
206   N02 = M01 * M12 - M11 * M02 ;
207   N12 = -(M00 * M12 - M10 * M02) ;
208   N22 = M00 * M11 - M01 * M10 ;
209   Standard_Real det =  M00 * N00 + M01* N10 + M02 * N20 ;
210   Standard_Real val = det;
211   if (val < 0) val = - val;
212   Standard_ConstructionError_Raise_if (val <= gp::Resolution(), "gp_Mat::Invert() - matrix has zero determinant");
213   det = 1.0e0 / det ;
214   M00 = N00;
215   M10 = N10;
216   M20 = N20;
217   M01 = N01;
218   M11 = N11;
219   M21 = N21;
220   M02 = N02;
221   M12 = N12;
222   M22 = N22;
223   Multiply(det) ;
224 }
225
226 gp_Mat gp_Mat::Inverted () const
227
228   gp_Mat NewMat;
229   const Standard_Address M = (Standard_Address)&(       matrix[0][0]);
230   const Standard_Address N = (Standard_Address)&(NewMat.matrix[0][0]);
231   //
232   // calcul de  la transposee de la commatrice
233   //
234   N00 = M11 * M22 - M12 * M21 ;
235   N10 = -(M10 * M22 - M20 * M12) ;
236   N20 = M10 * M21 - M20 * M11 ;
237   N01 = - (M01 * M22 - M21 * M02) ;
238   N11 = M00 * M22 - M20 * M02 ;
239   N21 = -(M00 * M21 - M20 * M01) ;
240   N02 = M01 * M12 - M11 * M02 ;
241   N12 = -(M00 * M12 - M10 * M02) ;
242   N22 = M00 * M11 - M01 * M10 ;
243   Standard_Real det =  M00 * N00 + M01* N10 + M02 * N20 ;
244   Standard_Real val = det;
245   if (val < 0) val = - val;
246   Standard_ConstructionError_Raise_if (val <= gp::Resolution(), "gp_Mat::Inverted() - matrix has zero determinant");
247   det = 1.0e0 / det ;
248   NewMat.Multiply(det) ;
249   return NewMat;
250 }
251
252 void gp_Mat::Power (const Standard_Integer N)
253 {
254   if (N == 1) { }
255   else if (N == 0)  { SetIdentity() ; }
256   else if (N == -1) { Invert(); }
257   else {
258     if (N < 0) { Invert(); }
259     Standard_Integer Npower = N;
260     if (Npower < 0) Npower = - Npower;
261     Npower--;
262     gp_Mat Temp = *this;
263     for(;;) {
264       if (IsOdd(Npower)) Multiply (Temp);
265       if (Npower == 1)   break; 
266       Temp.Multiply (Temp);
267       Npower>>=1;
268     }
269   }
270 }
271
272 //=======================================================================
273 //function : DumpJson
274 //purpose  : 
275 //=======================================================================
276 void gp_Mat::DumpJson (Standard_OStream& theOStream, const Standard_Integer) const
277 {
278   OCCT_DUMP_VECTOR_CLASS (theOStream, gp_Mat, 9, Mat00, Mat01, Mat02, Mat10, Mat11, Mat12, Mat20, Mat21, Mat22);
279 }