blob: 6be260b0f2c5726fc5e5cfc82aa8b15f3584c472 (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
|
// File gp_GTrsf.cxx JCV 14/12/90
#define No_Standard_OutOfRange
#include <gp_GTrsf.ixx>
#include <gp_Mat.hxx>
void gp_GTrsf::SetTranslationPart (const gp_XYZ& Coord)
{
loc = Coord;
if (Form() == gp_CompoundTrsf || Form() == gp_Other ||
Form() == gp_Translation) { }
else if (Form() == gp_Identity) { shape = gp_Translation; }
else { shape = gp_CompoundTrsf; }
}
void gp_GTrsf::Invert ()
{
if (shape == gp_Other) {
matrix.Invert() ;
loc.Multiply (matrix);
loc.Reverse();
}
else {
gp_Trsf T = Trsf();
T.Invert ();
SetTrsf (T);
}
}
void gp_GTrsf::Multiply (const gp_GTrsf& T)
{
if (Form() == gp_Other || T.Form() == gp_Other) {
shape = gp_Other;
loc.Add (T.loc.Multiplied (matrix));
matrix.Multiply(T.matrix);
}
else {
gp_Trsf T1 = Trsf();
gp_Trsf T2 = T.Trsf();
T1.Multiply(T2);
matrix = T1.matrix;
loc = T1.loc;
scale = T1.scale;
shape = T1.shape;
}
}
void gp_GTrsf::Power (const Standard_Integer N)
{
if (N == 0) {
scale = 1.;
shape = gp_Identity;
matrix.SetIdentity();
loc = gp_XYZ (0.,0.,0.);
}
else if (N == 1) { }
else if (N == -1) { Invert(); }
else {
if (shape == gp_Other) {
Standard_Integer Npower = N;
if (Npower < 0) Npower = - Npower;
Npower--;
gp_XYZ Temploc = loc;
// Standard_Real Tempscale = scale;
gp_Mat Tempmatrix (matrix);
while (1) {
if (IsOdd(Npower)) {
loc.Add (Temploc.Multiplied (matrix));
matrix.Multiply (Tempmatrix);
}
if (Npower == 1) { break; }
Temploc.Add (Temploc.Multiplied (Tempmatrix));
Tempmatrix.Multiply (Tempmatrix);
Npower = Npower/2;
}
}
else {
gp_Trsf T = Trsf ();
T.Power (N);
SetTrsf (T);
}
}
}
void gp_GTrsf::PreMultiply (const gp_GTrsf& T)
{
if (Form() == gp_Other || T.Form() == gp_Other) {
shape = gp_Other;
loc.Multiply (T.matrix);
loc.Add (T.loc);
matrix.PreMultiply(T.matrix);
}
else {
gp_Trsf T1 = Trsf();
gp_Trsf T2 = T.Trsf();
T1.PreMultiply(T2);
matrix = T1.matrix;
loc = T1.loc;
scale = T1.scale;
shape = T1.shape;
}
}
void gp_GTrsf::SetForm()
{
Standard_Real tol = 1.e-12; // Precision::Angular();
//
// don t trust the initial values !
//
gp_Mat M(matrix);
Standard_Real s = M.Determinant();
Standard_Real As = s;
if (As < 0) As = - As;
Standard_ConstructionError_Raise_if
(As < gp::Resolution(),"gp_GTrsf::SetForm, null determinant");
if (s > 0)
s = Pow(s,1./3.);
else
s = -Pow(-s,1./3.);
M.Divide(s);
// check if the matrix is an uniform matrix
// the transposition should be the invert.
gp_Mat TM(M);
TM.Transpose();
TM.Multiply(M);
gp_Mat anIdentity ;
anIdentity.SetIdentity() ;
TM.Subtract(anIdentity);
if (shape==gp_Other) shape = gp_CompoundTrsf;
Standard_Integer i, j;
for (i=1; i<=3; i++) {
for (j=1; j<=3; j++) {
As = TM.Value(i,j);
if (As < 0) As = - As;
if (As > tol) {
shape = gp_Other;
return;
}
}
}
}
|
