rigidBodyDynamics: New library providing support for the dynamics of n-DoF articulated bodies

Based on the principles, algorithms, data structures and notation
presented in the book:

    Featherstone, R. (2008).
    Rigid body dynamics algorithms.
    Springer.

This development is sponsored by Carnegie Wave Energy Ltd.

src/rigidBodyDynamics/Make/files

@@ -0,0 +1,25 @@
+bodies/rigidBody/rigidBody.C
+bodies/masslessBody/masslessBody.C
+
+joints/joint/joint.C
+joints/nullJoint/nullJoint.C
+
+joints/Rx/Rx.C
+joints/Ry/Ry.C
+joints/Rz/Rz.C
+joints/Ra/Ra.C
+joints/Rs/Rs.C
+joints/Rzyx/Rzyx.C
+joints/Rxyz/Rxyz.C
+joints/Ryxz/Ryxz.C
+
+joints/Px/Px.C
+joints/Py/Py.C
+joints/Pz/Pz.C
+joints/Pa/Pa.C
+joints/Pxyz/Pxyz.C
+
+rigidBodyModel/rigidBodyModel.C
+rigidBodyModel/forwardDynamics.C
+
+LIB = $(FOAM_LIBBIN)/librigidBodyDynamics

src/rigidBodyDynamics/Make/options

@@ -0,0 +1,3 @@
+EXE_INC =
+
+LIB_LIBS =

src/rigidBodyDynamics/bodies/masslessBody/masslessBody.C

@@ -0,0 +1,42 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "masslessBody.H"
+
+// * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
+
+Foam::RBD::masslessBody::~masslessBody()
+{}
+
+
+// * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * * //
+
+bool Foam::RBD::masslessBody::massless() const
+{
+    return true;
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/bodies/masslessBody/masslessBody.H

@@ -0,0 +1,106 @@
+/*---------------------------------------------------------------------------*\3
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+Class
+    Foam::masslessBody
+
+Description
+
+SourceFiles
+    masslessBodyI.H
+    masslessBody.C
+
+\*---------------------------------------------------------------------------*/
+
+#ifndef RBD_masslessBody_H
+#define RBD_masslessBody_H
+
+#include "rigidBody.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+namespace Foam
+{
+
+// Forward declaration of Foam classes
+class Istream;
+class Ostream;
+
+namespace RBD
+{
+
+// Forward declaration of friend functions and operators
+class masslessBody;
+Ostream& operator<<(Ostream&, const masslessBody&);
+
+
+/*---------------------------------------------------------------------------*\
+                         Class masslessBody Declaration
+\*---------------------------------------------------------------------------*/
+
+class masslessBody
+:
+    public rigidBody
+{
+
+public:
+
+    // Constructors
+
+        //- Construct a massless body
+        inline masslessBody();
+
+        //- Construct a named massless body
+        inline masslessBody(const word& name);
+
+
+    //- Destructor
+    virtual ~masslessBody();
+
+
+    // Member Functions
+
+        //- Return true if this body is a massless component of a composite body
+        virtual bool massless() const;
+
+
+    // IOstream Operators
+
+        //friend Ostream& operator<<(Ostream&, const masslessBody&);
+};
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace RBD
+} // End namespace Foam
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#include "masslessBodyI.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#endif
+
+// ************************************************************************* //

src/rigidBodyDynamics/bodies/masslessBody/masslessBodyI.H

@@ -0,0 +1,40 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+// * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
+
+inline Foam::RBD::masslessBody::masslessBody()
+:
+    rigidBody("massless", rigidBodyInertia())
+{}
+
+
+inline Foam::RBD::masslessBody::masslessBody(const word& name)
+:
+    rigidBody(name, rigidBodyInertia())
+{}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/bodies/rigidBody/rigidBody.C

@@ -0,0 +1,67 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "rigidBody.H"
+#include "subBody.H"
+
+// * * * * * * * * * * * * * * * * Selectors * * * * * * * * * * * * * * * * //
+
+Foam::autoPtr<Foam::RBD::rigidBody> Foam::RBD::rigidBody::New
+(
+    const word& name,
+    const scalar& m,
+    const vector& c,
+    const symmTensor& Ic
+)
+{
+    return autoPtr<rigidBody>(new rigidBody(name, m, c, Ic));
+}
+
+
+// * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
+
+Foam::RBD::rigidBody::~rigidBody()
+{}
+
+
+// * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * * //
+
+bool Foam::RBD::rigidBody::massless() const
+{
+    return false;
+}
+
+
+void Foam::RBD::rigidBody::merge(const subBody& subBody)
+{
+    *this = rigidBody
+    (
+        name(),
+        *this + transform(subBody.parentXT(), subBody)
+    );
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/bodies/rigidBody/rigidBody.H

@@ -0,0 +1,145 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+Class
+    Foam::RBD::rigidBody
+
+Description
+
+SourceFiles
+    rigidBodyI.H
+
+\*---------------------------------------------------------------------------*/
+
+#ifndef RBD_rigidBody_H
+#define RBD_rigidBody_H
+
+#include "rigidBodyInertia.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+namespace Foam
+{
+
+// Forward declaration of Foam classes
+class Istream;
+class Ostream;
+
+namespace RBD
+{
+
+// Forward declaration of classes
+class subBody;
+
+// Forward declaration of friend functions and operators
+class rigidBody;
+Ostream& operator<<(Ostream&, const rigidBody&);
+
+
+/*---------------------------------------------------------------------------*\
+                         Class rigidBody Declaration
+\*---------------------------------------------------------------------------*/
+
+class rigidBody
+:
+    public rigidBodyInertia
+{
+    // Private data
+
+        //- Name of body
+        word name_;
+
+
+public:
+
+    // Constructors
+
+        //- Construct from mass, centre of mass and moment of inertia tensor
+        //  about the centre of mass
+        inline rigidBody
+        (
+            const word& name,
+            const scalar& m,
+            const vector& c,
+            const symmTensor& Ic
+        );
+
+        //- Construct from with the given rigidBodyInertia
+        inline rigidBody
+        (
+            const word& name,
+            const rigidBodyInertia& rbi
+        );
+
+        //- Return clone of this rigidBody
+        inline autoPtr<rigidBody> clone() const;
+
+
+    // Selectors
+
+        //- Select constructed from components
+        static autoPtr<rigidBody> New
+        (
+            const word& name,
+            const scalar& m,
+            const vector& c,
+            const symmTensor& Ic
+        );
+
+
+    //- Destructor
+    virtual ~rigidBody();
+
+
+    // Member Functions
+
+        //- Return name
+        inline const word& name() const;
+
+        //- Return false as this body is not massless
+        virtual bool massless() const;
+
+        //- Merge a body into this parent body
+        void merge(const subBody&);
+
+
+    // IOstream Operators
+
+        //friend Ostream& operator<<(Ostream&, const rigidBody&);
+};
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace RBD
+} // End namespace Foam
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#include "rigidBodyI.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#endif
+
+// ************************************************************************* //

src/rigidBodyDynamics/bodies/rigidBody/rigidBodyI.H

@@ -0,0 +1,66 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+// * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
+
+inline Foam::RBD::rigidBody::rigidBody
+(
+    const word& name,
+    const scalar& m,
+    const vector& c,
+    const symmTensor& Ic
+)
+:
+    rigidBodyInertia(m, c, Ic),
+    name_(name)
+{}
+
+
+inline Foam::RBD::rigidBody::rigidBody
+(
+    const word& name,
+    const rigidBodyInertia& rbi
+)
+    :
+    rigidBodyInertia(rbi),
+    name_(name)
+{}
+
+
+inline Foam::autoPtr<Foam::RBD::rigidBody> Foam::RBD::rigidBody::clone() const
+{
+    return autoPtr<rigidBody>(new rigidBody(*this));
+}
+
+
+// * * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * //
+
+inline const Foam::word& Foam::RBD::rigidBody::name() const
+{
+    return name_;
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/bodies/subBody/subBody.H

@@ -0,0 +1,125 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+Class
+    Foam::subBody
+
+Description
+    This specialized rigidBody holds the original body after it has been merged
+    into a parent.
+
+SourceFiles
+    subBodyI.H
+    subBody.C
+
+\*---------------------------------------------------------------------------*/
+
+#ifndef RBD_subBody_H
+#define RBD_subBody_H
+
+#include "rigidBody.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+namespace Foam
+{
+
+// Forward declaration of Foam classes
+class Istream;
+class Ostream;
+
+namespace RBD
+{
+
+// Forward declaration of friend functions and operators
+class subBody;
+Ostream& operator<<(Ostream&, const subBody&);
+
+
+/*---------------------------------------------------------------------------*\
+                         Class subBody Declaration
+\*---------------------------------------------------------------------------*/
+
+class subBody
+:
+    public rigidBody
+{
+    // Private data
+
+        //- Original body from which this sub-body was constructed
+        autoPtr<rigidBody> body_;
+
+        //- Parent body ID
+        label parentID_;
+
+        //- Transform with respect to parent body
+        spatialTransform parentXT_;
+
+
+public:
+
+    // Constructors
+
+        //- Construct a merged version of the given rigidBody
+        //  providing the ID of the parent body to which this will be merged
+        //  and the transform relative to the parent
+        inline subBody
+        (
+            autoPtr<rigidBody> bodyPtr,
+            const label parentID,
+            const spatialTransform& parentXT
+        );
+
+
+    // Member Functions
+
+        //- Return the original body from which this sub-body was constructed
+        inline const rigidBody& body() const;
+
+        //- Return the parent body Id
+        inline label parentID() const;
+
+        //- Return the transform with respect to the parent body
+        inline const spatialTransform& parentXT() const;
+
+
+    // IOstream Operators
+
+        //friend Ostream& operator<<(Ostream&, const subBody&);
+};
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace RBD
+} // End namespace Foam
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#include "subBodyI.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#endif
+
+// ************************************************************************* //

src/rigidBodyDynamics/bodies/subBody/subBodyI.H

@@ -0,0 +1,62 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+// * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
+
+inline Foam::RBD::subBody::subBody
+(
+    autoPtr<rigidBody> bodyPtr,
+    const label parentID,
+    const spatialTransform& parentXT
+)
+:
+    rigidBody(bodyPtr()),
+    body_(bodyPtr),
+    parentID_(parentID),
+    parentXT_(parentXT)
+{}
+
+
+// * * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * //
+
+inline const Foam::RBD::rigidBody& Foam::RBD::subBody::body() const
+{
+    return body_();
+}
+
+
+inline Foam::label Foam::RBD::subBody::parentID() const
+{
+    return parentID_;
+}
+
+
+inline const Foam::spatialTransform& Foam::RBD::subBody::parentXT() const
+{
+    return parentXT_;
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Pa/Pa.C

@@ -0,0 +1,98 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "Pa.H"
+#include "rigidBodyModel.H"
+#include "addToRunTimeSelectionTable.H"
+
+// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+    defineTypeNameAndDebug(Pa, 0);
+
+    addToRunTimeSelectionTable
+    (
+        joint,
+        Pa,
+        dictionary
+    );
+}
+}
+}
+
+
+// * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Pa::Pa(const rigidBodyModel& model, const vector& axis)
+:
+    joint(model, 1)
+{
+    S_[0] = spatialVector(Zero, axis);
+}
+
+
+Foam::RBD::joints::Pa::Pa(const rigidBodyModel& model, const dictionary& dict)
+:
+    joint(model, 1)
+{
+    S_[0] = spatialVector(Zero, dict.lookup("axis"));
+}
+
+
+Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Pa::clone() const
+{
+    return autoPtr<joint>(new Pa(*this));
+}
+
+
+// * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Pa::~Pa()
+{}
+
+
+// * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * * //
+
+void Foam::RBD::joints::Pa::jcalc
+(
+    joint::XSvc& J,
+    const scalarField& q,
+    const scalarField& w,
+    const scalarField& qDot
+) const
+{
+    J.X = Xt(S_[0].l()*q[qIndex_]);
+    J.S1 = S_[0];
+    J.v = S_[0]*qDot[qIndex_];
+    J.c = Zero;
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Pa/Pa.H

@@ -0,0 +1,111 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+Class
+    Foam::RBD::joints::Pa
+
+Description
+    Prismatic joint for translation along the specified arbitrary axis.
+
+    Reference:
+    \verbatim
+        Featherstone, R. (2008).
+        Rigid body dynamics algorithms.
+        Springer.
+        Chapter 4.
+    \endverbatim
+
+SourceFiles
+    Pa.C
+
+\*---------------------------------------------------------------------------*/
+
+#ifndef RBD_joints_Pa_H
+#define RBD_joints_Pa_H
+
+#include "joint.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+
+/*---------------------------------------------------------------------------*\
+                         Class Pa Declaration
+\*---------------------------------------------------------------------------*/
+
+class Pa
+:
+    public joint
+{
+
+public:
+
+    //- Runtime type information
+    TypeName("Pa");
+
+
+    // Constructors
+
+        //- Construct for given model and axis
+        Pa(const rigidBodyModel& model, const vector& axis);
+
+        //- Construct for given model from dictionary
+        Pa(const rigidBodyModel&, const dictionary& dict);
+
+        //- Clone this joint
+        virtual autoPtr<joint> clone() const;
+
+
+    //- Destructor
+    virtual ~Pa();
+
+
+    // Member Functions
+
+        //- Update the model state for this joint
+        virtual void jcalc
+        (
+            joint::XSvc& J,
+            const scalarField& q,
+            const scalarField& w,
+            const scalarField& qDot
+        ) const;
+};
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace joints
+} // End namespace RBD
+} // End namespace Foam
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#endif
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Px/Px.C

@@ -0,0 +1,98 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "Px.H"
+#include "rigidBodyModel.H"
+#include "addToRunTimeSelectionTable.H"
+
+// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+    defineTypeNameAndDebug(Px, 0);
+
+    addToRunTimeSelectionTable
+    (
+        joint,
+        Px,
+        dictionary
+    );
+}
+}
+}
+
+
+// * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Px::Px(const rigidBodyModel& model)
+:
+    joint(model, 1)
+{
+    S_[0] = spatialVector(0, 0, 0, 1, 0, 0);
+}
+
+
+Foam::RBD::joints::Px::Px(const rigidBodyModel& model, const dictionary& dict)
+:
+    joint(model, 1)
+{
+    S_[0] = spatialVector(0, 0, 0, 1, 0, 0);
+}
+
+
+Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Px::clone() const
+{
+    return autoPtr<joint>(new Px(*this));
+}
+
+
+// * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Px::~Px()
+{}
+
+
+// * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * * //
+
+void Foam::RBD::joints::Px::jcalc
+(
+    joint::XSvc& J,
+    const scalarField& q,
+    const scalarField& w,
+    const scalarField& qDot
+) const
+{
+    J.X = Xt(S_[0].l()*q[qIndex_]);
+    J.S1 = S_[0];
+    J.v = S_[0]*qDot[qIndex_];
+    J.c = Zero;
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Px/Px.H

@@ -0,0 +1,111 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+Class
+    Foam::RBD::joints::Px
+
+Description
+    Prismatic joint for translation along the x-axis.
+
+    Reference:
+    \verbatim
+        Featherstone, R. (2008).
+        Rigid body dynamics algorithms.
+        Springer.
+        Chapter 4.
+    \endverbatim
+
+SourceFiles
+    Px.C
+
+\*---------------------------------------------------------------------------*/
+
+#ifndef RBD_joints_Px_H
+#define RBD_joints_Px_H
+
+#include "joint.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+
+/*---------------------------------------------------------------------------*\
+                         Class Px Declaration
+\*---------------------------------------------------------------------------*/
+
+class Px
+:
+    public joint
+{
+
+public:
+
+    //- Runtime type information
+    TypeName("Px");
+
+
+    // Constructors
+
+        //- Construct for given model
+        Px(const rigidBodyModel& model);
+
+        //- Construct for given model from dictionary
+        Px(const rigidBodyModel&, const dictionary& dict);
+
+        //- Clone this joint
+        virtual autoPtr<joint> clone() const;
+
+
+    //- Destructor
+    virtual ~Px();
+
+
+    // Member Functions
+
+        //- Update the model state for this joint
+        virtual void jcalc
+        (
+            joint::XSvc& J,
+            const scalarField& q,
+            const scalarField& w,
+            const scalarField& qDot
+        ) const;
+};
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace joints
+} // End namespace RBD
+} // End namespace Foam
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#endif
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Pxyz/Pxyz.C

