Kean Walmsley


  • About the Author
    Kean on Google+

July 2014

Sun Mon Tue Wed Thu Fri Sat
    1 2 3 4 5
6 7 8 9 10 11 12
13 14 15 16 17 18 19
20 21 22 23 24 25 26
27 28 29 30 31    








« Creating a face-recognising security cam with a Raspberry Pi – Part 2 | Main | Jeremy Sawicki wins the ICFP Programming Contest 2012 »

September 21, 2012

Overriding the grips of an AutoCAD polyline to maintain fillet segments using .NET

Many, many thanks to Massimo Cicognani for contributing the code in today’s post. Massimo contacted me as he was working through some issues with his implementation and then kindly offered to share it with this blog’s readers.

We’ve looked at a few different types of overrule on this blog, in the past, and even taken a look at a grip overrule or two. Massimo’s much more advanced grip overrule works with a very particular type of polyline: those that alternate between straight and arc segments (with the first and last segments being straight). This might sound a touch specific, unless of course you’re working with such polylines on a regular basis to represent pipes, etc.

The code is split into two C# source files. The first contains the implementation of the overrule and the command to toggle it on and off.

using Autodesk.AutoCAD.Runtime;

using Autodesk.AutoCAD.ApplicationServices;

using Autodesk.AutoCAD.DatabaseServices;

using Autodesk.AutoCAD.Geometry;

using Autodesk.AutoCAD.GraphicsInterface;

using Autodesk.AutoCAD.EditorInput;

using System.Collections.Generic;

using System;

 

namespace comsPLine

{

  // Derived class for grip handling

 

  public class myGripData : GripData

  {

    // Object id of the original entity

 

    public ObjectId m_key = ObjectId.Null;

 

    // Progressive grip number

 

    public int m_index = 0;

 

    // Previous radius at this grip vertex

 

    public Double m_radius = 0.0;

 

    // Original grip location

 

    public Point3d m_original_point = Point3d.Origin;

 

    public override void OnGripStatusChanged(

      ObjectId entityId, GripData.Status newStatus

    )

    {

      if (newStatus == Status.GripAbort)

      {

        // Revert grips to original location

 

        GripVectorOverrule.ResetGrips(entityId);

      }

    }

 

    public override bool ViewportDraw(

      ViewportDraw worldDraw,

      ObjectId entityId,

      GripData.DrawType type,

      Point3d? imageGripPoint,

      int gripSizeInPixels

    )

    {

      // Calculate the size of the glyph in WCS

 

      Point2d glyphSize =

        worldDraw.Viewport.GetNumPixelsInUnitSquare(this.GripPoint);

      Double glyphHeight = (gripSizeInPixels / glyphSize.Y);

 

      // Transform to viewport

 

      Matrix3d e2w = worldDraw.Viewport.EyeToWorldTransform;

      Point3d pt = this.GripPoint.TransformBy(e2w);

 

      // Draw a glyph

 

      Point3dCollection pnts = new Point3dCollection();

      pnts.Add(

        new Point3d(pt.X - glyphHeight, pt.Y + glyphHeight, pt.Z)

      );

      pnts.Add(new Point3d(pt.X, pt.Y - glyphHeight, pt.Z));

      pnts.Add(

        new Point3d(pt.X + glyphHeight, pt.Y + glyphHeight, pt.Z)

      );

      pnts.Add(

        new Point3d(pt.X - glyphHeight, pt.Y + glyphHeight, pt.Z)

      );

 

      worldDraw.Geometry.DeviceContextPolygon(pnts);

 

      return false;

    }

  }

 

  // This class is instantiated by AutoCAD for each document when

  // a command is called by the user the first time in the context

  // of a given document. In other words, non static data in this

  // class is implicitly per-document!

 

  public class GripVectorOverrule : GripOverrule

  {

    // A static pointer to our overrule instance

 

    static public GripVectorOverrule theOverrule =

      new GripVectorOverrule();

 

    // A flag to indicate whether we're overruling

 

    static bool overruling = false;

 

    // gripdata for each selected polyline

 

    static Dictionary<ObjectId, GripDataCollection> _ents_handled =

      new Dictionary<ObjectId, GripDataCollection>();

 

 

    public GripVectorOverrule()

    {

      // Set event handlers on documents to get access to

      // OnImpliedSelectionChanged

 

      DocumentCollection dm = Application.DocumentManager;

 

      dm.DocumentCreated +=

        new DocumentCollectionEventHandler(dm_DocumentCreated);

      dm.DocumentToBeDestroyed +=

        new DocumentCollectionEventHandler(dm_DocumentToBeDestroyed);

 

      // Attach handler to currently loaded documents

 

      foreach (Document doc in dm)

      {

        doc.ImpliedSelectionChanged +=

          new EventHandler(doc_ImpliedSelectionChanged);

      }

    }

 

    void dm_DocumentCreated(

      object sender, DocumentCollectionEventArgs e

    )

    {

      e.Document.ImpliedSelectionChanged +=

        new EventHandler(doc_ImpliedSelectionChanged);

    }

 

    void dm_DocumentToBeDestroyed(

      object sender, DocumentCollectionEventArgs e

    )

    {

      e.Document.ImpliedSelectionChanged -=

        new EventHandler(doc_ImpliedSelectionChanged);

    }

 

