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