MODULE RobotAnim;


(*****************************************************************************)
(*                                                                           *)
(*  Modul: RobotAnim                       Version 1.0 vom 1. November 1990  *)
(*                                                                           *)
(*  Für M2Amiga Modula-2 von Olaf Pfeiffer                                   *)
(*                                                                           *)
(*****************************************************************************)

(* $F- $R- $V- $N- *)


FROM GfxDecs      IMPORT GfxMode, GfxPtr, Voxel, Direction, ScenePtr, Pixel;
FROM GfxSystem    IMPORT OpenStdGfx, CloseGfx, ClearGfx, GfxToFront;
FROM Camera       IMPORT SetCameraTo;
FROM System3D     IMPORT SetVoxelTo, SetDirectTo, AddDirect,
                         AddVoxel, SubVoxel;
FROM GfxMath3D    IMPORT TurnVoxelY, TurnVoxelZ;
FROM SceneLib     IMPORT InitScene, EndScene, MergeObjects, RenameObject,
                         MoveRefVoxTo, TurnObjectY, TurnObjectZ, RecolorObject;
FROM ShapeLib     IMPORT Block, RotatePoly, StretchPoly;
FROM ShowScene    IMPORT HiddenLine;
FROM ColorLib     IMPORT SetColors, SetEHBColors;


VAR Gfx:            ARRAY BOOLEAN OF GfxPtr;
    Scene:          ScenePtr;
    GO,GU,GH:       Voxel;   (* Drehpunkte der drei Gelenke *)
    OY,OZ,UZ,HY,HZ: INTEGER; (* Mometane Stellung der drei Gelenke *)
    ActTurn:        CHAR;    (* Momentan akitives Gelenk: O, U oder H *)
    adddir:         Direction;
    tur,add,lit:    Direction;
    bool:           BOOLEAN;
    Gel,Ach:        CHAR;
    n,Ang:          INTEGER;


PROCEDURE RobotToScene;

VAR Lin:     ARRAY [0..7] OF Pixel;
    tur:     Direction;
    siz,pos: Voxel;

BEGIN
  (* Zusammensetzen des Roboter-Arms aus einzelnen Elementen. *)
  SetDirectTo(tur,0,0,0);
  SetVoxelTo(siz,5000,1000,5000);
  SetVoxelTo(pos,0,-14000,0);
  Block(Scene,"Basis",siz,tur,pos,1);
  RecolorObject(Scene,"Basis",TRUE,1);
  Lin[0].x  := -2000; Lin[0].y  :=     0;
  Lin[1].x  := -1414; Lin[1].y  :=  1414;
  Lin[2].x  :=     0; Lin[2].y  :=  2000;
  Lin[3].x  :=  1414; Lin[3].y  :=  1414;
  Lin[4].x  :=  2000; Lin[4].y  :=     0;
  Lin[5].x  :=  1414; Lin[5].y  := -1414;
  Lin[6].x  :=     0; Lin[6].y  := -2000;
  Lin[7].x  := -1414; Lin[7].y  := -1414;
  SetVoxelTo(pos,0,-11000,-2000);
  StretchPoly(Scene,"1.Gelenk",4000,8,Lin,tur,pos,2);
  RecolorObject(Scene,"1.Gelenk",TRUE,2);
  SetVoxelTo(siz,1100,6000,2000);
  SetVoxelTo(pos,0,-3000,0);
  Block(Scene,"Oberarm",siz,tur,pos,3);
  RecolorObject(Scene,"Oberarm",TRUE,3);
  SetVoxelTo(pos,0,5000,-2000);
  StretchPoly(Scene,"2.Gelenk",4000,8,Lin,tur,pos,2);
  RecolorObject(Scene,"2.Gelenk",TRUE,2);
  SetVoxelTo(siz,1100,4000,2000);
  SetVoxelTo(pos,6000,5000,0);
  SetDirectTo(tur,0,0,900);
  Block(Scene,"Unterarm",siz,tur,pos,4);
  RecolorObject(Scene,"Unterarm",TRUE,4);
  Lin[0].x  := -2000; Lin[0].y  :=     0;
  Lin[1].x  := -1732; Lin[1].y  :=  1000;
  Lin[2].x  := -1000; Lin[2].y  :=  1732;
  Lin[3].x  :=     0; Lin[3].y  :=  2000;
  Lin[4].x  :=  1000; Lin[4].y  :=  1732;
  Lin[5].x  :=  2000; Lin[5].y  :=  1000;
  Lin[6].x  :=  3500; Lin[6].y  :=   750;
  Lin[7].x  :=  4000; Lin[7].y  :=     0;
  SetVoxelTo(pos,12000,5000,0);
  SetDirectTo(tur,0,0,0);
  RotatePoly(Scene,"3.Gelenk",8,8,Lin,tur,pos,5);
  RecolorObject(Scene,"3.Gelenk",TRUE,5);
  (* Zusammenfassen der zusammengehörigen Roboterteile. *)
  RenameObject(Scene,"Basis","B");                  (* Basis *)
  MergeObjects(Scene,"1.Gelenk","Oberarm","O");     (* Oberarm *)
  MergeObjects(Scene,"2.Gelenk","Unterarm","U");    (* Unterarm *)
  RenameObject(Scene,"3.Gelenk","H");               (* Hand *)
  (* Setzen der drei Gelenkpunkte und der zugehörigen Winkel. *)
  SetVoxelTo(GO,0,-11000,0);
  SetVoxelTo(GU,0,5000,0);
  SetVoxelTo(GH,12000,5000,0);
  ActTurn := "H";
  OY := 0; OZ := 0;
  UZ := 900;
  HY := 0; HZ := 0;
  SetDirectTo(adddir,0,0,0);
