More Inverse Kinematics.

< Display of Angles added >

Code is in Attachment (code.zip)

unit ikmain;

{$mode objfpc}{$H+}

interface

uses
   Classes, SysUtils, FileUtil, Forms, Controls, Graphics, Dialogs, StdCtrls,
   BGRABitmap, BGRABitmapTypes, LMessages, ExtCtrls, BGRAPath;

const
   END_POINT = 2;

type

   { TfrmIKmain }

   TfrmIKmain = class(TForm)
      procedure FormCreate(Sender: TObject);
      procedure FormDestroy(Sender: TObject);
      procedure FormPaint(Sender: TObject);
      procedure FormMouseDown(Sender: TObject; Button: TMouseButton;
        {%H-}Shift: TShiftState; X, Y: Integer);
      procedure FormMouseMove(Sender: TObject; Shift: TShiftState; X, Y: Integer);
      procedure FormMouseUp(Sender: TObject; Button: TMouseButton;
        {%H-}Shift: TShiftState; {%H-}X, {%H-}Y: Integer);
   public
      { public declarations }
      pts: array of TPointF;
      MovingPointIndex: Integer;
      MovingOrigin: TPointF;
      MyScreen: TBGRAPath;
      distB2J, distJ2E, distB2E, distMax, AngA, AngJ, AngJa,
      StartAng, EndingAng, xA, xJ : real;
      procedure RepaintScreenChange;
      function  dist(x1, y1, x2, y2 : single): real;
      function  ik(a, b : TPointF; ra, rb : real) : TPointF;
   end;

var
   frmIKmain: TfrmIKmain;

implementation

{$R *.lfm}

uses math;

{ TfrmIKmain }

// elbow = ik(shoulder, hand, forearm, upperArm)
// On mine : pts[1] := ik(pts[0], pts[2], distJ2E, distB2J)
function TfrmIKmain.ik(a, b : TPointF; ra, rb : real) : TPointF;
   var c, th, phi : real;
      tp : TPointF;
begin
   c := dist(a.x, a.y, b.x, b.y);
   th := arccos((rb**2 + c**2 - ra**2)/(2*rb*c));
   phi := arctan2(-(b.y - a.y), b.x - a.x);

   tp.x := a.x + rb*cos(th + phi);
   tp.y := a.y - rb*sin(th + phi);
   result := tp;
end;

function TfrmIKmain.dist(x1, y1, x2, y2 : single): real;
begin
   dist := sqrt((x2-x1)**2 + (y2-y1)**2);
end;

procedure TfrmIKmain.FormCreate(Sender: TObject);
begin
   setlength(pts,3); // set PTS point array's number of points

   // set position of each points in PTS point array
   pts[0] := PointF(300,300);  // B : Base
   pts[1] := PointF(400,200);  // J : Joint
   pts[2] := PointF(500,250);  // E : End Point

   // Getting distance : distB2J, distJ2E, distB2E, distMax
   distB2J := round(dist(pts[0].x, pts[0].y, pts[1].x, pts[1].y));
   distJ2E := round(dist(pts[1].x, pts[1].y, pts[2].x, pts[2].y));
   distB2E := round(dist(pts[0].x, pts[0].y, pts[2].x, pts[2].y));
   distMax := distB2J + distJ2E;
   xA := pts[1].x - pts[0].x;
   AngA := arccos(xA/distB2J);
   xJ := pts[2].x - pts[1].x;
   AngJ := arccos(xA/distB2J);
end;

procedure TfrmIKmain.FormDestroy(Sender: TObject);
begin
   FreeAndNil(MyScreen);
end;

procedure TfrmIKmain.FormPaint(Sender: TObject);
   var bmp: TBGRABitmap;
      i: Integer;
      style: TSplineStyle;
      nbPoints: integer;
      pt,tangent: TPointF;