    void doc_ImpliedSelectionChanged(object sender, EventArgs e)

    {

      // Check for empty selection on current document

 

      Document doc = Application.DocumentManager.MdiActiveDocument;

      PromptSelectionResult res = doc.Editor.SelectImplied();

 

      // If nothing selected, it's a good time to reset GripData

      // dictionary

 

      if (res != null)

        if (res.Value == null) GripVectorOverrule.ResetAllGrips();

    }

 

    /// <summary>

    /// Called when entity is first selected.

    /// Analyze it and return alternative grips data collection if

    /// we must handle it.

    /// </summary>

    /// <param name="entity"></param>

    /// <param name="grips"></param>

    /// <param name="curViewUnitSize"></param>

    /// <param name="gripSize"></param>

    /// <param name="curViewDir"></param>

    /// <param name="bitFlags"></param>

 

    public override void GetGripPoints(

      Entity entity,

      GripDataCollection grips,

      double curViewUnitSize,

      int gripSize,

      Vector3d curViewDir,

      GetGripPointsFlags bitFlags

    )

    {

      bool bValid = false;

 

      // get polyline object

 

      Autodesk.AutoCAD.DatabaseServices.Polyline p =

        entity as Autodesk.AutoCAD.DatabaseServices.Polyline;

 

      // check if the polyline is of the type we search for.

      // In the final version it may contain some xdata, maybe

      // holding a fixed radius value, to allow other plines to

      // work as expected.

      // However, we must always check if the geometry is still

      // valid. We should also check if the linear segments are

      // tangent to the arcs, but for the moment, we'll check if

      // we have a pattern of alternating line and arc segments,

      // starting with a line

 

      bValid = csMath.IsPolylineRoundedEdge(p);

 

      if (bValid)

      {

        // seems right, gather line segments

 

        LineSegment3d[] lsegs =

          new LineSegment3d[p.NumberOfVertices];

        int nSegs = 0;

 

        for (int i = 0; i < p.NumberOfVertices; i++)

        {

          if (p.GetSegmentType(i) == SegmentType.Line)

          {

            lsegs[nSegs++] = p.GetLineSegmentAt(i);

          }

        }

 

        // Gather start, end and intersection points

 

        Point3d[] pnts = new Point3d[p.NumberOfVertices];

 

        // Current radius for inner intersection

 

        Double[] rads = new Double[p.NumberOfVertices];

        int nPnts = 0;

 

        // At least two linear segments are needed

 

        if (nSegs > 1)

        {

          // Working plane for intersecting segments

 

          Plane pPlane = p.GetPlane();

 

          // Temporary intersecting point

 

          Point3d pnt = Point3d.Origin;

 

          // First point: if the polyline id closed, the first pt

          // is the intersection of the first and last segments

          if (p.Closed)

          {

            if (

              csMath.CheckIntersect(

                lsegs[0], lsegs[nSegs - 1], pPlane, ref pnt

              )

            )

            {

              pnts[nPnts] = pnt;

              rads[nPnts] =

                csMath.GetFilletRadius(

                  lsegs[0], lsegs[nSegs - 1], pPlane

                );

              nPnts++;

            }

            else

            {

              // Polyline with overlapping segments? Not good for us

 

              bValid = false;

            }

          }

          else

            pnts[nPnts++] = lsegs[0].StartPoint;

 

          // Add intersection points for internal segments

 

          for (int i = 0; i < (nSegs - 1); i++)

          {

            if (

              csMath.CheckIntersect(

                lsegs[i], lsegs[i + 1], pPlane, ref pnt

              )

            )

            {

              pnts[nPnts] = pnt;

              rads[nPnts] =

                csMath.GetFilletRadius(

                  lsegs[i], lsegs[i + 1], pPlane

                );

              nPnts++;

            }

            else

            {

              // No intersection, overlapping or co-linear segments?

 

              bValid = false;

            }

          }

 

          // Last point: add if not a closed pline

 

          if (!p.Closed) pnts[nPnts++] = lsegs[nSegs - 1].EndPoint;

 

          if (bValid) // Still valid?

          {

            // Everything seems ok, add grip points

            // Use also a private GripDataCollection, don't mess

            // with AutoCAD's

 

            GripDataCollection myGrips = new GripDataCollection();

 

            for (int i = 0; i < nPnts; i++)

            {

              myGripData gd = new myGripData();

              gd.m_index = i;

              gd.m_key = entity.ObjectId;

              gd.GripPoint = gd.m_original_point = pnts[i];

              gd.m_radius = rads[i];

              gd.GizmosEnabled = true;

              grips.Add(gd);

              myGrips.Add(gd);

            }

 

            // Check for same entity already in list. If so,

            // remove it

 

            _ents_handled.Remove(entity.ObjectId);

 

            // Add to our managed list

 

            _ents_handled.Add(entity.ObjectId, myGrips);

          }

        }

        else

        {

          bValid = false;

        }

      }

 

      if (!bValid)

      {

        // Polyline not good for us, let it be treated as usual

 

        base.GetGripPoints(

          entity, grips, curViewUnitSize,

          gripSize, curViewDir, bitFlags

        );

      }

    }

 

    /// <summary>

    /// Handler called during grip streching.

