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