@@ -0,0 +1,112 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "Pxyz.H"
+#include "rigidBodyModel.H"
+#include "addToRunTimeSelectionTable.H"
+
+// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+    defineTypeNameAndDebug(Pxyz, 0);
+
+    addToRunTimeSelectionTable
+    (
+        joint,
+        Pxyz,
+        dictionary
+    );
+}
+}
+}
+
+
+// * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Pxyz::Pxyz(const rigidBodyModel& model)
+:
+    joint(model, 3)
+{
+    S_[0] = spatialVector(0, 0, 0, 1, 0, 0);
+    S_[1] = spatialVector(0, 0, 0, 0, 1, 0);
+    S_[2] = spatialVector(0, 0, 0, 0, 0, 1);
+}
+
+
+Foam::RBD::joints::Pxyz::Pxyz
+(
+    const rigidBodyModel& model,
+    const dictionary& dict
+)
+:
+    joint(model, 3)
+{
+    S_[0] = spatialVector(0, 0, 0, 1, 0, 0);
+    S_[1] = spatialVector(0, 0, 0, 0, 1, 0);
+    S_[2] = spatialVector(0, 0, 0, 0, 0, 1);
+}
+
+
+Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Pxyz::clone() const
+{
+    return autoPtr<joint>(new Pxyz(*this));
+}
+
+
+// * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Pxyz::~Pxyz()
+{}
+
+
+// * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * * //
+
+void Foam::RBD::joints::Pxyz::jcalc
+(
+    joint::XSvc& J,
+    const scalarField& q,
+    const scalarField& w,
+    const scalarField& qDot
+) const
+{
+    J.X.E() = tensor::I;
+    J.X.r() = q.block<vector>(qIndex_);
+
+    J.S = Zero;
+    J.S(3,0) = 1;
+    J.S(4,1) = 1;
+    J.S(5,2) = 1;
+
+    J.v = spatialVector(Zero, qDot.block<vector>(qIndex_));
+    J.c = Zero;
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Pxyz/Pxyz.H

@@ -0,0 +1,111 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+Class
+    Foam::RBD::joints::Pxyz
+
+Description
+    Prismatic joint for translation in the x/y/z directions.
+
+    Reference:
+    \verbatim
+        Featherstone, R. (2008).
+        Rigid body dynamics algorithms.
+        Springer.
+        Chapter 4.
+    \endverbatim
+
+SourceFiles
+    Pxyz.C
+
+\*---------------------------------------------------------------------------*/
+
+#ifndef RBD_joints_Pxyz_H
+#define RBD_joints_Pxyz_H
+
+#include "joint.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+
+/*---------------------------------------------------------------------------*\
+                         Class Pxyz Declaration
+\*---------------------------------------------------------------------------*/
+
+class Pxyz
+:
+    public joint
+{
+
+public:
+
+    //- Runtime type information
+    TypeName("Pxyz");
+
+
+    // Constructors
+
+        //- Construct for given model
+        Pxyz(const rigidBodyModel& model);
+
+        //- Construct for given model from dictionary
+        Pxyz(const rigidBodyModel&, const dictionary& dict);
+
+        //- Clone this joint
+        virtual autoPtr<joint> clone() const;
+
+
+    //- Destructor
+    virtual ~Pxyz();
+
+
+    // Member Functions
+
+        //- Update the model state for this joint
+        virtual void jcalc
+        (
+            joint::XSvc& J,
+            const scalarField& q,
+            const scalarField& w,
+            const scalarField& qDot
+        ) const;
+};
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace joints
+} // End namespace RBD
+} // End namespace Foam
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#endif
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Py/Py.C

@@ -0,0 +1,98 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "Py.H"
+#include "rigidBodyModel.H"
+#include "addToRunTimeSelectionTable.H"
+
+// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+    defineTypeNameAndDebug(Py, 0);
+
+    addToRunTimeSelectionTable
+    (
+        joint,
+        Py,
+        dictionary
+    );
+}
+}
+}
+
+
+// * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Py::Py(const rigidBodyModel& model)
+:
+    joint(model, 1)
+{
+    S_[0] = spatialVector(0, 0, 0, 0, 1, 0);
+}
+
+
+Foam::RBD::joints::Py::Py(const rigidBodyModel& model, const dictionary& dict)
+:
+    joint(model, 1)
+{
+    S_[0] = spatialVector(0, 0, 0, 0, 1, 0);
+}
+
+
+Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Py::clone() const
+{
+    return autoPtr<joint>(new Py(*this));
+}
+
+
+// * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Py::~Py()
+{}
+
+
+// * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * * //
+
+void Foam::RBD::joints::Py::jcalc
+(
+    joint::XSvc& J,
+    const scalarField& q,
+    const scalarField& w,
+    const scalarField& qDot
+) const
+{
+    J.X = Xt(S_[0].l()*q[qIndex_]);
+    J.S1 = S_[0];
+    J.v = S_[0]*qDot[qIndex_];
+    J.c = Zero;
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Py/Py.H

@@ -0,0 +1,111 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+Class
+    Foam::RBD::joints::Py
+
+Description
+    Prismatic joint for translation along the y-axis.
+
+    Reference:
+    \verbatim
+        Featherstone, R. (2008).
+        Rigid body dynamics algorithms.
+        Springer.
+        Chapter 4.
+    \endverbatim
+
+SourceFiles
+    Py.C
+
+\*---------------------------------------------------------------------------*/
+
+#ifndef RBD_joints_Py_H
+#define RBD_joints_Py_H
+
+#include "joint.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+
+/*---------------------------------------------------------------------------*\
+                         Class Py Declaration
+\*---------------------------------------------------------------------------*/
+
+class Py
+:
+    public joint
+{
+
+public:
+
+    //- Runtime type information
+    TypeName("Py");
+
+
+    // Constructors
+
+        //- Construct for given model
+        Py(const rigidBodyModel& model);
+
+        //- Construct for given model from dictionary
+        Py(const rigidBodyModel&, const dictionary& dict);
+
+        //- Clone this joint
+        virtual autoPtr<joint> clone() const;
+
+
+    //- Destructor
+    virtual ~Py();
+
+
+    // Member Functions
+
+        //- Update the model state for this joint
+        virtual void jcalc
+        (
+            joint::XSvc& J,
+            const scalarField& q,
+            const scalarField& w,
+            const scalarField& qDot
+        ) const;
+};
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace joints
+} // End namespace RBD
+} // End namespace Foam
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#endif
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Pz/Pz.C

@@ -0,0 +1,98 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "Pz.H"
+#include "rigidBodyModel.H"
+#include "addToRunTimeSelectionTable.H"
+
+// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+    defineTypeNameAndDebug(Pz, 0);
+
+    addToRunTimeSelectionTable
+    (
+        joint,
+        Pz,
+        dictionary
+    );
+}
+}
+}
+
+
+// * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Pz::Pz(const rigidBodyModel& model)
+:
+    joint(model, 1)
+{
+    S_[0] = spatialVector(0, 0, 0, 0, 0, 1);
+}
+
+
+Foam::RBD::joints::Pz::Pz(const rigidBodyModel& model, const dictionary& dict)
+:
+    joint(model, 1)
+{
+    S_[0] = spatialVector(0, 0, 0, 0, 0, 1);
+}
+
+
+Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Pz::clone() const
+{
+    return autoPtr<joint>(new Pz(*this));
+}
+
+
+// * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Pz::~Pz()
+{}
+
+
+// * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * * //
+
+void Foam::RBD::joints::Pz::jcalc
+(
+    joint::XSvc& J,
+    const scalarField& q,
+    const scalarField& w,
+    const scalarField& qDot
+) const
+{
+    J.X = Xt(S_[0].l()*q[qIndex_]);
+    J.S1 = S_[0];
+    J.v = S_[0]*qDot[qIndex_];
+    J.c = Zero;
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Pz/Pz.H

@@ -0,0 +1,111 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+Class
+    Foam::RBD::joints::Pz
+
+Description
+    Prismatic joint for translation along the x-axis.
+
+    Reference:
+    \verbatim
+        Featherstone, R. (2008).
+        Rigid body dynamics algorithms.
+        Springer.
+        Chapter 4.
+    \endverbatim
+
+SourceFiles
+    Pz.C
+
+\*---------------------------------------------------------------------------*/
+
+#ifndef RBD_joints_Pz_H
+#define RBD_joints_Pz_H
+
+#include "joint.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+
+/*---------------------------------------------------------------------------*\
+                         Class Pz Declaration
+\*---------------------------------------------------------------------------*/
+
+class Pz
+:
+    public joint
+{
+
+public:
+
+    //- Runtime type information
+    TypeName("Pz");
+
+
+    // Constructors
+
+        //- Construct for given model
+        Pz(const rigidBodyModel& model);
+
+        //- Construct for given model from dictionary
+        Pz(const rigidBodyModel&, const dictionary& dict);
+
+        //- Clone this joint
+        virtual autoPtr<joint> clone() const;
+
+
+    //- Destructor
+    virtual ~Pz();
+
+
+    // Member Functions
+
+        //- Update the model state for this joint
+        virtual void jcalc
+        (
+            joint::XSvc& J,
+            const scalarField& q,
+            const scalarField& w,
+            const scalarField& qDot
+        ) const;
+};
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace joints
+} // End namespace RBD
+} // End namespace Foam
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#endif
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Ra/Ra.C