    /// Rebuild polyline on the fly given modified grips

    /// </summary>

    /// <param name="entity">Clone of the original entity to

    /// manage</param>

    /// <param name="grips">Altered grips</param>

    /// <param name="offset">Vector of displacement from original

    /// grip position</param>

    /// <param name="bitFlags"></param>

 

    public override void MoveGripPointsAt(

      Entity entity,

      GripDataCollection grips,

      Vector3d offset,

      MoveGripPointsFlags bitFlags

    )

    {

      // Retrieve from the streched grips the ObjectId/dictionary

      // key

 

      // shouldn't happen. Programmer's paranoia

 

      if (grips.Count == 0) return;

 

      myGripData gda = grips[0] as myGripData;

      if (gda != null) // It's one of our grips

      {

        if (_ents_handled.ContainsKey(gda.m_key))

        {

          // Retrieve our original grip collection

 

          GripDataCollection original_grips =

            _ents_handled[gda.m_key];

 

          // Correct grips with offset information

 

          foreach (myGripData gdo in grips)

          {

            // Retrieve original grip and set current

            // dragged location

 

            myGripData gd =

              original_grips[gdo.m_index] as myGripData;

 

            gd.GripPoint = gd.m_original_point + offset;

          }

 

          // Recalc polyline from new sets of grips

 

          csMath.RebuildPolyline(

            entity as Autodesk.AutoCAD.DatabaseServices.Polyline,

            original_grips

          );

 

          // Done, don't fall into standard handling

 

          return;

        }

      }

 

      // Revert to standard handling

 

      base.MoveGripPointsAt(entity, grips, offset, bitFlags);

    }

 

    /// <summary>

    /// Reset grip position for the selected entity.

    /// Revert position to initial location, useful when aborting

    /// a grip handling.

    /// </summary>

    /// <param name="entity_id">objectid/key of the selected

    /// entity</param>

 

    public static void ResetGrips(ObjectId entity_id)

    {

      // Reset grips to their original point

 

      if (_ents_handled.ContainsKey(entity_id))

      {

        GripDataCollection grips = _ents_handled[entity_id];

 

        foreach (myGripData gdo in grips)

        {

          gdo.GripPoint = gdo.m_original_point;

        }

      }

    }

 

    public static void ResetAllGrips()

    {

      // Clear handled list, to be called when selection is cleared

 

      _ents_handled.Clear();

    }

 

    [CommandMethod("GOO")]

    public void GripOverruleOnOff()

    {

      Document doc = Application.DocumentManager.MdiActiveDocument;

      Editor ed = doc.Editor;

 

      if (overruling)

      {

        ObjectOverrule.RemoveOverrule(

          RXClass.GetClass(

            typeof(Autodesk.AutoCAD.DatabaseServices.Polyline)

          ),

          GripVectorOverrule.theOverrule

        );

      }

      else

      {

        ObjectOverrule.AddOverrule(

          RXClass.GetClass(

            typeof(Autodesk.AutoCAD.DatabaseServices.Polyline)

          ),

          GripVectorOverrule.theOverrule,

          true

        );

      }

 

      overruling = !overruling;

 

      GripOverrule.Overruling = overruling;

 

      ed.WriteMessage(

        "\nGrip overruling turned {0}.",

        overruling ? "on" : "off"

      );

    }

  }

}

The second contains mathematical support functions – that Massimo has developed over many years, so it’s especially nice of him to share these (he even translated the bulk of the comments from Italian! :-).

// (C) Copyright 2008-2012 by COMSAL Srl - RSM

//

// COMSAL PROVIDES THIS PROGRAM "AS IS" AND WITH ALL FAULTS.

// COMSAL SPECIFICALLY DISCLAIMS ANY IMPLIED WARRANTY OF

// MERCHANTABILITY OR FITNESS FOR A PARTICULAR USE.

// COMSAL SRL DOES NOT WARRANT THAT THE OPERATION OF THE

// PROGRAM WILL BE UNINTERRUPTED OR ERROR FREE.

//

// Description:

// Classe di supporto per funzioni geometriche comuni

//

// History:

// 02.10.2008 [MC] Prima stesura

//

//

 

using System;

using System.Collections.Generic;

using System.Text;

 

using Autodesk.AutoCAD.DatabaseServices;

using Autodesk.AutoCAD.Geometry;

using Autodesk.AutoCAD.GraphicsSystem;

 

namespace comsPLine

{

  // Support class for non-trigonometric math

 

  public class csCosDir

  {

    public Double cx;

    public Double cy;

    public Double cz;

 

    public csCosDir()

    {

      cx = cy = cz = 0.0;

    }

 

    public csCosDir(Point2d p1, Point2d p2)

    {

      Double dx = p2.X - p1.X;

      Double dy = p2.Y - p1.Y;

      Double dd = Math.Sqrt(dx * dx + dy * dy);

 

      if (dd > csMath.dEpsilon)

      {

        cx = dx / dd;

        cy = dy / dd;

      }

    }

 

    public csCosDir(Point3d p1, Point3d p2)

    {

      Double dx = p2.X - p1.X;

      Double dy = p2.Y - p1.Y;

      Double dz = p2.Z - p1.Z;

      Double dd = Math.Sqrt(dx * dx + dy * dy + dz * dz);

 

      if (dd > csMath.dEpsilon)

      {

        cx = dx / dd;

        cy = dy / dd;

        cz = dz / dd;

      }

    }

  }

 

