From c5fc66ee58f2c60f2d226868bb1cf5b91badaf53 Mon Sep 17 00:00:00 2001 From: sanine Date: Sat, 1 Oct 2022 20:59:36 -0500 Subject: add ode --- libs/ode-0.16.1/ode/src/joints/universal.cpp | 803 +++++++++++++++++++++++++++ 1 file changed, 803 insertions(+) create mode 100644 libs/ode-0.16.1/ode/src/joints/universal.cpp (limited to 'libs/ode-0.16.1/ode/src/joints/universal.cpp') 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 +#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(); +} + -- cgit v1.2.1