Optimized IK "inverse kinematics" solver for 2 segments.
Here is the optimized solution to solve the inverse kinematics problem for 2 segments.
Explanation of the algorithm by the original author
https://iquilezles.org/articles/simpleik/
Here is how Inigo Quilez conludes:
"As you can see, we solved the problem by reasoning geometrically, without resorting to trigonometry, and our implementation does not even make use of trigonometric functions. In general, as I have expressed before, if you find yourself using trigonometric functions to solve some 2D or 3D geometric problem, you are probably doing things less elegantly and efficiently. Of course, this is a matter of opinion, but now you have heard mine :).
In any case, this IK formulation is very useful for performing simple animation operations on articulated objects, such as the legs of this animated creature."
I implemented it in VB6 and added the repositioning of the Target to the most appropriate position in case there is no solution.
I hope it can be useful.
Here is an example of how I used it:
Origin is the fixed point.
Target is the point to be reached.
R1 and R2 are the Segments lengths
Sol1 and Sol2 the 2 solutions in both sides
Code to put in a FORM: (It uses RC6 for render)
Code to put in a module (Core Function)
Click and drag mouse on the form to see how the system responds to different configurations
Here is the optimized solution to solve the inverse kinematics problem for 2 segments.
Explanation of the algorithm by the original author
https://iquilezles.org/articles/simpleik/
Here is how Inigo Quilez conludes:
Quote:
"As you can see, we solved the problem by reasoning geometrically, without resorting to trigonometry, and our implementation does not even make use of trigonometric functions. In general, as I have expressed before, if you find yourself using trigonometric functions to solve some 2D or 3D geometric problem, you are probably doing things less elegantly and efficiently. Of course, this is a matter of opinion, but now you have heard mine :).
In any case, this IK formulation is very useful for performing simple animation operations on articulated objects, such as the legs of this animated creature."
I hope it can be useful.
Here is an example of how I used it:
Code:
Public function IKSolve(ByRef Origin As tVec2, _
ByRef Target As tVec2, _
ByRef R1 As Double, _
ByRef R2 As Double, _
ByRef Sol1 As tVec2, _
ByRef Sol2 As tVec2) As BooleanTarget is the point to be reached.
R1 and R2 are the Segments lengths
Sol1 and Sol2 the 2 solutions in both sides
Code to put in a FORM: (It uses RC6 for render)
Code:
Option Explicit
Dim Srf As cCairoSurface
Dim CC As cCairoContext
Dim Center As tVec2
Dim Length1 As Double
Dim Length2 As Double
Dim Target As tVec2
Private Sub Form_Load()
ScaleMode = vbPixels
Set Srf = Cairo.CreateSurface(Me.ScaleWidth, Me.ScaleHeight, ImageSurface)
Set CC = Srf.CreateContext
CC.SetLineCap CAIRO_LINE_CAP_ROUND
CC.SetLineJoin CAIRO_LINE_JOIN_ROUND
CC.AntiAlias = CAIRO_ANTIALIAS_FAST
Center.X = Me.ScaleWidth * 0.5
Center.Y = Me.ScaleHeight * 0.5
Length1 = ScaleWidth * 0.26
Length2 = ScaleWidth * 0.14
End Sub
Private Sub Form_MouseMove(Button As Integer, Shift As Integer, X As Single, Y As Single)
Dim Sol1 As tVec2
Dim Sol2 As tVec2
Dim Solvable As Boolean
If Button Then
Target.X = X
Target.Y = Y
Solvable = IKSolve(Center, Target, Length1, Length2, Sol1, Sol2)
With CC
.SetSourceRGB 0.2, 0.2, 0.2: .Paint
If Solvable Then .SetSourceRGB 0.4, 1, 0 Else .SetSourceRGB 0.8, 0, 0
'DRAW Nodes
.Arc Center.X, Center.Y, 10: .Fill
.Arc Sol1.X, Sol1.Y, 7: .Fill
.Arc Target.X, Target.Y, 5: .Fill
'DRAW Solution 1
.SetLineWidth 4: .MoveTo Center.X, Center.Y: .LineTo Sol1.X, Sol1.Y
.LineTo Target.X, Target.Y: .Stroke
'DRAW Solution 2
.SetLineWidth 1: .MoveTo Center.X, Center.Y: .LineTo Sol2.X, Sol2.Y
.LineTo Target.X, Target.Y: .Stroke
End With
fMain.Picture = Srf.Picture
End If
End SubCode:
Option Explicit
Public Type tVec2
X As Double
Y As Double
End Type
'SOURCE:
'https://iquilezles.org/articles/simpleik/
Public Function IKSolve(ByRef Origin As tVec2, _
ByRef Target As tVec2, _
ByRef R1 As Double, _
ByRef R2 As Double, _
ByRef Sol1 As tVec2, _
ByRef Sol2 As tVec2) As Boolean
' NOTE: Target will change position if no solution found!
Dim pX#, pY#
Dim H#, W#, S#
pX = Origin.X - Target.X
pY = Origin.Y - Target.Y
H = pX * pX + pY * pY 'DOT3(p, p)
W = H + R2 * R2 - R1 * R1
S = 4# * R2 * R2 * H - W * W
If S > 0# Then
S = Sqr(S)
H = 0.5 / H
Sol1.X = Target.X + (pX * W - pY * S) * H
Sol1.Y = Target.Y + (pY * W + pX * S) * H
Sol2.X = Target.X + (pX * W + pY * S) * H
Sol2.Y = Target.Y + (pY * W - pX * S) * H
IKSolve = True
Else
'No solution (so Move target and get Solutions [by reexre])
H = 1# / Sqr(H)
pX = pX * H: pY = pY * H
Sol1.X = Origin.X - pX * R1
Sol1.Y = Origin.Y - pY * R1
Sol2 = Sol1
If W > 0 Then 'Target too far
Target.X = Origin.X - pX * (R1 + R2)
Target.Y = Origin.Y - pY * (R1 + R2)
Else 'Target too close to Origin
Target.X = Sol1.X + pX * R2
Target.Y = Sol1.Y + pY * R2
End If
End If
'{
' float h = dot(p,p);
' float w = h + r2*r2 - r1*r1;
' float s = max(4.0*r2*r2*h - w*w,0.0);
' return (w*p + vec2(-py,px)*sqrt(s)) * 0.5/h;
'}
End Function