Screw.mac

vec(a1,b1,c1):=block(
  /*this function gets 3 values as elements of a 3D vector and gives a (3,1) column matrix as the representation of the vector*/

  matrix([a1],[b1],[c1])
)$

dotprod(V1,V2):=block(
  transpose(V1).V2
)$




vecmag(V1):= block(
  /*this function gets a (3,1) column matrix as representation of a 3D vector and delivers a scalar as the magnitude of the vector*/
	/*sqrt(transpose(V1).V1)*/

  sqrt(dotprod(V1,V1))
)$

unitvec(V1):=block(
  V1*(1/vecmag(V1))
)$


crosprod(V1,V2):= block(
  /*this function calculates the vector/cross product of the vector on the left on the vector on the right*/

  matrix(
  [(V1[2,1]*V2[3,1]-V1[3,1]*V2[2,1])],
  [(V1[3,1]*V2[1,1]-V1[1,1]*V2[3,1])],
  [(V1[1,1]*V2[2,1]-V1[2,1]*V2[1,1])]
  )
)$

Rodrigues(V, K, theta) := V * cos(theta) + crosprod(K, V) * sin(theta) + K * dotprod(K, V) * (1 - cos(that))$


vec2screw(v,r):=block(
  [sv, sr, SC1],
  /*
  VEC2SCREW transforms line direction v and a point r on the line to screw vector
  	-v is the screw axis direction vector
  	-r is a point on the axis
  	SC is the screws generated by v and r
  */

  /*
  to be translated from MATLAB to Maxima Language
  % wrong format
  sv=size(v);
  sr=size(r);
  if sv==[1 3]v=v';sv=size(v);end
  if sr==[1 3],r=r';sr=size(r);end

  % wrong format
  if sv(1) ~= 3 || sr(1) ~= 3
      error('vec2screw:wrongsize.');
  end
  */

  SC1: crosprod(r,v),
  matrix(
    [v[1,1]],
    [v[2,1]],
    [v[3,1]],
    [SC1[1,1]],
    [SC1[2,1]],
    [SC1[3,1]]
  )
)$


/*
Algebra of screws
based on this wikipedia page:https://en.wikipedia.org/wiki/Screw_theory

*/

ScrSum(S1,S2):=block(
  [S1[1]+S2[1],S1[2]+S2[2]]
)$

ScrScMult(S1,S2):=block(
  /*
  screw scalar multiplication
  at least one of the inputs must be scaler
  */
  [(S1[1]*S2[1]),(S1[1]*S2[2]-S1[2]*S2[1])]
)$

ScrInProd(S1,S2):=block(
  [(dotprod(S1[1],S2[1])),(dotprod(S1[1],S2[2])-dotprod(S1[2],S2[1]))]
)$

ScrCrosProd(S1,S2):=block(
  [(crosprod(S1[1],S2[1])),(crosprod(S1[1],S2[2])-crosprod(S1[2],S2[1]))]
)$


/*reciprocal product of two screws*/
RcprclProd(S1,S2):=block(
  (dotprod(S1[1],S2[2])+dotprod(S1[2],S2[1]))
)$


/*
this function gets a vector and returns a skew symmetric matrix as tilde operator.
this is a function similar to the Skew function in 20-sim
this function gets a 3*1 column matrix as a vector and gives a 3*3 matrix as output
*/

Skew(V1):=block(
  matrix([0       ,-V1[3,1], V1[2,1]],
         [V1[3,1] ,0       ,-V1[1,1]],
         [-V1[2,1],V1[1,1] ,0       ])
)$




PlcFrmTwoPnt(P1,P2):=block(
  [S,V_l],

  /*
  Plucker coordinates (PC) of a line given 2 points on that line
  also known as Grassmann coordinates, or referred to as the homogeneous line coordinates
  the result is a unit line vector (dual vector)
  */

  /*unit direction vector of the line*/
  S:unitvec(P2-P1),

  /*moment of the line about the origin*/
  V_l:crosprod(P2,S),

  /*PC*/
  [S,V_l]
)$



FndClsPntLnOrg(PC1):=block(
  /*given the Plucker coordinates of a line finds the closest point of the line to the origin*/
  crosprod(PC1[1],PC1[2])
)$


FndClsPntLnPnt(PC1,P1):=block(
  /*closest point of a line to a point*/
  P0:FndClsPntLnOrg(PC1),
  S01:crosprod((P0-P1),PC1[1]),
  P1+crosprod(S01,PC1[1])
)$


/*
closest points of two skew lines
feet of the common perpendicular
*/

FtCmnPPndclr(PC1,PC2):=block(
  P1:(crosprod(-PC1[2],(crosprod(PC2[1],(crosprod(PC1[1],PC2[1])))))+(dotprod(PC2[2],(crosprod(PC1[1],PC2[1]))))*PC1[1])/(vecmag(crosprod(PC1[1],PC2[1])))^2,
  P2:(crosprod(+PC2[2],(crosprod(PC1[1],(crosprod(PC1[1],PC2[1])))))-(dotprod(PC1[2],(crosprod(PC1[1],PC2[1]))))*PC2[1])/(vecmag(crosprod(PC1[1],PC2[1])))^2,
  [P1,P2]
)$


/*
this function gets a unit vector as the axis of rotation and an angle of rotation and uses the Rodrigues' rotation formula to calculate the rotation matrix
*/
RdrgsToRMat(k1,T1):=block(
  /*k1 unit vector as axis of rotation and T1 angle of rotation according to the right hand rule*/
  K1:Skew(k1),
  ident(3)+sin(T1)*K1+(1-cos(T1))(K.K)
)$


/*
this function takes the Plucker coordinates of the axis of rotation of a rigid body
and the rotational velocity
and a point in space attached to the rigid body
and delivers the velocity of that point
*/