END RobotToScene;


PROCEDURE TurnRobot (Around, Axis: CHAR; Angle: INTEGER);

VAR ctrl: INTEGER;

BEGIN
  IF ActTurn # Around THEN
    IF Around = "H" THEN
      MoveRefVoxTo(Scene,"H",GH);
    ELSIF Around = "U" THEN
      MoveRefVoxTo(Scene,"H",GU);
      MoveRefVoxTo(Scene,"U",GU);
    ELSE (* Around = "O" *)
      MoveRefVoxTo(Scene,"H",GO);
      MoveRefVoxTo(Scene,"U",GO);
    END;
    ActTurn := Around;
  END;
  IF Around = "U" THEN               (* Drehung: Ellenbogen *)
    ctrl := UZ + Angle;
    IF ctrl < -300 THEN
      Angle := -300 - UZ;
    ELSIF ctrl > 1200 THEN
      Angle := 1200 - UZ;
    END;
    IF Angle # 0 THEN
      INC(UZ,Angle);
      TurnObjectZ(Scene,"U",Angle);
      TurnObjectZ(Scene,"H",Angle);
      SubVoxel(GH,GU);
      TurnVoxelZ(Angle,GH,GH);
      AddVoxel(GH,GU);
    END;
  ELSIF Around = "H" THEN            (* Drehung: Handgelenk *)
    IF Axis = "Z" THEN
      ctrl := HZ + Angle;
      IF ctrl < -1200 THEN
        Angle := -1200 - HZ;
      ELSIF ctrl > 1200 THEN
        Angle := 1200 - HZ;
      END;
      INC(HZ,Angle);
      TurnObjectZ(Scene,"H",Angle);
    ELSE (* Axis = "Y" *)
      ctrl := HY + Angle;
      IF ctrl < -1200 THEN
        Angle := -1200 - HY;
      ELSIF ctrl > 1200 THEN
        Angle := 1200 - HY;
      END;
      INC(HY,Angle);
      TurnObjectY(Scene,"H",Angle);
    END;
  ELSE (* Around = "O" *)            (* Drehung: Schulter *)
    IF Axis = "Z" THEN
      ctrl := OZ + Angle;
      IF ctrl < -450 THEN
        Angle := -450 - OZ;
      ELSIF ctrl > 900 THEN
        Angle := 900 - OZ;
      END;
      IF Angle # 0 THEN
        INC(OZ,Angle);
        TurnObjectZ(Scene,"O",Angle);
        TurnObjectZ(Scene,"U",Angle);
        TurnObjectZ(Scene,"H",Angle);
        SubVoxel(GH,GO);
        SubVoxel(GU,GO);
        TurnVoxelZ(Angle,GH,GH);
        TurnVoxelZ(Angle,GU,GU);
        AddVoxel(GH,GO);
        AddVoxel(GU,GO);
      END;
    ELSE (* Axis = "Y" *)
      INC(OY,Angle);
      adddir[2] := OY;
      TurnObjectY(Scene,"B",Angle);
      SetCameraTo(Gfx[TRUE],130,adddir,750,0,0);
      SetCameraTo(Gfx[FALSE],130,adddir,750,0,0);
    END;
  END;