@@ -0,0 +1,98 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "Ra.H"
+#include "rigidBodyModel.H"
+#include "addToRunTimeSelectionTable.H"
+
+// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+    defineTypeNameAndDebug(Ra, 0);
+
+    addToRunTimeSelectionTable
+    (
+        joint,
+        Ra,
+        dictionary
+    );
+}
+}
+}
+
+
+// * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Ra::Ra(const rigidBodyModel& model, const vector& axis)
+:
+    joint(model, 1)
+{
+    S_[0] = spatialVector(axis, Zero);
+}
+
+
+Foam::RBD::joints::Ra::Ra(const rigidBodyModel& model, const dictionary& dict)
+:
+    joint(model, 1)
+{
+    S_[0] = spatialVector(dict.lookup("axis"), Zero);
+}
+
+
+Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Ra::clone() const
+{
+    return autoPtr<joint>(new Ra(*this));
+}
+
+
+// * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Ra::~Ra()
+{}
+
+
+// * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * * //
+
+void Foam::RBD::joints::Ra::jcalc
+(
+    joint::XSvc& J,
+    const scalarField& q,
+    const scalarField& w,
+    const scalarField& qDot
+) const
+{
+    J.X = Xr(S_[0].w(), q[qIndex_]);
+    J.S1 = S_[0];
+    J.v = S_[0]*qDot[qIndex_];
+    J.c = Zero;
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Ra/Ra.H

@@ -0,0 +1,111 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+Class
+    Foam::RBD::joints::Ra
+
+Description
+    Revolute joint for rotation about the specified arbitrary axis.
+
+    Reference:
+    \verbatim
+        Featherstone, R. (2008).
+        Rigid body dynamics algorithms.
+        Springer.
+        Chapter 4.
+    \endverbatim
+
+SourceFiles
+    Ra.C
+
+\*---------------------------------------------------------------------------*/
+
+#ifndef RBD_joints_Ra_H
+#define RBD_joints_Ra_H
+
+#include "joint.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+
+/*---------------------------------------------------------------------------*\
+                         Class Ra Declaration
+\*---------------------------------------------------------------------------*/
+
+class Ra
+:
+    public joint
+{
+
+public:
+
+    //- Runtime type information
+    TypeName("Ra");
+
+
+    // Constructors
+
+        //- Construct for given model and axis
+        Ra(const rigidBodyModel& model, const vector& axis);
+
+        //- Construct for given model from dictionary
+        Ra(const rigidBodyModel&, const dictionary& dict);
+
+        //- Clone this joint
+        virtual autoPtr<joint> clone() const;
+
+
+    //- Destructor
+    virtual ~Ra();
+
+
+    // Member Functions
+
+        //- Update the model state for this joint
+        virtual void jcalc
+        (
+            joint::XSvc& J,
+            const scalarField& q,
+            const scalarField& w,
+            const scalarField& qDot
+        ) const;
+};
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace joints
+} // End namespace RBD
+} // End namespace Foam
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#endif
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Rs/Rs.C

@@ -0,0 +1,114 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "Rs.H"
+#include "rigidBodyModel.H"
+#include "addToRunTimeSelectionTable.H"
+
+// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+    defineTypeNameAndDebug(Rs, 0);
+
+    addToRunTimeSelectionTable
+    (
+        joint,
+        Rs,
+        dictionary
+    );
+}
+}
+}
+
+
+// * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Rs::Rs(const rigidBodyModel& model)
+:
+    joint(model, 3)
+{
+    S_[0] = spatialVector(1, 0, 0, 0, 0, 0);
+    S_[1] = spatialVector (0, 1, 0, 0, 0, 0);
+    S_[2] = spatialVector(0, 0, 1, 0, 0, 0);
+}
+
+
+Foam::RBD::joints::Rs::Rs(const rigidBodyModel& model, const dictionary& dict)
+:
+    joint(model, 3)
+{
+    S_[0] = spatialVector(1, 0, 0, 0, 0, 0);
+    S_[1] = spatialVector (0, 1, 0, 0, 0, 0);
+    S_[2] = spatialVector(0, 0, 1, 0, 0, 0);
+}
+
+
+Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Rs::clone() const
+{
+    return autoPtr<joint>(new Rs(*this));
+}
+
+
+// * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Rs::~Rs()
+{}
+
+
+// * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * * //
+
+Foam::label Foam::RBD::joints::Rs::nw() const
+{
+    return 1;
+}
+
+
+void Foam::RBD::joints::Rs::jcalc
+(
+    joint::XSvc& J,
+    const scalarField& q,
+    const scalarField& w,
+    const scalarField& qDot
+) const
+{
+    J.X.E() = operator()(q, w).R().T();
+    J.X.r() = Zero;
+
+    J.S = Zero;
+    J.S.xx() = 1;
+    J.S.yy() = 1;
+    J.S.zz() = 1;
+
+    J.v = spatialVector(qDot.block<vector>(qIndex_), Zero);
+    J.c = Zero;
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Rs/Rs.H

@@ -0,0 +1,116 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+Class
+    Foam::RBD::joints::Rs
+
+Description
+    Spherical joint for rotation about the x/y/z-axes using a quaternion
+    (Euler parameters) to avoid gimble-lock.
+
+    Reference:
+    \verbatim
+        Featherstone, R. (2008).
+        Rigid body dynamics algorithms.
+        Springer.
+        Chapter 4.
+    \endverbatim
+
+SourceFiles
+    Rs.C
+
+\*---------------------------------------------------------------------------*/
+
+#ifndef RBD_joints_Rs_H
+#define RBD_joints_Rs_H
+
+#include "joint.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+
+/*---------------------------------------------------------------------------*\
+                         Class Rs Declaration
+\*---------------------------------------------------------------------------*/
+
+class Rs
+:
+    public joint
+{
+
+public:
+
+    //- Runtime type information
+    TypeName("Rs");
+
+
+    // Constructors
+
+        //- Construct for given model
+        Rs(const rigidBodyModel& model);
+
+        //- Construct for given model from dictionary
+        Rs(const rigidBodyModel&, const dictionary& dict);
+
+        //- Clone this joint
+        virtual autoPtr<joint> clone() const;
+
+
+    //- Destructor
+    virtual ~Rs();
+
+
+    // Member Functions
+
+        //- Return the number of additional state variable for this joint
+        //  For the quaternion this is 1 in addition to the 3 stored in q
+        virtual label nw() const;
+
+        //- Update the model state for this joint
+        virtual void jcalc
+        (
+            joint::XSvc& J,
+            const scalarField& q,
+            const scalarField& w,
+            const scalarField& qDot
+        ) const;
+};
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace joints
+} // End namespace RBD
+} // End namespace Foam
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#endif
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Rx/Rx.C

@@ -0,0 +1,99 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "Rx.H"
+#include "rigidBodyModel.H"
+#include "addToRunTimeSelectionTable.H"
+
+// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+    defineTypeNameAndDebug(Rx, 0);
+
+    addToRunTimeSelectionTable
+    (
+        joint,
+        Rx,
+        dictionary
+    );
+}
+}
+}
+
+
+// * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Rx::Rx(const rigidBodyModel& model)
+:
+    joint(model, 1)
+{
+    S_[0] = spatialVector(1, 0, 0, 0, 0, 0);
+}
+
+
+Foam::RBD::joints::Rx::Rx(const rigidBodyModel& model, const dictionary& dict)
+:
+    joint(model, 1)
+{
+    S_[0] = spatialVector(1, 0, 0, 0, 0, 0);
+}
+
+
+Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Rx::clone() const
+{
+    return autoPtr<joint>(new Rx(*this));
+}
+
+
+// * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Rx::~Rx()
+{}
+
+
+// * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * * //
+
+void Foam::RBD::joints::Rx::jcalc
+(
+    joint::XSvc& J,
+    const scalarField& q,
+    const scalarField& w,
+    const scalarField& qDot
+) const
+{
+    J.X = Xrx(q[qIndex_]);
+    J.S1 = S_[0];
+    J.v = Zero;
+    J.v.wx() = qDot[qIndex_];
+    J.c = Zero;
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Rx/Rx.H

@@ -0,0 +1,111 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+Class
+    Foam::RBD::joints::Rx
+
+Description
+    Revolute joint for rotation about the x-axis
+
+    Reference:
+    \verbatim
+        Featherstone, R. (2008).
+        Rigid body dynamics algorithms.
+        Springer.
+        Chapter 4.
+    \endverbatim
+
+SourceFiles
+    Rx.C
+
+\*---------------------------------------------------------------------------*/
+
+#ifndef RBD_joints_Rx_H
+#define RBD_joints_Rx_H
+
+#include "joint.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+
+/*---------------------------------------------------------------------------*\
+                         Class Rx Declaration
+\*---------------------------------------------------------------------------*/
+
+class Rx
+:
+    public joint
+{
+
+public:
+
+    //- Runtime type information
+    TypeName("Rx");
+
+
+    // Constructors
+
+        //- Construct for given model
+        Rx(const rigidBodyModel& model);
+
+        //- Construct for given model from dictionary
+        Rx(const rigidBodyModel&, const dictionary& dict);
+
+        //- Clone this joint
+        virtual autoPtr<joint> clone() const;
+
+
+    //- Destructor
+    virtual ~Rx();
+
+
+    // Member Functions
+
+        //- Update the model state for this joint
+        virtual void jcalc
+        (
+            joint::XSvc& J,
+            const scalarField& q,
+            const scalarField& w,
+            const scalarField& qDot
+        ) const;
+};
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace joints
+} // End namespace RBD
+} // End namespace Foam
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#endif
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Rxyz/Rxyz.C