  // Generic info holder for polyline vertex with fillet

 

  public class myVertex

  {

    public Double radius; // Fillet radius (requested or real)

    public Point3d pOrg;  // Origin of the fillet arc

    public Point3d p1;    // Arc start point (end of 1st segment)

    public Point3d p2;    // Arc end point (start of 2nd segment)

    public Point3d pc;    // Arc mid point laying on the bisectrix

    public Double bulge;  // Bulge parameter as usual: B=2*H/D

 

    public myVertex()

    {

      pOrg = p1 = p2 = pc = Point3d.Origin;

      radius = bulge = 0.0;

    }

 

    public myVertex(Point3d pInit)

    {

      pOrg = p1 = p2 = pc = pInit;

      radius = bulge = 0.0;

    }

  }

 

  // Common math functions

 

  public static class csMath

  {

    public const Double PI =

      3.141592653589793; // More decimal places than Math.PI

    public const Double dEpsilon =

      0.001; // General precision allowed for identity

 

 

    /// <summary>

    /// Check if the polyline topology is compatible with a

    /// sequence of segments with internal rounded edges.

    /// Used to activate an alternate grip handling where

    /// user can control segment vertexes through apparent

    /// intersections.

    /// </summary>

    /// <param name="p">Polyline to check</param>

    /// <returns>Compatibility with alternate grip handling</returns>

 

    public static bool IsPolylineRoundedEdge(Polyline p)

    {

      bool result = false;

 

      if (p != null)

      {

        // Quick check if it contains at least one arc and two

        // segments, that means at least 4 points also check

        // for planar entity

 

        if (p.IsPlanar && p.HasBulges && (p.NumberOfVertices > 3))

        {

          SegmentType prevType = p.GetSegmentType(0);

 

          // First segment must be a line

 

          if (prevType == SegmentType.Line)

          {

            result = true;

            for (int i = 1; i < p.NumberOfVertices; i++)

            {

              SegmentType currType = p.GetSegmentType(i);

              if (

                currType == SegmentType.Line ||

                currType == SegmentType.Arc

              )

              {

                if (currType == prevType)

                {

                  result = false;

                  break;

                }

 

                prevType = currType;

              }

            }

 

            // Check if also the last segment is a line, or if

            // it's an arc, the polyline must be closed, like

            // a rounded box

 

            if (

              prevType == SegmentType.Arc && !p.Closed

            )

              result = false;

          }

        }

      }

      return result;

    }

 

    /// <summary>

    /// Rebuild polyline with new grip information

    /// </summary>

    /// <param name="polyline">Polyline to be rebuilt</param>

    /// <param name="original_grips">Grips collection</param>

 

    public static void RebuildPolyline(

      Polyline p, GripDataCollection grips

    )

