spherical_joint.mac

G(p) := matrix(
    [-p[2], p[1],-p[4], p[3]],
    [-p[3], p[4], p[1],-p[2]],
    [-p[4],-p[3], p[2], p[1]]);

L(p) := matrix(
    [-p[2], p[1], p[4],-p[3]],
    [-p[3],-p[4], p[1], p[2]],
    [-p[4], p[3],-p[2], p[1]]);

r1 : matrix(
    [r1x],
    [r1y],
    [r1z]);

r2 : matrix(
    [r2x],
    [r2y],
    [r2z]);

p1 : matrix(
    [p1_0],
    [p1_1],
    [p1_2],
    [p1_3]);

p2 : matrix(
    [p2_0],
    [p2_1],
    [p2_2],
    [p2_3]);

s1_p : matrix(
    [s1_px],
    [s1_py],
    [s1_pz]);

s2_p : matrix(
    [s2_px],
    [s2_py],
    [s2_pz]);


PHI : r1 + G(p1) . transpose(L(p1)) . s1_p - r2 - G(p2) . transpose(L(p2)) . s2_p;