1 #include "SiconosException.hpp"
2 #include "SiconosConfig.h"
3 #include "cadmbtb.hpp"
4
5 #include "OccContactFace.hpp"
6 #include "OccContactEdge.hpp"
7
8 #include <TopoDS.hxx>
9 #include <TopExp_Explorer.hxx>
10 #include <BRepTools.hxx>
11 #include <BRep_Builder.hxx>
12 #include <gp_Dir.hxx>
13 #include <BRepAdaptor_Surface.hxx>
14 #include <BRepAdaptor_Curve.hxx>
15 #include <BRep_Tool.hxx>
16 #include <GeomLProp_SLProps.hxx>
17 #include <TopLoc_Location.hxx>
18
19 #include <gp_Vec.hxx>
20 #include <gp_Quaternion.hxx>
21
22 #include <iostream>
23 //#define DEBUG_MESSAGES 1
24 #include "siconos_debug.h"
25
cadmbtb_FacePoint(const TopoDS_Face & face,Standard_Real u,Standard_Real v)26 gp_Pnt cadmbtb_FacePoint(const TopoDS_Face &face,Standard_Real u, Standard_Real v)
27 {
28 // get bounds of face
29 /* Standard_Real umin, umax, vmin, vmax;
30 BRepTools::UVBounds(face, umin, umax, vmin, vmax); // create surface
31 if (u < umin) u = umin;
32 if (v < vmin) v = vmin;
33 if (u > umax) u = umax;
34 if (v > vmax) v = vmax;*/
35
36 BRepAdaptor_Surface SF(face);
37 // gp_Vec VecU,VecV;
38 gp_Pnt aPaux;
39 SF.D0(u,v,aPaux);//,VecU,VecV);// compute point on surface
40 return aPaux;
41 }
42
cadmbtb_EdgePoint(const TopoDS_Edge & edge,Standard_Real u)43 gp_Pnt cadmbtb_EdgePoint(const TopoDS_Edge &edge,Standard_Real u)
44 {
45 // get bounds of face
46 //Standard_Real umin, umax, vmin, vmax;
47 //BRepTools::UVBounds(face, umin, umax, vmin, vmax); // create surface
48 //if (u < umin) u = umin;
49 //if (v < vmin) v = vmin;
50 //if (u > umax) u = umax;
51 //if (v > vmax) v = vmax;
52
53 BRepAdaptor_Curve SC(edge);
54 // gp_Vec VecU,VecV;
55 gp_Pnt aPaux;
56 SC.D0(u,aPaux);
57
58 return aPaux;
59 }
60
61
cadmbtb_FaceNormal(const TopoDS_Face & face,Standard_Real u,Standard_Real v)62 gp_Dir cadmbtb_FaceNormal(const TopoDS_Face &face,Standard_Real u, Standard_Real v)
63 {
64 // get bounds of face
65 // Standard_Real umin, umax, vmin, vmax;
66 // BRepTools::UVBounds(face, umin, umax, vmin, vmax);
67
68 // create surface
69 Handle(Geom_Surface) surf=BRep_Tool::Surface(face);
70
71 // get surface properties
72 GeomLProp_SLProps props(surf, u, v, 1, 0.01);
73
74 // get surface normal
75 gp_Dir norm=props.Normal();
76
77 // check orientation
78 // if(face.Orientation()==TopAbs_REVERSED) norm.Reverse();
79 return norm;
80
81 }
82 #ifdef HAS_FORTRAN
83 extern "C"
84 {
85 void n2qn1_(int* n, double* x, double* f, double* g, double* dxmin, double* df1, double* epsabs, int* imp,
86 int *io,int* mode, int* iter, int * nsim, double* binf, double* bsup, int* iz, double* rz, int * reverse);
87 }
88 #endif
89 void cadmbtb_myf_FaceFace(double *x, double * fx, double * gx,const TopoDS_Face& face1,const TopoDS_Face& face2);
cadmbtb_myf_FaceFace(double * x,double * fx,double * gx,const TopoDS_Face & face1,const TopoDS_Face & face2)90 void cadmbtb_myf_FaceFace(double *x, double * fx, double * gx,const TopoDS_Face& face1,const TopoDS_Face& face2)
91 {
92 gp_Pnt aP1;
93 gp_Pnt aP2;
94 gp_Vec aVP2P1;
95 gp_Vec aV1u;
96 gp_Vec aV1v;
97 gp_Vec aV2u;
98 gp_Vec aV2v;
99 BRepAdaptor_Surface SF1(face1);
100 BRepAdaptor_Surface SF2(face2);
101
102 SF1.D1(x[0],x[1],aP1, aV1u, aV1v);
103 SF2.D1(x[2],x[3],aP2, aV2u, aV2v);
104
105 aVP2P1.SetX(aP1.X()-aP2.X());
106 aVP2P1.SetY(aP1.Y()-aP2.Y());
107 aVP2P1.SetZ(aP1.Z()-aP2.Z());
108 *fx = aVP2P1.X()*aVP2P1.X()+aVP2P1.Y()*aVP2P1.Y()+aVP2P1.Z()*aVP2P1.Z();
109 DEBUG_PRINTF("myf %e %e %e %e --> %e\n",x[0],x[1],x[2],x[3],*fx);
110 gx[0]=2*aV1u.Dot(aVP2P1);
111 gx[1]=2*aV1v.Dot(aVP2P1);
112 gx[2]=-2*aV2u.Dot(aVP2P1);
113 gx[3]=-2*aV2v.Dot(aVP2P1);
114
115 }
116
117 void cadmbtb_myf_FaceEdge(double *x, double * fx, double * gx,const TopoDS_Face& face1,const TopoDS_Edge& edge2);
cadmbtb_myf_FaceEdge(double * x,double * fx,double * gx,const TopoDS_Face & face1,const TopoDS_Edge & edge2)118 void cadmbtb_myf_FaceEdge(double *x, double * fx, double * gx,const TopoDS_Face& face1,const TopoDS_Edge& edge2)
119 {
120
121 gp_Pnt aP1;
122 gp_Pnt aP2;
123 gp_Vec aVP2P1;
124 gp_Vec aV1u;
125 gp_Vec aV1v;
126 gp_Vec aV2u;
127 // gp_Vec aV2v;/*here, zero*/
128 BRepAdaptor_Surface SF1(face1);
129 BRepAdaptor_Curve SC(edge2);
130
131 SF1.D1(x[0],x[1],aP1, aV1u, aV1v);
132 SC.D1(x[2],aP2, aV2u);
133
134 aVP2P1.SetX(aP1.X()-aP2.X());
135 aVP2P1.SetY(aP1.Y()-aP2.Y());
136 aVP2P1.SetZ(aP1.Z()-aP2.Z());
137 *fx = aVP2P1.X()*aVP2P1.X()+aVP2P1.Y()*aVP2P1.Y()+aVP2P1.Z()*aVP2P1.Z();
138 DEBUG_PRINTF("myf %e %e %e %e --> %e\n",x[0],x[1],x[2],x[3],*fx);
139 gx[0]=2*aV1u.Dot(aVP2P1);
140 gx[1]=2*aV1v.Dot(aVP2P1);
141 gx[2]=-2*aV2u.Dot(aVP2P1);
142 // gx[3]=-2*aV2v.Dot(aVP2P1);
143
144 }
145
146
147
148 // adapted from _CADMBTB_getMinDistanceFace*_using_n2qn1 (Olivier Bonnefon)
cadmbtb_distanceFaceFace(const OccContactFace & csh1,const OccContactFace & csh2,Standard_Real & X1,Standard_Real & Y1,Standard_Real & Z1,Standard_Real & X2,Standard_Real & Y2,Standard_Real & Z2,Standard_Real & nX,Standard_Real & nY,Standard_Real & nZ,Standard_Real & MinDist)149 void cadmbtb_distanceFaceFace(const OccContactFace& csh1,
150 const OccContactFace& csh2,
151 Standard_Real& X1, Standard_Real& Y1, Standard_Real& Z1,
152 Standard_Real& X2, Standard_Real& Y2, Standard_Real& Z2,
153 Standard_Real& nX, Standard_Real& nY, Standard_Real& nZ,
154 Standard_Real& MinDist)
155 {
156 // need the 2 sp pointers to keep memory
157 SPC::TopoDS_Face pface1 = csh1.contact();
158 SPC::TopoDS_Face pface2 = csh2.contact();
159
160 const TopoDS_Face& face1 = *pface1;
161 const TopoDS_Face& face2 = *pface2;
162
163 double x[4];
164 double f = 0;
165 double g[4];
166 double dxim[4];
167 double df1 =0;
168 double epsabs=0;
169 double binf[4];
170 double bsup[4];
171 double rz[4*(4+9)/2 + 1];
172 int iz[2*4+1];
173
174 binf[0] = csh1.binf1[0];
175 binf[1] = csh1.binf1[1];
176 bsup[0] = csh1.bsup1[0];
177 bsup[1] = csh1.bsup1[1];
178
179 binf[2] = csh2.binf1[0];
180 binf[3] = csh2.binf1[1];
181 bsup[2] = csh2.bsup1[0];
182 bsup[3] = csh2.bsup1[1];
183
184 dxim[0]=1e-6*(bsup[0]-binf[0]);
185 dxim[1]=1e-6*(bsup[1]-binf[1]);
186 dxim[2]=1e-6*(bsup[2]-binf[2]);
187 dxim[3]=1e-6*(bsup[3]-binf[3]);
188
189 x[0]=(binf[0]+bsup[0])*0.5;
190 x[1]=(binf[1]+bsup[1])*0.5;
191 x[2]=(binf[2]+bsup[2])*0.5;
192 x[3]=(binf[3]+bsup[3])*0.5;
193
194 cadmbtb_myf_FaceFace(x,&f,g,face1,face2);
195
196 df1=f;
197
198
199 int mode =1;
200 int imp=0;
201 int io=16;
202 int iter=500;
203 int nsim=3*iter;
204 int reverse=1;
205
206 int n = 4;
207
208 // DEBUG_PRINTF("call n2qn1_: n=%d,x[0]=%e,x[1]=%e,x[2]=%e,x[3]=%e,fx=%e \n g[0]=%e,g[1]=%e,g[2]=%e,g[3]=%e \n dxim[0]=%e,dxim[1]=%e,dxim[2]=%e,dxim[3]=%e,epsabs=%e,imp=%d,io=%d,mode=%d,iter=%d,nsim=%d \n binf[0]=%e,binf[1]=%e,binf[2]=%e,binf[3]=%e \n bsup[0]=%e,bsup[1]=%e,bsup[2]=%e,bsup[3]=%e \n sizeD=%d,sizeI=%d\n",n,x[0],x[1],x[2],x[3],f,g[0],g[1],g[2],g[3],dxim[0],dxim[1],dxim[2],dxim[3],epsabs,imp,io,mode,iter,nsim,binf[0],binf[1],binf[2],binf[3],bsup[0],bsup[1],bsup[2],bsup[3],sizeD,sizeI);
209 #ifdef HAS_FORTRAN
210 n2qn1_(&n, x, &f, g, dxim, &df1, &epsabs, &imp, &io,&mode, &iter, &nsim, binf, bsup, iz, rz, &reverse);
211 #else
212 THROW_EXCEPTION("_CADMBTB_getMinDistanceFaceFace_using_n2qn1, Fortran Language is not enabled in siconos mechanisms. Compile with fortran if you need n2qn1");
213 #endif
214 while(mode > 7)
215 {
216 cadmbtb_myf_FaceFace(x,&f,g,face1,face2);
217 #ifdef HAS_FORTRAN
218 n2qn1_(&n, x, &f, g, dxim, &df1, &epsabs, &imp, &io,&mode, &iter, &nsim, binf, bsup, iz, rz, &reverse);
219 #else
220 THROW_EXCEPTION("_CADMBTB_getMinDistanceFaceFace_using_n2qn1, Fortran Language is not enabled in siconos mechanisms. Compile with fortran if you need n2qn1");
221 #endif
222 }
223
224 DEBUG_PRINTF("mode=%d and min value at u=%e,v=%e f=%e\n",mode,x[0],x[1],sqrt(f));
225 DEBUG_PRINTF("_CADMBTB_getMinDistanceFaceFace_using_n2qn1 dist = %e\n",sqrt(f));
226
227 MinDist=sqrt(f);
228
229 gp_Dir normal = cadmbtb_FaceNormal(face2,x[2],x[3]);
230 normal.Coord(nX,nY,nZ);
231
232 /**check orientation of normal from face 2**/
233 gp_Pnt aPaux1 = cadmbtb_FacePoint(face1,x[0],x[1]);
234 aPaux1.Coord(X1, Y1, Z1);
235 gp_Pnt aPaux2 = cadmbtb_FacePoint(face2,x[2],x[3]);
236 aPaux2.Coord(X2, Y2, Z2);
237 if(((X1-X2)*nX+(Y1-Y2)*nY+(Z1-Z2)*nZ)<0)
238 {
239 normal.Reverse();
240 }
241 normal.Coord(nX,nY,nZ);
242
243 }
244
245
246
247 /*idContact useful for the memory management of n2qn1.*/
cadmbtb_distanceFaceEdge(const OccContactFace & csh1,const OccContactEdge & csh2,Standard_Real & X1,Standard_Real & Y1,Standard_Real & Z1,Standard_Real & X2,Standard_Real & Y2,Standard_Real & Z2,Standard_Real & nX,Standard_Real & nY,Standard_Real & nZ,Standard_Real & MinDist)248 void cadmbtb_distanceFaceEdge(
249 const OccContactFace& csh1, const OccContactEdge& csh2,
250 Standard_Real& X1, Standard_Real& Y1, Standard_Real& Z1,
251 Standard_Real& X2, Standard_Real& Y2, Standard_Real& Z2,
252 Standard_Real& nX, Standard_Real& nY, Standard_Real& nZ,
253 Standard_Real& MinDist)
254 {
255
256 // need the 2 sp pointers to keep memory
257 SPC::TopoDS_Face pface1 = csh1.contact();
258 SPC::TopoDS_Edge pedge2 = csh2.contact();
259
260 const TopoDS_Face& face1 = *pface1;
261 const TopoDS_Edge& edge2 = *pedge2;
262
263 int n = 3;
264 double x[3];
265 double f = 0;
266 double g[3];
267 double dxim[3];
268 double df1 =0;
269 double epsabs=0;
270 double binf[3];
271 double bsup[3];
272 double rz[4*(4+9)/2 + 1];
273 int iz[2*4+1];
274
275 binf[0] = csh1.binf1[0];
276 binf[1] = csh1.binf1[1];
277 bsup[0] = csh1.bsup1[0];
278 bsup[1] = csh1.bsup1[1];
279
280 binf[2] = csh2.binf1[0];
281 bsup[2] = csh2.bsup1[0];
282
283 dxim[0]=1e-6*(bsup[0]-binf[0]);
284 dxim[1]=1e-6*(bsup[1]-binf[1]);
285 dxim[2]=1e-6*(bsup[2]-binf[2]);
286
287 x[0]=(binf[0]+bsup[0])*0.5;
288 x[1]=(binf[1]+bsup[1])*0.5;
289 x[2]=(binf[2]+bsup[2])*0.5;
290 cadmbtb_myf_FaceEdge(x,&f,g,face1,edge2);
291
292 df1=f;
293
294 int mode =1;
295 int imp=0;
296 int io=16;
297 int iter=500;
298 int nsim=3*iter;
299 int reverse=1;
300
301 // DEBUG_PRINTF("call n2qn1_: n=%d,x[0]=%e,x[1]=%e,x[2]=%e,fx=%e \n g[0]=%e,g[1]=%e,g[2]=%e \n dxim[0]=%e,dxim[1]=%e,dxim[2]=%e,epsabs=%e,imp=%d,io=%d,mode=%d,iter=%d,nsim=%d \n binf[0]=%e,binf[1]=%e,binf[2]=%e \n bsup[0]=%e,bsup[1]=%e,bsup[2]=%e \n sizeD=%d,sizeI=%d\n",n,x[0],x[1],x[2],f,g[0],g[1],g[2],dxim[0],dxim[1],dxim[2],epsabs,imp,io,mode,iter,nsim,binf[0],binf[1],binf[2],bsup[0],bsup[1],bsup[2],sizeD,sizeI);
302
303 #ifdef HAS_FORTRAN
304 n2qn1_(&n, x, &f, g, dxim, &df1, &epsabs, &imp, &io,&mode, &iter, &nsim, binf, bsup, iz, rz, &reverse);
305 #else
306 THROW_EXCEPTION("_CADMBTB_getMinDistanceFaceFace_using_n2qn1, Fortran Language is not enabled in siconos mechanisms. Compile with fortran if you need n2qn1");
307 #endif
308 while(mode > 7)
309 {
310 cadmbtb_myf_FaceEdge(x,&f,g,face1,edge2);
311 #ifdef HAS_FORTRAN
312 n2qn1_(&n, x, &f, g, dxim, &df1, &epsabs, &imp, &io,&mode, &iter, &nsim, binf, bsup, iz, rz, &reverse);
313 #else
314 THROW_EXCEPTION("_CADMBTB_getMinDistanceFaceFace_using_n2qn1, Fortran Language is not enabled in siconos mechanisms. Compile with fortran if you need n2qn1");
315 #endif
316 }
317
318 MinDist=sqrt(f);
319
320 DEBUG_PRINTF("mode=%d and min value at u=%e,v=%e f=%e\n",mode,x[0],x[1],MinDist);
321 DEBUG_PRINTF("cadmbtb_getMinDistanceFaceEdge_using_n2qn1 dist = %e\n",MinDist);
322
323 /** V.A. Normal is always computed form the surface which is safer */
324 gp_Dir normal = cadmbtb_FaceNormal(face1,x[0],x[1]);
325 gp_Pnt aPaux = cadmbtb_FacePoint(face1,x[0],x[1]);
326
327 /** Coordinate of the contact point on the surface */
328 aPaux.Coord(X1, Y1, Z1);
329 normal.Coord(nX,nY,nZ);
330
331 /** Coordinate of the contact point on the edge */
332 aPaux = cadmbtb_EdgePoint(edge2,x[2]);
333 aPaux.Coord(X2, Y2, Z2);
334
335 /** check orientation of normal*/
336 if(((X1-X2)*nX+(Y1-Y2)*nY+(Z1-Z2)*nZ)>0)
337 {
338 normal.Reverse();
339 normal.Coord(nX,nY,nZ);
340 }
341
342 }
343