--* From postmaster%watson.vnet.ibm.com@yktvmv.watson.ibm.com  Tue May  3 14:01:29 1994
--* Received: from yktvmv.watson.ibm.com by asharp.watson.ibm.com (AIX 3.2/UCB 5.64/930311)
--*           id AA35013; Tue, 3 May 1994 14:01:29 -0400
--* Received: from watson.vnet.ibm.com by yktvmv.watson.ibm.com (IBM VM SMTP V2R3)
--*    with BSMTP id 9953; Tue, 03 May 94 14:01:29 EDT
--* Received: from YKTVMV by watson.vnet.ibm.com with "VAGENT.V1.0"
--*           id <A.ROOT.NOTE.VAGENT2.8521.May.03.14:01:28.-0400>
--*           for asbugs@watson; Tue, 03 May 94 14:01:29 -0400
--* Received: from YKTVMV by watson.vnet.ibm.com with "VAGENT.V1.0"
--*           id 8517; Tue, 3 May 1994 14:01:28 EDT
--* Received: from daly.watson.ibm.com by yktvmv.watson.ibm.com (IBM VM SMTP V2R3)
--*    with TCP; Tue, 03 May 94 14:01:27 EDT
--* Received: by daly.watson.ibm.com (AIX 3.2/UCB 5.64/4.03)
--*           id AA13984; Tue, 3 May 1994 14:01:50 -0500
--* Date: Tue, 3 May 1994 14:01:50 -0500
--* From: root@daly.watson.ibm.com
--* Message-Id: <9405031901.AA13984@daly.watson.ibm.com>
--* To: asbugs@watson.ibm.com
--* Subject: [5] function missing from extended domain [/u/daly/as/dhmatrix.as][35.0]

--@ Fixed  by:  SSD   Fri Jun 24 11:04:36 EDT 1994 
--@ Tested by:  none 
--@ Summary:    Extend substitution fixes correct this bug [v35.8.3]. 

#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: 26 June 1991
+++ 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]])

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

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

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

    rotatez(degree:R):% ==
     angle := degree * pi() / 180::R
     cosAngle := cos(angle)
     sinAngle := 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]])