Open CASCADE学习|刚体（ TopoDS_Shape）按某种轨迹运动，停在指定位置上

今天实现如下功能：刚体做做螺旋运动，轨迹已知，求刚体在每个位置上的所占据的空间，就是把刚体从初始位置变换到该位置。

这里的刚体是一个砂轮截面，螺旋运动轨迹由B样条曲线拟合，通过Frenet标架确定运动轨迹上的局部坐标系，据此计算变换矩阵，将砂轮截面变换到指定位置。

目前可以实现平面刚体的运动，还无法实现三维刚体的运动。

1、起始位置空间的确定

#include <Geom_CylindricalSurface.hxx>
#include <gp_Ax3.hxx>
#include <GeomAPI_Interpolate.hxx>
#include <BRepAdaptor_Curve.hxx>
#include <BRepBuilderAPI_MakeEdge.hxx>
#include <Geom2d_TrimmedCurve.hxx>
#include <GCE2d_MakeSegment.hxx>#include <GeomAPI_PointsToBSpline.hxx>
#include <BRepBuilderAPI_MakeFace.hxx>
#include <GC_MakeCircle.hxx>
#include <BRepBuilderAPI_MakeWire.hxx>
#include <BRepOffsetAPI_MakePipe.hxx>
#include <GC_MakeArcOfCircle.hxx>
#include <BRepAlgoAPI_Fuse.hxx>#include <gp_GTrsf.hxx>
#include <BRepBuilderAPI_Transform.hxx>#include"Viewer.h"#include <BRepPrimAPI_MakeCylinder.hxx>
#include <BRepBuilderAPI_MakePolygon.hxx>
#include <BRep_Tool.hxx>
#include <TopoDS.hxx>
#include <BRepAlgoAPI_Cut.hxx>
#include <BRepAlgoAPI_Common.hxx>
#include <BRepAlgoAPI_Section.hxx>
#include <BRepPrimAPI_MakePrism.hxx>
#include <GC_MakeSegment.hxx>
#include <IntAna2d_AnaIntersection.hxx>
#include <ShapeUpgrade_UnifySameDomain.hxx>
#include <BRepTools_WireExplorer.hxx>
#include <GeomFill_TrihedronLaw.hxx>
#include <GeomFill_Frenet.hxx>
#include <GeomFill_CurveAndTrihedron.hxx>
#include <BRepFill_Edge3DLaw.hxx>
#include <BRepFill_SectionPlacement.hxx>
#include <ShapeUpgrade_RemoveLocations.hxx>TopoDS_Edge createHelix(const Standard_Real HelixRadius, const Standard_Real HelixAngle, const Standard_Real HelixLength)
{Standard_Real u0 = 0.0;Standard_Real u1 = 2 * M_PI;Standard_Real v0 = 0.0;Standard_Real v1 = HelixLength;double uInter = (u1 - u0) / 1000;double vInter = (v1 - v0) / 1000;TColgp_HArray1OfPnt Points(1, 1001);Handle(Geom_CylindricalSurface) aCylinder = new Geom_CylindricalSurface(gp::XOY(), HelixRadius);double u;double v;//生成点for (int i = 0; i < 1001; i++) {u = i * vInter * tan(HelixAngle) / HelixRadius;v = i * vInter;Points[i + 1] = aCylinder->Value(u, v);}GeomAPI_PointsToBSpline Approx(Points);Handle_Geom_BSplineCurve K = Approx.Curve();TopoDS_Edge aHelixEdge = BRepBuilderAPI_MakeEdge(K);return aHelixEdge;}
TopoDS_Shape createGrindingwheel()
{Standard_Real Line1_angle = 280 * M_PI / 180;Standard_Real Line1_length = 0.5031;Standard_Real Line2_angle = 236 * M_PI / 180;Standard_Real Line2_length = 0.5925;Standard_Real Arc1_r = 0.112;Standard_Real Arc1_angle = (180 + 10 + 50) * M_PI / 180;gp_Pnt Line1_p1(-0.6822 / 2, 0, 0);gp_Pnt Line2_p1(0.6822 / 2, 0, 0);gp_Lin Line1(Line1_p1, gp_Dir(cos(Line1_angle), sin(Line1_angle), 0.));gp_Lin Line2(Line2_p1, gp_Dir(cos(Line2_angle), sin(Line2_angle), 0.));Handle(Geom_TrimmedCurve) L1 = GC_MakeSegment(Line1, 0., Line1_length);TopoDS_Edge L1e = BRepBuilderAPI_MakeEdge(L1);Handle(Geom_TrimmedCurve) L2 = GC_MakeSegment(Line2, 0., Line2_length);TopoDS_Edge L2e = BRepBuilderAPI_MakeEdge(L2);gp_Pnt l1end = L1->EndPoint();gp_Pnt l2end = L2->EndPoint();gp_Lin Line1v(l1end, gp_Dir(cos(Line1_angle + M_PI_2), sin(Line1_angle + M_PI_2), 0.));gp_Lin2d Line2v(gp_Pnt2d(l2end.X(), l2end.Y()), gp_Dir2d(cos(Line2_angle - M_PI_2), sin(Line2_angle - M_PI_2)));gp_Lin Line2v3d(l2end, gp_Dir(cos(Line2_angle - M_PI_2), sin(Line2_angle - M_PI_2), 0.));Handle(Geom_TrimmedCurve) L1v = GC_MakeSegment(Line1v, 0., Arc1_r);gp_Pnt l1vend = L1v->EndPoint();gp_Circ c1(gp_Ax2(l1vend, gp_Dir(0, 0, 1)), Arc1_r);Handle(Geom_TrimmedCurve) c1c = GC_MakeArcOfCircle(c1, l1end, Arc1_angle, 1);gp_Pnt c1end = c1c->EndPoint();gp_Lin2d Line3(gp_Pnt2d(c1end.X(), c1end.Y()), gp_Dir2d(l2end.X() - c1end.X(), l2end.Y() - c1end.Y()));gp_Lin2d Line3v = Line3.Normal(gp_Pnt2d((l2end.X() + c1end.X()) / 2, (l2end.Y() + c1end.Y()) / 2));IntAna2d_AnaIntersection aIntAna;aIntAna.Perform(Line2v, Line3v);IntAna2d_IntPoint aIntPoint = aIntAna.Point(1);gp_Pnt o2(aIntPoint.Value().X(), aIntPoint.Value().Y(), 0.);Handle(Geom_TrimmedCurve) L2v = GC_MakeSegment(Line2v3d, l2end, o2);Standard_Real r2 = L2v->LastParameter();gp_Circ c2(gp_Ax2(o2, gp_Dir(0, 0, 1)), r2);Handle(Geom_TrimmedCurve) c2c = GC_MakeArcOfCircle(c2, c1end, l2end, 0);gp_Pnt c2low = c2c->Value(M_PI_2);TopoDS_Edge c1ce = BRepBuilderAPI_MakeEdge(c1c);TopoDS_Edge L1ev = BRepBuilderAPI_MakeEdge(L1v);TopoDS_Edge c2ce = BRepBuilderAPI_MakeEdge(c2c);TopoDS_Edge anEdge = BRepBuilderAPI_MakeEdge(Line1_p1, Line2_p1);TopTools_ListOfShape listEdge;listEdge.Append(anEdge);listEdge.Append(L1e);listEdge.Append(c1ce);listEdge.Append(c2ce);listEdge.Append(L2e);BRepBuilderAPI_MakeWire mw;mw.Add(listEdge);mw.Build();TopoDS_Face out = BRepBuilderAPI_MakeFace(mw);gp_Circ  cutcircle(gp_Ax2(gp_Pnt(0, 0, 0), gp_Dir(0, 0, 1)), 0.406 / 2);TopoDS_Edge cute = BRepBuilderAPI_MakeEdge(cutcircle);TopoDS_Wire cutw = BRepBuilderAPI_MakeWire(cute);TopoDS_Face cutf = BRepBuilderAPI_MakeFace(cutw);//平移：gp_Trsf theTransformation1;gp_Vec theVectorOfTranslation1(-c2low.X(), -c2low.Y(), 0.);theTransformation1.SetTranslation(theVectorOfTranslation1);BRepBuilderAPI_Transform myBRepTransformation1(out, theTransformation1);TopoDS_Shape outzero = myBRepTransformation1.Shape();TopoDS_Shape theCommonSurface = BRepAlgoAPI_Common(outzero, cutf);gp_Trsf theTransformation2;gp_Vec theVectorOfTranslation2(0., 0.125 / 2, 0.);theTransformation2.SetTranslation(theVectorOfTranslation2);//绕一个轴旋转：gp_Trsf theTransformation3;gp_Ax1 axez = gp_Ax1(gp_Pnt(0, 0, 0), gp_Dir(0., 0., 1.));theTransformation3.SetRotation(axez, -90 * M_PI / 180);gp_Trsf theTransformation4;gp_Ax1 axex = gp_Ax1(gp_Pnt(0, 0, 0), gp_Dir(1., 0., 0.));theTransformation4.SetRotation(axex, -50 * M_PI / 180);BRepBuilderAPI_Transform myBRepTransformation(theCommonSurface, theTransformation4 * theTransformation3 * theTransformation2);TopoDS_Shape TransformedShape = myBRepTransformation.Shape();return TransformedShape;
}
int main(int argc, char* argv[])
{gp_Dir  Z(0.0, 0.0, 1.0);gp_Pnt center(0, 0, 0.0);gp_Pnt xr(0.5, 0, 0.0);gp_Pnt yr(0.0, 1.0, 0.0);gp_Pnt zr(0.0, 0.0, 7.0);gp_Ax2  wb(center, Z);gp_Circ  wbcircle(wb, 0.125 / 2);TopoDS_Edge wbe = BRepBuilderAPI_MakeEdge(wbcircle);TopoDS_Edge xline = BRepBuilderAPI_MakeEdge(center, xr);TopoDS_Edge yline = BRepBuilderAPI_MakeEdge(center, yr);TopoDS_Edge zline = BRepBuilderAPI_MakeEdge(center, zr);//creat a profile of gringing wheelTopoDS_Shape gw = createGrindingwheel();//creat a cylinder surfaceHandle(Geom_CylindricalSurface) aCylinder = new Geom_CylindricalSurface(gp::XOY(), 0.306 / 2);TopoDS_Shape cF = BRepBuilderAPI_MakeFace(aCylinder->Cylinder(), 0, 2 * M_PI, 0, 3.);TopoDS_Solid cys = BRepPrimAPI_MakeCylinder(gp::XOY(), 0.306 / 2, 7);TopoDS_Edge aE = createHelix(0.306 / 2, M_PI / 4, 6.);TopoDS_Wire spine = BRepBuilderAPI_MakeWire(aE);//crate a sweep surfaceTopoDS_Shape pipe = BRepOffsetAPI_MakePipe(spine, gw, GeomFill_IsFrenet, 1);TopoDS_Wire mySpine;TopoDS_Shape myProfile;TopoDS_Shape myShape;gp_Trsf myTrsf;Handle(BRepFill_LocationLaw) myLoc;Handle(TopTools_HArray2OfShape) mySections;Handle(TopTools_HArray2OfShape) myFaces;Handle(TopTools_HArray2OfShape) myEdges;TopTools_MapOfShape myReversedEdges;BRepFill_DataMapOfShapeHArray2OfShape myTapes;BRepFill_DataMapOfShapeHArray2OfShape myRails;Standard_Integer myCurIndexOfSectionEdge;TopoDS_Shape myFirst;TopoDS_Shape myLast;TopTools_DataMapOfShapeListOfShape myGenMap;Standard_Integer myDegmax;Standard_Integer mySegmax;GeomAbs_Shape myContinuity;GeomFill_Trihedron myMode;Standard_Boolean myForceApproxC1;Standard_Real myErrorOnSurf;mySections.Nullify();myFaces.Nullify();myEdges.Nullify();mySpine = spine;myProfile = gw;TopoDS_Shape TheProf;Handle(GeomFill_TrihedronLaw) TLaw;TLaw = new GeomFill_Frenet();Handle(GeomFill_CurveAndTrihedron) Loc = new (GeomFill_CurveAndTrihedron) (TLaw);myLoc = new (BRepFill_Edge3DLaw) (mySpine, Loc);if (myLoc->NbLaw() == 0) {return 0; // Degenerated case}myLoc->TransformInG0Law(); // Set into continuityBRepFill_SectionPlacement Place(myLoc, gw);myTrsf = Place.Transformation();TopLoc_Location Loc2(myTrsf), Loc1;Loc1 = gw.Location();TopoDS_Shape aux;TheProf = myProfile;TheProf.Location(Loc2.Multiplied(Loc1));// Construct First && Last ShapeHandle(GeomFill_LocationLaw) law;gp_Mat M;gp_Vec V;gp_Trsf fila;Standard_Real first,last;myLoc->Law(1)->GetDomain(first, last);std::cout << "first=" <<first<< std::endl;std::cout << "last=" << last << std::endl;myLoc->Law(1)->D0(first, M, V);fila.SetValues(M(1, 1), M(1, 2), M(1, 3), V.X(),M(2, 1), M(2, 2), M(2, 3), V.Y(),M(3, 1), M(3, 2), M(3, 3), V.Z());fila.Multiply(myTrsf);TopLoc_Location LocFirst(fila);myFirst = myProfile;if (!LocFirst.IsIdentity()) {//myFirst.Location( LocFirst.Multiplied(myProfile.Location()) );myFirst = BRepBuilderAPI_Transform(myProfile, fila, Standard_True); //copy}ShapeUpgrade_RemoveLocations RemLoc;RemLoc.SetRemoveLevel(TopAbs_COMPOUND);RemLoc.Remove(myFirst);myFirst = RemLoc.GetResult();myLoc->Law(myLoc->NbLaw())->GetDomain(first, last);myLoc->Law(myLoc->NbLaw())->D0(last, M, V);//    try { // Not good, but there are no other means to test SetValuesfila.SetValues(M(1, 1), M(1, 2), M(1, 3), V.X(),M(2, 1), M(2, 2), M(2, 3), V.Y(),M(3, 1), M(3, 2), M(3, 3), V.Z());fila.Multiply(myTrsf);TopLoc_Location LocLast(fila);if (!myLoc->IsClosed() || LocFirst != LocLast) {myLast = myProfile;if (!LocLast.IsIdentity()) {//myLast.Location(LocLast.Multiplied(myProfile.Location()) );myLast = BRepBuilderAPI_Transform(myProfile, fila, Standard_True); //copy}}else {myLast = myFirst;}RemLoc.Remove(myLast);myLast = RemLoc.GetResult();Viewer vout(50, 50, 500, 500);vout << wbe;vout << xline;vout << yline;vout << zline;vout << myProfile;vout << myLast;vout << spine;vout.StartMessageLoop();return 0;
}

2、中间任一位置空间的确定

#include <Geom_CylindricalSurface.hxx>
#include <gp_Ax3.hxx>
#include <GeomAPI_Interpolate.hxx>
#include <BRepAdaptor_Curve.hxx>
#include <BRepBuilderAPI_MakeEdge.hxx>
#include <Geom2d_TrimmedCurve.hxx>
#include <GCE2d_MakeSegment.hxx>#include <GeomAPI_PointsToBSpline.hxx>
#include <BRepBuilderAPI_MakeFace.hxx>
#include <GC_MakeCircle.hxx>
#include <BRepBuilderAPI_MakeWire.hxx>
#include <BRepOffsetAPI_MakePipe.hxx>
#include <GC_MakeArcOfCircle.hxx>
#include <BRepAlgoAPI_Fuse.hxx>#include <gp_GTrsf.hxx>
#include <BRepBuilderAPI_Transform.hxx>#include"Viewer.h"#include <BRepPrimAPI_MakeCylinder.hxx>
#include <BRepBuilderAPI_MakePolygon.hxx>
#include <BRep_Tool.hxx>
#include <TopoDS.hxx>
#include <BRepAlgoAPI_Cut.hxx>
#include <BRepAlgoAPI_Common.hxx>
#include <BRepAlgoAPI_Section.hxx>
#include <BRepPrimAPI_MakePrism.hxx>
#include <GC_MakeSegment.hxx>
#include <IntAna2d_AnaIntersection.hxx>
#include <ShapeUpgrade_UnifySameDomain.hxx>
#include <BRepTools_WireExplorer.hxx>
#include <GeomFill_TrihedronLaw.hxx>
#include <GeomFill_Frenet.hxx>
#include <GeomFill_CurveAndTrihedron.hxx>
#include <BRepFill_Edge3DLaw.hxx>
#include <BRepFill_SectionPlacement.hxx>
#include <ShapeUpgrade_RemoveLocations.hxx>TopoDS_Edge createHelix(const Standard_Real HelixRadius, const Standard_Real HelixAngle, const Standard_Real HelixLength)
{Standard_Real u0 = 0.0;Standard_Real u1 = 2 * M_PI;Standard_Real v0 = 0.0;Standard_Real v1 = HelixLength;double uInter = (u1 - u0) / 1000;double vInter = (v1 - v0) / 1000;TColgp_HArray1OfPnt Points(1, 1001);Handle(Geom_CylindricalSurface) aCylinder = new Geom_CylindricalSurface(gp::XOY(), HelixRadius);double u;double v;//生成点for (int i = 0; i < 1001; i++) {u = i * vInter * tan(HelixAngle) / HelixRadius;v = i * vInter;Points[i + 1] = aCylinder->Value(u, v);}GeomAPI_PointsToBSpline Approx(Points);Handle_Geom_BSplineCurve K = Approx.Curve();TopoDS_Edge aHelixEdge = BRepBuilderAPI_MakeEdge(K);return aHelixEdge;}
TopoDS_Shape createGrindingwheel()
{Standard_Real Line1_angle = 280 * M_PI / 180;Standard_Real Line1_length = 0.5031;Standard_Real Line2_angle = 236 * M_PI / 180;Standard_Real Line2_length = 0.5925;Standard_Real Arc1_r = 0.112;Standard_Real Arc1_angle = (180 + 10 + 50) * M_PI / 180;gp_Pnt Line1_p1(-0.6822 / 2, 0, 0);gp_Pnt Line2_p1(0.6822 / 2, 0, 0);gp_Lin Line1(Line1_p1, gp_Dir(cos(Line1_angle), sin(Line1_angle), 0.));gp_Lin Line2(Line2_p1, gp_Dir(cos(Line2_angle), sin(Line2_angle), 0.));Handle(Geom_TrimmedCurve) L1 = GC_MakeSegment(Line1, 0., Line1_length);TopoDS_Edge L1e = BRepBuilderAPI_MakeEdge(L1);Handle(Geom_TrimmedCurve) L2 = GC_MakeSegment(Line2, 0., Line2_length);TopoDS_Edge L2e = BRepBuilderAPI_MakeEdge(L2);gp_Pnt l1end = L1->EndPoint();gp_Pnt l2end = L2->EndPoint();gp_Lin Line1v(l1end, gp_Dir(cos(Line1_angle + M_PI_2), sin(Line1_angle + M_PI_2), 0.));gp_Lin2d Line2v(gp_Pnt2d(l2end.X(), l2end.Y()), gp_Dir2d(cos(Line2_angle - M_PI_2), sin(Line2_angle - M_PI_2)));gp_Lin Line2v3d(l2end, gp_Dir(cos(Line2_angle - M_PI_2), sin(Line2_angle - M_PI_2), 0.));Handle(Geom_TrimmedCurve) L1v = GC_MakeSegment(Line1v, 0., Arc1_r);gp_Pnt l1vend = L1v->EndPoint();gp_Circ c1(gp_Ax2(l1vend, gp_Dir(0, 0, 1)), Arc1_r);Handle(Geom_TrimmedCurve) c1c = GC_MakeArcOfCircle(c1, l1end, Arc1_angle, 1);gp_Pnt c1end = c1c->EndPoint();gp_Lin2d Line3(gp_Pnt2d(c1end.X(), c1end.Y()), gp_Dir2d(l2end.X() - c1end.X(), l2end.Y() - c1end.Y()));gp_Lin2d Line3v = Line3.Normal(gp_Pnt2d((l2end.X() + c1end.X()) / 2, (l2end.Y() + c1end.Y()) / 2));IntAna2d_AnaIntersection aIntAna;aIntAna.Perform(Line2v, Line3v);IntAna2d_IntPoint aIntPoint = aIntAna.Point(1);gp_Pnt o2(aIntPoint.Value().X(), aIntPoint.Value().Y(), 0.);Handle(Geom_TrimmedCurve) L2v = GC_MakeSegment(Line2v3d, l2end, o2);Standard_Real r2 = L2v->LastParameter();gp_Circ c2(gp_Ax2(o2, gp_Dir(0, 0, 1)), r2);Handle(Geom_TrimmedCurve) c2c = GC_MakeArcOfCircle(c2, c1end, l2end, 0);gp_Pnt c2low = c2c->Value(M_PI_2);TopoDS_Edge c1ce = BRepBuilderAPI_MakeEdge(c1c);TopoDS_Edge L1ev = BRepBuilderAPI_MakeEdge(L1v);TopoDS_Edge c2ce = BRepBuilderAPI_MakeEdge(c2c);TopoDS_Edge anEdge = BRepBuilderAPI_MakeEdge(Line1_p1, Line2_p1);TopTools_ListOfShape listEdge;listEdge.Append(anEdge);listEdge.Append(L1e);listEdge.Append(c1ce);listEdge.Append(c2ce);listEdge.Append(L2e);BRepBuilderAPI_MakeWire mw;mw.Add(listEdge);mw.Build();TopoDS_Face out = BRepBuilderAPI_MakeFace(mw);gp_Circ  cutcircle(gp_Ax2(gp_Pnt(0, 0, 0), gp_Dir(0, 0, 1)), 0.406 / 2);TopoDS_Edge cute = BRepBuilderAPI_MakeEdge(cutcircle);TopoDS_Wire cutw = BRepBuilderAPI_MakeWire(cute);TopoDS_Face cutf = BRepBuilderAPI_MakeFace(cutw);//平移：gp_Trsf theTransformation1;gp_Vec theVectorOfTranslation1(-c2low.X(), -c2low.Y(), 0.);theTransformation1.SetTranslation(theVectorOfTranslation1);BRepBuilderAPI_Transform myBRepTransformation1(out, theTransformation1);TopoDS_Shape outzero = myBRepTransformation1.Shape();TopoDS_Shape theCommonSurface = BRepAlgoAPI_Common(outzero, cutf);gp_Trsf theTransformation2;gp_Vec theVectorOfTranslation2(0., 0.125 / 2, 0.);theTransformation2.SetTranslation(theVectorOfTranslation2);//绕一个轴旋转：gp_Trsf theTransformation3;gp_Ax1 axez = gp_Ax1(gp_Pnt(0, 0, 0), gp_Dir(0., 0., 1.));theTransformation3.SetRotation(axez, -90 * M_PI / 180);gp_Trsf theTransformation4;gp_Ax1 axex = gp_Ax1(gp_Pnt(0, 0, 0), gp_Dir(1., 0., 0.));theTransformation4.SetRotation(axex, -50 * M_PI / 180);BRepBuilderAPI_Transform myBRepTransformation(theCommonSurface, theTransformation4 * theTransformation3 * theTransformation2);TopoDS_Shape TransformedShape = myBRepTransformation.Shape();return TransformedShape;
}
TopoDS_Shape getShapeOnPosition(TopoDS_Shape myProfile,Handle(BRepFill_LocationLaw) myLoc,Standard_Real pos, Standard_Real a, Standard_Real b, gp_Trsf myTrsf)
{TopoDS_Shape myPos;Handle(GeomFill_LocationLaw) law;gp_Mat M;gp_Vec V;gp_Trsf fila;Standard_Real first, last;myLoc->Law(1)->GetDomain(first, last);Standard_Real px = (pos - a) / (b - a);myLoc->Law(1)->D0(px, M, V);fila.SetValues(M(1, 1), M(1, 2), M(1, 3), V.X(),M(2, 1), M(2, 2), M(2, 3), V.Y(),M(3, 1), M(3, 2), M(3, 3), V.Z());fila.Multiply(myTrsf);myPos = myProfile;TopLoc_Location LocPos(fila);if (!LocPos.IsIdentity()) {//myFirst.Location( LocFirst.Multiplied(myProfile.Location()) );myPos = BRepBuilderAPI_Transform(myProfile, fila, Standard_True); //copy}ShapeUpgrade_RemoveLocations RemLoc;RemLoc.SetRemoveLevel(TopAbs_COMPOUND);RemLoc.Remove(myPos);myPos = RemLoc.GetResult();return myPos;
}
int main(int argc, char* argv[])
{gp_Dir  Z(0.0, 0.0, 1.0);gp_Pnt center(0, 0, 0.0);gp_Pnt xr(0.5, 0, 0.0);gp_Pnt yr(0.0, 1.0, 0.0);gp_Pnt zr(0.0, 0.0, 7.0);gp_Ax2  wb(center, Z);gp_Circ  wbcircle(wb, 0.125 / 2);TopoDS_Edge wbe = BRepBuilderAPI_MakeEdge(wbcircle);TopoDS_Edge xline = BRepBuilderAPI_MakeEdge(center, xr);TopoDS_Edge yline = BRepBuilderAPI_MakeEdge(center, yr);TopoDS_Edge zline = BRepBuilderAPI_MakeEdge(center, zr);//creat a profile of gringing wheelTopoDS_Shape gw = createGrindingwheel();//creat a cylinder surfaceHandle(Geom_CylindricalSurface) aCylinder = new Geom_CylindricalSurface(gp::XOY(), 0.306 / 2);TopoDS_Shape cF = BRepBuilderAPI_MakeFace(aCylinder->Cylinder(), 0, 2 * M_PI, 0, 3.);TopoDS_Solid cys = BRepPrimAPI_MakeCylinder(gp::XOY(), 0.306 / 2, 7);TopoDS_Edge aE = createHelix(0.306 / 2, M_PI / 4, 6.);TopoDS_Wire spine = BRepBuilderAPI_MakeWire(aE);//crate a sweep surfaceTopoDS_Shape pipe = BRepOffsetAPI_MakePipe(spine, gw, GeomFill_IsFrenet, 1);TopoDS_Wire mySpine;TopoDS_Shape myProfile;TopoDS_Shape myShape;gp_Trsf myTrsf;Handle(BRepFill_LocationLaw) myLoc;Handle(TopTools_HArray2OfShape) mySections;Handle(TopTools_HArray2OfShape) myFaces;Handle(TopTools_HArray2OfShape) myEdges;TopTools_MapOfShape myReversedEdges;BRepFill_DataMapOfShapeHArray2OfShape myTapes;BRepFill_DataMapOfShapeHArray2OfShape myRails;Standard_Integer myCurIndexOfSectionEdge;TopoDS_Shape myFirst;TopoDS_Shape myLast;TopTools_DataMapOfShapeListOfShape myGenMap;Standard_Integer myDegmax;Standard_Integer mySegmax;GeomAbs_Shape myContinuity;GeomFill_Trihedron myMode;Standard_Boolean myForceApproxC1;Standard_Real myErrorOnSurf;mySections.Nullify();myFaces.Nullify();myEdges.Nullify();mySpine = spine;myProfile = gw;TopoDS_Shape TheProf;Handle(GeomFill_TrihedronLaw) TLaw;TLaw = new GeomFill_Frenet();Handle(GeomFill_CurveAndTrihedron) Loc = new (GeomFill_CurveAndTrihedron) (TLaw);myLoc = new (BRepFill_Edge3DLaw) (mySpine, Loc);if (myLoc->NbLaw() == 0) {return 0; // Degenerated case}myLoc->TransformInG0Law(); // Set into continuityBRepFill_SectionPlacement Place(myLoc, gw);myTrsf = Place.Transformation();TopLoc_Location Loc2(myTrsf), Loc1;Loc1 = gw.Location();TopoDS_Shape aux;TheProf = myProfile;TheProf.Location(Loc2.Multiplied(Loc1));// Construct First && Last ShapeHandle(GeomFill_LocationLaw) law;gp_Mat M;gp_Vec V;gp_Trsf fila;Standard_Real first,last;myLoc->Law(1)->GetDomain(first, last);std::cout << "first=" <<first<< std::endl;std::cout << "last=" << last << std::endl;std::cout << "myLoc->NbLaw()=" << myLoc->NbLaw() << std::endl;myLoc->Law(1)->D0(first, M, V);fila.SetValues(M(1, 1), M(1, 2), M(1, 3), V.X(),M(2, 1), M(2, 2), M(2, 3), V.Y(),M(3, 1), M(3, 2), M(3, 3), V.Z());fila.Multiply(myTrsf);TopLoc_Location LocFirst(fila);myFirst = myProfile;if (!LocFirst.IsIdentity()) {//myFirst.Location( LocFirst.Multiplied(myProfile.Location()) );myFirst = BRepBuilderAPI_Transform(myProfile, fila, Standard_True); //copy}ShapeUpgrade_RemoveLocations RemLoc;RemLoc.SetRemoveLevel(TopAbs_COMPOUND);RemLoc.Remove(myFirst);myFirst = RemLoc.GetResult();myLoc->Law(myLoc->NbLaw())->GetDomain(first, last);myLoc->Law(myLoc->NbLaw())->D0(last, M, V);//    try { // Not good, but there are no other means to test SetValuesfila.SetValues(M(1, 1), M(1, 2), M(1, 3), V.X(),M(2, 1), M(2, 2), M(2, 3), V.Y(),M(3, 1), M(3, 2), M(3, 3), V.Z());fila.Multiply(myTrsf);TopLoc_Location LocLast(fila);if (!myLoc->IsClosed() || LocFirst != LocLast) {myLast = myProfile;if (!LocLast.IsIdentity()) {//myLast.Location(LocLast.Multiplied(myProfile.Location()) );myLast = BRepBuilderAPI_Transform(myProfile, fila, Standard_True); //copy}}else {myLast = myFirst;}RemLoc.Remove(myLast);myLast = RemLoc.GetResult();TopoDS_Shape md1 = getShapeOnPosition(myProfile, myLoc, 0.2, 0., 6., myTrsf);TopoDS_Shape md2 = getShapeOnPosition(myProfile, myLoc, 0.4, 0., 6., myTrsf);TopoDS_Shape md3 = getShapeOnPosition(myProfile, myLoc, 0.6, 0., 6., myTrsf);TopoDS_Shape md4 = getShapeOnPosition(myProfile, myLoc, 0.8, 0., 6., myTrsf);TopoDS_Shape md5 = getShapeOnPosition(myProfile, myLoc, 1.0, 0., 6., myTrsf);Viewer vout(50, 50, 500, 500);vout << wbe;vout << xline;vout << yline;vout << zline;vout << myProfile;vout << myLast;vout << spine;vout << md1;vout << md2;vout << md3;vout << md4;vout << md5;vout.StartMessageLoop();return 0;
}