1 // Copyright (c) 1995-1999 Matra Datavision
2 // Copyright (c) 1999-2014 OPEN CASCADE SAS
4 // This file is part of Open CASCADE Technology software library.
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.
12 // Alternatively, this file may be used under the terms of Open CASCADE
13 // commercial license or contractual agreement.
15 // 10/09/97 : PMN : Correction BUC40192 (pb avec les matrices negatives)
18 #define No_Standard_OutOfRange
19 #define No_Standard_ConstructionError
23 #include <gp_GTrsf.hxx>
25 #include <gp_Trsf.hxx>
27 #include <Standard_ConstructionError.hxx>
28 #include <Standard_OutOfRange.hxx>
29 #include <Standard_Dump.hxx>
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]
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]
51 gp_Mat::gp_Mat (const gp_XYZ& Col1,
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();
61 void gp_Mat::SetCol (const Standard_Integer Col,
62 const gp_XYZ& Value) {
64 Standard_OutOfRange_Raise_if (Col < 1 || Col > 3, " ");
65 const Standard_Address M = (Standard_Address)&(matrix[0][0]);
67 M00 = Value.X(); M10 = Value.Y(); M20 = Value.Z();
70 M01 = Value.X(); M11 = Value.Y(); M21 = Value.Z();
73 M02 = Value.X(); M12 = Value.Y(); M22 = Value.Z();
77 void gp_Mat::SetCols (const gp_XYZ& Col1,
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();
86 void gp_Mat::SetCross (const gp_XYZ& Ref)
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;
101 void gp_Mat::SetDot (const gp_XYZ& Ref)
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();
118 void gp_Mat::SetRotation (const gp_XYZ& Axis,
119 const Standard_Real Ang)
121 // Rot = I + sin(Ang) * M + (1. - cos(Ang)) * M*M
122 // avec M . XYZ = Axis ^ XYZ
124 // const Standard_Address M = (Standard_Address)&(matrix[0][0]);
125 gp_XYZ V = Axis.Normalized();
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));
141 void gp_Mat::SetRow (const Standard_Integer Row,
144 Standard_OutOfRange_Raise_if (Row < 1 || Row > 3, " ");
145 const Standard_Address M = (Standard_Address)&(matrix[0][0]);
147 M00 = Value.X(); M01 = Value.Y(); M02 = Value.Z();
150 M10 = Value.X(); M11 = Value.Y(); M12 = Value.Z();
153 M20 = Value.X(); M21 = Value.Y(); M22 = Value.Z();
157 void gp_Mat::SetRows (const gp_XYZ& Row1,
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();
167 gp_XYZ gp_Mat::Column (const Standard_Integer Col) const
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);
176 gp_XYZ gp_Mat::Diagonal () const
178 const Standard_Address M = (Standard_Address)&(matrix[0][0]);
179 return gp_XYZ (M00, M11, M22);
182 gp_XYZ gp_Mat::Row (const Standard_Integer Row) const
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);
191 void gp_Mat::Invert ()
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]);
198 // calcul de la transposee de la commatrice
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");
226 gp_Mat gp_Mat::Inverted () const
229 const Standard_Address M = (Standard_Address)&( matrix[0][0]);
230 const Standard_Address N = (Standard_Address)&(NewMat.matrix[0][0]);
232 // calcul de la transposee de la commatrice
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");
248 NewMat.Multiply(det) ;
252 void gp_Mat::Power (const Standard_Integer N)
255 else if (N == 0) { SetIdentity() ; }
256 else if (N == -1) { Invert(); }
258 if (N < 0) { Invert(); }
259 Standard_Integer Npower = N;
260 if (Npower < 0) Npower = - Npower;
264 if (IsOdd(Npower)) Multiply (Temp);
265 if (Npower == 1) break;
266 Temp.Multiply (Temp);
272 //=======================================================================
273 //function : DumpJson
275 //=======================================================================
276 void gp_Mat::DumpJson (Standard_OStream& theOStream, Standard_Integer) const
278 OCCT_DUMP_VECTOR_CLASS (theOStream, "gp_Mat", 9, Mat00, Mat01, Mat02, Mat10, Mat11, Mat12, Mat20, Mat21, Mat22)