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