@@ -0,0 +1,146 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "Rxyz.H"
+#include "rigidBodyModel.H"
+#include "addToRunTimeSelectionTable.H"
+
+// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+    defineTypeNameAndDebug(Rxyz, 0);
+
+    addToRunTimeSelectionTable
+    (
+        joint,
+        Rxyz,
+        dictionary
+    );
+}
+}
+}
+
+
+// * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Rxyz::Rxyz(const rigidBodyModel& model)
+:
+    joint(model, 3)
+{
+    S_[0] = spatialVector(1, 0, 0, 0, 0, 0);
+    S_[1] = spatialVector(0, 1, 0, 0, 0, 0);
+    S_[2] = spatialVector(0, 0, 1, 0, 0, 0);
+}
+
+
+Foam::RBD::joints::Rxyz::Rxyz
+(
+    const rigidBodyModel& model,
+    const dictionary& dict
+)
+:
+    joint(model, 3)
+{
+    S_[0] = spatialVector(1, 0, 0, 0, 0, 0);
+    S_[1] = spatialVector(0, 1, 0, 0, 0, 0);
+    S_[2] = spatialVector(0, 0, 1, 0, 0, 0);
+}
+
+
+Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Rxyz::clone() const
+{
+    return autoPtr<joint>(new Rxyz(*this));
+}
+
+
+// * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Rxyz::~Rxyz()
+{}
+
+
+// * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * * //
+
+void Foam::RBD::joints::Rxyz::jcalc
+(
+    joint::XSvc& J,
+    const scalarField& q,
+    const scalarField& w,
+    const scalarField& qDot
+) const
+{
+    vector qj(q.block<vector>(qIndex_));
+
+    scalar s0 = sin(qj.x());
+    scalar c0 = cos(qj.x());
+    scalar s1 = sin(qj.y());
+    scalar c1 = cos(qj.y());
+    scalar s2 = sin(qj.z());
+    scalar c2 = cos(qj.z());
+
+    J.X.E() = tensor
+    (
+        c2*c1,            s2*c0 + c2*s1*s0, s2*s0 - c2*s1*c0,
+       -s2*c1,            c2*c0 - s2*s1*s0, c2*s0 + s2*s1*c0,
+        s1,              -c1*s0,            c1*c0
+    );
+    J.X.r() = Zero;
+
+    J.S = Zero;
+    J.S.xx() = c2*c1;
+    J.S.xy() = s2;
+    J.S.yx() = -s2*c1;
+    J.S.yy() = c2;
+    J.S.zx() = s1;
+    J.S.zz() = 1;
+
+    vector qDotj(qDot.block<vector>(qIndex_));
+    J.v = J.S & qDotj;
+
+    J.c = spatialVector
+    (
+       -s2*c1*qDotj.z()*qDotj.x()
+      - c2*s1*qDotj.y()*qDotj.x()
+      + c2*qDotj.z()*qDotj.y(),
+
+       -c2*c1*qDotj.z()*qDotj.x()
+      + s2*s1*qDotj.y()*qDotj.x()
+      - s2*qDotj.z()*qDotj.y(),
+
+        c1*qDotj.y()*qDotj.x(),
+
+        0,
+        0,
+        0
+    );
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Rxyz/Rxyz.H

@@ -0,0 +1,112 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+Class
+    Foam::RBD::joints::Rxyz
+
+Description
+    Spherical joint for rotation about the x/y/z-axes using Euler-angles
+    in the order x, y, z.
+
+    Reference:
+    \verbatim
+        Featherstone, R. (2008).
+        Rigid body dynamics algorithms.
+        Springer.
+        Chapter 4.
+    \endverbatim
+
+SourceFiles
+    Rxyz.C
+
+\*---------------------------------------------------------------------------*/
+
+#ifndef RBD_joints_Rxyz_H
+#define RBD_joints_Rxyz_H
+
+#include "joint.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+
+/*---------------------------------------------------------------------------*\
+                         Class Rxyz Declaration
+\*---------------------------------------------------------------------------*/
+
+class Rxyz
+:
+    public joint
+{
+
+public:
+
+    //- Runtime type information
+    TypeName("Rxyz");
+
+
+    // Constructors
+
+        //- Construct for given model
+        Rxyz(const rigidBodyModel& model);
+
+        //- Construct for given model from dictionary
+        Rxyz(const rigidBodyModel&, const dictionary& dict);
+
+        //- Clone this joint
+        virtual autoPtr<joint> clone() const;
+
+
+    //- Destructor
+    virtual ~Rxyz();
+
+
+    // Member Functions
+
+        //- Update the model state for this joint
+        virtual void jcalc
+        (
+            joint::XSvc& J,
+            const scalarField& q,
+            const scalarField& w,
+            const scalarField& qDot
+        ) const;
+};
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace joints
+} // End namespace RBD
+} // End namespace Foam
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#endif
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Ry/Ry.C

@@ -0,0 +1,99 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "Ry.H"
+#include "rigidBodyModel.H"
+#include "addToRunTimeSelectionTable.H"
+
+// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+    defineTypeNameAndDebug(Ry, 0);
+
+    addToRunTimeSelectionTable
+    (
+        joint,
+        Ry,
+        dictionary
+    );
+}
+}
+}
+
+
+// * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Ry::Ry(const rigidBodyModel& model)
+:
+    joint(model, 1)
+{
+    S_[0] = spatialVector(0, 1, 0, 0, 0, 0);
+}
+
+
+Foam::RBD::joints::Ry::Ry(const rigidBodyModel& model, const dictionary& dict)
+:
+    joint(model, 1)
+{
+    S_[0] = spatialVector(0, 1, 0, 0, 0, 0);
+}
+
+
+Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Ry::clone() const
+{
+    return autoPtr<joint>(new Ry(*this));
+}
+
+
+// * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Ry::~Ry()
+{}
+
+
+// * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * * //
+
+void Foam::RBD::joints::Ry::jcalc
+(
+    joint::XSvc& J,
+    const scalarField& q,
+    const scalarField& w,
+    const scalarField& qDot
+) const
+{
+    J.X = Xry(q[qIndex_]);
+    J.S1 = S_[0];
+    J.v = Zero;
+    J.v.wy() = qDot[qIndex_];
+    J.c = Zero;
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Ry/Ry.H

@@ -0,0 +1,111 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+Class
+    Foam::RBD::joints::Ry
+
+Description
+    Revolute joint for rotation about the y-axis
+
+    Reference:
+    \verbatim
+        Featherstone, R. (2008).
+        Rigid body dynamics algorithms.
+        Springer.
+        Chapter 4.
+    \endverbatim
+
+SourceFiles
+    Ry.C
+
+\*---------------------------------------------------------------------------*/
+
+#ifndef RBD_joints_Ry_H
+#define RBD_joints_Ry_H
+
+#include "joint.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+
+/*---------------------------------------------------------------------------*\
+                         Class Ry Declaration
+\*---------------------------------------------------------------------------*/
+
+class Ry
+:
+    public joint
+{
+
+public:
+
+    //- Runtime type information
+    TypeName("Ry");
+
+
+    // Constructors
+
+        //- Construct for given model
+        Ry(const rigidBodyModel& model);
+
+        //- Construct for given model from dictionary
+        Ry(const rigidBodyModel&, const dictionary& dict);
+
+        //- Clone this joint
+        virtual autoPtr<joint> clone() const;
+
+
+    //- Destructor
+    virtual ~Ry();
+
+
+    // Member Functions
+
+        //- Update the model state for this joint
+        virtual void jcalc
+        (
+            joint::XSvc& J,
+            const scalarField& q,
+            const scalarField& w,
+            const scalarField& qDot
+        ) const;
+};
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace joints
+} // End namespace RBD
+} // End namespace Foam
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#endif
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Ryxz/Ryxz.C

@@ -0,0 +1,146 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "Ryxz.H"
+#include "rigidBodyModel.H"
+#include "addToRunTimeSelectionTable.H"
+
+// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+    defineTypeNameAndDebug(Ryxz, 0);
+
+    addToRunTimeSelectionTable
+    (
+        joint,
+        Ryxz,
+        dictionary
+    );
+}
+}
+}
+
+
+// * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Ryxz::Ryxz(const rigidBodyModel& model)
+:
+    joint(model, 3)
+{
+    S_[0] = spatialVector(0, 1, 0, 0, 0, 0);
+    S_[1] = spatialVector(1, 0, 0, 0, 0, 0);
+    S_[2] = spatialVector(0, 0, 1, 0, 0, 0);
+}
+
+
+Foam::RBD::joints::Ryxz::Ryxz
+(
+    const rigidBodyModel& model,
+    const dictionary& dict
+)
+:
+    joint(model, 3)
+{
+    S_[0] = spatialVector(0, 1, 0, 0, 0, 0);
+    S_[1] = spatialVector(1, 0, 0, 0, 0, 0);
+    S_[2] = spatialVector(0, 0, 1, 0, 0, 0);
+}
+
+
+Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Ryxz::clone() const
+{
+    return autoPtr<joint>(new Ryxz(*this));
+}
+
+
+// * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Ryxz::~Ryxz()
+{}
+
+
+// * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * * //
+
+void Foam::RBD::joints::Ryxz::jcalc
+(
+    joint::XSvc& J,
+    const scalarField& q,
+    const scalarField& w,
+    const scalarField& qDot
+) const
+{
+    vector qj(q.block<vector>(qIndex_));
+
+    scalar s0 = sin(qj.x());
+    scalar c0 = cos(qj.x());
+    scalar s1 = sin(qj.y());
+    scalar c1 = cos(qj.y());
+    scalar s2 = sin(qj.z());
+    scalar c2 = cos(qj.z());
+
+    J.X.E() = tensor
+    (
+        c2*c0 + s2*s1*s0, s2*c1,           -c2*s0 + s2*s1*c0,
+       -s2*c0 + c2*s1*s0, c2*c1,            s2*s0 + c2*s1*c0,
+        c1*s0,           -s1,               c1*c0
+    );
+    J.X.r() = Zero;
+
+    J.S = Zero;
+    J.S.xx() = s2*c1;
+    J.S.xy() = c2;
+    J.S.yx() = c2*c1;
+    J.S.yy() = -s2;
+    J.S.zx() = -s1;
+    J.S.zz() = 1;
+
+    vector qDotj(qDot.block<vector>(qIndex_));
+    J.v = J.S & qDotj;
+
+    J.c = spatialVector
+    (
+        c2*c1*qDotj.z()*qDotj.x()
+      - s2*s1*qDotj.y()*qDotj.x()
+      - s2*qDotj.z()*qDotj.y(),
+
+       -s2*c1*qDotj.z()*qDotj.x()
+      - c2*s1*qDotj.y()*qDotj.x()
+      - c2*qDotj.z()*qDotj.y(),
+
+       -c1*qDotj.y()*qDotj.x(),
+
+        0,
+        0,
+        0
+    );
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Ryxz/Ryxz.H

@@ -0,0 +1,112 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+Class
+    Foam::RBD::joints::Ryxz
+
+Description
+    Spherical joint for rotation about the x/y/z-axes using Euler-angles
+    in the order y, x, z.
+
+    Reference:
+    \verbatim
+        Featherstone, R. (2008).
+        Rigid body dynamics algorithms.
+        Springer.
+        Chapter 4.
+    \endverbatim
+
+SourceFiles
+    Ryxz.C
+
+\*---------------------------------------------------------------------------*/
+
+#ifndef RBD_joints_Ryxz_H
+#define RBD_joints_Ryxz_H
+
+#include "joint.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+
+/*---------------------------------------------------------------------------*\
+                         Class Ryxz Declaration
+\*---------------------------------------------------------------------------*/
+
+class Ryxz
+:
+    public joint
+{
+
+public:
+
+    //- Runtime type information
+    TypeName("Ryxz");
+
+
+    // Constructors
+
+        //- Construct for given model
+        Ryxz(const rigidBodyModel& model);
+
+        //- Construct for given model from dictionary
+        Ryxz(const rigidBodyModel&, const dictionary& dict);
+
+        //- Clone this joint
+        virtual autoPtr<joint> clone() const;
+
+
+    //- Destructor
+    virtual ~Ryxz();
+
+
+    // Member Functions
+
+        //- Update the model state for this joint
+        virtual void jcalc
+        (
+            joint::XSvc& J,
+            const scalarField& q,
+            const scalarField& w,
+            const scalarField& qDot
+        ) const;
+};
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace joints
+} // End namespace RBD
+} // End namespace Foam
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#endif
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Rz/Rz.C

@@ -0,0 +1,99 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "Rz.H"
+#include "rigidBodyModel.H"
+#include "addToRunTimeSelectionTable.H"
+
+// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+    defineTypeNameAndDebug(Rz, 0);
+
+    addToRunTimeSelectionTable
+    (
+        joint,
+        Rz,
+        dictionary
+    );
+}
+}
+}
+
+
+// * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Rz::Rz(const rigidBodyModel& model)
+:
+    joint(model, 1)
+{
+    S_[0] = spatialVector(0, 0, 1, 0, 0, 0);
+}
+
+
+Foam::RBD::joints::Rz::Rz(const rigidBodyModel& model, const dictionary& dict)
+:
+    joint(model, 1)
+{
+    S_[0] = spatialVector(0, 0, 1, 0, 0, 0);
+}
+
+
+Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Rz::clone() const
+{
+    return autoPtr<joint>(new Rz(*this));
+}
+
+
+// * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Rz::~Rz()
+{}
+
+
+// * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * * //
+
+void Foam::RBD::joints::Rz::jcalc
+(
+    joint::XSvc& J,
+    const scalarField& q,
+    const scalarField& w,
+    const scalarField& qDot
+) const
+{
+    J.X = Xrz(q[qIndex_]);
+    J.S1 = S_[0];
+    J.v = Zero;
+    J.v.wz() = qDot[qIndex_];
+    J.c = Zero;
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Rz/Rz.H

@@ -0,0 +1,111 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+Class
+    Foam::RBD::joints::Rz
+
+Description
+    Revolute joint for rotation about the z-axis
+
+    Reference:
+    \verbatim
+        Featherstone, R. (2008).
+        Rigid body dynamics algorithms.
+        Springer.
+        Chapter 4.
+    \endverbatim
+
+SourceFiles
+    Rz.C
+
+\*---------------------------------------------------------------------------*/
+
+#ifndef RBD_joints_Rz_H
+#define RBD_joints_Rz_H
+
+#include "joint.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+
+/*---------------------------------------------------------------------------*\
+                         Class Rz Declaration
+\*---------------------------------------------------------------------------*/
+
+class Rz
+:
+    public joint
+{
+
+public:
+
+    //- Runtime type information
+    TypeName("Rz");
+
+
+    // Constructors
+
+        //- Construct for given model
+        Rz(const rigidBodyModel& model);
+
+        //- Construct for given model from dictionary
+        Rz(const rigidBodyModel&, const dictionary& dict);
+
+        //- Clone this joint
+        virtual autoPtr<joint> clone() const;
+
+
+    //- Destructor
+    virtual ~Rz();
+
+
+    // Member Functions
+
+        //- Update the model state for this joint
+        virtual void jcalc
+        (
+            joint::XSvc& J,
+            const scalarField& q,
+            const scalarField& w,
+            const scalarField& qDot
+        ) const;
+};
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace joints
+} // End namespace RBD
+} // End namespace Foam
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#endif
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Rzyx/Rzyx.C

