Reusable Math Utilities and System Definitions

In the following template, the Templex macro eulerXYZ.tpl has been created to receive three angular rotations and then calculate the corresponding Euler parameters. Euler parameters are commonly used to orient entities such as joints, bodies, markers, and so on within mechanical system simulation codes.

{include "eulerXYZ.tpl"}
SYSTEM 1 JOINT DOF
{eulerXYZ( 90, 0, 0)}
 1  FREE  {e0}  {e1}  {e2}  {e3}    0.5  0.5  0.5    0.0  0.0  0.0    50.0  0.0  0.0
END JOINT DOF
END SYS
SYSTEM 2 JOINT DOF
{eulerXYZ( 0, 90, 0)}
 1  FREE  {e0}  {e1}  {e2}  {e3}    0.5  0.5  0.5    0.0  0.0  0.0    50.0  0.0  0.0
END JOINT DOF
END SYS

The Templex macro is executed by the eulerXYZ( rx, ry, rz) statement in the above template. The macro itself resides in the Templex file referenced by the include statement. When Templex encounters an include statement, execution passes from the current template to the specified template.

{' This templex macro calculates the Euler params based on three sequential rotations.}
{' Rotation sequence Rx -> Ry -> Rz.}
{define eulerXYZ(rx, ry, rz)}
{   t1 = dtor(rx)}
{   t2 = dtor(ry)}
{   t3 = dtor(rz)}
{' Cosine matrix:}
{   a11=cos(t2)*cos(t3)}
{   a12=-cos(t2)*sin(t3)}
{   a13=sin(t2)}
{   a21=sin(t1)*sin(t2)*cos(t3)+sin(t3)*cos(t1)}
{   a22=cos(t1)*cos(t3)-sin(t1)*sin(t2)*sin(t3)}
{   a23=-sin(t1)*cos(t2)}
{   a31=sin(t1)*sin(t3)-cos(t1)*sin(t2)*cos(t3)}
{   a32=sin(t1)*cos(t3)+cos(t1)*sin(t2)*sin(t3)}
{   a33=cos(t1)*cos(t2)}
{   trace = a11+a22+a33 }
{' Euler parameters:}
{   e0 = 0.5*sqrt(trace+1.0) }
{   e1 = 0.5*sqrt(1.0 + 2.0*a11 - trace) }
{   e2 = 0.5*sqrt(1.0 + 2.0*a22 - trace) }
{   e3 = 0.5*sqrt(1.0 + 2.0*a33 - trace) }
{ if e0 != 0.0 }
{   e0 = sqrt(e0*e0) }
{   e1 = (a32-a23)/(4.0*e0) }
{   e2 = (a13-a31)/(4.0*e0) }
{   e3 = (a21-a12)/(4.0*e0) }
{ elseif e1 != 0.0 }
{   e1 = sqrt(e1*e1) }
{   e2 = (a13+a31)/(4.0*e1) }
{   e3 = (a21+a12)/(4.0*e1) }
{ elseif e2 != 0.0 }
{   e2 = sqrt(e2*e2) }
{   e3 = (a32+a23)/(4.0*e2) }
{ else }
{   e3 = 1.0 }
{ endif }
{enddefine}

The results of the template are:

SYSTEM 1 JOINT DOF
  1  FREE  0.707107  0.707107  0  0    0.5  0.5  0.5    0.0  0.0  0.0    50.0  0.0  0.0
END JOINT DOF
END SYS
SYSTEM 2 JOINT DOF
  1  FREE  0.707107  0  0.707107  0    0.5  0.5  0.5    0.0  0.0  0.0    50.0  0.0  0.0
END JOINT DOF
END SYS