New base model to convert all 2D Kinematics to 3D.

After all, final designs should be 6DOF robot arms and factory machines.

unit labscene;

{$mode ObjFPC}{$H+}

interface

uses
  Classes, SysUtils, BGRAScene3D, BGRABitmap, BGRABitmapTypes;

const texSize = 256;

type
  TMyLabLighting = (e2lNone,e2lLightness,e2lColored);

  TLab = class(TBGRAScene3D)
      Tip : IBGRAVertex3D; // Sharp Tip
      SandColor: TBGRAPixel;
      water,wood,vWood: TBGRABitmap;
      box1, box2, tube1, tub2, ground, light1, light2: IBGRAObject3D;
      alpha: integer;
      cury: single;
      FLighting : TMyLabLighting;
      procedure CreateScene;
      procedure ApplyTexCoord(face: IBGRAFace3D; Times: integer = 1);
      procedure SetLighting(AValue: TMyLabLighting);
   public
      constructor Create(ALighting: TMyLabLighting);
      procedure Elapse;
      destructor Destroy; override;
      property Lighting: TMyLabLighting read FLighting write SetLighting;
   end;

implementation

   uses utexture;

   { TLab }

constructor TLab.Create(ALighting: TMyLabLighting);
begin
   inherited Create;

   //create textures
   water := CreateWaterTexture(texSize,texSize);
   vWood := CreateVerticalWoodTexture(texSize,texSize);
   wood := CreateWoodTexture(texSize,texSize);

   FLighting:= ALighting;
   CreateScene;
end;

procedure TLab.Elapse;
   var dy: single;
begin
  if light1 <> nil then light1.MainPart.RotateYDeg(1,False);
  if light2 <> nil then light2.MainPart.RotateYDeg(-1.3,False);
  if ground <> nil then
  begin
    dy := cos(alpha*Pi/180)*0.05;
    cury += dy;
    ground.MainPart.Translate(0,dy,0,False);
    ViewPoint := Point3D(ViewPoint.x,-40+cury,ViewPoint.z);
    LookAt(Point3D(0,cury,0),Point3D(0,-1,0));
    inc(alpha);
    if alpha = 360 then alpha := 0;
  end;
end;

procedure TLab.CreateScene;
   var
      base,v: array of IBGRAVertex3D;
      lamp,shiny: IBGRAMaterial3D;