    {

      try

      {

        int nPnt = grips.Count;

 

        // Create vertex info from gripdata collection

 

        myVertex[] vFill = new myVertex[nPnt];

 

        // Initializa first and last vertex with first and

        // last grip points

 

        vFill[0] = new myVertex(grips[0].GripPoint);

        vFill[nPnt - 1] =

          new myVertex(grips[grips.Count - 1].GripPoint);

 

        // Retrieve adjacent segments (three consecutive points)

 

        for (int i = 1; i < (nPnt - 1); i++)

        {

          myGripData gd_prev = grips[i - 1] as myGripData;

          myGripData gd_center = grips[i] as myGripData;

          myGripData gd_next = grips[i + 1] as myGripData;

 

          // New vertex info

 

          vFill[i] = new myVertex();

          vFill[i].radius = gd_center.m_radius;

 

          // Calc fillet information, if possible with current

          // radius and segments length

 

          if (

            !csMath.LinesFillet(

              gd_center.GripPoint,

              gd_prev.GripPoint,

              gd_next.GripPoint,

              ref vFill[i],

              true

            )

          )

          {

            // Unable to find a solution, remove fillet information

 

            vFill[i].p1 = vFill[i].p2 = vFill[i].pc =

              vFill[i].pOrg = gd_center.GripPoint;

            vFill[i].radius = 0.0;

          }

        }

 

        if (p.Closed)

        {

          // Add vertex information on last grip

 

          myGripData gd_prev = grips[grips.Count - 2] as myGripData;

          myGripData gd_center =

            grips[grips.Count - 1] as myGripData;

          myGripData gd_next = grips[0] as myGripData;

 

          // Last point may coincident with first or not

 

          if (

            gd_center.GripPoint.DistanceTo(gd_prev.GripPoint) <

            csMath.dEpsilon

          )

            gd_prev = grips[grips.Count - 3] as myGripData;

 

          vFill[nPnt - 1] = new myVertex();

          vFill[nPnt - 1].radius = gd_center.m_radius;

 

          // Calc fillet information, if possible with current

          // radius and segments length

 

          if (

            !csMath.LinesFillet(

              gd_center.GripPoint,

              gd_prev.GripPoint,

              gd_next.GripPoint,

              ref vFill[nPnt - 1],

              false)

          )

          {

            // Unable to find a solution, remove fillet information

 

            vFill[nPnt - 1].p1 = vFill[nPnt - 1].p2 =

              vFill[nPnt - 1].pc = vFill[nPnt - 1].pOrg =

                gd_center.GripPoint;

            vFill[nPnt - 1].radius = 0.0;

          }

 

          // Recalc vertex information on first grip

 

          gd_prev = grips[grips.Count - 1] as myGripData;

          gd_center = grips[0] as myGripData;

          gd_next = grips[1] as myGripData;

 

          // Last point may coincident with first or not

 

          if (

            gd_center.GripPoint.DistanceTo(gd_prev.GripPoint) <

            csMath.dEpsilon

          )

            gd_prev = grips[grips.Count - 2] as myGripData;

 

          vFill[0] = new myVertex();

          vFill[0].radius = gd_center.m_radius;

 

          // Calc fillet information, if possible with

          // current radius and segments length

 

          if (

            !csMath.LinesFillet(

              gd_center.GripPoint,

              gd_prev.GripPoint,

              gd_next.GripPoint,

              ref vFill[0],

              false

            )

          )

          {

            // Unable to find a solution, remove fillet information

            vFill[0].p1 = vFill[0].p2 = vFill[0].pc =

              vFill[0].pOrg = gd_center.GripPoint;

            vFill[0].radius = 0.0;

          }

        }

 

        // Everything seem ok, rebuild polyline

 

        bool bIsClosed = p.Closed; // remember if original was closed

 

        // Clear current points definitions

 

        p.Closed = false;

        while (p.NumberOfVertices > 1)

          p.RemoveVertexAt(0); // Cannot completely clear points

 

        // Add new points and segments definition

 

        for (int i = 0; i < (nPnt - 1); i++)

        {

          // Add linesegment only if lenght is not null

 

          if (

            vFill[i].p2.DistanceTo(vFill[i + 1].p1) >

            csMath.dEpsilon

          )

            p.AddVertexAt(

              p.NumberOfVertices,

              new Point2d(vFill[i].p2.X, vFill[i].p2.Y),

              0, 0, 0

            );

 

          // End point always valid with bulge information if needed

 

          p.AddVertexAt(

            p.NumberOfVertices,

            new Point2d(vFill[i + 1].p1.X, vFill[i + 1].p1.Y),

            vFill[i + 1].bulge, 0, 0

          );

        }

 

        p.RemoveVertexAt(0); // Remove last of old points

 

        // If closed re-add first point with bulge

 

        if (bIsClosed)

        {

          // Add linesegment only if length is not null

 

          if (

            vFill[nPnt - 1].p2.DistanceTo(vFill[0].p1) >

            csMath.dEpsilon

          )

            p.AddVertexAt(

              p.NumberOfVertices,

              new Point2d(

                vFill[nPnt - 1].p2.X,

                vFill[nPnt - 1].p2.Y

              ),

              0, 0, 0

            );

 

          // End point always valid with bulge information if needed

 

          p.AddVertexAt(

            p.NumberOfVertices,

            new Point2d(vFill[0].p1.X, vFill[0].p1.Y),

            vFill[0].bulge, 0, 0

          );

 

          p.Closed = true; // Restore closed status

        }

      }

      catch { }

    }

 

    /// <summary>

    /// Compute fillet information for two converging segments.

    /// The fillet information is returned through a myVertex class

    /// that will hold information about the arc start point on the

    /// first segment, the end point on the second segment, the arc

    /// origin and the arc midpoint on the bisetrix

    /// </summary>

    /// <param name="pOrg">Intersection point of the segments</param>

    /// <param name="p1">Start point of the first segment</param>

    /// <param name="p2">End point of the second segment</param>

    /// <param name="v">Info class with requested radius to be

    /// filled with fillet info</param>

    /// <returns>True if a fillet is possible, false if no solution

    /// can be found</returns>

 

    public static bool LinesFillet(

      Point3d pOrg,

      Point3d p1,

      Point3d p2,

      ref myVertex v,

      bool bAllowRadiusReduction

    )

