--* From BMT%WATSON.vnet.ibm.com@yktvmv.watson.ibm.com  Thu Jun  9 04:01:49 1994
--* Received: from yktvmv-ob.watson.ibm.com by asharp.watson.ibm.com (AIX 3.2/UCB 5.64/930311)
--*           id AA24689; Thu, 9 Jun 1994 04:01:49 -0400
--* Received: from watson.vnet.ibm.com by yktvmv.watson.ibm.com (IBM VM SMTP V2R3)
--*    with BSMTP id 3073; Thu, 09 Jun 94 04:01:49 EDT
--* Received: from YKTVMV by watson.vnet.ibm.com with "VAGENT.V1.0"
--*           id <A.BMT.NOTE.VAGENT2.5285.Jun.09.04:01:48.-0400>
--*           for asbugs@watson; Thu, 09 Jun 94 04:01:49 -0400
--* Received: from YKTVMV by watson.vnet.ibm.com with "VAGENT.V1.0"
--*           id 5281; Thu, 9 Jun 1994 04:01:48 EDT
--* Received: from spadserv.watson.ibm.com by yktvmv.watson.ibm.com
--*    (IBM VM SMTP V2R3) with TCP; Thu, 09 Jun 94 04:01:48 EDT
--* Received: by spadserv.watson.ibm.com (AIX 3.2/UCB 5.64/900524)
--*           id AA22479; Thu, 9 Jun 1994 04:04:36 -0400
--* Date: Thu, 9 Jun 1994 04:04:36 -0400
--* From: bmt@spadserv.watson.ibm.com
--* X-External-Networks: yes
--* Message-Id: <9406090804.AA22479@spadserv.watson.ibm.com>
--* To: asbugs@watson.ibm.com
--* Subject: [1] asharp -Y /spad/mnt/rios/asharp/lib gets seg fault [/spad/src/as/dhmatrix.as][0.356.7]

--@ Fixed  by:  SSD   Mon May 15 11:56:12 EDT 1995 
--@ Tested by:  none 
--@ Summary:    No longer produces segmentation fault. Compiles fine w/ a couple of extra imports, declarations. 


#include "axiom.as"

--Copyright The Numerical Algorithms Group Limited 1991.

+++ 4x4 Matrices for coordinate transformations
+++ Author: Timothy Daly
+++ Date Created: June 26, 1991
+++ Date Last Updated: May 26, 1994 asharp conversion by tim daly
+++ Description:
+++   This package contains functions to create 4x4 matrices
+++   useful for rotating and transforming coordinate systems.
+++   These matrices are useful for graphics and robotics.
+++   (Reference: Robot Manipulators Richard Paul MIT Press 1981)


--)abbrev domain DHMATRIX DenavitHartenbergMatrix

--% DHMatrix

{DenavitHartenbergMatrix(R : Join(Field,  TranscendentalFunctionCategory)):
 MatrixCategory(R,Vector R,Vector R) with
 {-- A Denavit-Hartenberg Matrix is a 4x4 Matrix of the form:
  --  \spad{nx ox ax px}
  --  \spad{ny oy ay py}
  --  \spad{nz oz az pz}
  --   \spad{0  0  0  1}
  -- (n, o, and a are the direction cosines)

   *: (%, Point R) -> Point R;
   identity: () -> %;
     ++ identity() create the identity dhmatrix
   rotatex: R -> %;
     ++ rotatex(r) returns a dhmatrix for rotation about axis X for r degrees
   rotatey: R -> %;
     ++ rotatey(r) returns a dhmatrix for rotation about axis Y for r degrees
   rotatez: R -> %;
     ++ rotatez(r) returns a dhmatrix for rotation about axis Z for r degrees
   scale: (R,R,R) -> %;
     ++ scale(sx,sy,sz) returns a dhmatrix for scaling in the X, Y and Z
     ++ directions
   translate: (R,R,R) -> %;
     ++ translate(X,Y,Z) returns a dhmatrix for translation by X, Y, and Z
  }
 ==   Matrix(R) add
   {Rep ==> Matrix R;
    import from Rep;
    import from R;
    import from List R;
    import from List List R;

    nx ==> x(1,1)::R;
    ny ==> x(2,1)::R;
    nz ==> x(3,1)::R;
    ox ==> x(1,2)::R;
    oy ==> x(2,2)::R;
    oz ==> x(3,2)::R;
    ax ==> x(1,3)::R;
    ay ==> x(2,3)::R;
    az ==> x(3,3)::R;
    px ==> x(1,4)::R;
    py ==> x(2,4)::R;
    pz ==> x(3,4)::R;
    radians ==> pi()/180;

    identity():% ==
     {per matrix([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]])}

--    inverse(x) == (inverse(x pretend (Matrix R))$Matrix(R)) pretend %
--    dhinverse(x) == matrix( _
--        [[nx,ny,nz,-(px*nx+py*ny+pz*nz)],_
--         [ox,oy,oz,-(px*ox+py*oy+pz*oz)],_
--         [ax,ay,az,-(px*ax+py*ay+pz*az)],_
--         [ 0, 0, 0, 1]])

    make4vec(v:Vector R, c:R):Vector R ==
      {import from SingleInteger;
       v2:=new(4,c);
       v2.1:=v.1;
       v2.2:=v.2;
       v2.3:=v.3;
       v2}

    ((d:%) * (p:Point(R))):Point(R) ==
      {v:Vector R := p pretend Vector R;
       v := make4vec(v, 1$R);
       v := d * v;
       point ([v.1, v.2, v.3]$List(R))}

    rotatex(degree:R):% ==
    {angle:R := degree * pi() / 180::R;
     cosAngle:R := cos(angle);
     sinAngle:R := sin(angle);
     per matrix(
      [[1,     0,           0,      0],
       [0, cosAngle, -sinAngle, 0],
       [0, sinAngle,  cosAngle, 0],
       [0,     0,           0,      1]])}

    rotatey(degree:R):% ==
    {angle:R := degree * pi() / 180::R;
     cosAngle:R := cos(angle);
     sinAngle:R := sin(angle);
     per matrix(
      [[ cosAngle, 0, sinAngle, 0],
       [    0,       1,     0,      0],
       [-sinAngle, 0, cosAngle, 0],
       [    0,       0,     0,      1]])}

    rotatez(degree:R):% ==
    {angle:R := degree * pi() / 180::R;
     cosAngle:R := cos(angle);
     sinAngle:R := sin(angle);
     per matrix(
      [[cosAngle, -sinAngle, 0, 0],
       [sinAngle,  cosAngle, 0, 0],
       [   0,           0,       1, 0],
       [   0,           0,       0, 1]])}

    scale(scalex:R, scaley:R, scalez:R):% ==
    {per matrix(
      [[scalex, 0      ,0     , 0],
       [0     , scaley ,0     , 0],
       [0     , 0      ,scalez, 0],
       [0     , 0      ,0     , 1]])}

    translate(x:R,y:R,z:R):% ==
    {per matrix(
      [[1,0,0,x],
       [0,1,0,y],
       [0,0,1,z],
       [0,0,0,1]])}
 }
} -- DenavitHartenbergMatrix