7fd59977 |
1 | #include <GProp_GProps.ixx> |
2 | #include <GProp.hxx> |
3 | #include <math_Jacobi.hxx> |
4 | #include <gp.hxx> |
5 | #include <gp.hxx> |
6 | #include <gp_XYZ.hxx> |
7 | #include <gp_Vec.hxx> |
8 | |
9 | |
10 | GProp_GProps::GProp_GProps () : g (gp::Origin()) , loc (gp::Origin()), dim (0.0) |
11 | { |
12 | inertia = gp_Mat(0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0); |
13 | } |
14 | |
15 | |
16 | GProp_GProps::GProp_GProps (const gp_Pnt& SystemLocation) : |
17 | g (gp::Origin()), loc (SystemLocation), dim (0.0) |
18 | { |
19 | inertia = gp_Mat(0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0); |
20 | } |
21 | |
22 | |
23 | |
24 | void GProp_GProps::Add (const GProp_GProps& Item, const Standard_Real Density) { |
25 | if (Density <= gp::Resolution()) Standard_DomainError::Raise(); |
26 | if (loc.Distance (Item.loc) <= gp::Resolution ()) { |
27 | gp_XYZ GXYZ = (Item.g.XYZ()).Multiplied (Item.dim * Density); |
28 | g.SetXYZ (g.XYZ().Multiplied (dim)); |
29 | GXYZ.Add (g.XYZ()); |
30 | dim = dim + Item.dim * Density; |
31 | if (Abs(dim) >= 1.e-20 ) { |
32 | GXYZ.Divide (dim); |
33 | g.SetXYZ (GXYZ); |
34 | } |
35 | else { |
36 | g.SetCoord(0., 0., 0.); |
37 | } |
38 | inertia = inertia + Item.inertia * Density; |
39 | }else{ |
40 | gp_XYZ Itemloc = loc.XYZ() - Item.loc.XYZ(); |
41 | gp_XYZ Itemg = Item.loc.XYZ() + Item.g.XYZ(); |
42 | gp_XYZ GXYZ = Item.g.XYZ() - Itemloc; |
43 | GXYZ = GXYZ.Multiplied (Item.dim * Density); |
44 | g.SetXYZ (g.XYZ().Multiplied (dim)); |
45 | GXYZ.Add (g.XYZ()); |
46 | dim = dim + Item.dim * Density; |
47 | if (Abs(dim) >= 1.e-20 ) { |
48 | GXYZ.Divide (dim); |
49 | g.SetXYZ (GXYZ); |
50 | } |
51 | else { |
52 | g.SetCoord(0., 0., 0.); |
53 | } |
54 | //We have to compute the inertia of the Item at the location point |
55 | //of the system using the Huyghens theorem |
56 | gp_Mat HMat; |
57 | gp_Mat ItemInertia = Item.inertia; |
58 | if (Item.g.XYZ().Modulus() > gp::Resolution()) { |
59 | //Computes the inertia of Item at its dim centre |
60 | GProp::HOperator (Itemg, Item.loc, Item.dim, HMat); |
61 | ItemInertia = ItemInertia - HMat; |
62 | } |
63 | //Computes the inertia of Item at the location point of the system |
64 | GProp::HOperator (Itemg, loc, Item.dim, HMat); |
65 | ItemInertia = ItemInertia + HMat; |
66 | inertia = inertia + ItemInertia * Density; |
67 | } |
68 | } |
69 | |
70 | Standard_Real GProp_GProps::Mass () const { return dim; } |
71 | |
72 | gp_Pnt GProp_GProps::CentreOfMass () const |
73 | { |
74 | return gp_Pnt(loc.XYZ() + g.XYZ()); |
75 | } |
76 | |
77 | gp_Mat GProp_GProps::MatrixOfInertia () const { |
78 | gp_Mat HMat; |
79 | GProp::HOperator (g,gp::Origin(),dim, HMat); |
80 | return inertia - HMat; |
81 | } |
82 | |
83 | |
84 | |
85 | void GProp_GProps::StaticMoments (Standard_Real& Ix, |
86 | Standard_Real& Iy, |
87 | Standard_Real& Iz) const { |
88 | |
89 | gp_XYZ G = loc.XYZ() + g.XYZ(); |
90 | Ix = G.X() * dim; |
91 | Iy = G.Y() * dim; |
92 | Iz = G.Z() * dim; |
93 | } |
94 | |
95 | |
96 | |
97 | |
98 | Standard_Real GProp_GProps::MomentOfInertia (const gp_Ax1& A) const { |
99 | // Moment of inertia / axis A |
100 | // 1] computes the math_Matrix of inertia / A.location() |
101 | // 2] applies this math_Matrix to A.Direction() |
102 | // 3] then computes the scalar product between this vector and |
103 | // A.Direction() |
104 | |
105 | |
106 | if (loc.Distance (A.Location()) <= gp::Resolution()) { |
107 | return (A.Direction().XYZ()).Dot ( |
108 | (A.Direction().XYZ()).Multiplied (inertia)); |
109 | } |
110 | else { |
111 | gp_Mat HMat; |
112 | gp_Mat AxisInertia = MatrixOfInertia(); |
113 | GProp::HOperator (gp_Pnt (loc.XYZ() + g.XYZ()), A.Location(), dim, HMat); |
114 | AxisInertia = AxisInertia + HMat; |
115 | return (A.Direction().XYZ()).Dot ( |
116 | (A.Direction().XYZ()).Multiplied (AxisInertia)); |
117 | } |
118 | } |
119 | |
120 | |
121 | |
122 | Standard_Real GProp_GProps::RadiusOfGyration (const gp_Ax1& A) const { |
123 | |
124 | return Sqrt (MomentOfInertia (A) / dim); |
125 | } |
126 | |
127 | |
128 | |
129 | GProp_PrincipalProps GProp_GProps::PrincipalProperties () const { |
130 | |
131 | math_Matrix DiagMat (1, 3, 1, 3); |
132 | Standard_Integer i, j; |
133 | gp_Mat AxisInertia = MatrixOfInertia(); |
134 | for (j = 1; j <= 3; j++) { |
135 | for (i = 1; i <= 3; i++) { |
136 | DiagMat (i, j) = AxisInertia.Value (i, j); |
137 | } |
138 | } |
139 | math_Jacobi J (DiagMat); |
140 | Standard_Real Ixx = J.Value (1); |
141 | Standard_Real Iyy = J.Value (2); |
142 | Standard_Real Izz = J.Value (3); |
143 | DiagMat = J.Vectors (); |
144 | gp_Vec Vxx (DiagMat (1,1), DiagMat (2, 1), DiagMat (3, 1)); |
145 | gp_Vec Vyy (DiagMat (1,2), DiagMat (2, 2), DiagMat (3, 2)); |
146 | gp_Vec Vzz (DiagMat (1,3), DiagMat (2, 3), DiagMat (3, 3)); |
147 | // |
148 | // protection contre dim == 0.0e0 au cas ou on aurait rentre qu'un point |
149 | // |
150 | Standard_Real Rxx = 0.0e0 ; |
151 | Standard_Real Ryy = 0.0e0 ; |
152 | Standard_Real Rzz = 0.0e0 ; |
153 | if (0.0e0 != dim) { |
154 | Rxx = Sqrt (Abs(Ixx / dim)); |
155 | Ryy = Sqrt (Abs(Iyy / dim)); |
156 | Rzz = Sqrt (Abs(Izz / dim)); |
157 | } |
158 | return GProp_PrincipalProps (Ixx, Iyy, Izz, Rxx, Ryy, Rzz, Vxx, Vyy, Vzz, |
159 | gp_Pnt(g.XYZ() + loc.XYZ())); |
160 | } |
161 | |
162 | |
163 | |
164 | |
165 | |
166 | |
167 | |
168 | |
169 | |
170 | |
171 | |
172 | |
173 | |
174 | |
175 | |
176 | |
177 | |
178 | |