    {

      try

      {

        bool bInversionOccured = false;

 

        // Check point validity

 

        Double d1 = pOrg.DistanceTo(p1);

        Double d2 = pOrg.DistanceTo(p2);

        Double dd = p1.DistanceTo(p2);

 

        if (d1 < csMath.dEpsilon)

          return false; // Segment p1-porg null

        if (d2 < csMath.dEpsilon)

          return false; // Segment p2-porg null

        if (dd < csMath.dEpsilon)

          return false; // Overlapping segments

        if (Math.Abs(dd - d1 - d2) < csMath.dEpsilon)

          return false; // Co-linear segments

        if (v.radius < csMath.dEpsilon)

          return false; // Radius too small

 

        // Sort points to keep the smaller angle always as p1-pOrg-p2

 

        if (csMath.DistFromLine(pOrg, p1, p2) < 0)

        {

          // Point p2 is on the left side of vector pOrg->p1

          // should be on the right: switch points

 

          Point3d pp = p2;

          p2 = p1;

          p1 = pp;

          bInversionOccured = true// Mark that inversion occurred

        }

 

        // Get sine/cosine coefficients

 

        csCosDir r1 = new csCosDir(pOrg, p1);

        csCosDir r2 = new csCosDir(pOrg, p2);

 

        // Get bisetrix where arc origin and midpoint lay

 

        csCosDir rb = new csCosDir();

 

        // Shouldn't happen, co-linear segments already checked

 

        if (!csMath.FindBisect(r1, r2, ref rb))

          return false;

 

        // The bisetrix and the projection of the arc origin on

        // either segment forms a rect triangle. Align segment

        // coefficient to X axis, so the radius would be aligned

        // to Y axis

 

        Double cxr = rb.cx * r1.cx + rb.cy * r1.cy;

        Double cyr = -rb.cx * r1.cy + rb.cy * r1.cx;

 

        // Get hypotenuse given the radius and sine coefficient

 

        Double ipo = v.radius / cyr;

 

        // Get other catete: distance from pOrg and the fillet

        // points on segments

 

        Double l1 = ipo * cxr;

 

        // If allowed, check if fillet point lays outside the

        // smallest segment lenght

 

        if (bAllowRadiusReduction)

        {

          Double dmin = (d1 < d2) ? d1 : d2;

 

          if (l1 > dmin)

          {

            // Reduce radius to keep fillet inside segment

 

            v.radius = dmin * cyr / cxr;

 

            // Use new radius for computation

 

            ipo = v.radius / cyr;

            l1 = ipo * cxr;

          }

        }

 

        // Given the length, get arc start and end points on

        // each segment. Beware of segment switch, if occurred

 

        if (bInversionOccured)

        {

          v.p2 =

            new Point3d(

              pOrg.X + l1 * r1.cx, pOrg.Y + l1 * r1.cy, 0.0

            );

          v.p1 =

            new Point3d(

              pOrg.X + l1 * r2.cx, pOrg.Y + l1 * r2.cy, 0.0

            );

        }

        else

        {

          v.p1 =

            new Point3d(

              pOrg.X + l1 * r1.cx, pOrg.Y + l1 * r1.cy, 0.0

            );

          v.p2 =

            new Point3d(

              pOrg.X + l1 * r2.cx, pOrg.Y + l1 * r2.cy, 0.0

            );

        }

 

        // Get arc midpoint on bisetrix

 

        v.pc =

          new Point3d(

            pOrg.X + (ipo - v.radius) * rb.cx,

            pOrg.Y + (ipo - v.radius) * rb.cy,

            0.0

          );

 

        // Get arc origin

 

        v.pOrg =

          new Point3d(

            pOrg.X + ipo * rb.cx, pOrg.Y + ipo * rb.cy, 0.0

          );

 

        // Compute bulge using the formula B = 2*H/D, where D is

        // the chord and H the distance of the chord midpoint

        // and the arc midpoint

 

        Double D = v.p1.DistanceTo(v.p2);

        Double H = Math.Abs(csMath.DistFromLine(v.p1, v.p2, v.pc));

        if (D > csMath.dEpsilon) v.bulge = 2 * H / D;

 

        // Bulge should be positive for counterclockwise arcs and

        // negative for clockwise. Adjust sign according with

        // segment order switch, if occurred

 

        if (!bInversionOccured) v.bulge = -v.bulge;

      }

      catch (Exception ex)

      {

        System.Diagnostics.Debug.WriteLine(

          "[LinesFillet]" + ex.Message

        );

        return false;

      }

 

      return true;

    }

 

    /// <summary>

    /// Get plausible fillet radius from two converging linesegments

    /// </summary>

    /// <param name="l1">First segment</param>

    /// <param name="l2">Second segment</param>

    /// <param name="pPlane">Working plane</param>

    /// <param name="pOut">Resulting point, if available</param>

    /// <returns></returns>

 

    public static Double GetFilletRadius(

      LineSegment3d l1, LineSegment3d l2, Plane pPlane

    )

    {

      Double result = 0.0;

 

      // Get 2d points on working plane

 

      Point2d p1 = l1.StartPoint.Convert2d(pPlane);

      Point2d p2 = l1.EndPoint.Convert2d(pPlane);

 

      Point2d q1 = l2.StartPoint.Convert2d(pPlane);

      Point2d q2 = l2.EndPoint.Convert2d(pPlane);

 

      Point2d pInt = Point2d.Origin;

 

      // Get intersection point

 

      IntersectLinesState res =

        IntersectLines(p1, p2, q1, q2, out pInt);

      if (

        res == IntersectLinesState.ApparentIntersect ||

        res == IntersectLinesState.RealIntersect

      )

      {

        // Get the endpoints closest to the intersection

 

        Point2d l1_end =

          p1.GetDistanceTo(pInt) < p2.GetDistanceTo(pInt) ? p1 : p2;

        Point2d l2_end =

          q1.GetDistanceTo(pInt) < q2.GetDistanceTo(pInt) ? q1 : q2;

 

        // Throw a perpendicular vector on the endpoints closest

        // to the intersection

 

        Line2d lp1 =

          new LineSegment2d(p1, p2).GetPerpendicularLine(l1_end);

        Line2d lp2 =

          new LineSegment2d(q1, q2).GetPerpendicularLine(l2_end);

 

        // Get intersection of projection lines (center of fillet,

        // if segments already have fillet)

 

        Point2d[] pInts = lp1.IntersectWith(lp2);

        if (pInts != null)

        {

          // Get distance of center of fillet from endpoints

 

          Double r1 = l1_end.GetDistanceTo(pInts[0]);

          Double r2 = l2_end.GetDistanceTo(pInts[0]);

 

          // If the two segments were already rounded, the two

          // distances should be the same and equals to the fillet

          // radius. If the segments have been moved or stretched,

          // the distances may differ. We take the lower radius

          // as the minimum fillet radius available

 

          result = Math.Min(r1, r2);

        }

      }

 

      return result;

    }

 

 

