Exemple #1
0
        public PointDirection3 Transform(MathTransform tran, MathUtility uti)
        {
            var vec   = Direction.ToSwMathVector(uti).IMultiplyTransform(tran);
            var point = Point.ToSwMathPoint(uti).IMultiplyTransform(tran);

            return(new PointDirection3(vec.ToVector3(), point.ToVector3()));
        }
 /// <summary>
 /// Creates a Point in the global frame that was in the specified direction in the given component's frame
 /// </summary>
 /// <param name="compTransform">The transfrom that the orginal vector was in. If null the vector will be based in the global frame</param>
 /// <param name="point">The local location of the point to be created</param>
 /// <returns>The transformed MathPoint</returns>
 public static MathPoint CreatePoint(MathTransform compTransform, double[] point)
 {
     if (compTransform != null)
         return RobotInfo.mathUtil.CreatePoint(point).MultiplyTransform(compTransform);
     else
         return RobotInfo.mathUtil.CreatePoint(point);
 }
            public GeomVue(View vue, Sketch esquisse)
            {
                MathUtility SwMath = App.Sw.GetMathUtility();

                gPoint ptCentreVue = new gPoint(vue.Position);
                gPoint ptMin       = new gPoint(Double.PositiveInfinity, Double.PositiveInfinity, 0);
                gPoint ptMax       = new gPoint(Double.NegativeInfinity, Double.NegativeInfinity, 0);

                foreach (SketchPoint s in esquisse.GetSketchPoints2())
                {
                    MathPoint swStartPoint = SwMath.CreatePoint(new Double[3] {
                        s.X, s.Y, s.Z
                    });
                    MathTransform SketchXform = esquisse.ModelToSketchTransform;
                    SketchXform  = SketchXform.Inverse();
                    swStartPoint = swStartPoint.MultiplyTransform(SketchXform);
                    MathTransform ViewXform = vue.ModelToViewTransform;
                    swStartPoint = swStartPoint.MultiplyTransform(ViewXform);
                    gPoint swViewStartPt = new gPoint(swStartPoint);
                    ptMin.Min(swViewStartPt);
                    ptMax.Max(swViewStartPt);
                }

                ptMinX = ptMin.X;
                ptMinY = ptMin.Y;
                ptMaxX = ptMax.X;
                ptMaxY = ptMax.Y;
                MajCentreRectangle();
                ptCentreVueX = ptCentreVue.X;
                ptCentreVueY = ptCentreVue.Y;
            }
Exemple #4
0
        private double[] putInComponentSpace(ReflectionPoint rp, Component2 component)
        {
            // TODO : debug!!
            MathTransform invertMainBodyXForm = component.Transform2.Inverse();

            return(rp.location.IMultiplyTransform(invertMainBodyXForm).ArrayData);
        }
 /// <summary>
 /// Class constructor
 /// </summary>
 /// <param name="elasticSearchGateway"></param>
 /// <param name="elevationDataStorage"></param>
 /// <param name="clentsFactory"></param>
 /// <param name="osmGeoJsonPreprocessorExecutor"></param>
 /// <param name="osmRepository"></param>
 /// <param name="dataContainerConverterService"></param>
 /// <param name="wikipediaGateway"></param>
 /// <param name="itmWgs84MathTransfromFactory"></param>
 /// <param name="latestFileFetcherExecutor"></param>
 /// <param name="tagsHelper"></param>
 /// <param name="options"></param>
 /// <param name="logger"></param>
 public OsmPointsOfInterestAdapter(IElasticSearchGateway elasticSearchGateway,
                                   IElevationDataStorage elevationDataStorage,
                                   IClientsFactory clentsFactory,
                                   IOsmGeoJsonPreprocessorExecutor osmGeoJsonPreprocessorExecutor,
                                   IOsmRepository osmRepository,
                                   IDataContainerConverterService dataContainerConverterService,
                                   IWikipediaGateway wikipediaGateway,
                                   IItmWgs84MathTransfromFactory itmWgs84MathTransfromFactory,
                                   IOsmLatestFileFetcherExecutor latestFileFetcherExecutor,
                                   ITagsHelper tagsHelper,
                                   IOptions <ConfigurationData> options,
                                   ILogger logger) :
     base(dataContainerConverterService,
          logger)
 {
     _clientsFactory = clentsFactory;
     _osmGeoJsonPreprocessorExecutor = osmGeoJsonPreprocessorExecutor;
     _osmRepository             = osmRepository;
     _wikipediaGateway          = wikipediaGateway;
     _tagsHelper                = tagsHelper;
     _latestFileFetcherExecutor = latestFileFetcherExecutor;
     _elevationDataStorage      = elevationDataStorage;
     _wgs84ItmMathTransform     = itmWgs84MathTransfromFactory.CreateInverse();
     _options = options.Value;
     _elasticSearchGateway = elasticSearchGateway;
 }
Exemple #6
0
        public static Camera createCamera(Component2 fieldOfView, SldWorks swApp, ModelDoc2 swDoc, AssemblyDoc swAssembly, SelectionMgr swSelectionMgr, MathUtility mathUtils)
        {
            IMathPoint centreOfVision = getCentreOfVision(fieldOfView, swDoc, swSelectionMgr, mathUtils);

            if (centreOfVision == null)
            {
                swApp.SendMsgToUser2("you need to insert the camera first!", 1, 1);
                return(null);
            }

            RefPlane cameraNormalPlane = getCameraNormalPlane(fieldOfView, swDoc, swSelectionMgr);

            // note that we reverse this transform because our plane normal is pointing the wrong direction :(
            MathTransform cameraTransform = Camera.reverseTransform(cameraNormalPlane.Transform, mathUtils);
            MathVector    cameraDirection = getCameraDirection(cameraTransform, mathUtils);

            drawRayForCamera(cameraDirection, centreOfVision, swDoc, swSelectionMgr);

            Camera camera = new Camera(fieldOfView, mathUtils.CreatePoint((double[])centreOfVision.ArrayData), cameraDirection);

            camera.swApp          = swApp;
            camera.swDoc          = swDoc;
            camera.swAssembly     = swAssembly;
            camera.swSelectionMgr = swSelectionMgr;
            camera.mathUtils      = mathUtils;

            return(camera);
        }
Exemple #7
0
        public CoordinateConverter(string _projection)
        {
            try
            {
                CoordinateSystemFactory csFactory = new CoordinateSystemFactory();
                CoordinateSystem = csFactory.CreateFromWkt(_projection);

                CoordinateTransformationFactory ctFactory = new CoordinateTransformationFactory();

                XYToLonLatTransform = ctFactory.CreateFromCoordinateSystems(CoordinateSystem, WGS84).MathTransform;
                LonLatToXYTransform = ctFactory.CreateFromCoordinateSystems(WGS84, CoordinateSystem).MathTransform;

                XYToWebTransform = ctFactory.CreateFromCoordinateSystems(CoordinateSystem, WebMercator).MathTransform;
                WebToXYTransform = ctFactory.CreateFromCoordinateSystems(WebMercator, CoordinateSystem).MathTransform;
            } catch (Exception ex)
            {
                if (_projection.ToLowerInvariant().Contains("web_mercator"))
                {
                    CoordinateTransformationFactory ctFactory = new CoordinateTransformationFactory();
                    LonLatToXYTransform = ctFactory.CreateFromCoordinateSystems(WGS84, WebMercator).MathTransform;
                    XYToLonLatTransform = LonLatToXYTransform.Inverse();
                }
                else
                {
                    throw ex;
                }
            }
        }
Exemple #8
0
 private static Geometry Transform(Geometry geom, MathTransform transform, int targetSrid)
 {
     geom = geom.Copy();
     geom.Apply(new MathTransformFilter(transform));
     geom.SRID = targetSrid;
     return(geom);
 }
 /// <summary>
 /// Service's constructor
 /// </summary>
 /// <param name="clinetsFactory"></param>
 /// <param name="elasticSearchGateway"></param>
 /// <param name="osmGeoJsonPreprocessorExecutor"></param>
 /// <param name="tagsHelper"></param>
 /// <param name="osmRepository"></param>
 /// <param name="pointsOfInterestAdapterFactory"></param>
 /// <param name="featuresMergeExecutor"></param>
 /// <param name="latestFileFetcherExecutor"></param>
 /// <param name="graphHopperGateway"></param>
 /// <param name="pointsOfInterestFilesCreatorExecutor"></param>
 /// <param name="elevationDataStorage"></param>
 /// <param name="itmWgs84MathTransfromFactory"></param>
 /// <param name="logger"></param>
 public DatabasesUpdaterService(IClientsFactory clinetsFactory,
                                IElasticSearchGateway elasticSearchGateway,
                                IOsmGeoJsonPreprocessorExecutor osmGeoJsonPreprocessorExecutor,
                                ITagsHelper tagsHelper, IOsmRepository osmRepository,
                                IPointsOfInterestAdapterFactory pointsOfInterestAdapterFactory,
                                IFeaturesMergeExecutor featuresMergeExecutor,
                                IOsmLatestFileFetcherExecutor latestFileFetcherExecutor,
                                IGraphHopperGateway graphHopperGateway,
                                IPointsOfInterestFilesCreatorExecutor pointsOfInterestFilesCreatorExecutor,
                                IElevationDataStorage elevationDataStorage,
                                IItmWgs84MathTransfromFactory itmWgs84MathTransfromFactory,
                                ILogger logger)
 {
     _elasticSearchGateway           = elasticSearchGateway;
     _osmGeoJsonPreprocessorExecutor = osmGeoJsonPreprocessorExecutor;
     _tagsHelper    = tagsHelper;
     _osmRepository = osmRepository;
     _pointsOfInterestAdapterFactory       = pointsOfInterestAdapterFactory;
     _pointsOfInterestFilesCreatorExecutor = pointsOfInterestFilesCreatorExecutor;
     _featuresMergeExecutor     = featuresMergeExecutor;
     _latestFileFetcherExecutor = latestFileFetcherExecutor;
     _graphHopperGateway        = graphHopperGateway;
     _osmGateway           = clinetsFactory.CreateNonAuthClient();
     _elevationDataStorage = elevationDataStorage;
     _logger        = logger;
     _mathTransform = itmWgs84MathTransfromFactory.CreateInverse();
 }