@@ -0,0 +1,146 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "Rzyx.H"
+#include "rigidBodyModel.H"
+#include "addToRunTimeSelectionTable.H"
+
+// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+    defineTypeNameAndDebug(Rzyx, 0);
+
+    addToRunTimeSelectionTable
+    (
+        joint,
+        Rzyx,
+        dictionary
+    );
+}
+}
+}
+
+
+// * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Rzyx::Rzyx(const rigidBodyModel& model)
+:
+    joint(model, 3)
+{
+    S_[0] = spatialVector(0, 0, 1, 0, 0, 0);
+    S_[1] = spatialVector(0, 1, 0, 0, 0, 0);
+    S_[2] = spatialVector(1, 0, 0, 0, 0, 0);
+}
+
+
+Foam::RBD::joints::Rzyx::Rzyx
+(
+    const rigidBodyModel& model,
+    const dictionary& dict
+)
+:
+    joint(model, 3)
+{
+    S_[0] = spatialVector(0, 0, 1, 0, 0, 0);
+    S_[1] = spatialVector(0, 1, 0, 0, 0, 0);
+    S_[2] = spatialVector(1, 0, 0, 0, 0, 0);
+}
+
+
+Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Rzyx::clone() const
+{
+    return autoPtr<joint>(new Rzyx(*this));
+}
+
+
+// * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::Rzyx::~Rzyx()
+{}
+
+
+// * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * * //
+
+void Foam::RBD::joints::Rzyx::jcalc
+(
+    joint::XSvc& J,
+    const scalarField& q,
+    const scalarField& w,
+    const scalarField& qDot
+) const
+{
+    vector qj(q.block<vector>(qIndex_));
+
+    scalar s0 = sin(qj.x());
+    scalar c0 = cos(qj.x());
+    scalar s1 = sin(qj.y());
+    scalar c1 = cos(qj.y());
+    scalar s2 = sin(qj.z());
+    scalar c2 = cos(qj.z());
+
+    J.X.E() = tensor
+    (
+        c0*c1,            s0*c1,           -s1,
+        c0*s1*s2 - s0*c2, s0*s1*s2 + c0*c2, c1*s2,
+        c0*s1*c2 + s0*s2, s0*s1*c2 - c0*s2, c1*c2
+    );
+    J.X.r() = Zero;
+
+    J.S = Zero;
+    J.S.xx() = -s1;
+    J.S.xz() = 1;
+    J.S.yx() = c1*s2;
+    J.S.yy() = c2;
+    J.S.zx() = c1*c2;
+    J.S.zy() = -s2;
+
+    vector qDotj(qDot.block<vector>(qIndex_));
+    J.v = J.S & qDotj;
+
+    J.c = spatialVector
+    (
+       -c1*qDotj.x()*qDotj.y(),
+
+       -s1*s2*qDotj.x()*qDotj.y()
+      + c1*c2*qDotj.x()*qDotj.z()
+      - s2*qDotj.y()*qDotj.z(),
+
+       -s1*c2*qDotj.x()*qDotj.y()
+      - c1*s2*qDotj.x()*qDotj.z()
+      - c2*qDotj.y()*qDotj.z(),
+
+        0,
+        0,
+        0
+    );
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/Rzyx/Rzyx.H

@@ -0,0 +1,112 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+Class
+    Foam::RBD::joints::Rzyx
+
+Description
+    Spherical joint for rotation about the x/y/z-axes using Euler-angles
+    in the order z, y, x.
+
+    Reference:
+    \verbatim
+        Featherstone, R. (2008).
+        Rigid body dynamics algorithms.
+        Springer.
+        Chapter 4.
+    \endverbatim
+
+SourceFiles
+    Rzyx.C
+
+\*---------------------------------------------------------------------------*/
+
+#ifndef RBD_joints_Rzyx_H
+#define RBD_joints_Rzyx_H
+
+#include "joint.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+
+/*---------------------------------------------------------------------------*\
+                         Class Rzyx Declaration
+\*---------------------------------------------------------------------------*/
+
+class Rzyx
+:
+    public joint
+{
+
+public:
+
+    //- Runtime type information
+    TypeName("Rzyx");
+
+
+    // Constructors
+
+        //- Construct for given model
+        Rzyx(const rigidBodyModel& model);
+
+        //- Construct for given model from dictionary
+        Rzyx(const rigidBodyModel&, const dictionary& dict);
+
+        //- Clone this joint
+        virtual autoPtr<joint> clone() const;
+
+
+    //- Destructor
+    virtual ~Rzyx();
+
+
+    // Member Functions
+
+        //- Update the model state for this joint
+        virtual void jcalc
+        (
+            joint::XSvc& J,
+            const scalarField& q,
+            const scalarField& w,
+            const scalarField& qDot
+        ) const;
+};
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace joints
+} // End namespace RBD
+} // End namespace Foam
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#endif
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/joint/joint.C

@@ -0,0 +1,77 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "joint.H"
+#include "rigidBodyModel.H"
+#include "Rs.H"
+#include "Rzyx.H"
+#include "Pxyz.H"
+
+// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+    defineTypeNameAndDebug(joint, 0);
+    defineRunTimeSelectionTable(joint, dictionary);
+}
+}
+
+
+// * * * * * * * * * * * * * * * * Selectors * * * * * * * * * * * * * * * * //
+
+Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joint::New(joint* jointPtr)
+{
+    return autoPtr<joint>(jointPtr);
+}
+
+
+// * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
+
+Foam::RBD::joint::~joint()
+{}
+
+
+// * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * * //
+
+Foam::PtrList<Foam::RBD::joint> Foam::RBD::joint::floating
+(
+    const rigidBodyModel& model
+)
+{
+    PtrList<joint> cj(2);
+    cj.set(0, new joints::Pxyz(model));
+    //cj.set(1, new joints::Rs(model));
+    cj.set(1, new joints::Rzyx(model));
+    return cj;
+}
+
+
+void Foam::RBD::joint::write(Ostream& os) const
+{}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/joint/joint.H