    /// <summary>

    /// Check intersection for two segments on plane pPlane

    /// </summary>

    /// <param name="l1">First segment</param>

    /// <param name="l2">Second segment</param>

    /// <param name="pPlane">Working plane</param>

    /// <param name="pOut">Resulting point, if available</param>

    /// <returns></returns>

 

    public static bool CheckIntersect(

      LineSegment3d l1,

      LineSegment3d l2,

      Plane pPlane,

      ref Point3d pOut

    )

    {

      bool result = false;

 

      // Get 2d points on working plane

 

      Point2d p1 = l1.StartPoint.Convert2d(pPlane);

      Point2d p2 = l1.EndPoint.Convert2d(pPlane);

 

      Point2d q1 = l2.StartPoint.Convert2d(pPlane);

      Point2d q2 = l2.EndPoint.Convert2d(pPlane);

 

      Point2d pInt = Point2d.Origin;

 

      IntersectLinesState res =

        IntersectLines(p1, p2, q1, q2, out pInt);

      if (

        res == IntersectLinesState.ApparentIntersect ||

        res == IntersectLinesState.RealIntersect

      )

      {

        pOut = new Point3d(pPlane, pInt);

        result = true;

      }

 

      return result;

    }

 

    /// <summary>

    /// Possible results for IntersectLines() function

    /// </summary>

 

    public enum IntersectLinesState

    {

      InvalidPoints = -1,    // Invalid points (coincident?)

      RealIntersect = 0,     // Real intersection found, pInt valid

      ApparentIntersect = 1, // Apparent inters. found, pInt valid

      NoIntersection = 2,    // Segments are parallel

      OverLapping = 3,       // Segments are overlapping

      Colinear = 4           // Segments are co-linear

    }

 

    /// <summary>

    /// Try to get intersection point of two segment.

    /// The intersection may be real or apparent.

    /// The resulting point is

    /// </summary>

    /// <param name="p1">Start point of first segment</param>

    /// <param name="p2">End point of first segment</param>

    /// <param name="q1">Start point of second segment</param>

    /// <param name="q2">End point of second segment</param>

    /// <param name="pInt">[out] Resulting intersection point</param>

    /// <returns>Result validity state</returns>

    ///

    public static IntersectLinesState IntersectLines(

      Point2d p1, Point2d p2, // First segment

      Point2d q1, Point2d q2, // Second segment

      out Point2d pInt        // Intersecting point

    )

    {

      IntersectLinesState result =

        IntersectLinesState.NoIntersection;

      pInt = Point2d.Origin;

 

      // Get sine/cosine coefficients for the two segments

 

      csCosDir r1 = new csCosDir(p1, p2);

      csCosDir r2 = new csCosDir(q1, q2);

 

      // Check coefficients, if points are coincident,

      // segments are null

 

      if (

        (r1.cx == 0.0 && r1.cy == 0.0) ||

        (r2.cx == 0.0 && r2.cy == 0.0)

      )

      {

        // Coincident points? Invalid segments data

 

        return IntersectLinesState.InvalidPoints;

      }

 

      // Intersection coefficients

 

      Double a1 = r1.cx, a2 = r1.cy;

      Double b1 = -r2.cx, b2 = -r2.cy;

      Double c1 = q1.X - p1.X, c2 = q1.Y - p1.Y;

 

      // Get denominator, if null, segments have same direction

 

      Double dden = a1 * b2 - a2 * b1;

 

      if (Math.Abs(dden) > csMath.dEpsilon)

      {

        // Valid denominator, lines are not parallels or co-linear

        // now, intersection linear parameter may be get either

        // from first or second segment. Do both to check for

        // real or apparent intersection.

 

        // Linear parameter for second segment

 

        Double tt = (c1 * b2 - c2 * b1) / dden;

 

        // Linear parameter for first segment

 

        Double vv = (c2 * a1 - c1 * a2) / dden;

 

        // Intersection point from first segment parameter

 

        pInt = new Point2d(q1.X + r2.cx * vv, q1.Y + r2.cy * vv);

 

        // To be 'real', intersection point must lay on both

        // segments (parameter from 0 to 1). Otherwise, we set

        // it as 'apparent'. Get normalized linear parameter

        // (at this stage denominator are intrinsicly valid,

        // no need to check for DIVBYZERO)

 

        tt = tt / p1.GetDistanceTo(p2);

        vv = vv / q1.GetDistanceTo(q2);

 

        // Check if both coefficients lay within 0 and 1

 

        if (

          tt > -csMath.dEpsilon && tt < (1 + csMath.dEpsilon) &&

          vv > -csMath.dEpsilon && vv < (1 + csMath.dEpsilon)

        )

        {

          result = IntersectLinesState.RealIntersect;

        }

        else

        {

          result = IntersectLinesState.ApparentIntersect;

        }

      }

      else

      {

        // Segments are parallel or co-linear (have same direction).

        // Check coefficients for a new connecting segment with

        // points taken from both original segment. If coefficients

        // are the same, segments are co-linear, otherwise are

        // parallel

 

        csCosDir rx;

 

        // Avoid coincident points

 

        if (p1.GetDistanceTo(q1) > csMath.dEpsilon)

          rx = new csCosDir(p1, q1);

        else

          rx = new csCosDir(p1, q2);

 

        if (Math.Abs(rx.cx - r1.cx) < csMath.dEpsilon &&

            Math.Abs(rx.cy - r1.cy) < csMath.dEpsilon

        )

        {

          // Same coefficient, segments lay on the same vector.

          // Check if there is any overlapping by checking distances

          // from the two ends of a segments and another end.

          // Sum of distances must be equal or higher than segment

          // length, otherwise they are partially overlapping

 

          Double ll = p2.GetDistanceTo(p1);

 

          bool bOver1 =

            q1.GetDistanceTo(p1) + q1.GetDistanceTo(p2) >

              ll + csMath.dEpsilon;

          bool bOver2 =

            q2.GetDistanceTo(p1) + q2.GetDistanceTo(p2) >

              ll + csMath.dEpsilon;

 

          result =

            bOver1 && bOver2 ?

              IntersectLinesState.Colinear :

              IntersectLinesState.OverLapping;

        }

        else

        {

          // Parallel segments, no intersection

 

          result = IntersectLinesState.NoIntersection;

        }

      }

 

      return result;

    }

 