Exemple #10
0
        /// <summary>
        /// Converts coordinates in projected meters to decimal degrees.
        /// </summary>
        /// <param name="p">Point in meters</param>
        /// <returns>Transformed point in decimal degrees</returns>
        public override double[] MetersToDegrees(double[] p)
        {
            double num  = Math.Atan((p[0] - this._falseEasting) / (this.ro0 - (p[1] - this._falseNorthing)));
            double x    = Math.Sqrt(Math.Pow(p[0] - this._falseEasting, 2) + Math.Pow(this.ro0 - (p[1] - this._falseNorthing), 2));
            double num3 = (this.C - ((Math.Pow(x, 2) * Math.Pow(this.n, 2)) / Math.Pow(base._semiMajor, 2))) / this.n;

            Math.Sin(num3 / (1 - (((1 - this.e_sq) / (2 * this.e)) * Math.Log((1 - this.e) / (1 + this.e)))));
            double a        = Math.Asin(num3 * 0.5);
            double maxValue = double.MaxValue;
            int    num6     = 0;

            while (Math.Abs((double)(a - maxValue)) > 1E-06)
            {
                maxValue = a;
                double num7 = Math.Sin(a);
                double num8 = this.e_sq * Math.Pow(num7, 2);
                a += (Math.Pow(1 - num8, 2) / (2 * Math.Cos(a))) * (((num3 / (1 - this.e_sq)) - (num7 / (1 - num8))) + ((1 / (2 * this.e)) * Math.Log((1 - (this.e * num7)) / (1 + (this.e * num7)))));
                num6++;
                if (num6 > 0x19)
                {
                    throw new ApplicationException("Transformation failed to converge in Albers backwards transformation");
                }
            }
            double rad = this.lon_center + (num / this.n);

            return(new double[] { MathTransform.Radians2Degrees(rad), MathTransform.Radians2Degrees(a) });
        }
        /// <summary>
        /// Converts coordinates in decimal degrees to projected meters.
        /// </summary>
        /// <param name="lonlat">The point in decimal degrees.</param>
        /// <returns>Point in projected meters</returns>
        public override double[] DegreesToMeters(double[] lonlat)
        {
            double num4;
            double num  = MathTransform.Degrees2Radians(lonlat[0]);
            double num2 = MathTransform.Degrees2Radians(lonlat[1]);

            if (Math.Abs((double)(Math.Abs(num2) - 1.5707963267948966)) > 1E-10)
            {
                double sinphi = Math.Sin(num2);
                double x      = MapProjection.tsfnz(this.e, num2, sinphi);
                num4 = (base._semiMajor * this.f0) * Math.Pow(x, this.ns);
            }
            else
            {
                double num3 = num2 * this.ns;
                if (num3 <= 0)
                {
                    throw new ApplicationException();
                }
                num4 = 0;
            }
            double a = this.ns * MapProjection.adjust_lon(num - this.center_lon);

            return(new double[] { ((num4 * Math.Sin(a)) + this._falseEasting), ((this.rh - (num4 * Math.Cos(a))) + this._falseNorthing) });
        }
 /// <summary>
 /// Creates a vector in the global frame that was in the specified direction in the given component's frame
 /// </summary>
 /// <param name="compTransform">The transfrom that the orginal vector was in. If null the vector will be based in the global frame</param>
 /// <param name="vector">The direction of the vector to be created</param>
 /// <returns>The transformed MathVector</returns>
 public static MathVector CreateVector(MathTransform compTransform, double[] vector)
 {
     if (compTransform != null)
         return RobotInfo.mathUtil.CreateVector(vector).MultiplyTransform(compTransform).Normalise();
     else
         return RobotInfo.mathUtil.CreateVector(vector).Normalise();
 }
Exemple #13
0
 /// <summary>
 /// Converts a latitude value in degrees to radians.
 /// </summary>
 /// <param name="y">The value in degrees to to radians.</param>
 /// <param name="edge">If true, -90 and +90 are valid, otherwise they are considered out of range.</param>
 /// <returns></returns>
 protected static double LatitudeToRadians(double y, bool edge)
 {
     if (!(edge ? ((y >= -90) && (y <= 90)) : ((y > -90) && (y < 90))))
     {
         throw new ArgumentOutOfRangeException("x", y, " not a valid latitude in degrees.");
     }
     return(MathTransform.Degrees2Radians(y));
 }
Exemple #14
0
        private static MathVector getCameraDirection(MathTransform cameraTransform, MathUtility mathUtils)
        {
            double[]   canonicalRay    = { 0, 0, 1 };
            MathVector normalVector    = mathUtils.CreateVector(canonicalRay);
            MathVector cameraDirection = normalVector.MultiplyTransform(cameraTransform);

            return(cameraDirection);
        }
Exemple #15
0
 /// <summary>
 /// Converts a longitude value in degrees to radians.
 /// </summary>
 /// <param name="x">The value in degrees to convert to radians.</param>
 /// <param name="edge">If true, -180 and +180 are valid, otherwise they are considered out of range.</param>
 /// <returns></returns>
 protected static double LongitudeToRadians(double x, bool edge)
 {
     if (!(edge ? ((x >= -180) && (x <= 180)) : ((x > -180) && (x < 180))))
     {
         throw new ArgumentOutOfRangeException("x", x, " not a valid longitude in degrees.");
     }
     return(MathTransform.Degrees2Radians(x));
 }
 public static double[] GetXYZ(MathTransform transform)
 {
     double[] XYZ = new double[3];
     XYZ[0] = transform.ArrayData[9];
     XYZ[1] = transform.ArrayData[10];
     XYZ[2] = transform.ArrayData[11];
     return(XYZ);
 }
Exemple #17
0
        // Used only by the part exporter
        public void exportLink(bool zIsUp)
        {
            createBaseRefOrigin(zIsUp);
            MathTransform coordSysTransform =
                ActiveSWModel.Extension.GetCoordinateSystemTransformByName("Origin_global");
            Matrix <double> GlobalTransform = ops.getTransformation(coordSysTransform);

            localizeLink(mRobot.BaseLink, GlobalTransform);

            //Creating package directories
            URDFPackage package = new URDFPackage(mPackageName, mSavePath);

            package.createDirectories();
            string meshFileName            = package.MeshesDirectory + mRobot.BaseLink.name + ".STL";
            string windowsMeshFileName     = package.WindowsMeshesDirectory + mRobot.BaseLink.name + ".STL";
            string windowsURDFFileName     = package.WindowsRobotsDirectory + mRobot.name + ".urdf";
            string windowsManifestFileName = package.WindowsPackageDirectory + "manifest.xml";

            //Creating manifest file
            PackageXMLWriter manifestWriter = new PackageXMLWriter(windowsManifestFileName);
            PackageXML       Manifest       = new PackageXML(mRobot.name);

            Manifest.writeElement(manifestWriter);

            //Customizing STL preferences to how I want them
            saveUserPreferences();
            setSTLExportPreferences();
            setLinkSpecificSTLPreferences("", mRobot.BaseLink.STLQualityFine, ActiveSWModel);
            int errors   = 0;
            int warnings = 0;

            //Saving part as STL mesh

            ActiveSWModel.Extension.SaveAs(windowsMeshFileName, (int)swSaveAsVersion_e.swSaveAsCurrentVersion,
                                           (int)swSaveAsOptions_e.swSaveAsOptions_Silent, null, ref errors, ref warnings);
            mRobot.BaseLink.Visual.Geometry.Mesh.filename    = meshFileName;
            mRobot.BaseLink.Collision.Geometry.Mesh.filename = meshFileName;

            correctSTLMesh(windowsMeshFileName);

            mRobot.BaseLink.Visual.Material.Texture.filename =
                package.TexturesDirectory + Path.GetFileName(mRobot.BaseLink.Visual.Material.Texture.wFilename);
            string textureSavePath =
                package.WindowsTexturesDirectory + Path.GetFileName(mRobot.BaseLink.Visual.Material.Texture.wFilename);

            if (mRobot.BaseLink.Visual.Material.Texture.wFilename != "")
            {
                System.IO.File.Copy(mRobot.BaseLink.Visual.Material.Texture.wFilename, textureSavePath, true);
            }

            //Writing URDF to file
            URDFWriter uWriter = new URDFWriter(windowsURDFFileName);

            //mRobot.addLink(mLink);
            mRobot.writeURDF(uWriter.writer);

            resetUserPreferences();
        }
        private Double AngleCubeDeVisualisation(View vue, Sketch esquisse)
        {
            MathUtility SwMath = App.Sw.GetMathUtility();

            List <gPoint> LstPt = new List <gPoint>();

            foreach (SketchPoint s in esquisse.GetSketchPoints2())
            {
                MathPoint point = SwMath.CreatePoint(new Double[3] {
                    s.X, s.Y, s.Z
                });
                MathTransform SketchXform = esquisse.ModelToSketchTransform;
                SketchXform = SketchXform.Inverse();
                point       = point.MultiplyTransform(SketchXform);
                MathTransform ViewXform = vue.ModelToViewTransform;
                point = point.MultiplyTransform(ViewXform);
                gPoint swViewStartPt = new gPoint(point);
                LstPt.Add(swViewStartPt);
            }

            // On recherche le point le point le plus à droite puis le plus haut
            LstPt.Sort(new gPointComparer(ListSortDirection.Descending, p => p.X));
            LstPt.Sort(new gPointComparer(ListSortDirection.Descending, p => p.Y));

            // On le supprime
            LstPt.RemoveAt(0);

            // On recherche le point le point le plus à gauche puis le plus bas
            LstPt.Sort(new gPointComparer(ListSortDirection.Ascending, p => p.X));
            LstPt.Sort(new gPointComparer(ListSortDirection.Ascending, p => p.Y));


            // C'est le point de rotation
            gPoint pt1 = LstPt[0];

            // On recherche le plus loin
            gPoint pt2;
            Double d1 = pt1.Distance(LstPt[1]);
            Double d2 = pt1.Distance(LstPt[2]);

            if (d1 > d2)
            {
                pt2 = LstPt[1];
            }
            // En cas d'égalité, on renvoi le point le plus à gauche
            else if (d1 == d2)
            {
                pt2 = (LstPt[1].X < LstPt[2].X) ? LstPt[1] : LstPt[2];
            }
            else
            {
                pt2 = LstPt[2];
            }

            gVecteur v = new gVecteur(pt1, pt2);

            return(Math.Atan2(v.Y, v.X));
        }
