0024927: Getting rid of "Persistent" functionality -- Storable
[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 #include <gp_Mat.ixx>
23
24 #define M00 ((Standard_Real*)M)[0]
25 #define M01 ((Standard_Real*)M)[1]
26 #define M02 ((Standard_Real*)M)[2]
27 #define M10 ((Standard_Real*)M)[3]
28 #define M11 ((Standard_Real*)M)[4]
29 #define M12 ((Standard_Real*)M)[5]
30 #define M20 ((Standard_Real*)M)[6]
31 #define M21 ((Standard_Real*)M)[7]
32 #define M22 ((Standard_Real*)M)[8]
33
34 #define N00 ((Standard_Real*)N)[0]
35 #define N01 ((Standard_Real*)N)[1]
36 #define N02 ((Standard_Real*)N)[2]
37 #define N10 ((Standard_Real*)N)[3]
38 #define N11 ((Standard_Real*)N)[4]
39 #define N12 ((Standard_Real*)N)[5]
40 #define N20 ((Standard_Real*)N)[6]
41 #define N21 ((Standard_Real*)N)[7]
42 #define N22 ((Standard_Real*)N)[8]
43
44 gp_Mat::gp_Mat (const gp_XYZ& Col1,
45                 const gp_XYZ& Col2,
46                 const gp_XYZ& Col3)
47 {
48   const Standard_Address M = (Standard_Address)&(matrix[0][0]);
49   M00 = Col1.X(); M10 = Col1.Y(); M20 = Col1.Z();
50   M01 = Col2.X(); M11 = Col2.Y(); M21 = Col2.Z();
51   M02 = Col3.X(); M12 = Col3.Y(); M22 = Col3.Z();
52 }
53
54 void gp_Mat::SetCol (const Standard_Integer Col,
55                      const gp_XYZ& Value) {
56
57   Standard_OutOfRange_Raise_if (Col < 1 || Col > 3, " ");
58   const Standard_Address M = (Standard_Address)&(matrix[0][0]);
59   if      (Col == 1) {
60     M00 = Value.X(); M10 = Value.Y(); M20 = Value.Z();
61   }
62   else if (Col == 2) {
63     M01 = Value.X(); M11 = Value.Y(); M21 = Value.Z();
64   }
65   else {
66     M02 = Value.X(); M12 = Value.Y(); M22 = Value.Z();
67   }
68 }
69
70 void gp_Mat::SetCols (const gp_XYZ& Col1,
71                       const gp_XYZ& Col2,
72                       const gp_XYZ& Col3)
73 {
74   Mat00 = Col1.X(); Mat10 = Col1.Y(); Mat20 = Col1.Z();
75   Mat01 = Col2.X(); Mat11 = Col2.Y(); Mat21 = Col2.Z();
76   Mat02 = Col3.X(); Mat12 = Col3.Y(); Mat22 = Col3.Z();
77 }
78
79 void gp_Mat::SetCross (const gp_XYZ& Ref)
80 {
81   const Standard_Address M = (Standard_Address)&(matrix[0][0]);
82   Standard_Real X = Ref.X();
83   Standard_Real Y = Ref.Y();
84   Standard_Real Z = Ref.Z();
85   M00 = M11 = M22 = 0.0;
86   M01 = - Z;  
87   M02 = Y;
88   M12 = - X;
89   M10 = Z;
90   M20 = - Y;
91   M21 = X;
92 }
93
94 void gp_Mat::SetDot (const gp_XYZ& Ref)
95 {
96   const Standard_Address M = (Standard_Address)&(matrix[0][0]);
97   Standard_Real X = Ref.X();
98   Standard_Real Y = Ref.Y();
99   Standard_Real Z = Ref.Z();
100   M00 = X * X;
101   M11 = Y * Y;
102   M22 = Z * Z;
103   M01 = X * Y;
104   M02 = X * Z;
105   M12 = Y * Z;
106   M10 = M01;
107   M20 = M02;
108   M21 = M12;
109 }
110
111 void gp_Mat::SetRotation (const gp_XYZ& Axis,
112                           const Standard_Real Ang)
113 {
114   //    Rot = I + sin(Ang) * M + (1. - cos(Ang)) * M*M
115   //    avec  M . XYZ = Axis ^ XYZ
116
117 //  const Standard_Address M = (Standard_Address)&(matrix[0][0]);
118   gp_XYZ V = Axis.Normalized();
119   SetCross (V);
120   Multiply (sin(Ang));
121   gp_Mat Temp;
122   Temp.SetScale (1.0);
123   Add (Temp);
124   Standard_Real A = V.X();
125   Standard_Real B = V.Y();
126   Standard_Real C = V.Z();
127   Temp.SetRow (1, gp_XYZ(- C*C - B*B,      A*B,           A*C     ));
128   Temp.SetRow (2, gp_XYZ(     A*B,      -A*A - C*C,        B*C    ));
129   Temp.SetRow (3, gp_XYZ(     A*C,          B*C,       - A*A - B*B));
130   Temp.Multiply (1.0 - cos(Ang));
131   Add (Temp);
132 }
133
134 void gp_Mat::SetRow (const Standard_Integer Row,
135                      const gp_XYZ& Value)
136 {
137   Standard_OutOfRange_Raise_if (Row < 1 || Row > 3, " ");
138   const Standard_Address M = (Standard_Address)&(matrix[0][0]);
139   if      (Row == 1) {
140     M00 = Value.X(); M01 = Value.Y(); M02 = Value.Z();
141   }
142   else if (Row == 2) {
143     M10 = Value.X(); M11 = Value.Y(); M12 = Value.Z();
144   }
145   else {
146     M20 = Value.X(); M21 = Value.Y(); M22 = Value.Z();
147   }
148 }
149
150 void gp_Mat::SetRows (const gp_XYZ& Row1,
151                       const gp_XYZ& Row2,
152                       const gp_XYZ& Row3)
153 {
154   const Standard_Address M = (Standard_Address)&(matrix[0][0]);
155   M00 = Row1.X(); M01 = Row1.Y(); M02 = Row1.Z();
156   M10 = Row2.X(); M11 = Row2.Y(); M12 = Row2.Z();
157   M20 = Row3.X(); M21 = Row3.Y(); M22 = Row3.Z();
158 }
159
160 gp_XYZ gp_Mat::Column (const Standard_Integer Col) const
161 {
162   Standard_OutOfRange_Raise_if (Col < 1 || Col > 3, "");
163   const Standard_Address M = (Standard_Address)&(matrix[0][0]);
164   if (Col == 1) return gp_XYZ (M00,M10,M20);
165   if (Col == 2) return gp_XYZ (M01,M11,M21);
166   return gp_XYZ (M02,M12,M22);
167 }
168
169 gp_XYZ gp_Mat::Diagonal () const
170 {
171   const Standard_Address M = (Standard_Address)&(matrix[0][0]);
172   return gp_XYZ (M00, M11, M22);
173 }
174
175 gp_XYZ gp_Mat::Row (const Standard_Integer Row) const
176 {
177   Standard_OutOfRange_Raise_if (Row < 1 || Row > 3, "");
178   const Standard_Address M = (Standard_Address)&(matrix[0][0]);
179   if (Row == 1) return gp_XYZ (M00,M01,M02);
180   if (Row == 2) return gp_XYZ (M10,M11,M12);
181   return gp_XYZ (M20,M21,M22);
182 }
183
184 void gp_Mat::Invert ()
185
186   Standard_Real new_array[3][3] ;
187   const Standard_Address M = (Standard_Address)&(   matrix[0][0]);
188   const Standard_Address N = (Standard_Address)&(new_array[0][0]);  
189
190   //
191   // calcul de  la transposee de la commatrice
192   //
193   N00 = M11 * M22 - M12 * M21 ;
194   N10 = -(M10 * M22 - M20 * M12) ;
195   N20 = M10 * M21 - M20 * M11 ;
196   N01 = - (M01 * M22 - M21 * M02) ;
197   N11 = M00 * M22 - M20 * M02 ;
198   N21 = -(M00 * M21 - M20 * M01) ;
199   N02 = M01 * M12 - M11 * M02 ;
200   N12 = -(M00 * M12 - M10 * M02) ;
201   N22 = M00 * M11 - M01 * M10 ;
202   Standard_Real det =  M00 * N00 + M01* N10 + M02 * N20 ;
203   Standard_Real val = det;
204   if (val < 0) val = - val;
205   Standard_ConstructionError_Raise_if
206     (val <= gp::Resolution(),"");
207   det = 1.0e0 / det ;
208   M00 = N00;
209   M10 = N10;
210   M20 = N20;
211   M01 = N01;
212   M11 = N11;
213   M21 = N21;
214   M02 = N02;
215   M12 = N12;
216   M22 = N22;
217   Multiply(det) ;
218 }
219
220 gp_Mat gp_Mat::Inverted () const
221
222   gp_Mat NewMat;
223   const Standard_Address M = (Standard_Address)&(       matrix[0][0]);
224   const Standard_Address N = (Standard_Address)&(NewMat.matrix[0][0]);
225   //
226   // calcul de  la transposee de la commatrice
227   //
228   N00 = M11 * M22 - M12 * M21 ;
229   N10 = -(M10 * M22 - M20 * M12) ;
230   N20 = M10 * M21 - M20 * M11 ;
231   N01 = - (M01 * M22 - M21 * M02) ;
232   N11 = M00 * M22 - M20 * M02 ;
233   N21 = -(M00 * M21 - M20 * M01) ;
234   N02 = M01 * M12 - M11 * M02 ;
235   N12 = -(M00 * M12 - M10 * M02) ;
236   N22 = M00 * M11 - M01 * M10 ;
237   Standard_Real det =  M00 * N00 + M01* N10 + M02 * N20 ;
238   Standard_Real val = det;
239   if (val < 0) val = - val;
240   Standard_ConstructionError_Raise_if
241     (val <= gp::Resolution(),"");
242   det = 1.0e0 / det ;
243   NewMat.Multiply(det) ;
244   return NewMat;
245 }
246
247 void gp_Mat::Power (const Standard_Integer N)
248 {
249   if (N == 1) { }
250   else if (N == 0)  { SetIdentity() ; }
251   else if (N == -1) { Invert(); }
252   else {
253     if (N < 0) { Invert(); }
254     Standard_Integer Npower = N;
255     if (Npower < 0) Npower = - Npower;
256     Npower--;
257     gp_Mat Temp = *this;
258     for(;;) {
259       if (IsOdd(Npower)) Multiply (Temp);
260       if (Npower == 1)   break; 
261       Temp.Multiply (Temp);
262       Npower>>=1;
263     }
264   }
265 }
266