END TurnRobot;


PROCEDURE TurnHim (Steps: INTEGER;
                   Around1, Axis1: CHAR; Angle1: INTEGER;
                   Around2, Axis2: CHAR; Angle2: INTEGER);
(*
WIRKUNG: Dreht den Roboterarm mehrmals hintereinander um zwei Achsen.
EINGABE: Steps -    Anzahl der Bewegungsschritte.
         Around1  - 1.Drehgelenk ("O"berarm, "U"nterarm oder "H"and).
         Axis1 -    Drehachse (bei O oder H; "Y" oder "Z", bei U nur "Z")
         Angle1 -   Der Drehwinkel (wird Steps-mal ausgeführt).
         Around2 -  2.Drehgelenk (um den wird der Arm gleichzeitig auch gedreht)
         Axis2 -    Zugehörige Drehachse.
         Angle2     Zugehöriger Drehwinkel.
*)

VAR l: INTEGER;

BEGIN
  FOR l := 1 TO Steps DO
    bool := NOT bool;
    TurnRobot(Around1,Axis1,Angle1);
    IF Angle2 # 0 THEN
      TurnRobot(Around2,Axis2,Angle2);
    END;
    ClearGfx(Gfx[bool]);
    HiddenLine(Gfx[bool],Scene);
    GfxToFront(Gfx[bool]);
  END;
END TurnHim;


BEGIN
  Gfx[TRUE]  := OpenStdGfx(EHB,FALSE);
  Gfx[FALSE] := OpenStdGfx(EHB,FALSE);
  SetColors(Gfx[TRUE], 0,0,"444 000");
  SetColors(Gfx[FALSE],0,0,"444 000");
  SetEHBColors(Gfx[TRUE], 0,"A46 46A A4A 777 35B");
  SetEHBColors(Gfx[FALSE],0,"A46 46A A4A 777 35B");
  Scene := InitScene();
  RobotToScene;
  SetDirectTo(tur,-200,0,0);
  SetDirectTo(lit,900,-900,0);
  SetCameraTo(Gfx[TRUE],130,tur,750,0,0);
  SetCameraTo(Gfx[FALSE],130,tur,750,0,0);
(****)
  TurnHim( 7,"O","Z", -75,"U","Z",  75);
  TurnHim(14,"O","Y", 100,"H","Y",  75);
  TurnHim( 7,"H","Y", -75,"O","Z",  75);
  TurnHim( 7,"H","Z", 150,"O","Y",-100);
  TurnHim( 7,"O","Z", -75,"U","Z", -75);
  TurnHim(14,"H","Z", -75,"O","Z",-100);
  TurnHim(14,"O","Y",  75,"U","Z",  40);
(****)
  EndScene(Scene);
  CloseGfx(Gfx[TRUE]); CloseGfx(Gfx[FALSE]);
END RobotAnim.