Exemple #19
0
 public static MathTransform reverseTransform(MathTransform transform, MathUtility mathUtility)
 {
     double[] transformData = (double[])transform.ArrayData;
     for (int i = 0; i < transformData.GetLength(0); i++)
     {
         transformData[i] = -transformData[i];
     }
     return(mathUtility.CreateTransform(transformData));
 }
Exemple #20
0
        /// <summary>
        /// Creates an instance of an TransverseMercatorProjection projection object.
        /// </summary>
        /// <param name="parameters">List of parameters to initialize the projection.</param>
        /// <param name="inverse">Flag indicating wether is a forward/projection (false) or an inverse projection (true).</param>
        /// <remarks>
        /// <list type="bullet">
        /// <listheader><term>Items</term><description>Descriptions</description></listheader>
        /// <item><term>semi_major</term><description>Semi major radius</description></item>
        /// <item><term>semi_minor</term><description>Semi minor radius</description></item>
        /// <item><term>scale_factor</term><description></description></item>
        /// <item><term>central meridian</term><description></description></item>
        /// <item><term>latitude_origin</term><description></description></item>
        /// <item><term>false_easting</term><description></description></item>
        /// <item><term>false_northing</term><description></description></item>
        /// </list>
        /// </remarks>
        public TransverseMercator(List <ProjectionParameter> parameters, bool inverse) : base(parameters, inverse)
        {
            base.Name          = "Transverse_Mercator";
            base.Authority     = "EPSG";
            base.AuthorityCode = 0x264fL;
            ProjectionParameter parameter  = base.GetParameter("semi_major");
            ProjectionParameter parameter2 = base.GetParameter("semi_minor");
            ProjectionParameter parameter3 = base.GetParameter("scale_factor");
            ProjectionParameter parameter4 = base.GetParameter("central_meridian");
            ProjectionParameter parameter5 = base.GetParameter("latitude_of_origin");
            ProjectionParameter parameter6 = base.GetParameter("false_easting");
            ProjectionParameter parameter7 = base.GetParameter("false_northing");

            if (parameter == null)
            {
                throw new ArgumentException("Missing projection parameter 'semi_major'");
            }
            if (parameter2 == null)
            {
                throw new ArgumentException("Missing projection parameter 'semi_minor'");
            }
            if (parameter3 == null)
            {
                throw new ArgumentException("Missing projection parameter 'scale_factor'");
            }
            if (parameter4 == null)
            {
                throw new ArgumentException("Missing projection parameter 'central_meridian'");
            }
            if (parameter5 == null)
            {
                throw new ArgumentException("Missing projection parameter 'latitude_of_origin'");
            }
            if (parameter6 == null)
            {
                throw new ArgumentException("Missing projection parameter 'false_easting'");
            }
            if (parameter7 == null)
            {
                throw new ArgumentException("Missing projection parameter 'false_northing'");
            }
            this.r_major          = parameter.Value;
            this.r_minor          = parameter2.Value;
            this.scale_factor     = parameter3.Value;
            this.central_meridian = MathTransform.Degrees2Radians(parameter4.Value);
            this.lat_origin       = MathTransform.Degrees2Radians(parameter5.Value);
            this.false_easting    = parameter6.Value;
            this.false_northing   = parameter7.Value;
            this.es  = 1 - Math.Pow(this.r_minor / this.r_major, 2);
            this.e   = Math.Sqrt(this.es);
            this.e0  = MapProjection.e0fn(this.es);
            this.e1  = MapProjection.e1fn(this.es);
            this.e2  = MapProjection.e2fn(this.es);
            this.e3  = MapProjection.e3fn(this.es);
            this.ml0 = this.r_major * MapProjection.mlfn(this.e0, this.e1, this.e2, this.e3, this.lat_origin);
            this.esp = this.es / (1 - this.es);
        }
Exemple #21
0
        /// <summary>
        /// Creates an instance of an Albers projection object.
        /// </summary>
        /// <remarks>
        /// <para>The parameters this projection expects are listed below.</para>
        /// <list type="table">
        /// <listheader><term>Items</term><description>Descriptions</description></listheader>
        /// <item><term>latitude_of_center</term><description>The latitude of the point which is not the natural origin and at which grid coordinate values false easting and false northing are defined.</description></item>
        /// <item><term>longitude_of_center</term><description>The longitude of the point which is not the natural origin and at which grid coordinate values false easting and false northing are defined.</description></item>
        /// <item><term>standard_parallel_1</term><description>For a conic projection with two standard parallels, this is the latitude of intersection of the cone with the ellipsoid that is nearest the pole.  Scale is true along this parallel.</description></item>
        /// <item><term>standard_parallel_2</term><description>For a conic projection with two standard parallels, this is the latitude of intersection of the cone with the ellipsoid that is furthest from the pole.  Scale is true along this parallel.</description></item>
        /// <item><term>false_easting</term><description>The easting value assigned to the false origin.</description></item>
        /// <item><term>false_northing</term><description>The northing value assigned to the false origin.</description></item>
        /// </list>
        /// </remarks>
        /// <param name="parameters">List of parameters to initialize the projection.</param>
        /// <param name="isInverse">Indicates whether the projection forward (meters to degrees or degrees to meters).</param>
        public AlbersProjection(List <ProjectionParameter> parameters, bool isInverse) : base(parameters, isInverse)
        {
            base.Name = "Albers_Conic_Equal_Area";
            ProjectionParameter parameter  = base.GetParameter("longitude_of_center");
            ProjectionParameter parameter2 = base.GetParameter("latitude_of_center");
            ProjectionParameter parameter3 = base.GetParameter("standard_parallel_1");
            ProjectionParameter parameter4 = base.GetParameter("standard_parallel_2");
            ProjectionParameter parameter5 = base.GetParameter("false_easting");
            ProjectionParameter parameter6 = base.GetParameter("false_northing");

            if (parameter == null)
            {
                throw new ArgumentException("Missing projection parameter 'longitude_of_center'");
            }
            if (parameter2 == null)
            {
                throw new ArgumentException("Missing projection parameter 'latitude_of_center'");
            }
            if (parameter3 == null)
            {
                throw new ArgumentException("Missing projection parameter 'standard_parallel_1'");
            }
            if (parameter4 == null)
            {
                throw new ArgumentException("Missing projection parameter 'standard_parallel_2'");
            }
            if (parameter5 == null)
            {
                throw new ArgumentException("Missing projection parameter 'false_easting'");
            }
            if (parameter6 == null)
            {
                throw new ArgumentException("Missing projection parameter 'false_northing'");
            }
            this.lon_center = MathTransform.Degrees2Radians(parameter.Value);
            double lat  = MathTransform.Degrees2Radians(parameter2.Value);
            double num2 = MathTransform.Degrees2Radians(parameter3.Value);
            double num3 = MathTransform.Degrees2Radians(parameter4.Value);

            this._falseEasting  = MathTransform.Degrees2Radians(parameter5.Value);
            this._falseNorthing = MathTransform.Degrees2Radians(parameter6.Value);
            if (Math.Abs((double)(num2 + num3)) < double.Epsilon)
            {
                throw new ApplicationException("Equal latitudes for standard parallels on opposite sides of Equator.");
            }
            this.e_sq = 1 - Math.Pow(base._semiMinor / base._semiMajor, 2);
            this.e    = Math.Sqrt(this.e_sq);
            double num4 = this.alpha(num2);
            double num5 = this.alpha(num3);
            double x    = Math.Cos(num2) / Math.Sqrt(1 - (this.e_sq * Math.Pow(Math.Sin(num2), 2)));
            double num7 = Math.Cos(num3) / Math.Sqrt(1 - (this.e_sq * Math.Pow(Math.Sin(num3), 2)));

            this.n   = (Math.Pow(x, 2) - Math.Pow(num7, 2)) / (num5 - num4);
            this.C   = Math.Pow(x, 2) + (this.n * num4);
            this.ro0 = this.Ro(this.alpha(lat));
        }
Exemple #22
0
        /// <summary>
        /// Converts coordinates in decimal degrees to projected meters.
        /// </summary>
        /// <param name="lonlat">The point in decimal degrees.</param>
        /// <returns>Point in projected meters</returns>
        public override double[] DegreesToMeters(double[] lonlat)
        {
            double num  = MathTransform.Degrees2Radians(lonlat[0]);
            double lat  = MathTransform.Degrees2Radians(lonlat[1]);
            double a    = this.alpha(lat);
            double num4 = this.Ro(a);
            double num5 = this.n * (num - this.lon_center);

            return(new double[] { (this._falseEasting + (num4 * Math.Sin(num5))), ((this._falseNorthing + this.ro0) - (num4 * Math.Cos(num5))) });
        }
Exemple #23
0
        public CrsTransform(CoordinateSystem from, CoordinateSystem to)
        {
            _from = from;
            _to   = to;

            CoordinateTransformationFactory ctfac = new CoordinateTransformationFactory();

            _trans         = ctfac.CreateFromCoordinateSystems(_from, _to);
            _mathTransform = _trans.MathTransform;
        }
Exemple #24
0
 /// <summary>
 /// Controller's constructor
 /// </summary>
 /// <param name="graphHopperGateway"></param>
 /// <param name="elevationDataStorage"></param>
 /// <param name="itmWgs84MathTransfromFactory"></param>
 /// <param name="geometryFactory"></param>
 public RoutingController(IGraphHopperGateway graphHopperGateway,
                          IElevationDataStorage elevationDataStorage,
                          IItmWgs84MathTransfromFactory itmWgs84MathTransfromFactory,
                          GeometryFactory geometryFactory)
 {
     _graphHopperGateway    = graphHopperGateway;
     _elevationDataStorage  = elevationDataStorage;
     _geometryFactory       = geometryFactory;
     _wgs84ItmMathTransform = itmWgs84MathTransfromFactory.CreateInverse();
 }
        public static Matrix3D GetMatrixFromMathTransform(ref MathTransform trasf)
        {
            double[] amatr = (double[])trasf.ArrayData;

            Matrix3D res = new Matrix3D(amatr[0], amatr[1], amatr[2], 0,
                                        amatr[3], amatr[4], amatr[5], 0,
                                        amatr[6], amatr[7], amatr[8], 0,
                                        amatr[9], amatr[10], amatr[11], 1);
            return res;
        }