begin
  Clear;

  shiny := CreateMaterial(500);
  lamp := CreateMaterial;
  lamp.LightThroughFactor := 0.01;

  //create wooden box
  box1 := CreateObject(Wood);

  with box1 do begin
    // Add 3d dots
    v := MainPart.Add([-1,-1,-1, 1,-1,-1, 1,1,-1, -1,1,-1,
                       -1,-1,+1, 1,-1,+1, 1,1,+1, -1,1,+1]);
    // Add faces
    ApplyTexCoord(AddFace([v[0],v[1],v[2],v[3]]));
    ApplyTexCoord(AddFace([v[4],v[5],v[1],v[0]],wood));
    ApplyTexCoord(AddFace([v[5],v[4],v[7],v[6]]));
    ApplyTexCoord(AddFace([v[3],v[2],v[6],v[7]],wood));
    ApplyTexCoord(AddFace([v[1],v[5],v[6],v[2]]));
    ApplyTexCoord(AddFace([v[4],v[0],v[3],v[7]]));

    MainPart.Scale(1, 2, 10, false);
    //MainPart.
    MainPart.Translate(-2,-2,0); // move
  end;

  box2 := CreateObject(vWood);
  with box2 do begin
    v := MainPart.Add([-1,-1,-1, 1,-1,-1, 1,1,-1, -1,1,-1,
                       -1,-1,+1, 1,-1,+1, 1,1,+1, -1,1,+1]);

    ApplyTexCoord(AddFace([v[0],v[1],v[2],v[3]]));
    ApplyTexCoord(AddFace([v[4],v[5],v[1],v[0]],vWood));
    ApplyTexCoord(AddFace([v[5],v[4],v[7],v[6]]));
    ApplyTexCoord(AddFace([v[3],v[2],v[6],v[7]],vWood));
    ApplyTexCoord(AddFace([v[1],v[5],v[6],v[2]]));
    ApplyTexCoord(AddFace([v[4],v[0],v[3],v[7]]));

    MainPart.Scale(1, 2, 10, false);
  end;

  DefaultLightingNormal:= lnFace;

  if Lighting = e2lColored then
  begin
    ViewPoint := Point3D(0,0,-150);
    AmbiantLightColor := BGRA(192,192,192);
    box1.Material := shiny;

    //lights
    light1 := CreateHalfSphere(10, BGRA(255,128,0), 8,8);
    with light1 do
    begin
      AddPointLight(MainPart.Add(0,0,-5),60,BGRA(255,128,0),0);
      MainPart.Translate(-100,-50,0);
      MainPart.LookAt(Point3D(0,0,0),Point3D(0,-1,0));
      Material := lamp;
      LightingNormal := lnVertex;
    end;
    light2 := CreateHalfSphere(10, BGRA(0,128,255), 8,8);
    with light2 do
    begin
      AddPointLight(MainPart.Add(0,0,-5),100,BGRA(0,128,255),0);
      MainPart.Translate(50,0,-100);
      MainPart.LookAt(Point3D(0,0,0),Point3D(0,-1,0));
      Material := lamp;
      LightingNormal := lnVertex;
    end;
  end else
  begin
    //create ground
    ground := CreateObject(water);
    if Lighting = e2lLightness then
    begin
      with ground do
      begin
        base := MainPart.Add([-50,0,-50, -50,0,50, 50,0,50, 50,0,-50]);
        ApplyTexCoord(AddFace(base,True),2);
      end;
      ViewPoint := Point3D(-40,-40,-100);
      AmbiantLightness := 0.25;
      with CreateObject do
        AddPointLight(MainPart.Add(-100,-80,0),100,1.25, -0.15);
    end else
    begin
      AmbiantLightness := 1;
      with ground do
      begin
        base := MainPart.Add([-50,0,-50, -50,0,50, 50,0,50, 50,0,-50]);
        MainPart.Scale(2);
        ApplyTexCoord(AddFace(base,True),2);
      end;
      ViewPoint := Point3D(0,-40,-120);
    end;
    RenderingOptions.PerspectiveMode:= pmZBuffer;
  end;

  RenderingOptions.TextureInterpolation := false;
end;

procedure TLab.ApplyTexCoord(face: IBGRAFace3D; Times: integer);
begin
   with face do begin
      TexCoord[0] := PointF(0,0);
      TexCoord[1] := PointF(texSize*Times-1,0);
      TexCoord[2] := PointF(texSize*Times-1,texSize*Times-1);
      TexCoord[3] := PointF(0,texSize*Times-1);
   end;
end;

procedure TLab.SetLighting(AValue: TMyLabLighting);
begin
  if FLighting=AValue then Exit;
  FLighting:=AValue;
  CreateScene;
end;

destructor TLab.Destroy;
begin
  water.free;
  wood.free;
  vWood.free;
  inherited Destroy;
end;

end.
 

//////////////////////////////////////////////////////////////////////////////////

unit ikmain;

{$mode objfpc}{$H+}

interface

uses
   cthreads, Classes, SysUtils, FileUtil, Forms, Controls, Graphics, Dialogs,
   StdCtrls, LMessages, ExtCtrls, BGRABitmap, BGRAVirtualScreen,
   BGRABitmapTypes, BGRAScene3D, BGRAPath, EpikTimer, BCTypes, BGRAMatrix3D,
   labscene;

