/************************************************************************* * * * 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 "hinge.h" #include "joint_internal.h" //**************************************************************************** // hinge dxJointHinge::dxJointHinge( dxWorld *w ) : dxJoint( w ) { dSetZero( anchor1, 4 ); dSetZero( anchor2, 4 ); dSetZero( axis1, 4 ); axis1[0] = 1; dSetZero( axis2, 4 ); axis2[0] = 1; dSetZero( qrel, 4 ); limot.init( world ); } void dxJointHinge::getSureMaxInfo( SureMaxInfo* info ) { info->max_m = 6; } void dxJointHinge::getInfo1( dxJoint::Info1 *info ) { info->nub = 5; // see if joint is powered if ( limot.fmax > 0 ) info->m = 6; // powered hinge needs an extra constraint row else info->m = 5; // see if we're at a joint limit. if (( limot.lostop >= -M_PI || limot.histop <= M_PI ) && limot.lostop <= limot.histop ) { dReal angle = getHingeAngle( node[0].body, node[1].body, axis1, qrel ); if ( limot.testRotationalLimit( angle ) ) info->m = 6; } } void dxJointHinge::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 two hinge rows. the hinge axis should be the only unconstrained // rotational axis, the angular velocity of the two bodies perpendicular to // the hinge axis should be equal. thus the constraint equations are // p*w1 - p*w2 = 0 // q*w1 - q*w2 = 0 // where p and q are unit vectors normal to the hinge axis, and w1 and w2 // are the angular velocity vectors of the two bodies. dVector3 ax1; // length 1 joint axis in global coordinates, from 1st body dVector3 p, q; // plane space vectors for ax1 dMultiply0_331( ax1, node[0].body->posr.R, axis1 ); dPlaneSpace( ax1, p, q ); dxBody *body1 = node[1].body; int currRowSkip = 3 * rowskip; dCopyVector3(J1 + currRowSkip + GI2__JA_MIN, p); if ( body1 ) { dCopyNegatedVector3(J2 + currRowSkip + GI2__JA_MIN, p); } currRowSkip += rowskip; dCopyVector3(J1 + currRowSkip + GI2__JA_MIN, q); if ( body1 ) { dCopyNegatedVector3(J2 + currRowSkip + GI2__JA_MIN, q); } // compute the right hand side of the constraint equation. set relative // body velocities along p and q to bring the hinge back into alignment. // if ax1,ax2 are the unit length hinge axes as computed from body1 and // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). // if `theta' is the angle between ax1 and ax2, we need an angular velocity // along u to cover angle erp*theta in one step : // |angular_velocity| = angle/time = erp*theta / stepsize // = (erp*fps) * theta // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) // ...as ax1 and ax2 are unit length. if theta is smallish, // theta ~= sin(theta), so // angular_velocity = (erp*fps) * (ax1 x ax2) // ax1 x ax2 is in the plane space of ax1, so we project the angular // velocity to p and q to find the right hand side. dVector3 b; if ( body1 ) { dVector3 ax2; dMultiply0_331( ax2, body1->posr.R, axis2 ); dCalcVectorCross3( b, ax1, ax2 ); } else { dCalcVectorCross3( b, ax1, axis2 ); } dReal k = worldFPS * worldERP; int currPairSkip = 3 * pairskip; pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3( b, p ); currPairSkip += pairskip; pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3( b, q ); // if the hinge is powered, or has joint limits, add in the stuff currRowSkip += rowskip; currPairSkip += pairskip; limot.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 ); } void dJointSetHingeAnchor( dJointID j, dReal x, dReal y, dReal z ) { dxJointHinge* joint = ( dxJointHinge* )j; dUASSERT( joint, "bad joint argument" ); checktype( joint, Hinge ); setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 ); joint->computeInitialRelativeRotation(); } void dJointSetHingeAnchorDelta( dJointID j, dReal x, dReal y, dReal z, dReal dx, dReal dy, dReal dz ) { dxJointHinge* joint = ( dxJointHinge* )j; dUASSERT( joint, "bad joint argument" ); checktype( joint, Hinge ); if ( joint->node[0].body ) { dReal q[4]; q[0] = x - joint->node[0].body->posr.pos[0]; q[1] = y - joint->node[0].body->posr.pos[1]; q[2] = z - joint->node[0].body->posr.pos[2]; q[3] = 0; dMultiply1_331( joint->anchor1, joint->node[0].body->posr.R, q ); if ( joint->node[1].body ) { q[0] = x - joint->node[1].body->posr.pos[0]; q[1] = y - joint->node[1].body->posr.pos[1]; q[2] = z - joint->node[1].body->posr.pos[2]; q[3] = 0; dMultiply1_331( joint->anchor2, joint->node[1].body->posr.R, q ); } else { // Move the relative displacement between the passive body and the // anchor in the same direction as the passive body has just moved joint->anchor2[0] = x + dx; joint->anchor2[1] = y + dy; joint->anchor2[2] = z + dz; } } joint->anchor1[3] = 0; joint->anchor2[3] = 0; joint->computeInitialRelativeRotation(); } void dJointSetHingeAxis( dJointID j, dReal x, dReal y, dReal z ) { dxJointHinge* joint = ( dxJointHinge* )j; dUASSERT( joint, "bad joint argument" ); checktype( joint, Hinge ); setAxes( joint, x, y, z, joint->axis1, joint->axis2 ); joint->computeInitialRelativeRotation(); } void dJointSetHingeAxisOffset( dJointID j, dReal x, dReal y, dReal z, dReal dangle ) { dxJointHinge* joint = ( dxJointHinge* )j; dUASSERT( joint, "bad joint argument" ); checktype( joint, Hinge ); setAxes( joint, x, y, z, joint->axis1, joint->axis2 ); joint->computeInitialRelativeRotation(); if ( joint->flags & dJOINT_REVERSE ) dangle = -dangle; dQuaternion qAngle, qOffset; dQFromAxisAndAngle(qAngle, x, y, z, dangle); dQMultiply3(qOffset, qAngle, joint->qrel); joint->qrel[0] = qOffset[0]; joint->qrel[1] = qOffset[1]; joint->qrel[2] = qOffset[2]; joint->qrel[3] = qOffset[3]; } void dJointGetHingeAnchor( dJointID j, dVector3 result ) { dxJointHinge* joint = ( dxJointHinge* )j; dUASSERT( joint, "bad joint argument" ); dUASSERT( result, "bad result argument" ); checktype( joint, Hinge ); if ( joint->flags & dJOINT_REVERSE ) getAnchor2( joint, result, joint->anchor2 ); else getAnchor( joint, result, joint->anchor1 ); } void dJointGetHingeAnchor2( dJointID j, dVector3 result ) { dxJointHinge* joint = ( dxJointHinge* )j; dUASSERT( joint, "bad joint argument" ); dUASSERT( result, "bad result argument" ); checktype( joint, Hinge ); if ( joint->flags & dJOINT_REVERSE ) getAnchor( joint, result, joint->anchor1 ); else getAnchor2( joint, result, joint->anchor2 ); } void dJointGetHingeAxis( dJointID j, dVector3 result ) { dxJointHinge* joint = ( dxJointHinge* )j; dUASSERT( joint, "bad joint argument" ); dUASSERT( result, "bad result argument" ); checktype( joint, Hinge ); getAxis( joint, result, joint->axis1 ); } void dJointSetHingeParam( dJointID j, int parameter, dReal value ) { dxJointHinge* joint = ( dxJointHinge* )j; dUASSERT( joint, "bad joint argument" ); checktype( joint, Hinge ); joint->limot.set( parameter, value ); } dReal dJointGetHingeParam( dJointID j, int parameter ) { dxJointHinge* joint = ( dxJointHinge* )j; dUASSERT( joint, "bad joint argument" ); checktype( joint, Hinge ); return joint->limot.get( parameter ); } dReal dJointGetHingeAngle( dJointID j ) { dxJointHinge* joint = ( dxJointHinge* )j; dAASSERT( joint ); checktype( joint, Hinge ); if ( joint->node[0].body ) { dReal ang = getHingeAngle( joint->node[0].body, joint->node[1].body, joint->axis1, joint->qrel ); if ( joint->flags & dJOINT_REVERSE ) return -ang; else return ang; } else return 0; } dReal dJointGetHingeAngleRate( dJointID j ) { dxJointHinge* joint = ( dxJointHinge* )j; dAASSERT( joint ); checktype( joint, Hinge ); if ( joint->node[0].body ) { dVector3 axis; dMultiply0_331( axis, joint->node[0].body->posr.R, joint->axis1 ); dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel ); if ( joint->node[1].body ) rate -= dCalcVectorDot3( axis, joint->node[1].body->avel ); if ( joint->flags & dJOINT_REVERSE ) rate = - rate; return rate; } else return 0; } void dJointAddHingeTorque( dJointID j, dReal torque ) { dxJointHinge* joint = ( dxJointHinge* )j; dVector3 axis; dAASSERT( joint ); checktype( joint, Hinge ); if ( joint->flags & dJOINT_REVERSE ) torque = -torque; getAxis( joint, axis, joint->axis1 ); axis[0] *= torque; axis[1] *= torque; axis[2] *= torque; if ( joint->node[0].body != 0 ) dBodyAddTorque( joint->node[0].body, axis[0], axis[1], axis[2] ); if ( joint->node[1].body != 0 ) dBodyAddTorque( joint->node[1].body, -axis[0], -axis[1], -axis[2] ); } dJointType dxJointHinge::type() const { return dJointTypeHinge; } sizeint dxJointHinge::size() const { return sizeof( *this ); } void dxJointHinge::setRelativeValues() { dVector3 vec; dJointGetHingeAnchor(this, vec); setAnchors( this, vec[0], vec[1], vec[2], anchor1, anchor2 ); dJointGetHingeAxis(this, vec); setAxes( this, vec[0], vec[1], vec[2], axis1, axis2 ); computeInitialRelativeRotation(); } /// Compute initial relative rotation body1 -> body2, or env -> body1 void dxJointHinge::computeInitialRelativeRotation() { if ( node[0].body ) { if ( node[1].body ) { dQMultiply1( qrel, node[0].body->q, node[1].body->q ); } else { // set qrel to the transpose of the first body q qrel[0] = node[0].body->q[0]; qrel[1] = -node[0].body->q[1]; qrel[2] = -node[0].body->q[2]; qrel[3] = -node[0].body->q[3]; } } }