Robotik,Featherstone Algorithmus,Kugelgelenk

Anhand featherstone Algorithmus sollte ich einen Kugelgelenk (Gelenk mit 3 Freiheitsgraden z.B Schultergelenk)programieren.ich habe schon ein Program das ein drehgelenk (Gelenk mit nur einen Freiheitsgrad z.B Kniegelenk)implementiert.
in der Klasse multibody ist eine methode connect für Gelenk mit nur einen Freiheitsgrad so defeniert:
// Connect two links of a multibody. For each link, we specify: (1)
// loc, the location of the joint; (2) axis, the direction of the
// joint axis, and (3) ref, the direction of a reference axis. All
// quantities are specified in the body frame of the corresponding
// link. This routine connects the two links, such that the axes
// coincide. For prismatic joints, the ref axes are always in the
// same direction, and the locs coincide when the joint is in the "0"
// configuration. For revolute joints, the locs always coincide, and
// the ref axes are in the same direction when the joint is in the "0"
// configuration. The base link should be connected to the inertial
// frame; pass 0 (zero or NULL) in as the inboard link for this case.
void connect(Link::JointType jointType,
Link* pInboardLink,
const Vect3 &inLoc,
const Vect3 &inAxis,
const Vect3 &inRef,
Link* pOutboardLink,
const Vect3 &outLoc,
const Vect3 &outAxis,
const Vect3 &outRef);
Wie kann ich connect für Kugelgelenk erweitern?
Im Fall von Kugelgelenk ist der Axis ein Matrix.
Danke für jede Hilfe