Code Yarns ‍👨‍💻
Tech BlogPersonal Blog

How to create shoulder joint in PhysX

📅 2013-Nov-22 ⬩ ✍️ Ashwin Nanjappa ⬩ 🏷️ joint, physx ⬩ 📚 Archive

The shoulder joint of the human body can be simulated in PhysX by using its spherical joint. The spherical joint is used to represent a ball-and-socket joint.

In the case of the shoulder joint, we assume that the ball-and-socket is placed at the shoulder location and it is connected by the upper arm to elbow. So, we use two PhysX bodies: one located at the shoulder itself and another at the elbow. Alternatively you could put the first body at the neck and connect it by the shoulder bone to the ball-and-socket position.

The code below creates the shoulder joint:

#include <PxPhysicsAPI.h>

struct Point3
{
    // Fill in details please
};

/**
 * Get the rotation quaternion that can be used to rotate the orientation of
 * fromPos to points its X axis to "look at" toPos
 *
 * Assumption 1: fromPos is in upright orientation to begin with. For upright
 *               orientation see the book by Dunn and Parberry
 * Assumption 2: Right handed coordinate system (PhysX is)
 * 
 */
physx::PxQuat getLookAtQuat(const Point3& fromPos, const Point3& toPos)
{
    // Position of toPos relative to fromPos
    const Point3 relToPos = Point3::sub(toPos, fromPos);

    /**
     * First we rotate fromPos around Y axis to look at toPos
     * This gives us Euler angle around Y axis
     */

    // Compute the angle
    // theta = atan(z/x)
    const float yAng0 = atan(fabs(relToPos.z()) / fabs(relToPos.x()));

    // Fix the angle based on XZ quadrant point lies in
    float yAng;
    if (relToPos.x() >= 0)
    {
        if (relToPos.z() >= 0)
            yAng = 2 * Pi - yAng0; // 360 - theta
        else
            yAng = yAng0;
    }
    else
    {
        if (relToPos.z() >= 0)
            yAng = Pi + yAng0; // 180 + theta
        else
            yAng = Pi - yAng0; // 180 - theta
    }

    /**
     * Next fromPos will look "up" to see toPos
     * This gives us Euler angle around Z axis
     */

    // Compute the angle
    // theta = atan( y / sqrt(x^2 + z^2))
    const float zAng0 = atan(fabs(relToPos.y()) /
        sqrt(relToPos.x() * relToPos.x() + relToPos.z() * relToPos.z()));

    // Fix angle based on whether toPos is above or below XZ plane
    const float zAng = (relToPos.y() >= 0) ? zAng0 : -zAng0;

    /**
     * Convert Euler angles to quaternion that rotates
     * X axis of upright orientation to point at toPos
     * Reference: PhysX Math Primer
     */

    // Convert to quaternions
    physx::PxQuat qy(yAng, physx::PxVec3(0, 1, 0));
    physx::PxQuat qz(zAng, physx::PxVec3(0, 0, 1));

    // Rotate local axes
    physx::PxQuat q = qy * qz;

    return q;
}

/**
 * Create shoulder joint using the spherical joint of PhysX
 * 
 * Idea is that the ball-socket will lie at the shoulder itself
 * So, the first body and the location of ball-socket both
 * lie at shoulder position.
 * This also means that the cone of rotation lies at shoulder too.
 * Elbow is second body and this allows upper arm to rotate.
 * 
 * We call should position as fromPos and elbow position as toPos
 */
void createShoulderJoint()
{
    // Orientation for ball-and-socket at shoulder
    const physx::PxQuat fromToQuat = getLookAtQuat(fromPos, toPos);
    
    // Transform for shoulder
    const physx::PxTransform fromTrans(physx::PxVec3(0, 0, 0), fromToQuat);
    
    // Transform for elbow
    const physx::PxTransform toTrans(Point3ToPx(fromToPos), fromToQuat);

    Joint* sjoint = physx::PxSphericalJointCreate(
        phyWorld, // PhysX world you would've created
        fromBody,
        fromTrans,
        toBody,
        toTrans);

    // Cone of rotation
    sjoint->setLimitCone(
        physx::PxJointLimitCone(.2f, .2f, .1f));

    // Limit rotation to this cone
    sjoint->setSphericalJointFlag(
        physx::PxSphericalJointFlag::eLIMIT_ENABLED,
        true);
}

To create the joint we need to provide the position of the two bodies involved, the relative location of the joint to those bodies. In addition, we also need to provide the local orientation of the joint to those bodies.

The joint begins life with an upright orientation, that is, its axes are same as the global axes. We need to point the X-axis of the orientation to look at the second body. The above code does that by computing the Euler angles of rotation around the Y-axis and Z-axis of the joint that point the X-axis to look at the second body. The X-axis of the joint will also be the axis around which the cone of rotation is created to bound the rotational freedom of the joint.

Once we have the Euler angles, we convert that to a quaternion. We also compute the position of the joint relative to the first and second bodies. Each pair of position and orientation rotation forms a transform. Two pairs of these are used to define the joint.

Tried with: PhysX 3.2.4, Visual Studio 2010 and Windows 7 x64