type
   { TfrmIKmain }

   TfrmIKmain = class(TForm)
      BGRASurface: TBGRAVirtualScreen;
      m: TMemo;
      pnlRight: TPanel;
      procedure BGRASurfaceMouseDown(Sender: TObject; Button: TMouseButton;
        Shift: TShiftState; X, Y: Integer);
      procedure BGRASurfaceMouseMove(Sender: TObject; Shift: TShiftState; X,
        Y: Integer);
      procedure BGRASurfaceMouseUp(Sender: TObject; Button: TMouseButton;
        Shift: TShiftState; X, Y: Integer);
      procedure BGRASurfaceRedraw(Sender: TObject; Bitmap: TBGRABitmap);
      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 }
      scene: TBGRAScene3D;
      moving3D: boolean;
      moveOrigin3D: TPoint;
      timer: TEpikTimer;
      procedure RedrawScene;
   end;

var
   frmIKmain: TfrmIKmain;

implementation

{$R *.lfm}

uses math;

{ TfrmIKmain }

procedure TfrmIKmain.RedrawScene;
begin
  if BGRASurface.Visible then BGRASurface.DiscardBitmap;
end;

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

// elbow = ik(shoulder, hand, forearm, upperArm)
// On mine : pts[1] := ik(pts[0], pts[2], distJ2E, distB2J)
function 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;

procedure TfrmIKmain.FormCreate(Sender: TObject);
begin
   scene := TLab.Create(e2lLightness);
   timer := TEpikTimer.Create(nil);
   timer.TimebaseSource := HardwareTimebase;
   BGRASurface.Align := alClient;
   RedrawScene;
end;

procedure TfrmIKmain.BGRASurfaceRedraw(Sender: TObject; Bitmap: TBGRABitmap);
begin
   if scene <> nil then begin
      scene.RenderingOptions.AntialiasingMode := am3dResample;
      scene.RenderingOptions.AntialiasingResampleLevel := 1;
      scene.RenderingOptions.MinZ := 1;

      scene.Surface := Bitmap;
      scene.Render;
      scene.Surface := nil;
   end;
end;

procedure TfrmIKmain.BGRASurfaceMouseDown(Sender: TObject;
  Button: TMouseButton; Shift: TShiftState; X, Y: Integer);
begin
   if (button = mbLeft) and (scene <> nil) then begin
      moving3D := true;
      moveOrigin3D := point(x,y);
   end;
end;

procedure TfrmIKmain.BGRASurfaceMouseMove(Sender: TObject; Shift: TShiftState;
  X, Y: Integer);
begin
   if moving3D then begin
     // if for view angle control (3rd person view)
     //  scene.LookRight(X-moveOrigin3D.X);
     //  scene.LookDown(Y-moveOrigin3D.Y);
     if scene.Object3DCount > 0 then begin
        scene.Object3D[0].MainPart.RotateYDeg(-(X-moveOrigin3D.X),False);
        scene.Object3D[0].MainPart.RotateXDeg(Y-moveOrigin3D.Y,False);
     end;
     RedrawScene;
     moveOrigin3D := point(x,y);
   end;
end;

procedure TfrmIKmain.BGRASurfaceMouseUp(Sender: TObject; Button: TMouseButton;
  Shift: TShiftState; X, Y: Integer);
begin
   if button = mbLeft then moving3D := false;
end;

procedure TfrmIKmain.FormDestroy(Sender: TObject);
begin
   FreeAndNil(scene);
   timer.Free;
end;

procedure TfrmIKmain.FormPaint(Sender: TObject);
begin

end;

procedure TfrmIKmain.FormMouseDown(Sender: TObject; Button: TMouseButton;
  Shift: TShiftState; X, Y: Integer);
begin

end;

procedure TfrmIKmain.FormMouseMove(Sender: TObject; Shift: TShiftState; X,
  Y: Integer);
begin

end;

procedure TfrmIKmain.FormMouseUp(Sender: TObject; Button: TMouseButton;
  Shift: TShiftState; X, Y: Integer);
begin

end;


end.