API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Nov 14th, 2022
45
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C# 35.59 KB | None | 0 0
raw download clone embed print report
  1. using Pathfinding;
  2. using UnityEngine;
  3. using System.Collections.Generic;
  4. using Pathfinding.Drawing;
  5. using Pathfinding.Util;
  6. using UnityEngine.Profiling;
  7. using Unity.Mathematics;
  8. using static Pathfinding.Drawing.Palette.Colorbrewer.Set1;
  9. using Palette = Pathfinding.Drawing.Palette.Colorbrewer.Set1;
  10. using Unity.Collections;
  11.  
  12. public class TurnPalette : VersionedMonoBehaviour {
  13.     public Transform start;
  14.     public Transform end;
  15.     public float curvature = 1;
  16.     public int steps = 3;
  17.     Seeker seeker;
  18.     public int maxCorners = 1000;
  19.     Filter filter = new Filter();
  20.     Follower.PIDFollowerState state;
  21.     public Follower.PIDFollower follower = Follower.PIDFollower.Default();
  22.     public float timeLimit = 10;
  23.     PathInterpolator interpolator = new PathInterpolator();
  24.  
  25.     protected override void Awake () {
  26.         base.Awake();
  27.         seeker = GetComponent<Seeker>();
  28.     }
  29.  
  30.     public override void DrawGizmos () {
  31.         if (!Application.isPlaying) {
  32.             AstarPath.FindAstarPath();
  33.             seeker = GetComponent<Seeker>();
  34.             Search();
  35.  
  36.             // Tangents((Vector2)start.position, start.localScale.x, (Vector2)end.position, end.localScale.x);
  37.             // Tangents((Vector2)start.position, start.localScale.x, (Vector2)end.position, -end.localScale.x);
  38.             // Tangents((Vector2)start.position, -start.localScale.x, (Vector2)end.position, end.localScale.x);
  39.             // Tangents((Vector2)start.position, -start.localScale.x, (Vector2)end.position, -end.localScale.x);
  40.  
  41.             // var buffer = new NativeList<TurnStraightTurnPath>(Allocator.Temp);
  42.             // Profiler.BeginSample("TurnPalette");
  43.             // var localToWorld = float4x4.TRS(transform.position, quaternion.EulerXZY(new float3(math.radians(90), 0, 0)), 1);
  44.             // var worldToLocal = math.inverse(localToWorld);
  45.  
  46.             // var startPos = math.transform(worldToLocal, start.position).xy;
  47.             // var endPos = math.transform(worldToLocal, end.position).xy;
  48.             // var startTangent = math.mul(worldToLocal, new float4(start.right, 0)).xy;
  49.             // var endTangent = math.mul(worldToLocal, new float4(end.right, 0)).xy;
  50.             // TurnStraightTurnPath.Calculate(
  51.             //  startPos, startTangent, math.min(start.localScale.x, math.length(endPos - startPos)*0.5f),
  52.             //  endPos, endTangent, math.min(end.localScale.x, math.length(endPos - startPos)*0.5f),
  53.             //  buffer
  54.             // );
  55.  
  56.             // buffer.Sort();
  57.  
  58.             // buffer.Add(new TurnStraightTurnPath {
  59.             //  length = math.length(endPos - startPos),
  60.             //  startCenter = startPos,
  61.             //  endCenter = endPos,
  62.             //  startRadius = 0,
  63.             //  endRadius = 0,
  64.             //  startAngle1 = math.atan2(startTangent.y, startTangent.x),
  65.             //  startAngle2 = math.atan2(endPos.y - startPos.y, endPos.x - startPos.x),
  66.             //  endAngle1 = math.atan2(endPos.y - startPos.y, endPos.x - startPos.x),
  67.             //  endAngle2 = math.atan2(endTangent.y, endTangent.x),
  68.             // });
  69.  
  70.  
  71.             // bool found = false;
  72.             // for (int i = 0; i < buffer.Length; i++) {
  73.             //  if (Validate(AstarPath.active.data.layerGridGraph, buffer[i], localToWorld)) {
  74.             //      using(Draw.WithMatrix(localToWorld)) {
  75.             //          using(Draw.WithColor(!found ? Palette.Green : Palette.Blue)) {
  76.             //              using(Draw.WithLineWidth(found ? 1 : 2)) {
  77.             //                  buffer[i].Plot();
  78.             //              }
  79.             //          }
  80.             //      }
  81.             //      found = true;
  82.             //  } else {
  83.             //      using(Draw.WithMatrix(localToWorld)) {
  84.             //          using(Draw.WithColor(Palette.Red)) {
  85.             //              using(Draw.WithLineWidth(1)) {
  86.             //                  buffer[i].Plot();
  87.             //              }
  88.             //          }
  89.             //      }
  90.             //  }
  91.             // }
  92.             // Profiler.EndSample();
  93.         }
  94.     }
  95.  
  96.     public static bool ValidateInsideFunnel<C>(Funnel.FunnelPortals funnelPortals, C curve, int startSegment, int endSegment) where C : Follower2.ICurve {
  97.         if (funnelPortals.left == null || funnelPortals.right == null) return true;
  98.  
  99.         var nextExpectedSegment = startSegment + 1;
  100.         for (int k = 0; k < 10; k++) {
  101.             var a = curve.Eval(k/10.0f);
  102.             var b = curve.Eval((k+1)/10.0f);
  103.             for (int j = startSegment + 1; j <= endSegment; j++) {
  104.                 var sa = funnelPortals.left[j];
  105.                 var sb = funnelPortals.right[j];
  106.  
  107.                 if (VectorMath.LineIntersectionFactorXZ(a, b, sa, sb, out float factor1, out float factor2)) {
  108.                     if (factor1 >= 0 && factor1 <= 1) {
  109.                         if ((factor2 < 0 || factor2 > 1)) {
  110.                             // The curve intersects the portal outside the required range. This is invalid.
  111.                             Draw.Line(sa + Vector3.up*0.1f, sb + Vector3.up*0.1f, Color.red);
  112.                             return false;
  113.                         } else {
  114.                             if (j == nextExpectedSegment) {
  115.                                 nextExpectedSegment++;
  116.                             }
  117.                             Draw.Line(sa + Vector3.up*0.1f, sb + Vector3.up*0.1f, Color.green);
  118.                         }
  119.                     }
  120.                 }
  121.             }
  122.         }
  123.  
  124.         return nextExpectedSegment == endSegment || nextExpectedSegment == endSegment + 1;
  125.     }
  126.  
  127.     static bool Validate (IRaycastableGraph graph, TurnStraightTurnPath turn, float4x4 localToWorld) {
  128.         var a = math.transform(localToWorld, new float3(turn.straightStart, 0));
  129.         var b = math.transform(localToWorld, new float3(turn.straightEnd, 0));
  130.  
  131.         if (graph.Linecast(a, b)) {
  132.             // Draw.Line(a, b, Palette.Red);
  133.             return false;
  134.         } else {
  135.             // Draw.Line(a, b, Palette.Green);
  136.         }
  137.         return true;
  138.     }
  139.  
  140.     public struct TurnStraightTurnPath : System.IComparable<TurnStraightTurnPath>, Follower2.ICurve {
  141.         public float length;
  142.         public float additionalCost;
  143.         public float2 startCenter;
  144.         public float2 endCenter;
  145.         public float startRadius;
  146.         public float endRadius;
  147.         public float startAngle1;
  148.         public float startAngle2;
  149.         public float endAngle1;
  150.         public float endAngle2;
  151.  
  152.         public float2 straightStart {
  153.             get
  154.             {
  155.                 return startCenter + startRadius * new float2(math.cos(startAngle2), math.sin(startAngle2));
  156.             }
  157.         }
  158.  
  159.         public float2 straightEnd {
  160.             get
  161.             {
  162.                 return endCenter + endRadius * new float2(math.cos(endAngle1), math.sin(endAngle1));
  163.             }
  164.         }
  165.  
  166.         public float arcLength => length;
  167.  
  168.         public void Plot () {
  169.             Draw.xy.Circle(new float3(startCenter, 0), startRadius, startAngle1, startAngle2);
  170.             Draw.Line(new float3(straightStart, 0), new float3(straightEnd, 0));
  171.             Draw.xy.Circle(new float3(endCenter, 0), endRadius, endAngle1, endAngle2);
  172.         }
  173.  
  174.         public static void CalculateSingleArc (float2 startPos, float2 startTangent, float2 endPos, NativeList<TurnStraightTurnPath> buffer) {
  175.             var arc = CalculateSingleArc(startPos, startTangent, endPos);
  176.             if (arc != null) {
  177.                 buffer.Add(arc.Value);
  178.             }
  179.         }
  180.  
  181.         public static TurnStraightTurnPath? CalculateBiarc (float2 startPos, float2 startTangent, float2 endPos, float2 endTangent) {
  182.             startTangent = math.normalizesafe(startTangent);
  183.             endTangent = math.normalizesafe(endTangent);
  184.  
  185.             var dir = (endPos - startPos)*0.5f;
  186.             var dirAngle = math.atan2(dir.y, dir.x);
  187.             var startAngle = math.atan2(startTangent.y, startTangent.x);
  188.             var endAngle = math.atan2(endTangent.y, endTangent.x);
  189.  
  190.             var halfDist = math.length(dir);
  191.             var p = 1;
  192.             var omega = ((startAngle - dirAngle) + (endAngle - dirAngle)) * 0.5f;
  193.             var sinOmega = math.sin(omega);
  194.             var startCurvature = math.rcp(halfDist) * (math.sin(startAngle - dirAngle) + math.rcp(p)*sinOmega);
  195.             var endCurvature = math.rcp(halfDist) * (math.sin(endAngle - dirAngle) + p*sinOmega);
  196.  
  197.             var startSignedRadius = math.rcp(startCurvature);
  198.             var endSignedRadius = math.rcp(endCurvature);
  199.             var startNormal = new float2(startTangent.y, -startTangent.x);
  200.             var endNormal = new float2(endTangent.y, -endTangent.x);
  201.             var startCenter = startPos + startNormal * startSignedRadius;
  202.             var endCenter = endPos + endNormal * startSignedRadius;
  203.  
  204.             var startAngle1 = startAngle + math.sign(startSignedRadius)*math.PI*0.5f;
  205.             var startDeltaAngle = startCurvature;
  206.             return new TurnStraightTurnPath {
  207.                        startCenter = startCenter,
  208.                        endCenter = endCenter,
  209.                        startAngle1 = startAngle1,
  210.                        startAngle2 = startAngle + math.sign(startSignedRadius)*math.PI*0.5f,
  211.                        // endAngle1 = startAngle + halfDeltaAngle,
  212.                        // endAngle2 = startAngle + 2*halfDeltaAngle,
  213.                        // startRadius = math.abs(startSignedRadius),
  214.                        // endRadius = math.abs(endSignedRadius),
  215.                        // length = radius * 2*math.abs(halfDeltaAngle),
  216.             };
  217.         }
  218.  
  219.         public static TurnStraightTurnPath? CalculateSingleArc (float2 startPos, float2 startTangent, float2 endPos) {
  220.             var d = (endPos - startPos) * 0.5f;
  221.             var cosAlpha = math.dot(startTangent, d) / math.sqrt(math.lengthsq(startTangent) * math.lengthsq(d));
  222.             if (cosAlpha < 0) {
  223.                 return null;
  224.             }
  225.  
  226.             var side = math.sign(d.x*startTangent.y - d.y*startTangent.x);
  227.             var sinAlpha = math.sqrt(1 - cosAlpha*cosAlpha) * side;
  228.  
  229.             var tanInvAlpha = cosAlpha/sinAlpha;
  230.             var centerRelativeToStart = d + new float2(d.y, -d.x) * tanInvAlpha;
  231.             var radius = math.length(centerRelativeToStart);
  232.             var startAngle = math.atan2(startTangent.y, startTangent.x) + side*0.5f*math.PI;
  233.             var halfDeltaAngle = -math.asin(sinAlpha);
  234.             var center = startPos + centerRelativeToStart;
  235.  
  236.             using (Draw.WithMatrix(float4x4.TRS(float3.zero, quaternion.EulerXZY(new float3(math.radians(90), 0, 0)), 1))) {
  237.                 // Draw.xy.Circle(new float3(center, 0), radius, Color.red);
  238.                 // Draw.Line(new float3(startPos, 0), new float3(endPos, 0), Color.red);
  239.                 // Draw.Line(new float3((startPos + d), 0), new float3(center, 0), Color.magenta);
  240.             }
  241.  
  242.             return new TurnStraightTurnPath {
  243.                        startCenter = center,
  244.                        endCenter = center,
  245.                        // Note: it's not strictly necessary to spread out the progress over both the start circle and end circle
  246.                        // but it makes for a slightly more well behaved derivative function with respect to t.
  247.                        startAngle1 = startAngle,
  248.                        startAngle2 = startAngle + halfDeltaAngle,
  249.                        endAngle1 = startAngle + halfDeltaAngle,
  250.                        endAngle2 = startAngle + 2*halfDeltaAngle,
  251.                        startRadius = radius,
  252.                        endRadius = radius,
  253.                        length = radius * 2*math.abs(halfDeltaAngle),
  254.             };
  255.         }
  256.  
  257.         public static TurnStraightTurnPath? Calculate (float2 startCircleCenter, float startRadius, float2 endCircleCenter, float endRadius, float startAngle, float endAngle, int startDir, int endDir) {
  258.             // Draw.xy.Circle(new float3(c1, 0), startRadius, Color.black);
  259.             // Draw.xy.Circle(new float3(c2, 0), endRadius, Red);
  260.             var normal = Tangents(startCircleCenter, startRadius * startDir, endCircleCenter, endRadius * endDir);
  261.  
  262.             if (normal == null) return null;
  263.             var startNormal = normal.Value * startRadius * startDir;
  264.             var endNormal = normal.Value * endRadius * endDir;
  265.             // Draw.Line((Vector2)(c1 + startNormal), (Vector2)(c2 + endNormal), colors[(startDir+1) + (endDir+1)/2]);
  266.  
  267.             var sAngle1 = startAngle;
  268.             var sAngle2 = math.atan2(startNormal.y, startNormal.x);
  269.             var eAngle1 = math.atan2(endNormal.y, endNormal.x);
  270.             var eAngle2 = endAngle;
  271.  
  272.             OrderAngles(sAngle1, ref sAngle2, startDir == -1);
  273.             OrderAngles(eAngle1, ref eAngle2, endDir == -1);
  274.             var startArc = math.abs(sAngle2 - sAngle1);
  275.             var endArc = math.abs(eAngle2 - eAngle1);
  276.             var straightDistance = math.length((startCircleCenter + startNormal) - (endCircleCenter + endNormal));
  277.             var totalLength = startArc * startRadius + endArc * endRadius + straightDistance;
  278.  
  279.             return new TurnStraightTurnPath {
  280.                        length = totalLength,
  281.                        startCenter = startCircleCenter,
  282.                        endCenter = endCircleCenter,
  283.                        startAngle1 = sAngle1,
  284.                        startAngle2 = sAngle2,
  285.                        endAngle1 = eAngle1,
  286.                        endAngle2 = eAngle2,
  287.                        startRadius = startRadius,
  288.                        endRadius = endRadius,
  289.             };
  290.         }
  291.  
  292.         public static void Calculate (float2 startPos, float2 startTangent, float startRadius, float2 endPos, float2 endTangent, float endRadius, NativeList<TurnStraightTurnPath> buffer) {
  293.             var startRight = new float2(startTangent.y, -startTangent.x);
  294.             var endRight = new float2(endTangent.y, -endTangent.x);
  295.  
  296.             // Draw.Line((Vector2)startPos, (Vector2)(startPos + normal*r1), Purple);
  297.             // Draw.Line((Vector2)c2, (Vector2)(c2 + normal*r2), Purple);
  298.             // Draw.Line((Vector2)(startPos + normal*r1), (Vector2)(c2 + normal*r2), Purple);
  299.  
  300.  
  301.             // RR, RL, LR, LL
  302.             var startAngle = math.atan2(startTangent.y, startTangent.x);
  303.             var endAngle = math.atan2(endTangent.y, endTangent.x);
  304.             var colors = new[] { Red, Orange, Blue, Purple };
  305.  
  306.             for (var startDir = -1; startDir <= 1; startDir += 2) {
  307.                 for (var endDir = -1; endDir <= 1; endDir += 2) {
  308.                     var c1 = startPos - startRight * startDir * startRadius;
  309.                     var c2 = endPos - endRight * endDir * endRadius;
  310.                     var turn = Calculate(
  311.                         c1, startRadius,
  312.                         c2, endRadius,
  313.                         startAngle - math.PI * 0.5f * startDir,
  314.                         endAngle - math.PI * 0.5f * endDir,
  315.                         startDir,
  316.                         endDir
  317.                         );
  318.                     if (turn != null) {
  319.                         buffer.Add(turn.Value);
  320.                     }
  321.  
  322.                     // Debug.Log(startDir + " " + endDir + " " + math.degrees(l1) + " " + math.degrees(l2) + ": " + math.degrees(startAngle - math.PI*0.5f*startDir) + " " + math.degrees(math.atan2(startNormal.y, startNormal.x)));
  323.                     // Draw.xy.Circle(new float3(c1, -0.1f), startRadius, startAngle - math.PI*0.5f*startDir, math.atan2(startNormal.y, startNormal.x), colors[(startDir+1) + (endDir+1)/2]);
  324.                 }
  325.             }
  326.         }
  327.  
  328.         public float Cost {
  329.             get {
  330.                 if (math.all(this.startCenter == this.endCenter)) {
  331.                     // Single arc
  332.                     return this.length * 0.9f + additionalCost;
  333.                 } else {
  334.                     return this.length + additionalCost;
  335.                 }
  336.             }
  337.         }
  338.  
  339.         public int CompareTo (TurnStraightTurnPath other) {
  340.             return this.Cost.CompareTo(other.Cost);
  341.         }
  342.  
  343.         public Vector3 Eval (float t) {
  344.             t = math.clamp(t, 0, 1);
  345.             if (t < 0.25f) {
  346.                 t = t * 4;
  347.                 float angle = math.lerp(startAngle1, startAngle2, t);
  348.                 return new Vector3(this.startCenter.x, 0, this.startCenter.y) + new Vector3(math.cos(angle) * startRadius, 0, math.sin(angle) * startRadius);
  349.             } else if (t < 0.75f) {
  350.                 t = (t - 0.25f)*2;
  351.                 var p = math.lerp(this.straightStart, this.straightEnd, t);
  352.                 return new Vector3(p.x, 0, p.y);
  353.             } else {
  354.                 t = (t - 0.75f)*4;
  355.                 float angle = math.lerp(endAngle1, endAngle2, t);
  356.                 return new Vector3(this.endCenter.x, 0, this.endCenter.y) + new Vector3(math.cos(angle) * endRadius, 0, math.sin(angle) * endRadius);
  357.             }
  358.         }
  359.  
  360.         public void Plot (Color color) {
  361.             using (Draw.WithColor(color)) {
  362.                 this.Plot();
  363.             }
  364.         }
  365.  
  366.         public Vector3 Derivative (float t) {
  367.             t = math.clamp(t, 0, 1);
  368.             if (t < 0.25f && startAngle2 != startAngle1) {
  369.                 var duDt = 4;
  370.                 var u = t * duDt;
  371.                 float angle = math.lerp(startAngle1, startAngle2, u);
  372.                 var dangleDu = startAngle2 - startAngle1;
  373.                 var dpDangle = new Vector3(-math.sin(angle) * startRadius, 0, math.cos(angle) * startRadius);
  374.                 return dpDangle * dangleDu * duDt;     // = dp/dt
  375.             } else if (t < 0.75f) {
  376.                 var duDt = 2;
  377.                 var d = (this.straightEnd - this.straightStart)*duDt;
  378.                 return new Vector3(d.x, 0, d.y);
  379.             } else {
  380.                 var duDt = 4;
  381.                 var u = (t - 0.75f) * duDt;
  382.                 float angle = math.lerp(endAngle1, endAngle2, u);
  383.                 var dangleDu = endAngle2 - endAngle1;
  384.                 var dpDangle = new Vector3(-math.sin(angle) * endRadius, 0, math.cos(angle) * endRadius);
  385.                 return dpDangle * dangleDu * duDt;     // = dp/dt
  386.             }
  387.         }
  388.  
  389.         public Vector3 SecondDerivative (float t) {
  390.             t = math.clamp(t, 0, 1);
  391.             if (t < 0.25f && startAngle2 != startAngle1) {
  392.                 var duDt = 4;
  393.                 var u = t * duDt;
  394.                 float angle = math.lerp(startAngle1, startAngle2, u);
  395.                 var dangleDu = startAngle2 - startAngle1;
  396.                 var dpDangle2 = new Vector3(-math.cos(angle) * startRadius, 0, -math.sin(angle) * startRadius);
  397.                 Debug.Log(t + " " + startAngle1 + " " + startAngle2 + " " + startRadius + " " + dpDangle2 + " " + dangleDu + " " + duDt);
  398.                 return dpDangle2 * dangleDu * dangleDu * duDt * duDt;     // = dp/dt^2
  399.             } else if (t < 0.75f) {
  400.                 return Vector3.zero;
  401.             } else {
  402.                 var duDt = 4;
  403.                 var u = (t-0.75f) * duDt;
  404.                 float angle = math.lerp(endAngle1, endAngle2, u);
  405.                 var dangleDu = endAngle2 - endAngle1;
  406.                 var dpDangle2 = new Vector3(-math.cos(angle) * endRadius, 0, -math.sin(angle) * endRadius);
  407.                 return dpDangle2 * dangleDu * dangleDu * duDt * duDt;     // = dp/dt^2
  408.             }
  409.         }
  410.  
  411.         public Vector3 IntegrateCurvature (float arcLength) {
  412.             var lstart = Mathf.Abs(startAngle2 - startAngle1) * startRadius;
  413.             var lend = Mathf.Abs(endAngle2 - endAngle1) * endRadius;
  414.             var lstraight = this.length - lstart - lend;
  415.             var cstart = Curvature(0);
  416.             var cend = Curvature(1);
  417.             Debug.Log("Integrate " + arcLength + " of " + length + " (" + lstart + ", " + lstraight + " " + lend + ") " + Mathf.Clamp01(arcLength / lstart) + " " + Mathf.Clamp01((arcLength - lstart - lstraight) / lend));
  418.             return
  419.                 cstart * (Mathf.Clamp01(arcLength / lstart) * lstart)
  420.                 + cend * (Mathf.Clamp01((arcLength - lstart - lstraight) / lend) * lend);
  421.         }
  422.  
  423.         public Vector3 Curvature (float t) {
  424.             // TODO: Can be much simpler based on the start/end radii
  425.             var xp = Derivative(t);
  426.             var xpp = SecondDerivative(t);
  427.             var dx = xp.magnitude;
  428.  
  429.             if (dx < 0.000001f) return Vector3.zero;
  430.             return Vector3.Cross(xp, xpp) / (dx*dx*dx);
  431.         }
  432.  
  433.         public Follower2.ICurve FitWithStartPoint (Vector3 startPoint, Vector3 normalizedTangent) {
  434.             if (math.all(this.startCenter == this.endCenter)) {
  435.                 // Single arc
  436.                 var v = CalculateSingleArc(new float3(startPoint).xz, new float3(normalizedTangent).xz, new float3(this.Eval(1)).xz);
  437.                 if (v != null) return v.Value;
  438.             }
  439.  
  440.             var tangentAngle = math.atan2(normalizedTangent.z, normalizedTangent.x);
  441.             var normal = new float2(normalizedTangent.z, -normalizedTangent.x);
  442.             var startDir = this.startAngle2 > this.startAngle1 ? 1 : -1;
  443.             var endDir = this.endAngle2 > this.endAngle1 ? 1 : -1;
  444.             var startAngle = tangentAngle - startDir * math.PI * 0.5f;
  445.  
  446.             if (startDir == 1) normal = -normal;
  447.             var startCenter = new float3(startPoint).xz + normal * startRadius;
  448.             var curve = Calculate(startCenter, startRadius, endCenter, endRadius, startAngle, endAngle2, startDir, endDir);
  449.  
  450.             if (curve != null) {
  451.                 return curve;
  452.             } else {
  453.                 var maxRad = math.length(endCenter - startCenter) * 0.25f;
  454.                 return Calculate(startCenter, maxRad, endCenter, maxRad, startAngle, endAngle2, startDir, endDir);
  455.             }
  456.         }
  457.     }
  458.  
  459.     static void OrderAngles (float a1, ref float a2, bool cw) {
  460.         var delta = AngleDifference(a1, a2, cw);
  461.  
  462.         a2 = a1 + delta * (cw ? -1 : 1);
  463.     }
  464.  
  465.     static float AngleDifference (float a1, float a2, bool cw) {
  466.         if (cw) {
  467.             var tmp = a1;
  468.             a1 = a2;
  469.             a2 = tmp;
  470.         }
  471.         var delta = a2 - a1;
  472.  
  473.         delta = math.fmod(delta, math.PI * 2);
  474.         if (delta < 0.0f) delta += math.PI * 2;
  475.         return delta;
  476.     }
  477.  
  478.     void Update () {
  479.         Search();
  480.     }
  481.  
  482.     void Search () {
  483.         var p = seeker.StartPath(this.start.position, this.end.position, null);
  484.  
  485.         p.BlockUntilCalculated();
  486.  
  487.         seeker.startEndModifier.Apply(p);
  488.  
  489.         if (p.error) return;
  490.         filter.path = p;
  491.         bool filterIsRequired = filter.path.traversalProvider != null || filter.path.enabledTags != ~0;
  492.         var filterDelegate = filterIsRequired ? filter.cachedDelegate : null;
  493.         var result = GridStringPulling.Calculate(p.path, 0, p.path.Count - 1, p.vectorPath[0], p.vectorPath[p.vectorPath.Count - 1], (GraphNode node) => 0, filterDelegate, maxCorners);
  494.  
  495.         p.Claim(this);
  496.         p.Release(this);
  497.  
  498.         var localToWorld = float4x4.TRS(transform.position, quaternion.EulerXZY(new float3(math.radians(90), 0, 0)), 1);
  499.         var worldToLocal = math.inverse(localToWorld);
  500.  
  501.         var shortestPath = new List<float2>(result.Count);
  502.         for (int i = 0; i < result.Count; i++) {
  503.             shortestPath.Add(math.transform(worldToLocal, result[i]).xy);
  504.         }
  505.         var startTangent = math.mul(worldToLocal, new float4(this.start.forward, 0)).xy;
  506.         var endTangent = math.mul(worldToLocal, new float4(this.end.forward, 0)).xy;
  507.         var funnelPortals = new Funnel.FunnelPortals {
  508.             left = null,
  509.             right = null,
  510.         };
  511.         FindBestCurves(shortestPath, funnelPortals, this.start.localScale.x, startTangent, endTangent, 0, 4, AstarPath.active.data.layerGridGraph, localToWorld);
  512.     }
  513.  
  514.     public static List<TurnStraightTurnPath> FindBestCurves (List<float2> shortestPath, Funnel.FunnelPortals funnelPortals, float turningRadius, float2 startTangent, float2 endTangent, int startIndex, int maxCorners, IRaycastableGraph graph, float4x4 localToWorld) {
  515.         var n = Mathf.Min(maxCorners, shortestPath.Count - startIndex);
  516.         var lowestCosts = ArrayPool<float>.Claim(n);
  517.         var positions = ArrayPool<float2>.Claim(n);
  518.         var tangents = ArrayPool<float2>.Claim(n);
  519.         var tangentPriors = ArrayPool<float2>.Claim(n);
  520.         var best = ArrayPool<TurnStraightTurnPath>.Claim(n);
  521.         var parents = ArrayPool<int>.Claim(n);
  522.  
  523.         for (int i = 1; i < lowestCosts.Length; i++) lowestCosts[i] = float.PositiveInfinity;
  524.         lowestCosts[0] = 0;
  525.  
  526.         for (int i = startIndex; i < startIndex + n; i++) {
  527.             float2 tangent;
  528.             if (i == 0) tangent = shortestPath[startIndex + i + 1] - shortestPath[i];
  529.             else if (i == shortestPath.Count - 1) tangent = shortestPath[i] - shortestPath[i - 1];
  530.             else tangent = math.normalizesafe(shortestPath[i + 1] - shortestPath[i]) - math.normalizesafe(shortestPath[i - 1] - shortestPath[i]);
  531.  
  532.             tangent = math.normalizesafe(tangent);
  533.             tangentPriors[i - startIndex] = tangent; // math.mul(worldToLocal, new float4(tangent, 0)).xy;
  534.             positions[i - startIndex] = shortestPath[i]; // math.transform(worldToLocal, shortestPath[i]).xy;
  535.         }
  536.  
  537.         tangentPriors[0] = startTangent;
  538.         if (n == shortestPath.Count - startIndex) {
  539.             tangentPriors[n - 1] = endTangent;
  540.         }
  541.  
  542.         var buffer = new NativeList<TurnStraightTurnPath>(Allocator.Temp);
  543.  
  544.         for (int i = 0; i < n; i++) {
  545.             const int MaxForwardStep = 3;
  546.             for (int j = i + 1; j < Mathf.Min(i + MaxForwardStep + 1, n); j++) {
  547.                 buffer.Clear();
  548.                 var startPos = positions[i];
  549.                 var endPos = positions[j];
  550.                 var radius1 = math.min(turningRadius, math.length(endPos - startPos) * 0.25f);
  551.                 var radius2 = math.min(turningRadius, math.length(endPos - startPos) * 0.25f);
  552.                 var tangent = i > 0 ? math.normalizesafe(new float3(best[i].Derivative(1)).xz) : tangentPriors[i];
  553.                 TurnStraightTurnPath.Calculate(
  554.                     startPos, tangent, radius1,
  555.                     endPos, tangentPriors[j], radius2,
  556.                     buffer
  557.                     );
  558.  
  559.                 var singleArc = TurnPalette.TurnStraightTurnPath.CalculateSingleArc(
  560.                     startPos,
  561.                     tangent,
  562.                     endPos
  563.                     );
  564.                 if (singleArc.HasValue) {
  565.                     var arc = singleArc.Value;
  566.                     // +0% if in the right direction, +100% if 90 degree wrong, +200% if completely wrong direction.
  567.                     arc.additionalCost = arc.Cost * (1 - math.dot(math.normalizesafe(new float3(arc.Derivative(1.0f)).xz), tangentPriors[j]));
  568.                     buffer.Add(arc);
  569.                 }
  570.  
  571.                 if (j == i + 1) {
  572.                     buffer.Add(new TurnStraightTurnPath {
  573.                         length = math.length(endPos - startPos) * 2 + 1,
  574.                         startCenter = startPos,
  575.                         endCenter = endPos,
  576.                         startRadius = 0,
  577.                         endRadius = 0,
  578.                         startAngle1 = math.atan2(tangent.y, tangent.x),
  579.                         startAngle2 = math.atan2(endPos.y - startPos.y, endPos.x - startPos.x),
  580.                         endAngle1 = math.atan2(endPos.y - startPos.y, endPos.x - startPos.x),
  581.                         endAngle2 = math.atan2(tangentPriors[j].y, tangentPriors[j].x),
  582.                     });
  583.                 }
  584.  
  585.                 buffer.Sort();
  586.  
  587.                 for (int k = 0; k < buffer.Length; k++) {
  588.                     var costAtEnd = lowestCosts[i] + buffer[k].Cost;
  589.                     if (costAtEnd < lowestCosts[j]) {
  590.                         if (ValidateInsideFunnel(funnelPortals, buffer[k], startIndex + i, startIndex + j) && (graph == null || Validate(graph, buffer[k], localToWorld))) {
  591.                             using (Draw.WithMatrix(localToWorld))
  592.                             {
  593.                                 // using (Draw.WithColor(!found ? Palette.Green : Palette.Blue))
  594.                                 // {
  595.                                 //     using (Draw.WithLineWidth(found ? 1 : 2))
  596.                                 //     {
  597.                                 //         buffer[i].Plot();
  598.                                 //     }
  599.                                 // }
  600.                                 using (Draw.WithColor(Palette.Blue))
  601.                                 {
  602.                                     // using (Draw.WithLineWidth(found ? 1 : 2))
  603.                                     // {
  604.                                     buffer[k].Plot();
  605.                                     // }
  606.                                 }
  607.                             }
  608.                             lowestCosts[j] = costAtEnd;
  609.                             best[j] = buffer[k];
  610.                             parents[j] = i;
  611.                         } else {
  612.                             using (Draw.WithMatrix(localToWorld))
  613.                             {
  614.                                 using (Draw.WithColor(Palette.Red))
  615.                                 {
  616.                                     buffer[k].Plot();
  617.                                 }
  618.                             }
  619.                         }
  620.                     } else {
  621.                         using (Draw.WithMatrix(localToWorld))
  622.                         {
  623.                             using (Draw.WithColor(Palette.Orange * new Color(1, 1, 1, 0.2f)))
  624.                             {
  625.                                 buffer[k].Plot();
  626.                             }
  627.                         }
  628.                         // Buffer was sorted, so we can never succeed again
  629.                         // break;
  630.                     }
  631.                 }
  632.             }
  633.         }
  634.  
  635.         var result = new List<TurnStraightTurnPath>();
  636.         Debug.Log("Best cost " + lowestCosts[n - 1]);
  637.         if (!float.IsPositiveInfinity(lowestCosts[n - 1])) {
  638.             var c = n - 1;
  639.             while (c != 0) {
  640.                 result.Add(best[c]);
  641.                 using (Draw.WithMatrix(localToWorld)) {
  642.                     using (Draw.WithColor(Palette.Green))
  643.                     {
  644.                         using (Draw.WithLineWidth(2))
  645.                         {
  646.                             best[c].Plot();
  647.                         }
  648.                     }
  649.                 }
  650.                 c = parents[c];
  651.             }
  652.         }
  653.  
  654.         result.Reverse();
  655.         return result;
  656.  
  657.         //     var tangent1 = i > 0 ? shortestPath[i + 1] - shortestPath[i - 1] : shortestPath[1] - shortestPath[0];
  658.         //     var tangent2 = i + 2 < shortestPath.Count ? shortestPath[i + 2] - shortestPath[i] : shortestPath[i + 1] - shortestPath[i];
  659.         //     tangent1 = tangent1.normalized;
  660.         //     tangent2 = tangent2.normalized;
  661.         //  var start = shortestPath[i];
  662.         //  var end = shortestPath[i+1];
  663.         //  var radius1 = i == 0 ? this.start.localScale.x : this.end.localScale.x;
  664.         //  var radius2 = this.end.localScale.x;
  665.         //     var startTangent = math.mul(worldToLocal, new float4(tangent1, 0)).xy;
  666.         //     var endTangent = math.mul(worldToLocal, new float4(tangent2, 0)).xy;
  667.         // }
  668.         // for (int j = 0; j < shortestPath.Count - 1; j++)
  669.         // {
  670.  
  671.  
  672.         //     var buffer = new NativeList<TurnStraightTurnPath>(Allocator.Temp);
  673.         //     Profiler.BeginSample("TurnPalette");
  674.  
  675.  
  676.         //     var startPos = math.transform(worldToLocal, start).xy;
  677.         //     var endPos = math.transform(worldToLocal, end).xy;
  678.         //     radius1 = math.min(radius1, math.length(endPos - startPos) * 0.5f);
  679.         //     radius2 = math.min(radius2, math.length(endPos - startPos) * 0.5f);
  680.  
  681.         //     TurnStraightTurnPath.Calculate(
  682.         //         startPos, startTangent, radius1,
  683.         //         endPos, endTangent, radius2,
  684.         //         buffer
  685.         //     );
  686.  
  687.         //     buffer.Sort();
  688.  
  689.         //     buffer.Add(new TurnStraightTurnPath
  690.         //     {
  691.         //         length = math.length(endPos - startPos),
  692.         //         startCenter = startPos,
  693.         //         endCenter = endPos,
  694.         //         startRadius = 0,
  695.         //         endRadius = 0,
  696.         //         startAngle1 = math.atan2(startTangent.y, startTangent.x),
  697.         //         startAngle2 = math.atan2(endPos.y - startPos.y, endPos.x - startPos.x),
  698.         //         endAngle1 = math.atan2(endPos.y - startPos.y, endPos.x - startPos.x),
  699.         //         endAngle2 = math.atan2(endTangent.y, endTangent.x),
  700.         //     });
  701.  
  702.  
  703.         //     bool found = false;
  704.         //     for (int i = 0; i < buffer.Length; i++)
  705.         //     {
  706.         //         if (Validate(AstarPath.active.data.layerGridGraph, buffer[i], localToWorld))
  707.         //         {
  708.         //             using (Draw.WithMatrix(localToWorld))
  709.         //             {
  710.         //                 using (Draw.WithColor(!found ? Palette.Green : Palette.Blue))
  711.         //                 {
  712.         //                     using (Draw.WithLineWidth(found ? 1 : 2))
  713.         //                     {
  714.         //                         buffer[i].Plot();
  715.         //                     }
  716.         //                 }
  717.         //             }
  718.         //             found = true;
  719.         //         }
  720.         //         else
  721.         //         {
  722.         //             // using (Draw.WithMatrix(localToWorld))
  723.         //             // {
  724.         //             //     using (Draw.WithColor(Palette.Red))
  725.         //             //     {
  726.         //             //         using (Draw.WithLineWidth(1))
  727.         //             //         {
  728.         //             //             buffer[i].Plot();
  729.         //             //         }
  730.         //             //     }
  731.         //             // }
  732.         //         }
  733.         //     }
  734.         //     Profiler.EndSample();
  735.         // }
  736.     }
  737.  
  738.     class Filter {
  739.         public Path path;
  740.         public readonly System.Func<GraphNode, bool> cachedDelegate;
  741.  
  742.         public Filter() {
  743.             cachedDelegate = this.CanTraverse;
  744.         }
  745.  
  746.         bool CanTraverse (GraphNode node) {
  747.             return path.CanTraverse(node);
  748.         }
  749.     }
  750.  
  751.     float lastT = 0;
  752.  
  753.     void OnPathComplete (Path _p) {
  754.         seeker.startEndModifier.Apply(_p);
  755.         var p = _p as ABPath;
  756.  
  757.         if (p.error) return;
  758.         filter.path = p;
  759.         bool filterIsRequired = filter.path.traversalProvider != null || filter.path.enabledTags != ~0;
  760.         var filterDelegate = filterIsRequired ? filter.cachedDelegate : null;
  761.         var result = GridStringPulling.Calculate(p.path, 0, p.path.Count - 1, p.vectorPath[0], p.vectorPath[p.vectorPath.Count - 1], (GraphNode node) => 0, filterDelegate, maxCorners);
  762.  
  763.         p.Claim(this);
  764.         p.Release(this);
  765.  
  766.         Draw.Polyline(result, new Color(1, 1, 1, 0.2f));
  767.  
  768.         var output = Calculate(result, Follower.PIDFollower.Direction(state.direction), end.forward, curvature, steps, out float firstCurvature);
  769.  
  770.         Profiler.BeginSample("Validate original");
  771.         Validate(p.path[0].Graph as GridGraph, result);
  772.         Profiler.EndSample();
  773.         bool valid = Validate(p.path[0].Graph as GridGraph, output);
  774.  
  775.         Draw.Polyline(output, valid ? Color.black : Color.red);
  776.  
  777.         ListPool<Vector3>.Release(ref result);
  778.  
  779.         interpolator.SetPath(output);
  780.  
  781.         if ((state.t > timeLimit || !state.cursor.valid)) {
  782.             state = new Follower.PIDFollowerState {
  783.                 p = transform.position,
  784.                 direction = 0,
  785.                 t = 0,
  786.                 cursor = interpolator.start,
  787.             };
  788.  
  789.             Debug.Log("Error " + follower.totalError);
  790.             follower.totalError = 0;
  791.         }
  792.  
  793.         // state.interpolator.distance = d;
  794.  
  795.         var prev = state;
  796.         var dt = Application.isPlaying ? Time.deltaTime : Time.realtimeSinceStartup - lastT;
  797.  
  798.         lastT = Time.realtimeSinceStartup;
  799.         follower.fixedCurvature = firstCurvature;
  800.         follower.Eval(ref state, dt);
  801.         if (float.IsNaN(state.p.x)) {
  802.             Debug.LogError("NaN");
  803.             state = prev;
  804.         }
  805.         Debug.Log(state.p);
  806.         Draw.Line(prev.p, state.p, Color.blue);
  807.         using (Draw.WithDuration(5))
  808.         {
  809.             Draw.xz.Circle(state.p, 0.01f, Color.blue);
  810.         }
  811.         Draw.xz.Circle(state.p, 0.008f, Color.magenta);
  812.     }
  813.  
  814.     public static bool Validate (GridGraph graph, List<Vector3> points) {
  815.         Profiler.BeginSample("Validate");
  816.         var node = graph.GetNearest(points[0], null).node as GridNodeBase;
  817.         var normalized = node.NormalizePoint(points[0]);
  818.  
  819.         for (int i = 1; i < points.Count; i++) {
  820.             var nextNode = graph.GetNearest(points[i], null).node as GridNodeBase;
  821.             var nextNormalized = nextNode.NormalizePoint(points[i]);
  822.             GridHitInfo hit;
  823.             if (graph.Linecast(node, normalized, nextNode, nextNormalized, out hit)) {
  824.                 Draw.Ray((Vector3)hit.node.position, Vector3.up, Color.red);
  825.                 return false;
  826.             }
  827.             node = nextNode;
  828.             normalized = nextNormalized;
  829.         }
  830.         Profiler.EndSample();
  831.         return true;
  832.     }
  833.  
  834.     public static List<Vector3> Calculate (List<Vector3> points, Vector3 tangentStart, Vector3 tangentEnd, float desiredCurvature, int steps, out float firstCurvature) {
  835.         Draw.Polyline(points);
  836.         var output = new List<Vector3>();
  837.  
  838.         firstCurvature = 0;
  839.  
  840.         for (int i = 0; i < points.Count - 1; i++) {
  841.             var t0 = i == 0 ? tangentStart : points[i + 1] - points[i - 1];
  842.             t0 = t0.normalized;
  843.  
  844.             var t1 = i >= points.Count - 2 ? tangentEnd : points[i + 2] - points[i];
  845.             t1 = t1.normalized;
  846.  
  847.             var normal0 = i == 0 ? Vector3.zero : 2 * points[i] - (points[i + 1] + points[i - 1]);
  848.             var normal1 = i >= points.Count - 2 ? Vector3.zero : 2 * points[i + 1] - (points[i + 2] + points[i]);
  849.             var p0 = points[i] + normal0.normalized * 0.001f;
  850.             var p3 = points[i + 1] + normal1.normalized * 0.001f;
  851.  
  852.             // var p2 = p3 - t1;
  853.             // tau = |6*t2*(p2 - 2*p1 + p0) + 6*t*(p3 - 2*p2 + p1)|
  854.             // tau(0) = |6*(p2 - 2*p1 + p0)| = |6*(p2 - 2*(p0 + t0*x) + p0)| = |6*p2 - 12*p0 - 12*t0*x + p0|
  855.             // = |6*p2 - 11*p0 - 12*t0*x| = (6*p2 - 11*p0 - 12*t0*x)*(6*p2 - 11*p0 - 12*t0*x)
  856.             // 121*p0^2 - 132*p0*p2 + 264*p0*t0*x + 36*p2^2 - 144*p2*t0*x + 144*t0^2*x^2
  857.             // (121*p0^2 - 132*p0*p2 + 36*p2^2) + (264*p0 - 144*p2)*t0*x + 144*t0^2*x^2
  858.  
  859.             // dx/dt = 3*t2*t2*(p1-p0) + 6*t2*t*(p2 - p1) + 3*t*t*(p3 - p2)
  860.             // dx(0)/dt = 3*t0*x
  861.  
  862.             // (121*p0^2 - 132*p0*p2 + 36*p2^2) + (264*p0 - 144*p2)*t0*x + 144*t0^2*x^2 = desiredCurvature * (9*t0^2*x*x)
  863.             // var c = (121*p0.sqrMagnitude - 132*Vector3.Dot(p0, p2) + 36*p2.sqrMagnitude);
  864.             // var b = Vector3.Dot(264*p0 - 144*p2, t0);
  865.             // var a = 144*t0.sqrMagnitude - desiredCurvature*9*t0.sqrMagnitude;
  866.             // var disc = Mathf.Sqrt(b*b - 4*a*c);
  867.             // Debug.Log(b*b - 4*a*c);
  868.             // var x1 = (-b + disc)/(2*a);
  869.             // var x2 = (-b - disc)/(2*a);
  870.             // var x = Mathf.Max(x1, x2);
  871.  
  872.             // 6*p2 - 6*p0 - 12*t0*x
  873.             // cross(6*p2 - 6*p0 - 12*t0*x, 3*t0*x) / (27*x^3)
  874.             // d/dx cross(6*p2 - 6*p0 - 12*t0*x, t0/(x^2*9))
  875.             // = (cross(d(6*p2 - 6*p0 - 12*t0*x)/dx, t0/(x^2*9)) + cross(6*p2 - 6*p0 - 12*t0*x, -2*t0/(x^3*9)))
  876.             // = (cross(-12*t0, t0/(x^2*9)) + cross(6*p2 - 6*p0 - 12*t0*x, -2*t0/(x^3*9)))
  877.             // = cross(6*p2 - 6*p0 - 12*t0*x6, -2*t0/(x^3*9)))
  878.             // = cross(6*p2 - 6*p0, -2*t0/(x^3*9)))
  879.             float x0 = 1;
  880.             float x1 = 1;
  881.             var p1 = p0 + t0 * x0;
  882.             var p2 = p3 - t1 * x1;
  883.             for (int k = 0; k < 4; k++) {
  884.                 float t = (k % 2) == 0 ? 0 : 1;
  885.                 for (int j = 0; j < steps; j++) {
  886.                     var derivative = AstarSplines.CubicBezierDerivative(p0, p1, p2, p3, t);
  887.                     var secondDerivative = AstarSplines.CubicBezierSecondDerivative(p0, p1, p2, p3, t);
  888.                     var curvature = Vector3.Cross(derivative, secondDerivative) / Mathf.Pow(derivative.magnitude, 3);
  889.                     Vector3 curvatureDerivative;
  890.                     if ((k % 2) == 0) {
  891.                         curvatureDerivative = Vector3.Cross(6 * p2 - 6 * p0, -2 * t0) / (x0 * x0 * x0 * 9);
  892.                         x0 += (curvature.magnitude - desiredCurvature) / curvatureDerivative.magnitude * 0.2f;
  893.                         x0 = Mathf.Clamp(x0, 0.01f, 100);
  894.                     } else {
  895.                         curvatureDerivative = Vector3.Cross(-6 * p3 + 6 * p1, -2 * t1) / (x1 * x1 * x1 * 9);
  896.                         x1 += (curvature.magnitude - desiredCurvature) / curvatureDerivative.magnitude * 0.2f;
  897.                         x1 = Mathf.Clamp(x1, 0.01f, 100);
  898.                     }
  899.  
  900.                     x1 = 1;
  901.                     x0 = 1;
  902.                     // 6*t2*(p2 - 2*p1 + p0) + 6*t*(p3 - 2*p2 + p1
  903.  
  904.                     p1 = p0 + t0 * x0;
  905.                     p2 = p3 - t1 * x1;
  906.                 }
  907.             }
  908.  
  909.             {
  910.                 Draw.Line(p0, p1, Color.blue);
  911.                 Draw.Line(p2, p3, Color.blue);
  912.  
  913.                 // Draw.Bezier(p0, p1, p2, p3, Color.black);
  914.                 // Draw.Bezier(p0, p0 + t0, p3 - t1, p3, Color.magenta);
  915.             }
  916.  
  917.             if (i == 0) {
  918.                 var derivative = AstarSplines.CubicBezierDerivative(p0, p1, p2, p3, 0);
  919.                 var secondDerivative = AstarSplines.CubicBezierSecondDerivative(p0, p1, p2, p3, 0);
  920.                 var curvature = Vector3.Cross(derivative, secondDerivative) / Mathf.Pow(derivative.magnitude, 3);
  921.                 Debug.Log("Curvature: " + curvature);
  922.                 firstCurvature = curvature.y;
  923.             }
  924.  
  925.             for (int j = 0; j <= 50; j++) {
  926.                 output.Add(AstarSplines.CubicBezier(p0, p1, p2, p3, j / 50f));
  927.             }
  928.         }
  929.         return output;
  930.     }
  931.  
  932.     struct Arc {
  933.     }
  934.  
  935.     struct TangentsInfo {
  936.         public float2 normal;
  937.     }
  938.  
  939.     static float2? Tangents (float2 c1, float r1, float2 c2, float r2) {
  940.         // L, R
  941.         // L, L
  942.         // R, L
  943.         // R, R
  944.         float2 c = c2 - c1;
  945.         float r = r2 - r1;
  946.         float z = math.lengthsq(c);
  947.         float d = z - r * r;
  948.  
  949.         if (d < -0.00001f) return null;
  950.         d = math.sqrt(math.abs(d));
  951.         var normal = (-c * r + new float2(c.y, -c.x) * d) / z;
  952.  
  953.         return normal;
  954.         // Draw.Line((Vector2)c1, (Vector2)(c1 + normal*r1), Purple);
  955.         // Draw.Line((Vector2)c2, (Vector2)(c2 + normal*r2), Purple);
  956.         // Draw.Line((Vector2)(c1 + normal*r1), (Vector2)(c2 + normal*r2), Purple);
  957.         // Draw.xy.Circle(new float3(c1, 0), r1, Pure.Black);
  958.         // Draw.xy.Circle(new float3(c2, 0), r2, Red);
  959.         // line l;
  960.         // l.a = (c.x * r + c.y * d) / z;
  961.         // l.b = (c.y * r - c.x * d) / z;
  962.         // l.c = r1;
  963.         // ans.push_back (l);
  964.     }
  965.  
  966. // public static List<Vector3> Calculate(List<Vector3> points, Vector3 tangentStart, Vector3 tangentEnd, float desiredCurvature, int steps, out float firstCurvature) {
  967.  
  968. // }
  969. }
  970.  
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!