@@ -0,0 +1,269 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+Class
+    Foam::RBD::joint
+
+Description
+    Abstract base-class for all rigid-body joints.
+
+    Reference:
+    \verbatim
+        Featherstone, R. (2008).
+        Rigid body dynamics algorithms.
+        Springer.
+        Chapter 4.
+    \endverbatim
+
+SourceFiles
+    jointI.H
+    joint.C
+
+\*---------------------------------------------------------------------------*/
+
+#ifndef RBD_joint_H
+#define RBD_joint_H
+
+#include "List.H"
+#include "spatialVector.H"
+#include "compactSpatialTensor.H"
+#include "CompactSpatialTensorT.H"
+#include "spatialTransform.H"
+#include "quaternion.H"
+#include "scalarField.H"
+#include "runTimeSelectionTables.H"
+
+namespace Foam
+{
+namespace RBD
+{
+
+// Forward declaration of classes
+class rigidBodyModel;
+
+/*---------------------------------------------------------------------------*\
+                         Class joint Declaration
+\*---------------------------------------------------------------------------*/
+
+class joint
+{
+
+protected:
+
+    // Protected data
+
+    //- Joint motion sub-space
+    List<spatialVector> S_;
+
+    //- Index of this joint in the rigidBodyModel
+    label index_;
+
+    //- Index of this joints data in the rigidBodyModel state
+    label qIndex_;
+
+    //- Index of this joints quaternion data in the rigidBodyModel state
+    label wIndex_;
+
+
+private:
+
+    // Private member functions to be used by rigidBodyModel
+
+        //- Allow the rigidBodyModel to set the index for this joint
+        label& index()
+        {
+            return index_;
+        }
+
+        //- Allow the rigidBodyModel to set the qIndex for this joint
+        label& qIndex()
+        {
+            return qIndex_;
+        }
+
+        //- Allow the rigidBodyModel to set the wIndex for this joint
+        label& wIndex()
+        {
+            return wIndex_;
+        }
+
+
+public:
+
+    //- Allow the rigidBodyModel class to set the joint indices
+    friend class rigidBodyModel;
+
+    //- Joint state returned by jcalc
+    class XSvc
+    {
+    public:
+
+        //- The joint transformation
+        spatialTransform X;
+
+        //- The joint motion sub-space (3-DoF)
+        compactSpatialTensor S;
+
+        //- The joint motion sub-space (1-DoF)
+        spatialVector S1;
+
+        //- The constrained joint velocity
+        spatialVector v;
+
+        //- The constrained joint acceleration correction
+        //  due to changes in the motion sub-space S
+        spatialVector c;
+
+        //- Null constructor
+        XSvc()
+        :
+            X(),
+            v(Zero),
+            c(Zero)
+        {}
+    };
+
+
+public:
+
+    //- Runtime type information
+    TypeName("joint");
+
+
+    // Declare run-time constructor selection table
+
+        declareRunTimeSelectionTable
+        (
+            autoPtr,
+            joint,
+            dictionary,
+            (
+                const rigidBodyModel& model,
+                const dictionary& dict
+            ),
+            (model, dict)
+        );
+
+
+    // Constructors
+
+        //- Construct joint for given rigidBodyModel
+        //  setting the size of the motion sub-space
+        //  to the given degrees of freedom of the joint
+        inline joint(const rigidBodyModel&, const label nDoF);
+
+        //- Clone this joint (needed by PtrList)
+        virtual autoPtr<joint> clone() const = 0;
+
+
+    //- Destructor
+    virtual ~joint();
+
+
+    // Selectors
+
+        //- Simple selector to return an autoPtr<joint> of the given joint*
+        static autoPtr<joint> New(joint* jointPtr);
+
+        //- Select constructed from dictionary
+        static autoPtr<joint> New
+        (
+            const rigidBodyModel& model,
+            const dictionary& dict
+        );
+
+
+    // Member Functions
+
+        //- Return the number of degrees of freedom in this joint
+        inline label nDoF() const;
+
+        //- Return the number of additional state variables need by this joint
+        inline virtual label nw() const;
+
+        //- Return the index of this joint in the model
+        inline label index() const;
+
+        //- Return start index for the state variables for this joint
+        //  in the rigidBodyModel state fields
+        inline label qIndex() const;
+
+        //- Return start index for the additional state variables for this joint
+        //  in the rigidBodyModel w state field
+        inline label wIndex() const;
+
+        //- Return the joint motion sub-space
+        inline const List<spatialVector>& S() const;
+
+        //- Return a list of joints needed to emulate a floating body
+        static PtrList<joint> floating(const rigidBodyModel& model);
+
+        //- Update the rigidBodyModel state for the joint given
+        //  the joint state q, w and velocity qDot
+        virtual void jcalc
+        (
+            XSvc& J,
+            const scalarField& q,
+            const scalarField& w,
+            const scalarField& qDot
+        ) const = 0;
+
+        //- Write
+        virtual void write(Ostream&) const;
+
+
+    // Member Operators
+
+        //- Return the state quaternion for this joint
+        //  if it uses a quaternion representation for rotation
+        inline quaternion operator()
+        (
+            const scalarField& q,
+            const scalarField& w
+        ) const;
+
+        //- Set the state quaternion for this joint
+        //  if it uses a quaternion representation for rotation
+        inline void operator()
+        (
+            const quaternion& quat,
+            scalarField& q,
+            scalarField& w
+        ) const;
+};
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace RBD
+} // End namespace Foam
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#include "jointI.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#endif
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/joint/jointI.H

@@ -0,0 +1,110 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+// * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
+
+inline Foam::RBD::joint::joint(const rigidBodyModel&, const label nDoF)
+:
+    S_(nDoF),
+    index_(0),
+    qIndex_(0),
+    wIndex_(0)
+{}
+
+
+// * * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * //
+
+inline Foam::label Foam::RBD::joint::nDoF() const
+{
+    return S_.size();
+}
+
+inline Foam::label Foam::RBD::joint::nw() const
+{
+    return 0;
+}
+
+inline Foam::label Foam::RBD::joint::index() const
+{
+    return index_;
+}
+
+inline Foam::label Foam::RBD::joint::qIndex() const
+{
+    return qIndex_;
+}
+
+inline Foam::label Foam::RBD::joint::wIndex() const
+{
+    return wIndex_;
+}
+
+inline const Foam::List<Foam::spatialVector>& Foam::RBD::joint::S() const
+{
+    return S_;
+}
+
+
+// * * * * * * * * * * * * * * * Member Operators  * * * * * * * * * * * * * //
+
+inline Foam::quaternion Foam::RBD::joint::operator()
+(
+    const scalarField& q,
+    const scalarField& w
+) const
+{
+    if (nw() != 1)
+    {
+        FatalErrorInFunction
+            << "Attempt to get the quaternion for a non-spherical joint"
+            << abort(FatalError);
+    }
+
+    return quaternion(w[wIndex_], q.block<vector>(qIndex_));
+}
+
+
+inline void Foam::RBD::joint::operator()
+(
+    const quaternion& quat,
+    scalarField& q,
+    scalarField& w
+) const
+{
+    if (nw() != 1)
+    {
+        FatalErrorInFunction
+            << "Attempt to set quaternion for a non-spherical joint"
+            << abort(FatalError);
+    }
+
+    w[wIndex_] = quat.w();
+    q[qIndex_] = quat.v().x();
+    q[qIndex_+1] = quat.v().y();
+    q[qIndex_+2] = quat.v().z();
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/joints.H

@@ -0,0 +1,19 @@
+// Null joint for the root-body
+#include "nullJoint.H"
+
+// Revolute joints
+#include "Rx.H"
+#include "Ry.H"
+#include "Rz.H"
+#include "Ra.H"
+#include "Rs.H"
+#include "Rzyx.H"
+#include "Rxyz.H"
+#include "Ryxz.H"
+
+// Prismatic joints
+#include "Px.H"
+#include "Py.H"
+#include "Pz.H"
+#include "Pa.H"
+#include "Pxyz.H"

src/rigidBodyDynamics/joints/nullJoint/nullJoint.C

@@ -0,0 +1,97 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "nullJoint.H"
+#include "rigidBodyModel.H"
+#include "addToRunTimeSelectionTable.H"
+
+// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+    defineTypeNameAndDebug(null, 0);
+
+    addToRunTimeSelectionTable
+    (
+        joint,
+        null,
+        dictionary
+    );
+}
+}
+}
+
+
+// * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::null::null(const rigidBodyModel& model)
+:
+    joint(model, 0)
+{}
+
+
+Foam::RBD::joints::null::null
+(
+    const rigidBodyModel& model,
+    const dictionary& dict
+)
+:
+    joint(model, 0)
+{}
+
+
+Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::null::clone() const
+{
+    return autoPtr<joint>(new null(*this));
+}
+
+
+// * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
+
+Foam::RBD::joints::null::~null()
+{}
+
+
+// * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * * //
+
+void Foam::RBD::joints::null::jcalc
+(
+    joint::XSvc& J,
+    const scalarField& q,
+    const scalarField& w,
+    const scalarField& qDot
+) const
+{
+    FatalErrorInFunction
+        << "Cannot calculate the state of a null-joint"
+        << abort(FatalError);
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/joints/nullJoint/nullJoint.H

@@ -0,0 +1,111 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+Class
+    Foam::RBD::joints::null
+
+Description
+    Null joint for the root-body.
+
+    Reference:
+    \verbatim
+        Featherstone, R. (2008).
+        Rigid body dynamics algorithms.
+        Springer.
+        Chapter 4.
+    \endverbatim
+
+SourceFiles
+    nullJoint.C
+
+\*---------------------------------------------------------------------------*/
+
+#ifndef RBD_joints_null_H
+#define RBD_joints_null_H
+
+#include "joint.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace joints
+{
+
+/*---------------------------------------------------------------------------*\
+                         Class null Declaration
+\*---------------------------------------------------------------------------*/
+
+class null
+:
+    public joint
+{
+
+public:
+
+    //- Runtime type information
+    TypeName("null");
+
+
+    // Constructors
+
+        //- Construct for given model
+        null(const rigidBodyModel& model);
+
+        //- Construct for given model from dictionary
+        null(const rigidBodyModel&, const dictionary& dict);
+
+        //- Clone this joint
+        virtual autoPtr<joint> clone() const;
+
+
+    //- Destructor
+    virtual ~null();
+
+
+    // Member Functions
+
+        //- Update the model state for this joint
+        virtual void jcalc
+        (
+            joint::XSvc& J,
+            const scalarField& q,
+            const scalarField& w,
+            const scalarField& qDot
+        ) const;
+};
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace joints
+} // End namespace RBD
+} // End namespace Foam
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#endif
+
+// ************************************************************************* //

src/rigidBodyDynamics/rigidBodyInertia/rigidBodyInertia.H