Exemple #26
0
        // ReSharper restore InconsistentNaming

        /// <summary>
        /// Creates an instance of this class
        /// </summary>
        /// <param name="parameters">An enumeration of projection parameters</param>
        /// <param name="inverse">Indicator if this projection is inverse</param>
        protected MapProjection(IEnumerable <ProjectionParameter> parameters, MapProjection inverse)
            : this(parameters)
        {
            _inverse = inverse;
            if (_inverse != null)
            {
                inverse._inverse = this;
                _isInverse       = !inverse._isInverse;
            }
        }
Exemple #27
0
        public override Point LocationToPoint(Location location)
        {
            if (MathTransform == null)
            {
                throw new InvalidOperationException("The CoordinateSystem property is not set.");
            }

            var coordinate = MathTransform.Transform(new Coordinate(location.Longitude, location.Latitude));

            return(new Point(coordinate.X, coordinate.Y));
        }
Exemple #28
0
        public void MultiplyTransfom(MathTransform trans)
        {
            MathUtility Mu = App.Sw.GetMathUtility();
            MathPoint   mp = Mu.CreatePoint(new double[] { X, Y, Z });

            mp = mp.MultiplyTransform(trans);
            Double[] pt = (Double[])mp.ArrayData;
            X = pt[0];
            Y = pt[1];
            Z = pt[2];
        }
        public void TestGetRPYMathTransform(double[] matrixData, double[] expected)
        {
            MathUtility   util      = SwApp.GetMathUtility();
            MathTransform transform = util.CreateTransform((object)matrixData);

            double[] result = MathOps.GetRPY(transform);
            for (int i = 0; i < expected.Length; i++)
            {
                Assert.Equal(expected[i], result[i], 10);
            }
        }
        public void TestGetTransformationFromMathTransform(double[] matrixData, double[] expected)
        {
            MathUtility   util      = SwApp.GetMathUtility();
            MathTransform transform = util.CreateTransform((object)matrixData);

            double[] result = MathOps.GetTransformation(transform).AsColumnMajorArray();
            for (int i = 0; i < expected.Length; i++)
            {
                Assert.Equal(expected[i], result[i]);
            }
        }
