0023024: Update headers of OCCT files
[occt.git] / src / gp / gp_Mat.lxx
1 // Copyright (c) 1995-1999 Matra Datavision
2 // Copyright (c) 1999-2012 OPEN CASCADE SAS
3 //
4 // The content of this file is subject to the Open CASCADE Technology Public
5 // License Version 6.5 (the "License"). You may not use the content of this file
6 // except in compliance with the License. Please obtain a copy of the License
7 // at http://www.opencascade.org and read it completely before using this file.
8 //
9 // The Initial Developer of the Original Code is Open CASCADE S.A.S., having its
10 // main offices at: 1, place des Freres Montgolfier, 78280 Guyancourt, France.
11 //
12 // The Original Code and all software distributed under the License is
13 // distributed on an "AS IS" basis, without warranty of any kind, and the
14 // Initial Developer hereby disclaims all such warranties, including without
15 // limitation, any warranties of merchantability, fitness for a particular
16 // purpose or non-infringement. Please see the License for the specific terms
17 // and conditions governing the rights and limitations under the License.
18
19
20 #include <gp.hxx>
21 #include <Standard_OutOfRange.hxx>
22 #include <Standard_ConstructionError.hxx>
23
24 #define Mat00 matrix[0][0]
25 #define Mat01 matrix[0][1]
26 #define Mat02 matrix[0][2]
27 #define Mat10 matrix[1][0]
28 #define Mat11 matrix[1][1]
29 #define Mat12 matrix[1][2]
30 #define Mat20 matrix[2][0]
31 #define Mat21 matrix[2][1]
32 #define Mat22 matrix[2][2]
33
34 #define Nat00 NewMat.matrix[0][0]
35 #define Nat01 NewMat.matrix[0][1]
36 #define Nat02 NewMat.matrix[0][2]
37 #define Nat10 NewMat.matrix[1][0]
38 #define Nat11 NewMat.matrix[1][1]
39 #define Nat12 NewMat.matrix[1][2]
40 #define Nat20 NewMat.matrix[2][0]
41 #define Nat21 NewMat.matrix[2][1]
42 #define Nat22 NewMat.matrix[2][2]
43
44 #define Oat00 Other.matrix[0][0]
45 #define Oat01 Other.matrix[0][1]
46 #define Oat02 Other.matrix[0][2]
47 #define Oat10 Other.matrix[1][0]
48 #define Oat11 Other.matrix[1][1]
49 #define Oat12 Other.matrix[1][2]
50 #define Oat20 Other.matrix[2][0]
51 #define Oat21 Other.matrix[2][1]
52 #define Oat22 Other.matrix[2][2]
53
54 inline gp_Mat::gp_Mat () {
55   Mat00 =
56     Mat01 =
57       Mat02 =
58         Mat10 =
59           Mat11 =
60             Mat12 =
61               Mat20 =
62                 Mat21 =
63                   Mat22 = 0.0;
64 }
65
66 inline gp_Mat::gp_Mat (const Standard_Real a11,
67                        const Standard_Real a12,
68                        const Standard_Real a13,
69                        const Standard_Real a21,
70                        const Standard_Real a22,
71                        const Standard_Real a23,
72                        const Standard_Real a31,
73                        const Standard_Real a32,
74                        const Standard_Real a33) {
75
76   Mat00 = a11;
77   Mat01 = a12;
78   Mat02 = a13;
79   Mat10 = a21;
80   Mat11 = a22;
81   Mat12 = a23;
82   Mat20 = a31;
83   Mat21 = a32;
84   Mat22 = a33;
85 }
86
87 inline void gp_Mat::SetDiagonal (const Standard_Real X1,
88                                  const Standard_Real X2,
89                                  const Standard_Real X3)
90 {
91   Mat00 = X1;   Mat11 = X2;   Mat22 = X3;
92 }
93
94 inline void gp_Mat::SetIdentity ()
95 {
96   Mat00 = Mat11 = Mat22 = 1.0;
97   Mat01 = Mat02 = Mat10  = Mat12 = Mat20 = Mat21 = 0.0;
98  }
99
100 inline void gp_Mat::SetScale (const Standard_Real S)
101 {
102   Mat00 = Mat11 =  Mat22 = S;
103   Mat01 = Mat02 = Mat10 = Mat12 = Mat20 = Mat21 = 0.0;
104 }
105
106 inline void gp_Mat::SetValue (const Standard_Integer Row, 
107                               const Standard_Integer Col, 
108                               const Standard_Real Value)
109 {
110   Standard_OutOfRange_Raise_if
111     (Row < 1 || Row > 3 || Col < 1 || Col > 3, " ");
112   matrix[Row-1][Col-1] = Value;
113 }
114
115 inline Standard_Real gp_Mat::Determinant () const
116 {
117   return
118     Mat00 * (Mat11 * Mat22 - Mat21 * Mat12) -
119       Mat01 * (Mat10 * Mat22 - Mat20 * Mat12) +
120         Mat02 * (Mat10 * Mat21 - Mat20 * Mat11);
121 }
122
123 inline const Standard_Real& gp_Mat::Value (const Standard_Integer Row, 
124                                            const Standard_Integer Col) const
125 {
126   Standard_OutOfRange_Raise_if
127     (Row < 1 || Row > 3 || Col < 1 || Col > 3, " ");
128   return matrix[Row-1][Col-1];
129 }
130
131 inline Standard_Real& gp_Mat::ChangeValue (const Standard_Integer Row, 
132                                            const Standard_Integer Col)
133 {
134   Standard_OutOfRange_Raise_if
135     (Row < 1 || Row > 3 || Col < 1 || Col > 3, " ");
136   return matrix[Row-1][Col-1];
137 }
138
139 inline Standard_Boolean gp_Mat::IsSingular () const
140 {
141   // Pour etre sur que Gauss va fonctionner, il faut faire Gauss ...
142   Standard_Real val = Determinant();
143   if (val < 0) val = - val;
144   return val <= gp::Resolution();
145 }
146
147 inline void gp_Mat::Add (const gp_Mat& Other)
148 {
149   Mat00 = Mat00 + Oat00;
150   Mat01 = Mat01 + Oat01;
151   Mat02 = Mat02 + Oat02;
152   Mat10 = Mat10 + Oat10;
153   Mat11 = Mat11 + Oat11;
154   Mat12 = Mat12 + Oat12;
155   Mat20 = Mat20 + Oat20;
156   Mat21 = Mat21 + Oat21;
157   Mat22 = Mat22 + Oat22;
158 }
159
160 inline gp_Mat gp_Mat::Added (const gp_Mat& Other) const
161 {
162   gp_Mat NewMat;
163   Nat00 = Mat00 + Oat00;
164   Nat01 = Mat01 + Oat01;
165   Nat02 = Mat02 + Oat02;
166   Nat10 = Mat10 + Oat10;
167   Nat11 = Mat11 + Oat11;
168   Nat12 = Mat12 + Oat12;
169   Nat20 = Mat20 + Oat20;
170   Nat21 = Mat21 + Oat21;
171   Nat22 = Mat22 + Oat22;
172   return NewMat;
173 }
174
175 inline void gp_Mat::Divide (const Standard_Real Scalar)
176 {
177   Standard_Real val = Scalar;
178   if (val < 0) val = - val;
179   Standard_ConstructionError_Raise_if
180       (val <= gp::Resolution(),"gp_Mat : Divide by 0");
181   Standard_Real UnSurScalar = 1.0 / Scalar;
182   Mat00 *= UnSurScalar;
183   Mat01 *= UnSurScalar; 
184   Mat02 *= UnSurScalar; 
185   Mat10 *= UnSurScalar; 
186   Mat11 *= UnSurScalar; 
187   Mat12 *= UnSurScalar; 
188   Mat20 *= UnSurScalar; 
189   Mat21 *= UnSurScalar; 
190   Mat22 *= UnSurScalar; 
191 }
192
193 inline gp_Mat gp_Mat::Divided (const Standard_Real Scalar) const
194 {
195   Standard_Real val = Scalar;
196   if (val < 0) val = - val;
197   Standard_ConstructionError_Raise_if
198       (val <= gp::Resolution(),"gp_Mat : Divide by 0");
199   gp_Mat NewMat;
200   Standard_Real UnSurScalar = 1.0 / Scalar;
201   Nat00 = Mat00 * UnSurScalar;
202   Nat01 = Mat01 * UnSurScalar; 
203   Nat02 = Mat02 * UnSurScalar; 
204   Nat10 = Mat10 * UnSurScalar; 
205   Nat11 = Mat11 * UnSurScalar; 
206   Nat12 = Mat12 * UnSurScalar; 
207   Nat20 = Mat20 * UnSurScalar; 
208   Nat21 = Mat21 * UnSurScalar; 
209   Nat22 = Mat22 * UnSurScalar; 
210   return NewMat;
211 }
212
213 inline gp_Mat gp_Mat::Multiplied (const gp_Mat& Other) const
214 {
215   gp_Mat NewMat = *this;
216   NewMat.Multiply(Other);
217   return NewMat;
218 }
219
220 inline void gp_Mat::Multiply (const gp_Mat& Other)
221 {
222   Standard_Real T00,T01,T02,T10,T11,T12,T20,T21,T22;
223   T00 = Mat00 * Oat00 + Mat01 * Oat10 + Mat02 * Oat20;
224   T01 = Mat00 * Oat01 + Mat01 * Oat11 + Mat02 * Oat21;
225   T02 = Mat00 * Oat02 + Mat01 * Oat12 + Mat02 * Oat22;
226   T10 = Mat10 * Oat00 + Mat11 * Oat10 + Mat12 * Oat20;
227   T11 = Mat10 * Oat01 + Mat11 * Oat11 + Mat12 * Oat21;
228   T12 = Mat10 * Oat02 + Mat11 * Oat12 + Mat12 * Oat22;
229   T20 = Mat20 * Oat00 + Mat21 * Oat10 + Mat22 * Oat20;
230   T21 = Mat20 * Oat01 + Mat21 * Oat11 + Mat22 * Oat21;
231   T22 = Mat20 * Oat02 + Mat21 * Oat12 + Mat22 * Oat22;
232   Mat00 = T00; 
233   Mat01 = T01; 
234   Mat02 = T02; 
235   Mat10 = T10; 
236   Mat11 = T11; 
237   Mat12 = T12; 
238   Mat20 = T20; 
239   Mat21 = T21; 
240   Mat22 = T22; 
241 }
242
243 inline void gp_Mat::PreMultiply (const gp_Mat& Other)
244 {
245   Standard_Real T00,T01,T02,T10,T11,T12,T20,T21,T22;
246   T00 = Oat00 * Mat00 + Oat01 * Mat10 + Oat02 * Mat20;
247   T01 = Oat00 * Mat01 + Oat01 * Mat11 + Oat02 * Mat21;
248   T02 = Oat00 * Mat02 + Oat01 * Mat12 + Oat02 * Mat22;    
249   T10 = Oat10 * Mat00 + Oat11 * Mat10 + Oat12 * Mat20;
250   T11 = Oat10 * Mat01 + Oat11 * Mat11 + Oat12 * Mat21;
251   T12 = Oat10 * Mat02 + Oat11 * Mat12 + Oat12 * Mat22;    
252   T20 = Oat20 * Mat00 + Oat21 * Mat10 + Oat22 * Mat20;
253   T21 = Oat20 * Mat01 + Oat21 * Mat11 + Oat22 * Mat21;
254   T22 = Oat20 * Mat02 + Oat21 * Mat12 + Oat22 * Mat22;    
255   Mat00 = T00; 
256   Mat01 = T01; 
257   Mat02 = T02; 
258   Mat10 = T10; 
259   Mat11 = T11; 
260   Mat12 = T12; 
261   Mat20 = T20; 
262   Mat21 = T21; 
263   Mat22 = T22; 
264 }
265
266 inline gp_Mat gp_Mat::Multiplied (const Standard_Real Scalar) const
267 {
268   gp_Mat NewMat;
269   Nat00 = Scalar * Mat00;
270   Nat01 = Scalar * Mat01;
271   Nat02 = Scalar * Mat02;
272   Nat10 = Scalar * Mat10;
273   Nat11 = Scalar * Mat11;
274   Nat12 = Scalar * Mat12;
275   Nat20 = Scalar * Mat20;
276   Nat21 = Scalar * Mat21;
277   Nat22 = Scalar * Mat22;
278   return NewMat;
279 }
280
281 inline void gp_Mat::Multiply (const Standard_Real Scalar)
282 {
283   Mat00 *= Scalar; 
284   Mat01 *= Scalar; 
285   Mat02 *= Scalar; 
286   Mat10 *= Scalar; 
287   Mat11 *= Scalar; 
288   Mat12 *= Scalar; 
289   Mat20 *= Scalar; 
290   Mat21 *= Scalar; 
291   Mat22 *= Scalar; 
292 }
293
294 inline gp_Mat gp_Mat::Powered (const Standard_Integer N) const
295 {
296    gp_Mat MatN = *this;
297    MatN.Power (N);
298    return MatN;
299 }
300
301 inline void gp_Mat::Subtract (const gp_Mat& Other)
302 {
303   Mat00 -= Oat00;
304   Mat01 -= Oat01;
305   Mat02 -= Oat02;
306   Mat10 -= Oat10;
307   Mat11 -= Oat11;
308   Mat12 -= Oat12;
309   Mat20 -= Oat20;
310   Mat21 -= Oat21;
311   Mat22 -= Oat22;
312 }
313
314 inline gp_Mat gp_Mat::Subtracted (const gp_Mat& Other) const
315 {
316   gp_Mat NewMat;
317   Nat00 = Mat00 - Oat00;
318   Nat01 = Mat01 - Oat01;
319   Nat02 = Mat02 - Oat02;
320   Nat10 = Mat10 - Oat10;
321   Nat11 = Mat11 - Oat11;
322   Nat12 = Mat12 - Oat12;
323   Nat20 = Mat20 - Oat20;
324   Nat21 = Mat21 - Oat21;
325   Nat22 = Mat22 - Oat22;
326   return NewMat;
327 }
328
329 inline void gp_Mat::Transpose ()
330 {
331   Standard_Real Temp;
332   Temp   = Mat01;
333   Mat01  = Mat10;
334   Mat10  = Temp;
335   Temp   = Mat02;
336   Mat02  = Mat20;
337   Mat20  = Temp;
338   Temp   = Mat12;
339   Mat12  = Mat21;
340   Mat21  = Temp;
341 }
342
343 inline gp_Mat gp_Mat::Transposed () const
344 {
345   gp_Mat NewMat = *this;
346   NewMat.Transpose();
347   return NewMat; 
348 }
349
350 inline gp_Mat operator* (const Standard_Real Scalar, const gp_Mat& Mat3D)
351 { return Mat3D.Multiplied (Scalar); }
352