@@ -0,0 +1,175 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+Class
+    Foam::rigidBodyInertia
+
+Description
+    This class represents the linear and angular inertia of a rigid body
+    by the mass, centre of mass and moment of inertia tensor about the
+    centre of mass.
+
+    Reference:
+    \verbatim
+        Featherstone, R. (2008).
+        Rigid body dynamics algorithms.
+        Springer.
+    \endverbatim
+
+SourceFiles
+    rigidBodyInertiaI.H
+    rigidBodyInertia.C
+    rigidBodyInertiaIO.C
+
+\*---------------------------------------------------------------------------*/
+
+#ifndef RBD_rigidBodyInertia_H
+#define RBD_rigidBodyInertia_H
+
+#include "vector.H"
+#include "symmTensor.H"
+#include "spatialVector.H"
+#include "spatialTensor.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+namespace Foam
+{
+
+// Forward declaration of classes
+class Istream;
+class Ostream;
+
+namespace RBD
+{
+
+// Forward declaration of friend functions and operators
+class rigidBodyInertia;
+Istream& operator>>(Istream&, rigidBodyInertia&);
+Ostream& operator<<(Ostream&, const rigidBodyInertia&);
+
+
+/*---------------------------------------------------------------------------*\
+                         Class rigidBodyInertia Declaration
+\*---------------------------------------------------------------------------*/
+
+class rigidBodyInertia
+{
+    // Private data
+
+        //- Mass of the rigid-body
+        scalar m_;
+
+        //- Centre of mass of the rigid-body
+        vector c_;
+
+        //- Inertia tensor about the centre of mass
+        symmTensor Ic_;
+
+
+public:
+
+    // Static member functions
+
+        //- Return the difference between the inertia tensor of the rigid-body
+        //  about the origin - about the centre of mass
+        //  for the given mass and centre of mass
+        inline static symmTensor Ioc(const scalar m, const vector& c);
+
+
+    // Constructors
+
+        //- Null constructor, initializes to zero
+        inline rigidBodyInertia();
+
+        //- Construct from mass, centre of mass and moment of inertia tensor
+        //  about the centre of mass
+        inline rigidBodyInertia
+        (
+            const scalar m,
+            const vector& c,
+            const symmTensor& Ic
+        );
+
+        //- Construct from the components of a spatial tensor
+        inline explicit rigidBodyInertia(const spatialTensor& st);
+
+        //- Construct from Istream
+        inline explicit rigidBodyInertia(Istream& is);
+
+
+    // Member Functions
+
+        //- Return the mass of the rigid-body
+        inline scalar m() const;
+
+        //- Return the centre of mass of the rigid-body
+        inline const vector& c() const;
+
+        //- Return the inertia tensor of the rigid-body about the centre of mass
+        inline const symmTensor& Ic() const;
+
+        //- Return the difference between the inertia tensor of the rigid-body
+        //  about the origin - about the centre of mass
+        inline symmTensor Ioc() const;
+
+        //- Return the difference between the inertia tensor of the rigid-body
+        //  about the a new centre of mass - about the current centre of mass
+        inline symmTensor Icc(const vector& c) const;
+
+        //- Return the inertia tensor of the rigid-body about the origin
+        inline symmTensor Io() const;
+
+        //- Return the kinetic energy of the body with the given velocity
+        inline scalar kineticEnergy(const spatialVector& v);
+
+
+    // Member Operators
+
+        //- Conversion to spatial tensor
+        inline operator spatialTensor() const;
+
+        inline void operator+=(const rigidBodyInertia&);
+
+
+    // IOstream Operators
+
+        friend Istream& operator>>(Istream&, rigidBodyInertia&);
+        friend Ostream& operator<<(Ostream&, const rigidBodyInertia&);
+};
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace RBD
+} // End namespace Foam
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#include "rigidBodyInertiaI.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#endif
+
+// ************************************************************************* //

src/rigidBodyDynamics/rigidBodyInertia/rigidBodyInertiaI.H

@@ -0,0 +1,243 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "spatialTransform.H"
+#include "transform.H"
+
+// * * * * * * * * * * * * * Static Member Functions * * * * * * * * * * * * //
+
+inline Foam::symmTensor Foam::RBD::rigidBodyInertia::Ioc
+(
+    const scalar m,
+    const vector& c
+)
+{
+    return m*(Foam::I*magSqr(c) - sqr(c));
+}
+
+
+// * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
+
+inline Foam::RBD::rigidBodyInertia::rigidBodyInertia()
+:
+    m_(0),
+    c_(Zero),
+    Ic_(Zero)
+{}
+
+
+inline Foam::RBD::rigidBodyInertia::rigidBodyInertia
+(
+    const scalar m,
+    const vector& c,
+    const symmTensor& Ic
+)
+:
+    m_(m),
+    c_(c),
+    Ic_(Ic)
+{}
+
+
+inline Foam::RBD::rigidBodyInertia::rigidBodyInertia(const spatialTensor& st)
+:
+    m_(st(3, 3)),
+    c_(vector(-st(1, 5), st(0, 5), -st(0, 4))/m_),
+    Ic_(symm(st.block<tensor, 0, 0>()()) - Ioc())
+{}
+
+
+inline Foam::RBD::rigidBodyInertia::rigidBodyInertia(Istream& is)
+:
+    m_(readScalar(is)),
+    c_(is),
+    Ic_(is)
+{}
+
+
+// * * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * //
+
+inline Foam::scalar Foam::RBD::rigidBodyInertia::m() const
+{
+    return m_;
+}
+
+inline const Foam::vector& Foam::RBD::rigidBodyInertia::c() const
+{
+    return c_;
+}
+
+inline const Foam::symmTensor& Foam::RBD::rigidBodyInertia::Ic() const
+{
+    return Ic_;
+}
+
+inline Foam::symmTensor Foam::RBD::rigidBodyInertia::Ioc() const
+{
+    return Ioc(m_, c_);
+}
+
+inline Foam::symmTensor Foam::RBD::rigidBodyInertia::Icc(const vector& c) const
+{
+    return Ioc(m_, c - c_);
+}
+
+inline Foam::symmTensor Foam::RBD::rigidBodyInertia::Io() const
+{
+    return Ic_ + Ioc();
+}
+
+
+// * * * * * * * * * * * * * * * Member Operators  * * * * * * * * * * * * * //
+
+inline Foam::RBD::rigidBodyInertia::operator spatialTensor() const
+{
+    tensor mcStar(m_*(*c_));
+
+    return spatialTensor
+    (
+        Io(),   mcStar,
+       -mcStar, m_*I
+    );
+}
+
+
+// * * * * * * * * * * * * * * IOstream Operators  * * * * * * * * * * * * * //
+
+inline Foam::Istream& Foam::RBD::operator>>
+(
+    Istream& is,
+    rigidBodyInertia& rbi
+)
+{
+    is  >> rbi.m_ >> rbi.c_ >> rbi.Ic_;
+    return is;
+}
+
+
+inline Foam::Ostream& Foam::RBD::operator<<
+(
+    Ostream& os,
+    const rigidBodyInertia& rbi
+)
+{
+    os  << rbi.m_ << nl << rbi.c_ << nl << rbi.Ic_ << endl;
+    return os;
+}
+
+
+// * * * * * * * * * * * * * * * Global Operators  * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+//- Return the rigid-body inertia of the combined body
+inline rigidBodyInertia operator+
+(
+    const rigidBodyInertia& rbi1,
+    const rigidBodyInertia& rbi2
+)
+{
+    const scalar m12 = rbi1.m() + rbi2.m();
+    const vector c12 = (rbi1.m()*rbi1.c() + rbi2.m()*rbi2.c())/m12;
+
+    return rigidBodyInertia
+    (
+        m12,
+        c12,
+        rbi1.Ic() + rbi1.Icc(c12) + rbi2.Ic() + rbi2.Icc(c12)
+    );
+}
+
+
+//- Inner-product with a spatialVector (e.g. velocity returning the momentum)
+inline spatialVector operator&
+(
+    const rigidBodyInertia& rbi,
+    const spatialVector& sv
+)
+{
+    const vector av(sv.w());
+    const vector lv(sv.l());
+
+    return spatialVector
+    (
+        (rbi.Io() & av) + rbi.m()*(rbi.c() ^ lv),
+        rbi.m()*lv - rbi.m()*(rbi.c() ^ av)
+    );
+}
+
+
+//- Return (^BX_A)^* I ^AX_B
+inline rigidBodyInertia transform
+(
+    const spatialTransform& X,
+    const rigidBodyInertia& I
+)
+{
+    const vector Xc((X.E().T() & I.c()) + X.r());
+
+    return rigidBodyInertia
+    (
+        I.m(),
+        Xc,
+        transform(X.E().T(), I.Ic())
+    );
+}
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace RBD
+} // End namespace Foam
+
+
+// * * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * //
+
+inline Foam::scalar Foam::RBD::rigidBodyInertia::kineticEnergy
+(
+    const spatialVector& v
+)
+{
+    return 0.5*(v && (*this & v));
+}
+
+
+// * * * * * * * * * * * * * * * Member Operators  * * * * * * * * * * * * * //
+
+inline void Foam::RBD::rigidBodyInertia::operator+=
+(
+    const rigidBodyInertia& rbi
+)
+{
+    *this = *this + rbi;
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/rigidBodyModel/forwardDynamics.C

@@ -0,0 +1,239 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "rigidBodyModel.H"
+
+// * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * * //
+
+void Foam::RBD::rigidBodyModel::forwardDynamics
+(
+    const scalarField& q,
+    const scalarField& w,
+    const scalarField& qDot,
+    const scalarField& tau,
+    const Field<spatialVector>& fx,
+    scalarField& qDdot
+) const
+{
+    DebugInFunction
+        << "q = " << q << nl
+        << "qDot = " << qDot << nl
+        << "tau = " << tau << endl;
+
+    // Joint state returned by jcalc
+    joint::XSvc J;
+
+    v_[0] = Zero;
+
+    for (label i=1; i<nBodies(); i++)
+    {
+        const joint& jnt = joints()[i];
+        jnt.jcalc(J, q, w, qDot);
+
+        S_[i] = J.S;
+        S1_[i] = J.S1;
+
+        Xlambda_[i] = J.X & XT_[i];
+
+        const label lambdai = lambda_[i];
+
+        if (lambdai != 0)
+        {
+            X0_[i] = Xlambda_[i] & X0_[lambdai];
+        }
+        else
+        {
+            X0_[i] = Xlambda_[i];
+        }
+
+        v_[i] = (Xlambda_[i] & v_[lambdai]) + J.v;
+        c_[i] = J.c + (v_[i] ^ J.v);
+        IA_[i] = I(i);
+        pA_[i] = v_[i] ^* (I(i) & v_[i]);
+
+        if (fx.size())
+        {
+            pA_[i] -= *X0_[i] & fx[i];
+        }
+    }
+
+    for (label i=nBodies()-1; i>0; i--)
+    {
+        const joint& jnt = joints()[i];
+        const label qi = jnt.qIndex();
+
+        if (jnt.nDoF() == 1)
+        {
+            U1_[i] = IA_[i] & S1_[i];
+            Dinv_[i].xx() = 1/(S1_[i] && U1_[i]);
+            u_[i].x() = tau[qi] - (S1_[i] && pA_[i]);
+
+            const label lambdai = lambda_[i];
+
+            if (lambdai != 0)
+            {
+                const spatialTensor Ia
+                (
+                    IA_[i] - (U1_[i]*(Dinv_[i].xx()*U1_[i]))
+                );
+
+                const spatialVector pa
+                (
+                    pA_[i] + (Ia & c_[i]) + U1_[i]*(Dinv_[i].xx()*u_[i].x())
+                );
+
+                IA_[lambdai] +=
+                    spatialTensor(Xlambda_[i].T())
+                  & Ia
+                  & spatialTensor(Xlambda_[i]);
+
+                pA_[lambdai] += Xlambda_[i].T() & pa;
+            }
+        }
+        else
+        {
+            U_[i] = IA_[i] & S_[i];
+            Dinv_[i] = (S_[i].T() & U_[i]).inv();
+
+            u_[i] = tau.block<vector>(qi) - (S_[i].T() & pA_[i]);
+
+            const label lambdai = lambda_[i];
+
+            if (lambdai != 0)
+            {
+                spatialTensor Ia
+                (
+                    IA_[i]
+                  - (U_[i] & Dinv_[i] & U_[i].T())
+                );
+
+                spatialVector pa
+                (
+                    pA_[i]
+                  + (Ia & c_[i])
+                  + (U_[i] & Dinv_[i] & u_[i])
+                );
+
+                IA_[lambdai] +=
+                    spatialTensor(Xlambda_[i].T())
+                  & Ia
+                  & spatialTensor(Xlambda_[i]);
+
+                pA_[lambdai] += Xlambda_[i].T() & pa;
+            }
+        }
+    }
+
+    a_[0] = spatialVector(Zero, -g_);
+
+    for (label i=1; i<nBodies(); i++)
+    {
+        const joint& jnt = joints()[i];
+        const label qi = jnt.qIndex();
+
+        a_[i] = (Xlambda_[i] & a_[lambda_[i]]) + c_[i];
+
+        if (jnt.nDoF() == 1)
+        {
+            qDdot[qi] = Dinv_[i].xx()*(u_[i].x() - (U1_[i] && a_[i]));
+            a_[i] += S1_[i]*qDdot[qi];
+        }
+        else
+        {
+            vector qDdoti(Dinv_[i] & (u_[i] - (U_[i].T() & a_[i])));
+
+            // Need to add mutable "block<vector>" to Field
+            qDdot[qi] = qDdoti.x();
+            qDdot[qi+1] = qDdoti.y();
+            qDdot[qi+2] = qDdoti.z();
+
+            a_[i] += (S_[i] & qDdoti);
+        }
+    }
+
+    DebugInfo
+        << "qDdot = " << qDdot << nl
+        << "a = " << a_ << endl;
+}
+
+
+void Foam::RBD::rigidBodyModel::forwardDynamicsCorrection
+(
+    const scalarField& q,
+    const scalarField& w,
+    const scalarField& qDot,
+    const scalarField& qDdot
+) const
+{
+    DebugInFunction << endl;
+
+    // Joint state returned by jcalc
+    joint::XSvc J;
+
+    v_[0] = Zero;
+    a_[0] = spatialVector(Zero, -g_);
+
+    for (label i=1; i<nBodies(); i++)
+    {
+        const joint& jnt = joints()[i];
+        const label qi = jnt.qIndex();
+
+        jnt.jcalc(J, q, w, qDot);
+
+        S_[i] = J.S;
+        S1_[i] = J.S1;
+
+        Xlambda_[i] = J.X & XT_[i];
+
+        const label lambdai = lambda_[i];
+
+        if (lambdai != 0)
+        {
+            X0_[i] = Xlambda_[i] & X0_[lambdai];
+        }
+        else
+        {
+            X0_[i] = Xlambda_[i];
+        }
+
+        v_[i] = (Xlambda_[i] & v_[lambdai]) + J.v;
+        c_[i] = J.c + (v_[i] ^ J.v);
+        a_[i] = (Xlambda_[i] & a_[lambdai]) + c_[i];
+
+        if (jnt.nDoF() == 1)
+        {
+            a_[i] += S1_[i]*qDdot[qi];
+        }
+        else
+        {
+            a_[i] += S_[i] & qDdot.block<vector>(qi);
+        }
+    }
+
+    DebugInfo<< "a = " << a_ << endl;
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/rigidBodyModel/rigidBodyModel.C

@@ -0,0 +1,224 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "rigidBodyModel.H"
+#include "masslessBody.H"
+#include "nullJoint.H"
+
+// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+    defineTypeNameAndDebug(rigidBodyModel, 0);
+}
+}
+
+
+// * * * * * * * * * * * * * Private Member Functions  * * * * * * * * * * * //
+
+void Foam::RBD::rigidBodyModel::initializeRootBody()
+{
+    bodies_.append(new masslessBody);
+    lambda_.append(0);
+    joints_.append(new joints::null(*this));
+    XT_.append(spatialTransform());
+
+    nDoF_ = 0;
+    nw_ = 0;
+
+    resizeState();
+}
+
+
+void Foam::RBD::rigidBodyModel::resizeState()
+{
+    Xlambda_.append(spatialTransform());
+    X0_.append(spatialTransform());
+
+    v_.append(Zero);
+    a_.append(Zero);
+    c_.append(Zero);
+
+    IA_.append(spatialTensor::I);
+    pA_.append(Zero);
+
+    S_.append(Zero);
+    S1_.append(Zero);
+    U_.append(Zero);
+    U1_.append(Zero);
+    Dinv_.append(Zero);
+    u_.append(Zero);
+}
+
+
+// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * * //
+
+Foam::RBD::rigidBodyModel::rigidBodyModel()
+:
+    g_(Zero)
+{
+    initializeRootBody();
+}
+
+
+// * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * * //
+
+Foam::label Foam::RBD::rigidBodyModel::join
+(
+    const label parentID,
+    const spatialTransform& XT,
+    const autoPtr<joint>& jointPtr,
+    const autoPtr<rigidBody>& bodyPtr
+)
+{
+    // Append the body
+    bodies_.append(bodyPtr);
+
+    // If the parentID refers to a merged body find the parent into which it has
+    // been merged and set lambda and XT accordingly
+    if (merged(parentID))
+    {
+        const subBody& sBody = mergedBody(parentID);
+        lambda_.append(sBody.parentID());
+        XT_.append(XT & sBody.parentXT());
+    }
+    else
+    {
+        lambda_.append(parentID);
+        XT_.append(XT);
+    }
+
+    // Append the joint
+    const joint& prevJoint = joints_[joints_.size() - 1];
+    joints_.append(jointPtr);
+    joint& curJoint = joints_[joints_.size() - 1];
+    curJoint.index() = joints_.size() - 1;
+    curJoint.qIndex() = prevJoint.qIndex() + prevJoint.nDoF();
+    curJoint.wIndex() = prevJoint.wIndex() + prevJoint.nw();
+
+    // Increment the degrees of freedom
+    nDoF_ += curJoint.nDoF();
+    nw_ += curJoint.nw();
+
+    resizeState();
+
+    return nBodies()-1;
+}
+
+
+Foam::label Foam::RBD::rigidBodyModel::join
+(
+    const label parentID,
+    const spatialTransform& XT,
+    const PtrList<joint>& compositeJoint,
+    const autoPtr<rigidBody>& bodyPtr
+)
+{
+    label parent = parentID;
+
+    // For all but the final joint in the set add a masslessBody with the
+    // joint and transform
+    for (label j=0; j<compositeJoint.size()-1; j++)
+    {
+        parent = join
+        (
+            parent,
+            j == 0 ? XT : spatialTransform(),
+            compositeJoint[j].clone(),
+            autoPtr<rigidBody>(new masslessBody)
+        );
+    }
+
+    // For the final joint in the set add the read body
+    return join
+    (
+        parent,
+        compositeJoint.size() == 1 ? XT : spatialTransform(),
+        compositeJoint[compositeJoint.size()-1].clone(),
+        bodyPtr
+    );
+}
+
+
+Foam::label Foam::RBD::rigidBodyModel::merge
+(
+    const label parentID,
+    const spatialTransform& XT,
+    const autoPtr<rigidBody>& bodyPtr
+)
+{
+    autoPtr<subBody> sBodyPtr;
+
+    // If the parentID refers to a merged body find the parent into which it has
+    // been merged and merge this on into the same parent with the appropriate
+    // transform
+    if (merged(parentID))
+    {
+        const subBody& sBody = mergedBody(parentID);
+        sBodyPtr.set
+        (
+            new subBody
+            (
+                bodyPtr,
+                sBody.parentID(),
+                XT & sBody.parentXT()
+            )
+        );
+    }
+    else
+    {
+        sBodyPtr.set(new subBody(bodyPtr, parentID, XT));
+    }
+
+    const subBody& sBody = sBodyPtr();
+    mergedBodies_.append(sBodyPtr);
+
+    // Merge the sub-body with the parent
+    bodies_[sBody.parentID()].merge(sBody);
+
+    return mergedBodyID(mergedBodies_.size() - 1);
+}
+
+
+Foam::spatialTransform Foam::RBD::rigidBodyModel::X0
+(
+    const label bodyId
+) const
+{
+    if (merged(bodyId))
+    {
+        const subBody& mBody = mergedBody(bodyId);
+        return mBody.parentXT() & X0_[mBody.parentID()];
+    }
+    else
+    {
+        return X0_[bodyId];
+    }
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/rigidBodyModel/rigidBodyModel.H

@@ -0,0 +1,313 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+Class
+    Foam::RBD::rigidBodyModel
+
+Description
+    Basic rigid-body model representing a system of rigid-bodies connected by
+    1-6 DoF joints.
+
+    This class holds various body and joint state fields needed by the
+    kinematics and forward-dynamics algorithms presented in
+
+    reference:
+    \verbatim
+        Featherstone, R. (2008).
+        Rigid body dynamics algorithms.
+        Springer.
+        Chapter 4.
+    \endverbatim
+
+SourceFiles
+    rigidBodyModel.C
+    kinematics.C
+    forwardDynamics.C
+
+\*---------------------------------------------------------------------------*/
+
+#ifndef RBD_rigidBodyModel_H
+#define RBD_rigidBodyModel_H
+
+#include "rigidBody.H"
+#include "subBody.H"
+#include "joint.H"
+#include "PtrList.H"
+
+namespace Foam
+{
+namespace RBD
+{
+
+/*---------------------------------------------------------------------------*\
+                         Class rigidBodyModel Declaration
+\*---------------------------------------------------------------------------*/
+
+class rigidBodyModel
+{
+    //- Initialize the model with the root-body which is a fixed massless body
+    //  at the origin.
+    void initializeRootBody();
+
+    //- Resize the state fields following the joining of a body
+    void resizeState();
+
+
+protected:
+
+    // Protected data representing the model structure
+
+        //- List of the bodies.
+        //  The 0'th body represents the fixed origin and is constructed
+        //  automatically.  The subsequent (moving) bodies are appended by the
+        //  join member function.
+        PtrList<rigidBody> bodies_;
+
+        //- Bodies may be merged into existing bodies, the inertia of which is
+        //  updated to represent the combined body which is more efficient than
+        //  attaching them with fixed joints.  These 'merged' bodies are held on
+        //  this list.
+        PtrList<subBody> mergedBodies_;
+
+        //- List of indices of the parent of each body
+        DynamicList<label> lambda_;
+
+        //- Each body it attached with a joint which are held on this list.
+        PtrList<joint> joints_;
+
+        //- Transform from the parent body frame to the joint frame.
+        DynamicList<spatialTransform> XT_;
+
+        //- The number of degrees of freedom of the model
+        //  used to set the size of the of joint state fields q, qDot and qDdot.
+        label nDoF_;
+
+        //- The number of additional state variable needed to describe the joint
+        //  states.  Typically this is the number of quaternion joints for which
+        //  the 'w' element is additional to the 3-degrees of freedom of the
+        //  joint.
+        label nw_;
+
+
+    // Other protected member data
+
+        //- Acceleration due to gravity
+        vector g_;
+
+
+    // Mutable transforms maintained by kinematics and forward-dynamics
+
+        //- Transform from the parent body to the current body
+        mutable DynamicList<spatialTransform> Xlambda_;
+
+        //- Transform for external forces to the bodies reference frame
+        mutable DynamicList<spatialTransform> X0_;
+
+
+    // Mutable kinematic body state fields
+
+        //- The spatial velocity of the bodies
+        mutable DynamicList<spatialVector> v_;
+
+        //- The spatial acceleration of the bodies
+        mutable DynamicList<spatialVector> a_;
+
+        //- The velocity dependent spatial acceleration of the joints
+        mutable DynamicList<spatialVector> c_;
+
+
+    // Mutable state fields needed by the forward-dynamics algorithm
+
+        //- Velocity-product acceleration
+
+        //- Articulated body inertia
+        mutable DynamicList<spatialTensor> IA_;
+
+        //- Articulated body bias force
+        mutable DynamicList<spatialVector> pA_;
+
+
+    // Mutable joint state fields
+
+        //- Motion subspace for joints with 3 degrees of freedom
+        mutable DynamicList<compactSpatialTensor> S_;
+
+        //- Motion subspace for joints with 1 degrees of freedom
+        mutable DynamicList<spatialVector> S1_;
+
+        //- Sub-expression IA.S in the forward-dynamics algorithm
+        mutable DynamicList<compactSpatialTensor> U_;
+
+        //- Sub-expression IA.S1 in the forward-dynamics algorithm
+        mutable DynamicList<spatialVector> U1_;
+
+        //- Sub-expression (S^T.U)^-1 in the forward-dynamics algorithm
+        mutable DynamicList<tensor> Dinv_;
+
+        //- Sub-expression tau - S^T.pA in the forward-dynamics algorithm
+        mutable DynamicList<vector> u_;
+
+
+public:
+
+    //- Runtime type information
+    TypeName("rigidBodyModel");
+
+
+    // Constructors
+
+        //- Null-constructor which adds the single root-body at the origin
+        rigidBodyModel();
+
+
+    //- Destructor
+    virtual ~rigidBodyModel()
+    {}
+
+
+    // Member Functions
+
+        //- Return the number of bodies in the model (bodies().size())
+        inline label nBodies() const;
+
+        //- Return the list of the bodies in the model
+        inline PtrList<rigidBody> bodies() const;
+
+        //- List of indices of the parent of each body
+        inline const DynamicList<label>& lambda() const;
+
+        //- Return the list of joints in the model
+        inline const PtrList<joint>& joints() const;
+
+        //- Return the number of degrees of freedom of the model
+        //  used to set the size of the of joint state fields q, qDot and qDdot.
+        inline label nDoF() const;
+
+        //- Return the number of additional state variable needed to describe
+        //  the joint states.  Typically this is the number of quaternion
+        //  joints for which the 'w' element is additional to the 3-degrees of
+        //  freedom of the joint.
+        inline label nw() const;
+
+        //- Return the acceleration due to gravity
+        inline const vector& g() const;
+
+        //- Allow the acceleration due to gravity to be set
+        //  after model construction
+        inline vector& g();
+
+        //- Return the inertia of body i
+        inline const rigidBodyInertia& I(const label i) const;
+
+        //- Join the given body to the parent with ID parentID via the given
+        //  joint with transform from the parent frame to the joint frame XT.
+        virtual label join
+        (
+            const label parentID,
+            const spatialTransform& XT,
+            const autoPtr<joint>& jointPtr,
+            const autoPtr<rigidBody>& bodyPtr
+        );
+
+        //- Join the given body to the parent with ID parentID via the given
+        //  composite joint (specified as a list of co-located joints) with
+        //  transform from the parent frame to the joint frame XT.
+        //  Composite joins are useful to represent complex joints with degrees
+        //  of freedom other than 1 or 3 which are directly supported.
+        label join
+        (
+            const label parentID,
+            const spatialTransform& XT,
+            const PtrList<joint>& compositeJoint,
+            const autoPtr<rigidBody>& bodyPtr
+        );
+
+        //- Merge the given body with transform X into the parent with ID
+        //  parentID.  The parent body assumes the properties of the combined
+        //  body (inertia etc.) and the merged body is held on a
+        //  separate list for reference.
+        label merge
+        (
+            const label parentID,
+            const spatialTransform& X,
+            const autoPtr<rigidBody>& bodyPtr
+        );
+
+        //- Return true if the body with given ID has been merged with a parent
+        inline bool merged(label bodyID) const;
+
+        //- Return the index of the merged body in the mergedBody list
+        //  from the given body ID
+        inline label mergedBodyIndex(const label mergedBodyID) const;
+
+        //- Return the merged body ID for the given merged body index
+        //  in the mergedBody list
+        inline label mergedBodyID(const label mergedBodyIndex) const;
+
+        //- Return the merged body for the given body ID
+        inline const subBody& mergedBody(label mergedBodyID) const;
+
+        //- Return the current transform to the global frame for the given body
+        spatialTransform X0(const label bodyId) const;
+
+        //- Calculate the joint acceleration qDdot from the joint state q,
+        //  velocity qDot, internal force tau (in the joint frame) and
+        //  external force fx (in the global frame) using the articulated body
+        //  algorithm (Section 7.3 and Table 7.1)
+        void forwardDynamics
+        (
+            const scalarField& q,
+            const scalarField& w,
+            const scalarField& qDot,
+            const scalarField& tau,
+            const Field<spatialVector>& fx,
+            scalarField& qDdot
+        ) const;
+
+        //- Correct the velocity and acceleration of the bodies in the model
+        //  from the given joint state fields following an integration step
+        //  of the forwardDynamics
+        void forwardDynamicsCorrection
+        (
+            const scalarField& q,
+            const scalarField& w,
+            const scalarField& qDot,
+            const scalarField& qDdot
+        ) const;
+};
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace RBD
+} // End namespace Foam
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#include "rigidBodyModelI.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#endif
+
+// ************************************************************************* //

src/rigidBodyDynamics/rigidBodyModel/rigidBodyModelI.H

@@ -0,0 +1,120 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+// * * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * //
+
+inline Foam::label Foam::RBD::rigidBodyModel::nBodies() const
+{
+    return bodies_.size();
+}
+
+
+inline Foam::PtrList<Foam::RBD::rigidBody>
+Foam::RBD::rigidBodyModel::bodies() const
+{
+    return bodies_;
+}
+
+
+inline const Foam::PtrList<Foam::RBD::joint>&
+Foam::RBD::rigidBodyModel::joints() const
+{
+    return joints_;
+}
+
+
+inline Foam::label Foam::RBD::rigidBodyModel::nDoF() const
+{
+    return nDoF_;
+}
+
+
+inline Foam::label Foam::RBD::rigidBodyModel::nw() const
+{
+    return nw_;
+}
+
+
+inline const Foam::vector& Foam::RBD::rigidBodyModel::g() const
+{
+    return g_;
+}
+
+
+inline Foam::vector& Foam::RBD::rigidBodyModel::g()
+{
+    return g_;
+}
+
+
+inline const Foam::DynamicList<Foam::label>&
+Foam::RBD::rigidBodyModel::lambda() const
+{
+    return lambda_;
+}
+
+
+inline const Foam::RBD::rigidBodyInertia&
+Foam::RBD::rigidBodyModel::I(const label i) const
+{
+    return bodies_[i];
+}
+
+
+inline bool Foam::RBD::rigidBodyModel::merged(label bodyID) const
+{
+    return bodyID < 0;
+}
+
+
+inline Foam::label
+Foam::RBD::rigidBodyModel::mergedBodyID(const label mergedBodyIndex) const
+{
+    return -1 - mergedBodyIndex;
+}
+
+
+inline Foam::label
+Foam::RBD::rigidBodyModel::mergedBodyIndex(const label mergedBodyID) const
+{
+    return -1 - mergedBodyID;
+}
+
+
+inline const Foam::RBD::subBody&
+Foam::RBD::rigidBodyModel::mergedBody(label mergedBodyID) const
+{
+    if (!merged(mergedBodyID))
+    {
+        FatalErrorInFunction
+            << "Body " << mergedBodyID << " has not been merged"
+            << abort(FatalError);
+    }
+
+    return mergedBodies_[mergedBodyIndex(mergedBodyID)];
+}
+
+
+// ************************************************************************* //