Exemple #31
0
 /// <summary>
 /// Class constructor
 /// </summary>
 /// <param name="adaptersFactory"></param>
 /// <param name="elevationDataStorage"></param>
 /// <param name="elasticSearchGateway"></param>
 /// <param name="itmWgs84MathTransfromFactory"></param>
 /// <param name="logger"></param>
 public ExternalSourcesController(IPointsOfInterestAdapterFactory adaptersFactory,
                                  IElevationDataStorage elevationDataStorage,
                                  IElasticSearchGateway elasticSearchGateway,
                                  IItmWgs84MathTransfromFactory itmWgs84MathTransfromFactory,
                                  ILogger logger)
 {
     _adaptersFactory      = adaptersFactory;
     _elevationDataStorage = elevationDataStorage;
     _elasticSearchGateway = elasticSearchGateway;
     _wgs84ItmTransform    = itmWgs84MathTransfromFactory.CreateInverse();
     _logger = logger;
 }
 /// <summary>
 /// Constructor
 /// </summary>
 /// <param name="logger"></param>
 /// <param name="elevationDataStorage"></param>
 /// <param name="itmWgs84MathTransfromFactory"></param>
 /// <param name="osmGeoJsonConverter"></param>
 /// <param name="tagsHelper"></param>
 public OsmGeoJsonPreprocessorExecutor(ILogger logger,
                                       IElevationDataStorage elevationDataStorage,
                                       IItmWgs84MathTransfromFactory itmWgs84MathTransfromFactory,
                                       IOsmGeoJsonConverter osmGeoJsonConverter,
                                       ITagsHelper tagsHelper)
 {
     _logger = logger;
     _osmGeoJsonConverter  = osmGeoJsonConverter;
     _elevationDataStorage = elevationDataStorage;
     _wgs84ItmConverter    = itmWgs84MathTransfromFactory.CreateInverse();
     _tagsHelper           = tagsHelper;
 }
        public static Matrix getRotationMatrix(MathTransform transform)
        {
            var rot = new DenseMatrix(3);

            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    rot.At(i, j, transform.ArrayData[i + 3 * j]);
                }
            }
            return(rot);
        }
        //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        public double[] GetQuaternionFromMatrix(ref MathTransform trasf)
        {
            double[] q = new double[4];
            double[] amatr = (double[])trasf.ArrayData;
            double s, tr;
                                        // for speed reasons: ..
            double m00 = amatr[0];
            double m01 = amatr[3];
            double m02 = amatr[6];
            double m10 = amatr[1];
            double m11 = amatr[4];
            double m12 = amatr[7];
            double m20 = amatr[2];
            double m21 = amatr[5];
            double m22 = amatr[8];

            tr=m00 + m11 + m22;		// diag sum

            if (tr >= 0)
            {
                s = Math.Sqrt(tr + 1.0);
                q[0] = 0.5 * s;
                s = 0.5 / s;
                q[1] = (m21 - m12) * s;
                q[2] = (m02 - m20) * s;
                q[3] = (m10 - m01) * s;
            }
            else
            {
                int i = 0;

                if (m11 > m00)
                {
                    i = 1;
                    if (m22 > m11)	i = 2;
                }
                else
                {
                    if (m22 > m00)  i = 2;
                }

                switch (i)
                {
                case 0:
                    s = Math.Sqrt(m00 - m11 - m22 + 1);
                    q[1] = 0.5 * s;
                    s = 0.5 / s;
                    q[2] = (m01 + m10) * s;
                    q[3] = (m20 + m02) * s;
                    q[0] = (m21 - m12) * s;
                    break;
                case 1:
                    s = Math.Sqrt(m11 - m22 - m00 + 1);
                    q[2] = 0.5 * s;
                    s = 0.5 / s;
                    q[3] = (m12 + m21) * s;
                    q[1] = (m01 + m10) * s;
                    q[0] = (m02 - m20) * s;
                    break;
                case 2:
                    s = Math.Sqrt(m22 - m00 - m11 + 1);
                    q[3] = 0.5 * s;
                    s = 0.5 / s;
                    q[1] = (m20 + m02) * s;
                    q[2] = (m12 + m21) * s;
                    q[0] = (m10 - m01) * s;
                    break;
                }
            }
            return q;
        }
        public void PythonTraverseFeatures_for_collshapes(Component2 swComp, long nLevel, ref  string asciitext, int nbody, ref MathTransform chbodytransform, ref bool found_collisionshapes, Component2 swCompBase)
        {
            CultureInfo bz = new CultureInfo("en-BZ");
            Feature swFeat;
            swFeat = (Feature)swComp.FirstFeature();

            String bodyname = "body_" + nbody;

            MathTransform subcomp_transform = swComp.GetTotalTransform(true);
            MathTransform invchbody_trasform = (MathTransform)chbodytransform.Inverse();
            MathTransform collshape_subcomp_transform = subcomp_transform.IMultiply(invchbody_trasform); // row-ordered transf. -> reverse mult.order!

            // Export collision shapes
            if (this.checkBox_collshapes.Checked)
            {
                object[] bodies;
                object bodyInfo;
                bodies = (object[])swComp.GetBodies3((int)swBodyType_e.swAllBodies, out bodyInfo);

                if (bodies != null)
                {
                    // see if it contains some collision shape
                    bool build_collision_model = false;
                    for (int ib = 0; ib < bodies.Length; ib++)
                    {
                        Body2 swBody = (Body2)bodies[ib];
                        if (swBody.Name.StartsWith("COLL."))
                            build_collision_model = true;
                    }

                    if (build_collision_model)
                    {
                        if (!found_collisionshapes)
                        {
                            found_collisionshapes = true;

                            // fetch SW attribute with Chrono parameters
                            SolidWorks.Interop.sldworks.Attribute myattr = (SolidWorks.Interop.sldworks.Attribute)swCompBase.FindAttribute(this.mSWintegration.defattr_chbody, 0);

                            if (myattr != null)
                            {
                                asciitext += "\n# Collision parameters \n";
                                double param_friction = ((Parameter)myattr.GetParameter("friction")).GetDoubleValue();
                                double param_restitution = ((Parameter)myattr.GetParameter("restitution")).GetDoubleValue();
                                double param_rolling_friction = ((Parameter)myattr.GetParameter("rolling_friction")).GetDoubleValue();
                                double param_spinning_friction = ((Parameter)myattr.GetParameter("spinning_friction")).GetDoubleValue();
                                double param_collision_envelope = ((Parameter)myattr.GetParameter("collision_envelope")).GetDoubleValue();
                                double param_collision_margin = ((Parameter)myattr.GetParameter("collision_margin")).GetDoubleValue();
                                int    param_collision_family = (int)((Parameter)myattr.GetParameter("collision_family")).GetDoubleValue();

                                asciitext += String.Format(bz, "{0}.SetFriction({1:g});\n", bodyname, param_friction);
                                if (param_restitution != 0)
                                    asciitext += String.Format(bz, "{0}.SetImpactC({1:g});\n", bodyname, param_restitution);
                                if (param_rolling_friction != 0)
                                    asciitext += String.Format(bz, "{0}.SetRollingFriction({1:g});\n", bodyname, param_rolling_friction);
                                if (param_spinning_friction != 0)
                                    asciitext += String.Format(bz, "{0}.SetSpinningFriction({1:g});\n", bodyname, param_spinning_friction);
                                //if (param_collision_envelope != 0.03)
                                    asciitext += String.Format(bz, "{0}.GetCollisionModel().SetEnvelope({1:g});\n", bodyname, param_collision_envelope * ChScale.L);
                                //if (param_collision_margin != 0.01)
                                    asciitext += String.Format(bz, "{0}.GetCollisionModel().SetSafeMargin({1:g});\n", bodyname, param_collision_margin * ChScale.L);
                                if (param_collision_family != 0)
                                    asciitext += String.Format(bz, "{0}.GetCollisionModel().SetFamily({1});\n", bodyname, param_collision_family);
                            }

                            // clear model only at 1st subcomponent where coll shapes are found in features:
                            asciitext += "\n# Collision shapes \n";
                            asciitext += String.Format(bz, "{0}.GetCollisionModel().ClearModel()\n", bodyname);
                        }

                        for (int ib = 0; ib < bodies.Length; ib++)
                        {
                            Body2 swBody = (Body2)bodies[ib];

                            if (swBody.Name.StartsWith("COLL."))
                            {
                                bool rbody_converted = false;
                                if (ConvertToCollisionShapes.SWbodyToSphere(swBody))
                                {
                                    Point3D center_l = new Point3D(); // in local subcomponent
                                    double rad = 0;
                                    ConvertToCollisionShapes.SWbodyToSphere(swBody, ref rad, ref center_l);
                                    Point3D center = SWTaskpaneHost.PointTransform(center_l, ref collshape_subcomp_transform);
                                    asciitext += String.Format(bz, "{0}.GetCollisionModel().AddSphere({1}, chrono.ChVectorD({2},{3},{4}))\n", bodyname,
                                        rad * ChScale.L,
                                        center.X * ChScale.L,
                                        center.Y * ChScale.L,
                                        center.Z * ChScale.L);
                                    rbody_converted = true;
                                }
                                if (ConvertToCollisionShapes.SWbodyToBox(swBody))
                                {
                                    Point3D  vC_l = new Point3D();
                                    Vector3D eX_l = new Vector3D(); Vector3D eY_l = new Vector3D(); Vector3D eZ_l = new Vector3D();
                                    ConvertToCollisionShapes.SWbodyToBox(swBody, ref vC_l, ref eX_l, ref eY_l, ref eZ_l);
                                    Point3D  vC = SWTaskpaneHost.PointTransform(vC_l, ref collshape_subcomp_transform);
                                    Vector3D eX = SWTaskpaneHost.DirTransform(eX_l, ref collshape_subcomp_transform);
                                    Vector3D eY = SWTaskpaneHost.DirTransform(eY_l, ref collshape_subcomp_transform);
                                    Vector3D eZ = SWTaskpaneHost.DirTransform(eZ_l, ref collshape_subcomp_transform);
                                    double hsX = eX.Length * 0.5;
                                    double hsY = eY.Length * 0.5;
                                    double hsZ = eZ.Length * 0.5;
                                    Point3D  vO = vC + 0.5 * eX + 0.5 * eY + 0.5 * eZ;
                                    Vector3D Dx = eX; Dx.Normalize();
                                    Vector3D Dy = eY; Dy.Normalize();
                                    Vector3D Dz = Vector3D.CrossProduct(Dx, Dy);
                                    asciitext += String.Format(bz, "mr = chrono.ChMatrix33D()\n");
                                    asciitext += String.Format(bz, "mr[0,0]={0}; mr[1,0]={1}; mr[2,0]={2} \n", Dx.X, Dx.Y, Dx.Z);
                                    asciitext += String.Format(bz, "mr[0,1]={0}; mr[1,1]={1}; mr[2,1]={2} \n", Dy.X, Dy.Y, Dy.Z);
                                    asciitext += String.Format(bz, "mr[0,2]={0}; mr[1,2]={1}; mr[2,2]={2} \n", Dz.X, Dz.Y, Dz.Z);
                                    asciitext += String.Format(bz, "{0}.GetCollisionModel().AddBox({1},{2},{3},chrono.ChVectorD({4},{5},{6}),mr)\n", bodyname,
                                        hsX * ChScale.L,
                                        hsY * ChScale.L,
                                        hsZ * ChScale.L,
                                        vO.X * ChScale.L,
                                        vO.Y * ChScale.L,
                                        vO.Z * ChScale.L);
                                    rbody_converted = true;
                                }
                                if (ConvertToCollisionShapes.SWbodyToCylinder(swBody))
                                {
                                    Point3D p1_l = new Point3D();
                                    Point3D p2_l = new Point3D();
                                    double rad = 0;
                                    ConvertToCollisionShapes.SWbodyToCylinder(swBody, ref p1_l, ref p2_l, ref rad);
                                    Point3D p1 = SWTaskpaneHost.PointTransform(p1_l, ref collshape_subcomp_transform);
                                    Point3D p2 = SWTaskpaneHost.PointTransform(p2_l, ref collshape_subcomp_transform);
                                    Vector3D Dy = p1 - p2; Dy.Normalize();
                                    double hsY = (p1 - p2).Length * 0.5;
                                    double hsZ = rad;
                                    double hsX = rad;
                                    Point3D  vO = p1 + 0.5 * (p2 - p1);
                                    Vector3D Dx = new Vector3D();
                                    if (Dy.X < 0.9)
                                    {
                                        Vector3D Dtst = new Vector3D(1, 0, 0);
                                        Dx = Vector3D.CrossProduct(Dtst, Dy);
                                    }
                                    else
                                    {
                                        Vector3D Dtst = new Vector3D(0, 1, 0);
                                        Dx = Vector3D.CrossProduct(Dtst, Dy);
                                    }
                                    Vector3D Dz = Vector3D.CrossProduct(Dx, Dy);
                                    asciitext += String.Format(bz, "mr = chrono.ChMatrix33D()\n");
                                    asciitext += String.Format(bz, "mr[0,0]={0}; mr[1,0]={1}; mr[2,0]={2} \n", Dx.X, Dx.Y, Dx.Z);
                                    asciitext += String.Format(bz, "mr[0,1]={0}; mr[1,1]={1}; mr[2,1]={2} \n", Dy.X, Dy.Y, Dy.Z);
                                    asciitext += String.Format(bz, "mr[0,2]={0}; mr[1,2]={1}; mr[2,2]={2} \n", Dz.X, Dz.Y, Dz.Z);
                                    asciitext += String.Format(bz, "{0}.GetCollisionModel().AddCylinder({1},{2},{3},chrono.ChVectorD({4},{5},{6}),mr)\n", bodyname,
                                        hsX * ChScale.L,
                                        hsZ * ChScale.L,
                                        hsY * ChScale.L,
                                        vO.X * ChScale.L,
                                        vO.Y * ChScale.L,
                                        vO.Z * ChScale.L); // note order hsX-Z-Y
                                    rbody_converted = true;
                                }

                                if (ConvertToCollisionShapes.SWbodyToConvexHull(swBody, 30) && !rbody_converted)
                                {
                                    Point3D[] vertexes = new Point3D[1]; // will be resized by SWbodyToConvexHull
                                    ConvertToCollisionShapes.SWbodyToConvexHull(swBody, ref vertexes, 30);
                                    if (vertexes.Length > 0)
                                    {
                                        asciitext += String.Format(bz, "pt_vect = chrono.vector_ChVectorD()\n");
                                        for (int iv = 0; iv < vertexes.Length; iv++)
                                        {
                                            Point3D vert_l = vertexes[iv];
                                            Point3D vert   = SWTaskpaneHost.PointTransform(vert_l, ref collshape_subcomp_transform);
                                            asciitext += String.Format(bz, "pt_vect.push_back(chrono.ChVectorD({0},{1},{2}))\n",
                                                vert.X * ChScale.L,
                                                vert.Y * ChScale.L,
                                                vert.Z * ChScale.L);
                                        }
                                        asciitext += String.Format(bz, "{0}.GetCollisionModel().AddConvexHull(pt_vect)\n", bodyname);
                                    }
                                }

                            } // end dealing with a collision shape

                        } // end solid bodies traversal for converting to coll.shapes

                    } // end if build_collision_model
                }

            } // end collision shapes export
        }
 public static Point3D PointTransform(Point3D pt, ref MathTransform trasf)
 {
     //Point3D pp = new Point3D(pt.X,pt.Y,pt.Z);
     Matrix3D M = GetMatrixFromMathTransform(ref trasf);
     return M.Transform(pt);
 }
 public void globalAxis(double[] Axis, MathTransform coordsysTransform)
 {
     if (coordsysTransform != null)
     {
         Vector<double> vec = new DenseVector(new double[] { Axis[0], Axis[1], Axis[2], 0 });
         Matrix<double> transform = ops.getTransformation(coordsysTransform);
         vec = transform * vec;
         Axis[0] = vec[0]; Axis[1] = vec[1]; Axis[2] = vec[2];
     }
     ops.threshold(Axis, 0.00001);
 }
        public void PythonTraverseFeatures_for_links(Feature swFeat, long nLevel, ref  string asciitext, ref MathTransform roottrasf, ref Component2 assemblyofmates)
        {
            Feature swSubFeat;

            int num_link = 0;

            while ((swFeat != null))
            {
                // Export mates as constraints

                if ((swFeat.GetTypeName2() == "MateGroup") &&
                    (this.checkBox_constraints.Checked))
                {
                    swSubFeat = (Feature)swFeat.GetFirstSubFeature();

                    while ((swSubFeat != null))
                    {
                        ConvertMates.ConvertMateToPython(ref swSubFeat, ref asciitext, ref mSWApplication, ref saved_parts, ref num_link, ref roottrasf, ref assemblyofmates);

                        swSubFeat = (Feature)swSubFeat.GetNextSubFeature();

                    } // end while loop on subfeatures mates

                } // end if mate group

                swFeat = (Feature)swFeat.GetNextFeature();

            } // end while loop on features
        }
        public void PythonTraverseFeatures_for_markers(Feature swFeat, long nLevel, ref  string asciitext, int nbody, MathTransform swCompTotalTrasf)
        {
            CultureInfo bz = new CultureInfo("en-BZ");

            int nmarker = 0;

            String bodyname = "body_" + nbody;

            while ((swFeat != null))
            {
                // asciitext += "# feature: " + swFeat.Name + " [" + swFeat.GetTypeName2() + "]" + "\n";

                // Export markers, if any (as coordinate systems)
                if (swFeat.GetTypeName2() == "CoordSys")
                {
                    nmarker++;
                    CoordinateSystemFeatureData swCoordSys = (CoordinateSystemFeatureData)swFeat.GetDefinition();
                    MathTransform tr = swCoordSys.Transform;

                    MathTransform tr_part = swCompTotalTrasf;
                    MathTransform tr_abs = tr.IMultiply(tr_part);  // row-ordered transf. -> reverse mult.order!

                    double[] quat = GetQuaternionFromMatrix(ref tr_abs);
                    double[] amatr = (double[])tr_abs.ArrayData;
                    String markername = "marker_" + nbody + "_" + nmarker;
                    asciitext += "\n# Auxiliary marker (coordinate system feature)\n";
                    asciitext += String.Format(bz, "{0} =chrono.ChMarker()\n", markername);
                    asciitext += String.Format(bz, "{0}.SetName('{1}')" + "\n", markername, swFeat.Name);
                    asciitext += String.Format(bz, "{0}.AddMarker({1})\n", bodyname, markername);
                    asciitext += String.Format(bz, "{0}.Impose_Abs_Coord(chrono.ChCoordsysD(chrono.ChVectorD({1},{2},{3}),chrono.ChQuaternionD({4},{5},{6},{7})))\n",
                               markername,
                               amatr[9]*ChScale.L,
                               amatr[10]*ChScale.L,
                               amatr[11]*ChScale.L,
                               quat[0], quat[1], quat[2], quat[3]);
                }

                swFeat = (Feature)swFeat.GetNextFeature();
            }
        }
        public static bool ConvertMateToPython( 
                                    ref Feature swMateFeature,
                                    ref string asciitext, 
                                    ref ISldWorks mSWApplication,
                                    ref Hashtable saved_parts,
                                    ref int num_link,
                                    ref MathTransform roottrasf,
                                    ref Component2 assemblyofmates
                                    )
        {
            if (swMateFeature == null)
                return false;

            Mate2 swMate = (Mate2)swMateFeature.GetSpecificFeature2();

            if (swMate == null)
                return false;

            object foo =null;
            bool[] suppressedflags;
            suppressedflags = (bool[])swMateFeature.IsSuppressed2((int)swInConfigurationOpts_e.swThisConfiguration, foo);
            if (suppressedflags[0] == true)
                return false;

            if (swMate.GetMateEntityCount() >= 2)
            {
                 // Get the mated parts
                MateEntity2 swEntityA = swMate.MateEntity(0);
                MateEntity2 swEntityB = swMate.MateEntity(1);
                Component2 swCompA = swEntityA.ReferenceComponent;
                Component2 swCompB = swEntityB.ReferenceComponent;
                double[] paramsA = (double[])swEntityA.EntityParams;
                double[] paramsB = (double[])swEntityB.EntityParams;
                 // this is needed because parts might reside in subassemblies, and mate params are expressed in parent subassembly
                MathTransform invroottrasf = (MathTransform)roottrasf.Inverse();
                MathTransform trA = roottrasf;
                MathTransform trB = roottrasf;

                if (assemblyofmates != null)
                {
                    MathTransform partrasfA = assemblyofmates.GetTotalTransform(true);
                    if (partrasfA != null)
                        trA = partrasfA.IMultiply(invroottrasf); // row-ordered transf. -> reverse mult.order!
                    MathTransform partrasfB = assemblyofmates.GetTotalTransform(true);
                    if (partrasfB != null)
                        trB = partrasfB.IMultiply(invroottrasf); // row-ordered transf. -> reverse mult.order!
                }

                 // Fetch the python names using hash map (python names added when scanning parts)
                ModelDocExtension swModelDocExt = default(ModelDocExtension);
                ModelDoc2 swModel = (ModelDoc2)mSWApplication.ActiveDoc;
                swModelDocExt = swModel.Extension;
                String name1 = (String)saved_parts[swModelDocExt.GetPersistReference3(swCompA)];
                String name2 = (String)saved_parts[swModelDocExt.GetPersistReference3(swCompB)];

                 // Only constraints between two parts or part & layout can be created
                if ( ((name1 != null) || (name2 != null)) && (name1 != name2) )
                {
                    CultureInfo bz = new CultureInfo("en-BZ");

                    if (name1 == null)
                        name1 = "body_0";
                    if (name2 == null)
                        name2 = "body_0";

                    // Add some comment in Python, to list the referenced SW items
                    asciitext += "\n# Mate constraint: " + swMateFeature.Name + " [" + swMateFeature.GetTypeName2() + "]" + " type:" + swMate.Type + " align:" + swMate.Alignment + " flip:" + swMate.Flipped + "\n";
                    for (int e = 0; e < swMate.GetMateEntityCount(); e++)
                    {
                        MateEntity2 swEntityN = swMate.MateEntity(e);
                        Component2 swCompN = swEntityN.ReferenceComponent;
                        String ce_nameN = (String)saved_parts[swModelDocExt.GetPersistReference3(swCompN)];
                        asciitext += "#   Entity " + e + ": C::E name: " + ce_nameN + " , SW name: " + swCompN.Name2 + " ,  SW ref.type:" + swEntityN.Reference.GetType() + " (" + swEntityN.ReferenceType2 + ")\n";
                    }
                    asciitext += "\n";

                    //
                    // For each type of SW mate, see which C::E mate constraint(s)
                    // must be created. Some SW mates correspond to more than one C::E constraints.
                    //

                    bool swapAB_1 = false;
                    bool do_CHmate_Xdistance  = false;
                    double do_distance_val  = 0.0;
                    bool do_CHmate_parallel   = false;
                    bool   do_parallel_flip     = false;
                    bool do_CHmate_orthogonal = false;
                    bool do_CHmate_spherical  = false;
                    bool do_CHmate_pointline  = false;

                    // to simplify things later...
                    // NOTE: swMate.MateEntity(0).Reference.GetType() seems equivalent to  swMate.MateEntity(0).ReferenceType2
                    // but in some cases the latter fails.
                    bool entity_0_as_FACE   = (swMate.MateEntity(0).Reference.GetType() == (int)swSelectType_e.swSelFACES);
                    bool entity_0_as_EDGE   = (swMate.MateEntity(0).Reference.GetType() == (int)swSelectType_e.swSelEDGES) ||
                                              (swMate.MateEntity(0).Reference.GetType() == (int)swSelectType_e.swSelSKETCHSEGS) ||
                                              (swMate.MateEntity(0).Reference.GetType() == (int)swSelectType_e.swSelDATUMAXES);
                    bool entity_0_as_VERTEX = (swMate.MateEntity(0).Reference.GetType() == (int)swSelectType_e.swSelVERTICES) ||
                                              (swMate.MateEntity(0).Reference.GetType() == (int)swSelectType_e.swSelSKETCHPOINTS) ||
                                              (swMate.MateEntity(0).Reference.GetType() == (int)swSelectType_e.swSelDATUMPOINTS);

                    bool entity_1_as_FACE   = (swMate.MateEntity(1).Reference.GetType() == (int)swSelectType_e.swSelFACES);
                    bool entity_1_as_EDGE   = (swMate.MateEntity(1).Reference.GetType() == (int)swSelectType_e.swSelEDGES) ||
                                              (swMate.MateEntity(1).Reference.GetType() == (int)swSelectType_e.swSelSKETCHSEGS) ||
                                              (swMate.MateEntity(1).Reference.GetType() == (int)swSelectType_e.swSelDATUMAXES);
                    bool entity_1_as_VERTEX = (swMate.MateEntity(1).Reference.GetType() == (int)swSelectType_e.swSelVERTICES) ||
                                              (swMate.MateEntity(1).Reference.GetType() == (int)swSelectType_e.swSelSKETCHPOINTS) ||
                                              (swMate.MateEntity(1).Reference.GetType() == (int)swSelectType_e.swSelDATUMPOINTS);

                    Point3D cA  = new Point3D(0, 0, 0);
                    Point3D cB  = new Point3D(0, 0, 0);
                    Vector3D dA = new Vector3D(1, 0, 0);
                    Vector3D dB = new Vector3D(1, 0, 0);

                    Point3D cAloc = new Point3D(paramsA[0], paramsA[1], paramsA[2]);
                    cA = SWTaskpaneHost.PointTransform(cAloc, ref trA);
                    Point3D cBloc = new Point3D(paramsB[0], paramsB[1], paramsB[2]);
                    cB = SWTaskpaneHost.PointTransform(cBloc, ref trB);

                    if (!entity_0_as_VERTEX)
                    {
                        Vector3D dAloc = new Vector3D(paramsA[3], paramsA[4], paramsA[5]);
                        dA = SWTaskpaneHost.DirTransform(dAloc, ref trA);
                    }

                    if (!entity_1_as_VERTEX)
                    {
                        Vector3D dBloc = new Vector3D(paramsB[3], paramsB[4], paramsB[5]);
                        dB = SWTaskpaneHost.DirTransform(dBloc, ref trB);
                    }

                    if (swMateFeature.GetTypeName2() == "MateCoincident")
                    {
                        if ((entity_0_as_FACE) &&
                            (entity_1_as_FACE))
                        {
                            do_CHmate_Xdistance = true;
                            do_CHmate_parallel  = true;
                        }
                        if ((entity_0_as_EDGE) &&
                            (entity_1_as_EDGE))
                        {
                            do_CHmate_pointline = true;
                            do_CHmate_parallel  = true;
                        }
                        if ((entity_0_as_VERTEX) &&
                            (entity_1_as_VERTEX))
                        {
                            do_CHmate_spherical = true;
                        }
                        if ((entity_0_as_VERTEX) &&
                            (entity_1_as_EDGE))
                        {
                            do_CHmate_pointline = true;
                        }
                        if ((entity_0_as_EDGE) &&
                            (entity_1_as_VERTEX))
                        {
                            do_CHmate_pointline = true;
                            swapAB_1 = true;
                        }
                        if ((entity_0_as_VERTEX) &&
                            (entity_1_as_FACE))
                        {
                            do_CHmate_Xdistance = true;
                        }
                        if ((entity_0_as_FACE) &&
                            (entity_1_as_VERTEX))
                        {
                            do_CHmate_Xdistance = true;
                            swapAB_1 = true;
                        }
                        if ((entity_0_as_EDGE) &&
                            (entity_1_as_FACE))
                        {
                            do_CHmate_Xdistance = true;
                            do_CHmate_orthogonal = true;
                        }
                        if ((entity_0_as_FACE) &&
                            (entity_1_as_EDGE))
                        {
                            do_CHmate_Xdistance = true;
                            do_CHmate_orthogonal = true;
                            swapAB_1 = true;
                        }

                        if (swMate.Alignment == (int)swMateAlign_e.swMateAlignANTI_ALIGNED)
                            do_parallel_flip = true;
                    }

                    if (swMateFeature.GetTypeName2() == "MateConcentric")
                    {
                        if ((entity_0_as_FACE) &&
                            (entity_1_as_FACE))
                        {
                            do_CHmate_pointline = true;
                            do_CHmate_parallel = true;
                        }
                        if ((entity_0_as_EDGE) &&
                            (entity_1_as_EDGE))
                        {
                            do_CHmate_pointline = true;
                            do_CHmate_parallel = true;
                        }
                        if ((entity_0_as_VERTEX) &&
                            (entity_1_as_FACE))
                        {
                            do_CHmate_pointline = true;
                        }
                        if ((entity_0_as_FACE) &&
                            (entity_1_as_VERTEX))
                        {
                            do_CHmate_pointline = true;
                            swapAB_1 = true;
                        }
                        if ((entity_0_as_EDGE) &&
                            (entity_1_as_FACE))
                        {
                            do_CHmate_pointline = true;
                            do_CHmate_parallel = true;
                        }
                        if ((entity_0_as_FACE) &&
                            (entity_1_as_EDGE))
                        {
                            do_CHmate_pointline = true;
                            do_CHmate_parallel = true;
                            swapAB_1 = true;
                        }

                        if (swMate.Alignment == (int)swMateAlign_e.swMateAlignANTI_ALIGNED)
                            do_parallel_flip = true;
                    }

                    if (swMateFeature.GetTypeName2() == "MateParallel")
                    {
                        if ((entity_0_as_FACE) &&
                            (entity_1_as_FACE))
                        {
                            do_CHmate_parallel = true;
                        }
                        if ((entity_0_as_EDGE) &&
                            (entity_1_as_EDGE))
                        {
                            do_CHmate_parallel = true;
                        }
                        if ((entity_0_as_EDGE) &&
                            (entity_1_as_FACE))
                        {
                            do_CHmate_orthogonal = true;
                        }
                        if ((entity_0_as_FACE) &&
                            (entity_1_as_EDGE))
                        {
                            do_CHmate_orthogonal = true;
                            swapAB_1 = true;
                        }

                        if (swMate.Alignment == (int)swMateAlign_e.swMateAlignANTI_ALIGNED)
                            do_parallel_flip = true;
                    }

                    if (swMateFeature.GetTypeName2() == "MatePerpendicular")
                    {
                        if ((entity_0_as_FACE) &&
                            (entity_1_as_FACE))
                        {
                            do_CHmate_orthogonal = true;
                        }
                        if ((entity_0_as_EDGE) &&
                            (entity_1_as_EDGE))
                        {
                            do_CHmate_orthogonal = true;
                        }
                        if ((entity_0_as_EDGE) &&
                            (entity_1_as_FACE))
                        {
                            do_CHmate_parallel = true;
                        }
                        if ((entity_0_as_FACE) &&
                            (entity_1_as_EDGE))
                        {
                            do_CHmate_parallel = true;
                            swapAB_1 = true;
                        }

                        if (swMate.Alignment == (int)swMateAlign_e.swMateAlignANTI_ALIGNED)
                            do_parallel_flip = true;
                    }

                    if (swMateFeature.GetTypeName2() == "MateDistanceDim")
                    {
                        if ((entity_0_as_FACE) &&
                            (entity_1_as_FACE))
                        {
                            do_CHmate_Xdistance  = true;
                            do_CHmate_parallel   = true;
                        }
                        if ((entity_0_as_VERTEX) &&
                            (entity_1_as_FACE))
                        {
                            do_CHmate_Xdistance = true;
                        }
                        if ((entity_0_as_FACE) &&
                            (entity_1_as_VERTEX))
                        {
                            do_CHmate_Xdistance = true;
                            swapAB_1 = true;
                        }

                        //***TO DO*** cases of distance line-vs-line and line-vs-vertex and vert-vert.
                        //           Those will require another .cpp ChLinkMate specialized class(es).

                        if (swMate.Alignment == (int)swMateAlign_e.swMateAlignANTI_ALIGNED)
                            do_parallel_flip = true;

                        // Get the imposed distance value, in SI units
                        string confnames = "";
                        do_distance_val = swMate.DisplayDimension.GetDimension2(0).IGetSystemValue3((int)swInConfigurationOpts_e.swThisConfiguration, 0, ref confnames);

                        if (swMate.Flipped)
                            do_distance_val = -do_distance_val;
                    }

                    ////
                    //// WRITE PYHTON CODE CORRESPONDING TO CONSTRAINTS
                    ////

                    if (do_CHmate_Xdistance)
                    {
                        num_link++;
                        String linkname = "link_" + num_link;
                        asciitext += String.Format(bz, "{0} = chrono.ChLinkMateXdistance()\n", linkname);

                        asciitext += String.Format(bz, "cA = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                  cA.X * ChScale.L,
                                  cA.Y * ChScale.L,
                                  cA.Z * ChScale.L);
                        asciitext += String.Format(bz, "cB = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                  cB.X * ChScale.L,
                                  cB.Y * ChScale.L,
                                  cB.Z * ChScale.L);
                        if (!entity_0_as_VERTEX)
                         asciitext += String.Format(bz, "dA = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                  dA.X, dA.Y, dA.Z);
                        if (!entity_1_as_VERTEX)
                         asciitext += String.Format(bz, "dB = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                  dB.X, dB.Y, dB.Z);

                        // Initialize link, by setting the two csys, in absolute space,
                        if (!swapAB_1)
                            asciitext += String.Format(bz, "{0}.Initialize({1},{2},False,cA,cB,dB)\n", linkname, name1, name2);
                        else
                            asciitext += String.Format(bz, "{0}.Initialize({1},{2},False,cB,cA,dA)\n", linkname, name2, name1);

                        //if (do_distance_val!=0)
                            asciitext += String.Format(bz, "{0}.SetDistance({1})\n", linkname,
                                do_distance_val * ChScale.L *-1);

                        asciitext += String.Format(bz, "{0}.SetName(\"{1}\")\n", linkname, swMateFeature.Name);
                        // Insert to a list of exported items
                        asciitext += String.Format(bz, "exported_items.append({0})\n\n", linkname);
                    }

                    if (do_CHmate_parallel)
                    {
                        if (Math.Abs(Vector3D.DotProduct(dA, dB)) > 0.98)
                        {
                            num_link++;
                            String linkname = "link_" + num_link;
                            asciitext += String.Format(bz, "{0} = chrono.ChLinkMateParallel()\n", linkname);

                            asciitext += String.Format(bz, "cA = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                      cA.X * ChScale.L,
                                      cA.Y * ChScale.L,
                                      cA.Z * ChScale.L);
                            asciitext += String.Format(bz, "dA = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                      dA.X, dA.Y, dA.Z);
                            asciitext += String.Format(bz, "cB = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                      cB.X * ChScale.L,
                                      cB.Y * ChScale.L,
                                      cB.Z * ChScale.L);
                            asciitext += String.Format(bz, "dB = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                      dB.X, dB.Y, dB.Z);

                            if (do_parallel_flip)
                                asciitext += String.Format(bz, "{0}.SetFlipped(True)\n", linkname);

                            // Initialize link, by setting the two csys, in absolute space,
                            if (!swapAB_1)
                                asciitext += String.Format(bz, "{0}.Initialize({1},{2},False,cA,cB,dA,dB)\n", linkname, name1, name2);
                            else
                                asciitext += String.Format(bz, "{0}.Initialize({1},{2},False,cB,cA,dB,dA)\n", linkname, name2, name1);

                            asciitext += String.Format(bz, "{0}.SetName(\"{1}\")\n", linkname, swMateFeature.Name);
                            // Insert to a list of exported items
                            asciitext += String.Format(bz, "exported_items.append({0})\n\n", linkname);
                        }
                        else
                        {
                            asciitext += "\n# ChLinkMateParallel skipped because directions not parallel! \n";
                        }
                    }

                    if (do_CHmate_orthogonal)
                    {
                        if (Math.Abs(Vector3D.DotProduct(dA, dB)) < 0.02)
                        {
                            num_link++;
                            String linkname = "link_" + num_link;
                            asciitext += String.Format(bz, "{0} = chrono.ChLinkMateOrthogonal()\n", linkname);

                            asciitext += String.Format(bz, "cA = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                      cA.X * ChScale.L,
                                      cA.Y * ChScale.L,
                                      cA.Z * ChScale.L);
                            asciitext += String.Format(bz, "dA = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                      dA.X, dA.Y, dA.Z);
                            asciitext += String.Format(bz, "cB = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                      cB.X * ChScale.L,
                                      cB.Y * ChScale.L,
                                      cB.Z * ChScale.L);
                            asciitext += String.Format(bz, "dB = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                      dB.X, dB.Y, dB.Z);

                            // Initialize link, by setting the two csys, in absolute space,
                            if (!swapAB_1)
                                asciitext += String.Format(bz, "{0}.Initialize({1},{2},False,cA,cB,dA,dB)\n", linkname, name1, name2);
                            else
                                asciitext += String.Format(bz, "{0}.Initialize({1},{2},False,cB,cA,dB,dA)\n", linkname, name2, name1);

                            asciitext += String.Format(bz, "{0}.SetName(\"{1}\")\n", linkname, swMateFeature.Name);
                            // Insert to a list of exported items
                            asciitext += String.Format(bz, "exported_items.append({0})\n\n", linkname);
                        }
                        else
                        {
                            asciitext += "\n# ChLinkMateOrthogonal skipped because directions not orthogonal! \n";
                        }
                    }

                    if (do_CHmate_spherical)
                    {
                        num_link++;
                        String linkname = "link_" + num_link;
                        asciitext += String.Format(bz, "{0} = chrono.ChLinkMateSpherical()\n", linkname);

                        asciitext += String.Format(bz, "cA = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                  cA.X * ChScale.L,
                                  cA.Y * ChScale.L,
                                  cA.Z * ChScale.L);
                        asciitext += String.Format(bz, "cB = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                  cB.X * ChScale.L,
                                  cB.Y * ChScale.L,
                                  cB.Z * ChScale.L);

                        // Initialize link, by setting the two csys, in absolute space,
                        if (!swapAB_1)
                            asciitext += String.Format(bz, "{0}.Initialize({1},{2},False,cA,cB)\n", linkname, name1, name2);
                        else
                            asciitext += String.Format(bz, "{0}.Initialize({1},{2},False,cB,cA)\n", linkname, name2, name1);

                        asciitext += String.Format(bz, "{0}.SetName(\"{1}\")\n", linkname, swMateFeature.Name);
                        // Insert to a list of exported items
                        asciitext += String.Format(bz, "exported_items.append({0})\n\n", linkname);
                    }

                    if (do_CHmate_pointline)
                    {
                        num_link++;
                        String linkname = "link_" + num_link;
                        asciitext += String.Format(bz, "{0} = chrono.ChLinkMateGeneric()\n", linkname);
                        asciitext += String.Format(bz, "{0}.SetConstrainedCoords(False, True, True, False, False, False)\n", linkname);

                        asciitext += String.Format(bz, "cA = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                  cA.X * ChScale.L,
                                  cA.Y * ChScale.L,
                                  cA.Z * ChScale.L);
                        asciitext += String.Format(bz, "cB = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                  cB.X * ChScale.L,
                                  cB.Y * ChScale.L,
                                  cB.Z * ChScale.L);
                        if (!entity_0_as_VERTEX)
                            asciitext += String.Format(bz, "dA = chrono.ChVectorD({0:g},{1:g},{2:g})\n", dA.X, dA.Y, dA.Z);
                        else
                            asciitext += String.Format(bz, "dA = chrono.VNULL\n");
                        if (!entity_1_as_VERTEX)
                            asciitext += String.Format(bz, "dB = chrono.ChVectorD({0:g},{1:g},{2:g})\n", dB.X, dB.Y, dB.Z);
                        else
                            asciitext += String.Format(bz, "dB = chrono.VNULL\n");

                        // Initialize link, by setting the two csys, in absolute space,
                        if (!swapAB_1)
                            asciitext += String.Format(bz, "{0}.Initialize({1},{2},False,cA,cB,dA,dB)\n", linkname, name1, name2);
                        else
                            asciitext += String.Format(bz, "{0}.Initialize({1},{2},False,cB,cA,dB,dA)\n", linkname, name2, name1);

                        asciitext += String.Format(bz, "{0}.SetName(\"{1}\")\n", linkname, swMateFeature.Name);
                        // Insert to a list of exported items
                        asciitext += String.Format(bz, "exported_items.append({0})\n\n", linkname);
                    }

                    // Now, do some other special mate type that did not fall in combinations
                    // of do_CHmate_pointline, do_CHmate_spherical, etc etc

                    if (swMateFeature.GetTypeName2() == "MateHinge")
                    {
                        // auto flip direction if anti aligned (seems that this is assumed automatically in MateHinge in SW)
                        if (Vector3D.DotProduct(dA, dB) < 0)
                            dB.Negate();

                        // Hinge constraint must be splitted in two C::E constraints: a coaxial and a point-vs-plane
                        num_link++;
                        String linkname = "link_" + num_link;
                        asciitext += String.Format(bz, "{0} = chrono.ChLinkMateCoaxial()\n", linkname);

                        asciitext += String.Format(bz, "cA = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                  cA.X * ChScale.L,
                                  cA.Y * ChScale.L,
                                  cA.Z * ChScale.L);
                        asciitext += String.Format(bz, "dA = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                  dA.X, dA.Y, dA.Z);
                        asciitext += String.Format(bz, "cB = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                  cB.X * ChScale.L,
                                  cB.Y * ChScale.L,
                                  cB.Z * ChScale.L);
                        asciitext += String.Format(bz, "dB = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                  dB.X, dB.Y, dB.Z);

                        asciitext += String.Format(bz, "{0}.SetName(\"{1}\")\n", linkname, swMateFeature.Name);

                        // Initialize link, by setting the two csys, in absolute space,
                        asciitext += String.Format(bz, "{0}.Initialize({1},{2},False,cA,cB,dA,dB)\n", linkname, name1, name2);

                        // Insert to a list of exported items
                        asciitext += String.Format(bz, "exported_items.append({0})\n", linkname);

                        // NOTE!!! The 'hinge' mate uses 4 references: fetch the two others remaining
                        // and build another C::E link, for point-vs-face mating

                        MateEntity2 swEntityC = swMate.MateEntity(2);
                        MateEntity2 swEntityD = swMate.MateEntity(3);
                        Component2 swCompC = swEntityC.ReferenceComponent;
                        Component2 swCompD = swEntityD.ReferenceComponent;
                        double[] paramsC = (double[])swEntityC.EntityParams;
                        double[] paramsD = (double[])swEntityD.EntityParams;
                        String name3 = (String)saved_parts[swModelDocExt.GetPersistReference3(swCompC)];
                        String name4 = (String)saved_parts[swModelDocExt.GetPersistReference3(swCompD)];
                        MathTransform trC = roottrasf;
                        MathTransform trD = roottrasf;

                        if (assemblyofmates != null)
                        {
                            MathTransform partrasfC = assemblyofmates.GetTotalTransform(true);
                            if (partrasfC != null)
                                trC = partrasfC.IMultiply(invroottrasf);
                            MathTransform partrasfD = assemblyofmates.GetTotalTransform(true);
                            if (partrasfD != null)
                                trD = partrasfD.IMultiply(invroottrasf);
                        }

                        bool entity_2_as_VERTEX = (swMate.MateEntity(2).Reference.GetType() == (int)swSelectType_e.swSelVERTICES) ||
                                                  (swMate.MateEntity(2).Reference.GetType() == (int)swSelectType_e.swSelSKETCHPOINTS) ||
                                                  (swMate.MateEntity(2).Reference.GetType() == (int)swSelectType_e.swSelDATUMPOINTS);
                        bool entity_3_as_VERTEX = (swMate.MateEntity(3).Reference.GetType() == (int)swSelectType_e.swSelVERTICES) ||
                                                  (swMate.MateEntity(3).Reference.GetType() == (int)swSelectType_e.swSelSKETCHPOINTS) ||
                                                  (swMate.MateEntity(3).Reference.GetType() == (int)swSelectType_e.swSelDATUMPOINTS);
                        Point3D cC  = new Point3D(0, 0, 0);
                        Point3D cD  = new Point3D(0, 0, 0);
                        Vector3D dC = new Vector3D(1, 0, 0);
                        Vector3D dD = new Vector3D(1, 0, 0);

                        Point3D cCloc = new Point3D(paramsC[0], paramsC[1], paramsC[2]);
                        cC = SWTaskpaneHost.PointTransform(cCloc, ref trC);
                        Point3D cDloc = new Point3D(paramsD[0], paramsD[1], paramsD[2]);
                        cD = SWTaskpaneHost.PointTransform(cDloc, ref trD);

                        if (!entity_2_as_VERTEX)
                        {
                            Vector3D dCloc = new Vector3D(paramsC[3], paramsC[4], paramsC[5]);
                            dC = SWTaskpaneHost.DirTransform(dCloc, ref trC);
                        }

                        if (!entity_3_as_VERTEX)
                        {
                            Vector3D dDloc = new Vector3D(paramsD[3], paramsD[4], paramsD[5]);
                            dD = SWTaskpaneHost.DirTransform(dDloc, ref trD);
                        }

                        num_link++;
                        linkname = "link_" + num_link;
                        asciitext += String.Format(bz, "{0} = chrono.ChLinkMateXdistance()\n", linkname);

                        asciitext += String.Format(bz, "cA = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                  cC.X * ChScale.L,
                                  cC.Y * ChScale.L,
                                  cC.Z * ChScale.L);
                        asciitext += String.Format(bz, "dA = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                  dC.X, dC.Y, dC.Z);
                        asciitext += String.Format(bz, "cB = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                  cD.X * ChScale.L,
                                  cD.Y * ChScale.L,
                                  cD.Z * ChScale.L);
                        asciitext += String.Format(bz, "dB = chrono.ChVectorD({0:g},{1:g},{2:g})\n",
                                  dD.X, dD.Y, dD.Z);

                        asciitext += String.Format(bz, "{0}.SetName(\"{1}\")\n", linkname, swMateFeature.Name);

                        // Initialize link, by setting the two csys, in absolute space,
                        if (entity_2_as_VERTEX)
                            asciitext += String.Format(bz, "{0}.Initialize({1},{2},False,cA,cB,dA)\n", linkname, name3, name4);
                        else
                            asciitext += String.Format(bz, "{0}.Initialize({1},{2},False,cA,cB,dB)\n", linkname, name3, name4);

                        // Insert to a list of exported items
                        asciitext += String.Format(bz, "exported_items.append({0})\n", linkname);

                        return true;
                    }
                }
            }
            return false;
        }
 /// <summary>
 /// Transforms the inputed array by the inputted transform.
 /// </summary>
 /// <param name="trans">Transform to move the point by</param>
 /// <param name="Pt">Point to be Transformed</param>
 /// <returns>The transformed point</returns>
 public static double[] TransformPoint(MathTransform trans, double[] Pt)
 {
     double[] newPoint;
     if (Pt.Length > 3)
         newPoint = Pt.Take(3).ToArray();
     else
         newPoint = Pt;
     return RobotInfo.mathUtil.CreatePoint(newPoint).MultiplyTransform(trans).ArrayData;
 }
        public void PythonTraverseComponent_for_collshapes(Component2 swComp, long nLevel, ref  string asciitext, int nbody, ref MathTransform chbodytransform, ref bool found_collisionshapes, Component2 swCompBase)
        {
            // Look if component contains collision shapes (customized SW solid bodies):
            PythonTraverseFeatures_for_collshapes(swComp, nLevel, ref asciitext, nbody, ref chbodytransform, ref found_collisionshapes, swCompBase);

            // Recursive scan of subcomponents

            Component2 swChildComp;
            object[] vChildComp = (object[])swComp.GetChildren();

            for (long i = 0; i < vChildComp.Length; i++)
            {
                swChildComp = (Component2)vChildComp[i];

                PythonTraverseComponent_for_collshapes(swChildComp, nLevel + 1, ref asciitext, nbody, ref chbodytransform, ref found_collisionshapes, swCompBase);
            }
        }
        //
        // LINK EXPORTING FUNCTIONS
        //
        public void PythonTraverseComponent_for_links(Component2 swComp, long nLevel, ref  string asciitext, ref MathTransform roottrasf)
        {
            // Scan assembly features and save mating info

            if (nLevel > 1)
            {
                Feature swFeat = (Feature)swComp.FirstFeature();
                PythonTraverseFeatures_for_links(swFeat, nLevel, ref asciitext, ref roottrasf, ref swComp);
            }

            // Recursive scan of subassemblies

            object[] vChildComp;
            Component2 swChildComp;

            vChildComp = (object[])swComp.GetChildren();

            for (long i = 0; i < vChildComp.Length; i++)
            {
                swChildComp = (Component2)vChildComp[i];

                if (swChildComp.Solving == (int)swComponentSolvingOption_e.swComponentFlexibleSolving)
                    PythonTraverseComponent_for_links(swChildComp, nLevel + 1, ref asciitext, ref roottrasf);
            }
        }
 public static Vector3D DirTransform(Vector3D dir, ref MathTransform trasf)
 {
     Matrix3D M = GetMatrixFromMathTransform(ref trasf);
     M.OffsetX = 0; M.OffsetY = 0; M.OffsetZ = 0;
     return M.Transform(dir);
 }