begin
   //PreviousSize := PointF(ClientWidth,clientheight);
   bmp := TBGRABitmap.Create(clientwidth,clientheight,BGRAWhite);

   nbPoints := length(pts);

   if MyScreen = nil then
      MyScreen := TBGRAPath.Create;

   if Assigned(MyScreen) then begin
      MyScreen.fill(bmp, BGRA(250,250,0));

      // Draw ground rectangle & symbol marks
      bmp.FillRect(50, 300, 550, 350, BGRA(255, 192, 0), dmSet);
      bmp.TextOut(280,  305, 'Base', BGRABlack);
      bmp.TextOut(pts[1].x-20, pts[1].y-30, 'Joint', BGRABlack);
      bmp.TextOut(pts[2].x-40, pts[2].y-30, 'End-Point', BGRABlack);

      // Draw line between points
      bmp.DrawPolyLineAntialias(slice(pts,nbPoints),BGRA(102,50,179),3);

      for i := 0 to nbPoints-1 do  // Draw circle on each points
         bmp.FillEllipseAntialias(pts[i].x,pts[i].y,5,5,BGRA(50,200,200));

      // distB2J, distJ2E, distB2E, distMax
      bmp.TextOut(10,  0, 'B-J : ' + inttostr(round(distB2J)), BGRABlack);
      bmp.TextOut(10, 25, 'J-E : ' + inttostr(round(distJ2E)), BGRABlack);
      bmp.TextOut(10, 50, 'MAX : ' + inttostr(round(distMax)), BGRABlack);

      xA := pts[1].x - pts[0].x;
      AngA := arccos(xA/distB2J);
      bmp.Canvas2D.strokeStyle (VGAFuchsia);
      bmp.Canvas2D.arc(pts[0].x, pts[0].y, 40, 0, -AngA, TRUE);
      bmp.Canvas2D.stroke();

      //distB2E := round(dist(pts[0].x, pts[0].y, pts[2].x, pts[2].y));
      bmp.TextOut(pts[0].x+20, pts[0].y-30, format('%5.2f', [RadToDeg(AngA)]), VGAFuchsia);

      // Center Radian : C = atan ((P2.y-C.y)/(P2.x-C.x)) - atan((P1.y-C.y)/(P1.x-C.x))
      AngJ := ArcTan((pts[2].y-pts[1].y)/(pts[2].x-pts[1].x))
         - ArcTan((pts[0].y-pts[1].y)/(pts[0].x-pts[1].x));
      bmp.Canvas2D.moveto(pts[1].x,pts[1].y);
      //StartAng := ArcTan((pts[1].y-pts[0].y)/(pts[1].x-pts[0].x)) + PI; // Get Start Engle
      if pts[1].x > pts[0].x then  // Left Side
         begin
            StartAng := ArcTan((pts[1].y-pts[0].y)/(pts[1].x-pts[0].x)) + PI; // Get Start Engle
            EndingAng:= ArcTan((pts[2].y-pts[1].y)/(pts[2].x-pts[1].x));
            AngJ := AngJ - PI;
            AngJa := -RadToDeg(AngJ);
            bmp.Canvas2D.arc(pts[1].x, pts[1].y, 40, StartAng, EndingAng, true);
         end
      else
         begin                     // Right Side
            StartAng := ArcTan((pts[1].y-pts[0].y)/(pts[1].x-pts[0].x)); // Get Start Engle
            EndingAng:= ArcTan((pts[2].y-pts[1].y)/(pts[2].x-pts[1].x));
            AngJa := -RadToDeg(AngJ);
            bmp.Canvas2D.arc(pts[1].x, pts[1].y, 40, StartAng, EndingAng, true);
         end;
      bmp.Canvas2D.stroke();
      bmp.TextOut(pts[1].x+20, pts[1].y-10, format('%5.2f', [AngJa]), VGAFuchsia);
   end;

   bmp.draw(Canvas,0,0);
   bmp.Free;
end;

procedure TfrmIKmain.FormMouseDown(Sender: TObject; Button: TMouseButton;
  Shift: TShiftState; X, Y: Integer);
   var maxDist, distance: single;
      mousePos,vect: TPointF;
      i: Integer;
begin
   if Button <> mbLeft then exit;  // react to only left button

   //select point to move
   MovingPointIndex := -1;   // deselect a point
   maxDist := 10;            // point selection proximity distance
   mousePos := PointF(X,Y);  // set current position
   MovingOrigin := mousePos; // new MovingOrigin

   vect := pts[END_POINT] - mousePos;
   distance := sqrt(vect*vect);
   if distance < maxDist then begin  // a point is selected (within proximity)
      maxDist := distance;
      MovingPointIndex := END_POINT;
   end;
end;

procedure TfrmIKmain.FormMouseMove(Sender: TObject; Shift: TShiftState; X,
  Y: Integer);
   var
      mousePos: TPointF;
      i: Integer;
      tp0, tp1, tp2 : TPointF;
      OverLimit : boolean;
begin
   if Y >= 300 then Y := 300;  // Limit Bottom

   OverLimit := false;

   mousePos := PointF(X,Y);

   if MovingPointIndex = 2 then begin // if selected point is the END POINT
      // Preserve Old Points in case it move over the limit.
      tp0 := pts[0]; // base
      tp1 := pts[1]; // joint
      tp2 := pts[2]; // end point
      // change the end-points to new location
      if tp2.y >= 300 then
         OverLimit := true
      else
         tp2.Offset(mousePos-MovingOrigin);
      tp1 := ik(tp0, tp2, distJ2E, distB2J);

      if tp2.y >= 300 then OverLimit := true;

      // prevent joint goes under the base
      if tp1.y >= 300 then OverLimit := true;
      // prevent end point go over the full length
      if dist(tp0.x, tp0.y, tp2.x, tp2.y) > distMax then OverLimit := true;

      // if new motion is not over the limit
      if not OverLimit then begin
         pts[2] := tp2;
         pts[1] := tp1;
      end;
   end;

   RepaintScreenChange;
   MovingOrigin := mousePos;
end;

procedure TfrmIKmain.FormMouseUp(Sender: TObject; Button: TMouseButton;
  Shift: TShiftState; X, Y: Integer);
begin
   if Button = mbLeft then MovingPointIndex := -1; // no more a point is selected
end;

procedure TfrmIKmain.RepaintScreenChange;
begin
   FreeAndNil(MyScreen);
   Invalidate;  // repaint
end;

end.

(New .mkv video demo is in attachment)