    /// <summary>

    /// Given a vector going from L1 to L2, get the projected

    /// distance of point P from the vector.

    /// If distance is positive, the point is on the right side

    /// of the vector, if distance is negative, the point is on

    /// the left side of the vector.

    /// Function assume all points lay on the same plane

    /// </summary>

    /// <param name="l1">Vector origin</param>

    /// <param name="l2">Vector direction</param>

    /// <param name="p">Point to get distance from</param>

    /// <returns>Projected distance of P from vector L1->L2</returns>

 

    public static Double DistFromLine(

      Point3d l1, Point3d l2, Point3d p

    )

    {

      csCosDir c = new csCosDir(l1, l2);

 

      return (((p.Y - l1.Y) * c.cx) - ((p.X - l1.X) * c.cy));

    }

 

    /// <summary>

    /// Get bisetrix coefficients of two vectors.

    /// Vectors are provided as coefficients and the order must

    /// be given in counterclockwise order.

    /// This is a 2d computation, vectors must lay on the same plane

    /// </summary>

    /// <param name="r1">First vector</param>

    /// <param name="r2">Second vector</param>

    /// <param name="rb">[out]Resulting coefficients</param>

    /// <returns>True if bisetrix has been found, false if not

    /// (parallel vectors?)</returns>

 

    public static bool FindBisect(

      csCosDir r1, csCosDir r2, ref csCosDir rb

    )

    {

      bool result = true;

 

      // Coefficients may be considered as points of a limited

      // space that goes from -1 to 1

      // Origin 0,0 is the intersection of the two vectors and

      // origin of the bisetrix

 

      // Quick check for vectors (almost) co-linear

 

      Double diste =

        Math.Sqrt(

          (r2.cx - r1.cx) * (r2.cx - r1.cx) +

          (r2.cy - r1.cy) * (r2.cy - r1.cy)

        );

      if (diste < csMath.dEpsilon)

      {

        // Vectors have very similar coefficients, use alternate

        // algorithm for a borderline situation

 

        Double dcx = (r2.cx + r1.cx) * 0.5;

        Double dcy = (r2.cy + r1.cy) * 0.5;

        Double dd = Math.Sqrt(dcx * dcx + dcy * dcy);

 

        if (dd < csMath.dEpsilon)

        {

          // Really same coefficients, could be co-linear or

          // parallel but cannot say because we don't have

          // points on segments, just direction

 

          result = false;

        }

        else

        {

          // Denominator valid, use point distance from vector

          // to get position of the first vector

 

          Double distc =

            r1.cx * (r2.cy - r1.cy) - r1.cy * (r2.cx - r1.cx);

          Double dSign = (distc < 0) ? -1.0 : 1.0;

 

          rb.cx = dSign * dcx / dd;

          rb.cy = dSign * dcy / dd;

        }

      }

      else

      {

        // Vectors normally spaced, use simpler formula

 

        Double dcx = (r2.cx - r1.cx);

        Double dcy = (r2.cy - r1.cy);

        Double dd = Math.Sqrt(dcx * dcx + dcy * dcy);

 

        rb.cx = dcy / dd;

        rb.cy = -dcx / dd;

      }

 

      return result;

    }

  }

}

Before we show how the code works, let’s start by drawing the type of polyline the code will control:

Our basic polyline

Here are its standard grips:

With its normal grips

When we run the GOO command to toggle our overrule on and redisplay the grips, here’s what we see:

With overruled grips displayed

And here’s what happens when we manipulate one of the “corners” via its grip:

With an overruled grip in action

You can see how the overrule maintains the coherence of the overall polyline as you move the grip. Very cool stuff.

Thanks again for sharing this, Massimo! :-)

blog comments powered by Disqus

10 Random Posts