From 37102167a98a198119f6bd030de27b7cc097f24b Mon Sep 17 00:00:00 2001
From: Kevin-Mattheus-Moerman <kevin.moerman@gmail.com>
Date: Wed, 3 Jul 2024 11:42:02 +0100
Subject: [PATCH] Added fillet curve functionality

---
 examples/demo_filletcurve.jl |  9 +++++++--
 src/functions.jl             | 12 +++++-------
 2 files changed, 12 insertions(+), 9 deletions(-)

diff --git a/examples/demo_filletcurve.jl b/examples/demo_filletcurve.jl
index 2d5679e..0ee18b1 100644
--- a/examples/demo_filletcurve.jl
+++ b/examples/demo_filletcurve.jl
@@ -9,7 +9,7 @@ using Random
 # Set seed so demo performs the same each time
 Random.seed!(1)
 
-testCase = 6
+testCase = 11
 
 if testCase == 1
     V = Point{3,Float64}[ [0.0,0.0,0.0], [10.0,0.0,0.0]]
@@ -33,6 +33,7 @@ elseif testCase == 6
     d = 10.0 
     nc = 12
     V = circlepoints(d,nc)  
+    close_loop = false
 elseif testCase == 7 
     V = Point{3,Float64}[ [-2.0,0.0,0.0], [-1.0,0.0,0.0], [0.0,0.0,0.0], [1.0,-1.0,1.0], [2.0,1.0,0.0], [3.0,2.0,8.0], [5.0,0.0,0.0], [7.0,0.0,0.0]]
     close_loop = false
@@ -51,10 +52,14 @@ elseif testCase == 10
         V[i] = Point{3,Float64}(v[1],v[2],v[3]+20.0)
     end
     close_loop = true
+elseif testCase == 11
+    d = 10.0 
+    V = Point{3,Float64}[ [-d,-d,0.0], [d,-d,0.0], [d,d,0.0], [-d,d,0.0]]
+    close_loop = true
 end
 
 
-rMax = nothing
+rMax = [2.0,3.0,0.0,5.0] #nothing
 
 VC = filletcurve(V; rMax=rMax,  constrain_method = :max, n=25, close_loop = close_loop, eps_level = 1e-6)
 # VC2,_ = evenly_sample(VC,100)
diff --git a/src/functions.jl b/src/functions.jl
index 0a3752e..b1217a1 100644
--- a/src/functions.jl
+++ b/src/functions.jl
@@ -4649,7 +4649,7 @@ function filletcurve(V::Vector{Point{NV,TV}}; rMax::Union{Vector{T},T,Nothing}=n
         end
         
         e1 = VP[2]-VP[1]
-        e2 = VP[3]-VP[3]
+        e2 = VP[3]-VP[2]
         lp = 0.0
         i_last = length(VP)-1
         L = [norm(VP[i+1]-VP[i]) for i in 1:i_last]
@@ -4712,8 +4712,8 @@ function filletcurve(V::Vector{Point{NV,TV}}; rMax::Union{Vector{T},T,Nothing}=n
                 rFit = l/tan(β)
                 if !isnothing(r) && r<=rFit
                     rNow = r
-                    lNow = r*tan(β) # Update as as radius used is smaller
-                    if tan(β)*rNow == l
+                    lNow = rNow*tan(β) # Update as as radius used is smaller
+                    if lNow == l
                         fullRound = true
                     else
                         fullRound = false
@@ -4726,10 +4726,8 @@ function filletcurve(V::Vector{Point{NV,TV}}; rMax::Union{Vector{T},T,Nothing}=n
 
                 if rNow > 0                    
                     d = abs(rNow/cos(β))
-                    S1 = mapreduce(permutedims,vcat,[-m1,normalizevector(cross(n3,-m1)),n3])        
-                    S2 = one(RotMatrix{3,Float64}) 
-                    Q = RotMatrix3{Float64}(S1\S2)
-                    
+                    Q = RotMatrix3{Float64}(reduce(hcat,[-m1,normalizevector(cross(n3,-m1)),n3]))        
+                                        
                     m1p = Q'*(-m1)
                     a = atan(m1p[2]/m1p[1])