add ode

1 files changed, 803 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/ode/src/joints/universal.cpp b/libs/ode-0.16.1/ode/src/joints/universal.cpp
new file mode 100644
index 0000000..1ef00a7
--- /dev/null
+++ b/libs/ode-0.16.1/ode/src/joints/universal.cpp
@@ -0,0 +1,803 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
+ *                                                                       *
+ * This library is free software; you can redistribute it and/or         *
+ * modify it under the terms of EITHER:                                  *
+ *   (1) The GNU Lesser General Public License as published by the Free  *
+ *       Software Foundation; either version 2.1 of the License, or (at  *
+ *       your option) any later version. The text of the GNU Lesser      *
+ *       General Public License is included with this library in the     *
+ *       file LICENSE.TXT.                                               *
+ *   (2) The BSD-style license that is included with this library in     *
+ *       the file LICENSE-BSD.TXT.                                       *
+ *                                                                       *
+ * This library 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 files    *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
+ *                                                                       *
+ *************************************************************************/
+
+
+#include <ode/odeconfig.h>
+#include "config.h"
+#include "universal.h"
+#include "joint_internal.h"
+
+
+
+//****************************************************************************
+// universal
+
+// I just realized that the universal joint is equivalent to a hinge 2 joint with
+// perfectly stiff suspension.  By comparing the hinge 2 implementation to
+// the universal implementation, you may be able to improve this
+// implementation (or, less likely, the hinge2 implementation).
+
+dxJointUniversal::dxJointUniversal( dxWorld *w ) :
+    dxJoint( w )
+{
+    dSetZero( anchor1, 4 );
+    dSetZero( anchor2, 4 );
+    dSetZero( axis1, 4 );
+    axis1[0] = 1;
+    dSetZero( axis2, 4 );
+    axis2[1] = 1;
+    dSetZero( qrel1, 4 );
+    dSetZero( qrel2, 4 );
+    limot1.init( world );
+    limot2.init( world );
+}
+
+
+void
+dxJointUniversal::getAxes( dVector3 ax1, dVector3 ax2 )
+{
+    // This says "ax1 = joint->node[0].body->posr.R * joint->axis1"
+    dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
+
+    if ( node[1].body )
+    {
+        dMultiply0_331( ax2, node[1].body->posr.R, axis2 );
+    }
+    else
+    {
+        ax2[0] = axis2[0];
+        ax2[1] = axis2[1];
+        ax2[2] = axis2[2];
+    }
+}
+
+void
+dxJointUniversal::getAngles( dReal *angle1, dReal *angle2 )
+{
+    if ( node[0].body )
+    {
+        // length 1 joint axis in global coordinates, from each body
+        dVector3 ax1, ax2;
+        dMatrix3 R;
+        dQuaternion qcross, qq, qrel;
+
+        getAxes( ax1, ax2 );
+
+        // It should be possible to get both angles without explicitly
+        // constructing the rotation matrix of the cross.  Basically,
+        // orientation of the cross about axis1 comes from body 2,
+        // about axis 2 comes from body 1, and the perpendicular
+        // axis can come from the two bodies somehow.  (We don't really
+        // want to assume it's 90 degrees, because in general the
+        // constraints won't be perfectly satisfied, or even very well
+        // satisfied.)
+        //
+        // However, we'd need a version of getHingeAngleFromRElativeQuat()
+        // that CAN handle when its relative quat is rotated along a direction
+        // other than the given axis.  What I have here works,
+        // although it's probably much slower than need be.
+
+        dRFrom2Axes( R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2] );
+
+        dRtoQ( R, qcross );
+
+
+        // This code is essentialy the same as getHingeAngle(), see the comments
+        // there for details.
+
+        // get qrel = relative rotation between node[0] and the cross
+        dQMultiply1( qq, node[0].body->q, qcross );
+        dQMultiply2( qrel, qq, qrel1 );
+
+        *angle1 = getHingeAngleFromRelativeQuat( qrel, axis1 );
+
+        // This is equivalent to
+        // dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]);
+        // You see that the R is constructed from the same 2 axis as for angle1
+        // but the first and second axis are swapped.
+        // So we can take the first R and rapply a rotation to it.
+        // The rotation is around the axis between the 2 axes (ax1 and ax2).
+        // We do a rotation of 180deg.
+
+        dQuaternion qcross2;
+        // Find the vector between ax1 and ax2 (i.e. in the middle)
+        // We need to turn around this vector by 180deg
+
+        // The 2 axes should be normalize so to find the vector between the 2.
+        // Add and devide by 2 then normalize or simply normalize
+        //    ax2
+        //    ^
+        //    |
+        //    |
+        ///   *------------> ax1
+        //    We want the vector a 45deg
+        //
+        // N.B. We don't need to normalize the ax1 and ax2 since there are
+        //      normalized when we set them.
+
+        // We set the quaternion q = [cos(theta), dir*sin(theta)] = [w, x, y, Z]
+        qrel[0] = 0;                // equivalent to cos(Pi/2)
+        qrel[1] = ax1[0] + ax2[0];  // equivalent to x*sin(Pi/2); since sin(Pi/2) = 1
+        qrel[2] = ax1[1] + ax2[1];
+        qrel[3] = ax1[2] + ax2[2];
+
+        dReal l = dRecip( sqrt( qrel[1] * qrel[1] + qrel[2] * qrel[2] + qrel[3] * qrel[3] ) );
+        qrel[1] *= l;
+        qrel[2] *= l;
+        qrel[3] *= l;
+
+        dQMultiply0( qcross2, qrel, qcross );
+
+        if ( node[1].body )
+        {
+            dQMultiply1( qq, node[1].body->q, qcross2 );
+            dQMultiply2( qrel, qq, qrel2 );
+        }
+        else
+        {
+            // pretend joint->node[1].body->q is the identity
+            dQMultiply2( qrel, qcross2, qrel2 );
+        }
+
+        *angle2 = - getHingeAngleFromRelativeQuat( qrel, axis2 );
+    }
+    else
+    {
+        *angle1 = 0;
+        *angle2 = 0;
+    }
+}
+
+dReal
+dxJointUniversal::getAngle1()
+{
+    if ( node[0].body )
+    {
+        // length 1 joint axis in global coordinates, from each body
+        dVector3 ax1, ax2;
+        dMatrix3 R;
+        dQuaternion qcross, qq, qrel;
+
+        getAxes( ax1, ax2 );
+
+        // It should be possible to get both angles without explicitly
+        // constructing the rotation matrix of the cross.  Basically,
+        // orientation of the cross about axis1 comes from body 2,
+        // about axis 2 comes from body 1, and the perpendicular
+        // axis can come from the two bodies somehow.  (We don't really
+        // want to assume it's 90 degrees, because in general the
+        // constraints won't be perfectly satisfied, or even very well
+        // satisfied.)
+        //
+        // However, we'd need a version of getHingeAngleFromRElativeQuat()
+        // that CAN handle when its relative quat is rotated along a direction
+        // other than the given axis.  What I have here works,
+        // although it's probably much slower than need be.
+
+        dRFrom2Axes( R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2] );
+        dRtoQ( R, qcross );
+
+        // This code is essential the same as getHingeAngle(), see the comments
+        // there for details.
+
+        // get qrel = relative rotation between node[0] and the cross
+        dQMultiply1( qq, node[0].body->q, qcross );
+        dQMultiply2( qrel, qq, qrel1 );
+
+        return getHingeAngleFromRelativeQuat( qrel, axis1 );
+    }
+    return 0;
+}
+
+
+dReal
+dxJointUniversal::getAngle2()
+{
+    if ( node[0].body )
+    {
+        // length 1 joint axis in global coordinates, from each body
+        dVector3 ax1, ax2;
+        dMatrix3 R;
+        dQuaternion qcross, qq, qrel;
+
+        getAxes( ax1, ax2 );
+
+        // It should be possible to get both angles without explicitly
+        // constructing the rotation matrix of the cross.  Basically,
+        // orientation of the cross about axis1 comes from body 2,
+        // about axis 2 comes from body 1, and the perpendicular
+        // axis can come from the two bodies somehow.  (We don't really
+        // want to assume it's 90 degrees, because in general the
+        // constraints won't be perfectly satisfied, or even very well
+        // satisfied.)
+        //
+        // However, we'd need a version of getHingeAngleFromRElativeQuat()
+        // that CAN handle when its relative quat is rotated along a direction
+        // other than the given axis.  What I have here works,
+        // although it's probably much slower than need be.
+
+        dRFrom2Axes( R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2] );
+        dRtoQ( R, qcross );
+
+        if ( node[1].body )
+        {
+            dQMultiply1( qq, node[1].body->q, qcross );
+            dQMultiply2( qrel, qq, qrel2 );
+        }
+        else
+        {
+            // pretend joint->node[1].body->q is the identity
+            dQMultiply2( qrel, qcross, qrel2 );
+        }
+
+        return - getHingeAngleFromRelativeQuat( qrel, axis2 );
+    }
+    return 0;
+}
+
+
+void 
+dxJointUniversal::getSureMaxInfo( SureMaxInfo* info )
+{
+    info->max_m = 6;
+}
+
+
+void
+dxJointUniversal::getInfo1( dxJoint::Info1 *info )
+{
+    info->nub = 4;
+    info->m = 4;
+
+    bool limiting1 = ( limot1.lostop >= -M_PI || limot1.histop <= M_PI ) &&
+        limot1.lostop <= limot1.histop;
+    bool limiting2 = ( limot2.lostop >= -M_PI || limot2.histop <= M_PI ) &&
+        limot2.lostop <= limot2.histop;
+
+    // We need to call testRotationLimit() even if we're motored, since it
+    // records the result.
+    limot1.limit = 0;
+    limot2.limit = 0;
+
+    if ( limiting1 || limiting2 )
+    {
+        dReal angle1, angle2;
+        getAngles( &angle1, &angle2 );
+        if ( limiting1 )
+            limot1.testRotationalLimit( angle1 );
+        if ( limiting2 )
+            limot2.testRotationalLimit( angle2 );
+    }
+
+    if ( limot1.limit || limot1.fmax > 0 ) info->m++;
+    if ( limot2.limit || limot2.fmax > 0 ) info->m++;
+}
+
+
+void
+dxJointUniversal::getInfo2( dReal worldFPS, dReal worldERP, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+    int *findex )
+{
+    // set the three ball-and-socket rows
+    setBall( this, worldFPS, worldERP, rowskip, J1, J2, pairskip, pairRhsCfm, anchor1, anchor2 );
+
+    // set the universal joint row. the angular velocity about an axis
+    // perpendicular to both joint axes should be equal. thus the constraint
+    // equation is
+    //    p*w1 - p*w2 = 0
+    // where p is a vector normal to both joint axes, and w1 and w2
+    // are the angular velocity vectors of the two bodies.
+
+    // length 1 joint axis in global coordinates, from each body
+    dVector3 ax1, ax2;
+    // length 1 vector perpendicular to ax1 and ax2. Neither body can rotate
+    // about this.
+    dVector3 p;
+    
+    // Since axis1 and axis2 may not be perpendicular
+    // we find a axis2_tmp which is really perpendicular to axis1
+    // and in the plane of axis1 and axis2
+    getAxes( ax1, ax2 );
+
+    dReal k = dCalcVectorDot3( ax1, ax2 );
+
+    dVector3 ax2_temp;
+    dAddVectorScaledVector3(ax2_temp, ax2, ax1, -k);
+    dCalcVectorCross3( p, ax1, ax2_temp );
+    dNormalize3( p );
+
+    int currRowSkip = 3 * rowskip;
+    {
+        dCopyVector3( J1 + currRowSkip + GI2__JA_MIN, p);
+
+        if ( node[1].body )
+        {
+            dCopyNegatedVector3( J2 + currRowSkip + GI2__JA_MIN, p);
+        }
+    }
+
+    // compute the right hand side of the constraint equation. set relative
+    // body velocities along p to bring the axes back to perpendicular.
+    // If ax1, ax2 are unit length joint axes as computed from body1 and
+    // body2, we need to rotate both bodies along the axis p.  If theta
+    // is the angle between ax1 and ax2, we need an angular velocity
+    // along p to cover the angle erp * (theta - Pi/2) in one step:
+    //
+    //   |angular_velocity| = angle/time = erp*(theta - Pi/2) / stepsize
+    //                      = (erp*fps) * (theta - Pi/2)
+    //
+    // if theta is close to Pi/2,
+    // theta - Pi/2 ~= cos(theta), so
+    //    |angular_velocity|  ~= (erp*fps) * (ax1 dot ax2)
+
+    int currPairSkip = 3 * pairskip;
+    {
+        pairRhsCfm[currPairSkip + GI2_RHS] = worldFPS * worldERP * (-k);
+    }
+
+    currRowSkip += rowskip; currPairSkip += pairskip;
+
+    // if the first angle is powered, or has joint limits, add in the stuff
+    if (limot1.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 ))
+    {
+        currRowSkip += rowskip; currPairSkip += pairskip;
+    }
+
+    // if the second angle is powered, or has joint limits, add in more stuff
+    limot2.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax2, 1 );
+}
+
+
+void
+dxJointUniversal::computeInitialRelativeRotations()
+{
+    if ( node[0].body )
+    {
+        dVector3 ax1, ax2;
+        dMatrix3 R;
+        dQuaternion qcross;
+
+        getAxes( ax1, ax2 );
+
+        // Axis 1.
+        dRFrom2Axes( R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2] );
+        dRtoQ( R, qcross );
+        dQMultiply1( qrel1, node[0].body->q, qcross );
+
+        // Axis 2.
+        dRFrom2Axes( R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2] );
+        dRtoQ( R, qcross );
+        if ( node[1].body )
+        {
+            dQMultiply1( qrel2, node[1].body->q, qcross );
+        }
+        else
+        {
+            // set joint->qrel to qcross
+            for ( int i = 0; i < 4; i++ ) qrel2[i] = qcross[i];
+        }
+    }
+}
+
+
+void dJointSetUniversalAnchor( dJointID j, dReal x, dReal y, dReal z )
+{
+    dxJointUniversal* joint = ( dxJointUniversal* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Universal );
+    setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 );
+    joint->computeInitialRelativeRotations();
+}
+
+
+void dJointSetUniversalAxis1( dJointID j, dReal x, dReal y, dReal z )
+{
+    dxJointUniversal* joint = ( dxJointUniversal* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Universal );
+    if ( joint->flags & dJOINT_REVERSE )
+        setAxes( joint, x, y, z, NULL, joint->axis2 );
+    else
+        setAxes( joint, x, y, z, joint->axis1, NULL );
+    joint->computeInitialRelativeRotations();
+}
+
+void dJointSetUniversalAxis1Offset( dJointID j, dReal x, dReal y, dReal z,
+                                   dReal offset1, dReal offset2 )
+{
+    dxJointUniversal* joint = ( dxJointUniversal* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Universal );
+    if ( joint->flags & dJOINT_REVERSE )
+    {
+        setAxes( joint, x, y, z, NULL, joint->axis2 );
+        offset1 = -offset1;
+        offset2 = -offset2;
+    }
+    else
+        setAxes( joint, x, y, z, joint->axis1, NULL );
+
+    joint->computeInitialRelativeRotations();
+
+
+    dVector3 ax2;
+    getAxis2( joint, ax2, joint->axis2 );
+
+    {
+        dVector3 ax1;
+        joint->getAxes(ax1, ax2);
+    }
+
+
+
+    dQuaternion qAngle;
+    dQFromAxisAndAngle(qAngle, x, y, z, offset1);
+
+    dMatrix3 R;
+    dRFrom2Axes( R, x, y, z, ax2[0], ax2[1], ax2[2] );
+
+    dQuaternion qcross;
+    dRtoQ( R, qcross );
+
+    dQuaternion qOffset;
+    dQMultiply0(qOffset, qAngle, qcross);
+
+    dQMultiply1( joint->qrel1, joint->node[0].body->q, qOffset );
+
+    // Calculating the second offset
+    dQFromAxisAndAngle(qAngle, ax2[0], ax2[1], ax2[2], offset2);
+
+    dRFrom2Axes( R, ax2[0], ax2[1], ax2[2], x, y, z );
+    dRtoQ( R, qcross );
+
+    dQMultiply1(qOffset, qAngle, qcross);
+    if ( joint->node[1].body )
+    {
+        dQMultiply1( joint->qrel2, joint->node[1].body->q, qOffset );
+    }
+    else
+    {
+        joint->qrel2[0] = qcross[0];
+        joint->qrel2[1] = qcross[1];
+        joint->qrel2[2] = qcross[2];
+        joint->qrel2[3] = qcross[3];
+    }
+}
+
+
+void dJointSetUniversalAxis2( dJointID j, dReal x, dReal y, dReal z )
+{
+    dxJointUniversal* joint = ( dxJointUniversal* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Universal );
+    if ( joint->flags & dJOINT_REVERSE )
+        setAxes( joint, x, y, z, joint->axis1, NULL );
+    else
+        setAxes( joint, x, y, z, NULL, joint->axis2 );
+    joint->computeInitialRelativeRotations();
+}
+
+void dJointSetUniversalAxis2Offset( dJointID j, dReal x, dReal y, dReal z,
+                                   dReal offset1, dReal offset2 )
+{
+    dxJointUniversal* joint = ( dxJointUniversal* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Universal );
+
+    if ( joint->flags & dJOINT_REVERSE )
+    {
+        setAxes( joint, x, y, z, joint->axis1, NULL );
+        offset1 = -offset2;
+        offset2 = -offset1;
+    }
+    else
+        setAxes( joint, x, y, z, NULL, joint->axis2 );
+
+
+    joint->computeInitialRelativeRotations();
+
+    // It is easier to retreive the 2 axes here since
+    // when there is only one body B2 (the axes switch position)
+    // Doing this way eliminate the need to write the code differently
+    // for both case.
+    dVector3 ax1, ax2;
+    joint->getAxes(ax1, ax2 );
+
+
+
+    dQuaternion qAngle;
+    dQFromAxisAndAngle(qAngle, ax1[0], ax1[1], ax1[2], offset1);
+
+    dMatrix3 R;
+    dRFrom2Axes( R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]);
+
+    dQuaternion qcross;
+    dRtoQ( R, qcross );
+
+    dQuaternion qOffset;
+    dQMultiply0(qOffset, qAngle, qcross);
+
+
+
+    dQMultiply1( joint->qrel1, joint->node[0].body->q, qOffset );
+
+
+    // Calculating the second offset
+    dQFromAxisAndAngle(qAngle, ax2[0], ax2[1], ax2[2], offset2);
+
+    dRFrom2Axes( R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]);
+    dRtoQ( R, qcross );
+
+    dQMultiply1(qOffset, qAngle, qcross);
+    if ( joint->node[1].body )
+    {
+        dQMultiply1( joint->qrel2, joint->node[1].body->q, qOffset );
+    }
+    else
+    {
+        joint->qrel2[0] = qcross[0];
+        joint->qrel2[1] = qcross[1];
+        joint->qrel2[2] = qcross[2];
+        joint->qrel2[3] = qcross[3];
+    }
+}
+
+
+void dJointGetUniversalAnchor( dJointID j, dVector3 result )
+{
+    dxJointUniversal* joint = ( dxJointUniversal* )j;
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+    checktype( joint, Universal );
+    if ( joint->flags & dJOINT_REVERSE )
+        getAnchor2( joint, result, joint->anchor2 );
+    else
+        getAnchor( joint, result, joint->anchor1 );
+}
+
+
+void dJointGetUniversalAnchor2( dJointID j, dVector3 result )
+{
+    dxJointUniversal* joint = ( dxJointUniversal* )j;
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+    checktype( joint, Universal );
+    if ( joint->flags & dJOINT_REVERSE )
+        getAnchor( joint, result, joint->anchor1 );
+    else
+        getAnchor2( joint, result, joint->anchor2 );
+}
+
+
+void dJointGetUniversalAxis1( dJointID j, dVector3 result )
+{
+    dxJointUniversal* joint = ( dxJointUniversal* )j;
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+    checktype( joint, Universal );
+    if ( joint->flags & dJOINT_REVERSE )
+        getAxis2( joint, result, joint->axis2 );
+    else
+        getAxis( joint, result, joint->axis1 );
+}
+
+
+void dJointGetUniversalAxis2( dJointID j, dVector3 result )
+{
+    dxJointUniversal* joint = ( dxJointUniversal* )j;
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+    checktype( joint, Universal );
+    if ( joint->flags & dJOINT_REVERSE )
+        getAxis( joint, result, joint->axis1 );
+    else
+        getAxis2( joint, result, joint->axis2 );
+}
+
+
+void dJointSetUniversalParam( dJointID j, int parameter, dReal value )
+{
+    dxJointUniversal* joint = ( dxJointUniversal* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Universal );
+    if (( parameter & 0xff00 ) == 0x100 )
+    {
+        joint->limot2.set( parameter & 0xff, value );
+    }
+    else
+    {
+        joint->limot1.set( parameter, value );
+    }
+}
+
+
+dReal dJointGetUniversalParam( dJointID j, int parameter )
+{
+    dxJointUniversal* joint = ( dxJointUniversal* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Universal );
+    if (( parameter & 0xff00 ) == 0x100 )
+    {
+        return joint->limot2.get( parameter & 0xff );
+    }
+    else
+    {
+        return joint->limot1.get( parameter );
+    }
+}
+
+void dJointGetUniversalAngles( dJointID j, dReal *angle1, dReal *angle2 )
+{
+    dxJointUniversal* joint = ( dxJointUniversal* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Universal );
+    if ( joint->flags & dJOINT_REVERSE )
+    {
+        joint->getAngles( angle2, angle1 );
+        *angle2 = -(*angle2);
+        return;
+    }
+    else
+        return joint->getAngles( angle1, angle2 );
+}
+
+
+dReal dJointGetUniversalAngle1( dJointID j )
+{
+    dxJointUniversal* joint = ( dxJointUniversal* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Universal );
+    if ( joint->flags & dJOINT_REVERSE )
+        return joint->getAngle2();
+    else
+        return joint->getAngle1();
+}
+
+
+dReal dJointGetUniversalAngle2( dJointID j )
+{
+    dxJointUniversal* joint = ( dxJointUniversal* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Universal );
+    if ( joint->flags & dJOINT_REVERSE )
+        return -joint->getAngle1();
+    else
+        return joint->getAngle2();
+}
+
+
+dReal dJointGetUniversalAngle1Rate( dJointID j )
+{
+    dxJointUniversal* joint = ( dxJointUniversal* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Universal );
+
+    if ( joint->node[0].body )
+    {
+        dVector3 axis;
+
+        if ( joint->flags & dJOINT_REVERSE )
+            getAxis2( joint, axis, joint->axis2 );
+        else
+            getAxis( joint, axis, joint->axis1 );
+
+        dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel );
+        if ( joint->node[1].body )
+            rate -= dCalcVectorDot3( axis, joint->node[1].body->avel );
+        return rate;
+    }
+    return 0;
+}
+
+
+dReal dJointGetUniversalAngle2Rate( dJointID j )
+{
+    dxJointUniversal* joint = ( dxJointUniversal* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Universal );
+
+    if ( joint->node[0].body )
+    {
+        dVector3 axis;
+
+        if ( joint->flags & dJOINT_REVERSE )
+            getAxis( joint, axis, joint->axis1 );
+        else
+            getAxis2( joint, axis, joint->axis2 );
+
+        dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel );
+        if ( joint->node[1].body ) rate -= dCalcVectorDot3( axis, joint->node[1].body->avel );
+        return rate;
+    }
+    return 0;
+}
+
+
+void dJointAddUniversalTorques( dJointID j, dReal torque1, dReal torque2 )
+{
+    dxJointUniversal* joint = ( dxJointUniversal* )j;
+    dVector3 axis1, axis2;
+    dAASSERT( joint );
+    checktype( joint, Universal );
+
+    if ( joint->flags & dJOINT_REVERSE )
+    {
+        dReal temp = torque1;
+        torque1 = - torque2;
+        torque2 = - temp;
+    }
+
+    getAxis( joint, axis1, joint->axis1 );
+    getAxis2( joint, axis2, joint->axis2 );
+    axis1[0] = axis1[0] * torque1 + axis2[0] * torque2;
+    axis1[1] = axis1[1] * torque1 + axis2[1] * torque2;
+    axis1[2] = axis1[2] * torque1 + axis2[2] * torque2;
+
+    if ( joint->node[0].body != 0 )
+        dBodyAddTorque( joint->node[0].body, axis1[0], axis1[1], axis1[2] );
+    if ( joint->node[1].body != 0 )
+        dBodyAddTorque( joint->node[1].body, -axis1[0], -axis1[1], -axis1[2] );
+}
+
+
+dJointType
+dxJointUniversal::type() const
+{
+    return dJointTypeUniversal;
+}
+
+
+sizeint
+dxJointUniversal::size() const
+{
+    return sizeof( *this );
+}
+
+
+
+void
+dxJointUniversal::setRelativeValues()
+{
+    dVector3 anchor;
+    dJointGetUniversalAnchor(this, anchor);
+    setAnchors( this, anchor[0], anchor[1], anchor[2], anchor1, anchor2 );
+
+    dVector3 ax1,ax2;
+    dJointGetUniversalAxis1(this, ax1);
+    dJointGetUniversalAxis2(this, ax2);
+
+    if ( flags & dJOINT_REVERSE )
+    {
+        setAxes( this, ax1[0],ax1[1],ax1[2], NULL, axis2 );
+        setAxes( this, ax2[0],ax2[1],ax2[2], axis1, NULL );
+    }
+    else
+    {
+        setAxes( this, ax1[0],ax1[1],ax1[2], axis1, NULL );
+        setAxes( this, ax2[0],ax2[1],ax2[2], NULL, axis2 );
+    }
+
+    computeInitialRelativeRotations();
+}
+